1187 files changed, 58230 insertions, 23076 deletions

diff --git a/drivers/acpi/acpica/tbutils.c b/drivers/acpi/acpica/tbutils.c
index 5a968a78652b..7abe66505739 100644
--- a/drivers/acpi/acpica/tbutils.c
+++ b/drivers/acpi/acpica/tbutils.c
@@ -418,11 +418,7 @@ acpi_tb_get_table(struct acpi_table_desc *table_desc,
 
 	table_desc->validation_count++;
 	if (table_desc->validation_count == 0) {
-		ACPI_ERROR((AE_INFO,
-			    "Table %p, Validation count is zero after increment\n",
-			    table_desc));
 		table_desc->validation_count--;
-		return_ACPI_STATUS(AE_LIMIT);
 	}
 
 	*out_table = table_desc->pointer;
diff --git a/drivers/acpi/button.c b/drivers/acpi/button.c
index b7c2a06963d6..9ad8cdb58743 100644
--- a/drivers/acpi/button.c
+++ b/drivers/acpi/button.c
@@ -57,6 +57,7 @@
 
 #define ACPI_BUTTON_LID_INIT_IGNORE	0x00
 #define ACPI_BUTTON_LID_INIT_OPEN	0x01
+#define ACPI_BUTTON_LID_INIT_METHOD	0x02
 
 #define _COMPONENT		ACPI_BUTTON_COMPONENT
 ACPI_MODULE_NAME("button");
@@ -112,7 +113,7 @@ struct acpi_button {
 
 static BLOCKING_NOTIFIER_HEAD(acpi_lid_notifier);
 static struct acpi_device *lid_device;
-static u8 lid_init_state = ACPI_BUTTON_LID_INIT_OPEN;
+static u8 lid_init_state = ACPI_BUTTON_LID_INIT_METHOD;
 
 static unsigned long lid_report_interval __read_mostly = 500;
 module_param(lid_report_interval, ulong, 0644);
@@ -376,6 +377,9 @@ static void acpi_lid_initialize_state(struct acpi_device *device)
 	case ACPI_BUTTON_LID_INIT_OPEN:
 		(void)acpi_lid_notify_state(device, 1);
 		break;
+	case ACPI_BUTTON_LID_INIT_METHOD:
+		(void)acpi_lid_update_state(device);
+		break;
 	case ACPI_BUTTON_LID_INIT_IGNORE:
 	default:
 		break;
@@ -560,6 +564,9 @@ static int param_set_lid_init_state(const char *val, struct kernel_param *kp)
 	if (!strncmp(val, "open", sizeof("open") - 1)) {
 		lid_init_state = ACPI_BUTTON_LID_INIT_OPEN;
 		pr_info("Notify initial lid state as open\n");
+	} else if (!strncmp(val, "method", sizeof("method") - 1)) {
+		lid_init_state = ACPI_BUTTON_LID_INIT_METHOD;
+		pr_info("Notify initial lid state with _LID return value\n");
 	} else if (!strncmp(val, "ignore", sizeof("ignore") - 1)) {
 		lid_init_state = ACPI_BUTTON_LID_INIT_IGNORE;
 		pr_info("Do not notify initial lid state\n");
@@ -573,6 +580,8 @@ static int param_get_lid_init_state(char *buffer, struct kernel_param *kp)
 	switch (lid_init_state) {
 	case ACPI_BUTTON_LID_INIT_OPEN:
 		return sprintf(buffer, "open");
+	case ACPI_BUTTON_LID_INIT_METHOD:
+		return sprintf(buffer, "method");
 	case ACPI_BUTTON_LID_INIT_IGNORE:
 		return sprintf(buffer, "ignore");
 	default:
diff --git a/drivers/acpi/nfit/mce.c b/drivers/acpi/nfit/mce.c
index 3ba1c3472cf9..fd86bec98dea 100644
--- a/drivers/acpi/nfit/mce.c
+++ b/drivers/acpi/nfit/mce.c
@@ -26,7 +26,7 @@ static int nfit_handle_mce(struct notifier_block *nb, unsigned long val,
 	struct nfit_spa *nfit_spa;
 
 	/* We only care about memory errors */
-	if (!(mce->status & MCACOD))
+	if (!mce_is_memory_error(mce))
 		return NOTIFY_DONE;
 
 	/*
diff --git a/drivers/acpi/sysfs.c b/drivers/acpi/sysfs.c
index 1b5ee1e0e5a3..e414fabf7315 100644
--- a/drivers/acpi/sysfs.c
+++ b/drivers/acpi/sysfs.c
@@ -333,14 +333,17 @@ static ssize_t acpi_table_show(struct file *filp, struct kobject *kobj,
 	    container_of(bin_attr, struct acpi_table_attr, attr);
 	struct acpi_table_header *table_header = NULL;
 	acpi_status status;
+	ssize_t rc;
 
 	status = acpi_get_table(table_attr->name, table_attr->instance,
 				&table_header);
 	if (ACPI_FAILURE(status))
 		return -ENODEV;
 
-	return memory_read_from_buffer(buf, count, &offset,
-				       table_header, table_header->length);
+	rc = memory_read_from_buffer(buf, count, &offset, table_header,
+			table_header->length);
+	acpi_put_table(table_header);
+	return rc;
 }
 
 static int acpi_table_attr_init(struct kobject *tables_obj,
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 2fc52407306c..fd712e0e9d87 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
diff --git a/drivers/ata/ahci.h b/drivers/ata/ahci.h
index 5db6ab261643..30f67a1a4f54 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
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..6154f0e2b81a 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
diff --git a/drivers/ata/libata-core.c b/drivers/ata/libata-core.c
index 2d83b8c75965..55aaa2e4c683 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/
diff --git a/drivers/ata/libata-eh.c b/drivers/ata/libata-eh.c
index ef68232b5222..7e33e200aae5 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/
diff --git a/drivers/ata/libata-scsi.c b/drivers/ata/libata-scsi.c
index 49ba9834c715..b0866f040d1f 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/
@@ -3398,9 +3398,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)
 {
diff --git a/drivers/ata/libata-sff.c b/drivers/ata/libata-sff.c
index 274d6d7193d7..052921352f31 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/
diff --git a/drivers/ata/libata.h b/drivers/ata/libata.h
index 120fce0befd3..5afe35baf61b 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
  *
  */
 
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/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_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_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..22e96fc77d09 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.
  *
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/base/power/wakeup.c b/drivers/base/power/wakeup.c
index f62082fdd670..9c36b27996fc 100644
--- a/drivers/base/power/wakeup.c
+++ b/drivers/base/power/wakeup.c
@@ -512,13 +512,12 @@ static bool wakeup_source_not_registered(struct wakeup_source *ws)
 /**
  * wakup_source_activate - Mark given wakeup source as active.
  * @ws: Wakeup source to handle.
- * @hard: If set, abort suspends in progress and wake up from suspend-to-idle.
  *
  * Update the @ws' statistics and, if @ws has just been activated, notify the PM
  * core of the event by incrementing the counter of of wakeup events being
  * processed.
  */
-static void wakeup_source_activate(struct wakeup_source *ws, bool hard)
+static void wakeup_source_activate(struct wakeup_source *ws)
 {
 	unsigned int cec;
 
@@ -526,9 +525,6 @@ static void wakeup_source_activate(struct wakeup_source *ws, bool hard)
 			"unregistered wakeup source\n"))
 		return;
 
-	if (hard)
-		pm_system_wakeup();
-
 	ws->active = true;
 	ws->active_count++;
 	ws->last_time = ktime_get();
@@ -554,7 +550,10 @@ static void wakeup_source_report_event(struct wakeup_source *ws, bool hard)
 		ws->wakeup_count++;
 
 	if (!ws->active)
-		wakeup_source_activate(ws, hard);
+		wakeup_source_activate(ws);
+
+	if (hard)
+		pm_system_wakeup();
 }
 
 /**
diff --git a/drivers/block/drbd/drbd_req.c b/drivers/block/drbd/drbd_req.c
index b5730e17b455..656624314f0d 100644
--- a/drivers/block/drbd/drbd_req.c
+++ b/drivers/block/drbd/drbd_req.c
@@ -315,24 +315,32 @@ void drbd_req_complete(struct drbd_request *req, struct bio_and_error *m)
 }
 
 /* still holds resource->req_lock */
-static int drbd_req_put_completion_ref(struct drbd_request *req, struct bio_and_error *m, int put)
+static void drbd_req_put_completion_ref(struct drbd_request *req, struct bio_and_error *m, int put)
 {
 	struct drbd_device *device = req->device;
 	D_ASSERT(device, m || (req->rq_state & RQ_POSTPONED));
 
+	if (!put)
+		return;
+
 	if (!atomic_sub_and_test(put, &req->completion_ref))
-		return 0;
+		return;
 
 	drbd_req_complete(req, m);
 
+	/* local completion may still come in later,
+	 * we need to keep the req object around. */
+	if (req->rq_state & RQ_LOCAL_ABORTED)
+		return;
+
 	if (req->rq_state & RQ_POSTPONED) {
 		/* don't destroy the req object just yet,
 		 * but queue it for retry */
 		drbd_restart_request(req);
-		return 0;
+		return;
 	}
 
-	return 1;
+	kref_put(&req->kref, drbd_req_destroy);
 }
 
 static void set_if_null_req_next(struct drbd_peer_device *peer_device, struct drbd_request *req)
@@ -519,12 +527,8 @@ static void mod_rq_state(struct drbd_request *req, struct bio_and_error *m,
 	if (req->i.waiting)
 		wake_up(&device->misc_wait);
 
-	if (c_put) {
-		if (drbd_req_put_completion_ref(req, m, c_put))
-			kref_put(&req->kref, drbd_req_destroy);
-	} else {
-		kref_put(&req->kref, drbd_req_destroy);
-	}
+	drbd_req_put_completion_ref(req, m, c_put);
+	kref_put(&req->kref, drbd_req_destroy);
 }
 
 static void drbd_report_io_error(struct drbd_device *device, struct drbd_request *req)
@@ -1366,8 +1370,7 @@ nodata:
 	}
 
 out:
-	if (drbd_req_put_completion_ref(req, &m, 1))
-		kref_put(&req->kref, drbd_req_destroy);
+	drbd_req_put_completion_ref(req, &m, 1);
 	spin_unlock_irq(&resource->req_lock);
 
 	/* Even though above is a kref_put(), this is safe.
diff --git a/drivers/block/rbd.c b/drivers/block/rbd.c
index 454bf9c34882..c16f74547804 100644
--- a/drivers/block/rbd.c
+++ b/drivers/block/rbd.c
@@ -4023,6 +4023,7 @@ static void rbd_queue_workfn(struct work_struct *work)
 
 	switch (req_op(rq)) {
 	case REQ_OP_DISCARD:
+	case REQ_OP_WRITE_ZEROES:
 		op_type = OBJ_OP_DISCARD;
 		break;
 	case REQ_OP_WRITE:
@@ -4420,6 +4421,7 @@ static int rbd_init_disk(struct rbd_device *rbd_dev)
 	q->limits.discard_granularity = segment_size;
 	q->limits.discard_alignment = segment_size;
 	blk_queue_max_discard_sectors(q, segment_size / SECTOR_SIZE);
+	blk_queue_max_write_zeroes_sectors(q, segment_size / SECTOR_SIZE);
 
 	if (!ceph_test_opt(rbd_dev->rbd_client->client, NOCRC))
 		q->backing_dev_info->capabilities |= BDI_CAP_STABLE_WRITES;
diff --git a/drivers/block/xen-blkback/xenbus.c b/drivers/block/xen-blkback/xenbus.c
index 8fe61b5dc5a6..1f3dfaa54d87 100644
--- a/drivers/block/xen-blkback/xenbus.c
+++ b/drivers/block/xen-blkback/xenbus.c
@@ -504,11 +504,13 @@ static int xen_blkbk_remove(struct xenbus_device *dev)
 
 	dev_set_drvdata(&dev->dev, NULL);
 
-	if (be->blkif)
+	if (be->blkif) {
 		xen_blkif_disconnect(be->blkif);
 
-	/* Put the reference we set in xen_blkif_alloc(). */
-	xen_blkif_put(be->blkif);
+		/* Put the reference we set in xen_blkif_alloc(). */
+		xen_blkif_put(be->blkif);
+	}
+
 	kfree(be->mode);
 	kfree(be);
 	return 0;
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/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/btusb.c b/drivers/bluetooth/btusb.c
index 7fa373b428f8..278e81186150 100644
--- a/drivers/bluetooth/btusb.c
+++ b/drivers/bluetooth/btusb.c
@@ -336,6 +336,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),
@@ -2036,6 +2037,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 +2140,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),
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/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..200288c87fc4 100644
--- a/drivers/bluetooth/hci_ll.c
+++ b/drivers/bluetooth/hci_ll.c
@@ -624,6 +624,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);
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/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/lp.c b/drivers/char/lp.c
index 565e4cf04a02..8249762192d5 100644
--- a/drivers/char/lp.c
+++ b/drivers/char/lp.c
@@ -859,7 +859,11 @@ static int __init lp_setup (char *str)
 	} else if (!strcmp(str, "auto")) {
 		parport_nr[0] = LP_PARPORT_AUTO;
 	} else if (!strcmp(str, "none")) {
-		parport_nr[parport_ptr++] = LP_PARPORT_NONE;
+		if (parport_ptr < LP_NO)
+			parport_nr[parport_ptr++] = LP_PARPORT_NONE;
+		else
+			printk(KERN_INFO "lp: too many ports, %s ignored.\n",
+			       str);
 	} else if (!strcmp(str, "reset")) {
 		reset = 1;
 	}
diff --git a/drivers/char/mem.c b/drivers/char/mem.c
index 7e4a9d1296bb..6e0cbe092220 100644
--- a/drivers/char/mem.c
+++ b/drivers/char/mem.c
@@ -340,6 +340,11 @@ static const struct vm_operations_struct mmap_mem_ops = {
 static int mmap_mem(struct file *file, struct vm_area_struct *vma)
 {
 	size_t size = vma->vm_end - vma->vm_start;
+	phys_addr_t offset = (phys_addr_t)vma->vm_pgoff << PAGE_SHIFT;
+
+	/* It's illegal to wrap around the end of the physical address space. */
+	if (offset + (phys_addr_t)size < offset)
+		return -EINVAL;
 
 	if (!valid_mmap_phys_addr_range(vma->vm_pgoff, size))
 		return -EINVAL;
diff --git a/drivers/clk/Kconfig b/drivers/clk/Kconfig
index 36cfea38135f..a0fd60f0cc89 100644
--- a/drivers/clk/Kconfig
+++ b/drivers/clk/Kconfig
@@ -224,6 +224,7 @@ source "drivers/clk/meson/Kconfig"
 source "drivers/clk/mvebu/Kconfig"
 source "drivers/clk/qcom/Kconfig"
 source "drivers/clk/renesas/Kconfig"
+source "drivers/clk/rockchip/Kconfig"
 source "drivers/clk/samsung/Kconfig"
 source "drivers/clk/sunxi-ng/Kconfig"
 source "drivers/clk/tegra/Kconfig"
diff --git a/drivers/clk/clk-divider.c b/drivers/clk/clk-divider.c
index 96386ffc8483..9bb472cccca6 100644
--- a/drivers/clk/clk-divider.c
+++ b/drivers/clk/clk-divider.c
@@ -275,7 +275,8 @@ static int _next_div(const struct clk_div_table *table, int div,
 	return div;
 }
 
-static int clk_divider_bestdiv(struct clk_hw *hw, unsigned long rate,
+static int clk_divider_bestdiv(struct clk_hw *hw, struct clk_hw *parent,
+			       unsigned long rate,
 			       unsigned long *best_parent_rate,
 			       const struct clk_div_table *table, u8 width,
 			       unsigned long flags)
@@ -314,8 +315,7 @@ static int clk_divider_bestdiv(struct clk_hw *hw, unsigned long rate,
 			*best_parent_rate = parent_rate_saved;
 			return i;
 		}
-		parent_rate = clk_hw_round_rate(clk_hw_get_parent(hw),
-					       rate * i);
+		parent_rate = clk_hw_round_rate(parent, rate * i);
 		now = DIV_ROUND_UP_ULL((u64)parent_rate, i);
 		if (_is_best_div(rate, now, best, flags)) {
 			bestdiv = i;
@@ -326,23 +326,24 @@ static int clk_divider_bestdiv(struct clk_hw *hw, unsigned long rate,
 
 	if (!bestdiv) {
 		bestdiv = _get_maxdiv(table, width, flags);
-		*best_parent_rate = clk_hw_round_rate(clk_hw_get_parent(hw), 1);
+		*best_parent_rate = clk_hw_round_rate(parent, 1);
 	}
 
 	return bestdiv;
 }
 
-long divider_round_rate(struct clk_hw *hw, unsigned long rate,
-			unsigned long *prate, const struct clk_div_table *table,
-			u8 width, unsigned long flags)
+long divider_round_rate_parent(struct clk_hw *hw, struct clk_hw *parent,
+			       unsigned long rate, unsigned long *prate,
+			       const struct clk_div_table *table,
+			       u8 width, unsigned long flags)
 {
 	int div;
 
-	div = clk_divider_bestdiv(hw, rate, prate, table, width, flags);
+	div = clk_divider_bestdiv(hw, parent, rate, prate, table, width, flags);
 
 	return DIV_ROUND_UP_ULL((u64)*prate, div);
 }
-EXPORT_SYMBOL_GPL(divider_round_rate);
+EXPORT_SYMBOL_GPL(divider_round_rate_parent);
 
 static long clk_divider_round_rate(struct clk_hw *hw, unsigned long rate,
 				unsigned long *prate)
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/rockchip/Kconfig b/drivers/clk/rockchip/Kconfig
new file mode 100644
index 000000000000..60c05d900622
--- /dev/null
+++ b/drivers/clk/rockchip/Kconfig
@@ -0,0 +1,39 @@
+config COMMON_CLK_RV1108
+	bool "Clock driver for Rockchip RV1108"
+	depends on (ARCH_ROCKCHIP && ARM) || COMPILE_TEST
+	default ARCH_ROCKCHIP && ARM
+
+config COMMON_CLK_RK3036
+	bool "Clock driver for Rockchip RK3036"
+	depends on (ARCH_ROCKCHIP && ARM) || COMPILE_TEST
+	default ARCH_ROCKCHIP && ARM
+
+config COMMON_CLK_RK3188
+	bool "Clock driver for Rockchip RK3066 and RK3188"
+	depends on (ARCH_ROCKCHIP && ARM) || COMPILE_TEST
+	default ARCH_ROCKCHIP && ARM
+
+config COMMON_CLK_RK3228
+	bool "Clock driver for Rockchip RK3228 and RK3229"
+	depends on (ARCH_ROCKCHIP && ARM) || COMPILE_TEST
+	default ARCH_ROCKCHIP && ARM
+
+config COMMON_CLK_RK3288
+	bool "Clock driver for Rockchip RK3288"
+	depends on (ARCH_ROCKCHIP && ARM) || COMPILE_TEST
+	default ARCH_ROCKCHIP && ARM
+
+config COMMON_CLK_RK3328
+	bool "Clock driver for Rockchip RK3328"
+	depends on (ARCH_ROCKCHIP && ARM64) || COMPILE_TEST
+	default ARCH_ROCKCHIP && ARM64
+
+config COMMON_CLK_RK3368
+	bool "Clock driver for Rockchip RK3368"
+	depends on (ARCH_ROCKCHIP && ARM64) || COMPILE_TEST
+	default ARCH_ROCKCHIP && ARM64
+
+config COMMON_CLK_RK3399
+	bool "Clock driver for Rockchip RK3399"
+	depends on (ARCH_ROCKCHIP && ARM64) || COMPILE_TEST
+	default ARCH_ROCKCHIP && ARM64
diff --git a/drivers/clk/rockchip/Makefile b/drivers/clk/rockchip/Makefile
index 26b220c988b2..497b7606217b 100644
--- a/drivers/clk/rockchip/Makefile
+++ b/drivers/clk/rockchip/Makefile
@@ -12,11 +12,11 @@ obj-y	+= clk-muxgrf.o
 obj-y	+= clk-ddr.o
 obj-$(CONFIG_RESET_CONTROLLER)	+= softrst.o
 
-obj-y	+= clk-rv1108.o
-obj-y	+= clk-rk3036.o
-obj-y	+= clk-rk3188.o
-obj-y	+= clk-rk3228.o
-obj-y	+= clk-rk3288.o
-obj-y	+= clk-rk3328.o
-obj-y	+= clk-rk3368.o
-obj-y	+= clk-rk3399.o
+obj-$(CONFIG_COMMON_CLK_RV1108)	+= clk-rv1108.o
+obj-$(CONFIG_COMMON_CLK_RK3036)	+= clk-rk3036.o
+obj-$(CONFIG_COMMON_CLK_RK3188)	+= clk-rk3188.o
+obj-$(CONFIG_COMMON_CLK_RK3228)	+= clk-rk3228.o
+obj-$(CONFIG_COMMON_CLK_RK3288)	+= clk-rk3288.o
+obj-$(CONFIG_COMMON_CLK_RK3328)	+= clk-rk3328.o
+obj-$(CONFIG_COMMON_CLK_RK3368)	+= clk-rk3368.o
+obj-$(CONFIG_COMMON_CLK_RK3399)	+= clk-rk3399.o
diff --git a/drivers/clk/rockchip/clk-rk3228.c b/drivers/clk/rockchip/clk-rk3228.c
index db6e5a9e6de6..d76da6d37c8b 100644
--- a/drivers/clk/rockchip/clk-rk3228.c
+++ b/drivers/clk/rockchip/clk-rk3228.c
@@ -86,25 +86,43 @@ static struct rockchip_pll_rate_table rk3228_pll_rates[] = {
 #define RK3228_DIV_PCLK_MASK		0x7
 #define RK3228_DIV_PCLK_SHIFT		12
 
-#define RK3228_CLKSEL1(_core_peri_div)					\
+#define RK3228_CLKSEL1(_core_aclk_div, _core_peri_div)				\
 	{									\
 		.reg = RK2928_CLKSEL_CON(1),					\
 		.val = HIWORD_UPDATE(_core_peri_div, RK3228_DIV_PERI_MASK,	\
-				RK3228_DIV_PERI_SHIFT)				\
-	}
+				     RK3228_DIV_PERI_SHIFT) |			\
+		       HIWORD_UPDATE(_core_aclk_div, RK3228_DIV_ACLK_MASK,	\
+				     RK3228_DIV_ACLK_SHIFT),			\
+}
 
-#define RK3228_CPUCLK_RATE(_prate, _core_peri_div)			\
-	{								\
-		.prate = _prate,					\
-		.divs = {						\
-			RK3228_CLKSEL1(_core_peri_div),		\
-		},							\
+#define RK3228_CPUCLK_RATE(_prate, _core_aclk_div, _core_peri_div)		\
+	{									\
+		.prate = _prate,						\
+		.divs = {							\
+			RK3228_CLKSEL1(_core_aclk_div, _core_peri_div),		\
+		},								\
 	}
 
 static struct rockchip_cpuclk_rate_table rk3228_cpuclk_rates[] __initdata = {
-	RK3228_CPUCLK_RATE(816000000, 4),
-	RK3228_CPUCLK_RATE(600000000, 4),
-	RK3228_CPUCLK_RATE(312000000, 4),
+	RK3228_CPUCLK_RATE(1800000000, 1, 7),
+	RK3228_CPUCLK_RATE(1704000000, 1, 7),
+	RK3228_CPUCLK_RATE(1608000000, 1, 7),
+	RK3228_CPUCLK_RATE(1512000000, 1, 7),
+	RK3228_CPUCLK_RATE(1488000000, 1, 5),
+	RK3228_CPUCLK_RATE(1416000000, 1, 5),
+	RK3228_CPUCLK_RATE(1392000000, 1, 5),
+	RK3228_CPUCLK_RATE(1296000000, 1, 5),
+	RK3228_CPUCLK_RATE(1200000000, 1, 5),
+	RK3228_CPUCLK_RATE(1104000000, 1, 5),
+	RK3228_CPUCLK_RATE(1008000000, 1, 5),
+	RK3228_CPUCLK_RATE(912000000, 1, 5),
+	RK3228_CPUCLK_RATE(816000000, 1, 3),
+	RK3228_CPUCLK_RATE(696000000, 1, 3),
+	RK3228_CPUCLK_RATE(600000000, 1, 3),
+	RK3228_CPUCLK_RATE(408000000, 1, 1),
+	RK3228_CPUCLK_RATE(312000000, 1, 1),
+	RK3228_CPUCLK_RATE(216000000,  1, 1),
+	RK3228_CPUCLK_RATE(96000000, 1, 1),
 };
 
 static const struct rockchip_cpuclk_reg_data rk3228_cpuclk_data = {
@@ -252,15 +270,15 @@ static struct rockchip_clk_branch rk3228_clk_branches[] __initdata = {
 			RK2928_CLKGATE_CON(0), 1, GFLAGS),
 	COMPOSITE_NOGATE(0, "aclk_cpu_src", mux_aclk_cpu_src_p, 0,
 			RK2928_CLKSEL_CON(0), 13, 2, MFLAGS, 8, 5, DFLAGS),
-	GATE(ARMCLK, "aclk_cpu", "aclk_cpu_src", 0,
+	GATE(ACLK_CPU, "aclk_cpu", "aclk_cpu_src", 0,
 			RK2928_CLKGATE_CON(6), 0, GFLAGS),
-	COMPOSITE_NOMUX(0, "hclk_cpu", "aclk_cpu_src", 0,
+	COMPOSITE_NOMUX(HCLK_CPU, "hclk_cpu", "aclk_cpu_src", 0,
 			RK2928_CLKSEL_CON(1), 8, 2, DFLAGS,
 			RK2928_CLKGATE_CON(6), 1, GFLAGS),
 	COMPOSITE_NOMUX(0, "pclk_bus_src", "aclk_cpu_src", 0,
 			RK2928_CLKSEL_CON(1), 12, 3, DFLAGS,
 			RK2928_CLKGATE_CON(6), 2, GFLAGS),
-	GATE(0, "pclk_cpu", "pclk_bus_src", 0,
+	GATE(PCLK_CPU, "pclk_cpu", "pclk_bus_src", 0,
 			RK2928_CLKGATE_CON(6), 3, GFLAGS),
 	GATE(0, "pclk_phy_pre", "pclk_bus_src", 0,
 			RK2928_CLKGATE_CON(6), 4, GFLAGS),
@@ -268,58 +286,58 @@ static struct rockchip_clk_branch rk3228_clk_branches[] __initdata = {
 			RK2928_CLKGATE_CON(6), 13, GFLAGS),
 
 	/* PD_VIDEO */
-	COMPOSITE(0, "aclk_vpu_pre", mux_pll_src_4plls_p, 0,
+	COMPOSITE(ACLK_VPU_PRE, "aclk_vpu_pre", mux_pll_src_4plls_p, 0,
 			RK2928_CLKSEL_CON(32), 5, 2, MFLAGS, 0, 5, DFLAGS,
 			RK2928_CLKGATE_CON(3), 11, GFLAGS),
-	FACTOR_GATE(0, "hclk_vpu_pre", "aclk_vpu_pre", 0, 1, 4,
+	FACTOR_GATE(HCLK_VPU_PRE, "hclk_vpu_pre", "aclk_vpu_pre", 0, 1, 4,
 			RK2928_CLKGATE_CON(4), 4, GFLAGS),
 
-	COMPOSITE(0, "aclk_rkvdec_pre", mux_pll_src_4plls_p, 0,
+	COMPOSITE(ACLK_RKVDEC_PRE, "aclk_rkvdec_pre", mux_pll_src_4plls_p, 0,
 			RK2928_CLKSEL_CON(28), 6, 2, MFLAGS, 0, 5, DFLAGS,
 			RK2928_CLKGATE_CON(3), 2, GFLAGS),
-	FACTOR_GATE(0, "hclk_rkvdec_pre", "aclk_rkvdec_pre", 0, 1, 4,
+	FACTOR_GATE(HCLK_RKVDEC_PRE, "hclk_rkvdec_pre", "aclk_rkvdec_pre", 0, 1, 4,
 			RK2928_CLKGATE_CON(4), 5, GFLAGS),
 
-	COMPOSITE(0, "sclk_vdec_cabac", mux_pll_src_4plls_p, 0,
+	COMPOSITE(SCLK_VDEC_CABAC, "sclk_vdec_cabac", mux_pll_src_4plls_p, 0,
 			RK2928_CLKSEL_CON(28), 14, 2, MFLAGS, 8, 5, DFLAGS,
 			RK2928_CLKGATE_CON(3), 3, GFLAGS),
 
-	COMPOSITE(0, "sclk_vdec_core", mux_pll_src_4plls_p, 0,
+	COMPOSITE(SCLK_VDEC_CORE, "sclk_vdec_core", mux_pll_src_4plls_p, 0,
 			RK2928_CLKSEL_CON(34), 13, 2, MFLAGS, 8, 5, DFLAGS,
 			RK2928_CLKGATE_CON(3), 4, GFLAGS),
 
 	/* PD_VIO */
-	COMPOSITE(0, "aclk_iep_pre", mux_pll_src_4plls_p, 0,
+	COMPOSITE(ACLK_IEP_PRE, "aclk_iep_pre", mux_pll_src_4plls_p, 0,
 			RK2928_CLKSEL_CON(31), 5, 2, MFLAGS, 0, 5, DFLAGS,
 			RK2928_CLKGATE_CON(3), 0, GFLAGS),
-	DIV(0, "hclk_vio_pre", "aclk_iep_pre", 0,
+	DIV(HCLK_VIO_PRE, "hclk_vio_pre", "aclk_iep_pre", 0,
 			RK2928_CLKSEL_CON(2), 0, 5, DFLAGS),
 
-	COMPOSITE(0, "aclk_hdcp_pre", mux_pll_src_4plls_p, 0,
+	COMPOSITE(ACLK_HDCP_PRE, "aclk_hdcp_pre", mux_pll_src_4plls_p, 0,
 			RK2928_CLKSEL_CON(31), 13, 2, MFLAGS, 8, 5, DFLAGS,
 			RK2928_CLKGATE_CON(1), 4, GFLAGS),
 
 	MUX(0, "sclk_rga_src", mux_pll_src_4plls_p, 0,
 			RK2928_CLKSEL_CON(33), 13, 2, MFLAGS),
-	COMPOSITE_NOMUX(0, "aclk_rga_pre", "sclk_rga_src", 0,
+	COMPOSITE_NOMUX(ACLK_RGA_PRE, "aclk_rga_pre", "sclk_rga_src", 0,
 			RK2928_CLKSEL_CON(33), 8, 5, DFLAGS,
 			RK2928_CLKGATE_CON(1), 2, GFLAGS),
-	COMPOSITE(0, "sclk_rga", mux_sclk_rga_p, 0,
+	COMPOSITE(SCLK_RGA, "sclk_rga", mux_sclk_rga_p, 0,
 			RK2928_CLKSEL_CON(22), 5, 2, MFLAGS, 0, 5, DFLAGS,
 			RK2928_CLKGATE_CON(3), 6, GFLAGS),
 
-	COMPOSITE(0, "aclk_vop_pre", mux_pll_src_4plls_p, 0,
+	COMPOSITE(ACLK_VOP_PRE, "aclk_vop_pre", mux_pll_src_4plls_p, 0,
 			RK2928_CLKSEL_CON(33), 5, 2, MFLAGS, 0, 5, DFLAGS,
 			RK2928_CLKGATE_CON(1), 1, GFLAGS),
 
-	COMPOSITE(0, "sclk_hdcp", mux_pll_src_3plls_p, 0,
+	COMPOSITE(SCLK_HDCP, "sclk_hdcp", mux_pll_src_3plls_p, 0,
 			RK2928_CLKSEL_CON(23), 14, 2, MFLAGS, 8, 6, DFLAGS,
 			RK2928_CLKGATE_CON(3), 5, GFLAGS),
 
 	GATE(SCLK_HDMI_HDCP, "sclk_hdmi_hdcp", "xin24m", 0,
 			RK2928_CLKGATE_CON(3), 7, GFLAGS),
 
-	COMPOSITE(0, "sclk_hdmi_cec", mux_sclk_hdmi_cec_p, 0,
+	COMPOSITE(SCLK_HDMI_CEC, "sclk_hdmi_cec", mux_sclk_hdmi_cec_p, 0,
 			RK2928_CLKSEL_CON(21), 14, 2, MFLAGS, 0, 14, DFLAGS,
 			RK2928_CLKGATE_CON(3), 8, GFLAGS),
 
@@ -354,18 +372,18 @@ static struct rockchip_clk_branch rk3228_clk_branches[] __initdata = {
 	GATE(SCLK_TIMER5, "sclk_timer5", "xin24m", 0,
 			RK2928_CLKGATE_CON(6), 10, GFLAGS),
 
-	COMPOSITE(0, "sclk_crypto", mux_pll_src_2plls_p, 0,
+	COMPOSITE(SCLK_CRYPTO, "sclk_crypto", mux_pll_src_2plls_p, 0,
 			RK2928_CLKSEL_CON(24), 5, 1, MFLAGS, 0, 5, DFLAGS,
 			RK2928_CLKGATE_CON(2), 7, GFLAGS),
 
-	COMPOSITE(0, "sclk_tsp", mux_pll_src_2plls_p, 0,
+	COMPOSITE(SCLK_TSP, "sclk_tsp", mux_pll_src_2plls_p, 0,
 			RK2928_CLKSEL_CON(22), 15, 1, MFLAGS, 8, 5, DFLAGS,
 			RK2928_CLKGATE_CON(2), 6, GFLAGS),
 
-	GATE(0, "sclk_hsadc", "ext_hsadc", 0,
+	GATE(SCLK_HSADC, "sclk_hsadc", "ext_hsadc", 0,
 			RK2928_CLKGATE_CON(10), 12, GFLAGS),
 
-	COMPOSITE(0, "sclk_wifi", mux_pll_src_cpll_gpll_usb480m_p, 0,
+	COMPOSITE(SCLK_WIFI, "sclk_wifi", mux_pll_src_cpll_gpll_usb480m_p, 0,
 			RK2928_CLKSEL_CON(23), 5, 2, MFLAGS, 0, 6, DFLAGS,
 			RK2928_CLKGATE_CON(2), 15, GFLAGS),
 
@@ -448,9 +466,9 @@ static struct rockchip_clk_branch rk3228_clk_branches[] __initdata = {
 	GATE(0, "jtag", "ext_jtag", 0,
 			RK2928_CLKGATE_CON(1), 3, GFLAGS),
 
-	GATE(0, "sclk_otgphy0", "xin24m", 0,
+	GATE(SCLK_OTGPHY0, "sclk_otgphy0", "xin24m", 0,
 			RK2928_CLKGATE_CON(1), 5, GFLAGS),
-	GATE(0, "sclk_otgphy1", "xin24m", 0,
+	GATE(SCLK_OTGPHY1, "sclk_otgphy1", "xin24m", 0,
 			RK2928_CLKGATE_CON(1), 6, GFLAGS),
 
 	COMPOSITE_NOMUX(SCLK_TSADC, "sclk_tsadc", "xin24m", 0,
@@ -526,28 +544,28 @@ static struct rockchip_clk_branch rk3228_clk_branches[] __initdata = {
 	 */
 
 	/* PD_VOP */
-	GATE(0, "aclk_rga", "aclk_rga_pre", 0, RK2928_CLKGATE_CON(13), 0, GFLAGS),
+	GATE(ACLK_RGA, "aclk_rga", "aclk_rga_pre", 0, RK2928_CLKGATE_CON(13), 0, GFLAGS),
 	GATE(0, "aclk_rga_noc", "aclk_rga_pre", 0, RK2928_CLKGATE_CON(13), 11, GFLAGS),
-	GATE(0, "aclk_iep", "aclk_iep_pre", 0, RK2928_CLKGATE_CON(13), 2, GFLAGS),
+	GATE(ACLK_IEP, "aclk_iep", "aclk_iep_pre", 0, RK2928_CLKGATE_CON(13), 2, GFLAGS),
 	GATE(0, "aclk_iep_noc", "aclk_iep_pre", 0, RK2928_CLKGATE_CON(13), 9, GFLAGS),
 
 	GATE(ACLK_VOP, "aclk_vop", "aclk_vop_pre", 0, RK2928_CLKGATE_CON(13), 5, GFLAGS),
 	GATE(0, "aclk_vop_noc", "aclk_vop_pre", 0, RK2928_CLKGATE_CON(13), 12, GFLAGS),
 
-	GATE(0, "aclk_hdcp", "aclk_hdcp_pre", 0, RK2928_CLKGATE_CON(14), 10, GFLAGS),
+	GATE(ACLK_HDCP, "aclk_hdcp", "aclk_hdcp_pre", 0, RK2928_CLKGATE_CON(14), 10, GFLAGS),
 	GATE(0, "aclk_hdcp_noc", "aclk_hdcp_pre", 0, RK2928_CLKGATE_CON(13), 10, GFLAGS),
 
-	GATE(0, "hclk_rga", "hclk_vio_pre", 0, RK2928_CLKGATE_CON(13), 1, GFLAGS),
-	GATE(0, "hclk_iep", "hclk_vio_pre", 0, RK2928_CLKGATE_CON(13), 3, GFLAGS),
+	GATE(HCLK_RGA, "hclk_rga", "hclk_vio_pre", 0, RK2928_CLKGATE_CON(13), 1, GFLAGS),
+	GATE(HCLK_IEP, "hclk_iep", "hclk_vio_pre", 0, RK2928_CLKGATE_CON(13), 3, GFLAGS),
 	GATE(HCLK_VOP, "hclk_vop", "hclk_vio_pre", 0, RK2928_CLKGATE_CON(13), 6, GFLAGS),
 	GATE(0, "hclk_vio_ahb_arbi", "hclk_vio_pre", 0, RK2928_CLKGATE_CON(13), 7, GFLAGS),
 	GATE(0, "hclk_vio_noc", "hclk_vio_pre", 0, RK2928_CLKGATE_CON(13), 8, GFLAGS),
 	GATE(0, "hclk_vop_noc", "hclk_vio_pre", 0, RK2928_CLKGATE_CON(13), 13, GFLAGS),
-	GATE(0, "hclk_vio_h2p", "hclk_vio_pre", 0, RK2928_CLKGATE_CON(14), 7, GFLAGS),
-	GATE(0, "hclk_hdcp_mmu", "hclk_vio_pre", 0, RK2928_CLKGATE_CON(14), 12, GFLAGS),
+	GATE(HCLK_VIO_H2P, "hclk_vio_h2p", "hclk_vio_pre", 0, RK2928_CLKGATE_CON(14), 7, GFLAGS),
+	GATE(HCLK_HDCP_MMU, "hclk_hdcp_mmu", "hclk_vio_pre", 0, RK2928_CLKGATE_CON(14), 12, GFLAGS),
 	GATE(PCLK_HDMI_CTRL, "pclk_hdmi_ctrl", "hclk_vio_pre", 0, RK2928_CLKGATE_CON(14), 6, GFLAGS),
-	GATE(0, "pclk_vio_h2p", "hclk_vio_pre", 0, RK2928_CLKGATE_CON(14), 8, GFLAGS),
-	GATE(0, "pclk_hdcp", "hclk_vio_pre", 0, RK2928_CLKGATE_CON(14), 11, GFLAGS),
+	GATE(PCLK_VIO_H2P, "pclk_vio_h2p", "hclk_vio_pre", 0, RK2928_CLKGATE_CON(14), 8, GFLAGS),
+	GATE(PCLK_HDCP, "pclk_hdcp", "hclk_vio_pre", 0, RK2928_CLKGATE_CON(14), 11, GFLAGS),
 
 	/* PD_PERI */
 	GATE(0, "aclk_peri_noc", "aclk_peri", CLK_IGNORE_UNUSED, RK2928_CLKGATE_CON(12), 0, GFLAGS),
@@ -557,12 +575,12 @@ static struct rockchip_clk_branch rk3228_clk_branches[] __initdata = {
 	GATE(HCLK_SDIO, "hclk_sdio", "hclk_peri", 0, RK2928_CLKGATE_CON(11), 1, GFLAGS),
 	GATE(HCLK_EMMC, "hclk_emmc", "hclk_peri", 0, RK2928_CLKGATE_CON(11), 2, GFLAGS),
 	GATE(HCLK_NANDC, "hclk_nandc", "hclk_peri", 0, RK2928_CLKGATE_CON(11), 3, GFLAGS),
-	GATE(0, "hclk_host0", "hclk_peri", 0, RK2928_CLKGATE_CON(11), 6, GFLAGS),
+	GATE(HCLK_HOST0, "hclk_host0", "hclk_peri", 0, RK2928_CLKGATE_CON(11), 6, GFLAGS),
 	GATE(0, "hclk_host0_arb", "hclk_peri", 0, RK2928_CLKGATE_CON(11), 7, GFLAGS),
-	GATE(0, "hclk_host1", "hclk_peri", 0, RK2928_CLKGATE_CON(11), 8, GFLAGS),
+	GATE(HCLK_HOST1, "hclk_host1", "hclk_peri", 0, RK2928_CLKGATE_CON(11), 8, GFLAGS),
 	GATE(0, "hclk_host1_arb", "hclk_peri", 0, RK2928_CLKGATE_CON(11), 9, GFLAGS),
-	GATE(0, "hclk_host2", "hclk_peri", 0, RK2928_CLKGATE_CON(11), 10, GFLAGS),
-	GATE(0, "hclk_otg", "hclk_peri", 0, RK2928_CLKGATE_CON(11), 12, GFLAGS),
+	GATE(HCLK_HOST2, "hclk_host2", "hclk_peri", 0, RK2928_CLKGATE_CON(11), 10, GFLAGS),
+	GATE(HCLK_OTG, "hclk_otg", "hclk_peri", 0, RK2928_CLKGATE_CON(11), 12, GFLAGS),
 	GATE(0, "hclk_otg_pmu", "hclk_peri", 0, RK2928_CLKGATE_CON(11), 13, GFLAGS),
 	GATE(0, "hclk_host2_arb", "hclk_peri", 0, RK2928_CLKGATE_CON(11), 14, GFLAGS),
 	GATE(0, "hclk_peri_noc", "hclk_peri", CLK_IGNORE_UNUSED, RK2928_CLKGATE_CON(12), 1, GFLAGS),
@@ -571,7 +589,7 @@ static struct rockchip_clk_branch rk3228_clk_branches[] __initdata = {
 	GATE(0, "pclk_peri_noc", "pclk_peri", CLK_IGNORE_UNUSED, RK2928_CLKGATE_CON(12), 2, GFLAGS),
 
 	/* PD_GPU */
-	GATE(0, "aclk_gpu", "aclk_gpu_pre", 0, RK2928_CLKGATE_CON(13), 14, GFLAGS),
+	GATE(ACLK_GPU, "aclk_gpu", "aclk_gpu_pre", 0, RK2928_CLKGATE_CON(13), 14, GFLAGS),
 	GATE(0, "aclk_gpu_noc", "aclk_gpu_pre", 0, RK2928_CLKGATE_CON(13), 15, GFLAGS),
 
 	/* PD_BUS */
@@ -585,16 +603,16 @@ static struct rockchip_clk_branch rk3228_clk_branches[] __initdata = {
 	GATE(HCLK_I2S1_8CH, "hclk_i2s1_8ch", "hclk_cpu", 0, RK2928_CLKGATE_CON(8), 8, GFLAGS),
 	GATE(HCLK_I2S2_2CH, "hclk_i2s2_2ch", "hclk_cpu", 0, RK2928_CLKGATE_CON(8), 9, GFLAGS),
 	GATE(HCLK_SPDIF_8CH, "hclk_spdif_8ch", "hclk_cpu", 0, RK2928_CLKGATE_CON(8), 10, GFLAGS),
-	GATE(0, "hclk_tsp", "hclk_cpu", 0, RK2928_CLKGATE_CON(10), 11, GFLAGS),
-	GATE(0, "hclk_crypto_mst", "hclk_cpu", 0, RK2928_CLKGATE_CON(8), 11, GFLAGS),
-	GATE(0, "hclk_crypto_slv", "hclk_cpu", 0, RK2928_CLKGATE_CON(8), 12, GFLAGS),
+	GATE(HCLK_TSP, "hclk_tsp", "hclk_cpu", 0, RK2928_CLKGATE_CON(10), 11, GFLAGS),
+	GATE(HCLK_M_CRYPTO, "hclk_crypto_mst", "hclk_cpu", 0, RK2928_CLKGATE_CON(8), 11, GFLAGS),
+	GATE(HCLK_S_CRYPTO, "hclk_crypto_slv", "hclk_cpu", 0, RK2928_CLKGATE_CON(8), 12, GFLAGS),
 
 	GATE(0, "pclk_ddrupctl", "pclk_ddr_pre", 0, RK2928_CLKGATE_CON(8), 4, GFLAGS),
 	GATE(0, "pclk_ddrmon", "pclk_ddr_pre", 0, RK2928_CLKGATE_CON(8), 6, GFLAGS),
 	GATE(0, "pclk_msch_noc", "pclk_ddr_pre", 0, RK2928_CLKGATE_CON(10), 2, GFLAGS),
 
-	GATE(0, "pclk_efuse_1024", "pclk_cpu", 0, RK2928_CLKGATE_CON(8), 13, GFLAGS),
-	GATE(0, "pclk_efuse_256", "pclk_cpu", 0, RK2928_CLKGATE_CON(8), 14, GFLAGS),
+	GATE(PCLK_EFUSE_1024, "pclk_efuse_1024", "pclk_cpu", 0, RK2928_CLKGATE_CON(8), 13, GFLAGS),
+	GATE(PCLK_EFUSE_256, "pclk_efuse_256", "pclk_cpu", 0, RK2928_CLKGATE_CON(8), 14, GFLAGS),
 	GATE(PCLK_I2C0, "pclk_i2c0", "pclk_cpu", 0, RK2928_CLKGATE_CON(8), 15, GFLAGS),
 	GATE(PCLK_I2C1, "pclk_i2c1", "pclk_cpu", 0, RK2928_CLKGATE_CON(9), 0, GFLAGS),
 	GATE(PCLK_I2C2, "pclk_i2c2", "pclk_cpu", 0, RK2928_CLKGATE_CON(9), 1, GFLAGS),
@@ -622,13 +640,13 @@ static struct rockchip_clk_branch rk3228_clk_branches[] __initdata = {
 	GATE(0, "pclk_vdacphy", "pclk_phy_pre", 0, RK2928_CLKGATE_CON(10), 8, GFLAGS),
 	GATE(0, "pclk_phy_noc", "pclk_phy_pre", 0, RK2928_CLKGATE_CON(10), 9, GFLAGS),
 
-	GATE(0, "aclk_vpu", "aclk_vpu_pre", 0, RK2928_CLKGATE_CON(15), 0, GFLAGS),
+	GATE(ACLK_VPU, "aclk_vpu", "aclk_vpu_pre", 0, RK2928_CLKGATE_CON(15), 0, GFLAGS),
 	GATE(0, "aclk_vpu_noc", "aclk_vpu_pre", 0, RK2928_CLKGATE_CON(15), 4, GFLAGS),
-	GATE(0, "aclk_rkvdec", "aclk_rkvdec_pre", 0, RK2928_CLKGATE_CON(15), 2, GFLAGS),
+	GATE(ACLK_RKVDEC, "aclk_rkvdec", "aclk_rkvdec_pre", 0, RK2928_CLKGATE_CON(15), 2, GFLAGS),
 	GATE(0, "aclk_rkvdec_noc", "aclk_rkvdec_pre", 0, RK2928_CLKGATE_CON(15), 6, GFLAGS),
-	GATE(0, "hclk_vpu", "hclk_vpu_pre", 0, RK2928_CLKGATE_CON(15), 1, GFLAGS),
+	GATE(HCLK_VPU, "hclk_vpu", "hclk_vpu_pre", 0, RK2928_CLKGATE_CON(15), 1, GFLAGS),
 	GATE(0, "hclk_vpu_noc", "hclk_vpu_pre", 0, RK2928_CLKGATE_CON(15), 5, GFLAGS),
-	GATE(0, "hclk_rkvdec", "hclk_rkvdec_pre", 0, RK2928_CLKGATE_CON(15), 3, GFLAGS),
+	GATE(HCLK_RKVDEC, "hclk_rkvdec", "hclk_rkvdec_pre", 0, RK2928_CLKGATE_CON(15), 3, GFLAGS),
 	GATE(0, "hclk_rkvdec_noc", "hclk_rkvdec_pre", 0, RK2928_CLKGATE_CON(15), 7, GFLAGS),
 
 	/* PD_MMC */
diff --git a/drivers/clk/rockchip/clk-rk3399.c b/drivers/clk/rockchip/clk-rk3399.c
index fa3cbef08776..6847120b61cd 100644
--- a/drivers/clk/rockchip/clk-rk3399.c
+++ b/drivers/clk/rockchip/clk-rk3399.c
@@ -1066,13 +1066,13 @@ static struct rockchip_clk_branch rk3399_clk_branches[] __initdata = {
 	/* cif_testout */
 	MUX(0, "clk_testout1_pll_src", mux_pll_src_cpll_gpll_npll_p, 0,
 			RK3399_CLKSEL_CON(38), 6, 2, MFLAGS),
-	COMPOSITE(0, "clk_testout1", mux_clk_testout1_p, 0,
+	COMPOSITE(SCLK_TESTCLKOUT1, "clk_testout1", mux_clk_testout1_p, 0,
 			RK3399_CLKSEL_CON(38), 5, 1, MFLAGS, 0, 5, DFLAGS,
 			RK3399_CLKGATE_CON(13), 14, GFLAGS),
 
 	MUX(0, "clk_testout2_pll_src", mux_pll_src_cpll_gpll_npll_p, 0,
 			RK3399_CLKSEL_CON(38), 14, 2, MFLAGS),
-	COMPOSITE(0, "clk_testout2", mux_clk_testout2_p, 0,
+	COMPOSITE(SCLK_TESTCLKOUT2, "clk_testout2", mux_clk_testout2_p, 0,
 			RK3399_CLKSEL_CON(38), 13, 1, MFLAGS, 8, 5, DFLAGS,
 			RK3399_CLKGATE_CON(13), 15, GFLAGS),
 
diff --git a/drivers/clk/sunxi-ng/Kconfig b/drivers/clk/sunxi-ng/Kconfig
index b0d551a8efe4..67acef3d2494 100644
--- a/drivers/clk/sunxi-ng/Kconfig
+++ b/drivers/clk/sunxi-ng/Kconfig
@@ -116,6 +116,18 @@ config SUN8I_A33_CCU
 	default MACH_SUN8I
 	depends on MACH_SUN8I || COMPILE_TEST
 
+config SUN8I_A83T_CCU
+	bool "Support for the Allwinner A83T CCU"
+	select SUNXI_CCU_DIV
+	select SUNXI_CCU_GATE
+	select SUNXI_CCU_MP
+	select SUNXI_CCU_MULT
+	select SUNXI_CCU_MUX
+	select SUNXI_CCU_NKMP
+	select SUNXI_CCU_NM
+	select SUNXI_CCU_PHASE
+	default MACH_SUN8I
+
 config SUN8I_H3_CCU
 	bool "Support for the Allwinner H3 CCU"
 	select SUNXI_CCU_DIV
@@ -140,6 +152,11 @@ config SUN8I_V3S_CCU
 	default MACH_SUN8I
 	depends on MACH_SUN8I || COMPILE_TEST
 
+config SUN8I_DE2_CCU
+	bool "Support for the Allwinner SoCs DE2 CCU"
+	select SUNXI_CCU_DIV
+	select SUNXI_CCU_GATE
+
 config SUN9I_A80_CCU
 	bool "Support for the Allwinner A80 CCU"
 	select SUNXI_CCU_DIV
@@ -156,6 +173,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/Makefile b/drivers/clk/sunxi-ng/Makefile
index 0ec02fe14c50..0185c6ffadcb 100644
--- a/drivers/clk/sunxi-ng/Makefile
+++ b/drivers/clk/sunxi-ng/Makefile
@@ -23,8 +23,10 @@ obj-$(CONFIG_SUN5I_CCU)		+= ccu-sun5i.o
 obj-$(CONFIG_SUN6I_A31_CCU)	+= ccu-sun6i-a31.o
 obj-$(CONFIG_SUN8I_A23_CCU)	+= ccu-sun8i-a23.o
 obj-$(CONFIG_SUN8I_A33_CCU)	+= ccu-sun8i-a33.o
+obj-$(CONFIG_SUN8I_A83T_CCU)	+= ccu-sun8i-a83t.o
 obj-$(CONFIG_SUN8I_H3_CCU)	+= ccu-sun8i-h3.o
 obj-$(CONFIG_SUN8I_V3S_CCU)	+= ccu-sun8i-v3s.o
+obj-$(CONFIG_SUN8I_DE2_CCU)	+= ccu-sun8i-de2.o
 obj-$(CONFIG_SUN8I_R_CCU)	+= ccu-sun8i-r.o
 obj-$(CONFIG_SUN9I_A80_CCU)	+= ccu-sun9i-a80.o
 obj-$(CONFIG_SUN9I_A80_CCU)	+= ccu-sun9i-a80-de.o
diff --git a/drivers/clk/sunxi-ng/ccu-sun50i-a64.c b/drivers/clk/sunxi-ng/ccu-sun50i-a64.c
index f54114c607df..2bb4cabf802f 100644
--- a/drivers/clk/sunxi-ng/ccu-sun50i-a64.c
+++ b/drivers/clk/sunxi-ng/ccu-sun50i-a64.c
@@ -211,6 +211,9 @@ static SUNXI_CCU_M(axi_clk, "axi", "cpux", 0x050, 0, 2, 0);
 
 static const char * const ahb1_parents[] = { "osc32k", "osc24M",
 					     "axi", "pll-periph0" };
+static const struct ccu_mux_var_prediv ahb1_predivs[] = {
+	{ .index = 3, .shift = 6, .width = 2 },
+};
 static struct ccu_div ahb1_clk = {
 	.div		= _SUNXI_CCU_DIV_FLAGS(4, 2, CLK_DIVIDER_POWER_OF_TWO),
 
@@ -218,11 +221,8 @@ static struct ccu_div ahb1_clk = {
 		.shift	= 12,
 		.width	= 2,
 
-		.variable_prediv	= {
-			.index	= 3,
-			.shift	= 6,
-			.width	= 2,
-		},
+		.var_predivs	= ahb1_predivs,
+		.n_var_predivs	= ARRAY_SIZE(ahb1_predivs),
 	},
 
 	.common		= {
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-sun5i.h b/drivers/clk/sunxi-ng/ccu-sun5i.h
index 8144487eb7ca..93a275fbd9a9 100644
--- a/drivers/clk/sunxi-ng/ccu-sun5i.h
+++ b/drivers/clk/sunxi-ng/ccu-sun5i.h
@@ -28,15 +28,17 @@
 #define CLK_PLL_AUDIO_4X	6
 #define CLK_PLL_AUDIO_8X	7
 #define CLK_PLL_VIDEO0		8
-#define CLK_PLL_VIDEO0_2X	9
+
+/* The PLL_VIDEO0_2X is exported for HDMI */
+
 #define CLK_PLL_VE		10
 #define CLK_PLL_DDR_BASE	11
 #define CLK_PLL_DDR		12
 #define CLK_PLL_DDR_OTHER	13
 #define CLK_PLL_PERIPH		14
 #define CLK_PLL_VIDEO1		15
-#define CLK_PLL_VIDEO1_2X	16
 
+/* The PLL_VIDEO1_2X is exported for HDMI */
 /* The CPU clock is exported */
 
 #define CLK_AXI			18
diff --git a/drivers/clk/sunxi-ng/ccu-sun6i-a31.c b/drivers/clk/sunxi-ng/ccu-sun6i-a31.c
index 89e68d29bf45..4d6078fca9ac 100644
--- a/drivers/clk/sunxi-ng/ccu-sun6i-a31.c
+++ b/drivers/clk/sunxi-ng/ccu-sun6i-a31.c
@@ -195,6 +195,9 @@ static SUNXI_CCU_DIV_TABLE(axi_clk, "axi", "cpu",
 
 static const char * const ahb1_parents[] = { "osc32k", "osc24M",
 					     "axi", "pll-periph" };
+static const struct ccu_mux_var_prediv ahb1_predivs[] = {
+	{ .index = 3, .shift = 6, .width = 2 },
+};
 
 static struct ccu_div ahb1_clk = {
 	.div		= _SUNXI_CCU_DIV_FLAGS(4, 2, CLK_DIVIDER_POWER_OF_TWO),
@@ -203,11 +206,8 @@ static struct ccu_div ahb1_clk = {
 		.shift	= 12,
 		.width	= 2,
 
-		.variable_prediv	= {
-			.index	= 3,
-			.shift	= 6,
-			.width	= 2,
-		},
+		.var_predivs	= ahb1_predivs,
+		.n_var_predivs	= ARRAY_SIZE(ahb1_predivs),
 	},
 
 	.common		= {
@@ -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-a23.c b/drivers/clk/sunxi-ng/ccu-sun8i-a23.c
index 5c6d37bdf247..8a753ed0426d 100644
--- a/drivers/clk/sunxi-ng/ccu-sun8i-a23.c
+++ b/drivers/clk/sunxi-ng/ccu-sun8i-a23.c
@@ -169,6 +169,9 @@ static SUNXI_CCU_M(axi_clk, "axi", "cpux", 0x050, 0, 2, 0);
 
 static const char * const ahb1_parents[] = { "osc32k", "osc24M",
 					     "axi" , "pll-periph" };
+static const struct ccu_mux_var_prediv ahb1_predivs[] = {
+	{ .index = 3, .shift = 6, .width = 2 },
+};
 static struct ccu_div ahb1_clk = {
 	.div		= _SUNXI_CCU_DIV_FLAGS(4, 2, CLK_DIVIDER_POWER_OF_TWO),
 
@@ -176,11 +179,8 @@ static struct ccu_div ahb1_clk = {
 		.shift	= 12,
 		.width	= 2,
 
-		.variable_prediv	= {
-			.index	= 3,
-			.shift	= 6,
-			.width	= 2,
-		},
+		.var_predivs	= ahb1_predivs,
+		.n_var_predivs	= ARRAY_SIZE(ahb1_predivs),
 	},
 
 	.common		= {
diff --git a/drivers/clk/sunxi-ng/ccu-sun8i-a33.c b/drivers/clk/sunxi-ng/ccu-sun8i-a33.c
index 8d38e6510e29..10b38dc46f75 100644
--- a/drivers/clk/sunxi-ng/ccu-sun8i-a33.c
+++ b/drivers/clk/sunxi-ng/ccu-sun8i-a33.c
@@ -180,6 +180,9 @@ static SUNXI_CCU_M(axi_clk, "axi", "cpux", 0x050, 0, 2, 0);
 
 static const char * const ahb1_parents[] = { "osc32k", "osc24M",
 					     "axi" , "pll-periph" };
+static const struct ccu_mux_var_prediv ahb1_predivs[] = {
+	{ .index = 3, .shift = 6, .width = 2 },
+};
 static struct ccu_div ahb1_clk = {
 	.div		= _SUNXI_CCU_DIV_FLAGS(4, 2, CLK_DIVIDER_POWER_OF_TWO),
 
@@ -187,11 +190,8 @@ static struct ccu_div ahb1_clk = {
 		.shift	= 12,
 		.width	= 2,
 
-		.variable_prediv	= {
-			.index	= 3,
-			.shift	= 6,
-			.width	= 2,
-		},
+		.var_predivs	= ahb1_predivs,
+		.n_var_predivs	= ARRAY_SIZE(ahb1_predivs),
 	},
 
 	.common		= {
diff --git a/drivers/clk/sunxi-ng/ccu-sun8i-a83t.c b/drivers/clk/sunxi-ng/ccu-sun8i-a83t.c
new file mode 100644
index 000000000000..947f9f6e05d2
--- /dev/null
+++ b/drivers/clk/sunxi-ng/ccu-sun8i-a83t.c
@@ -0,0 +1,922 @@
+/*
+ * Copyright (c) 2017 Chen-Yu Tsai. All rights reserved.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/clk-provider.h>
+#include <linux/of_address.h>
+#include <linux/platform_device.h>
+
+#include "ccu_common.h"
+#include "ccu_reset.h"
+
+#include "ccu_div.h"
+#include "ccu_gate.h"
+#include "ccu_mp.h"
+#include "ccu_mux.h"
+#include "ccu_nkmp.h"
+#include "ccu_nm.h"
+#include "ccu_phase.h"
+
+#include "ccu-sun8i-a83t.h"
+
+#define CCU_SUN8I_A83T_LOCK_REG	0x20c
+
+/*
+ * The CPU PLLs are actually NP clocks, with P being /1 or /4. However
+ * P should only be used for output frequencies lower than 228 MHz.
+ * Neither mainline Linux, U-boot, nor the vendor BSPs use these.
+ *
+ * For now we can just model it as a multiplier clock, and force P to /1.
+ */
+#define SUN8I_A83T_PLL_C0CPUX_REG	0x000
+#define SUN8I_A83T_PLL_C1CPUX_REG	0x004
+
+static struct ccu_mult pll_c0cpux_clk = {
+	.enable		= BIT(31),
+	.lock		= BIT(0),
+	.mult		= _SUNXI_CCU_MULT_OFFSET_MIN_MAX(8, 8, 0, 12, 0),
+	.common		= {
+		.reg		= SUN8I_A83T_PLL_C0CPUX_REG,
+		.lock_reg	= CCU_SUN8I_A83T_LOCK_REG,
+		.features	= CCU_FEATURE_LOCK_REG,
+		.hw.init	= CLK_HW_INIT("pll-c0cpux", "osc24M",
+					      &ccu_mult_ops,
+					      CLK_SET_RATE_UNGATE),
+	},
+};
+
+static struct ccu_mult pll_c1cpux_clk = {
+	.enable		= BIT(31),
+	.lock		= BIT(1),
+	.mult		= _SUNXI_CCU_MULT_OFFSET_MIN_MAX(8, 8, 0, 12, 0),
+	.common		= {
+		.reg		= SUN8I_A83T_PLL_C1CPUX_REG,
+		.lock_reg	= CCU_SUN8I_A83T_LOCK_REG,
+		.features	= CCU_FEATURE_LOCK_REG,
+		.hw.init	= CLK_HW_INIT("pll-c1cpux", "osc24M",
+					      &ccu_mult_ops,
+					      CLK_SET_RATE_UNGATE),
+	},
+};
+
+/*
+ * The Audio PLL has d1, d2 dividers in addition to the usual N, M
+ * factors. Since we only need 2 frequencies from this PLL: 22.5792 MHz
+ * and 24.576 MHz, ignore them for now. Enforce the default for them,
+ * which is d1 = 0, d2 = 1.
+ */
+#define SUN8I_A83T_PLL_AUDIO_REG	0x008
+
+static struct ccu_nm pll_audio_clk = {
+	.enable		= BIT(31),
+	.lock		= BIT(2),
+	.n		= _SUNXI_CCU_MULT_OFFSET_MIN_MAX(8, 8, 0, 12, 0),
+	.m		= _SUNXI_CCU_DIV(0, 6),
+	.common		= {
+		.reg		= SUN8I_A83T_PLL_AUDIO_REG,
+		.lock_reg	= CCU_SUN8I_A83T_LOCK_REG,
+		.features	= CCU_FEATURE_LOCK_REG,
+		.hw.init	= CLK_HW_INIT("pll-audio", "osc24M",
+					      &ccu_nm_ops, CLK_SET_RATE_UNGATE),
+	},
+};
+
+/* Some PLLs are input * N / div1 / P. Model them as NKMP with no K */
+static struct ccu_nkmp pll_video0_clk = {
+	.enable		= BIT(31),
+	.lock		= BIT(3),
+	.n		= _SUNXI_CCU_MULT_OFFSET_MIN_MAX(8, 8, 0, 12, 0),
+	.m		= _SUNXI_CCU_DIV(16, 1), /* input divider */
+	.p		= _SUNXI_CCU_DIV(0, 2), /* output divider */
+	.common		= {
+		.reg		= 0x010,
+		.lock_reg	= CCU_SUN8I_A83T_LOCK_REG,
+		.features	= CCU_FEATURE_LOCK_REG,
+		.hw.init	= CLK_HW_INIT("pll-video0", "osc24M",
+					      &ccu_nkmp_ops,
+					      CLK_SET_RATE_UNGATE),
+	},
+};
+
+static struct ccu_nkmp pll_ve_clk = {
+	.enable		= BIT(31),
+	.lock		= BIT(4),
+	.n		= _SUNXI_CCU_MULT_OFFSET_MIN_MAX(8, 8, 0, 12, 0),
+	.m		= _SUNXI_CCU_DIV(16, 1), /* input divider */
+	.p		= _SUNXI_CCU_DIV(18, 1), /* output divider */
+	.common		= {
+		.reg		= 0x018,
+		.lock_reg	= CCU_SUN8I_A83T_LOCK_REG,
+		.features	= CCU_FEATURE_LOCK_REG,
+		.hw.init	= CLK_HW_INIT("pll-ve", "osc24M",
+					      &ccu_nkmp_ops,
+					      CLK_SET_RATE_UNGATE),
+	},
+};
+
+static struct ccu_nkmp pll_ddr_clk = {
+	.enable		= BIT(31),
+	.lock		= BIT(5),
+	.n		= _SUNXI_CCU_MULT_MIN(8, 8, 12),
+	.m		= _SUNXI_CCU_DIV(16, 1), /* input divider */
+	.p		= _SUNXI_CCU_DIV(18, 1), /* output divider */
+	.common		= {
+		.reg		= 0x020,
+		.lock_reg	= CCU_SUN8I_A83T_LOCK_REG,
+		.features	= CCU_FEATURE_LOCK_REG,
+		.hw.init	= CLK_HW_INIT("pll-ddr", "osc24M",
+					      &ccu_nkmp_ops,
+					      CLK_SET_RATE_UNGATE),
+	},
+};
+
+static struct ccu_nkmp pll_periph_clk = {
+	.enable		= BIT(31),
+	.lock		= BIT(6),
+	.n		= _SUNXI_CCU_MULT_OFFSET_MIN_MAX(8, 8, 0, 12, 0),
+	.m		= _SUNXI_CCU_DIV(16, 1), /* input divider */
+	.p		= _SUNXI_CCU_DIV(18, 1), /* output divider */
+	.common		= {
+		.reg		= 0x028,
+		.lock_reg	= CCU_SUN8I_A83T_LOCK_REG,
+		.features	= CCU_FEATURE_LOCK_REG,
+		.hw.init	= CLK_HW_INIT("pll-periph", "osc24M",
+					      &ccu_nkmp_ops,
+					      CLK_SET_RATE_UNGATE),
+	},
+};
+
+static struct ccu_nkmp pll_gpu_clk = {
+	.enable		= BIT(31),
+	.lock		= BIT(7),
+	.n		= _SUNXI_CCU_MULT_OFFSET_MIN_MAX(8, 8, 0, 12, 0),
+	.m		= _SUNXI_CCU_DIV(16, 1), /* input divider */
+	.p		= _SUNXI_CCU_DIV(18, 1), /* output divider */
+	.common		= {
+		.reg		= 0x038,
+		.lock_reg	= CCU_SUN8I_A83T_LOCK_REG,
+		.features	= CCU_FEATURE_LOCK_REG,
+		.hw.init	= CLK_HW_INIT("pll-gpu", "osc24M",
+					      &ccu_nkmp_ops,
+					      CLK_SET_RATE_UNGATE),
+	},
+};
+
+static struct ccu_nkmp pll_hsic_clk = {
+	.enable		= BIT(31),
+	.lock		= BIT(8),
+	.n		= _SUNXI_CCU_MULT_OFFSET_MIN_MAX(8, 8, 0, 12, 0),
+	.m		= _SUNXI_CCU_DIV(16, 1), /* input divider */
+	.p		= _SUNXI_CCU_DIV(18, 1), /* output divider */
+	.common		= {
+		.reg		= 0x044,
+		.lock_reg	= CCU_SUN8I_A83T_LOCK_REG,
+		.features	= CCU_FEATURE_LOCK_REG,
+		.hw.init	= CLK_HW_INIT("pll-hsic", "osc24M",
+					      &ccu_nkmp_ops,
+					      CLK_SET_RATE_UNGATE),
+	},
+};
+
+static struct ccu_nkmp pll_de_clk = {
+	.enable		= BIT(31),
+	.lock		= BIT(9),
+	.n		= _SUNXI_CCU_MULT_OFFSET_MIN_MAX(8, 8, 0, 12, 0),
+	.m		= _SUNXI_CCU_DIV(16, 1), /* input divider */
+	.p		= _SUNXI_CCU_DIV(18, 1), /* output divider */
+	.common		= {
+		.reg		= 0x048,
+		.lock_reg	= CCU_SUN8I_A83T_LOCK_REG,
+		.features	= CCU_FEATURE_LOCK_REG,
+		.hw.init	= CLK_HW_INIT("pll-de", "osc24M",
+					      &ccu_nkmp_ops,
+					      CLK_SET_RATE_UNGATE),
+	},
+};
+
+static struct ccu_nkmp pll_video1_clk = {
+	.enable		= BIT(31),
+	.lock		= BIT(10),
+	.n		= _SUNXI_CCU_MULT_OFFSET_MIN_MAX(8, 8, 0, 12, 0),
+	.m		= _SUNXI_CCU_DIV(16, 1), /* input divider */
+	.p		= _SUNXI_CCU_DIV(0, 2), /* external divider p */
+	.common		= {
+		.reg		= 0x04c,
+		.lock_reg	= CCU_SUN8I_A83T_LOCK_REG,
+		.features	= CCU_FEATURE_LOCK_REG,
+		.hw.init	= CLK_HW_INIT("pll-video1", "osc24M",
+					      &ccu_nkmp_ops,
+					      CLK_SET_RATE_UNGATE),
+	},
+};
+
+static const char * const c0cpux_parents[] = { "osc24M", "pll-c0cpux" };
+static SUNXI_CCU_MUX(c0cpux_clk, "c0cpux", c0cpux_parents,
+		     0x50, 12, 1, CLK_SET_RATE_PARENT | CLK_IS_CRITICAL);
+
+static const char * const c1cpux_parents[] = { "osc24M", "pll-c1cpux" };
+static SUNXI_CCU_MUX(c1cpux_clk, "c1cpux", c1cpux_parents,
+		     0x50, 28, 1, CLK_SET_RATE_PARENT | CLK_IS_CRITICAL);
+
+static SUNXI_CCU_M(axi0_clk, "axi0", "c0cpux", 0x050, 0, 2, 0);
+static SUNXI_CCU_M(axi1_clk, "axi1", "c1cpux", 0x050, 16, 2, 0);
+
+static const char * const ahb1_parents[] = { "osc16M-d512", "osc24M",
+					     "pll-periph",
+					     "pll-periph" };
+static const struct ccu_mux_var_prediv ahb1_predivs[] = {
+	{ .index = 2, .shift = 6, .width = 2 },
+	{ .index = 3, .shift = 6, .width = 2 },
+};
+static struct ccu_div ahb1_clk = {
+	.div		= _SUNXI_CCU_DIV_FLAGS(4, 2, CLK_DIVIDER_POWER_OF_TWO),
+	.mux		= {
+		.shift	= 12,
+		.width	= 2,
+
+		.var_predivs	= ahb1_predivs,
+		.n_var_predivs	= ARRAY_SIZE(ahb1_predivs),
+	},
+	.common		= {
+		.reg		= 0x054,
+		.hw.init	= CLK_HW_INIT_PARENTS("ahb1",
+						      ahb1_parents,
+						      &ccu_div_ops,
+						      0),
+	},
+};
+
+static SUNXI_CCU_M(apb1_clk, "apb1", "ahb1", 0x054, 8, 2, 0);
+
+static const char * const apb2_parents[] = { "osc16M-d512", "osc24M",
+					     "pll-periph", "pll-periph" };
+
+static SUNXI_CCU_MP_WITH_MUX(apb2_clk, "apb2", apb2_parents, 0x058,
+			     0, 5,	/* M */
+			     16, 2,	/* P */
+			     24, 2,	/* mux */
+			     0);
+
+static const char * const ahb2_parents[] = { "ahb1", "pll-periph" };
+static const struct ccu_mux_fixed_prediv ahb2_prediv = {
+	.index = 1, .div = 2
+};
+static struct ccu_mux ahb2_clk = {
+	.mux		= {
+		.shift		= 0,
+		.width		= 2,
+		.fixed_predivs	= &ahb2_prediv,
+		.n_predivs	= 1,
+	},
+	.common		= {
+		.reg		= 0x05c,
+		.hw.init	= CLK_HW_INIT_PARENTS("ahb2",
+						      ahb2_parents,
+						      &ccu_mux_ops,
+						      0),
+	},
+};
+
+static SUNXI_CCU_GATE(bus_mipi_dsi_clk,	"bus-mipi-dsi",	"ahb1",
+		      0x060, BIT(1), 0);
+static SUNXI_CCU_GATE(bus_ss_clk,	"bus-ss",	"ahb1",
+		      0x060, BIT(5), 0);
+static SUNXI_CCU_GATE(bus_dma_clk,	"bus-dma",	"ahb1",
+		      0x060, BIT(6), 0);
+static SUNXI_CCU_GATE(bus_mmc0_clk,	"bus-mmc0",	"ahb1",
+		      0x060, BIT(8), 0);
+static SUNXI_CCU_GATE(bus_mmc1_clk,	"bus-mmc1",	"ahb1",
+		      0x060, BIT(9), 0);
+static SUNXI_CCU_GATE(bus_mmc2_clk,	"bus-mmc2",	"ahb1",
+		      0x060, BIT(10), 0);
+static SUNXI_CCU_GATE(bus_nand_clk,	"bus-nand",	"ahb1",
+		      0x060, BIT(13), 0);
+static SUNXI_CCU_GATE(bus_dram_clk,	"bus-dram",	"ahb1",
+		      0x060, BIT(14), 0);
+static SUNXI_CCU_GATE(bus_emac_clk,	"bus-emac",	"ahb2",
+		      0x060, BIT(17), 0);
+static SUNXI_CCU_GATE(bus_hstimer_clk,	"bus-hstimer",	"ahb1",
+		      0x060, BIT(19), 0);
+static SUNXI_CCU_GATE(bus_spi0_clk,	"bus-spi0",	"ahb1",
+		      0x060, BIT(20), 0);
+static SUNXI_CCU_GATE(bus_spi1_clk,	"bus-spi1",	"ahb1",
+		      0x060, BIT(21), 0);
+static SUNXI_CCU_GATE(bus_otg_clk,	"bus-otg",	"ahb1",
+		      0x060, BIT(24), 0);
+static SUNXI_CCU_GATE(bus_ehci0_clk,	"bus-ehci0",	"ahb2",
+		      0x060, BIT(26), 0);
+static SUNXI_CCU_GATE(bus_ehci1_clk,	"bus-ehci1",	"ahb2",
+		      0x060, BIT(27), 0);
+static SUNXI_CCU_GATE(bus_ohci0_clk,	"bus-ohci0",	"ahb2",
+		      0x060, BIT(29), 0);
+
+static SUNXI_CCU_GATE(bus_ve_clk,	"bus-ve",	"ahb1",
+		      0x064, BIT(0), 0);
+static SUNXI_CCU_GATE(bus_tcon0_clk,	"bus-tcon0",	"ahb1",
+		      0x064, BIT(4), 0);
+static SUNXI_CCU_GATE(bus_tcon1_clk,	"bus-tcon1",	"ahb1",
+		      0x064, BIT(5), 0);
+static SUNXI_CCU_GATE(bus_csi_clk,	"bus-csi",	"ahb1",
+		      0x064, BIT(8), 0);
+static SUNXI_CCU_GATE(bus_hdmi_clk,	"bus-hdmi",	"ahb1",
+		      0x064, BIT(11), 0);
+static SUNXI_CCU_GATE(bus_de_clk,	"bus-de",	"ahb1",
+		      0x064, BIT(12), 0);
+static SUNXI_CCU_GATE(bus_gpu_clk,	"bus-gpu",	"ahb1",
+		      0x064, BIT(20), 0);
+static SUNXI_CCU_GATE(bus_msgbox_clk,	"bus-msgbox",	"ahb1",
+		      0x064, BIT(21), 0);
+static SUNXI_CCU_GATE(bus_spinlock_clk,	"bus-spinlock",	"ahb1",
+		      0x064, BIT(22), 0);
+
+static SUNXI_CCU_GATE(bus_spdif_clk,	"bus-spdif",	"apb1",
+		      0x068, BIT(1), 0);
+static SUNXI_CCU_GATE(bus_pio_clk,	"bus-pio",	"apb1",
+		      0x068, BIT(5), 0);
+static SUNXI_CCU_GATE(bus_i2s0_clk,	"bus-i2s0",	"apb1",
+		      0x068, BIT(12), 0);
+static SUNXI_CCU_GATE(bus_i2s1_clk,	"bus-i2s1",	"apb1",
+		      0x068, BIT(13), 0);
+static SUNXI_CCU_GATE(bus_i2s2_clk,	"bus-i2s2",	"apb1",
+		      0x068, BIT(14), 0);
+static SUNXI_CCU_GATE(bus_tdm_clk,	"bus-tdm",	"apb1",
+		      0x068, BIT(15), 0);
+
+static SUNXI_CCU_GATE(bus_i2c0_clk,	"bus-i2c0",	"apb2",
+		      0x06c, BIT(0), 0);
+static SUNXI_CCU_GATE(bus_i2c1_clk,	"bus-i2c1",	"apb2",
+		      0x06c, BIT(0), 0);
+static SUNXI_CCU_GATE(bus_i2c2_clk,	"bus-i2c2",	"apb2",
+		      0x06c, BIT(0), 0);
+static SUNXI_CCU_GATE(bus_uart0_clk,	"bus-uart0",	"apb2",
+		      0x06c, BIT(16), 0);
+static SUNXI_CCU_GATE(bus_uart1_clk,	"bus-uart1",	"apb2",
+		      0x06c, BIT(17), 0);
+static SUNXI_CCU_GATE(bus_uart2_clk,	"bus-uart2",	"apb2",
+		      0x06c, BIT(18), 0);
+static SUNXI_CCU_GATE(bus_uart3_clk,	"bus-uart3",	"apb2",
+		      0x06c, BIT(19), 0);
+static SUNXI_CCU_GATE(bus_uart4_clk,	"bus-uart4",	"apb2",
+		      0x06c, BIT(20), 0);
+
+static const char * const cci400_parents[] = { "osc24M", "pll-periph",
+					       "pll-hsic" };
+static struct ccu_div cci400_clk = {
+	.div		= _SUNXI_CCU_DIV_FLAGS(0, 2, 0),
+	.mux		= _SUNXI_CCU_MUX(24, 2),
+	.common		= {
+		.reg		= 0x078,
+		.hw.init	= CLK_HW_INIT_PARENTS("cci400",
+						      cci400_parents,
+						      &ccu_div_ops,
+						      CLK_IS_CRITICAL),
+	},
+};
+
+static const char * const mod0_default_parents[] = { "osc24M", "pll-periph" };
+
+static SUNXI_CCU_MP_WITH_MUX_GATE(nand_clk, "nand", mod0_default_parents,
+				  0x080,
+				  0, 4,		/* M */
+				  16, 2,	/* P */
+				  24, 2,	/* mux */
+				  BIT(31),	/* gate */
+				  0);
+
+static SUNXI_CCU_MP_WITH_MUX_GATE(mmc0_clk, "mmc0", mod0_default_parents,
+				  0x088,
+				  0, 4,		/* M */
+				  16, 2,	/* P */
+				  24, 2,	/* mux */
+				  BIT(31),	/* gate */
+				  0);
+
+static SUNXI_CCU_PHASE(mmc0_sample_clk, "mmc0-sample", "mmc0",
+		       0x088, 20, 3, 0);
+static SUNXI_CCU_PHASE(mmc0_output_clk, "mmc0-output", "mmc0",
+		       0x088, 8, 3, 0);
+
+static SUNXI_CCU_MP_WITH_MUX_GATE(mmc1_clk, "mmc1", mod0_default_parents,
+				  0x08c,
+				  0, 4,		/* M */
+				  16, 2,	/* P */
+				  24, 2,	/* mux */
+				  BIT(31),	/* gate */
+				  0);
+
+static SUNXI_CCU_PHASE(mmc1_sample_clk, "mmc1-sample", "mmc1",
+		       0x08c, 20, 3, 0);
+static SUNXI_CCU_PHASE(mmc1_output_clk, "mmc1-output", "mmc1",
+		       0x08c, 8, 3, 0);
+
+/* TODO Support MMC2 clock's new timing mode. */
+static SUNXI_CCU_MP_WITH_MUX_GATE(mmc2_clk, "mmc2", mod0_default_parents,
+				  0x090,
+				  0, 4,		/* M */
+				  16, 2,	/* P */
+				  24, 2,	/* mux */
+				  BIT(31),	/* gate */
+				  0);
+
+static SUNXI_CCU_PHASE(mmc2_sample_clk, "mmc2-sample", "mmc2",
+		       0x090, 20, 3, 0);
+static SUNXI_CCU_PHASE(mmc2_output_clk, "mmc2-output", "mmc2",
+		       0x090, 8, 3, 0);
+
+static SUNXI_CCU_MP_WITH_MUX_GATE(ss_clk, "ss", mod0_default_parents,
+				  0x09c,
+				  0, 4,		/* M */
+				  16, 2,	/* P */
+				  24, 2,	/* mux */
+				  BIT(31),	/* gate */
+				  0);
+
+static SUNXI_CCU_MP_WITH_MUX_GATE(spi0_clk, "spi0", mod0_default_parents,
+				  0x0a0,
+				  0, 4,		/* M */
+				  16, 2,	/* P */
+				  24, 4,	/* mux */
+				  BIT(31),	/* gate */
+				  0);
+
+static SUNXI_CCU_MP_WITH_MUX_GATE(spi1_clk, "spi1", mod0_default_parents,
+				  0x0a4,
+				  0, 4,		/* M */
+				  16, 2,	/* P */
+				  24, 4,	/* mux */
+				  BIT(31),	/* gate */
+				  0);
+
+static SUNXI_CCU_M_WITH_GATE(i2s0_clk, "i2s0", "pll-audio",
+			     0x0b0, 0, 4, BIT(31), CLK_SET_RATE_PARENT);
+static SUNXI_CCU_M_WITH_GATE(i2s1_clk, "i2s1", "pll-audio",
+			     0x0b4, 0, 4, BIT(31), CLK_SET_RATE_PARENT);
+static SUNXI_CCU_M_WITH_GATE(i2s2_clk, "i2s2", "pll-audio",
+			     0x0b8, 0, 4, BIT(31), CLK_SET_RATE_PARENT);
+static SUNXI_CCU_M_WITH_GATE(tdm_clk, "tdm", "pll-audio",
+			     0x0bc, 0, 4, BIT(31), CLK_SET_RATE_PARENT);
+static SUNXI_CCU_M_WITH_GATE(spdif_clk, "spdif", "pll-audio",
+			     0x0c0, 0, 4, BIT(31), CLK_SET_RATE_PARENT);
+
+static SUNXI_CCU_GATE(usb_phy0_clk,	"usb-phy0",	"osc24M",
+		      0x0cc, BIT(8), 0);
+static SUNXI_CCU_GATE(usb_phy1_clk,	"usb-phy1",	"osc24M",
+		      0x0cc, BIT(9), 0);
+static SUNXI_CCU_GATE(usb_hsic_clk,	"usb-hsic",	"pll-hsic",
+		      0x0cc, BIT(10), 0);
+static struct ccu_gate usb_hsic_12m_clk = {
+	.enable	= BIT(11),
+	.common	= {
+		.reg		= 0x0cc,
+		.prediv		= 2,
+		.features	= CCU_FEATURE_ALL_PREDIV,
+		.hw.init	= CLK_HW_INIT("usb-hsic-12m", "osc24M",
+					      &ccu_gate_ops, 0),
+	}
+};
+static SUNXI_CCU_GATE(usb_ohci0_clk,	"usb-ohci0",	"osc24M",
+		      0x0cc, BIT(16), 0);
+
+/* TODO divider has minimum of 2 */
+static SUNXI_CCU_M(dram_clk, "dram", "pll-ddr", 0x0f4, 0, 4, CLK_IS_CRITICAL);
+
+static SUNXI_CCU_GATE(dram_ve_clk,	"dram-ve",	"dram",
+		      0x100, BIT(0), 0);
+static SUNXI_CCU_GATE(dram_csi_clk,	"dram-csi",	"dram",
+		      0x100, BIT(1), 0);
+
+static const char * const tcon0_parents[] = { "pll-video0" };
+static SUNXI_CCU_MUX_WITH_GATE(tcon0_clk, "tcon0", tcon0_parents,
+				 0x118, 24, 3, BIT(31), CLK_SET_RATE_PARENT);
+
+static const char * const tcon1_parents[] = { "pll-video1" };
+static SUNXI_CCU_MUX_WITH_GATE(tcon1_clk, "tcon1", tcon1_parents,
+				 0x11c, 24, 3, BIT(31), CLK_SET_RATE_PARENT);
+
+static SUNXI_CCU_GATE(csi_misc_clk, "csi-misc", "osc24M", 0x130, BIT(16), 0);
+
+static SUNXI_CCU_GATE(mipi_csi_clk, "mipi-csi", "osc24M", 0x130, BIT(31), 0);
+
+static const char * const csi_mclk_parents[] = { "pll-de", "osc24M" };
+static const u8 csi_mclk_table[] = { 3, 5 };
+static SUNXI_CCU_M_WITH_MUX_TABLE_GATE(csi_mclk_clk, "csi-mclk",
+				       csi_mclk_parents, csi_mclk_table,
+				       0x134,
+				       0, 5,	/* M */
+				       10, 3,	/* mux */
+				       BIT(15),	/* gate */
+				       0);
+
+static const char * const csi_sclk_parents[] = { "pll-periph", "pll-ve" };
+static const u8 csi_sclk_table[] = { 0, 5 };
+static SUNXI_CCU_M_WITH_MUX_TABLE_GATE(csi_sclk_clk, "csi-sclk",
+				       csi_sclk_parents, csi_sclk_table,
+				       0x134,
+				       16, 4,	/* M */
+				       24, 3,	/* mux */
+				       BIT(31),	/* gate */
+				       0);
+
+static SUNXI_CCU_M_WITH_GATE(ve_clk, "ve", "pll-ve", 0x13c,
+			     16, 3, BIT(31), CLK_SET_RATE_PARENT);
+
+static SUNXI_CCU_GATE(avs_clk, "avs", "osc24M", 0x144, BIT(31), 0);
+
+static const char * const hdmi_parents[] = { "pll-video1" };
+static SUNXI_CCU_M_WITH_MUX_GATE(hdmi_clk, "hdmi", hdmi_parents,
+				 0x150,
+				 0, 4,	/* M */
+				 24, 2,	/* mux */
+				 BIT(31),	/* gate */
+				 CLK_SET_RATE_PARENT);
+
+static SUNXI_CCU_GATE(hdmi_slow_clk, "hdmi-slow", "osc24M", 0x154, BIT(31), 0);
+
+static const char * const mbus_parents[] = { "osc24M", "pll-periph",
+					     "pll-ddr" };
+static SUNXI_CCU_M_WITH_MUX_GATE(mbus_clk, "mbus", mbus_parents,
+				 0x15c,
+				 0, 3,	/* M */
+				 24, 2,	/* mux */
+				 BIT(31),	/* gate */
+				 CLK_IS_CRITICAL);
+
+static const char * const mipi_dsi0_parents[] = { "pll-video0" };
+static const u8 mipi_dsi0_table[] = { 8 };
+static SUNXI_CCU_M_WITH_MUX_TABLE_GATE(mipi_dsi0_clk, "mipi-dsi0",
+				       mipi_dsi0_parents, mipi_dsi0_table,
+				       0x168,
+				       0, 4,	/* M */
+				       24, 4,	/* mux */
+				       BIT(31),	/* gate */
+				       CLK_SET_RATE_PARENT);
+
+static const char * const mipi_dsi1_parents[] = { "osc24M", "pll-video0" };
+static const u8 mipi_dsi1_table[] = { 0, 9 };
+static SUNXI_CCU_M_WITH_MUX_TABLE_GATE(mipi_dsi1_clk, "mipi-dsi1",
+				       mipi_dsi1_parents, mipi_dsi1_table,
+				       0x16c,
+				       0, 4,	/* M */
+				       24, 4,	/* mux */
+				       BIT(31),	/* gate */
+				       CLK_SET_RATE_PARENT);
+
+static SUNXI_CCU_M_WITH_GATE(gpu_core_clk, "gpu-core", "pll-gpu", 0x1a0,
+			     0, 3, BIT(31), CLK_SET_RATE_PARENT);
+
+static const char * const gpu_memory_parents[] = { "pll-gpu", "pll-ddr" };
+static SUNXI_CCU_M_WITH_MUX_GATE(gpu_memory_clk, "gpu-memory",
+				 gpu_memory_parents,
+				 0x1a4,
+				 0, 3,		/* M */
+				 24, 1,		/* mux */
+				 BIT(31),	/* gate */
+				 CLK_SET_RATE_PARENT);
+
+static SUNXI_CCU_M_WITH_GATE(gpu_hyd_clk, "gpu-hyd", "pll-gpu", 0x1a8,
+			     0, 3, BIT(31), CLK_SET_RATE_PARENT);
+
+static struct ccu_common *sun8i_a83t_ccu_clks[] = {
+	&pll_c0cpux_clk.common,
+	&pll_c1cpux_clk.common,
+	&pll_audio_clk.common,
+	&pll_video0_clk.common,
+	&pll_ve_clk.common,
+	&pll_ddr_clk.common,
+	&pll_periph_clk.common,
+	&pll_gpu_clk.common,
+	&pll_hsic_clk.common,
+	&pll_de_clk.common,
+	&pll_video1_clk.common,
+	&c0cpux_clk.common,
+	&c1cpux_clk.common,
+	&axi0_clk.common,
+	&axi1_clk.common,
+	&ahb1_clk.common,
+	&ahb2_clk.common,
+	&apb1_clk.common,
+	&apb2_clk.common,
+	&bus_mipi_dsi_clk.common,
+	&bus_ss_clk.common,
+	&bus_dma_clk.common,
+	&bus_mmc0_clk.common,
+	&bus_mmc1_clk.common,
+	&bus_mmc2_clk.common,
+	&bus_nand_clk.common,
+	&bus_dram_clk.common,
+	&bus_emac_clk.common,
+	&bus_hstimer_clk.common,
+	&bus_spi0_clk.common,
+	&bus_spi1_clk.common,
+	&bus_otg_clk.common,
+	&bus_ehci0_clk.common,
+	&bus_ehci1_clk.common,
+	&bus_ohci0_clk.common,
+	&bus_ve_clk.common,
+	&bus_tcon0_clk.common,
+	&bus_tcon1_clk.common,
+	&bus_csi_clk.common,
+	&bus_hdmi_clk.common,
+	&bus_de_clk.common,
+	&bus_gpu_clk.common,
+	&bus_msgbox_clk.common,
+	&bus_spinlock_clk.common,
+	&bus_spdif_clk.common,
+	&bus_pio_clk.common,
+	&bus_i2s0_clk.common,
+	&bus_i2s1_clk.common,
+	&bus_i2s2_clk.common,
+	&bus_tdm_clk.common,
+	&bus_i2c0_clk.common,
+	&bus_i2c1_clk.common,
+	&bus_i2c2_clk.common,
+	&bus_uart0_clk.common,
+	&bus_uart1_clk.common,
+	&bus_uart2_clk.common,
+	&bus_uart3_clk.common,
+	&bus_uart4_clk.common,
+	&cci400_clk.common,
+	&nand_clk.common,
+	&mmc0_clk.common,
+	&mmc0_sample_clk.common,
+	&mmc0_output_clk.common,
+	&mmc1_clk.common,
+	&mmc1_sample_clk.common,
+	&mmc1_output_clk.common,
+	&mmc2_clk.common,
+	&mmc2_sample_clk.common,
+	&mmc2_output_clk.common,
+	&ss_clk.common,
+	&spi0_clk.common,
+	&spi1_clk.common,
+	&i2s0_clk.common,
+	&i2s1_clk.common,
+	&i2s2_clk.common,
+	&tdm_clk.common,
+	&spdif_clk.common,
+	&usb_phy0_clk.common,
+	&usb_phy1_clk.common,
+	&usb_hsic_clk.common,
+	&usb_hsic_12m_clk.common,
+	&usb_ohci0_clk.common,
+	&dram_clk.common,
+	&dram_ve_clk.common,
+	&dram_csi_clk.common,
+	&tcon0_clk.common,
+	&tcon1_clk.common,
+	&csi_misc_clk.common,
+	&mipi_csi_clk.common,
+	&csi_mclk_clk.common,
+	&csi_sclk_clk.common,
+	&ve_clk.common,
+	&avs_clk.common,
+	&hdmi_clk.common,
+	&hdmi_slow_clk.common,
+	&mbus_clk.common,
+	&mipi_dsi0_clk.common,
+	&mipi_dsi1_clk.common,
+	&gpu_core_clk.common,
+	&gpu_memory_clk.common,
+	&gpu_hyd_clk.common,
+};
+
+static struct clk_hw_onecell_data sun8i_a83t_hw_clks = {
+	.hws	= {
+		[CLK_PLL_C0CPUX]	= &pll_c0cpux_clk.common.hw,
+		[CLK_PLL_C1CPUX]	= &pll_c1cpux_clk.common.hw,
+		[CLK_PLL_AUDIO]		= &pll_audio_clk.common.hw,
+		[CLK_PLL_VIDEO0]	= &pll_video0_clk.common.hw,
+		[CLK_PLL_VE]		= &pll_ve_clk.common.hw,
+		[CLK_PLL_DDR]		= &pll_ddr_clk.common.hw,
+		[CLK_PLL_PERIPH]	= &pll_periph_clk.common.hw,
+		[CLK_PLL_GPU]		= &pll_gpu_clk.common.hw,
+		[CLK_PLL_HSIC]		= &pll_hsic_clk.common.hw,
+		[CLK_PLL_DE]		= &pll_de_clk.common.hw,
+		[CLK_PLL_VIDEO1]	= &pll_video1_clk.common.hw,
+		[CLK_C0CPUX]		= &c0cpux_clk.common.hw,
+		[CLK_C1CPUX]		= &c1cpux_clk.common.hw,
+		[CLK_AXI0]		= &axi0_clk.common.hw,
+		[CLK_AXI1]		= &axi1_clk.common.hw,
+		[CLK_AHB1]		= &ahb1_clk.common.hw,
+		[CLK_AHB2]		= &ahb2_clk.common.hw,
+		[CLK_APB1]		= &apb1_clk.common.hw,
+		[CLK_APB2]		= &apb2_clk.common.hw,
+		[CLK_BUS_MIPI_DSI]	= &bus_mipi_dsi_clk.common.hw,
+		[CLK_BUS_SS]		= &bus_ss_clk.common.hw,
+		[CLK_BUS_DMA]		= &bus_dma_clk.common.hw,
+		[CLK_BUS_MMC0]		= &bus_mmc0_clk.common.hw,
+		[CLK_BUS_MMC1]		= &bus_mmc1_clk.common.hw,
+		[CLK_BUS_MMC2]		= &bus_mmc2_clk.common.hw,
+		[CLK_BUS_NAND]		= &bus_nand_clk.common.hw,
+		[CLK_BUS_DRAM]		= &bus_dram_clk.common.hw,
+		[CLK_BUS_EMAC]		= &bus_emac_clk.common.hw,
+		[CLK_BUS_HSTIMER]	= &bus_hstimer_clk.common.hw,
+		[CLK_BUS_SPI0]		= &bus_spi0_clk.common.hw,
+		[CLK_BUS_SPI1]		= &bus_spi1_clk.common.hw,
+		[CLK_BUS_OTG]		= &bus_otg_clk.common.hw,
+		[CLK_BUS_EHCI0]		= &bus_ehci0_clk.common.hw,
+		[CLK_BUS_EHCI1]		= &bus_ehci1_clk.common.hw,
+		[CLK_BUS_OHCI0]		= &bus_ohci0_clk.common.hw,
+		[CLK_BUS_VE]		= &bus_ve_clk.common.hw,
+		[CLK_BUS_TCON0]		= &bus_tcon0_clk.common.hw,
+		[CLK_BUS_TCON1]		= &bus_tcon1_clk.common.hw,
+		[CLK_BUS_CSI]		= &bus_csi_clk.common.hw,
+		[CLK_BUS_HDMI]		= &bus_hdmi_clk.common.hw,
+		[CLK_BUS_DE]		= &bus_de_clk.common.hw,
+		[CLK_BUS_GPU]		= &bus_gpu_clk.common.hw,
+		[CLK_BUS_MSGBOX]	= &bus_msgbox_clk.common.hw,
+		[CLK_BUS_SPINLOCK]	= &bus_spinlock_clk.common.hw,
+		[CLK_BUS_SPDIF]		= &bus_spdif_clk.common.hw,
+		[CLK_BUS_PIO]		= &bus_pio_clk.common.hw,
+		[CLK_BUS_I2S0]		= &bus_i2s0_clk.common.hw,
+		[CLK_BUS_I2S1]		= &bus_i2s1_clk.common.hw,
+		[CLK_BUS_I2S2]		= &bus_i2s2_clk.common.hw,
+		[CLK_BUS_TDM]		= &bus_tdm_clk.common.hw,
+		[CLK_BUS_I2C0]		= &bus_i2c0_clk.common.hw,
+		[CLK_BUS_I2C1]		= &bus_i2c1_clk.common.hw,
+		[CLK_BUS_I2C2]		= &bus_i2c2_clk.common.hw,
+		[CLK_BUS_UART0]		= &bus_uart0_clk.common.hw,
+		[CLK_BUS_UART1]		= &bus_uart1_clk.common.hw,
+		[CLK_BUS_UART2]		= &bus_uart2_clk.common.hw,
+		[CLK_BUS_UART3]		= &bus_uart3_clk.common.hw,
+		[CLK_BUS_UART4]		= &bus_uart4_clk.common.hw,
+		[CLK_CCI400]		= &cci400_clk.common.hw,
+		[CLK_NAND]		= &nand_clk.common.hw,
+		[CLK_MMC0]		= &mmc0_clk.common.hw,
+		[CLK_MMC0_SAMPLE]	= &mmc0_sample_clk.common.hw,
+		[CLK_MMC0_OUTPUT]	= &mmc0_output_clk.common.hw,
+		[CLK_MMC1]		= &mmc1_clk.common.hw,
+		[CLK_MMC1_SAMPLE]	= &mmc1_sample_clk.common.hw,
+		[CLK_MMC1_OUTPUT]	= &mmc1_output_clk.common.hw,
+		[CLK_MMC2]		= &mmc2_clk.common.hw,
+		[CLK_MMC2_SAMPLE]	= &mmc2_sample_clk.common.hw,
+		[CLK_MMC2_OUTPUT]	= &mmc2_output_clk.common.hw,
+		[CLK_SS]		= &ss_clk.common.hw,
+		[CLK_SPI0]		= &spi0_clk.common.hw,
+		[CLK_SPI1]		= &spi1_clk.common.hw,
+		[CLK_I2S0]		= &i2s0_clk.common.hw,
+		[CLK_I2S1]		= &i2s1_clk.common.hw,
+		[CLK_I2S2]		= &i2s2_clk.common.hw,
+		[CLK_TDM]		= &tdm_clk.common.hw,
+		[CLK_SPDIF]		= &spdif_clk.common.hw,
+		[CLK_USB_PHY0]		= &usb_phy0_clk.common.hw,
+		[CLK_USB_PHY1]		= &usb_phy1_clk.common.hw,
+		[CLK_USB_HSIC]		= &usb_hsic_clk.common.hw,
+		[CLK_USB_HSIC_12M]	= &usb_hsic_12m_clk.common.hw,
+		[CLK_USB_OHCI0]		= &usb_ohci0_clk.common.hw,
+		[CLK_DRAM]		= &dram_clk.common.hw,
+		[CLK_DRAM_VE]		= &dram_ve_clk.common.hw,
+		[CLK_DRAM_CSI]		= &dram_csi_clk.common.hw,
+		[CLK_TCON0]		= &tcon0_clk.common.hw,
+		[CLK_TCON1]		= &tcon1_clk.common.hw,
+		[CLK_CSI_MISC]		= &csi_misc_clk.common.hw,
+		[CLK_MIPI_CSI]		= &mipi_csi_clk.common.hw,
+		[CLK_CSI_MCLK]		= &csi_mclk_clk.common.hw,
+		[CLK_CSI_SCLK]		= &csi_sclk_clk.common.hw,
+		[CLK_VE]		= &ve_clk.common.hw,
+		[CLK_AVS]		= &avs_clk.common.hw,
+		[CLK_HDMI]		= &hdmi_clk.common.hw,
+		[CLK_HDMI_SLOW]		= &hdmi_slow_clk.common.hw,
+		[CLK_MBUS]		= &mbus_clk.common.hw,
+		[CLK_MIPI_DSI0]		= &mipi_dsi0_clk.common.hw,
+		[CLK_MIPI_DSI1]		= &mipi_dsi1_clk.common.hw,
+		[CLK_GPU_CORE]		= &gpu_core_clk.common.hw,
+		[CLK_GPU_MEMORY]	= &gpu_memory_clk.common.hw,
+		[CLK_GPU_HYD]		= &gpu_hyd_clk.common.hw,
+	},
+	.num	= CLK_NUMBER,
+};
+
+static struct ccu_reset_map sun8i_a83t_ccu_resets[] = {
+	[RST_USB_PHY0]		= { 0x0cc, BIT(0) },
+	[RST_USB_PHY1]		= { 0x0cc, BIT(1) },
+	[RST_USB_HSIC]		= { 0x0cc, BIT(2) },
+	[RST_DRAM]		= { 0x0f4, BIT(31) },
+	[RST_MBUS]		= { 0x0fc, BIT(31) },
+	[RST_BUS_MIPI_DSI]	= { 0x2c0, BIT(1) },
+	[RST_BUS_SS]		= { 0x2c0, BIT(5) },
+	[RST_BUS_DMA]		= { 0x2c0, BIT(6) },
+	[RST_BUS_MMC0]		= { 0x2c0, BIT(8) },
+	[RST_BUS_MMC1]		= { 0x2c0, BIT(9) },
+	[RST_BUS_MMC2]		= { 0x2c0, BIT(10) },
+	[RST_BUS_NAND]		= { 0x2c0, BIT(13) },
+	[RST_BUS_DRAM]		= { 0x2c0, BIT(14) },
+	[RST_BUS_EMAC]		= { 0x2c0, BIT(17) },
+	[RST_BUS_HSTIMER]	= { 0x2c0, BIT(19) },
+	[RST_BUS_SPI0]		= { 0x2c0, BIT(20) },
+	[RST_BUS_SPI1]		= { 0x2c0, BIT(21) },
+	[RST_BUS_OTG]		= { 0x2c0, BIT(24) },
+	[RST_BUS_EHCI0]		= { 0x2c0, BIT(26) },
+	[RST_BUS_EHCI1]		= { 0x2c0, BIT(27) },
+	[RST_BUS_OHCI0]		= { 0x2c0, BIT(29) },
+	[RST_BUS_VE]		= { 0x2c4, BIT(0) },
+	[RST_BUS_TCON0]		= { 0x2c4, BIT(4) },
+	[RST_BUS_TCON1]		= { 0x2c4, BIT(5) },
+	[RST_BUS_CSI]		= { 0x2c4, BIT(8) },
+	[RST_BUS_HDMI0]		= { 0x2c4, BIT(10) },
+	[RST_BUS_HDMI1]		= { 0x2c4, BIT(11) },
+	[RST_BUS_DE]		= { 0x2c4, BIT(12) },
+	[RST_BUS_GPU]		= { 0x2c4, BIT(20) },
+	[RST_BUS_MSGBOX]	= { 0x2c4, BIT(21) },
+	[RST_BUS_SPINLOCK]	= { 0x2c4, BIT(22) },
+	[RST_BUS_LVDS]		= { 0x2c8, BIT(0) },
+	[RST_BUS_SPDIF]		= { 0x2d0, BIT(1) },
+	[RST_BUS_I2S0]		= { 0x2d0, BIT(12) },
+	[RST_BUS_I2S1]		= { 0x2d0, BIT(13) },
+	[RST_BUS_I2S2]		= { 0x2d0, BIT(14) },
+	[RST_BUS_TDM]		= { 0x2d0, BIT(15) },
+	[RST_BUS_I2C0]		= { 0x2d8, BIT(0) },
+	[RST_BUS_I2C1]		= { 0x2d8, BIT(1) },
+	[RST_BUS_I2C2]		= { 0x2d8, BIT(2) },
+	[RST_BUS_UART0]		= { 0x2d8, BIT(16) },
+	[RST_BUS_UART1]		= { 0x2d8, BIT(17) },
+	[RST_BUS_UART2]		= { 0x2d8, BIT(18) },
+	[RST_BUS_UART3]		= { 0x2d8, BIT(19) },
+	[RST_BUS_UART4]		= { 0x2d8, BIT(20) },
+};
+
+static const struct sunxi_ccu_desc sun8i_a83t_ccu_desc = {
+	.ccu_clks	= sun8i_a83t_ccu_clks,
+	.num_ccu_clks	= ARRAY_SIZE(sun8i_a83t_ccu_clks),
+
+	.hw_clks	= &sun8i_a83t_hw_clks,
+
+	.resets		= sun8i_a83t_ccu_resets,
+	.num_resets	= ARRAY_SIZE(sun8i_a83t_ccu_resets),
+};
+
+#define SUN8I_A83T_PLL_P_SHIFT	16
+#define SUN8I_A83T_PLL_N_SHIFT	8
+#define SUN8I_A83T_PLL_N_WIDTH	8
+
+static void sun8i_a83t_cpu_pll_fixup(void __iomem *reg)
+{
+	u32 val = readl(reg);
+
+	/* bail out if P divider is not used */
+	if (!(val & BIT(SUN8I_A83T_PLL_P_SHIFT)))
+		return;
+
+	/*
+	 * If P is used, output should be less than 288 MHz. When we
+	 * set P to 1, we should also decrease the multiplier so the
+	 * output doesn't go out of range, but not too much such that
+	 * the multiplier stays above 12, the minimal operation value.
+	 *
+	 * To keep it simple, set the multiplier to 17, the reset value.
+	 */
+	val &= ~GENMASK(SUN8I_A83T_PLL_N_SHIFT + SUN8I_A83T_PLL_N_WIDTH - 1,
+			SUN8I_A83T_PLL_N_SHIFT);
+	val |= 17 << SUN8I_A83T_PLL_N_SHIFT;
+
+	/* And clear P */
+	val &= ~BIT(SUN8I_A83T_PLL_P_SHIFT);
+
+	writel(val, reg);
+}
+
+static int sun8i_a83t_ccu_probe(struct platform_device *pdev)
+{
+	struct resource *res;
+	void __iomem *reg;
+	u32 val;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	reg = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(reg))
+		return PTR_ERR(reg);
+
+	/* Enforce d1 = 0, d2 = 0 for Audio PLL */
+	val = readl(reg + SUN8I_A83T_PLL_AUDIO_REG);
+	val &= ~(BIT(16) | BIT(18));
+	writel(val, reg + SUN8I_A83T_PLL_AUDIO_REG);
+
+	/* Enforce P = 1 for both CPU cluster PLLs */
+	sun8i_a83t_cpu_pll_fixup(reg + SUN8I_A83T_PLL_C0CPUX_REG);
+	sun8i_a83t_cpu_pll_fixup(reg + SUN8I_A83T_PLL_C1CPUX_REG);
+
+	return sunxi_ccu_probe(pdev->dev.of_node, reg, &sun8i_a83t_ccu_desc);
+}
+
+static const struct of_device_id sun8i_a83t_ccu_ids[] = {
+	{ .compatible = "allwinner,sun8i-a83t-ccu" },
+	{ }
+};
+
+static struct platform_driver sun8i_a83t_ccu_driver = {
+	.probe	= sun8i_a83t_ccu_probe,
+	.driver	= {
+		.name	= "sun8i-a83t-ccu",
+		.of_match_table	= sun8i_a83t_ccu_ids,
+	},
+};
+builtin_platform_driver(sun8i_a83t_ccu_driver);
diff --git a/drivers/clk/sunxi-ng/ccu-sun8i-a83t.h b/drivers/clk/sunxi-ng/ccu-sun8i-a83t.h
new file mode 100644
index 000000000000..d67edaf76748
--- /dev/null
+++ b/drivers/clk/sunxi-ng/ccu-sun8i-a83t.h
@@ -0,0 +1,64 @@
+/*
+ * Copyright 2016 Chen-Yu Tsai
+ *
+ * Chen-Yu Tsai <wens@csie.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#ifndef _CCU_SUN8I_A83T_H_
+#define _CCU_SUN8I_A83T_H_
+
+#include <dt-bindings/clock/sun8i-a83t-ccu.h>
+#include <dt-bindings/reset/sun8i-a83t-ccu.h>
+
+#define CLK_PLL_C0CPUX		0
+#define CLK_PLL_C1CPUX		1
+#define CLK_PLL_AUDIO		2
+#define CLK_PLL_VIDEO0		3
+#define CLK_PLL_VE		4
+#define CLK_PLL_DDR		5
+
+/* pll-periph is exported to the PRCM block */
+
+#define CLK_PLL_GPU		7
+#define CLK_PLL_HSIC		8
+
+/* pll-de is exported for the display engine */
+
+#define CLK_PLL_VIDEO1		10
+
+/* The CPUX clocks are exported */
+
+#define CLK_AXI0		13
+#define CLK_AXI1		14
+#define CLK_AHB1		15
+#define CLK_AHB2		16
+#define CLK_APB1		17
+#define CLK_APB2		18
+
+/* bus gates exported */
+
+#define CLK_CCI400		58
+
+/* module and usb clocks exported */
+
+#define CLK_DRAM		82
+
+/* dram gates and more module clocks exported */
+
+#define CLK_MBUS		95
+
+/* more module clocks exported */
+
+#define CLK_NUMBER		(CLK_GPU_HYD + 1)
+
+#endif /* _CCU_SUN8I_A83T_H_ */
diff --git a/drivers/clk/sunxi-ng/ccu-sun8i-de2.c b/drivers/clk/sunxi-ng/ccu-sun8i-de2.c
new file mode 100644
index 000000000000..5cdaf52669e4
--- /dev/null
+++ b/drivers/clk/sunxi-ng/ccu-sun8i-de2.c
@@ -0,0 +1,260 @@
+/*
+ * Copyright (c) 2017 Icenowy Zheng <icenowy@aosc.io>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/clk.h>
+#include <linux/clk-provider.h>
+#include <linux/of_address.h>
+#include <linux/of_platform.h>
+#include <linux/platform_device.h>
+#include <linux/reset.h>
+
+#include "ccu_common.h"
+#include "ccu_div.h"
+#include "ccu_gate.h"
+#include "ccu_reset.h"
+
+#include "ccu-sun8i-de2.h"
+
+static SUNXI_CCU_GATE(bus_mixer0_clk,	"bus-mixer0",	"bus-de",
+		      0x04, BIT(0), 0);
+static SUNXI_CCU_GATE(bus_mixer1_clk,	"bus-mixer1",	"bus-de",
+		      0x04, BIT(1), 0);
+static SUNXI_CCU_GATE(bus_wb_clk,	"bus-wb",	"bus-de",
+		      0x04, BIT(2), 0);
+
+static SUNXI_CCU_GATE(mixer0_clk,	"mixer0",	"mixer0-div",
+		      0x00, BIT(0), CLK_SET_RATE_PARENT);
+static SUNXI_CCU_GATE(mixer1_clk,	"mixer1",	"mixer1-div",
+		      0x00, BIT(1), CLK_SET_RATE_PARENT);
+static SUNXI_CCU_GATE(wb_clk,		"wb",		"wb-div",
+		      0x00, BIT(2), CLK_SET_RATE_PARENT);
+
+static SUNXI_CCU_M(mixer0_div_clk, "mixer0-div", "de", 0x0c, 0, 4,
+		   CLK_SET_RATE_PARENT);
+static SUNXI_CCU_M(mixer1_div_clk, "mixer1-div", "de", 0x0c, 4, 4,
+		   CLK_SET_RATE_PARENT);
+static SUNXI_CCU_M(wb_div_clk, "wb-div", "de", 0x0c, 8, 4,
+		   CLK_SET_RATE_PARENT);
+
+static struct ccu_common *sun8i_a83t_de2_clks[] = {
+	&mixer0_clk.common,
+	&mixer1_clk.common,
+	&wb_clk.common,
+
+	&bus_mixer0_clk.common,
+	&bus_mixer1_clk.common,
+	&bus_wb_clk.common,
+
+	&mixer0_div_clk.common,
+	&mixer1_div_clk.common,
+	&wb_div_clk.common,
+};
+
+static struct ccu_common *sun8i_v3s_de2_clks[] = {
+	&mixer0_clk.common,
+	&wb_clk.common,
+
+	&bus_mixer0_clk.common,
+	&bus_wb_clk.common,
+
+	&mixer0_div_clk.common,
+	&wb_div_clk.common,
+};
+
+static struct clk_hw_onecell_data sun8i_a83t_de2_hw_clks = {
+	.hws	= {
+		[CLK_MIXER0]		= &mixer0_clk.common.hw,
+		[CLK_MIXER1]		= &mixer1_clk.common.hw,
+		[CLK_WB]		= &wb_clk.common.hw,
+
+		[CLK_BUS_MIXER0]	= &bus_mixer0_clk.common.hw,
+		[CLK_BUS_MIXER1]	= &bus_mixer1_clk.common.hw,
+		[CLK_BUS_WB]		= &bus_wb_clk.common.hw,
+
+		[CLK_MIXER0_DIV]	= &mixer0_div_clk.common.hw,
+		[CLK_MIXER1_DIV]	= &mixer1_div_clk.common.hw,
+		[CLK_WB_DIV]		= &wb_div_clk.common.hw,
+	},
+	.num	= CLK_NUMBER,
+};
+
+static struct clk_hw_onecell_data sun8i_v3s_de2_hw_clks = {
+	.hws	= {
+		[CLK_MIXER0]		= &mixer0_clk.common.hw,
+		[CLK_WB]		= &wb_clk.common.hw,
+
+		[CLK_BUS_MIXER0]	= &bus_mixer0_clk.common.hw,
+		[CLK_BUS_WB]		= &bus_wb_clk.common.hw,
+
+		[CLK_MIXER0_DIV]	= &mixer0_div_clk.common.hw,
+		[CLK_WB_DIV]		= &wb_div_clk.common.hw,
+	},
+	.num	= CLK_NUMBER,
+};
+
+static struct ccu_reset_map sun8i_a83t_de2_resets[] = {
+	[RST_MIXER0]	= { 0x08, BIT(0) },
+	/*
+	 * For A83T, H3 and R40, mixer1 reset line is shared with wb, so
+	 * only RST_WB is exported here.
+	 * For V3s there's just no mixer1, so it also shares this struct.
+	 */
+	[RST_WB]	= { 0x08, BIT(2) },
+};
+
+static struct ccu_reset_map sun50i_a64_de2_resets[] = {
+	[RST_MIXER0]	= { 0x08, BIT(0) },
+	[RST_MIXER1]	= { 0x08, BIT(1) },
+	[RST_WB]	= { 0x08, BIT(2) },
+};
+
+static const struct sunxi_ccu_desc sun8i_a83t_de2_clk_desc = {
+	.ccu_clks	= sun8i_a83t_de2_clks,
+	.num_ccu_clks	= ARRAY_SIZE(sun8i_a83t_de2_clks),
+
+	.hw_clks	= &sun8i_a83t_de2_hw_clks,
+
+	.resets		= sun8i_a83t_de2_resets,
+	.num_resets	= ARRAY_SIZE(sun8i_a83t_de2_resets),
+};
+
+static const struct sunxi_ccu_desc sun50i_a64_de2_clk_desc = {
+	.ccu_clks	= sun8i_a83t_de2_clks,
+	.num_ccu_clks	= ARRAY_SIZE(sun8i_a83t_de2_clks),
+
+	.hw_clks	= &sun8i_a83t_de2_hw_clks,
+
+	.resets		= sun50i_a64_de2_resets,
+	.num_resets	= ARRAY_SIZE(sun50i_a64_de2_resets),
+};
+
+static const struct sunxi_ccu_desc sun8i_v3s_de2_clk_desc = {
+	.ccu_clks	= sun8i_v3s_de2_clks,
+	.num_ccu_clks	= ARRAY_SIZE(sun8i_v3s_de2_clks),
+
+	.hw_clks	= &sun8i_v3s_de2_hw_clks,
+
+	.resets		= sun8i_a83t_de2_resets,
+	.num_resets	= ARRAY_SIZE(sun8i_a83t_de2_resets),
+};
+
+static int sunxi_de2_clk_probe(struct platform_device *pdev)
+{
+	struct resource *res;
+	struct clk *bus_clk, *mod_clk;
+	struct reset_control *rstc;
+	void __iomem *reg;
+	const struct sunxi_ccu_desc *ccu_desc;
+	int ret;
+
+	ccu_desc = of_device_get_match_data(&pdev->dev);
+	if (!ccu_desc)
+		return -EINVAL;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	reg = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(reg))
+		return PTR_ERR(reg);
+
+	bus_clk = devm_clk_get(&pdev->dev, "bus");
+	if (IS_ERR(bus_clk)) {
+		ret = PTR_ERR(bus_clk);
+		if (ret != -EPROBE_DEFER)
+			dev_err(&pdev->dev, "Couldn't get bus clk: %d\n", ret);
+		return ret;
+	}
+
+	mod_clk = devm_clk_get(&pdev->dev, "mod");
+	if (IS_ERR(mod_clk)) {
+		ret = PTR_ERR(mod_clk);
+		if (ret != -EPROBE_DEFER)
+			dev_err(&pdev->dev, "Couldn't get mod clk: %d\n", ret);
+		return ret;
+	}
+
+	rstc = devm_reset_control_get_exclusive(&pdev->dev, NULL);
+	if (IS_ERR(rstc)) {
+		ret = PTR_ERR(rstc);
+		if (ret != -EPROBE_DEFER)
+			dev_err(&pdev->dev,
+				"Couldn't get reset control: %d\n", ret);
+		return ret;
+	}
+
+	/* The clocks need to be enabled for us to access the registers */
+	ret = clk_prepare_enable(bus_clk);
+	if (ret) {
+		dev_err(&pdev->dev, "Couldn't enable bus clk: %d\n", ret);
+		return ret;
+	}
+
+	ret = clk_prepare_enable(mod_clk);
+	if (ret) {
+		dev_err(&pdev->dev, "Couldn't enable mod clk: %d\n", ret);
+		goto err_disable_bus_clk;
+	}
+
+	/* The reset control needs to be asserted for the controls to work */
+	ret = reset_control_deassert(rstc);
+	if (ret) {
+		dev_err(&pdev->dev,
+			"Couldn't deassert reset control: %d\n", ret);
+		goto err_disable_mod_clk;
+	}
+
+	ret = sunxi_ccu_probe(pdev->dev.of_node, reg, ccu_desc);
+	if (ret)
+		goto err_assert_reset;
+
+	return 0;
+
+err_assert_reset:
+	reset_control_assert(rstc);
+err_disable_mod_clk:
+	clk_disable_unprepare(mod_clk);
+err_disable_bus_clk:
+	clk_disable_unprepare(bus_clk);
+	return ret;
+}
+
+static const struct of_device_id sunxi_de2_clk_ids[] = {
+	{
+		.compatible = "allwinner,sun8i-a83t-de2-clk",
+		.data = &sun8i_a83t_de2_clk_desc,
+	},
+	{
+		.compatible = "allwinner,sun8i-v3s-de2-clk",
+		.data = &sun8i_v3s_de2_clk_desc,
+	},
+	{
+		.compatible = "allwinner,sun50i-h5-de2-clk",
+		.data = &sun50i_a64_de2_clk_desc,
+	},
+	/*
+	 * The Allwinner A64 SoC needs some bit to be poke in syscon to make
+	 * DE2 really working.
+	 * So there's currently no A64 compatible here.
+	 * H5 shares the same reset line with A64, so here H5 is using the
+	 * clock description of A64.
+	 */
+	{ }
+};
+
+static struct platform_driver sunxi_de2_clk_driver = {
+	.probe	= sunxi_de2_clk_probe,
+	.driver	= {
+		.name	= "sunxi-de2-clks",
+		.of_match_table	= sunxi_de2_clk_ids,
+	},
+};
+builtin_platform_driver(sunxi_de2_clk_driver);
diff --git a/drivers/clk/sunxi-ng/ccu-sun8i-de2.h b/drivers/clk/sunxi-ng/ccu-sun8i-de2.h
new file mode 100644
index 000000000000..530c006e0ae9
--- /dev/null
+++ b/drivers/clk/sunxi-ng/ccu-sun8i-de2.h
@@ -0,0 +1,28 @@
+/*
+ * Copyright 2016 Icenowy Zheng <icenowy@aosc.io>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#ifndef _CCU_SUN8I_DE2_H_
+#define _CCU_SUN8I_DE2_H_
+
+#include <dt-bindings/clock/sun8i-de2.h>
+#include <dt-bindings/reset/sun8i-de2.h>
+
+/* Intermediary clock dividers are not exported */
+#define CLK_MIXER0_DIV	3
+#define CLK_MIXER1_DIV	4
+#define CLK_WB_DIV	5
+
+#define CLK_NUMBER	(CLK_WB + 1)
+
+#endif /* _CCU_SUN8I_DE2_H_ */
diff --git a/drivers/clk/sunxi-ng/ccu-sun8i-h3.c b/drivers/clk/sunxi-ng/ccu-sun8i-h3.c
index 4cbc1b701b7c..62e4f0d2b2fc 100644
--- a/drivers/clk/sunxi-ng/ccu-sun8i-h3.c
+++ b/drivers/clk/sunxi-ng/ccu-sun8i-h3.c
@@ -141,6 +141,9 @@ static SUNXI_CCU_M(axi_clk, "axi", "cpux", 0x050, 0, 2, 0);
 
 static const char * const ahb1_parents[] = { "osc32k", "osc24M",
 					     "axi" , "pll-periph0" };
+static const struct ccu_mux_var_prediv ahb1_predivs[] = {
+	{ .index = 3, .shift = 6, .width = 2 },
+};
 static struct ccu_div ahb1_clk = {
 	.div		= _SUNXI_CCU_DIV_FLAGS(4, 2, CLK_DIVIDER_POWER_OF_TWO),
 
@@ -148,11 +151,8 @@ static struct ccu_div ahb1_clk = {
 		.shift	= 12,
 		.width	= 2,
 
-		.variable_prediv	= {
-			.index	= 3,
-			.shift	= 6,
-			.width	= 2,
-		},
+		.var_predivs	= ahb1_predivs,
+		.n_var_predivs	= ARRAY_SIZE(ahb1_predivs),
 	},
 
 	.common		= {
diff --git a/drivers/clk/sunxi-ng/ccu-sun8i-r.c b/drivers/clk/sunxi-ng/ccu-sun8i-r.c
index 119f47b568ea..e54816ec1dbe 100644
--- a/drivers/clk/sunxi-ng/ccu-sun8i-r.c
+++ b/drivers/clk/sunxi-ng/ccu-sun8i-r.c
@@ -27,6 +27,11 @@
 
 static const char * const ar100_parents[] = { "osc32k", "osc24M",
 					     "pll-periph0", "iosc" };
+static const char * const a83t_ar100_parents[] = { "osc16M-d512", "osc24M",
+						   "pll-periph0", "iosc" };
+static const struct ccu_mux_var_prediv ar100_predivs[] = {
+	{ .index = 2, .shift = 8, .width = 5 },
+};
 
 static struct ccu_div ar100_clk = {
 	.div		= _SUNXI_CCU_DIV_FLAGS(4, 2, CLK_DIVIDER_POWER_OF_TWO),
@@ -35,11 +40,8 @@ static struct ccu_div ar100_clk = {
 		.shift	= 16,
 		.width	= 2,
 
-		.variable_prediv	= {
-			.index	= 2,
-			.shift	= 8,
-			.width	= 5,
-		},
+		.var_predivs	= ar100_predivs,
+		.n_var_predivs	= ARRAY_SIZE(ar100_predivs),
 	},
 
 	.common		= {
@@ -52,6 +54,27 @@ static struct ccu_div ar100_clk = {
 	},
 };
 
+static struct ccu_div a83t_ar100_clk = {
+	.div		= _SUNXI_CCU_DIV_FLAGS(4, 2, CLK_DIVIDER_POWER_OF_TWO),
+
+	.mux		= {
+		.shift	= 16,
+		.width	= 2,
+
+		.var_predivs	= ar100_predivs,
+		.n_var_predivs	= ARRAY_SIZE(ar100_predivs),
+	},
+
+	.common		= {
+		.reg		= 0x00,
+		.features	= CCU_FEATURE_VARIABLE_PREDIV,
+		.hw.init	= CLK_HW_INIT_PARENTS("ar100",
+						      a83t_ar100_parents,
+						      &ccu_div_ops,
+						      0),
+	},
+};
+
 static CLK_FIXED_FACTOR(ahb0_clk, "ahb0", "ar100", 1, 1, 0);
 
 static struct ccu_div apb0_clk = {
@@ -66,6 +89,8 @@ static struct ccu_div apb0_clk = {
 	},
 };
 
+static SUNXI_CCU_M(a83t_apb0_clk, "apb0", "ahb0", 0x0c, 0, 2, 0);
+
 static SUNXI_CCU_GATE(apb0_pio_clk,	"apb0-pio",	"apb0",
 		      0x28, BIT(0), 0);
 static SUNXI_CCU_GATE(apb0_ir_clk,	"apb0-ir",	"apb0",
@@ -90,6 +115,46 @@ static SUNXI_CCU_MP_WITH_MUX_GATE(ir_clk, "ir",
 				  BIT(31),	/* gate */
 				  0);
 
+static const char *const a83t_r_mod0_parents[] = { "osc16M", "osc24M" };
+static const struct ccu_mux_fixed_prediv a83t_ir_predivs[] = {
+	{ .index = 0, .div = 16 },
+};
+static struct ccu_mp a83t_ir_clk = {
+	.enable	= BIT(31),
+
+	.m	= _SUNXI_CCU_DIV(0, 4),
+	.p	= _SUNXI_CCU_DIV(16, 2),
+
+	.mux	= {
+		.shift	= 24,
+		.width	= 2,
+		.fixed_predivs	= a83t_ir_predivs,
+		.n_predivs	= ARRAY_SIZE(a83t_ir_predivs),
+	},
+
+	.common		= {
+		.reg		= 0x54,
+		.features	= CCU_FEATURE_VARIABLE_PREDIV,
+		.hw.init	= CLK_HW_INIT_PARENTS("ir",
+						      a83t_r_mod0_parents,
+						      &ccu_mp_ops,
+						      0),
+	},
+};
+
+static struct ccu_common *sun8i_a83t_r_ccu_clks[] = {
+	&a83t_ar100_clk.common,
+	&a83t_apb0_clk.common,
+	&apb0_pio_clk.common,
+	&apb0_ir_clk.common,
+	&apb0_timer_clk.common,
+	&apb0_rsb_clk.common,
+	&apb0_uart_clk.common,
+	&apb0_i2c_clk.common,
+	&apb0_twd_clk.common,
+	&a83t_ir_clk.common,
+};
+
 static struct ccu_common *sun8i_h3_r_ccu_clks[] = {
 	&ar100_clk.common,
 	&apb0_clk.common,
@@ -115,6 +180,23 @@ static struct ccu_common *sun50i_a64_r_ccu_clks[] = {
 	&ir_clk.common,
 };
 
+static struct clk_hw_onecell_data sun8i_a83t_r_hw_clks = {
+	.hws	= {
+		[CLK_AR100]		= &a83t_ar100_clk.common.hw,
+		[CLK_AHB0]		= &ahb0_clk.hw,
+		[CLK_APB0]		= &a83t_apb0_clk.common.hw,
+		[CLK_APB0_PIO]		= &apb0_pio_clk.common.hw,
+		[CLK_APB0_IR]		= &apb0_ir_clk.common.hw,
+		[CLK_APB0_TIMER]	= &apb0_timer_clk.common.hw,
+		[CLK_APB0_RSB]		= &apb0_rsb_clk.common.hw,
+		[CLK_APB0_UART]		= &apb0_uart_clk.common.hw,
+		[CLK_APB0_I2C]		= &apb0_i2c_clk.common.hw,
+		[CLK_APB0_TWD]		= &apb0_twd_clk.common.hw,
+		[CLK_IR]		= &a83t_ir_clk.common.hw,
+	},
+	.num	= CLK_NUMBER,
+};
+
 static struct clk_hw_onecell_data sun8i_h3_r_hw_clks = {
 	.hws	= {
 		[CLK_AR100]		= &ar100_clk.common.hw,
@@ -148,6 +230,14 @@ static struct clk_hw_onecell_data sun50i_a64_r_hw_clks = {
 	.num	= CLK_NUMBER,
 };
 
+static struct ccu_reset_map sun8i_a83t_r_ccu_resets[] = {
+	[RST_APB0_IR]		=  { 0xb0, BIT(1) },
+	[RST_APB0_TIMER]	=  { 0xb0, BIT(2) },
+	[RST_APB0_RSB]		=  { 0xb0, BIT(3) },
+	[RST_APB0_UART]		=  { 0xb0, BIT(4) },
+	[RST_APB0_I2C]		=  { 0xb0, BIT(6) },
+};
+
 static struct ccu_reset_map sun8i_h3_r_ccu_resets[] = {
 	[RST_APB0_IR]		=  { 0xb0, BIT(1) },
 	[RST_APB0_TIMER]	=  { 0xb0, BIT(2) },
@@ -163,6 +253,16 @@ static struct ccu_reset_map sun50i_a64_r_ccu_resets[] = {
 	[RST_APB0_I2C]		=  { 0xb0, BIT(6) },
 };
 
+static const struct sunxi_ccu_desc sun8i_a83t_r_ccu_desc = {
+	.ccu_clks	= sun8i_a83t_r_ccu_clks,
+	.num_ccu_clks	= ARRAY_SIZE(sun8i_a83t_r_ccu_clks),
+
+	.hw_clks	= &sun8i_a83t_r_hw_clks,
+
+	.resets		= sun8i_a83t_r_ccu_resets,
+	.num_resets	= ARRAY_SIZE(sun8i_a83t_r_ccu_resets),
+};
+
 static const struct sunxi_ccu_desc sun8i_h3_r_ccu_desc = {
 	.ccu_clks	= sun8i_h3_r_ccu_clks,
 	.num_ccu_clks	= ARRAY_SIZE(sun8i_h3_r_ccu_clks),
@@ -198,6 +298,13 @@ static void __init sunxi_r_ccu_init(struct device_node *node,
 	sunxi_ccu_probe(node, reg, desc);
 }
 
+static void __init sun8i_a83t_r_ccu_setup(struct device_node *node)
+{
+	sunxi_r_ccu_init(node, &sun8i_a83t_r_ccu_desc);
+}
+CLK_OF_DECLARE(sun8i_a83t_r_ccu, "allwinner,sun8i-a83t-r-ccu",
+	       sun8i_a83t_r_ccu_setup);
+
 static void __init sun8i_h3_r_ccu_setup(struct device_node *node)
 {
 	sunxi_r_ccu_init(node, &sun8i_h3_r_ccu_desc);
diff --git a/drivers/clk/sunxi-ng/ccu-sun8i-v3s.c b/drivers/clk/sunxi-ng/ccu-sun8i-v3s.c
index e58706b40ae9..a34a78d7fb28 100644
--- a/drivers/clk/sunxi-ng/ccu-sun8i-v3s.c
+++ b/drivers/clk/sunxi-ng/ccu-sun8i-v3s.c
@@ -132,6 +132,9 @@ static SUNXI_CCU_M(axi_clk, "axi", "cpu", 0x050, 0, 2, 0);
 
 static const char * const ahb1_parents[] = { "osc32k", "osc24M",
 					     "axi", "pll-periph0" };
+static const struct ccu_mux_var_prediv ahb1_predivs[] = {
+	{ .index = 3, .shift = 6, .width = 2 },
+};
 static struct ccu_div ahb1_clk = {
 	.div		= _SUNXI_CCU_DIV_FLAGS(4, 2, CLK_DIVIDER_POWER_OF_TWO),
 
@@ -139,11 +142,8 @@ static struct ccu_div ahb1_clk = {
 		.shift	= 12,
 		.width	= 2,
 
-		.variable_prediv	= {
-			.index	= 3,
-			.shift	= 6,
-			.width	= 2,
-		},
+		.var_predivs	= ahb1_predivs,
+		.n_var_predivs	= ARRAY_SIZE(ahb1_predivs),
 	},
 
 	.common		= {
@@ -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/clk/sunxi-ng/ccu_div.c b/drivers/clk/sunxi-ng/ccu_div.c
index 4057e6021aa9..c0e5c10d0091 100644
--- a/drivers/clk/sunxi-ng/ccu_div.c
+++ b/drivers/clk/sunxi-ng/ccu_div.c
@@ -14,23 +14,17 @@
 #include "ccu_div.h"
 
 static unsigned long ccu_div_round_rate(struct ccu_mux_internal *mux,
-					unsigned long parent_rate,
+					struct clk_hw *parent,
+					unsigned long *parent_rate,
 					unsigned long rate,
 					void *data)
 {
 	struct ccu_div *cd = data;
-	unsigned long val;
-
-	/*
-	 * We can't use divider_round_rate that assumes that there's
-	 * several parents, while we might be called to evaluate
-	 * several different parents.
-	 */
-	val = divider_get_val(rate, parent_rate, cd->div.table, cd->div.width,
-			      cd->div.flags);
 
-	return divider_recalc_rate(&cd->common.hw, parent_rate, val,
-				   cd->div.table, cd->div.flags);
+	return divider_round_rate_parent(&cd->common.hw, parent,
+					 rate, parent_rate,
+					 cd->div.table, cd->div.width,
+					 cd->div.flags);
 }
 
 static void ccu_div_disable(struct clk_hw *hw)
@@ -65,8 +59,8 @@ static unsigned long ccu_div_recalc_rate(struct clk_hw *hw,
 	val = reg >> cd->div.shift;
 	val &= (1 << cd->div.width) - 1;
 
-	ccu_mux_helper_adjust_parent_for_prediv(&cd->common, &cd->mux, -1,
-						&parent_rate);
+	parent_rate = ccu_mux_helper_apply_prediv(&cd->common, &cd->mux, -1,
+						  parent_rate);
 
 	return divider_recalc_rate(hw, parent_rate, val, cd->div.table,
 				   cd->div.flags);
@@ -77,18 +71,6 @@ static int ccu_div_determine_rate(struct clk_hw *hw,
 {
 	struct ccu_div *cd = hw_to_ccu_div(hw);
 
-	if (clk_hw_get_num_parents(hw) == 1) {
-		req->rate = divider_round_rate(hw, req->rate,
-					       &req->best_parent_rate,
-					       cd->div.table,
-					       cd->div.width,
-					       cd->div.flags);
-
-		req->best_parent_hw = clk_hw_get_parent(hw);
-
-		return 0;
-	}
-
 	return ccu_mux_helper_determine_rate(&cd->common, &cd->mux,
 					     req, ccu_div_round_rate, cd);
 }
@@ -101,8 +83,8 @@ static int ccu_div_set_rate(struct clk_hw *hw, unsigned long rate,
 	unsigned long val;
 	u32 reg;
 
-	ccu_mux_helper_adjust_parent_for_prediv(&cd->common, &cd->mux, -1,
-						&parent_rate);
+	parent_rate = ccu_mux_helper_apply_prediv(&cd->common, &cd->mux, -1,
+						  parent_rate);
 
 	val = divider_get_val(rate, parent_rate, cd->div.table, cd->div.width,
 			      cd->div.flags);
diff --git a/drivers/clk/sunxi-ng/ccu_mp.c b/drivers/clk/sunxi-ng/ccu_mp.c
index b583f186a804..b917ad7a386c 100644
--- a/drivers/clk/sunxi-ng/ccu_mp.c
+++ b/drivers/clk/sunxi-ng/ccu_mp.c
@@ -41,7 +41,8 @@ static void ccu_mp_find_best(unsigned long parent, unsigned long rate,
 }
 
 static unsigned long ccu_mp_round_rate(struct ccu_mux_internal *mux,
-				       unsigned long parent_rate,
+				       struct clk_hw *hw,
+				       unsigned long *parent_rate,
 				       unsigned long rate,
 				       void *data)
 {
@@ -52,9 +53,9 @@ static unsigned long ccu_mp_round_rate(struct ccu_mux_internal *mux,
 	max_m = cmp->m.max ?: 1 << cmp->m.width;
 	max_p = cmp->p.max ?: 1 << ((1 << cmp->p.width) - 1);
 
-	ccu_mp_find_best(parent_rate, rate, max_m, max_p, &m, &p);
+	ccu_mp_find_best(*parent_rate, rate, max_m, max_p, &m, &p);
 
-	return parent_rate / p / m;
+	return *parent_rate / p / m;
 }
 
 static void ccu_mp_disable(struct clk_hw *hw)
@@ -86,8 +87,8 @@ static unsigned long ccu_mp_recalc_rate(struct clk_hw *hw,
 	u32 reg;
 
 	/* Adjust parent_rate according to pre-dividers */
-	ccu_mux_helper_adjust_parent_for_prediv(&cmp->common, &cmp->mux,
-						-1, &parent_rate);
+	parent_rate = ccu_mux_helper_apply_prediv(&cmp->common, &cmp->mux, -1,
+						  parent_rate);
 
 	reg = readl(cmp->common.base + cmp->common.reg);
 
@@ -122,8 +123,8 @@ static int ccu_mp_set_rate(struct clk_hw *hw, unsigned long rate,
 	u32 reg;
 
 	/* Adjust parent_rate according to pre-dividers */
-	ccu_mux_helper_adjust_parent_for_prediv(&cmp->common, &cmp->mux,
-						-1, &parent_rate);
+	parent_rate = ccu_mux_helper_apply_prediv(&cmp->common, &cmp->mux, -1,
+						  parent_rate);
 
 	max_m = cmp->m.max ?: 1 << cmp->m.width;
 	max_p = cmp->p.max ?: 1 << ((1 << cmp->p.width) - 1);
diff --git a/drivers/clk/sunxi-ng/ccu_mult.c b/drivers/clk/sunxi-ng/ccu_mult.c
index 671141359895..20d0300867f2 100644
--- a/drivers/clk/sunxi-ng/ccu_mult.c
+++ b/drivers/clk/sunxi-ng/ccu_mult.c
@@ -33,9 +33,10 @@ static void ccu_mult_find_best(unsigned long parent, unsigned long rate,
 }
 
 static unsigned long ccu_mult_round_rate(struct ccu_mux_internal *mux,
-					unsigned long parent_rate,
-					unsigned long rate,
-					void *data)
+					 struct clk_hw *parent,
+					 unsigned long *parent_rate,
+					 unsigned long rate,
+					 void *data)
 {
 	struct ccu_mult *cm = data;
 	struct _ccu_mult _cm;
@@ -47,9 +48,9 @@ static unsigned long ccu_mult_round_rate(struct ccu_mux_internal *mux,
 	else
 		_cm.max = (1 << cm->mult.width) + cm->mult.offset - 1;
 
-	ccu_mult_find_best(parent_rate, rate, &_cm);
+	ccu_mult_find_best(*parent_rate, rate, &_cm);
 
-	return parent_rate * _cm.mult;
+	return *parent_rate * _cm.mult;
 }
 
 static void ccu_mult_disable(struct clk_hw *hw)
@@ -87,8 +88,8 @@ static unsigned long ccu_mult_recalc_rate(struct clk_hw *hw,
 	val = reg >> cm->mult.shift;
 	val &= (1 << cm->mult.width) - 1;
 
-	ccu_mux_helper_adjust_parent_for_prediv(&cm->common, &cm->mux, -1,
-						&parent_rate);
+	parent_rate = ccu_mux_helper_apply_prediv(&cm->common, &cm->mux, -1,
+						  parent_rate);
 
 	return parent_rate * (val + cm->mult.offset);
 }
@@ -115,8 +116,8 @@ static int ccu_mult_set_rate(struct clk_hw *hw, unsigned long rate,
 	else
 		ccu_frac_helper_disable(&cm->common, &cm->frac);
 
-	ccu_mux_helper_adjust_parent_for_prediv(&cm->common, &cm->mux, -1,
-						&parent_rate);
+	parent_rate = ccu_mux_helper_apply_prediv(&cm->common, &cm->mux, -1,
+						  parent_rate);
 
 	_cm.min = cm->mult.min;
 
diff --git a/drivers/clk/sunxi-ng/ccu_mux.c b/drivers/clk/sunxi-ng/ccu_mux.c
index c6bb1f523232..cfe4538304fb 100644
--- a/drivers/clk/sunxi-ng/ccu_mux.c
+++ b/drivers/clk/sunxi-ng/ccu_mux.c
@@ -15,24 +15,20 @@
 #include "ccu_gate.h"
 #include "ccu_mux.h"
 
-void ccu_mux_helper_adjust_parent_for_prediv(struct ccu_common *common,
-					     struct ccu_mux_internal *cm,
-					     int parent_index,
-					     unsigned long *parent_rate)
+static u16 ccu_mux_get_prediv(struct ccu_common *common,
+			      struct ccu_mux_internal *cm,
+			      int parent_index)
 {
 	u16 prediv = 1;
 	u32 reg;
-	int i;
 
 	if (!((common->features & CCU_FEATURE_FIXED_PREDIV) ||
 	      (common->features & CCU_FEATURE_VARIABLE_PREDIV) ||
 	      (common->features & CCU_FEATURE_ALL_PREDIV)))
-		return;
+		return 1;
 
-	if (common->features & CCU_FEATURE_ALL_PREDIV) {
-		*parent_rate = *parent_rate / common->prediv;
-		return;
-	}
+	if (common->features & CCU_FEATURE_ALL_PREDIV)
+		return common->prediv;
 
 	reg = readl(common->base + common->reg);
 	if (parent_index < 0) {
@@ -40,28 +36,52 @@ void ccu_mux_helper_adjust_parent_for_prediv(struct ccu_common *common,
 		parent_index &= (1 << cm->width) - 1;
 	}
 
-	if (common->features & CCU_FEATURE_FIXED_PREDIV)
+	if (common->features & CCU_FEATURE_FIXED_PREDIV) {
+		int i;
+
 		for (i = 0; i < cm->n_predivs; i++)
 			if (parent_index == cm->fixed_predivs[i].index)
 				prediv = cm->fixed_predivs[i].div;
+	}
 
-	if (common->features & CCU_FEATURE_VARIABLE_PREDIV)
-		if (parent_index == cm->variable_prediv.index) {
-			u8 div;
+	if (common->features & CCU_FEATURE_VARIABLE_PREDIV) {
+		int i;
 
-			div = reg >> cm->variable_prediv.shift;
-			div &= (1 << cm->variable_prediv.width) - 1;
-			prediv = div + 1;
-		}
+		for (i = 0; i < cm->n_var_predivs; i++)
+			if (parent_index == cm->var_predivs[i].index) {
+				u8 div;
+
+				div = reg >> cm->var_predivs[i].shift;
+				div &= (1 << cm->var_predivs[i].width) - 1;
+				prediv = div + 1;
+			}
+	}
+
+	return prediv;
+}
 
-	*parent_rate = *parent_rate / prediv;
+unsigned long ccu_mux_helper_apply_prediv(struct ccu_common *common,
+					  struct ccu_mux_internal *cm,
+					  int parent_index,
+					  unsigned long parent_rate)
+{
+	return parent_rate / ccu_mux_get_prediv(common, cm, parent_index);
+}
+
+unsigned long ccu_mux_helper_unapply_prediv(struct ccu_common *common,
+					    struct ccu_mux_internal *cm,
+					    int parent_index,
+					    unsigned long parent_rate)
+{
+	return parent_rate * ccu_mux_get_prediv(common, cm, parent_index);
 }
 
 int ccu_mux_helper_determine_rate(struct ccu_common *common,
 				  struct ccu_mux_internal *cm,
 				  struct clk_rate_request *req,
 				  unsigned long (*round)(struct ccu_mux_internal *,
-							 unsigned long,
+							 struct clk_hw *,
+							 unsigned long *,
 							 unsigned long,
 							 void *),
 				  void *data)
@@ -75,41 +95,43 @@ int ccu_mux_helper_determine_rate(struct ccu_common *common,
 
 		best_parent = clk_hw_get_parent(hw);
 		best_parent_rate = clk_hw_get_rate(best_parent);
+		adj_parent_rate = ccu_mux_helper_apply_prediv(common, cm, -1,
+							      best_parent_rate);
 
-		adj_parent_rate = best_parent_rate;
-		ccu_mux_helper_adjust_parent_for_prediv(common, cm, -1,
-							&adj_parent_rate);
+		best_rate = round(cm, best_parent, &adj_parent_rate,
+				  req->rate, data);
 
-		best_rate = round(cm, adj_parent_rate, req->rate, data);
+		/*
+		 * adj_parent_rate might have been modified by our clock.
+		 * Unapply the pre-divider if there's one, and give
+		 * the actual frequency the parent needs to run at.
+		 */
+		best_parent_rate = ccu_mux_helper_unapply_prediv(common, cm, -1,
+								 adj_parent_rate);
 
 		goto out;
 	}
 
 	for (i = 0; i < clk_hw_get_num_parents(hw); i++) {
-		unsigned long tmp_rate, parent_rate, adj_parent_rate;
+		unsigned long tmp_rate, parent_rate;
 		struct clk_hw *parent;
 
 		parent = clk_hw_get_parent_by_index(hw, i);
 		if (!parent)
 			continue;
 
-		if (clk_hw_get_flags(hw) & CLK_SET_RATE_PARENT) {
-			struct clk_rate_request parent_req = *req;
-			int ret = __clk_determine_rate(parent, &parent_req);
-
-			if (ret)
-				continue;
+		parent_rate = ccu_mux_helper_apply_prediv(common, cm, i,
+							  clk_hw_get_rate(parent));
 
-			parent_rate = parent_req.rate;
-		} else {
-			parent_rate = clk_hw_get_rate(parent);
-		}
+		tmp_rate = round(cm, parent, &parent_rate, req->rate, data);
 
-		adj_parent_rate = parent_rate;
-		ccu_mux_helper_adjust_parent_for_prediv(common, cm, i,
-							&adj_parent_rate);
-
-		tmp_rate = round(cm, adj_parent_rate, req->rate, data);
+		/*
+		 * parent_rate might have been modified by our clock.
+		 * Unapply the pre-divider if there's one, and give
+		 * the actual frequency the parent needs to run at.
+		 */
+		parent_rate = ccu_mux_helper_unapply_prediv(common, cm, i,
+							    parent_rate);
 		if (tmp_rate == req->rate) {
 			best_parent = parent;
 			best_parent_rate = parent_rate;
@@ -217,10 +239,8 @@ static unsigned long ccu_mux_recalc_rate(struct clk_hw *hw,
 {
 	struct ccu_mux *cm = hw_to_ccu_mux(hw);
 
-	ccu_mux_helper_adjust_parent_for_prediv(&cm->common, &cm->mux, -1,
-						&parent_rate);
-
-	return parent_rate;
+	return ccu_mux_helper_apply_prediv(&cm->common, &cm->mux, -1,
+					   parent_rate);
 }
 
 const struct clk_ops ccu_mux_ops = {
diff --git a/drivers/clk/sunxi-ng/ccu_mux.h b/drivers/clk/sunxi-ng/ccu_mux.h
index 47aba3a48245..f20c0bd62a47 100644
--- a/drivers/clk/sunxi-ng/ccu_mux.h
+++ b/drivers/clk/sunxi-ng/ccu_mux.h
@@ -10,6 +10,12 @@ struct ccu_mux_fixed_prediv {
 	u16	div;
 };
 
+struct ccu_mux_var_prediv {
+	u8	index;
+	u8	shift;
+	u8	width;
+};
+
 struct ccu_mux_internal {
 	u8		shift;
 	u8		width;
@@ -18,11 +24,8 @@ struct ccu_mux_internal {
 	const struct ccu_mux_fixed_prediv	*fixed_predivs;
 	u8		n_predivs;
 
-	struct {
-		u8	index;
-		u8	shift;
-		u8	width;
-	} variable_prediv;
+	const struct ccu_mux_var_prediv		*var_predivs;
+	u8		n_var_predivs;
 };
 
 #define _SUNXI_CCU_MUX_TABLE(_shift, _width, _table)	\
@@ -78,15 +81,16 @@ static inline struct ccu_mux *hw_to_ccu_mux(struct clk_hw *hw)
 
 extern const struct clk_ops ccu_mux_ops;
 
-void ccu_mux_helper_adjust_parent_for_prediv(struct ccu_common *common,
-					     struct ccu_mux_internal *cm,
-					     int parent_index,
-					     unsigned long *parent_rate);
+unsigned long ccu_mux_helper_apply_prediv(struct ccu_common *common,
+					  struct ccu_mux_internal *cm,
+					  int parent_index,
+					  unsigned long parent_rate);
 int ccu_mux_helper_determine_rate(struct ccu_common *common,
 				  struct ccu_mux_internal *cm,
 				  struct clk_rate_request *req,
 				  unsigned long (*round)(struct ccu_mux_internal *,
-							 unsigned long,
+							 struct clk_hw *,
+							 unsigned long *,
 							 unsigned long,
 							 void *),
 				  void *data);
diff --git a/drivers/clk/sunxi-ng/ccu_nkm.c b/drivers/clk/sunxi-ng/ccu_nkm.c
index cba84afe1cf1..44b16dc8fea6 100644
--- a/drivers/clk/sunxi-ng/ccu_nkm.c
+++ b/drivers/clk/sunxi-ng/ccu_nkm.c
@@ -102,7 +102,8 @@ static unsigned long ccu_nkm_recalc_rate(struct clk_hw *hw,
 }
 
 static unsigned long ccu_nkm_round_rate(struct ccu_mux_internal *mux,
-					unsigned long parent_rate,
+					struct clk_hw *hw,
+					unsigned long *parent_rate,
 					unsigned long rate,
 					void *data)
 {
@@ -116,9 +117,9 @@ static unsigned long ccu_nkm_round_rate(struct ccu_mux_internal *mux,
 	_nkm.min_m = 1;
 	_nkm.max_m = nkm->m.max ?: 1 << nkm->m.width;
 
-	ccu_nkm_find_best(parent_rate, rate, &_nkm);
+	ccu_nkm_find_best(*parent_rate, rate, &_nkm);
 
-	return parent_rate * _nkm.n * _nkm.k / _nkm.m;
+	return *parent_rate * _nkm.n * _nkm.k / _nkm.m;
 }
 
 static int ccu_nkm_determine_rate(struct clk_hw *hw,
diff --git a/drivers/clk/sunxi-ng/ccu_reset.h b/drivers/clk/sunxi-ng/ccu_reset.h
index 36a4679210bd..ff8f5ebca435 100644
--- a/drivers/clk/sunxi-ng/ccu_reset.h
+++ b/drivers/clk/sunxi-ng/ccu_reset.h
@@ -15,6 +15,7 @@
 #define _CCU_RESET_H_
 
 #include <linux/reset-controller.h>
+#include <linux/spinlock.h>
 
 struct ccu_reset_map {
 	u16	reg;
diff --git a/drivers/cpufreq/Kconfig.arm b/drivers/cpufreq/Kconfig.arm
index 74ed7e9a7f27..2011fec2d6ad 100644
--- a/drivers/cpufreq/Kconfig.arm
+++ b/drivers/cpufreq/Kconfig.arm
@@ -71,6 +71,15 @@ config ARM_HIGHBANK_CPUFREQ
 
 	  If in doubt, say N.
 
+config ARM_DB8500_CPUFREQ
+	tristate "ST-Ericsson DB8500 cpufreq" if COMPILE_TEST && !ARCH_U8500
+	default ARCH_U8500
+	depends on HAS_IOMEM
+	depends on !CPU_THERMAL || THERMAL
+	help
+	  This adds the CPUFreq driver for ST-Ericsson Ux500 (DB8500) SoC
+	  series.
+
 config ARM_IMX6Q_CPUFREQ
 	tristate "Freescale i.MX6 cpufreq support"
 	depends on ARCH_MXC
diff --git a/drivers/cpufreq/Makefile b/drivers/cpufreq/Makefile
index b7e78f063c4f..ab3a42cd29ef 100644
--- a/drivers/cpufreq/Makefile
+++ b/drivers/cpufreq/Makefile
@@ -53,7 +53,7 @@ obj-$(CONFIG_ARM_DT_BL_CPUFREQ)		+= arm_big_little_dt.o
 
 obj-$(CONFIG_ARM_BRCMSTB_AVS_CPUFREQ)	+= brcmstb-avs-cpufreq.o
 obj-$(CONFIG_ARCH_DAVINCI)		+= davinci-cpufreq.o
-obj-$(CONFIG_UX500_SOC_DB8500)		+= dbx500-cpufreq.o
+obj-$(CONFIG_ARM_DB8500_CPUFREQ)	+= dbx500-cpufreq.o
 obj-$(CONFIG_ARM_EXYNOS5440_CPUFREQ)	+= exynos5440-cpufreq.o
 obj-$(CONFIG_ARM_HIGHBANK_CPUFREQ)	+= highbank-cpufreq.o
 obj-$(CONFIG_ARM_IMX6Q_CPUFREQ)		+= imx6q-cpufreq.o
diff --git a/drivers/cpufreq/arm_big_little.c b/drivers/cpufreq/arm_big_little.c
index 418042201e6d..ea6d62547b10 100644
--- a/drivers/cpufreq/arm_big_little.c
+++ b/drivers/cpufreq/arm_big_little.c
@@ -540,7 +540,7 @@ static void bL_cpufreq_ready(struct cpufreq_policy *policy)
 				     &power_coefficient);
 
 		cdev[cur_cluster] = of_cpufreq_power_cooling_register(np,
-				policy->related_cpus, power_coefficient, NULL);
+				policy, power_coefficient, NULL);
 		if (IS_ERR(cdev[cur_cluster])) {
 			dev_err(cpu_dev,
 				"running cpufreq without cooling device: %ld\n",
diff --git a/drivers/cpufreq/cpufreq-dt.c b/drivers/cpufreq/cpufreq-dt.c
index c943787d761e..fef3c2160691 100644
--- a/drivers/cpufreq/cpufreq-dt.c
+++ b/drivers/cpufreq/cpufreq-dt.c
@@ -326,7 +326,7 @@ static void cpufreq_ready(struct cpufreq_policy *policy)
 				     &power_coefficient);
 
 		priv->cdev = of_cpufreq_power_cooling_register(np,
-				policy->related_cpus, power_coefficient, NULL);
+				policy, power_coefficient, NULL);
 		if (IS_ERR(priv->cdev)) {
 			dev_err(priv->cpu_dev,
 				"running cpufreq without cooling device: %ld\n",
diff --git a/drivers/cpufreq/cpufreq.c b/drivers/cpufreq/cpufreq.c
index 0e3f6496524d..26b643d57847 100644
--- a/drivers/cpufreq/cpufreq.c
+++ b/drivers/cpufreq/cpufreq.c
@@ -2468,6 +2468,7 @@ int cpufreq_register_driver(struct cpufreq_driver *driver_data)
 	if (!(cpufreq_driver->flags & CPUFREQ_STICKY) &&
 	    list_empty(&cpufreq_policy_list)) {
 		/* if all ->init() calls failed, unregister */
+		ret = -ENODEV;
 		pr_debug("%s: No CPU initialized for driver %s\n", __func__,
 			 driver_data->name);
 		goto err_if_unreg;
diff --git a/drivers/cpufreq/cpufreq_stats.c b/drivers/cpufreq/cpufreq_stats.c
index f570ead62454..9c3d319dc129 100644
--- a/drivers/cpufreq/cpufreq_stats.c
+++ b/drivers/cpufreq/cpufreq_stats.c
@@ -170,11 +170,10 @@ void cpufreq_stats_create_table(struct cpufreq_policy *policy)
 	unsigned int i = 0, count = 0, ret = -ENOMEM;
 	struct cpufreq_stats *stats;
 	unsigned int alloc_size;
-	struct cpufreq_frequency_table *pos, *table;
+	struct cpufreq_frequency_table *pos;
 
-	/* We need cpufreq table for creating stats table */
-	table = policy->freq_table;
-	if (unlikely(!table))
+	count = cpufreq_table_count_valid_entries(policy);
+	if (!count)
 		return;
 
 	/* stats already initialized */
@@ -185,10 +184,6 @@ void cpufreq_stats_create_table(struct cpufreq_policy *policy)
 	if (!stats)
 		return;
 
-	/* Find total allocation size */
-	cpufreq_for_each_valid_entry(pos, table)
-		count++;
-
 	alloc_size = count * sizeof(int) + count * sizeof(u64);
 
 	alloc_size += count * count * sizeof(int);
@@ -205,7 +200,7 @@ void cpufreq_stats_create_table(struct cpufreq_policy *policy)
 	stats->max_state = count;
 
 	/* Find valid-unique entries */
-	cpufreq_for_each_valid_entry(pos, table)
+	cpufreq_for_each_valid_entry(pos, policy->freq_table)
 		if (freq_table_get_index(stats, pos->frequency) == -1)
 			stats->freq_table[i++] = pos->frequency;
 
diff --git a/drivers/cpufreq/dbx500-cpufreq.c b/drivers/cpufreq/dbx500-cpufreq.c
index 3575b82210ba..4ee0431579c1 100644
--- a/drivers/cpufreq/dbx500-cpufreq.c
+++ b/drivers/cpufreq/dbx500-cpufreq.c
@@ -43,7 +43,7 @@ static int dbx500_cpufreq_exit(struct cpufreq_policy *policy)
 
 static void dbx500_cpufreq_ready(struct cpufreq_policy *policy)
 {
-	cdev = cpufreq_cooling_register(policy->cpus);
+	cdev = cpufreq_cooling_register(policy);
 	if (IS_ERR(cdev))
 		pr_err("Failed to register cooling device %ld\n", PTR_ERR(cdev));
 	else
diff --git a/drivers/cpufreq/intel_pstate.c b/drivers/cpufreq/intel_pstate.c
index b7de5bd76a31..36ba6082d084 100644
--- a/drivers/cpufreq/intel_pstate.c
+++ b/drivers/cpufreq/intel_pstate.c
@@ -652,6 +652,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)
 {
@@ -663,17 +669,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:
@@ -711,15 +714,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);
diff --git a/drivers/cpufreq/kirkwood-cpufreq.c b/drivers/cpufreq/kirkwood-cpufreq.c
index 1b9bcd76c60e..c2dd43f3f5d8 100644
--- a/drivers/cpufreq/kirkwood-cpufreq.c
+++ b/drivers/cpufreq/kirkwood-cpufreq.c
@@ -127,7 +127,12 @@ static int kirkwood_cpufreq_probe(struct platform_device *pdev)
 		return PTR_ERR(priv.cpu_clk);
 	}
 
-	clk_prepare_enable(priv.cpu_clk);
+	err = clk_prepare_enable(priv.cpu_clk);
+	if (err) {
+		dev_err(priv.dev, "Unable to prepare cpuclk\n");
+		return err;
+	}
+
 	kirkwood_freq_table[0].frequency = clk_get_rate(priv.cpu_clk) / 1000;
 
 	priv.ddr_clk = of_clk_get_by_name(np, "ddrclk");
@@ -137,7 +142,11 @@ static int kirkwood_cpufreq_probe(struct platform_device *pdev)
 		goto out_cpu;
 	}
 
-	clk_prepare_enable(priv.ddr_clk);
+	err = clk_prepare_enable(priv.ddr_clk);
+	if (err) {
+		dev_err(priv.dev, "Unable to prepare ddrclk\n");
+		goto out_cpu;
+	}
 	kirkwood_freq_table[1].frequency = clk_get_rate(priv.ddr_clk) / 1000;
 
 	priv.powersave_clk = of_clk_get_by_name(np, "powersave");
@@ -146,7 +155,11 @@ static int kirkwood_cpufreq_probe(struct platform_device *pdev)
 		err = PTR_ERR(priv.powersave_clk);
 		goto out_ddr;
 	}
-	clk_prepare_enable(priv.powersave_clk);
+	err = clk_prepare_enable(priv.powersave_clk);
+	if (err) {
+		dev_err(priv.dev, "Unable to prepare powersave clk\n");
+		goto out_ddr;
+	}
 
 	of_node_put(np);
 	np = NULL;
diff --git a/drivers/cpufreq/mt8173-cpufreq.c b/drivers/cpufreq/mt8173-cpufreq.c
index fd1886faf33a..f9f00fb4bc3a 100644
--- a/drivers/cpufreq/mt8173-cpufreq.c
+++ b/drivers/cpufreq/mt8173-cpufreq.c
@@ -320,9 +320,7 @@ static void mtk_cpufreq_ready(struct cpufreq_policy *policy)
 		of_property_read_u32(np, DYNAMIC_POWER, &capacitance);
 
 		info->cdev = of_cpufreq_power_cooling_register(np,
-						policy->related_cpus,
-						capacitance,
-						NULL);
+						policy, capacitance, NULL);
 
 		if (IS_ERR(info->cdev)) {
 			dev_err(info->cpu_dev,
diff --git a/drivers/cpufreq/qoriq-cpufreq.c b/drivers/cpufreq/qoriq-cpufreq.c
index e2ea433a5f9c..4ada55b8856e 100644
--- a/drivers/cpufreq/qoriq-cpufreq.c
+++ b/drivers/cpufreq/qoriq-cpufreq.c
@@ -278,8 +278,7 @@ static void qoriq_cpufreq_ready(struct cpufreq_policy *policy)
 	struct device_node *np = of_get_cpu_node(policy->cpu, NULL);
 
 	if (of_find_property(np, "#cooling-cells", NULL)) {
-		cpud->cdev = of_cpufreq_cooling_register(np,
-							 policy->related_cpus);
+		cpud->cdev = of_cpufreq_cooling_register(np, policy);
 
 		if (IS_ERR(cpud->cdev) && PTR_ERR(cpud->cdev) != -ENOSYS) {
 			pr_err("cpu%d is not running as cooling device: %ld\n",
diff --git a/drivers/cpuidle/cpuidle-powernv.c b/drivers/cpuidle/cpuidle-powernv.c
index 12409a519cc5..45eaf06462ae 100644
--- a/drivers/cpuidle/cpuidle-powernv.c
+++ b/drivers/cpuidle/cpuidle-powernv.c
@@ -354,6 +354,7 @@ static int powernv_add_idle_states(void)
 
 	for (i = 0; i < dt_idle_states; i++) {
 		unsigned int exit_latency, target_residency;
+		bool stops_timebase = false;
 		/*
 		 * If an idle state has exit latency beyond
 		 * POWERNV_THRESHOLD_LATENCY_NS then don't use it
@@ -381,6 +382,9 @@ static int powernv_add_idle_states(void)
 			}
 		}
 
+		if (flags[i] & OPAL_PM_TIMEBASE_STOP)
+			stops_timebase = true;
+
 		/*
 		 * For nap and fastsleep, use default target_residency
 		 * values if f/w does not expose it.
@@ -392,8 +396,7 @@ static int powernv_add_idle_states(void)
 			add_powernv_state(nr_idle_states, "Nap",
 					  CPUIDLE_FLAG_NONE, nap_loop,
 					  target_residency, exit_latency, 0, 0);
-		} else if ((flags[i] & OPAL_PM_STOP_INST_FAST) &&
-				!(flags[i] & OPAL_PM_TIMEBASE_STOP)) {
+		} else if (has_stop_states && !stops_timebase) {
 			add_powernv_state(nr_idle_states, names[i],
 					  CPUIDLE_FLAG_NONE, stop_loop,
 					  target_residency, exit_latency,
@@ -405,8 +408,8 @@ static int powernv_add_idle_states(void)
 		 * within this config dependency check.
 		 */
 #ifdef CONFIG_TICK_ONESHOT
-		if (flags[i] & OPAL_PM_SLEEP_ENABLED ||
-			flags[i] & OPAL_PM_SLEEP_ENABLED_ER1) {
+		else if (flags[i] & OPAL_PM_SLEEP_ENABLED ||
+			 flags[i] & OPAL_PM_SLEEP_ENABLED_ER1) {
 			if (!rc)
 				target_residency = 300000;
 			/* Add FASTSLEEP state */
@@ -414,14 +417,15 @@ static int powernv_add_idle_states(void)
 					  CPUIDLE_FLAG_TIMER_STOP,
 					  fastsleep_loop,
 					  target_residency, exit_latency, 0, 0);
-		} else if ((flags[i] & OPAL_PM_STOP_INST_DEEP) &&
-				(flags[i] & OPAL_PM_TIMEBASE_STOP)) {
+		} else if (has_stop_states && stops_timebase) {
 			add_powernv_state(nr_idle_states, names[i],
 					  CPUIDLE_FLAG_TIMER_STOP, stop_loop,
 					  target_residency, exit_latency,
 					  psscr_val[i], psscr_mask[i]);
 		}
 #endif
+		else
+			continue;
 		nr_idle_states++;
 	}
 out:
diff --git a/drivers/crypto/Kconfig b/drivers/crypto/Kconfig
index fb1e60f5002e..9c7951bb05ac 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).
diff --git a/drivers/crypto/bcm/cipher.c b/drivers/crypto/bcm/cipher.c
index cc0d5b98006e..61393dc70b0b 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);
diff --git a/drivers/crypto/caam/caampkc.c b/drivers/crypto/caam/caampkc.c
index 49cbdcba7883..57f399caa977 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)
 {
@@ -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:
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/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/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-dev.c b/drivers/crypto/ccp/ccp-dev.c
index 92d1c6959f08..b7504562715c 100644
--- a/drivers/crypto/ccp/ccp-dev.c
+++ b/drivers/crypto/ccp/ccp-dev.c
@@ -31,6 +31,7 @@
 #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_DESCRIPTION("AMD Cryptographic Coprocessor driver");
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/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-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/omap-sham.c b/drivers/crypto/omap-sham.c
index d0b16e5e4ee5..1864a57caaa4 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
@@ -1326,8 +1327,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/dax/super.c b/drivers/dax/super.c
index ebf43f531ada..6ed32aac8bbe 100644
--- a/drivers/dax/super.c
+++ b/drivers/dax/super.c
@@ -44,6 +44,7 @@ void dax_read_unlock(int id)
 }
 EXPORT_SYMBOL_GPL(dax_read_unlock);
 
+#ifdef CONFIG_BLOCK
 int bdev_dax_pgoff(struct block_device *bdev, sector_t sector, size_t size,
 		pgoff_t *pgoff)
 {
@@ -112,6 +113,7 @@ int __bdev_dax_supported(struct super_block *sb, int blocksize)
 	return 0;
 }
 EXPORT_SYMBOL_GPL(__bdev_dax_supported);
+#endif
 
 /**
  * struct dax_device - anchor object for dax services
diff --git a/drivers/edac/amd64_edac.c b/drivers/edac/amd64_edac.c
index 82dab1692264..3aea55698165 100644
--- a/drivers/edac/amd64_edac.c
+++ b/drivers/edac/amd64_edac.c
@@ -782,24 +782,26 @@ static void debug_dump_dramcfg_low(struct amd64_pvt *pvt, u32 dclr, int chan)
 
 static void debug_display_dimm_sizes_df(struct amd64_pvt *pvt, u8 ctrl)
 {
-	u32 *dcsb = ctrl ? pvt->csels[1].csbases : pvt->csels[0].csbases;
-	int dimm, size0, size1;
+	int dimm, size0, size1, cs0, cs1;
 
 	edac_printk(KERN_DEBUG, EDAC_MC, "UMC%d chip selects:\n", ctrl);
 
 	for (dimm = 0; dimm < 4; dimm++) {
 		size0 = 0;
+		cs0 = dimm * 2;
 
-		if (dcsb[dimm*2] & DCSB_CS_ENABLE)
-			size0 = pvt->ops->dbam_to_cs(pvt, ctrl, 0, dimm);
+		if (csrow_enabled(cs0, ctrl, pvt))
+			size0 = pvt->ops->dbam_to_cs(pvt, ctrl, 0, cs0);
 
 		size1 = 0;
-		if (dcsb[dimm*2 + 1] & DCSB_CS_ENABLE)
-			size1 = pvt->ops->dbam_to_cs(pvt, ctrl, 0, dimm);
+		cs1 = dimm * 2 + 1;
+
+		if (csrow_enabled(cs1, ctrl, pvt))
+			size1 = pvt->ops->dbam_to_cs(pvt, ctrl, 0, cs1);
 
 		amd64_info(EDAC_MC ": %d: %5dMB %d: %5dMB\n",
-				dimm * 2,     size0,
-				dimm * 2 + 1, size1);
+				cs0,	size0,
+				cs1,	size1);
 	}
 }
 
@@ -2756,26 +2758,22 @@ skip:
  *	encompasses
  *
  */
-static u32 get_csrow_nr_pages(struct amd64_pvt *pvt, u8 dct, int csrow_nr)
+static u32 get_csrow_nr_pages(struct amd64_pvt *pvt, u8 dct, int csrow_nr_orig)
 {
-	u32 cs_mode, nr_pages;
 	u32 dbam = dct ? pvt->dbam1 : pvt->dbam0;
+	int csrow_nr = csrow_nr_orig;
+	u32 cs_mode, nr_pages;
 
+	if (!pvt->umc)
+		csrow_nr >>= 1;
 
-	/*
-	 * The math on this doesn't look right on the surface because x/2*4 can
-	 * be simplified to x*2 but this expression makes use of the fact that
-	 * it is integral math where 1/2=0. This intermediate value becomes the
-	 * number of bits to shift the DBAM register to extract the proper CSROW
-	 * field.
-	 */
-	cs_mode = DBAM_DIMM(csrow_nr / 2, dbam);
+	cs_mode = DBAM_DIMM(csrow_nr, dbam);
 
-	nr_pages = pvt->ops->dbam_to_cs(pvt, dct, cs_mode, (csrow_nr / 2))
-							   << (20 - PAGE_SHIFT);
+	nr_pages   = pvt->ops->dbam_to_cs(pvt, dct, cs_mode, csrow_nr);
+	nr_pages <<= 20 - PAGE_SHIFT;
 
 	edac_dbg(0, "csrow: %d, channel: %d, DBAM idx: %d\n",
-		    csrow_nr, dct,  cs_mode);
+		    csrow_nr_orig, dct,  cs_mode);
 	edac_dbg(0, "nr_pages/channel: %u\n", nr_pages);
 
 	return nr_pages;
diff --git a/drivers/firewire/net.c b/drivers/firewire/net.c
index 5d3640264f2d..655c259e37fd 100644
--- a/drivers/firewire/net.c
+++ b/drivers/firewire/net.c
@@ -1482,9 +1482,14 @@ static int fwnet_probe(struct fw_unit *unit,
 		goto out;
 	dev->local_fifo = dev->handler.offset;
 
+	/*
+	 * default MTU: RFC 2734 cl. 4, RFC 3146 cl. 4
+	 * maximum MTU: RFC 2734 cl. 4.2, fragment encapsulation header's
+	 *              maximum possible datagram_size + 1 = 0xfff + 1
+	 */
 	net->mtu = 1500U;
 	net->min_mtu = ETH_MIN_MTU;
-	net->max_mtu = 0xfff;
+	net->max_mtu = 4096U;
 
 	/* Set our hardware address while we're at it */
 	ha = (union fwnet_hwaddr *)net->dev_addr;
diff --git a/drivers/firmware/dmi-id.c b/drivers/firmware/dmi-id.c
index 44c01390d035..dc269cb288c2 100644
--- a/drivers/firmware/dmi-id.c
+++ b/drivers/firmware/dmi-id.c
@@ -47,6 +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(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);
@@ -191,6 +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(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 54be60ead08f..82ee042f075c 100644
--- a/drivers/firmware/dmi_scan.c
+++ b/drivers/firmware/dmi_scan.c
@@ -430,6 +430,7 @@ static void __init dmi_decode(const struct dmi_header *dm, void *dummy)
 		dmi_save_ident(dm, DMI_PRODUCT_VERSION, 6);
 		dmi_save_ident(dm, DMI_PRODUCT_SERIAL, 7);
 		dmi_save_uuid(dm, DMI_PRODUCT_UUID, 8);
+		dmi_save_ident(dm, DMI_PRODUCT_FAMILY, 26);
 		break;
 	case 2:		/* Base Board Information */
 		dmi_save_ident(dm, DMI_BOARD_VENDOR, 4);
@@ -649,6 +650,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
@@ -658,7 +674,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;
diff --git a/drivers/firmware/efi/efi-pstore.c b/drivers/firmware/efi/efi-pstore.c
index ed3137c1ceb0..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);
@@ -53,6 +53,7 @@ static int efi_pstore_read_func(struct efivar_entry *entry,
 	if (sscanf(name, "dump-type%u-%u-%d-%lu-%c",
 		   &record->type, &part, &cnt, &time, &data_type) == 5) {
 		record->id = generic_id(time, part, cnt);
+		record->part = part;
 		record->count = cnt;
 		record->time.tv_sec = time;
 		record->time.tv_nsec = 0;
@@ -64,6 +65,7 @@ static int efi_pstore_read_func(struct efivar_entry *entry,
 	} else if (sscanf(name, "dump-type%u-%u-%d-%lu",
 		   &record->type, &part, &cnt, &time) == 4) {
 		record->id = generic_id(time, part, cnt);
+		record->part = part;
 		record->count = cnt;
 		record->time.tv_sec = time;
 		record->time.tv_nsec = 0;
@@ -77,6 +79,7 @@ static int efi_pstore_read_func(struct efivar_entry *entry,
 		 * multiple logs, remains.
 		 */
 		record->id = generic_id(time, part, 0);
+		record->part = part;
 		record->count = 0;
 		record->time.tv_sec = time;
 		record->time.tv_nsec = 0;
@@ -155,19 +158,14 @@ static int efi_pstore_scan_sysfs_exit(struct efivar_entry *pos,
  * efi_pstore_sysfs_entry_iter
  *
  * @record: pstore record to pass to callback
- * @pos: entry to begin iterating from
  *
  * You MUST call efivar_enter_iter_begin() before this function, and
  * efivar_entry_iter_end() afterwards.
  *
- * It is possible to begin iteration from an arbitrary entry within
- * the list by passing @pos. @pos is updated on return to point to
- * the next entry of the last one passed to efi_pstore_read_func().
- * To begin iterating from the beginning of the list @pos must be %NULL.
  */
-static int efi_pstore_sysfs_entry_iter(struct pstore_record *record,
-				       struct efivar_entry **pos)
+static int efi_pstore_sysfs_entry_iter(struct pstore_record *record)
 {
+	struct efivar_entry **pos = (struct efivar_entry **)&record->psi->data;
 	struct efivar_entry *entry, *n;
 	struct list_head *head = &efivar_sysfs_list;
 	int size = 0;
@@ -218,7 +216,6 @@ static int efi_pstore_sysfs_entry_iter(struct pstore_record *record,
  */
 static ssize_t efi_pstore_read(struct pstore_record *record)
 {
-	struct efivar_entry *entry = (struct efivar_entry *)record->psi->data;
 	ssize_t size;
 
 	record->buf = kzalloc(EFIVARS_DATA_SIZE_MAX, GFP_KERNEL);
@@ -229,7 +226,7 @@ static ssize_t efi_pstore_read(struct pstore_record *record)
 		size = -EINTR;
 		goto out;
 	}
-	size = efi_pstore_sysfs_entry_iter(record, &entry);
+	size = efi_pstore_sysfs_entry_iter(record);
 	efivar_entry_iter_end();
 
 out:
@@ -247,9 +244,15 @@ static int efi_pstore_write(struct pstore_record *record)
 	efi_guid_t vendor = LINUX_EFI_CRASH_GUID;
 	int i, ret = 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,
-		 get_seconds(), record->compressed ? 'C' : 'D');
+		 record->time.tv_sec, record->compressed ? 'C' : 'D');
 
 	for (i = 0; i < DUMP_NAME_LEN; i++)
 		efi_name[i] = name[i];
@@ -261,48 +264,23 @@ static int efi_pstore_write(struct pstore_record *record)
 	if (record->reason == KMSG_DUMP_OOPS)
 		efivar_run_worker();
 
-	record->id = record->part;
 	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, (unsigned int)ed->record->id,
-			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) {
 		/*
@@ -319,38 +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;
-	unsigned int part;
-
-	do_div(record->id, 1000);
-	part = do_div(record->id, 100);
-	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/google/vpd.c b/drivers/firmware/google/vpd.c
index 3ce813110d5e..31058d400bda 100644
--- a/drivers/firmware/google/vpd.c
+++ b/drivers/firmware/google/vpd.c
@@ -116,9 +116,13 @@ static int vpd_section_attrib_add(const u8 *key, s32 key_len,
 		return VPD_OK;
 
 	info = kzalloc(sizeof(*info), GFP_KERNEL);
-	info->key = kzalloc(key_len + 1, GFP_KERNEL);
-	if (!info->key)
+	if (!info)
 		return -ENOMEM;
+	info->key = kzalloc(key_len + 1, GFP_KERNEL);
+	if (!info->key) {
+		ret = -ENOMEM;
+		goto free_info;
+	}
 
 	memcpy(info->key, key, key_len);
 
@@ -132,15 +136,20 @@ static int vpd_section_attrib_add(const u8 *key, s32 key_len,
 	info->value = value;
 
 	INIT_LIST_HEAD(&info->list);
-	list_add_tail(&info->list, &sec->attribs);
 
 	ret = sysfs_create_bin_file(sec->kobj, &info->bin_attr);
-	if (ret) {
-		kfree(info->key);
-		return ret;
-	}
+	if (ret)
+		goto free_info_key;
 
+	list_add_tail(&info->list, &sec->attribs);
 	return 0;
+
+free_info_key:
+	kfree(info->key);
+free_info:
+	kfree(info);
+
+	return ret;
 }
 
 static void vpd_section_attrib_destroy(struct vpd_section *sec)
@@ -149,8 +158,8 @@ static void vpd_section_attrib_destroy(struct vpd_section *sec)
 	struct vpd_attrib_info *temp;
 
 	list_for_each_entry_safe(info, temp, &sec->attribs, list) {
-		kfree(info->key);
 		sysfs_remove_bin_file(sec->kobj, &info->bin_attr);
+		kfree(info->key);
 		kfree(info);
 	}
 }
@@ -235,7 +244,7 @@ static int vpd_section_destroy(struct vpd_section *sec)
 {
 	if (sec->enabled) {
 		vpd_section_attrib_destroy(sec);
-		kobject_del(sec->kobj);
+		kobject_put(sec->kobj);
 		sysfs_remove_bin_file(vpd_kobj, &sec->bin_attr);
 		kfree(sec->raw_name);
 		iounmap(sec->baseaddr);
@@ -322,7 +331,7 @@ static void __exit vpd_platform_exit(void)
 {
 	vpd_section_destroy(&ro_vpd);
 	vpd_section_destroy(&rw_vpd);
-	kobject_del(vpd_kobj);
+	kobject_put(vpd_kobj);
 }
 
 module_init(vpd_platform_init);
diff --git a/drivers/firmware/ti_sci.c b/drivers/firmware/ti_sci.c
index 874ff32db366..00cfed3c3e1a 100644
--- a/drivers/firmware/ti_sci.c
+++ b/drivers/firmware/ti_sci.c
@@ -202,7 +202,8 @@ static int ti_sci_debugfs_create(struct platform_device *pdev,
 	info->debug_buffer[info->debug_region_size] = 0;
 
 	info->d = debugfs_create_file(strncat(debug_name, dev_name(dev),
-					      sizeof(debug_name)),
+					      sizeof(debug_name) -
+					      sizeof("ti_sci_debug@")),
 				      0444, NULL, info, &ti_sci_debug_fops);
 	if (IS_ERR(info->d))
 		return PTR_ERR(info->d);
diff --git a/drivers/gpu/drm/amd/amdgpu/amdgpu_fb.c b/drivers/gpu/drm/amd/amdgpu/amdgpu_fb.c
index 236d9950221b..c0d8c6ff6380 100644
--- a/drivers/gpu/drm/amd/amdgpu/amdgpu_fb.c
+++ b/drivers/gpu/drm/amd/amdgpu/amdgpu_fb.c
@@ -425,10 +425,15 @@ bool amdgpu_fbdev_robj_is_fb(struct amdgpu_device *adev, struct amdgpu_bo *robj)
 
 void amdgpu_fbdev_restore_mode(struct amdgpu_device *adev)
 {
-	struct amdgpu_fbdev *afbdev = adev->mode_info.rfbdev;
+	struct amdgpu_fbdev *afbdev;
 	struct drm_fb_helper *fb_helper;
 	int ret;
 
+	if (!adev)
+		return;
+
+	afbdev = adev->mode_info.rfbdev;
+
 	if (!afbdev)
 		return;
 
diff --git a/drivers/gpu/drm/amd/amdgpu/amdgpu_vm.c b/drivers/gpu/drm/amd/amdgpu/amdgpu_vm.c
index 749a6cde7985..83c172a6e938 100644
--- a/drivers/gpu/drm/amd/amdgpu/amdgpu_vm.c
+++ b/drivers/gpu/drm/amd/amdgpu/amdgpu_vm.c
@@ -635,7 +635,7 @@ int amdgpu_vm_flush(struct amdgpu_ring *ring, struct amdgpu_job *job)
 		mutex_unlock(&id_mgr->lock);
 	}
 
-	if (gds_switch_needed) {
+	if (ring->funcs->emit_gds_switch && gds_switch_needed) {
 		id->gds_base = job->gds_base;
 		id->gds_size = job->gds_size;
 		id->gws_base = job->gws_base;
@@ -673,6 +673,7 @@ void amdgpu_vm_reset_id(struct amdgpu_device *adev, unsigned vmhub,
 	struct amdgpu_vm_id_manager *id_mgr = &adev->vm_manager.id_mgr[vmhub];
 	struct amdgpu_vm_id *id = &id_mgr->ids[vmid];
 
+	atomic64_set(&id->owner, 0);
 	id->gds_base = 0;
 	id->gds_size = 0;
 	id->gws_base = 0;
@@ -682,6 +683,26 @@ void amdgpu_vm_reset_id(struct amdgpu_device *adev, unsigned vmhub,
 }
 
 /**
+ * amdgpu_vm_reset_all_id - reset VMID to zero
+ *
+ * @adev: amdgpu device structure
+ *
+ * Reset VMID to force flush on next use
+ */
+void amdgpu_vm_reset_all_ids(struct amdgpu_device *adev)
+{
+	unsigned i, j;
+
+	for (i = 0; i < AMDGPU_MAX_VMHUBS; ++i) {
+		struct amdgpu_vm_id_manager *id_mgr =
+			&adev->vm_manager.id_mgr[i];
+
+		for (j = 1; j < id_mgr->num_ids; ++j)
+			amdgpu_vm_reset_id(adev, i, j);
+	}
+}
+
+/**
  * amdgpu_vm_bo_find - find the bo_va for a specific vm & bo
  *
  * @vm: requested vm
@@ -2271,7 +2292,6 @@ void amdgpu_vm_manager_init(struct amdgpu_device *adev)
 	for (i = 0; i < AMDGPU_MAX_RINGS; ++i)
 		adev->vm_manager.seqno[i] = 0;
 
-
 	atomic_set(&adev->vm_manager.vm_pte_next_ring, 0);
 	atomic64_set(&adev->vm_manager.client_counter, 0);
 	spin_lock_init(&adev->vm_manager.prt_lock);
diff --git a/drivers/gpu/drm/amd/amdgpu/amdgpu_vm.h b/drivers/gpu/drm/amd/amdgpu/amdgpu_vm.h
index d97e28b4bdc4..e1d951ece433 100644
--- a/drivers/gpu/drm/amd/amdgpu/amdgpu_vm.h
+++ b/drivers/gpu/drm/amd/amdgpu/amdgpu_vm.h
@@ -204,6 +204,7 @@ int amdgpu_vm_grab_id(struct amdgpu_vm *vm, struct amdgpu_ring *ring,
 int amdgpu_vm_flush(struct amdgpu_ring *ring, struct amdgpu_job *job);
 void amdgpu_vm_reset_id(struct amdgpu_device *adev, unsigned vmhub,
 			unsigned vmid);
+void amdgpu_vm_reset_all_ids(struct amdgpu_device *adev);
 int amdgpu_vm_update_directories(struct amdgpu_device *adev,
 				 struct amdgpu_vm *vm);
 int amdgpu_vm_clear_freed(struct amdgpu_device *adev,
diff --git a/drivers/gpu/drm/amd/amdgpu/ci_dpm.c b/drivers/gpu/drm/amd/amdgpu/ci_dpm.c
index 00e56a28b593..cb508a211b2f 100644
--- a/drivers/gpu/drm/amd/amdgpu/ci_dpm.c
+++ b/drivers/gpu/drm/amd/amdgpu/ci_dpm.c
@@ -906,6 +906,12 @@ static bool ci_dpm_vblank_too_short(struct amdgpu_device *adev)
 	u32 vblank_time = amdgpu_dpm_get_vblank_time(adev);
 	u32 switch_limit = adev->mc.vram_type == AMDGPU_VRAM_TYPE_GDDR5 ? 450 : 300;
 
+	/* disable mclk switching if the refresh is >120Hz, even if the
+	 * blanking period would allow it
+	 */
+	if (amdgpu_dpm_get_vrefresh(adev) > 120)
+		return true;
+
 	if (vblank_time < switch_limit)
 		return true;
 	else
diff --git a/drivers/gpu/drm/amd/amdgpu/gmc_v6_0.c b/drivers/gpu/drm/amd/amdgpu/gmc_v6_0.c
index 2a3983036a30..9776ad3d2d71 100644
--- a/drivers/gpu/drm/amd/amdgpu/gmc_v6_0.c
+++ b/drivers/gpu/drm/amd/amdgpu/gmc_v6_0.c
@@ -950,10 +950,6 @@ static int gmc_v6_0_suspend(void *handle)
 {
 	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
 
-	if (adev->vm_manager.enabled) {
-		gmc_v6_0_vm_fini(adev);
-		adev->vm_manager.enabled = false;
-	}
 	gmc_v6_0_hw_fini(adev);
 
 	return 0;
@@ -968,16 +964,9 @@ static int gmc_v6_0_resume(void *handle)
 	if (r)
 		return r;
 
-	if (!adev->vm_manager.enabled) {
-		r = gmc_v6_0_vm_init(adev);
-		if (r) {
-			dev_err(adev->dev, "vm manager initialization failed (%d).\n", r);
-			return r;
-		}
-		adev->vm_manager.enabled = true;
-	}
+	amdgpu_vm_reset_all_ids(adev);
 
-	return r;
+	return 0;
 }
 
 static bool gmc_v6_0_is_idle(void *handle)
diff --git a/drivers/gpu/drm/amd/amdgpu/gmc_v7_0.c b/drivers/gpu/drm/amd/amdgpu/gmc_v7_0.c
index 6d347c1d2516..fca8e77182c9 100644
--- a/drivers/gpu/drm/amd/amdgpu/gmc_v7_0.c
+++ b/drivers/gpu/drm/amd/amdgpu/gmc_v7_0.c
@@ -1117,10 +1117,6 @@ static int gmc_v7_0_suspend(void *handle)
 {
 	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
 
-	if (adev->vm_manager.enabled) {
-		gmc_v7_0_vm_fini(adev);
-		adev->vm_manager.enabled = false;
-	}
 	gmc_v7_0_hw_fini(adev);
 
 	return 0;
@@ -1135,16 +1131,9 @@ static int gmc_v7_0_resume(void *handle)
 	if (r)
 		return r;
 
-	if (!adev->vm_manager.enabled) {
-		r = gmc_v7_0_vm_init(adev);
-		if (r) {
-			dev_err(adev->dev, "vm manager initialization failed (%d).\n", r);
-			return r;
-		}
-		adev->vm_manager.enabled = true;
-	}
+	amdgpu_vm_reset_all_ids(adev);
 
-	return r;
+	return 0;
 }
 
 static bool gmc_v7_0_is_idle(void *handle)
diff --git a/drivers/gpu/drm/amd/amdgpu/gmc_v8_0.c b/drivers/gpu/drm/amd/amdgpu/gmc_v8_0.c
index 2784ff49cf56..e9c127037b39 100644
--- a/drivers/gpu/drm/amd/amdgpu/gmc_v8_0.c
+++ b/drivers/gpu/drm/amd/amdgpu/gmc_v8_0.c
@@ -1209,10 +1209,6 @@ static int gmc_v8_0_suspend(void *handle)
 {
 	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
 
-	if (adev->vm_manager.enabled) {
-		gmc_v8_0_vm_fini(adev);
-		adev->vm_manager.enabled = false;
-	}
 	gmc_v8_0_hw_fini(adev);
 
 	return 0;
@@ -1227,16 +1223,9 @@ static int gmc_v8_0_resume(void *handle)
 	if (r)
 		return r;
 
-	if (!adev->vm_manager.enabled) {
-		r = gmc_v8_0_vm_init(adev);
-		if (r) {
-			dev_err(adev->dev, "vm manager initialization failed (%d).\n", r);
-			return r;
-		}
-		adev->vm_manager.enabled = true;
-	}
+	amdgpu_vm_reset_all_ids(adev);
 
-	return r;
+	return 0;
 }
 
 static bool gmc_v8_0_is_idle(void *handle)
diff --git a/drivers/gpu/drm/amd/amdgpu/gmc_v9_0.c b/drivers/gpu/drm/amd/amdgpu/gmc_v9_0.c
index dc1e1c1d6b24..f936332a069d 100644
--- a/drivers/gpu/drm/amd/amdgpu/gmc_v9_0.c
+++ b/drivers/gpu/drm/amd/amdgpu/gmc_v9_0.c
@@ -791,10 +791,6 @@ static int gmc_v9_0_suspend(void *handle)
 {
 	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
 
-	if (adev->vm_manager.enabled) {
-		gmc_v9_0_vm_fini(adev);
-		adev->vm_manager.enabled = false;
-	}
 	gmc_v9_0_hw_fini(adev);
 
 	return 0;
@@ -809,17 +805,9 @@ static int gmc_v9_0_resume(void *handle)
 	if (r)
 		return r;
 
-	if (!adev->vm_manager.enabled) {
-		r = gmc_v9_0_vm_init(adev);
-		if (r) {
-			dev_err(adev->dev,
-				"vm manager initialization failed (%d).\n", r);
-			return r;
-		}
-		adev->vm_manager.enabled = true;
-	}
+	amdgpu_vm_reset_all_ids(adev);
 
-	return r;
+	return 0;
 }
 
 static bool gmc_v9_0_is_idle(void *handle)
diff --git a/drivers/gpu/drm/amd/powerplay/hwmgr/smu7_hwmgr.c b/drivers/gpu/drm/amd/powerplay/hwmgr/smu7_hwmgr.c
index 975567f6813d..1f01020ce3a9 100644
--- a/drivers/gpu/drm/amd/powerplay/hwmgr/smu7_hwmgr.c
+++ b/drivers/gpu/drm/amd/powerplay/hwmgr/smu7_hwmgr.c
@@ -2655,6 +2655,28 @@ static int smu7_get_power_state_size(struct pp_hwmgr *hwmgr)
 	return sizeof(struct smu7_power_state);
 }
 
+static int smu7_vblank_too_short(struct pp_hwmgr *hwmgr,
+				 uint32_t vblank_time_us)
+{
+	struct smu7_hwmgr *data = (struct smu7_hwmgr *)(hwmgr->backend);
+	uint32_t switch_limit_us;
+
+	switch (hwmgr->chip_id) {
+	case CHIP_POLARIS10:
+	case CHIP_POLARIS11:
+	case CHIP_POLARIS12:
+		switch_limit_us = data->is_memory_gddr5 ? 190 : 150;
+		break;
+	default:
+		switch_limit_us = data->is_memory_gddr5 ? 450 : 150;
+		break;
+	}
+
+	if (vblank_time_us < switch_limit_us)
+		return true;
+	else
+		return false;
+}
 
 static int smu7_apply_state_adjust_rules(struct pp_hwmgr *hwmgr,
 				struct pp_power_state *request_ps,
@@ -2669,6 +2691,7 @@ static int smu7_apply_state_adjust_rules(struct pp_hwmgr *hwmgr,
 	bool disable_mclk_switching;
 	bool disable_mclk_switching_for_frame_lock;
 	struct cgs_display_info info = {0};
+	struct cgs_mode_info mode_info = {0};
 	const struct phm_clock_and_voltage_limits *max_limits;
 	uint32_t i;
 	struct smu7_hwmgr *data = (struct smu7_hwmgr *)(hwmgr->backend);
@@ -2677,6 +2700,7 @@ static int smu7_apply_state_adjust_rules(struct pp_hwmgr *hwmgr,
 	int32_t count;
 	int32_t stable_pstate_sclk = 0, stable_pstate_mclk = 0;
 
+	info.mode_info = &mode_info;
 	data->battery_state = (PP_StateUILabel_Battery ==
 			request_ps->classification.ui_label);
 
@@ -2703,8 +2727,6 @@ static int smu7_apply_state_adjust_rules(struct pp_hwmgr *hwmgr,
 
 	cgs_get_active_displays_info(hwmgr->device, &info);
 
-	/*TO DO result = PHM_CheckVBlankTime(hwmgr, &vblankTooShort);*/
-
 	minimum_clocks.engineClock = hwmgr->display_config.min_core_set_clock;
 	minimum_clocks.memoryClock = hwmgr->display_config.min_mem_set_clock;
 
@@ -2769,8 +2791,10 @@ static int smu7_apply_state_adjust_rules(struct pp_hwmgr *hwmgr,
 				    PHM_PlatformCaps_DisableMclkSwitchingForFrameLock);
 
 
-	disable_mclk_switching = (1 < info.display_count) ||
-				    disable_mclk_switching_for_frame_lock;
+	disable_mclk_switching = ((1 < info.display_count) ||
+				  disable_mclk_switching_for_frame_lock ||
+				  smu7_vblank_too_short(hwmgr, mode_info.vblank_time_us) ||
+				  (mode_info.refresh_rate > 120));
 
 	sclk = smu7_ps->performance_levels[0].engine_clock;
 	mclk = smu7_ps->performance_levels[0].memory_clock;
diff --git a/drivers/gpu/drm/amd/powerplay/hwmgr/vega10_hwmgr.c b/drivers/gpu/drm/amd/powerplay/hwmgr/vega10_hwmgr.c
index cd7bf6d3859a..ab17350e853d 100644
--- a/drivers/gpu/drm/amd/powerplay/hwmgr/vega10_hwmgr.c
+++ b/drivers/gpu/drm/amd/powerplay/hwmgr/vega10_hwmgr.c
@@ -4187,7 +4187,7 @@ static int vega10_force_clock_level(struct pp_hwmgr *hwmgr,
 		enum pp_clock_type type, uint32_t mask)
 {
 	struct vega10_hwmgr *data = (struct vega10_hwmgr *)(hwmgr->backend);
-	uint32_t i;
+	int i;
 
 	if (hwmgr->dpm_level != AMD_DPM_FORCED_LEVEL_MANUAL)
 		return -EINVAL;
diff --git a/drivers/gpu/drm/arm/hdlcd_crtc.c b/drivers/gpu/drm/arm/hdlcd_crtc.c
index 798a3cc480a2..1a3359c0f6cd 100644
--- a/drivers/gpu/drm/arm/hdlcd_crtc.c
+++ b/drivers/gpu/drm/arm/hdlcd_crtc.c
@@ -10,6 +10,7 @@
  */
 
 #include <drm/drmP.h>
+#include <drm/drm_atomic.h>
 #include <drm/drm_atomic_helper.h>
 #include <drm/drm_crtc.h>
 #include <drm/drm_crtc_helper.h>
@@ -226,16 +227,33 @@ static const struct drm_crtc_helper_funcs hdlcd_crtc_helper_funcs = {
 static int hdlcd_plane_atomic_check(struct drm_plane *plane,
 				    struct drm_plane_state *state)
 {
-	u32 src_w, src_h;
+	struct drm_rect clip = { 0 };
+	struct drm_crtc_state *crtc_state;
+	u32 src_h = state->src_h >> 16;
 
-	src_w = state->src_w >> 16;
-	src_h = state->src_h >> 16;
+	/* only the HDLCD_REG_FB_LINE_COUNT register has a limit */
+	if (src_h >= HDLCD_MAX_YRES) {
+		DRM_DEBUG_KMS("Invalid source width: %d\n", src_h);
+		return -EINVAL;
+	}
+
+	if (!state->fb || !state->crtc)
+		return 0;
 
-	/* we can't do any scaling of the plane source */
-	if ((src_w != state->crtc_w) || (src_h != state->crtc_h))
+	crtc_state = drm_atomic_get_existing_crtc_state(state->state,
+							state->crtc);
+	if (!crtc_state) {
+		DRM_DEBUG_KMS("Invalid crtc state\n");
 		return -EINVAL;
+	}
 
-	return 0;
+	clip.x2 = crtc_state->adjusted_mode.hdisplay;
+	clip.y2 = crtc_state->adjusted_mode.vdisplay;
+
+	return drm_plane_helper_check_state(state, &clip,
+					    DRM_PLANE_HELPER_NO_SCALING,
+					    DRM_PLANE_HELPER_NO_SCALING,
+					    false, true);
 }
 
 static void hdlcd_plane_atomic_update(struct drm_plane *plane,
@@ -244,21 +262,20 @@ static void hdlcd_plane_atomic_update(struct drm_plane *plane,
 	struct drm_framebuffer *fb = plane->state->fb;
 	struct hdlcd_drm_private *hdlcd;
 	struct drm_gem_cma_object *gem;
-	u32 src_w, src_h, dest_w, dest_h;
+	u32 src_x, src_y, dest_h;
 	dma_addr_t scanout_start;
 
 	if (!fb)
 		return;
 
-	src_w = plane->state->src_w >> 16;
-	src_h = plane->state->src_h >> 16;
-	dest_w = plane->state->crtc_w;
-	dest_h = plane->state->crtc_h;
+	src_x = plane->state->src.x1 >> 16;
+	src_y = plane->state->src.y1 >> 16;
+	dest_h = drm_rect_height(&plane->state->dst);
 	gem = drm_fb_cma_get_gem_obj(fb, 0);
+
 	scanout_start = gem->paddr + fb->offsets[0] +
-		plane->state->crtc_y * fb->pitches[0] +
-		plane->state->crtc_x *
-		fb->format->cpp[0];
+			src_y * fb->pitches[0] +
+			src_x *	fb->format->cpp[0];
 
 	hdlcd = plane->dev->dev_private;
 	hdlcd_write(hdlcd, HDLCD_REG_FB_LINE_LENGTH, fb->pitches[0]);
@@ -305,7 +322,6 @@ static struct drm_plane *hdlcd_plane_init(struct drm_device *drm)
 				       formats, ARRAY_SIZE(formats),
 				       DRM_PLANE_TYPE_PRIMARY, NULL);
 	if (ret) {
-		devm_kfree(drm->dev, plane);
 		return ERR_PTR(ret);
 	}
 
@@ -329,7 +345,6 @@ int hdlcd_setup_crtc(struct drm_device *drm)
 					&hdlcd_crtc_funcs, NULL);
 	if (ret) {
 		hdlcd_plane_destroy(primary);
-		devm_kfree(drm->dev, primary);
 		return ret;
 	}
 
diff --git a/drivers/gpu/drm/atmel-hlcdc/atmel_hlcdc_output.c b/drivers/gpu/drm/atmel-hlcdc/atmel_hlcdc_output.c
index 65a3bd7a0c00..423dda2785d4 100644
--- a/drivers/gpu/drm/atmel-hlcdc/atmel_hlcdc_output.c
+++ b/drivers/gpu/drm/atmel-hlcdc/atmel_hlcdc_output.c
@@ -152,8 +152,7 @@ static const struct drm_connector_funcs atmel_hlcdc_panel_connector_funcs = {
 	.atomic_destroy_state = drm_atomic_helper_connector_destroy_state,
 };
 
-static int atmel_hlcdc_attach_endpoint(struct drm_device *dev,
-				       const struct device_node *np)
+static int atmel_hlcdc_attach_endpoint(struct drm_device *dev, int endpoint)
 {
 	struct atmel_hlcdc_dc *dc = dev->dev_private;
 	struct atmel_hlcdc_rgb_output *output;
@@ -161,6 +160,11 @@ static int atmel_hlcdc_attach_endpoint(struct drm_device *dev,
 	struct drm_bridge *bridge;
 	int ret;
 
+	ret = drm_of_find_panel_or_bridge(dev->dev->of_node, 0, endpoint,
+					  &panel, &bridge);
+	if (ret)
+		return ret;
+
 	output = devm_kzalloc(dev->dev, sizeof(*output), GFP_KERNEL);
 	if (!output)
 		return -EINVAL;
@@ -177,10 +181,6 @@ static int atmel_hlcdc_attach_endpoint(struct drm_device *dev,
 
 	output->encoder.possible_crtcs = 0x1;
 
-	ret = drm_of_find_panel_or_bridge(np, 0, 0, &panel, &bridge);
-	if (ret)
-		return ret;
-
 	if (panel) {
 		output->connector.dpms = DRM_MODE_DPMS_OFF;
 		output->connector.polled = DRM_CONNECTOR_POLL_CONNECT;
@@ -220,22 +220,14 @@ err_encoder_cleanup:
 
 int atmel_hlcdc_create_outputs(struct drm_device *dev)
 {
-	struct device_node *remote;
-	int ret = -ENODEV;
-	int endpoint = 0;
-
-	while (true) {
-		/* Loop thru possible multiple connections to the output */
-		remote = of_graph_get_remote_node(dev->dev->of_node, 0,
-						  endpoint++);
-		if (!remote)
-			break;
-
-		ret = atmel_hlcdc_attach_endpoint(dev, remote);
-		of_node_put(remote);
-		if (ret)
-			return ret;
-	}
+	int endpoint, ret = 0;
+
+	for (endpoint = 0; !ret; endpoint++)
+		ret = atmel_hlcdc_attach_endpoint(dev, endpoint);
+
+	/* At least one device was successfully attached.*/
+	if (ret == -ENODEV && endpoint)
+		return 0;
 
 	return ret;
 }
diff --git a/drivers/gpu/drm/drm_atomic_helper.c b/drivers/gpu/drm/drm_atomic_helper.c
index 93b0221d5d0f..86d3093c6c9b 100644
--- a/drivers/gpu/drm/drm_atomic_helper.c
+++ b/drivers/gpu/drm/drm_atomic_helper.c
@@ -574,6 +574,8 @@ drm_atomic_helper_check_modeset(struct drm_device *dev,
 		bool has_connectors =
 			!!new_crtc_state->connector_mask;
 
+		WARN_ON(!drm_modeset_is_locked(&crtc->mutex));
+
 		if (!drm_mode_equal(&old_crtc_state->mode, &new_crtc_state->mode)) {
 			DRM_DEBUG_ATOMIC("[CRTC:%d:%s] mode changed\n",
 					 crtc->base.id, crtc->name);
@@ -617,6 +619,8 @@ drm_atomic_helper_check_modeset(struct drm_device *dev,
 	for_each_oldnew_connector_in_state(state, connector, old_connector_state, new_connector_state, i) {
 		const struct drm_connector_helper_funcs *funcs = connector->helper_private;
 
+		WARN_ON(!drm_modeset_is_locked(&dev->mode_config.connection_mutex));
+
 		/*
 		 * This only sets crtc->connectors_changed for routing changes,
 		 * drivers must set crtc->connectors_changed themselves when
@@ -720,6 +724,8 @@ drm_atomic_helper_check_planes(struct drm_device *dev,
 	for_each_oldnew_plane_in_state(state, plane, old_plane_state, new_plane_state, i) {
 		const struct drm_plane_helper_funcs *funcs;
 
+		WARN_ON(!drm_modeset_is_locked(&plane->mutex));
+
 		funcs = plane->helper_private;
 
 		drm_atomic_helper_plane_changed(state, old_plane_state, new_plane_state, plane);
@@ -2738,7 +2744,12 @@ int drm_atomic_helper_resume(struct drm_device *dev,
 
 	drm_modeset_acquire_init(&ctx, 0);
 	while (1) {
+		err = drm_modeset_lock_all_ctx(dev, &ctx);
+		if (err)
+			goto out;
+
 		err = drm_atomic_helper_commit_duplicated_state(state, &ctx);
+out:
 		if (err != -EDEADLK)
 			break;
 
diff --git a/drivers/gpu/drm/drm_plane.c b/drivers/gpu/drm/drm_plane.c
index fedd4d60d9cd..5dc8c4350602 100644
--- a/drivers/gpu/drm/drm_plane.c
+++ b/drivers/gpu/drm/drm_plane.c
@@ -948,8 +948,6 @@ retry:
 	}
 
 out:
-	if (ret && crtc->funcs->page_flip_target)
-		drm_crtc_vblank_put(crtc);
 	if (fb)
 		drm_framebuffer_put(fb);
 	if (crtc->primary->old_fb)
@@ -964,5 +962,8 @@ out:
 	drm_modeset_drop_locks(&ctx);
 	drm_modeset_acquire_fini(&ctx);
 
+	if (ret && crtc->funcs->page_flip_target)
+		drm_crtc_vblank_put(crtc);
+
 	return ret;
 }
diff --git a/drivers/gpu/drm/etnaviv/etnaviv_gem_submit.c b/drivers/gpu/drm/etnaviv/etnaviv_gem_submit.c
index a13930e1d8c9..ee7069e93eda 100644
--- a/drivers/gpu/drm/etnaviv/etnaviv_gem_submit.c
+++ b/drivers/gpu/drm/etnaviv/etnaviv_gem_submit.c
@@ -44,6 +44,7 @@ static struct etnaviv_gem_submit *submit_create(struct drm_device *dev,
 
 		/* initially, until copy_from_user() and bo lookup succeeds: */
 		submit->nr_bos = 0;
+		submit->fence = NULL;
 
 		ww_acquire_init(&submit->ticket, &reservation_ww_class);
 	}
@@ -294,7 +295,8 @@ static void submit_cleanup(struct etnaviv_gem_submit *submit)
 	}
 
 	ww_acquire_fini(&submit->ticket);
-	dma_fence_put(submit->fence);
+	if (submit->fence)
+		dma_fence_put(submit->fence);
 	kfree(submit);
 }
 
diff --git a/drivers/gpu/drm/gma500/psb_intel_lvds.c b/drivers/gpu/drm/gma500/psb_intel_lvds.c
index 0066fe7e622e..be3eefec5152 100644
--- a/drivers/gpu/drm/gma500/psb_intel_lvds.c
+++ b/drivers/gpu/drm/gma500/psb_intel_lvds.c
@@ -759,20 +759,23 @@ void psb_intel_lvds_init(struct drm_device *dev,
 		if (scan->type & DRM_MODE_TYPE_PREFERRED) {
 			mode_dev->panel_fixed_mode =
 			    drm_mode_duplicate(dev, scan);
+			DRM_DEBUG_KMS("Using mode from DDC\n");
 			goto out;	/* FIXME: check for quirks */
 		}
 	}
 
 	/* Failed to get EDID, what about VBT? do we need this? */
-	if (mode_dev->vbt_mode)
+	if (dev_priv->lfp_lvds_vbt_mode) {
 		mode_dev->panel_fixed_mode =
-		    drm_mode_duplicate(dev, mode_dev->vbt_mode);
+			drm_mode_duplicate(dev, dev_priv->lfp_lvds_vbt_mode);
 
-	if (!mode_dev->panel_fixed_mode)
-		if (dev_priv->lfp_lvds_vbt_mode)
-			mode_dev->panel_fixed_mode =
-				drm_mode_duplicate(dev,
-					dev_priv->lfp_lvds_vbt_mode);
+		if (mode_dev->panel_fixed_mode) {
+			mode_dev->panel_fixed_mode->type |=
+				DRM_MODE_TYPE_PREFERRED;
+			DRM_DEBUG_KMS("Using mode from VBT\n");
+			goto out;
+		}
+	}
 
 	/*
 	 * If we didn't get EDID, try checking if the panel is already turned
@@ -789,6 +792,7 @@ void psb_intel_lvds_init(struct drm_device *dev,
 		if (mode_dev->panel_fixed_mode) {
 			mode_dev->panel_fixed_mode->type |=
 			    DRM_MODE_TYPE_PREFERRED;
+			DRM_DEBUG_KMS("Using pre-programmed mode\n");
 			goto out;	/* FIXME: check for quirks */
 		}
 	}
diff --git a/drivers/gpu/drm/i915/Kconfig.debug b/drivers/gpu/drm/i915/Kconfig.debug
index b00edd3b8800..78c5c049a347 100644
--- a/drivers/gpu/drm/i915/Kconfig.debug
+++ b/drivers/gpu/drm/i915/Kconfig.debug
@@ -61,6 +61,18 @@ config DRM_I915_SW_FENCE_DEBUG_OBJECTS
 
           If in doubt, say "N".
 
+config DRM_I915_SW_FENCE_CHECK_DAG
+        bool "Enable additional driver debugging for detecting dependency cycles"
+        depends on DRM_I915
+        default n
+        help
+          Choose this option to turn on extra driver debugging that may affect
+          performance but will catch some internal issues.
+
+          Recommended for driver developers only.
+
+          If in doubt, say "N".
+
 config DRM_I915_SELFTEST
 	bool "Enable selftests upon driver load"
 	depends on DRM_I915
diff --git a/drivers/gpu/drm/i915/Makefile b/drivers/gpu/drm/i915/Makefile
index 2cf04504e494..16dccf550412 100644
--- a/drivers/gpu/drm/i915/Makefile
+++ b/drivers/gpu/drm/i915/Makefile
@@ -16,6 +16,7 @@ i915-y := i915_drv.o \
 	  i915_params.o \
 	  i915_pci.o \
           i915_suspend.o \
+	  i915_syncmap.o \
 	  i915_sw_fence.o \
 	  i915_sysfs.o \
 	  intel_csr.o \
@@ -57,6 +58,7 @@ i915-y += i915_cmd_parser.o \
 
 # general-purpose microcontroller (GuC) support
 i915-y += intel_uc.o \
+	  intel_guc_ct.o \
 	  intel_guc_log.o \
 	  intel_guc_loader.o \
 	  intel_huc.o \
diff --git a/drivers/gpu/drm/i915/dvo_ch7017.c b/drivers/gpu/drm/i915/dvo_ch7017.c
index b3c7c199200c..80b3e16cf48c 100644
--- a/drivers/gpu/drm/i915/dvo_ch7017.c
+++ b/drivers/gpu/drm/i915/dvo_ch7017.c
@@ -280,10 +280,10 @@ static void ch7017_mode_set(struct intel_dvo_device *dvo,
 			(0 << CH7017_PHASE_DETECTOR_SHIFT);
 	} else {
 		outputs_enable = CH7017_LVDS_CHANNEL_A | CH7017_CHARGE_PUMP_HIGH;
-		lvds_pll_feedback_div = CH7017_LVDS_PLL_FEEDBACK_DEFAULT_RESERVED |
+		lvds_pll_feedback_div =
+			CH7017_LVDS_PLL_FEEDBACK_DEFAULT_RESERVED |
 			(2 << CH7017_LVDS_PLL_FEED_BACK_DIVIDER_SHIFT) |
 			(3 << CH7017_LVDS_PLL_FEED_FORWARD_DIVIDER_SHIFT);
-		lvds_pll_feedback_div = 35;
 		lvds_control_2 = (3 << CH7017_LOOP_FILTER_SHIFT) |
 			(0 << CH7017_PHASE_DETECTOR_SHIFT);
 		if (1) { /* XXX: dual channel panel detection.  Assume yes for now. */
diff --git a/drivers/gpu/drm/i915/gvt/execlist.c b/drivers/gpu/drm/i915/gvt/execlist.c
index dca989eb2d42..24fe04d6307b 100644
--- a/drivers/gpu/drm/i915/gvt/execlist.c
+++ b/drivers/gpu/drm/i915/gvt/execlist.c
@@ -779,8 +779,26 @@ static void init_vgpu_execlist(struct intel_vgpu *vgpu, int ring_id)
 	vgpu_vreg(vgpu, ctx_status_ptr_reg) = ctx_status_ptr.dw;
 }
 
+static void clean_workloads(struct intel_vgpu *vgpu, unsigned long engine_mask)
+{
+	struct drm_i915_private *dev_priv = vgpu->gvt->dev_priv;
+	struct intel_engine_cs *engine;
+	struct intel_vgpu_workload *pos, *n;
+	unsigned int tmp;
+
+	/* free the unsubmited workloads in the queues. */
+	for_each_engine_masked(engine, dev_priv, engine_mask, tmp) {
+		list_for_each_entry_safe(pos, n,
+			&vgpu->workload_q_head[engine->id], list) {
+			list_del_init(&pos->list);
+			free_workload(pos);
+		}
+	}
+}
+
 void intel_vgpu_clean_execlist(struct intel_vgpu *vgpu)
 {
+	clean_workloads(vgpu, ALL_ENGINES);
 	kmem_cache_destroy(vgpu->workloads);
 }
 
@@ -811,17 +829,9 @@ void intel_vgpu_reset_execlist(struct intel_vgpu *vgpu,
 {
 	struct drm_i915_private *dev_priv = vgpu->gvt->dev_priv;
 	struct intel_engine_cs *engine;
-	struct intel_vgpu_workload *pos, *n;
 	unsigned int tmp;
 
-	for_each_engine_masked(engine, dev_priv, engine_mask, tmp) {
-		/* free the unsubmited workload in the queue */
-		list_for_each_entry_safe(pos, n,
-			&vgpu->workload_q_head[engine->id], list) {
-			list_del_init(&pos->list);
-			free_workload(pos);
-		}
-
+	clean_workloads(vgpu, engine_mask);
+	for_each_engine_masked(engine, dev_priv, engine_mask, tmp)
 		init_vgpu_execlist(vgpu, engine->id);
-	}
 }
diff --git a/drivers/gpu/drm/i915/gvt/handlers.c b/drivers/gpu/drm/i915/gvt/handlers.c
index 0ad1a508e2af..0ffd69654592 100644
--- a/drivers/gpu/drm/i915/gvt/handlers.c
+++ b/drivers/gpu/drm/i915/gvt/handlers.c
@@ -1244,7 +1244,7 @@ static int dma_ctrl_write(struct intel_vgpu *vgpu, unsigned int offset,
 	mode = vgpu_vreg(vgpu, offset);
 
 	if (GFX_MODE_BIT_SET_IN_MASK(mode, START_DMA)) {
-		WARN_ONCE(1, "VM(%d): iGVT-g doesn't supporte GuC\n",
+		WARN_ONCE(1, "VM(%d): iGVT-g doesn't support GuC\n",
 				vgpu->id);
 		return 0;
 	}
@@ -1366,18 +1366,28 @@ static int skl_misc_ctl_write(struct intel_vgpu *vgpu, unsigned int offset,
 		void *p_data, unsigned int bytes)
 {
 	struct drm_i915_private *dev_priv = vgpu->gvt->dev_priv;
-	i915_reg_t reg = {.reg = offset};
+	u32 v = *(u32 *)p_data;
+
+	if (!IS_SKYLAKE(dev_priv) && !IS_KABYLAKE(dev_priv))
+		return intel_vgpu_default_mmio_write(vgpu,
+				offset, p_data, bytes);
 
 	switch (offset) {
 	case 0x4ddc:
-		vgpu_vreg(vgpu, offset) = 0x8000003c;
-		/* WaCompressedResourceSamplerPbeMediaNewHashMode:skl */
-		I915_WRITE(reg, vgpu_vreg(vgpu, offset));
+		/* bypass WaCompressedResourceSamplerPbeMediaNewHashMode */
+		vgpu_vreg(vgpu, offset) = v & ~(1 << 31);
 		break;
 	case 0x42080:
-		vgpu_vreg(vgpu, offset) = 0x8000;
-		/* WaCompressedResourceDisplayNewHashMode:skl */
-		I915_WRITE(reg, vgpu_vreg(vgpu, offset));
+		/* bypass WaCompressedResourceDisplayNewHashMode */
+		vgpu_vreg(vgpu, offset) = v & ~(1 << 15);
+		break;
+	case 0xe194:
+		/* bypass WaCompressedResourceSamplerPbeMediaNewHashMode */
+		vgpu_vreg(vgpu, offset) = v & ~(1 << 8);
+		break;
+	case 0x7014:
+		/* bypass WaCompressedResourceSamplerPbeMediaNewHashMode */
+		vgpu_vreg(vgpu, offset) = v & ~(1 << 13);
 		break;
 	default:
 		return -EINVAL;
@@ -1634,7 +1644,8 @@ static int init_generic_mmio_info(struct intel_gvt *gvt)
 	MMIO_DFH(GAM_ECOCHK, D_ALL, F_CMD_ACCESS, NULL, NULL);
 	MMIO_DFH(GEN7_COMMON_SLICE_CHICKEN1, D_ALL, F_MODE_MASK | F_CMD_ACCESS,
 		NULL, NULL);
-	MMIO_DFH(COMMON_SLICE_CHICKEN2, D_ALL, F_MODE_MASK | F_CMD_ACCESS, NULL, NULL);
+	MMIO_DFH(COMMON_SLICE_CHICKEN2, D_ALL, F_MODE_MASK | F_CMD_ACCESS, NULL,
+		 skl_misc_ctl_write);
 	MMIO_DFH(0x9030, D_ALL, F_CMD_ACCESS, NULL, NULL);
 	MMIO_DFH(0x20a0, D_ALL, F_CMD_ACCESS, NULL, NULL);
 	MMIO_DFH(0x2420, D_ALL, F_CMD_ACCESS, NULL, NULL);
@@ -2568,7 +2579,8 @@ static int init_broadwell_mmio_info(struct intel_gvt *gvt)
 	MMIO_D(0x6e570, D_BDW_PLUS);
 	MMIO_D(0x65f10, D_BDW_PLUS);
 
-	MMIO_DFH(0xe194, D_BDW_PLUS, F_MODE_MASK | F_CMD_ACCESS, NULL, NULL);
+	MMIO_DFH(0xe194, D_BDW_PLUS, F_MODE_MASK | F_CMD_ACCESS, NULL,
+		 skl_misc_ctl_write);
 	MMIO_DFH(0xe188, D_BDW_PLUS, F_MODE_MASK | F_CMD_ACCESS, NULL, NULL);
 	MMIO_DFH(HALF_SLICE_CHICKEN2, D_BDW_PLUS, F_MODE_MASK | F_CMD_ACCESS, NULL, NULL);
 	MMIO_DFH(0x2580, D_BDW_PLUS, F_MODE_MASK | F_CMD_ACCESS, NULL, NULL);
diff --git a/drivers/gpu/drm/i915/gvt/render.c b/drivers/gpu/drm/i915/gvt/render.c
index c6e7972ac21d..a5e11d89df2f 100644
--- a/drivers/gpu/drm/i915/gvt/render.c
+++ b/drivers/gpu/drm/i915/gvt/render.c
@@ -340,6 +340,9 @@ void intel_gvt_restore_render_mmio(struct intel_vgpu *vgpu, int ring_id)
 		} else
 			v = mmio->value;
 
+		if (mmio->in_context)
+			continue;
+
 		I915_WRITE(mmio->reg, v);
 		POSTING_READ(mmio->reg);
 
diff --git a/drivers/gpu/drm/i915/gvt/sched_policy.c b/drivers/gpu/drm/i915/gvt/sched_policy.c
index 79ba4b3440aa..f25ff133865f 100644
--- a/drivers/gpu/drm/i915/gvt/sched_policy.c
+++ b/drivers/gpu/drm/i915/gvt/sched_policy.c
@@ -129,9 +129,13 @@ static void try_to_schedule_next_vgpu(struct intel_gvt *gvt)
 	struct vgpu_sched_data *vgpu_data;
 	ktime_t cur_time;
 
-	/* no target to schedule */
-	if (!scheduler->next_vgpu)
+	/* no need to schedule if next_vgpu is the same with current_vgpu,
+	 * let scheduler chose next_vgpu again by setting it to NULL.
+	 */
+	if (scheduler->next_vgpu == scheduler->current_vgpu) {
+		scheduler->next_vgpu = NULL;
 		return;
+	}
 
 	/*
 	 * after the flag is set, workload dispatch thread will
diff --git a/drivers/gpu/drm/i915/gvt/scheduler.c b/drivers/gpu/drm/i915/gvt/scheduler.c
index bada32b33237..6ae286cb5804 100644
--- a/drivers/gpu/drm/i915/gvt/scheduler.c
+++ b/drivers/gpu/drm/i915/gvt/scheduler.c
@@ -69,8 +69,7 @@ static int populate_shadow_context(struct intel_vgpu_workload *workload)
 	gvt_dbg_sched("ring id %d workload lrca %x", ring_id,
 			workload->ctx_desc.lrca);
 
-	context_page_num = intel_lr_context_size(
-			gvt->dev_priv->engine[ring_id]);
+	context_page_num = gvt->dev_priv->engine[ring_id]->context_size;
 
 	context_page_num = context_page_num >> PAGE_SHIFT;
 
@@ -181,6 +180,7 @@ static int dispatch_workload(struct intel_vgpu_workload *workload)
 	struct intel_engine_cs *engine = dev_priv->engine[ring_id];
 	struct drm_i915_gem_request *rq;
 	struct intel_vgpu *vgpu = workload->vgpu;
+	struct intel_ring *ring;
 	int ret;
 
 	gvt_dbg_sched("ring id %d prepare to dispatch workload %p\n",
@@ -199,8 +199,9 @@ static int dispatch_workload(struct intel_vgpu_workload *workload)
 	 * shadow_ctx pages invalid. So gvt need to pin itself. After update
 	 * the guest context, gvt can unpin the shadow_ctx safely.
 	 */
-	ret = engine->context_pin(engine, shadow_ctx);
-	if (ret) {
+	ring = engine->context_pin(engine, shadow_ctx);
+	if (IS_ERR(ring)) {
+		ret = PTR_ERR(ring);
 		gvt_vgpu_err("fail to pin shadow context\n");
 		workload->status = ret;
 		mutex_unlock(&dev_priv->drm.struct_mutex);
@@ -330,8 +331,7 @@ static void update_guest_context(struct intel_vgpu_workload *workload)
 	gvt_dbg_sched("ring id %d workload lrca %x\n", ring_id,
 			workload->ctx_desc.lrca);
 
-	context_page_num = intel_lr_context_size(
-			gvt->dev_priv->engine[ring_id]);
+	context_page_num = gvt->dev_priv->engine[ring_id]->context_size;
 
 	context_page_num = context_page_num >> PAGE_SHIFT;
 
diff --git a/drivers/gpu/drm/i915/i915_cmd_parser.c b/drivers/gpu/drm/i915/i915_cmd_parser.c
index 7af100f84410..f0cb22cc0dd6 100644
--- a/drivers/gpu/drm/i915/i915_cmd_parser.c
+++ b/drivers/gpu/drm/i915/i915_cmd_parser.c
@@ -1166,8 +1166,8 @@ static bool check_cmd(const struct intel_engine_cs *engine,
 				find_reg(engine, is_master, reg_addr);
 
 			if (!reg) {
-				DRM_DEBUG_DRIVER("CMD: Rejected register 0x%08X in command: 0x%08X (exec_id=%d)\n",
-						 reg_addr, *cmd, engine->exec_id);
+				DRM_DEBUG_DRIVER("CMD: Rejected register 0x%08X in command: 0x%08X (%s)\n",
+						 reg_addr, *cmd, engine->name);
 				return false;
 			}
 
@@ -1222,11 +1222,11 @@ static bool check_cmd(const struct intel_engine_cs *engine,
 				desc->bits[i].mask;
 
 			if (dword != desc->bits[i].expected) {
-				DRM_DEBUG_DRIVER("CMD: Rejected command 0x%08X for bitmask 0x%08X (exp=0x%08X act=0x%08X) (exec_id=%d)\n",
+				DRM_DEBUG_DRIVER("CMD: Rejected command 0x%08X for bitmask 0x%08X (exp=0x%08X act=0x%08X) (%s)\n",
 						 *cmd,
 						 desc->bits[i].mask,
 						 desc->bits[i].expected,
-						 dword, engine->exec_id);
+						 dword, engine->name);
 				return false;
 			}
 		}
@@ -1284,7 +1284,7 @@ int intel_engine_cmd_parser(struct intel_engine_cs *engine,
 
 		if (*cmd == MI_BATCH_BUFFER_END) {
 			if (needs_clflush_after) {
-				void *ptr = ptr_mask_bits(shadow_batch_obj->mm.mapping);
+				void *ptr = page_mask_bits(shadow_batch_obj->mm.mapping);
 				drm_clflush_virt_range(ptr,
 						       (void *)(cmd + 1) - ptr);
 			}
diff --git a/drivers/gpu/drm/i915/i915_debugfs.c b/drivers/gpu/drm/i915/i915_debugfs.c
index 1c66108f4333..3b088685a553 100644
--- a/drivers/gpu/drm/i915/i915_debugfs.c
+++ b/drivers/gpu/drm/i915/i915_debugfs.c
@@ -2482,8 +2482,6 @@ static void i915_guc_client_info(struct seq_file *m,
 		client->wq_size, client->wq_offset, client->wq_tail);
 
 	seq_printf(m, "\tWork queue full: %u\n", client->no_wq_space);
-	seq_printf(m, "\tFailed doorbell: %u\n", client->b_fail);
-	seq_printf(m, "\tLast submission result: %d\n", client->retcode);
 
 	for_each_engine(engine, dev_priv, id) {
 		u64 submissions = client->submissions[id];
@@ -2494,42 +2492,34 @@ static void i915_guc_client_info(struct seq_file *m,
 	seq_printf(m, "\tTotal: %llu\n", tot);
 }
 
-static int i915_guc_info(struct seq_file *m, void *data)
+static bool check_guc_submission(struct seq_file *m)
 {
 	struct drm_i915_private *dev_priv = node_to_i915(m->private);
 	const struct intel_guc *guc = &dev_priv->guc;
-	struct intel_engine_cs *engine;
-	enum intel_engine_id id;
-	u64 total;
 
 	if (!guc->execbuf_client) {
 		seq_printf(m, "GuC submission %s\n",
 			   HAS_GUC_SCHED(dev_priv) ?
 			   "disabled" :
 			   "not supported");
-		return 0;
+		return false;
 	}
 
+	return true;
+}
+
+static int i915_guc_info(struct seq_file *m, void *data)
+{
+	struct drm_i915_private *dev_priv = node_to_i915(m->private);
+	const struct intel_guc *guc = &dev_priv->guc;
+
+	if (!check_guc_submission(m))
+		return 0;
+
 	seq_printf(m, "Doorbell map:\n");
 	seq_printf(m, "\t%*pb\n", GUC_NUM_DOORBELLS, guc->doorbell_bitmap);
 	seq_printf(m, "Doorbell next cacheline: 0x%x\n\n", guc->db_cacheline);
 
-	seq_printf(m, "GuC total action count: %llu\n", guc->action_count);
-	seq_printf(m, "GuC action failure count: %u\n", guc->action_fail);
-	seq_printf(m, "GuC last action command: 0x%x\n", guc->action_cmd);
-	seq_printf(m, "GuC last action status: 0x%x\n", guc->action_status);
-	seq_printf(m, "GuC last action error code: %d\n", guc->action_err);
-
-	total = 0;
-	seq_printf(m, "\nGuC submissions:\n");
-	for_each_engine(engine, dev_priv, id) {
-		u64 submissions = guc->submissions[id];
-		total += submissions;
-		seq_printf(m, "\t%-24s: %10llu, last seqno 0x%08x\n",
-			engine->name, submissions, guc->last_seqno[id]);
-	}
-	seq_printf(m, "\t%s: %llu\n", "Total", total);
-
 	seq_printf(m, "\nGuC execbuf client @ %p:\n", guc->execbuf_client);
 	i915_guc_client_info(m, dev_priv, guc->execbuf_client);
 
@@ -2540,36 +2530,99 @@ static int i915_guc_info(struct seq_file *m, void *data)
 	return 0;
 }
 
-static int i915_guc_log_dump(struct seq_file *m, void *data)
+static int i915_guc_stage_pool(struct seq_file *m, void *data)
 {
 	struct drm_i915_private *dev_priv = node_to_i915(m->private);
-	struct drm_i915_gem_object *obj;
-	int i = 0, pg;
+	const struct intel_guc *guc = &dev_priv->guc;
+	struct guc_stage_desc *desc = guc->stage_desc_pool_vaddr;
+	struct i915_guc_client *client = guc->execbuf_client;
+	unsigned int tmp;
+	int index;
 
-	if (!dev_priv->guc.log.vma)
+	if (!check_guc_submission(m))
 		return 0;
 
-	obj = dev_priv->guc.log.vma->obj;
-	for (pg = 0; pg < obj->base.size / PAGE_SIZE; pg++) {
-		u32 *log = kmap_atomic(i915_gem_object_get_page(obj, pg));
+	for (index = 0; index < GUC_MAX_STAGE_DESCRIPTORS; index++, desc++) {
+		struct intel_engine_cs *engine;
 
-		for (i = 0; i < PAGE_SIZE / sizeof(u32); i += 4)
-			seq_printf(m, "0x%08x 0x%08x 0x%08x 0x%08x\n",
-				   *(log + i), *(log + i + 1),
-				   *(log + i + 2), *(log + i + 3));
+		if (!(desc->attribute & GUC_STAGE_DESC_ATTR_ACTIVE))
+			continue;
+
+		seq_printf(m, "GuC stage descriptor %u:\n", index);
+		seq_printf(m, "\tIndex: %u\n", desc->stage_id);
+		seq_printf(m, "\tAttribute: 0x%x\n", desc->attribute);
+		seq_printf(m, "\tPriority: %d\n", desc->priority);
+		seq_printf(m, "\tDoorbell id: %d\n", desc->db_id);
+		seq_printf(m, "\tEngines used: 0x%x\n",
+			   desc->engines_used);
+		seq_printf(m, "\tDoorbell trigger phy: 0x%llx, cpu: 0x%llx, uK: 0x%x\n",
+			   desc->db_trigger_phy,
+			   desc->db_trigger_cpu,
+			   desc->db_trigger_uk);
+		seq_printf(m, "\tProcess descriptor: 0x%x\n",
+			   desc->process_desc);
+		seq_printf(m, "\tWorkqueue address: 0x%x, size: 0x%x\n",
+			   desc->wq_addr, desc->wq_size);
+		seq_putc(m, '\n');
+
+		for_each_engine_masked(engine, dev_priv, client->engines, tmp) {
+			u32 guc_engine_id = engine->guc_id;
+			struct guc_execlist_context *lrc =
+						&desc->lrc[guc_engine_id];
+
+			seq_printf(m, "\t%s LRC:\n", engine->name);
+			seq_printf(m, "\t\tContext desc: 0x%x\n",
+				   lrc->context_desc);
+			seq_printf(m, "\t\tContext id: 0x%x\n", lrc->context_id);
+			seq_printf(m, "\t\tLRCA: 0x%x\n", lrc->ring_lrca);
+			seq_printf(m, "\t\tRing begin: 0x%x\n", lrc->ring_begin);
+			seq_printf(m, "\t\tRing end: 0x%x\n", lrc->ring_end);
+			seq_putc(m, '\n');
+		}
+	}
+
+	return 0;
+}
+
+static int i915_guc_log_dump(struct seq_file *m, void *data)
+{
+	struct drm_info_node *node = m->private;
+	struct drm_i915_private *dev_priv = node_to_i915(node);
+	bool dump_load_err = !!node->info_ent->data;
+	struct drm_i915_gem_object *obj = NULL;
+	u32 *log;
+	int i = 0;
+
+	if (dump_load_err)
+		obj = dev_priv->guc.load_err_log;
+	else if (dev_priv->guc.log.vma)
+		obj = dev_priv->guc.log.vma->obj;
 
-		kunmap_atomic(log);
+	if (!obj)
+		return 0;
+
+	log = i915_gem_object_pin_map(obj, I915_MAP_WC);
+	if (IS_ERR(log)) {
+		DRM_DEBUG("Failed to pin object\n");
+		seq_puts(m, "(log data unaccessible)\n");
+		return PTR_ERR(log);
 	}
 
+	for (i = 0; i < obj->base.size / sizeof(u32); i += 4)
+		seq_printf(m, "0x%08x 0x%08x 0x%08x 0x%08x\n",
+			   *(log + i), *(log + i + 1),
+			   *(log + i + 2), *(log + i + 3));
+
 	seq_putc(m, '\n');
 
+	i915_gem_object_unpin_map(obj);
+
 	return 0;
 }
 
 static int i915_guc_log_control_get(void *data, u64 *val)
 {
-	struct drm_device *dev = data;
-	struct drm_i915_private *dev_priv = to_i915(dev);
+	struct drm_i915_private *dev_priv = data;
 
 	if (!dev_priv->guc.log.vma)
 		return -EINVAL;
@@ -2581,14 +2634,13 @@ static int i915_guc_log_control_get(void *data, u64 *val)
 
 static int i915_guc_log_control_set(void *data, u64 val)
 {
-	struct drm_device *dev = data;
-	struct drm_i915_private *dev_priv = to_i915(dev);
+	struct drm_i915_private *dev_priv = data;
 	int ret;
 
 	if (!dev_priv->guc.log.vma)
 		return -EINVAL;
 
-	ret = mutex_lock_interruptible(&dev->struct_mutex);
+	ret = mutex_lock_interruptible(&dev_priv->drm.struct_mutex);
 	if (ret)
 		return ret;
 
@@ -2596,7 +2648,7 @@ static int i915_guc_log_control_set(void *data, u64 val)
 	ret = i915_guc_log_control(dev_priv, val);
 	intel_runtime_pm_put(dev_priv);
 
-	mutex_unlock(&dev->struct_mutex);
+	mutex_unlock(&dev_priv->drm.struct_mutex);
 	return ret;
 }
 
@@ -2855,7 +2907,8 @@ static int i915_dmc_info(struct seq_file *m, void *unused)
 	seq_printf(m, "version: %d.%d\n", CSR_VERSION_MAJOR(csr->version),
 		   CSR_VERSION_MINOR(csr->version));
 
-	if (IS_SKYLAKE(dev_priv) && csr->version >= CSR_VERSION(1, 6)) {
+	if (IS_KABYLAKE(dev_priv) ||
+	    (IS_SKYLAKE(dev_priv) && csr->version >= CSR_VERSION(1, 6))) {
 		seq_printf(m, "DC3 -> DC5 count: %d\n",
 			   I915_READ(SKL_CSR_DC3_DC5_COUNT));
 		seq_printf(m, "DC5 -> DC6 count: %d\n",
@@ -3043,36 +3096,6 @@ static void intel_connector_info(struct seq_file *m,
 		intel_seq_print_mode(m, 2, mode);
 }
 
-static bool cursor_active(struct drm_i915_private *dev_priv, int pipe)
-{
-	u32 state;
-
-	if (IS_I845G(dev_priv) || IS_I865G(dev_priv))
-		state = I915_READ(CURCNTR(PIPE_A)) & CURSOR_ENABLE;
-	else
-		state = I915_READ(CURCNTR(pipe)) & CURSOR_MODE;
-
-	return state;
-}
-
-static bool cursor_position(struct drm_i915_private *dev_priv,
-			    int pipe, int *x, int *y)
-{
-	u32 pos;
-
-	pos = I915_READ(CURPOS(pipe));
-
-	*x = (pos >> CURSOR_X_SHIFT) & CURSOR_POS_MASK;
-	if (pos & (CURSOR_POS_SIGN << CURSOR_X_SHIFT))
-		*x = -*x;
-
-	*y = (pos >> CURSOR_Y_SHIFT) & CURSOR_POS_MASK;
-	if (pos & (CURSOR_POS_SIGN << CURSOR_Y_SHIFT))
-		*y = -*y;
-
-	return cursor_active(dev_priv, pipe);
-}
-
 static const char *plane_type(enum drm_plane_type type)
 {
 	switch (type) {
@@ -3194,9 +3217,7 @@ static int i915_display_info(struct seq_file *m, void *unused)
 	seq_printf(m, "CRTC info\n");
 	seq_printf(m, "---------\n");
 	for_each_intel_crtc(dev, crtc) {
-		bool active;
 		struct intel_crtc_state *pipe_config;
-		int x, y;
 
 		drm_modeset_lock(&crtc->base.mutex, NULL);
 		pipe_config = to_intel_crtc_state(crtc->base.state);
@@ -3208,14 +3229,18 @@ static int i915_display_info(struct seq_file *m, void *unused)
 			   yesno(pipe_config->dither), pipe_config->pipe_bpp);
 
 		if (pipe_config->base.active) {
+			struct intel_plane *cursor =
+				to_intel_plane(crtc->base.cursor);
+
 			intel_crtc_info(m, crtc);
 
-			active = cursor_position(dev_priv, crtc->pipe, &x, &y);
-			seq_printf(m, "\tcursor visible? %s, position (%d, %d), size %dx%d, addr 0x%08x, active? %s\n",
-				   yesno(crtc->cursor_base),
-				   x, y, crtc->base.cursor->state->crtc_w,
-				   crtc->base.cursor->state->crtc_h,
-				   crtc->cursor_addr, yesno(active));
+			seq_printf(m, "\tcursor visible? %s, position (%d, %d), size %dx%d, addr 0x%08x\n",
+				   yesno(cursor->base.state->visible),
+				   cursor->base.state->crtc_x,
+				   cursor->base.state->crtc_y,
+				   cursor->base.state->crtc_w,
+				   cursor->base.state->crtc_h,
+				   cursor->cursor.base);
 			intel_scaler_info(m, crtc);
 			intel_plane_info(m, crtc);
 		}
@@ -3316,7 +3341,7 @@ static int i915_engine_info(struct seq_file *m, void *unused)
 
 		if (i915.enable_execlists) {
 			u32 ptr, read, write;
-			struct rb_node *rb;
+			unsigned int idx;
 
 			seq_printf(m, "\tExeclist status: 0x%08x %08x\n",
 				   I915_READ(RING_EXECLIST_STATUS_LO(engine)),
@@ -3334,8 +3359,7 @@ static int i915_engine_info(struct seq_file *m, void *unused)
 			if (read > write)
 				write += GEN8_CSB_ENTRIES;
 			while (read < write) {
-				unsigned int idx = ++read % GEN8_CSB_ENTRIES;
-
+				idx = ++read % GEN8_CSB_ENTRIES;
 				seq_printf(m, "\tExeclist CSB[%d]: 0x%08x, context: %d\n",
 					   idx,
 					   I915_READ(RING_CONTEXT_STATUS_BUF_LO(engine, idx)),
@@ -3343,28 +3367,30 @@ static int i915_engine_info(struct seq_file *m, void *unused)
 			}
 
 			rcu_read_lock();
-			rq = READ_ONCE(engine->execlist_port[0].request);
-			if (rq) {
-				seq_printf(m, "\t\tELSP[0] count=%d, ",
-					   engine->execlist_port[0].count);
-				print_request(m, rq, "rq: ");
-			} else {
-				seq_printf(m, "\t\tELSP[0] idle\n");
-			}
-			rq = READ_ONCE(engine->execlist_port[1].request);
-			if (rq) {
-				seq_printf(m, "\t\tELSP[1] count=%d, ",
-					   engine->execlist_port[1].count);
-				print_request(m, rq, "rq: ");
-			} else {
-				seq_printf(m, "\t\tELSP[1] idle\n");
+			for (idx = 0; idx < ARRAY_SIZE(engine->execlist_port); idx++) {
+				unsigned int count;
+
+				rq = port_unpack(&engine->execlist_port[idx],
+						 &count);
+				if (rq) {
+					seq_printf(m, "\t\tELSP[%d] count=%d, ",
+						   idx, count);
+					print_request(m, rq, "rq: ");
+				} else {
+					seq_printf(m, "\t\tELSP[%d] idle\n",
+						   idx);
+				}
 			}
 			rcu_read_unlock();
 
 			spin_lock_irq(&engine->timeline->lock);
-			for (rb = engine->execlist_first; rb; rb = rb_next(rb)) {
-				rq = rb_entry(rb, typeof(*rq), priotree.node);
-				print_request(m, rq, "\t\tQ ");
+			for (rb = engine->execlist_first; rb; rb = rb_next(rb)){
+				struct i915_priolist *p =
+					rb_entry(rb, typeof(*p), node);
+
+				list_for_each_entry(rq, &p->requests,
+						    priotree.link)
+					print_request(m, rq, "\t\tQ ");
 			}
 			spin_unlock_irq(&engine->timeline->lock);
 		} else if (INTEL_GEN(dev_priv) > 6) {
@@ -3704,16 +3730,10 @@ static ssize_t i915_displayport_test_active_write(struct file *file,
 	if (len == 0)
 		return 0;
 
-	input_buffer = kmalloc(len + 1, GFP_KERNEL);
-	if (!input_buffer)
-		return -ENOMEM;
-
-	if (copy_from_user(input_buffer, ubuf, len)) {
-		status = -EFAULT;
-		goto out;
-	}
+	input_buffer = memdup_user_nul(ubuf, len);
+	if (IS_ERR(input_buffer))
+		return PTR_ERR(input_buffer);
 
-	input_buffer[len] = '\0';
 	DRM_DEBUG_DRIVER("Copied %d bytes from user\n", (unsigned int)len);
 
 	drm_connector_list_iter_begin(dev, &conn_iter);
@@ -3739,7 +3759,6 @@ static ssize_t i915_displayport_test_active_write(struct file *file,
 		}
 	}
 	drm_connector_list_iter_end(&conn_iter);
-out:
 	kfree(input_buffer);
 	if (status < 0)
 		return status;
@@ -3900,6 +3919,8 @@ static void wm_latency_show(struct seq_file *m, const uint16_t wm[8])
 		num_levels = 3;
 	else if (IS_VALLEYVIEW(dev_priv))
 		num_levels = 1;
+	else if (IS_G4X(dev_priv))
+		num_levels = 3;
 	else
 		num_levels = ilk_wm_max_level(dev_priv) + 1;
 
@@ -3912,8 +3933,10 @@ static void wm_latency_show(struct seq_file *m, const uint16_t wm[8])
 		 * - WM1+ latency values in 0.5us units
 		 * - latencies are in us on gen9/vlv/chv
 		 */
-		if (INTEL_GEN(dev_priv) >= 9 || IS_VALLEYVIEW(dev_priv) ||
-		    IS_CHERRYVIEW(dev_priv))
+		if (INTEL_GEN(dev_priv) >= 9 ||
+		    IS_VALLEYVIEW(dev_priv) ||
+		    IS_CHERRYVIEW(dev_priv) ||
+		    IS_G4X(dev_priv))
 			latency *= 10;
 		else if (level > 0)
 			latency *= 5;
@@ -3974,7 +3997,7 @@ static int pri_wm_latency_open(struct inode *inode, struct file *file)
 {
 	struct drm_i915_private *dev_priv = inode->i_private;
 
-	if (INTEL_GEN(dev_priv) < 5)
+	if (INTEL_GEN(dev_priv) < 5 && !IS_G4X(dev_priv))
 		return -ENODEV;
 
 	return single_open(file, pri_wm_latency_show, dev_priv);
@@ -4016,6 +4039,8 @@ static ssize_t wm_latency_write(struct file *file, const char __user *ubuf,
 		num_levels = 3;
 	else if (IS_VALLEYVIEW(dev_priv))
 		num_levels = 1;
+	else if (IS_G4X(dev_priv))
+		num_levels = 3;
 	else
 		num_levels = ilk_wm_max_level(dev_priv) + 1;
 
@@ -4264,26 +4289,27 @@ i915_drop_caches_set(void *data, u64 val)
 {
 	struct drm_i915_private *dev_priv = data;
 	struct drm_device *dev = &dev_priv->drm;
-	int ret;
+	int ret = 0;
 
 	DRM_DEBUG("Dropping caches: 0x%08llx\n", val);
 
 	/* No need to check and wait for gpu resets, only libdrm auto-restarts
 	 * on ioctls on -EAGAIN. */
-	ret = mutex_lock_interruptible(&dev->struct_mutex);
-	if (ret)
-		return ret;
-
-	if (val & DROP_ACTIVE) {
-		ret = i915_gem_wait_for_idle(dev_priv,
-					     I915_WAIT_INTERRUPTIBLE |
-					     I915_WAIT_LOCKED);
+	if (val & (DROP_ACTIVE | DROP_RETIRE)) {
+		ret = mutex_lock_interruptible(&dev->struct_mutex);
 		if (ret)
-			goto unlock;
-	}
+			return ret;
+
+		if (val & DROP_ACTIVE)
+			ret = i915_gem_wait_for_idle(dev_priv,
+						     I915_WAIT_INTERRUPTIBLE |
+						     I915_WAIT_LOCKED);
 
-	if (val & DROP_RETIRE)
-		i915_gem_retire_requests(dev_priv);
+		if (val & DROP_RETIRE)
+			i915_gem_retire_requests(dev_priv);
+
+		mutex_unlock(&dev->struct_mutex);
+	}
 
 	lockdep_set_current_reclaim_state(GFP_KERNEL);
 	if (val & DROP_BOUND)
@@ -4296,9 +4322,6 @@ i915_drop_caches_set(void *data, u64 val)
 		i915_gem_shrink_all(dev_priv);
 	lockdep_clear_current_reclaim_state();
 
-unlock:
-	mutex_unlock(&dev->struct_mutex);
-
 	if (val & DROP_FREED) {
 		synchronize_rcu();
 		i915_gem_drain_freed_objects(dev_priv);
@@ -4776,6 +4799,8 @@ static const struct drm_info_list i915_debugfs_list[] = {
 	{"i915_guc_info", i915_guc_info, 0},
 	{"i915_guc_load_status", i915_guc_load_status_info, 0},
 	{"i915_guc_log_dump", i915_guc_log_dump, 0},
+	{"i915_guc_load_err_log_dump", i915_guc_log_dump, 0, (void *)1},
+	{"i915_guc_stage_pool", i915_guc_stage_pool, 0},
 	{"i915_huc_load_status", i915_huc_load_status_info, 0},
 	{"i915_frequency_info", i915_frequency_info, 0},
 	{"i915_hangcheck_info", i915_hangcheck_info, 0},
diff --git a/drivers/gpu/drm/i915/i915_drv.c b/drivers/gpu/drm/i915/i915_drv.c
index 3036d4835b0f..2fdfaf135ea9 100644
--- a/drivers/gpu/drm/i915/i915_drv.c
+++ b/drivers/gpu/drm/i915/i915_drv.c
@@ -350,6 +350,7 @@ static int i915_getparam(struct drm_device *dev, void *data,
 	case I915_PARAM_HAS_EXEC_SOFTPIN:
 	case I915_PARAM_HAS_EXEC_ASYNC:
 	case I915_PARAM_HAS_EXEC_FENCE:
+	case I915_PARAM_HAS_EXEC_CAPTURE:
 		/* For the time being all of these are always true;
 		 * if some supported hardware does not have one of these
 		 * features this value needs to be provided from
@@ -834,10 +835,6 @@ static int i915_driver_init_early(struct drm_i915_private *dev_priv,
 	intel_uc_init_early(dev_priv);
 	i915_memcpy_init_early(dev_priv);
 
-	ret = intel_engines_init_early(dev_priv);
-	if (ret)
-		return ret;
-
 	ret = i915_workqueues_init(dev_priv);
 	if (ret < 0)
 		goto err_engines;
@@ -855,7 +852,7 @@ static int i915_driver_init_early(struct drm_i915_private *dev_priv,
 	intel_init_audio_hooks(dev_priv);
 	ret = i915_gem_load_init(dev_priv);
 	if (ret < 0)
-		goto err_workqueues;
+		goto err_irq;
 
 	intel_display_crc_init(dev_priv);
 
@@ -867,7 +864,8 @@ static int i915_driver_init_early(struct drm_i915_private *dev_priv,
 
 	return 0;
 
-err_workqueues:
+err_irq:
+	intel_irq_fini(dev_priv);
 	i915_workqueues_cleanup(dev_priv);
 err_engines:
 	i915_engines_cleanup(dev_priv);
@@ -882,6 +880,7 @@ static void i915_driver_cleanup_early(struct drm_i915_private *dev_priv)
 {
 	i915_perf_fini(dev_priv);
 	i915_gem_load_cleanup(dev_priv);
+	intel_irq_fini(dev_priv);
 	i915_workqueues_cleanup(dev_priv);
 	i915_engines_cleanup(dev_priv);
 }
@@ -947,14 +946,21 @@ static int i915_driver_init_mmio(struct drm_i915_private *dev_priv)
 
 	ret = i915_mmio_setup(dev_priv);
 	if (ret < 0)
-		goto put_bridge;
+		goto err_bridge;
 
 	intel_uncore_init(dev_priv);
+
+	ret = intel_engines_init_mmio(dev_priv);
+	if (ret)
+		goto err_uncore;
+
 	i915_gem_init_mmio(dev_priv);
 
 	return 0;
 
-put_bridge:
+err_uncore:
+	intel_uncore_fini(dev_priv);
+err_bridge:
 	pci_dev_put(dev_priv->bridge_dev);
 
 	return ret;
@@ -991,6 +997,8 @@ static void intel_sanitize_options(struct drm_i915_private *dev_priv)
 	DRM_DEBUG_DRIVER("use GPU semaphores? %s\n", yesno(i915.semaphores));
 
 	intel_uc_sanitize_options(dev_priv);
+
+	intel_gvt_sanitize_options(dev_priv);
 }
 
 /**
@@ -1213,9 +1221,8 @@ int i915_driver_load(struct pci_dev *pdev, const struct pci_device_id *ent)
 	struct drm_i915_private *dev_priv;
 	int ret;
 
-	/* Enable nuclear pageflip on ILK+, except vlv/chv */
-	if (!i915.nuclear_pageflip &&
-	    (match_info->gen < 5 || match_info->has_gmch_display))
+	/* Enable nuclear pageflip on ILK+ */
+	if (!i915.nuclear_pageflip && match_info->gen < 5)
 		driver.driver_features &= ~DRIVER_ATOMIC;
 
 	ret = -ENOMEM;
@@ -1235,6 +1242,15 @@ int i915_driver_load(struct pci_dev *pdev, const struct pci_device_id *ent)
 		goto out_fini;
 
 	pci_set_drvdata(pdev, &dev_priv->drm);
+	/*
+	 * Disable the system suspend direct complete optimization, which can
+	 * leave the device suspended skipping the driver's suspend handlers
+	 * if the device was already runtime suspended. This is needed due to
+	 * the difference in our runtime and system suspend sequence and
+	 * becaue the HDA driver may require us to enable the audio power
+	 * domain during system suspend.
+	 */
+	pdev->dev_flags |= PCI_DEV_FLAGS_NEEDS_RESUME;
 
 	ret = i915_driver_init_early(dev_priv, ent);
 	if (ret < 0)
@@ -1272,10 +1288,6 @@ int i915_driver_load(struct pci_dev *pdev, const struct pci_device_id *ent)
 
 	dev_priv->ipc_enabled = false;
 
-	/* Everything is in place, we can now relax! */
-	DRM_INFO("Initialized %s %d.%d.%d %s for %s on minor %d\n",
-		 driver.name, driver.major, driver.minor, driver.patchlevel,
-		 driver.date, pci_name(pdev), dev_priv->drm.primary->index);
 	if (IS_ENABLED(CONFIG_DRM_I915_DEBUG))
 		DRM_INFO("DRM_I915_DEBUG enabled\n");
 	if (IS_ENABLED(CONFIG_DRM_I915_DEBUG_GEM))
diff --git a/drivers/gpu/drm/i915/i915_drv.h b/drivers/gpu/drm/i915/i915_drv.h
index c9b0949f6c1a..a3b2674a4b7d 100644
--- a/drivers/gpu/drm/i915/i915_drv.h
+++ b/drivers/gpu/drm/i915/i915_drv.h
@@ -55,6 +55,7 @@
 #include "i915_reg.h"
 #include "i915_utils.h"
 
+#include "intel_uncore.h"
 #include "intel_bios.h"
 #include "intel_dpll_mgr.h"
 #include "intel_uc.h"
@@ -79,8 +80,8 @@
 
 #define DRIVER_NAME		"i915"
 #define DRIVER_DESC		"Intel Graphics"
-#define DRIVER_DATE		"20170403"
-#define DRIVER_TIMESTAMP	1491198738
+#define DRIVER_DATE		"20170529"
+#define DRIVER_TIMESTAMP	1496041258
 
 /* Use I915_STATE_WARN(x) and I915_STATE_WARN_ON() (rather than WARN() and
  * WARN_ON()) for hw state sanity checks to check for unexpected conditions
@@ -114,6 +115,13 @@ typedef struct {
 	fp; \
 })
 
+static inline bool is_fixed16_zero(uint_fixed_16_16_t val)
+{
+	if (val.val == 0)
+		return true;
+	return false;
+}
+
 static inline uint_fixed_16_16_t u32_to_fixed_16_16(uint32_t val)
 {
 	uint_fixed_16_16_t fp;
@@ -152,8 +160,39 @@ static inline uint_fixed_16_16_t max_fixed_16_16(uint_fixed_16_16_t max1,
 	return max;
 }
 
-static inline uint_fixed_16_16_t fixed_16_16_div_round_up(uint32_t val,
-							  uint32_t d)
+static inline uint32_t div_round_up_fixed16(uint_fixed_16_16_t val,
+					    uint_fixed_16_16_t d)
+{
+	return DIV_ROUND_UP(val.val, d.val);
+}
+
+static inline uint32_t mul_round_up_u32_fixed16(uint32_t val,
+						uint_fixed_16_16_t mul)
+{
+	uint64_t intermediate_val;
+	uint32_t result;
+
+	intermediate_val = (uint64_t) val * mul.val;
+	intermediate_val = DIV_ROUND_UP_ULL(intermediate_val, 1 << 16);
+	WARN_ON(intermediate_val >> 32);
+	result = clamp_t(uint32_t, intermediate_val, 0, ~0);
+	return result;
+}
+
+static inline uint_fixed_16_16_t mul_fixed16(uint_fixed_16_16_t val,
+					     uint_fixed_16_16_t mul)
+{
+	uint64_t intermediate_val;
+	uint_fixed_16_16_t fp;
+
+	intermediate_val = (uint64_t) val.val * mul.val;
+	intermediate_val = intermediate_val >> 16;
+	WARN_ON(intermediate_val >> 32);
+	fp.val = clamp_t(uint32_t, intermediate_val, 0, ~0);
+	return fp;
+}
+
+static inline uint_fixed_16_16_t fixed_16_16_div(uint32_t val, uint32_t d)
 {
 	uint_fixed_16_16_t fp, res;
 
@@ -162,8 +201,7 @@ static inline uint_fixed_16_16_t fixed_16_16_div_round_up(uint32_t val,
 	return res;
 }
 
-static inline uint_fixed_16_16_t fixed_16_16_div_round_up_u64(uint32_t val,
-							      uint32_t d)
+static inline uint_fixed_16_16_t fixed_16_16_div_u64(uint32_t val, uint32_t d)
 {
 	uint_fixed_16_16_t res;
 	uint64_t interm_val;
@@ -176,6 +214,17 @@ static inline uint_fixed_16_16_t fixed_16_16_div_round_up_u64(uint32_t val,
 	return res;
 }
 
+static inline uint32_t div_round_up_u32_fixed16(uint32_t val,
+						uint_fixed_16_16_t d)
+{
+	uint64_t interm_val;
+
+	interm_val = (uint64_t)val << 16;
+	interm_val = DIV_ROUND_UP_ULL(interm_val, d.val);
+	WARN_ON(interm_val >> 32);
+	return clamp_t(uint32_t, interm_val, 0, ~0);
+}
+
 static inline uint_fixed_16_16_t mul_u32_fixed_16_16(uint32_t val,
 						     uint_fixed_16_16_t mul)
 {
@@ -676,116 +725,6 @@ struct drm_i915_display_funcs {
 	void (*load_luts)(struct drm_crtc_state *crtc_state);
 };
 
-enum forcewake_domain_id {
-	FW_DOMAIN_ID_RENDER = 0,
-	FW_DOMAIN_ID_BLITTER,
-	FW_DOMAIN_ID_MEDIA,
-
-	FW_DOMAIN_ID_COUNT
-};
-
-enum forcewake_domains {
-	FORCEWAKE_RENDER = BIT(FW_DOMAIN_ID_RENDER),
-	FORCEWAKE_BLITTER = BIT(FW_DOMAIN_ID_BLITTER),
-	FORCEWAKE_MEDIA	= BIT(FW_DOMAIN_ID_MEDIA),
-	FORCEWAKE_ALL = (FORCEWAKE_RENDER |
-			 FORCEWAKE_BLITTER |
-			 FORCEWAKE_MEDIA)
-};
-
-#define FW_REG_READ  (1)
-#define FW_REG_WRITE (2)
-
-enum decoupled_power_domain {
-	GEN9_DECOUPLED_PD_BLITTER = 0,
-	GEN9_DECOUPLED_PD_RENDER,
-	GEN9_DECOUPLED_PD_MEDIA,
-	GEN9_DECOUPLED_PD_ALL
-};
-
-enum decoupled_ops {
-	GEN9_DECOUPLED_OP_WRITE = 0,
-	GEN9_DECOUPLED_OP_READ
-};
-
-enum forcewake_domains
-intel_uncore_forcewake_for_reg(struct drm_i915_private *dev_priv,
-			       i915_reg_t reg, unsigned int op);
-
-struct intel_uncore_funcs {
-	void (*force_wake_get)(struct drm_i915_private *dev_priv,
-			       enum forcewake_domains domains);
-	void (*force_wake_put)(struct drm_i915_private *dev_priv,
-			       enum forcewake_domains domains);
-
-	uint8_t  (*mmio_readb)(struct drm_i915_private *dev_priv,
-			       i915_reg_t r, bool trace);
-	uint16_t (*mmio_readw)(struct drm_i915_private *dev_priv,
-			       i915_reg_t r, bool trace);
-	uint32_t (*mmio_readl)(struct drm_i915_private *dev_priv,
-			       i915_reg_t r, bool trace);
-	uint64_t (*mmio_readq)(struct drm_i915_private *dev_priv,
-			       i915_reg_t r, bool trace);
-
-	void (*mmio_writeb)(struct drm_i915_private *dev_priv,
-			    i915_reg_t r, uint8_t val, bool trace);
-	void (*mmio_writew)(struct drm_i915_private *dev_priv,
-			    i915_reg_t r, uint16_t val, bool trace);
-	void (*mmio_writel)(struct drm_i915_private *dev_priv,
-			    i915_reg_t r, uint32_t val, bool trace);
-};
-
-struct intel_forcewake_range {
-	u32 start;
-	u32 end;
-
-	enum forcewake_domains domains;
-};
-
-struct intel_uncore {
-	spinlock_t lock; /** lock is also taken in irq contexts. */
-
-	const struct intel_forcewake_range *fw_domains_table;
-	unsigned int fw_domains_table_entries;
-
-	struct notifier_block pmic_bus_access_nb;
-	struct intel_uncore_funcs funcs;
-
-	unsigned fifo_count;
-
-	enum forcewake_domains fw_domains;
-	enum forcewake_domains fw_domains_active;
-
-	u32 fw_set;
-	u32 fw_clear;
-	u32 fw_reset;
-
-	struct intel_uncore_forcewake_domain {
-		enum forcewake_domain_id id;
-		enum forcewake_domains mask;
-		unsigned wake_count;
-		struct hrtimer timer;
-		i915_reg_t reg_set;
-		i915_reg_t reg_ack;
-	} fw_domain[FW_DOMAIN_ID_COUNT];
-
-	int unclaimed_mmio_check;
-};
-
-#define __mask_next_bit(mask) ({					\
-	int __idx = ffs(mask) - 1;					\
-	mask &= ~BIT(__idx);						\
-	__idx;								\
-})
-
-/* Iterate over initialised fw domains */
-#define for_each_fw_domain_masked(domain__, mask__, dev_priv__, tmp__) \
-	for (tmp__ = (mask__); \
-	     tmp__ ? (domain__ = &(dev_priv__)->uncore.fw_domain[__mask_next_bit(tmp__)]), 1 : 0;)
-
-#define for_each_fw_domain(domain__, dev_priv__, tmp__) \
-	for_each_fw_domain_masked(domain__, (dev_priv__)->uncore.fw_domains, dev_priv__, tmp__)
-
 #define CSR_VERSION(major, minor)	((major) << 16 | (minor))
 #define CSR_VERSION_MAJOR(version)	((version) >> 16)
 #define CSR_VERSION_MINOR(version)	((version) & 0xffff)
@@ -812,7 +751,6 @@ struct intel_csr {
 	func(has_aliasing_ppgtt); \
 	func(has_csr); \
 	func(has_ddi); \
-	func(has_decoupled_mmio); \
 	func(has_dp_mst); \
 	func(has_fbc); \
 	func(has_fpga_dbg); \
@@ -821,8 +759,8 @@ struct intel_csr {
 	func(has_gmbus_irq); \
 	func(has_gmch_display); \
 	func(has_guc); \
+	func(has_guc_ct); \
 	func(has_hotplug); \
-	func(has_hw_contexts); \
 	func(has_l3_dpf); \
 	func(has_llc); \
 	func(has_logical_ring_contexts); \
@@ -1025,6 +963,9 @@ struct i915_gpu_state {
 			u32 *pages[0];
 		} *ringbuffer, *batchbuffer, *wa_batchbuffer, *ctx, *hws_page;
 
+		struct drm_i915_error_object **user_bo;
+		long user_bo_count;
+
 		struct drm_i915_error_object *wa_ctx;
 
 		struct drm_i915_error_request {
@@ -1511,11 +1452,7 @@ struct i915_gem_mm {
 	/** LRU list of objects with fence regs on them. */
 	struct list_head fence_list;
 
-	/**
-	 * Are we in a non-interruptible section of code like
-	 * modesetting?
-	 */
-	bool interruptible;
+	u64 unordered_timeline;
 
 	/* the indicator for dispatch video commands on two BSD rings */
 	atomic_t bsd_engine_dispatch_index;
@@ -1566,7 +1503,7 @@ struct i915_gpu_error {
 	 *
 	 * This is a counter which gets incremented when reset is triggered,
 	 *
-	 * Before the reset commences, the I915_RESET_IN_PROGRESS bit is set
+	 * Before the reset commences, the I915_RESET_BACKOFF bit is set
 	 * meaning that any waiters holding onto the struct_mutex should
 	 * relinquish the lock immediately in order for the reset to start.
 	 *
@@ -1763,13 +1700,15 @@ struct ilk_wm_values {
 	enum intel_ddb_partitioning partitioning;
 };
 
-struct vlv_pipe_wm {
+struct g4x_pipe_wm {
 	uint16_t plane[I915_MAX_PLANES];
+	uint16_t fbc;
 };
 
-struct vlv_sr_wm {
+struct g4x_sr_wm {
 	uint16_t plane;
 	uint16_t cursor;
+	uint16_t fbc;
 };
 
 struct vlv_wm_ddl_values {
@@ -1777,13 +1716,22 @@ struct vlv_wm_ddl_values {
 };
 
 struct vlv_wm_values {
-	struct vlv_pipe_wm pipe[3];
-	struct vlv_sr_wm sr;
+	struct g4x_pipe_wm pipe[3];
+	struct g4x_sr_wm sr;
 	struct vlv_wm_ddl_values ddl[3];
 	uint8_t level;
 	bool cxsr;
 };
 
+struct g4x_wm_values {
+	struct g4x_pipe_wm pipe[2];
+	struct g4x_sr_wm sr;
+	struct g4x_sr_wm hpll;
+	bool cxsr;
+	bool hpll_en;
+	bool fbc_en;
+};
+
 struct skl_ddb_entry {
 	uint16_t start, end;	/* in number of blocks, 'end' is exclusive */
 };
@@ -2100,7 +2048,7 @@ struct i915_oa_ops {
 		    size_t *offset);
 
 	/**
-	 * @oa_buffer_is_empty: Check if OA buffer empty (false positives OK)
+	 * @oa_buffer_check: Check for OA buffer data + update tail
 	 *
 	 * This is either called via fops or the poll check hrtimer (atomic
 	 * ctx) without any locks taken.
@@ -2113,7 +2061,7 @@ struct i915_oa_ops {
 	 * here, which will be handled gracefully - likely resulting in an
 	 * %EAGAIN error for userspace.
 	 */
-	bool (*oa_buffer_is_empty)(struct drm_i915_private *dev_priv);
+	bool (*oa_buffer_check)(struct drm_i915_private *dev_priv);
 };
 
 struct intel_cdclk_state {
@@ -2127,6 +2075,7 @@ struct drm_i915_private {
 	struct kmem_cache *vmas;
 	struct kmem_cache *requests;
 	struct kmem_cache *dependencies;
+	struct kmem_cache *priorities;
 
 	const struct intel_device_info info;
 
@@ -2362,7 +2311,6 @@ struct drm_i915_private {
 	 */
 	struct mutex av_mutex;
 
-	uint32_t hw_context_size;
 	struct list_head context_list;
 
 	u32 fdi_rx_config;
@@ -2413,6 +2361,7 @@ struct drm_i915_private {
 			struct ilk_wm_values hw;
 			struct skl_wm_values skl_hw;
 			struct vlv_wm_values vlv;
+			struct g4x_wm_values g4x;
 		};
 
 		uint8_t max_level;
@@ -2454,11 +2403,14 @@ struct drm_i915_private {
 			wait_queue_head_t poll_wq;
 			bool pollin;
 
+			/**
+			 * For rate limiting any notifications of spurious
+			 * invalid OA reports
+			 */
+			struct ratelimit_state spurious_report_rs;
+
 			bool periodic;
 			int period_exponent;
-			int timestamp_frequency;
-
-			int tail_margin;
 
 			int metrics_set;
 
@@ -2472,6 +2424,70 @@ struct drm_i915_private {
 				u8 *vaddr;
 				int format;
 				int format_size;
+
+				/**
+				 * Locks reads and writes to all head/tail state
+				 *
+				 * Consider: the head and tail pointer state
+				 * needs to be read consistently from a hrtimer
+				 * callback (atomic context) and read() fop
+				 * (user context) with tail pointer updates
+				 * happening in atomic context and head updates
+				 * in user context and the (unlikely)
+				 * possibility of read() errors needing to
+				 * reset all head/tail state.
+				 *
+				 * Note: Contention or performance aren't
+				 * currently a significant concern here
+				 * considering the relatively low frequency of
+				 * hrtimer callbacks (5ms period) and that
+				 * reads typically only happen in response to a
+				 * hrtimer event and likely complete before the
+				 * next callback.
+				 *
+				 * Note: This lock is not held *while* reading
+				 * and copying data to userspace so the value
+				 * of head observed in htrimer callbacks won't
+				 * represent any partial consumption of data.
+				 */
+				spinlock_t ptr_lock;
+
+				/**
+				 * One 'aging' tail pointer and one 'aged'
+				 * tail pointer ready to used for reading.
+				 *
+				 * Initial values of 0xffffffff are invalid
+				 * and imply that an update is required
+				 * (and should be ignored by an attempted
+				 * read)
+				 */
+				struct {
+					u32 offset;
+				} tails[2];
+
+				/**
+				 * Index for the aged tail ready to read()
+				 * data up to.
+				 */
+				unsigned int aged_tail_idx;
+
+				/**
+				 * A monotonic timestamp for when the current
+				 * aging tail pointer was read; used to
+				 * determine when it is old enough to trust.
+				 */
+				u64 aging_timestamp;
+
+				/**
+				 * Although we can always read back the head
+				 * pointer register, we prefer to avoid
+				 * trusting the HW state, just to avoid any
+				 * risk that some hardware condition could
+				 * somehow bump the head pointer unpredictably
+				 * and cause us to forward the wrong OA buffer
+				 * data to userspace.
+				 */
+				u32 head;
 			} oa_buffer;
 
 			u32 gen7_latched_oastatus1;
@@ -2870,7 +2886,6 @@ intel_info(const struct drm_i915_private *dev_priv)
 
 #define HWS_NEEDS_PHYSICAL(dev_priv)	((dev_priv)->info.hws_needs_physical)
 
-#define HAS_HW_CONTEXTS(dev_priv)	    ((dev_priv)->info.has_hw_contexts)
 #define HAS_LOGICAL_RING_CONTEXTS(dev_priv) \
 		((dev_priv)->info.has_logical_ring_contexts)
 #define USES_PPGTT(dev_priv)		(i915.enable_ppgtt)
@@ -2909,6 +2924,7 @@ intel_info(const struct drm_i915_private *dev_priv)
 #define HAS_FW_BLC(dev_priv) 	(INTEL_GEN(dev_priv) > 2)
 #define HAS_PIPE_CXSR(dev_priv) ((dev_priv)->info.has_pipe_cxsr)
 #define HAS_FBC(dev_priv)	((dev_priv)->info.has_fbc)
+#define HAS_CUR_FBC(dev_priv)	(!HAS_GMCH_DISPLAY(dev_priv) && INTEL_INFO(dev_priv)->gen >= 7)
 
 #define HAS_IPS(dev_priv)	(IS_HSW_ULT(dev_priv) || IS_BROADWELL(dev_priv))
 
@@ -2931,6 +2947,7 @@ intel_info(const struct drm_i915_private *dev_priv)
  * properties, so we have separate macros to test them.
  */
 #define HAS_GUC(dev_priv)	((dev_priv)->info.has_guc)
+#define HAS_GUC_CT(dev_priv)	((dev_priv)->info.has_guc_ct)
 #define HAS_GUC_UCODE(dev_priv)	(HAS_GUC(dev_priv))
 #define HAS_GUC_SCHED(dev_priv)	(HAS_GUC(dev_priv))
 #define HAS_HUC_UCODE(dev_priv)	(HAS_GUC(dev_priv))
@@ -2977,19 +2994,28 @@ intel_info(const struct drm_i915_private *dev_priv)
 #define GT_FREQUENCY_MULTIPLIER 50
 #define GEN9_FREQ_SCALER 3
 
-#define HAS_DECOUPLED_MMIO(dev_priv) (INTEL_INFO(dev_priv)->has_decoupled_mmio)
-
 #include "i915_trace.h"
 
-static inline bool intel_scanout_needs_vtd_wa(struct drm_i915_private *dev_priv)
+static inline bool intel_vtd_active(void)
 {
 #ifdef CONFIG_INTEL_IOMMU
-	if (INTEL_GEN(dev_priv) >= 6 && intel_iommu_gfx_mapped)
+	if (intel_iommu_gfx_mapped)
 		return true;
 #endif
 	return false;
 }
 
+static inline bool intel_scanout_needs_vtd_wa(struct drm_i915_private *dev_priv)
+{
+	return INTEL_GEN(dev_priv) >= 6 && intel_vtd_active();
+}
+
+static inline bool
+intel_ggtt_update_needs_vtd_wa(struct drm_i915_private *dev_priv)
+{
+	return IS_BROXTON(dev_priv) && intel_vtd_active();
+}
+
 int intel_sanitize_enable_ppgtt(struct drm_i915_private *dev_priv,
 				int enable_ppgtt);
 
@@ -3026,7 +3052,7 @@ extern unsigned long i915_gfx_val(struct drm_i915_private *dev_priv);
 extern void i915_update_gfx_val(struct drm_i915_private *dev_priv);
 int vlv_force_gfx_clock(struct drm_i915_private *dev_priv, bool on);
 
-int intel_engines_init_early(struct drm_i915_private *dev_priv);
+int intel_engines_init_mmio(struct drm_i915_private *dev_priv);
 int intel_engines_init(struct drm_i915_private *dev_priv);
 
 /* intel_hotplug.c */
@@ -3063,43 +3089,10 @@ void i915_handle_error(struct drm_i915_private *dev_priv,
 		       const char *fmt, ...);
 
 extern void intel_irq_init(struct drm_i915_private *dev_priv);
+extern void intel_irq_fini(struct drm_i915_private *dev_priv);
 int intel_irq_install(struct drm_i915_private *dev_priv);
 void intel_irq_uninstall(struct drm_i915_private *dev_priv);
 
-extern void intel_uncore_sanitize(struct drm_i915_private *dev_priv);
-extern void intel_uncore_init(struct drm_i915_private *dev_priv);
-extern bool intel_uncore_unclaimed_mmio(struct drm_i915_private *dev_priv);
-extern bool intel_uncore_arm_unclaimed_mmio_detection(struct drm_i915_private *dev_priv);
-extern void intel_uncore_fini(struct drm_i915_private *dev_priv);
-extern void intel_uncore_suspend(struct drm_i915_private *dev_priv);
-extern void intel_uncore_resume_early(struct drm_i915_private *dev_priv);
-const char *intel_uncore_forcewake_domain_to_str(const enum forcewake_domain_id id);
-void intel_uncore_forcewake_get(struct drm_i915_private *dev_priv,
-				enum forcewake_domains domains);
-void intel_uncore_forcewake_put(struct drm_i915_private *dev_priv,
-				enum forcewake_domains domains);
-/* Like above but the caller must manage the uncore.lock itself.
- * Must be used with I915_READ_FW and friends.
- */
-void intel_uncore_forcewake_get__locked(struct drm_i915_private *dev_priv,
-					enum forcewake_domains domains);
-void intel_uncore_forcewake_put__locked(struct drm_i915_private *dev_priv,
-					enum forcewake_domains domains);
-u64 intel_uncore_edram_size(struct drm_i915_private *dev_priv);
-
-void assert_forcewakes_inactive(struct drm_i915_private *dev_priv);
-
-int intel_wait_for_register(struct drm_i915_private *dev_priv,
-			    i915_reg_t reg,
-			    const u32 mask,
-			    const u32 value,
-			    const unsigned long timeout_ms);
-int intel_wait_for_register_fw(struct drm_i915_private *dev_priv,
-			       i915_reg_t reg,
-			       const u32 mask,
-			       const u32 value,
-			       const unsigned long timeout_ms);
-
 static inline bool intel_gvt_active(struct drm_i915_private *dev_priv)
 {
 	return dev_priv->gvt;
@@ -3447,8 +3440,9 @@ int i915_gem_object_wait_priority(struct drm_i915_gem_object *obj,
 #define I915_PRIORITY_DISPLAY I915_PRIORITY_MAX
 
 int __must_check
-i915_gem_object_set_to_gtt_domain(struct drm_i915_gem_object *obj,
-				  bool write);
+i915_gem_object_set_to_wc_domain(struct drm_i915_gem_object *obj, bool write);
+int __must_check
+i915_gem_object_set_to_gtt_domain(struct drm_i915_gem_object *obj, bool write);
 int __must_check
 i915_gem_object_set_to_cpu_domain(struct drm_i915_gem_object *obj, bool write);
 struct i915_vma * __must_check
@@ -3711,8 +3705,8 @@ int  intel_lpe_audio_init(struct drm_i915_private *dev_priv);
 void intel_lpe_audio_teardown(struct drm_i915_private *dev_priv);
 void intel_lpe_audio_irq_handler(struct drm_i915_private *dev_priv);
 void intel_lpe_audio_notify(struct drm_i915_private *dev_priv,
-			    void *eld, int port, int pipe, int tmds_clk_speed,
-			    bool dp_output, int link_rate);
+			    enum pipe pipe, enum port port,
+			    const void *eld, int ls_clock, bool dp_output);
 
 /* intel_i2c.c */
 extern int intel_setup_gmbus(struct drm_i915_private *dev_priv);
diff --git a/drivers/gpu/drm/i915/i915_gem.c b/drivers/gpu/drm/i915/i915_gem.c
index 0e07f35e270c..7b676fd1f075 100644
--- a/drivers/gpu/drm/i915/i915_gem.c
+++ b/drivers/gpu/drm/i915/i915_gem.c
@@ -46,8 +46,6 @@
 #include <linux/dma-buf.h>
 
 static void i915_gem_flush_free_objects(struct drm_i915_private *i915);
-static void i915_gem_object_flush_gtt_write_domain(struct drm_i915_gem_object *obj);
-static void i915_gem_object_flush_cpu_write_domain(struct drm_i915_gem_object *obj);
 
 static bool cpu_write_needs_clflush(struct drm_i915_gem_object *obj)
 {
@@ -705,6 +703,61 @@ i915_gem_create_ioctl(struct drm_device *dev, void *data,
 			       args->size, &args->handle);
 }
 
+static inline enum fb_op_origin
+fb_write_origin(struct drm_i915_gem_object *obj, unsigned int domain)
+{
+	return (domain == I915_GEM_DOMAIN_GTT ?
+		obj->frontbuffer_ggtt_origin : ORIGIN_CPU);
+}
+
+static void
+flush_write_domain(struct drm_i915_gem_object *obj, unsigned int flush_domains)
+{
+	struct drm_i915_private *dev_priv = to_i915(obj->base.dev);
+
+	if (!(obj->base.write_domain & flush_domains))
+		return;
+
+	/* No actual flushing is required for the GTT write domain.  Writes
+	 * to it "immediately" go to main memory as far as we know, so there's
+	 * no chipset flush.  It also doesn't land in render cache.
+	 *
+	 * However, we do have to enforce the order so that all writes through
+	 * the GTT land before any writes to the device, such as updates to
+	 * the GATT itself.
+	 *
+	 * We also have to wait a bit for the writes to land from the GTT.
+	 * An uncached read (i.e. mmio) seems to be ideal for the round-trip
+	 * timing. This issue has only been observed when switching quickly
+	 * between GTT writes and CPU reads from inside the kernel on recent hw,
+	 * and it appears to only affect discrete GTT blocks (i.e. on LLC
+	 * system agents we cannot reproduce this behaviour).
+	 */
+	wmb();
+
+	switch (obj->base.write_domain) {
+	case I915_GEM_DOMAIN_GTT:
+		if (INTEL_GEN(dev_priv) >= 6 && !HAS_LLC(dev_priv)) {
+			if (intel_runtime_pm_get_if_in_use(dev_priv)) {
+				spin_lock_irq(&dev_priv->uncore.lock);
+				POSTING_READ_FW(RING_ACTHD(dev_priv->engine[RCS]->mmio_base));
+				spin_unlock_irq(&dev_priv->uncore.lock);
+				intel_runtime_pm_put(dev_priv);
+			}
+		}
+
+		intel_fb_obj_flush(obj,
+				   fb_write_origin(obj, I915_GEM_DOMAIN_GTT));
+		break;
+
+	case I915_GEM_DOMAIN_CPU:
+		i915_gem_clflush_object(obj, I915_CLFLUSH_SYNC);
+		break;
+	}
+
+	obj->base.write_domain = 0;
+}
+
 static inline int
 __copy_to_user_swizzled(char __user *cpu_vaddr,
 			const char *gpu_vaddr, int gpu_offset,
@@ -794,7 +847,7 @@ int i915_gem_obj_prepare_shmem_read(struct drm_i915_gem_object *obj,
 			goto out;
 	}
 
-	i915_gem_object_flush_gtt_write_domain(obj);
+	flush_write_domain(obj, ~I915_GEM_DOMAIN_CPU);
 
 	/* If we're not in the cpu read domain, set ourself into the gtt
 	 * read domain and manually flush cachelines (if required). This
@@ -846,7 +899,7 @@ int i915_gem_obj_prepare_shmem_write(struct drm_i915_gem_object *obj,
 			goto out;
 	}
 
-	i915_gem_object_flush_gtt_write_domain(obj);
+	flush_write_domain(obj, ~I915_GEM_DOMAIN_CPU);
 
 	/* If we're not in the cpu write domain, set ourself into the
 	 * gtt write domain and manually flush cachelines (as required).
@@ -1501,13 +1554,6 @@ err:
 	return ret;
 }
 
-static inline enum fb_op_origin
-write_origin(struct drm_i915_gem_object *obj, unsigned domain)
-{
-	return (domain == I915_GEM_DOMAIN_GTT ?
-		obj->frontbuffer_ggtt_origin : ORIGIN_CPU);
-}
-
 static void i915_gem_object_bump_inactive_ggtt(struct drm_i915_gem_object *obj)
 {
 	struct drm_i915_private *i915;
@@ -1591,10 +1637,12 @@ i915_gem_set_domain_ioctl(struct drm_device *dev, void *data,
 	if (err)
 		goto out_unpin;
 
-	if (read_domains & I915_GEM_DOMAIN_GTT)
-		err = i915_gem_object_set_to_gtt_domain(obj, write_domain != 0);
+	if (read_domains & I915_GEM_DOMAIN_WC)
+		err = i915_gem_object_set_to_wc_domain(obj, write_domain);
+	else if (read_domains & I915_GEM_DOMAIN_GTT)
+		err = i915_gem_object_set_to_gtt_domain(obj, write_domain);
 	else
-		err = i915_gem_object_set_to_cpu_domain(obj, write_domain != 0);
+		err = i915_gem_object_set_to_cpu_domain(obj, write_domain);
 
 	/* And bump the LRU for this access */
 	i915_gem_object_bump_inactive_ggtt(obj);
@@ -1602,7 +1650,8 @@ i915_gem_set_domain_ioctl(struct drm_device *dev, void *data,
 	mutex_unlock(&dev->struct_mutex);
 
 	if (write_domain != 0)
-		intel_fb_obj_invalidate(obj, write_origin(obj, write_domain));
+		intel_fb_obj_invalidate(obj,
+					fb_write_origin(obj, write_domain));
 
 out_unpin:
 	i915_gem_object_unpin_pages(obj);
@@ -1737,6 +1786,9 @@ static unsigned int tile_row_pages(struct drm_i915_gem_object *obj)
  *     into userspace. (This view is aligned and sized appropriately for
  *     fenced access.)
  *
+ * 2 - Recognise WC as a separate cache domain so that we can flush the
+ *     delayed writes via GTT before performing direct access via WC.
+ *
  * Restrictions:
  *
  *  * snoopable objects cannot be accessed via the GTT. It can cause machine
@@ -1764,7 +1816,7 @@ static unsigned int tile_row_pages(struct drm_i915_gem_object *obj)
  */
 int i915_gem_mmap_gtt_version(void)
 {
-	return 1;
+	return 2;
 }
 
 static inline struct i915_ggtt_view
@@ -2228,7 +2280,7 @@ void __i915_gem_object_put_pages(struct drm_i915_gem_object *obj,
 	if (obj->mm.mapping) {
 		void *ptr;
 
-		ptr = ptr_mask_bits(obj->mm.mapping);
+		ptr = page_mask_bits(obj->mm.mapping);
 		if (is_vmalloc_addr(ptr))
 			vunmap(ptr);
 		else
@@ -2560,7 +2612,7 @@ void *i915_gem_object_pin_map(struct drm_i915_gem_object *obj,
 	}
 	GEM_BUG_ON(!obj->mm.pages);
 
-	ptr = ptr_unpack_bits(obj->mm.mapping, has_type);
+	ptr = page_unpack_bits(obj->mm.mapping, &has_type);
 	if (ptr && has_type != type) {
 		if (pinned) {
 			ret = -EBUSY;
@@ -2582,7 +2634,7 @@ void *i915_gem_object_pin_map(struct drm_i915_gem_object *obj,
 			goto err_unpin;
 		}
 
-		obj->mm.mapping = ptr_pack_bits(ptr, type);
+		obj->mm.mapping = page_pack_bits(ptr, type);
 	}
 
 out_unlock:
@@ -2967,12 +3019,14 @@ static void engine_set_wedged(struct intel_engine_cs *engine)
 	 */
 
 	if (i915.enable_execlists) {
+		struct execlist_port *port = engine->execlist_port;
 		unsigned long flags;
+		unsigned int n;
 
 		spin_lock_irqsave(&engine->timeline->lock, flags);
 
-		i915_gem_request_put(engine->execlist_port[0].request);
-		i915_gem_request_put(engine->execlist_port[1].request);
+		for (n = 0; n < ARRAY_SIZE(engine->execlist_port); n++)
+			i915_gem_request_put(port_request(&port[n]));
 		memset(engine->execlist_port, 0, sizeof(engine->execlist_port));
 		engine->execlist_queue = RB_ROOT;
 		engine->execlist_first = NULL;
@@ -3101,8 +3155,6 @@ i915_gem_idle_work_handler(struct work_struct *work)
 	struct drm_i915_private *dev_priv =
 		container_of(work, typeof(*dev_priv), gt.idle_work.work);
 	struct drm_device *dev = &dev_priv->drm;
-	struct intel_engine_cs *engine;
-	enum intel_engine_id id;
 	bool rearm_hangcheck;
 
 	if (!READ_ONCE(dev_priv->gt.awake))
@@ -3140,10 +3192,8 @@ i915_gem_idle_work_handler(struct work_struct *work)
 	if (wait_for(intel_engines_are_idle(dev_priv), 10))
 		DRM_ERROR("Timeout waiting for engines to idle\n");
 
-	for_each_engine(engine, dev_priv, id) {
-		intel_engine_disarm_breadcrumbs(engine);
-		i915_gem_batch_pool_fini(&engine->batch_pool);
-	}
+	intel_engines_mark_idle(dev_priv);
+	i915_gem_timelines_mark_idle(dev_priv);
 
 	GEM_BUG_ON(!dev_priv->gt.awake);
 	dev_priv->gt.awake = false;
@@ -3298,6 +3348,10 @@ int i915_gem_wait_for_idle(struct drm_i915_private *i915, unsigned int flags)
 {
 	int ret;
 
+	/* If the device is asleep, we have no requests outstanding */
+	if (!READ_ONCE(i915->gt.awake))
+		return 0;
+
 	if (flags & I915_WAIT_LOCKED) {
 		struct i915_gem_timeline *tl;
 
@@ -3320,56 +3374,6 @@ int i915_gem_wait_for_idle(struct drm_i915_private *i915, unsigned int flags)
 	return ret;
 }
 
-/** Flushes the GTT write domain for the object if it's dirty. */
-static void
-i915_gem_object_flush_gtt_write_domain(struct drm_i915_gem_object *obj)
-{
-	struct drm_i915_private *dev_priv = to_i915(obj->base.dev);
-
-	if (obj->base.write_domain != I915_GEM_DOMAIN_GTT)
-		return;
-
-	/* No actual flushing is required for the GTT write domain.  Writes
-	 * to it "immediately" go to main memory as far as we know, so there's
-	 * no chipset flush.  It also doesn't land in render cache.
-	 *
-	 * However, we do have to enforce the order so that all writes through
-	 * the GTT land before any writes to the device, such as updates to
-	 * the GATT itself.
-	 *
-	 * We also have to wait a bit for the writes to land from the GTT.
-	 * An uncached read (i.e. mmio) seems to be ideal for the round-trip
-	 * timing. This issue has only been observed when switching quickly
-	 * between GTT writes and CPU reads from inside the kernel on recent hw,
-	 * and it appears to only affect discrete GTT blocks (i.e. on LLC
-	 * system agents we cannot reproduce this behaviour).
-	 */
-	wmb();
-	if (INTEL_GEN(dev_priv) >= 6 && !HAS_LLC(dev_priv)) {
-		if (intel_runtime_pm_get_if_in_use(dev_priv)) {
-			spin_lock_irq(&dev_priv->uncore.lock);
-			POSTING_READ_FW(RING_ACTHD(dev_priv->engine[RCS]->mmio_base));
-			spin_unlock_irq(&dev_priv->uncore.lock);
-			intel_runtime_pm_put(dev_priv);
-		}
-	}
-
-	intel_fb_obj_flush(obj, write_origin(obj, I915_GEM_DOMAIN_GTT));
-
-	obj->base.write_domain = 0;
-}
-
-/** Flushes the CPU write domain for the object if it's dirty. */
-static void
-i915_gem_object_flush_cpu_write_domain(struct drm_i915_gem_object *obj)
-{
-	if (obj->base.write_domain != I915_GEM_DOMAIN_CPU)
-		return;
-
-	i915_gem_clflush_object(obj, I915_CLFLUSH_SYNC);
-	obj->base.write_domain = 0;
-}
-
 static void __i915_gem_object_flush_for_display(struct drm_i915_gem_object *obj)
 {
 	if (obj->base.write_domain != I915_GEM_DOMAIN_CPU && !obj->cache_dirty)
@@ -3390,6 +3394,69 @@ void i915_gem_object_flush_if_display(struct drm_i915_gem_object *obj)
 }
 
 /**
+ * Moves a single object to the WC read, and possibly write domain.
+ * @obj: object to act on
+ * @write: ask for write access or read only
+ *
+ * This function returns when the move is complete, including waiting on
+ * flushes to occur.
+ */
+int
+i915_gem_object_set_to_wc_domain(struct drm_i915_gem_object *obj, bool write)
+{
+	int ret;
+
+	lockdep_assert_held(&obj->base.dev->struct_mutex);
+
+	ret = i915_gem_object_wait(obj,
+				   I915_WAIT_INTERRUPTIBLE |
+				   I915_WAIT_LOCKED |
+				   (write ? I915_WAIT_ALL : 0),
+				   MAX_SCHEDULE_TIMEOUT,
+				   NULL);
+	if (ret)
+		return ret;
+
+	if (obj->base.write_domain == I915_GEM_DOMAIN_WC)
+		return 0;
+
+	/* Flush and acquire obj->pages so that we are coherent through
+	 * direct access in memory with previous cached writes through
+	 * shmemfs and that our cache domain tracking remains valid.
+	 * For example, if the obj->filp was moved to swap without us
+	 * being notified and releasing the pages, we would mistakenly
+	 * continue to assume that the obj remained out of the CPU cached
+	 * domain.
+	 */
+	ret = i915_gem_object_pin_pages(obj);
+	if (ret)
+		return ret;
+
+	flush_write_domain(obj, ~I915_GEM_DOMAIN_WC);
+
+	/* Serialise direct access to this object with the barriers for
+	 * coherent writes from the GPU, by effectively invalidating the
+	 * WC domain upon first access.
+	 */
+	if ((obj->base.read_domains & I915_GEM_DOMAIN_WC) == 0)
+		mb();
+
+	/* It should now be out of any other write domains, and we can update
+	 * the domain values for our changes.
+	 */
+	GEM_BUG_ON((obj->base.write_domain & ~I915_GEM_DOMAIN_WC) != 0);
+	obj->base.read_domains |= I915_GEM_DOMAIN_WC;
+	if (write) {
+		obj->base.read_domains = I915_GEM_DOMAIN_WC;
+		obj->base.write_domain = I915_GEM_DOMAIN_WC;
+		obj->mm.dirty = true;
+	}
+
+	i915_gem_object_unpin_pages(obj);
+	return 0;
+}
+
+/**
  * Moves a single object to the GTT read, and possibly write domain.
  * @obj: object to act on
  * @write: ask for write access or read only
@@ -3428,7 +3495,7 @@ i915_gem_object_set_to_gtt_domain(struct drm_i915_gem_object *obj, bool write)
 	if (ret)
 		return ret;
 
-	i915_gem_object_flush_cpu_write_domain(obj);
+	flush_write_domain(obj, ~I915_GEM_DOMAIN_GTT);
 
 	/* Serialise direct access to this object with the barriers for
 	 * coherent writes from the GPU, by effectively invalidating the
@@ -3802,7 +3869,7 @@ i915_gem_object_set_to_cpu_domain(struct drm_i915_gem_object *obj, bool write)
 	if (obj->base.write_domain == I915_GEM_DOMAIN_CPU)
 		return 0;
 
-	i915_gem_object_flush_gtt_write_domain(obj);
+	flush_write_domain(obj, ~I915_GEM_DOMAIN_CPU);
 
 	/* Flush the CPU cache if it's still invalid. */
 	if ((obj->base.read_domains & I915_GEM_DOMAIN_CPU) == 0) {
@@ -3996,7 +4063,7 @@ __busy_set_if_active(const struct dma_fence *fence,
 	if (i915_gem_request_completed(rq))
 		return 0;
 
-	return flag(rq->engine->exec_id);
+	return flag(rq->engine->uabi_id);
 }
 
 static __always_inline unsigned int
@@ -4195,7 +4262,7 @@ i915_gem_object_create(struct drm_i915_private *dev_priv, u64 size)
 	 * catch if we ever need to fix it. In the meantime, if you do spot
 	 * such a local variable, please consider fixing!
 	 */
-	if (WARN_ON(size >> PAGE_SHIFT > INT_MAX))
+	if (size >> PAGE_SHIFT > INT_MAX)
 		return ERR_PTR(-E2BIG);
 
 	if (overflows_type(size, obj->base.size))
@@ -4302,6 +4369,8 @@ static void __i915_gem_free_objects(struct drm_i915_private *i915,
 	intel_runtime_pm_put(i915);
 	mutex_unlock(&i915->drm.struct_mutex);
 
+	cond_resched();
+
 	llist_for_each_entry_safe(obj, on, freed, freed) {
 		GEM_BUG_ON(obj->bind_count);
 		GEM_BUG_ON(atomic_read(&obj->frontbuffer_bits));
@@ -4349,8 +4418,11 @@ static void __i915_gem_free_work(struct work_struct *work)
 	 * unbound now.
 	 */
 
-	while ((freed = llist_del_all(&i915->mm.free_list)))
+	while ((freed = llist_del_all(&i915->mm.free_list))) {
 		__i915_gem_free_objects(i915, freed);
+		if (need_resched())
+			break;
+	}
 }
 
 static void __i915_gem_free_object_rcu(struct rcu_head *head)
@@ -4415,10 +4487,9 @@ void i915_gem_sanitize(struct drm_i915_private *i915)
 	 * try to take over. The only way to remove the earlier state
 	 * is by resetting. However, resetting on earlier gen is tricky as
 	 * it may impact the display and we are uncertain about the stability
-	 * of the reset, so we only reset recent machines with logical
-	 * context support (that must be reset to remove any stray contexts).
+	 * of the reset, so this could be applied to even earlier gen.
 	 */
-	if (HAS_HW_CONTEXTS(i915)) {
+	if (INTEL_GEN(i915) >= 5) {
 		int reset = intel_gpu_reset(i915, ALL_ENGINES);
 		WARN_ON(reset && reset != -ENODEV);
 	}
@@ -4661,11 +4732,9 @@ bool intel_sanitize_semaphores(struct drm_i915_private *dev_priv, int value)
 	if (value >= 0)
 		return value;
 
-#ifdef CONFIG_INTEL_IOMMU
 	/* Enable semaphores on SNB when IO remapping is off */
-	if (INTEL_INFO(dev_priv)->gen == 6 && intel_iommu_gfx_mapped)
+	if (IS_GEN6(dev_priv) && intel_vtd_active())
 		return false;
-#endif
 
 	return true;
 }
@@ -4676,7 +4745,7 @@ int i915_gem_init(struct drm_i915_private *dev_priv)
 
 	mutex_lock(&dev_priv->drm.struct_mutex);
 
-	i915_gem_clflush_init(dev_priv);
+	dev_priv->mm.unordered_timeline = dma_fence_context_alloc(1);
 
 	if (!i915.enable_execlists) {
 		dev_priv->gt.resume = intel_legacy_submission_resume;
@@ -4799,12 +4868,16 @@ i915_gem_load_init(struct drm_i915_private *dev_priv)
 	if (!dev_priv->dependencies)
 		goto err_requests;
 
+	dev_priv->priorities = KMEM_CACHE(i915_priolist, SLAB_HWCACHE_ALIGN);
+	if (!dev_priv->priorities)
+		goto err_dependencies;
+
 	mutex_lock(&dev_priv->drm.struct_mutex);
 	INIT_LIST_HEAD(&dev_priv->gt.timelines);
 	err = i915_gem_timeline_init__global(dev_priv);
 	mutex_unlock(&dev_priv->drm.struct_mutex);
 	if (err)
-		goto err_dependencies;
+		goto err_priorities;
 
 	INIT_LIST_HEAD(&dev_priv->context_list);
 	INIT_WORK(&dev_priv->mm.free_work, __i915_gem_free_work);
@@ -4822,14 +4895,14 @@ i915_gem_load_init(struct drm_i915_private *dev_priv)
 
 	init_waitqueue_head(&dev_priv->pending_flip_queue);
 
-	dev_priv->mm.interruptible = true;
-
 	atomic_set(&dev_priv->mm.bsd_engine_dispatch_index, 0);
 
 	spin_lock_init(&dev_priv->fb_tracking.lock);
 
 	return 0;
 
+err_priorities:
+	kmem_cache_destroy(dev_priv->priorities);
 err_dependencies:
 	kmem_cache_destroy(dev_priv->dependencies);
 err_requests:
@@ -4853,6 +4926,7 @@ void i915_gem_load_cleanup(struct drm_i915_private *dev_priv)
 	WARN_ON(!list_empty(&dev_priv->gt.timelines));
 	mutex_unlock(&dev_priv->drm.struct_mutex);
 
+	kmem_cache_destroy(dev_priv->priorities);
 	kmem_cache_destroy(dev_priv->dependencies);
 	kmem_cache_destroy(dev_priv->requests);
 	kmem_cache_destroy(dev_priv->vmas);
@@ -4864,9 +4938,10 @@ void i915_gem_load_cleanup(struct drm_i915_private *dev_priv)
 
 int i915_gem_freeze(struct drm_i915_private *dev_priv)
 {
-	mutex_lock(&dev_priv->drm.struct_mutex);
+	/* Discard all purgeable objects, let userspace recover those as
+	 * required after resuming.
+	 */
 	i915_gem_shrink_all(dev_priv);
-	mutex_unlock(&dev_priv->drm.struct_mutex);
 
 	return 0;
 }
@@ -4891,12 +4966,13 @@ int i915_gem_freeze_late(struct drm_i915_private *dev_priv)
 	 * we update that state just before writing out the image.
 	 *
 	 * To try and reduce the hibernation image, we manually shrink
-	 * the objects as well.
+	 * the objects as well, see i915_gem_freeze()
 	 */
 
-	mutex_lock(&dev_priv->drm.struct_mutex);
 	i915_gem_shrink(dev_priv, -1UL, I915_SHRINK_UNBOUND);
+	i915_gem_drain_freed_objects(dev_priv);
 
+	mutex_lock(&dev_priv->drm.struct_mutex);
 	for (p = phases; *p; p++) {
 		list_for_each_entry(obj, *p, global_link) {
 			obj->base.read_domains = I915_GEM_DOMAIN_CPU;
diff --git a/drivers/gpu/drm/i915/i915_gem.h b/drivers/gpu/drm/i915/i915_gem.h
index 5a49487368ca..ee54597465b6 100644
--- a/drivers/gpu/drm/i915/i915_gem.h
+++ b/drivers/gpu/drm/i915/i915_gem.h
@@ -25,6 +25,8 @@
 #ifndef __I915_GEM_H__
 #define __I915_GEM_H__
 
+#include <linux/bug.h>
+
 #ifdef CONFIG_DRM_I915_DEBUG_GEM
 #define GEM_BUG_ON(expr) BUG_ON(expr)
 #define GEM_WARN_ON(expr) WARN_ON(expr)
diff --git a/drivers/gpu/drm/i915/i915_gem_clflush.c b/drivers/gpu/drm/i915/i915_gem_clflush.c
index ffd01e02fe94..ffac7a1f0caf 100644
--- a/drivers/gpu/drm/i915/i915_gem_clflush.c
+++ b/drivers/gpu/drm/i915/i915_gem_clflush.c
@@ -27,7 +27,6 @@
 #include "i915_gem_clflush.h"
 
 static DEFINE_SPINLOCK(clflush_lock);
-static u64 clflush_context;
 
 struct clflush {
 	struct dma_fence dma; /* Must be first for dma_fence_free() */
@@ -157,7 +156,7 @@ void i915_gem_clflush_object(struct drm_i915_gem_object *obj,
 		dma_fence_init(&clflush->dma,
 			       &i915_clflush_ops,
 			       &clflush_lock,
-			       clflush_context,
+			       to_i915(obj->base.dev)->mm.unordered_timeline,
 			       0);
 		i915_sw_fence_init(&clflush->wait, i915_clflush_notify);
 
@@ -182,8 +181,3 @@ void i915_gem_clflush_object(struct drm_i915_gem_object *obj,
 		GEM_BUG_ON(obj->base.write_domain != I915_GEM_DOMAIN_CPU);
 	}
 }
-
-void i915_gem_clflush_init(struct drm_i915_private *i915)
-{
-	clflush_context = dma_fence_context_alloc(1);
-}
diff --git a/drivers/gpu/drm/i915/i915_gem_clflush.h b/drivers/gpu/drm/i915/i915_gem_clflush.h
index b62d61a2d15f..2455a7820937 100644
--- a/drivers/gpu/drm/i915/i915_gem_clflush.h
+++ b/drivers/gpu/drm/i915/i915_gem_clflush.h
@@ -28,7 +28,6 @@
 struct drm_i915_private;
 struct drm_i915_gem_object;
 
-void i915_gem_clflush_init(struct drm_i915_private *i915);
 void i915_gem_clflush_object(struct drm_i915_gem_object *obj,
 			     unsigned int flags);
 #define I915_CLFLUSH_FORCE BIT(0)
diff --git a/drivers/gpu/drm/i915/i915_gem_context.c b/drivers/gpu/drm/i915/i915_gem_context.c
index 8bd0c4966913..c5d1666d7071 100644
--- a/drivers/gpu/drm/i915/i915_gem_context.c
+++ b/drivers/gpu/drm/i915/i915_gem_context.c
@@ -92,33 +92,6 @@
 
 #define ALL_L3_SLICES(dev) (1 << NUM_L3_SLICES(dev)) - 1
 
-static int get_context_size(struct drm_i915_private *dev_priv)
-{
-	int ret;
-	u32 reg;
-
-	switch (INTEL_GEN(dev_priv)) {
-	case 6:
-		reg = I915_READ(CXT_SIZE);
-		ret = GEN6_CXT_TOTAL_SIZE(reg) * 64;
-		break;
-	case 7:
-		reg = I915_READ(GEN7_CXT_SIZE);
-		if (IS_HASWELL(dev_priv))
-			ret = HSW_CXT_TOTAL_SIZE;
-		else
-			ret = GEN7_CXT_TOTAL_SIZE(reg) * 64;
-		break;
-	case 8:
-		ret = GEN8_CXT_TOTAL_SIZE;
-		break;
-	default:
-		BUG();
-	}
-
-	return ret;
-}
-
 void i915_gem_context_free(struct kref *ctx_ref)
 {
 	struct i915_gem_context *ctx = container_of(ctx_ref, typeof(*ctx), ref);
@@ -151,45 +124,6 @@ void i915_gem_context_free(struct kref *ctx_ref)
 	kfree(ctx);
 }
 
-static struct drm_i915_gem_object *
-alloc_context_obj(struct drm_i915_private *dev_priv, u64 size)
-{
-	struct drm_i915_gem_object *obj;
-	int ret;
-
-	lockdep_assert_held(&dev_priv->drm.struct_mutex);
-
-	obj = i915_gem_object_create(dev_priv, size);
-	if (IS_ERR(obj))
-		return obj;
-
-	/*
-	 * Try to make the context utilize L3 as well as LLC.
-	 *
-	 * On VLV we don't have L3 controls in the PTEs so we
-	 * shouldn't touch the cache level, especially as that
-	 * would make the object snooped which might have a
-	 * negative performance impact.
-	 *
-	 * Snooping is required on non-llc platforms in execlist
-	 * mode, but since all GGTT accesses use PAT entry 0 we
-	 * get snooping anyway regardless of cache_level.
-	 *
-	 * This is only applicable for Ivy Bridge devices since
-	 * later platforms don't have L3 control bits in the PTE.
-	 */
-	if (IS_IVYBRIDGE(dev_priv)) {
-		ret = i915_gem_object_set_cache_level(obj, I915_CACHE_L3_LLC);
-		/* Failure shouldn't ever happen this early */
-		if (WARN_ON(ret)) {
-			i915_gem_object_put(obj);
-			return ERR_PTR(ret);
-		}
-	}
-
-	return obj;
-}
-
 static void context_close(struct i915_gem_context *ctx)
 {
 	i915_gem_context_set_closed(ctx);
@@ -265,26 +199,7 @@ __create_hw_context(struct drm_i915_private *dev_priv,
 	kref_init(&ctx->ref);
 	list_add_tail(&ctx->link, &dev_priv->context_list);
 	ctx->i915 = dev_priv;
-
-	if (dev_priv->hw_context_size) {
-		struct drm_i915_gem_object *obj;
-		struct i915_vma *vma;
-
-		obj = alloc_context_obj(dev_priv, dev_priv->hw_context_size);
-		if (IS_ERR(obj)) {
-			ret = PTR_ERR(obj);
-			goto err_out;
-		}
-
-		vma = i915_vma_instance(obj, &dev_priv->ggtt.base, NULL);
-		if (IS_ERR(vma)) {
-			i915_gem_object_put(obj);
-			ret = PTR_ERR(vma);
-			goto err_out;
-		}
-
-		ctx->engine[RCS].state = vma;
-	}
+	ctx->priority = I915_PRIORITY_NORMAL;
 
 	/* Default context will never have a file_priv */
 	ret = DEFAULT_CONTEXT_HANDLE;
@@ -443,21 +358,6 @@ int i915_gem_context_init(struct drm_i915_private *dev_priv)
 	BUILD_BUG_ON(MAX_CONTEXT_HW_ID > INT_MAX);
 	ida_init(&dev_priv->context_hw_ida);
 
-	if (i915.enable_execlists) {
-		/* NB: intentionally left blank. We will allocate our own
-		 * backing objects as we need them, thank you very much */
-		dev_priv->hw_context_size = 0;
-	} else if (HAS_HW_CONTEXTS(dev_priv)) {
-		dev_priv->hw_context_size =
-			round_up(get_context_size(dev_priv),
-				 I915_GTT_PAGE_SIZE);
-		if (dev_priv->hw_context_size > (1<<20)) {
-			DRM_DEBUG_DRIVER("Disabling HW Contexts; invalid size %d\n",
-					 dev_priv->hw_context_size);
-			dev_priv->hw_context_size = 0;
-		}
-	}
-
 	ctx = i915_gem_create_context(dev_priv, NULL);
 	if (IS_ERR(ctx)) {
 		DRM_ERROR("Failed to create default global context (error %ld)\n",
@@ -477,8 +377,8 @@ int i915_gem_context_init(struct drm_i915_private *dev_priv)
 	GEM_BUG_ON(!i915_gem_context_is_kernel(ctx));
 
 	DRM_DEBUG_DRIVER("%s context support initialized\n",
-			i915.enable_execlists ? "LR" :
-			dev_priv->hw_context_size ? "HW" : "fake");
+			 dev_priv->engine[RCS]->context_size ? "logical" :
+			 "fake");
 	return 0;
 }
 
@@ -941,11 +841,6 @@ int i915_gem_switch_to_kernel_context(struct drm_i915_private *dev_priv)
 	return 0;
 }
 
-static bool contexts_enabled(struct drm_device *dev)
-{
-	return i915.enable_execlists || to_i915(dev)->hw_context_size;
-}
-
 static bool client_is_banned(struct drm_i915_file_private *file_priv)
 {
 	return file_priv->context_bans > I915_MAX_CLIENT_CONTEXT_BANS;
@@ -954,12 +849,13 @@ static bool client_is_banned(struct drm_i915_file_private *file_priv)
 int i915_gem_context_create_ioctl(struct drm_device *dev, void *data,
 				  struct drm_file *file)
 {
+	struct drm_i915_private *dev_priv = to_i915(dev);
 	struct drm_i915_gem_context_create *args = data;
 	struct drm_i915_file_private *file_priv = file->driver_priv;
 	struct i915_gem_context *ctx;
 	int ret;
 
-	if (!contexts_enabled(dev))
+	if (!dev_priv->engine[RCS]->context_size)
 		return -ENODEV;
 
 	if (args->pad != 0)
@@ -977,7 +873,7 @@ int i915_gem_context_create_ioctl(struct drm_device *dev, void *data,
 	if (ret)
 		return ret;
 
-	ctx = i915_gem_create_context(to_i915(dev), file_priv);
+	ctx = i915_gem_create_context(dev_priv, file_priv);
 	mutex_unlock(&dev->struct_mutex);
 	if (IS_ERR(ctx))
 		return PTR_ERR(ctx);
diff --git a/drivers/gpu/drm/i915/i915_gem_dmabuf.c b/drivers/gpu/drm/i915/i915_gem_dmabuf.c
index f225bf680b6d..6176e589cf09 100644
--- a/drivers/gpu/drm/i915/i915_gem_dmabuf.c
+++ b/drivers/gpu/drm/i915/i915_gem_dmabuf.c
@@ -122,12 +122,36 @@ static void i915_gem_dmabuf_kunmap_atomic(struct dma_buf *dma_buf, unsigned long
 }
 static void *i915_gem_dmabuf_kmap(struct dma_buf *dma_buf, unsigned long page_num)
 {
+	struct drm_i915_gem_object *obj = dma_buf_to_obj(dma_buf);
+	struct page *page;
+
+	if (page_num >= obj->base.size >> PAGE_SHIFT)
+		return NULL;
+
+	if (!i915_gem_object_has_struct_page(obj))
+		return NULL;
+
+	if (i915_gem_object_pin_pages(obj))
+		return NULL;
+
+	/* Synchronisation is left to the caller (via .begin_cpu_access()) */
+	page = i915_gem_object_get_page(obj, page_num);
+	if (IS_ERR(page))
+		goto err_unpin;
+
+	return kmap(page);
+
+err_unpin:
+	i915_gem_object_unpin_pages(obj);
 	return NULL;
 }
 
 static void i915_gem_dmabuf_kunmap(struct dma_buf *dma_buf, unsigned long page_num, void *addr)
 {
+	struct drm_i915_gem_object *obj = dma_buf_to_obj(dma_buf);
 
+	kunmap(virt_to_page(addr));
+	i915_gem_object_unpin_pages(obj);
 }
 
 static int i915_gem_dmabuf_mmap(struct dma_buf *dma_buf, struct vm_area_struct *vma)
diff --git a/drivers/gpu/drm/i915/i915_gem_execbuffer.c b/drivers/gpu/drm/i915/i915_gem_execbuffer.c
index 4ee2dc38b7c9..04211c970b9f 100644
--- a/drivers/gpu/drm/i915/i915_gem_execbuffer.c
+++ b/drivers/gpu/drm/i915/i915_gem_execbuffer.c
@@ -1114,6 +1114,18 @@ i915_gem_execbuffer_move_to_gpu(struct drm_i915_gem_request *req,
 	list_for_each_entry(vma, vmas, exec_list) {
 		struct drm_i915_gem_object *obj = vma->obj;
 
+		if (vma->exec_entry->flags & EXEC_OBJECT_CAPTURE) {
+			struct i915_gem_capture_list *capture;
+
+			capture = kmalloc(sizeof(*capture), GFP_KERNEL);
+			if (unlikely(!capture))
+				return -ENOMEM;
+
+			capture->next = req->capture_list;
+			capture->vma = vma;
+			req->capture_list = capture;
+		}
+
 		if (vma->exec_entry->flags & EXEC_OBJECT_ASYNC)
 			continue;
 
diff --git a/drivers/gpu/drm/i915/i915_gem_gtt.c b/drivers/gpu/drm/i915/i915_gem_gtt.c
index 7e3193aa7da1..0c1008a2bbda 100644
--- a/drivers/gpu/drm/i915/i915_gem_gtt.c
+++ b/drivers/gpu/drm/i915/i915_gem_gtt.c
@@ -168,13 +168,11 @@ int intel_sanitize_enable_ppgtt(struct drm_i915_private *dev_priv,
 	if (enable_ppgtt == 3 && has_full_48bit_ppgtt)
 		return 3;
 
-#ifdef CONFIG_INTEL_IOMMU
 	/* Disable ppgtt on SNB if VT-d is on. */
-	if (IS_GEN6(dev_priv) && intel_iommu_gfx_mapped) {
+	if (IS_GEN6(dev_priv) && intel_vtd_active()) {
 		DRM_INFO("Disabling PPGTT because VT-d is on\n");
 		return 0;
 	}
-#endif
 
 	/* Early VLV doesn't have this */
 	if (IS_VALLEYVIEW(dev_priv) && dev_priv->drm.pdev->revision < 0xb) {
@@ -195,9 +193,12 @@ static int ppgtt_bind_vma(struct i915_vma *vma,
 	u32 pte_flags;
 	int ret;
 
-	ret = vma->vm->allocate_va_range(vma->vm, vma->node.start, vma->size);
-	if (ret)
-		return ret;
+	if (!(vma->flags & I915_VMA_LOCAL_BIND)) {
+		ret = vma->vm->allocate_va_range(vma->vm, vma->node.start,
+						 vma->size);
+		if (ret)
+			return ret;
+	}
 
 	vma->pages = vma->obj->mm.pages;
 
@@ -1989,14 +1990,10 @@ void i915_ppgtt_release(struct kref *kref)
  */
 static bool needs_idle_maps(struct drm_i915_private *dev_priv)
 {
-#ifdef CONFIG_INTEL_IOMMU
 	/* Query intel_iommu to see if we need the workaround. Presumably that
 	 * was loaded first.
 	 */
-	if (IS_GEN5(dev_priv) && IS_MOBILE(dev_priv) && intel_iommu_gfx_mapped)
-		return true;
-#endif
-	return false;
+	return IS_GEN5(dev_priv) && IS_MOBILE(dev_priv) && intel_vtd_active();
 }
 
 void i915_check_and_clear_faults(struct drm_i915_private *dev_priv)
@@ -2188,6 +2185,101 @@ static void gen8_ggtt_clear_range(struct i915_address_space *vm,
 		gen8_set_pte(&gtt_base[i], scratch_pte);
 }
 
+static void bxt_vtd_ggtt_wa(struct i915_address_space *vm)
+{
+	struct drm_i915_private *dev_priv = vm->i915;
+
+	/*
+	 * Make sure the internal GAM fifo has been cleared of all GTT
+	 * writes before exiting stop_machine(). This guarantees that
+	 * any aperture accesses waiting to start in another process
+	 * cannot back up behind the GTT writes causing a hang.
+	 * The register can be any arbitrary GAM register.
+	 */
+	POSTING_READ(GFX_FLSH_CNTL_GEN6);
+}
+
+struct insert_page {
+	struct i915_address_space *vm;
+	dma_addr_t addr;
+	u64 offset;
+	enum i915_cache_level level;
+};
+
+static int bxt_vtd_ggtt_insert_page__cb(void *_arg)
+{
+	struct insert_page *arg = _arg;
+
+	gen8_ggtt_insert_page(arg->vm, arg->addr, arg->offset, arg->level, 0);
+	bxt_vtd_ggtt_wa(arg->vm);
+
+	return 0;
+}
+
+static void bxt_vtd_ggtt_insert_page__BKL(struct i915_address_space *vm,
+					  dma_addr_t addr,
+					  u64 offset,
+					  enum i915_cache_level level,
+					  u32 unused)
+{
+	struct insert_page arg = { vm, addr, offset, level };
+
+	stop_machine(bxt_vtd_ggtt_insert_page__cb, &arg, NULL);
+}
+
+struct insert_entries {
+	struct i915_address_space *vm;
+	struct sg_table *st;
+	u64 start;
+	enum i915_cache_level level;
+};
+
+static int bxt_vtd_ggtt_insert_entries__cb(void *_arg)
+{
+	struct insert_entries *arg = _arg;
+
+	gen8_ggtt_insert_entries(arg->vm, arg->st, arg->start, arg->level, 0);
+	bxt_vtd_ggtt_wa(arg->vm);
+
+	return 0;
+}
+
+static void bxt_vtd_ggtt_insert_entries__BKL(struct i915_address_space *vm,
+					     struct sg_table *st,
+					     u64 start,
+					     enum i915_cache_level level,
+					     u32 unused)
+{
+	struct insert_entries arg = { vm, st, start, level };
+
+	stop_machine(bxt_vtd_ggtt_insert_entries__cb, &arg, NULL);
+}
+
+struct clear_range {
+	struct i915_address_space *vm;
+	u64 start;
+	u64 length;
+};
+
+static int bxt_vtd_ggtt_clear_range__cb(void *_arg)
+{
+	struct clear_range *arg = _arg;
+
+	gen8_ggtt_clear_range(arg->vm, arg->start, arg->length);
+	bxt_vtd_ggtt_wa(arg->vm);
+
+	return 0;
+}
+
+static void bxt_vtd_ggtt_clear_range__BKL(struct i915_address_space *vm,
+					  u64 start,
+					  u64 length)
+{
+	struct clear_range arg = { vm, start, length };
+
+	stop_machine(bxt_vtd_ggtt_clear_range__cb, &arg, NULL);
+}
+
 static void gen6_ggtt_clear_range(struct i915_address_space *vm,
 				  u64 start, u64 length)
 {
@@ -2306,10 +2398,11 @@ static int aliasing_gtt_bind_vma(struct i915_vma *vma,
 	if (flags & I915_VMA_LOCAL_BIND) {
 		struct i915_hw_ppgtt *appgtt = i915->mm.aliasing_ppgtt;
 
-		if (appgtt->base.allocate_va_range) {
+		if (!(vma->flags & I915_VMA_LOCAL_BIND) &&
+		    appgtt->base.allocate_va_range) {
 			ret = appgtt->base.allocate_va_range(&appgtt->base,
 							     vma->node.start,
-							     vma->node.size);
+							     vma->size);
 			if (ret)
 				goto err_pages;
 		}
@@ -2579,14 +2672,14 @@ static size_t gen6_get_stolen_size(u16 snb_gmch_ctl)
 {
 	snb_gmch_ctl >>= SNB_GMCH_GMS_SHIFT;
 	snb_gmch_ctl &= SNB_GMCH_GMS_MASK;
-	return snb_gmch_ctl << 25; /* 32 MB units */
+	return (size_t)snb_gmch_ctl << 25; /* 32 MB units */
 }
 
 static size_t gen8_get_stolen_size(u16 bdw_gmch_ctl)
 {
 	bdw_gmch_ctl >>= BDW_GMCH_GMS_SHIFT;
 	bdw_gmch_ctl &= BDW_GMCH_GMS_MASK;
-	return bdw_gmch_ctl << 25; /* 32 MB units */
+	return (size_t)bdw_gmch_ctl << 25; /* 32 MB units */
 }
 
 static size_t chv_get_stolen_size(u16 gmch_ctrl)
@@ -2600,11 +2693,11 @@ static size_t chv_get_stolen_size(u16 gmch_ctrl)
 	 * 0x17 to 0x1d: 4MB increments start at 36MB
 	 */
 	if (gmch_ctrl < 0x11)
-		return gmch_ctrl << 25;
+		return (size_t)gmch_ctrl << 25;
 	else if (gmch_ctrl < 0x17)
-		return (gmch_ctrl - 0x11 + 2) << 22;
+		return (size_t)(gmch_ctrl - 0x11 + 2) << 22;
 	else
-		return (gmch_ctrl - 0x17 + 9) << 22;
+		return (size_t)(gmch_ctrl - 0x17 + 9) << 22;
 }
 
 static size_t gen9_get_stolen_size(u16 gen9_gmch_ctl)
@@ -2613,10 +2706,10 @@ static size_t gen9_get_stolen_size(u16 gen9_gmch_ctl)
 	gen9_gmch_ctl &= BDW_GMCH_GMS_MASK;
 
 	if (gen9_gmch_ctl < 0xf0)
-		return gen9_gmch_ctl << 25; /* 32 MB units */
+		return (size_t)gen9_gmch_ctl << 25; /* 32 MB units */
 	else
 		/* 4MB increments starting at 0xf0 for 4MB */
-		return (gen9_gmch_ctl - 0xf0 + 1) << 22;
+		return (size_t)(gen9_gmch_ctl - 0xf0 + 1) << 22;
 }
 
 static int ggtt_probe_common(struct i915_ggtt *ggtt, u64 size)
@@ -2743,13 +2836,17 @@ static int gen8_gmch_probe(struct i915_ggtt *ggtt)
 	struct pci_dev *pdev = dev_priv->drm.pdev;
 	unsigned int size;
 	u16 snb_gmch_ctl;
+	int err;
 
 	/* TODO: We're not aware of mappable constraints on gen8 yet */
 	ggtt->mappable_base = pci_resource_start(pdev, 2);
 	ggtt->mappable_end = pci_resource_len(pdev, 2);
 
-	if (!pci_set_dma_mask(pdev, DMA_BIT_MASK(39)))
-		pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(39));
+	err = pci_set_dma_mask(pdev, DMA_BIT_MASK(39));
+	if (!err)
+		err = pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(39));
+	if (err)
+		DRM_ERROR("Can't set DMA mask/consistent mask (%d)\n", err);
 
 	pci_read_config_word(pdev, SNB_GMCH_CTRL, &snb_gmch_ctl);
 
@@ -2781,6 +2878,14 @@ static int gen8_gmch_probe(struct i915_ggtt *ggtt)
 
 	ggtt->base.insert_entries = gen8_ggtt_insert_entries;
 
+	/* Serialize GTT updates with aperture access on BXT if VT-d is on. */
+	if (intel_ggtt_update_needs_vtd_wa(dev_priv)) {
+		ggtt->base.insert_entries = bxt_vtd_ggtt_insert_entries__BKL;
+		ggtt->base.insert_page    = bxt_vtd_ggtt_insert_page__BKL;
+		if (ggtt->base.clear_range != nop_clear_range)
+			ggtt->base.clear_range = bxt_vtd_ggtt_clear_range__BKL;
+	}
+
 	ggtt->invalidate = gen6_ggtt_invalidate;
 
 	return ggtt_probe_common(ggtt, size);
@@ -2792,6 +2897,7 @@ static int gen6_gmch_probe(struct i915_ggtt *ggtt)
 	struct pci_dev *pdev = dev_priv->drm.pdev;
 	unsigned int size;
 	u16 snb_gmch_ctl;
+	int err;
 
 	ggtt->mappable_base = pci_resource_start(pdev, 2);
 	ggtt->mappable_end = pci_resource_len(pdev, 2);
@@ -2804,8 +2910,11 @@ static int gen6_gmch_probe(struct i915_ggtt *ggtt)
 		return -ENXIO;
 	}
 
-	if (!pci_set_dma_mask(pdev, DMA_BIT_MASK(40)))
-		pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(40));
+	err = pci_set_dma_mask(pdev, DMA_BIT_MASK(40));
+	if (!err)
+		err = pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(40));
+	if (err)
+		DRM_ERROR("Can't set DMA mask/consistent mask (%d)\n", err);
 	pci_read_config_word(pdev, SNB_GMCH_CTRL, &snb_gmch_ctl);
 
 	ggtt->stolen_size = gen6_get_stolen_size(snb_gmch_ctl);
@@ -2924,10 +3033,8 @@ int i915_ggtt_probe_hw(struct drm_i915_private *dev_priv)
 		 ggtt->base.total >> 20);
 	DRM_DEBUG_DRIVER("GMADR size = %lldM\n", ggtt->mappable_end >> 20);
 	DRM_DEBUG_DRIVER("GTT stolen size = %uM\n", ggtt->stolen_size >> 20);
-#ifdef CONFIG_INTEL_IOMMU
-	if (intel_iommu_gfx_mapped)
+	if (intel_vtd_active())
 		DRM_INFO("VT-d active for gfx access\n");
-#endif
 
 	return 0;
 }
diff --git a/drivers/gpu/drm/i915/i915_gem_object.h b/drivers/gpu/drm/i915/i915_gem_object.h
index 174cf923c236..915057824284 100644
--- a/drivers/gpu/drm/i915/i915_gem_object.h
+++ b/drivers/gpu/drm/i915/i915_gem_object.h
@@ -37,8 +37,8 @@
 
 struct drm_i915_gem_object_ops {
 	unsigned int flags;
-#define I915_GEM_OBJECT_HAS_STRUCT_PAGE 0x1
-#define I915_GEM_OBJECT_IS_SHRINKABLE   0x2
+#define I915_GEM_OBJECT_HAS_STRUCT_PAGE BIT(0)
+#define I915_GEM_OBJECT_IS_SHRINKABLE   BIT(1)
 
 	/* Interface between the GEM object and its backing storage.
 	 * get_pages() is called once prior to the use of the associated set
@@ -68,8 +68,23 @@ struct drm_i915_gem_object {
 
 	const struct drm_i915_gem_object_ops *ops;
 
-	/** List of VMAs backed by this object */
+	/**
+	 * @vma_list: List of VMAs backed by this object
+	 *
+	 * The VMA on this list are ordered by type, all GGTT vma are placed
+	 * at the head and all ppGTT vma are placed at the tail. The different
+	 * types of GGTT vma are unordered between themselves, use the
+	 * @vma_tree (which has a defined order between all VMA) to find an
+	 * exact match.
+	 */
 	struct list_head vma_list;
+	/**
+	 * @vma_tree: Ordered tree of VMAs backed by this object
+	 *
+	 * All VMA created for this object are placed in the @vma_tree for
+	 * fast retrieval via a binary search in i915_vma_instance().
+	 * They are also added to @vma_list for easy iteration.
+	 */
 	struct rb_root vma_tree;
 
 	/** Stolen memory for this object, instead of being backed by shmem. */
diff --git a/drivers/gpu/drm/i915/i915_gem_request.c b/drivers/gpu/drm/i915/i915_gem_request.c
index 5ddbc9499775..0d1e0d8873ef 100644
--- a/drivers/gpu/drm/i915/i915_gem_request.c
+++ b/drivers/gpu/drm/i915/i915_gem_request.c
@@ -61,7 +61,7 @@ static bool i915_fence_enable_signaling(struct dma_fence *fence)
 	if (i915_fence_signaled(fence))
 		return false;
 
-	intel_engine_enable_signaling(to_request(fence));
+	intel_engine_enable_signaling(to_request(fence), true);
 	return true;
 }
 
@@ -159,7 +159,7 @@ i915_priotree_fini(struct drm_i915_private *i915, struct i915_priotree *pt)
 {
 	struct i915_dependency *dep, *next;
 
-	GEM_BUG_ON(!RB_EMPTY_NODE(&pt->node));
+	GEM_BUG_ON(!list_empty(&pt->link));
 
 	/* Everyone we depended upon (the fences we wait to be signaled)
 	 * should retire before us and remove themselves from our list.
@@ -185,7 +185,7 @@ i915_priotree_init(struct i915_priotree *pt)
 {
 	INIT_LIST_HEAD(&pt->signalers_list);
 	INIT_LIST_HEAD(&pt->waiters_list);
-	RB_CLEAR_NODE(&pt->node);
+	INIT_LIST_HEAD(&pt->link);
 	pt->priority = INT_MIN;
 }
 
@@ -214,12 +214,12 @@ static int reset_all_global_seqno(struct drm_i915_private *i915, u32 seqno)
 		}
 
 		/* Finally reset hw state */
-		tl->seqno = seqno;
 		intel_engine_init_global_seqno(engine, seqno);
+		tl->seqno = seqno;
 
 		list_for_each_entry(timeline, &i915->gt.timelines, link)
-			memset(timeline->engine[id].sync_seqno, 0,
-			       sizeof(timeline->engine[id].sync_seqno));
+			memset(timeline->engine[id].global_sync, 0,
+			       sizeof(timeline->engine[id].global_sync));
 	}
 
 	return 0;
@@ -271,6 +271,48 @@ void i915_gem_retire_noop(struct i915_gem_active *active,
 	/* Space left intentionally blank */
 }
 
+static void advance_ring(struct drm_i915_gem_request *request)
+{
+	unsigned int tail;
+
+	/* We know the GPU must have read the request to have
+	 * sent us the seqno + interrupt, so use the position
+	 * of tail of the request to update the last known position
+	 * of the GPU head.
+	 *
+	 * Note this requires that we are always called in request
+	 * completion order.
+	 */
+	if (list_is_last(&request->ring_link, &request->ring->request_list)) {
+		/* We may race here with execlists resubmitting this request
+		 * as we retire it. The resubmission will move the ring->tail
+		 * forwards (to request->wa_tail). We either read the
+		 * current value that was written to hw, or the value that
+		 * is just about to be. Either works, if we miss the last two
+		 * noops - they are safe to be replayed on a reset.
+		 */
+		tail = READ_ONCE(request->ring->tail);
+	} else {
+		tail = request->postfix;
+	}
+	list_del(&request->ring_link);
+
+	request->ring->head = tail;
+}
+
+static void free_capture_list(struct drm_i915_gem_request *request)
+{
+	struct i915_gem_capture_list *capture;
+
+	capture = request->capture_list;
+	while (capture) {
+		struct i915_gem_capture_list *next = capture->next;
+
+		kfree(capture);
+		capture = next;
+	}
+}
+
 static void i915_gem_request_retire(struct drm_i915_gem_request *request)
 {
 	struct intel_engine_cs *engine = request->engine;
@@ -287,16 +329,6 @@ static void i915_gem_request_retire(struct drm_i915_gem_request *request)
 	list_del_init(&request->link);
 	spin_unlock_irq(&engine->timeline->lock);
 
-	/* We know the GPU must have read the request to have
-	 * sent us the seqno + interrupt, so use the position
-	 * of tail of the request to update the last known position
-	 * of the GPU head.
-	 *
-	 * Note this requires that we are always called in request
-	 * completion order.
-	 */
-	list_del(&request->ring_link);
-	request->ring->head = request->postfix;
 	if (!--request->i915->gt.active_requests) {
 		GEM_BUG_ON(!request->i915->gt.awake);
 		mod_delayed_work(request->i915->wq,
@@ -304,6 +336,9 @@ static void i915_gem_request_retire(struct drm_i915_gem_request *request)
 				 msecs_to_jiffies(100));
 	}
 	unreserve_seqno(request->engine);
+	advance_ring(request);
+
+	free_capture_list(request);
 
 	/* Walk through the active list, calling retire on each. This allows
 	 * objects to track their GPU activity and mark themselves as idle
@@ -402,7 +437,7 @@ void __i915_gem_request_submit(struct drm_i915_gem_request *request)
 	spin_lock_nested(&request->lock, SINGLE_DEPTH_NESTING);
 	request->global_seqno = seqno;
 	if (test_bit(DMA_FENCE_FLAG_ENABLE_SIGNAL_BIT, &request->fence.flags))
-		intel_engine_enable_signaling(request);
+		intel_engine_enable_signaling(request, false);
 	spin_unlock(&request->lock);
 
 	engine->emit_breadcrumb(request,
@@ -503,9 +538,6 @@ submit_notify(struct i915_sw_fence *fence, enum i915_sw_fence_notify state)
  *
  * @engine: engine that we wish to issue the request on.
  * @ctx: context that the request will be associated with.
- *       This can be NULL if the request is not directly related to
- *       any specific user context, in which case this function will
- *       choose an appropriate context to use.
  *
  * Returns a pointer to the allocated request if successful,
  * or an error code if not.
@@ -516,6 +548,7 @@ i915_gem_request_alloc(struct intel_engine_cs *engine,
 {
 	struct drm_i915_private *dev_priv = engine->i915;
 	struct drm_i915_gem_request *req;
+	struct intel_ring *ring;
 	int ret;
 
 	lockdep_assert_held(&dev_priv->drm.struct_mutex);
@@ -530,9 +563,10 @@ i915_gem_request_alloc(struct intel_engine_cs *engine,
 	 * GGTT space, so do this first before we reserve a seqno for
 	 * ourselves.
 	 */
-	ret = engine->context_pin(engine, ctx);
-	if (ret)
-		return ERR_PTR(ret);
+	ring = engine->context_pin(engine, ctx);
+	if (IS_ERR(ring))
+		return ERR_CAST(ring);
+	GEM_BUG_ON(!ring);
 
 	ret = reserve_seqno(engine);
 	if (ret)
@@ -598,11 +632,13 @@ i915_gem_request_alloc(struct intel_engine_cs *engine,
 	req->i915 = dev_priv;
 	req->engine = engine;
 	req->ctx = ctx;
+	req->ring = ring;
 
 	/* No zalloc, must clear what we need by hand */
 	req->global_seqno = 0;
 	req->file_priv = NULL;
 	req->batch = NULL;
+	req->capture_list = NULL;
 
 	/*
 	 * Reserve space in the ring buffer for all the commands required to
@@ -623,7 +659,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);
@@ -651,6 +687,7 @@ i915_gem_request_await_request(struct drm_i915_gem_request *to,
 	int ret;
 
 	GEM_BUG_ON(to == from);
+	GEM_BUG_ON(to->timeline == from->timeline);
 
 	if (i915_gem_request_completed(from))
 		return 0;
@@ -663,9 +700,6 @@ i915_gem_request_await_request(struct drm_i915_gem_request *to,
 			return ret;
 	}
 
-	if (to->timeline == from->timeline)
-		return 0;
-
 	if (to->engine == from->engine) {
 		ret = i915_sw_fence_await_sw_fence_gfp(&to->submit,
 						       &from->submit,
@@ -674,55 +708,45 @@ i915_gem_request_await_request(struct drm_i915_gem_request *to,
 	}
 
 	seqno = i915_gem_request_global_seqno(from);
-	if (!seqno) {
-		ret = i915_sw_fence_await_dma_fence(&to->submit,
-						    &from->fence, 0,
-						    GFP_KERNEL);
-		return ret < 0 ? ret : 0;
-	}
+	if (!seqno)
+		goto await_dma_fence;
 
-	if (seqno <= to->timeline->sync_seqno[from->engine->id])
-		return 0;
+	if (!to->engine->semaphore.sync_to) {
+		if (!__i915_gem_request_started(from, seqno))
+			goto await_dma_fence;
 
-	trace_i915_gem_ring_sync_to(to, from);
-	if (!i915.semaphores) {
-		if (!i915_spin_request(from, TASK_INTERRUPTIBLE, 2)) {
-			ret = i915_sw_fence_await_dma_fence(&to->submit,
-							    &from->fence, 0,
-							    GFP_KERNEL);
-			if (ret < 0)
-				return ret;
-		}
+		if (!__i915_spin_request(from, seqno, TASK_INTERRUPTIBLE, 2))
+			goto await_dma_fence;
 	} else {
+		GEM_BUG_ON(!from->engine->semaphore.signal);
+
+		if (seqno <= to->timeline->global_sync[from->engine->id])
+			return 0;
+
+		trace_i915_gem_ring_sync_to(to, from);
 		ret = to->engine->semaphore.sync_to(to, from);
 		if (ret)
 			return ret;
+
+		to->timeline->global_sync[from->engine->id] = seqno;
 	}
 
-	to->timeline->sync_seqno[from->engine->id] = seqno;
 	return 0;
+
+await_dma_fence:
+	ret = i915_sw_fence_await_dma_fence(&to->submit,
+					    &from->fence, 0,
+					    GFP_KERNEL);
+	return ret < 0 ? ret : 0;
 }
 
 int
 i915_gem_request_await_dma_fence(struct drm_i915_gem_request *req,
 				 struct dma_fence *fence)
 {
-	struct dma_fence_array *array;
+	struct dma_fence **child = &fence;
+	unsigned int nchild = 1;
 	int ret;
-	int i;
-
-	if (test_bit(DMA_FENCE_FLAG_SIGNALED_BIT, &fence->flags))
-		return 0;
-
-	if (dma_fence_is_i915(fence))
-		return i915_gem_request_await_request(req, to_request(fence));
-
-	if (!dma_fence_is_array(fence)) {
-		ret = i915_sw_fence_await_dma_fence(&req->submit,
-						    fence, I915_FENCE_TIMEOUT,
-						    GFP_KERNEL);
-		return ret < 0 ? ret : 0;
-	}
 
 	/* Note that if the fence-array was created in signal-on-any mode,
 	 * we should *not* decompose it into its individual fences. However,
@@ -731,21 +755,46 @@ i915_gem_request_await_dma_fence(struct drm_i915_gem_request *req,
 	 * amdgpu and we should not see any incoming fence-array from
 	 * sync-file being in signal-on-any mode.
 	 */
+	if (dma_fence_is_array(fence)) {
+		struct dma_fence_array *array = to_dma_fence_array(fence);
+
+		child = array->fences;
+		nchild = array->num_fences;
+		GEM_BUG_ON(!nchild);
+	}
+
+	do {
+		fence = *child++;
+		if (test_bit(DMA_FENCE_FLAG_SIGNALED_BIT, &fence->flags))
+			continue;
+
+		/*
+		 * Requests on the same timeline are explicitly ordered, along
+		 * with their dependencies, by i915_add_request() which ensures
+		 * that requests are submitted in-order through each ring.
+		 */
+		if (fence->context == req->fence.context)
+			continue;
 
-	array = to_dma_fence_array(fence);
-	for (i = 0; i < array->num_fences; i++) {
-		struct dma_fence *child = array->fences[i];
+		/* Squash repeated waits to the same timelines */
+		if (fence->context != req->i915->mm.unordered_timeline &&
+		    intel_timeline_sync_is_later(req->timeline, fence))
+			continue;
 
-		if (dma_fence_is_i915(child))
+		if (dma_fence_is_i915(fence))
 			ret = i915_gem_request_await_request(req,
-							     to_request(child));
+							     to_request(fence));
 		else
-			ret = i915_sw_fence_await_dma_fence(&req->submit,
-							    child, I915_FENCE_TIMEOUT,
+			ret = i915_sw_fence_await_dma_fence(&req->submit, fence,
+							    I915_FENCE_TIMEOUT,
 							    GFP_KERNEL);
 		if (ret < 0)
 			return ret;
-	}
+
+		/* Record the latest fence used against each timeline */
+		if (fence->context != req->i915->mm.unordered_timeline)
+			intel_timeline_sync_set(req->timeline, fence);
+	} while (--nchild);
 
 	return 0;
 }
diff --git a/drivers/gpu/drm/i915/i915_gem_request.h b/drivers/gpu/drm/i915/i915_gem_request.h
index 129c58bb4805..7b7c84369d78 100644
--- a/drivers/gpu/drm/i915/i915_gem_request.h
+++ b/drivers/gpu/drm/i915/i915_gem_request.h
@@ -67,12 +67,18 @@ struct i915_dependency {
 struct i915_priotree {
 	struct list_head signalers_list; /* those before us, we depend upon */
 	struct list_head waiters_list; /* those after us, they depend upon us */
-	struct rb_node node;
+	struct list_head link;
 	int priority;
 #define I915_PRIORITY_MAX 1024
+#define I915_PRIORITY_NORMAL 0
 #define I915_PRIORITY_MIN (-I915_PRIORITY_MAX)
 };
 
+struct i915_gem_capture_list {
+	struct i915_gem_capture_list *next;
+	struct i915_vma *vma;
+};
+
 /**
  * Request queue structure.
  *
@@ -167,6 +173,12 @@ struct drm_i915_gem_request {
 	 * error state dump only).
 	 */
 	struct i915_vma *batch;
+	/** Additional buffers requested by userspace to be captured upon
+	 * a GPU hang. The vma/obj on this list are protected by their
+	 * active reference - all objects on this list must also be
+	 * on the active_list (of their final request).
+	 */
+	struct i915_gem_capture_list *capture_list;
 	struct list_head active_list;
 
 	/** Time at which this request was emitted, in jiffies. */
diff --git a/drivers/gpu/drm/i915/i915_gem_shrinker.c b/drivers/gpu/drm/i915/i915_gem_shrinker.c
index 129ed303a6c4..0fd2b58ce475 100644
--- a/drivers/gpu/drm/i915/i915_gem_shrinker.c
+++ b/drivers/gpu/drm/i915/i915_gem_shrinker.c
@@ -35,9 +35,9 @@
 #include "i915_drv.h"
 #include "i915_trace.h"
 
-static bool i915_gem_shrinker_lock(struct drm_device *dev, bool *unlock)
+static bool shrinker_lock(struct drm_i915_private *dev_priv, bool *unlock)
 {
-	switch (mutex_trylock_recursive(&dev->struct_mutex)) {
+	switch (mutex_trylock_recursive(&dev_priv->drm.struct_mutex)) {
 	case MUTEX_TRYLOCK_FAILED:
 		return false;
 
@@ -53,24 +53,29 @@ static bool i915_gem_shrinker_lock(struct drm_device *dev, bool *unlock)
 	BUG();
 }
 
-static void i915_gem_shrinker_unlock(struct drm_device *dev, bool unlock)
+static void shrinker_unlock(struct drm_i915_private *dev_priv, bool unlock)
 {
 	if (!unlock)
 		return;
 
-	mutex_unlock(&dev->struct_mutex);
-
-	/* expedite the RCU grace period to free some request slabs */
-	synchronize_rcu_expedited();
+	mutex_unlock(&dev_priv->drm.struct_mutex);
 }
 
 static bool any_vma_pinned(struct drm_i915_gem_object *obj)
 {
 	struct i915_vma *vma;
 
-	list_for_each_entry(vma, &obj->vma_list, obj_link)
+	list_for_each_entry(vma, &obj->vma_list, obj_link) {
+		/* Only GGTT vma may be permanently pinned, and are always
+		 * at the start of the list. We can stop hunting as soon
+		 * as we see a ppGTT vma.
+		 */
+		if (!i915_vma_is_ggtt(vma))
+			break;
+
 		if (i915_vma_is_pinned(vma))
 			return true;
+	}
 
 	return false;
 }
@@ -156,7 +161,7 @@ i915_gem_shrink(struct drm_i915_private *dev_priv,
 	unsigned long count = 0;
 	bool unlock;
 
-	if (!i915_gem_shrinker_lock(&dev_priv->drm, &unlock))
+	if (!shrinker_lock(dev_priv, &unlock))
 		return 0;
 
 	trace_i915_gem_shrink(dev_priv, target, flags);
@@ -244,7 +249,7 @@ i915_gem_shrink(struct drm_i915_private *dev_priv,
 
 	i915_gem_retire_requests(dev_priv);
 
-	i915_gem_shrinker_unlock(&dev_priv->drm, unlock);
+	shrinker_unlock(dev_priv, unlock);
 
 	return count;
 }
@@ -274,8 +279,6 @@ unsigned long i915_gem_shrink_all(struct drm_i915_private *dev_priv)
 				I915_SHRINK_ACTIVE);
 	intel_runtime_pm_put(dev_priv);
 
-	synchronize_rcu(); /* wait for our earlier RCU delayed slab frees */
-
 	return freed;
 }
 
@@ -284,12 +287,11 @@ i915_gem_shrinker_count(struct shrinker *shrinker, struct shrink_control *sc)
 {
 	struct drm_i915_private *dev_priv =
 		container_of(shrinker, struct drm_i915_private, mm.shrinker);
-	struct drm_device *dev = &dev_priv->drm;
 	struct drm_i915_gem_object *obj;
 	unsigned long count;
 	bool unlock;
 
-	if (!i915_gem_shrinker_lock(dev, &unlock))
+	if (!shrinker_lock(dev_priv, &unlock))
 		return 0;
 
 	i915_gem_retire_requests(dev_priv);
@@ -304,7 +306,7 @@ i915_gem_shrinker_count(struct shrinker *shrinker, struct shrink_control *sc)
 			count += obj->base.size >> PAGE_SHIFT;
 	}
 
-	i915_gem_shrinker_unlock(dev, unlock);
+	shrinker_unlock(dev_priv, unlock);
 
 	return count;
 }
@@ -314,11 +316,10 @@ i915_gem_shrinker_scan(struct shrinker *shrinker, struct shrink_control *sc)
 {
 	struct drm_i915_private *dev_priv =
 		container_of(shrinker, struct drm_i915_private, mm.shrinker);
-	struct drm_device *dev = &dev_priv->drm;
 	unsigned long freed;
 	bool unlock;
 
-	if (!i915_gem_shrinker_lock(dev, &unlock))
+	if (!shrinker_lock(dev_priv, &unlock))
 		return SHRINK_STOP;
 
 	freed = i915_gem_shrink(dev_priv,
@@ -332,26 +333,20 @@ i915_gem_shrinker_scan(struct shrinker *shrinker, struct shrink_control *sc)
 					 I915_SHRINK_BOUND |
 					 I915_SHRINK_UNBOUND);
 
-	i915_gem_shrinker_unlock(dev, unlock);
+	shrinker_unlock(dev_priv, unlock);
 
 	return freed;
 }
 
-struct shrinker_lock_uninterruptible {
-	bool was_interruptible;
-	bool unlock;
-};
-
 static bool
-i915_gem_shrinker_lock_uninterruptible(struct drm_i915_private *dev_priv,
-				       struct shrinker_lock_uninterruptible *slu,
-				       int timeout_ms)
+shrinker_lock_uninterruptible(struct drm_i915_private *dev_priv, bool *unlock,
+			      int timeout_ms)
 {
 	unsigned long timeout = jiffies + msecs_to_jiffies_timeout(timeout_ms);
 
 	do {
 		if (i915_gem_wait_for_idle(dev_priv, 0) == 0 &&
-		    i915_gem_shrinker_lock(&dev_priv->drm, &slu->unlock))
+		    shrinker_lock(dev_priv, unlock))
 			break;
 
 		schedule_timeout_killable(1);
@@ -364,29 +359,19 @@ i915_gem_shrinker_lock_uninterruptible(struct drm_i915_private *dev_priv,
 		}
 	} while (1);
 
-	slu->was_interruptible = dev_priv->mm.interruptible;
-	dev_priv->mm.interruptible = false;
 	return true;
 }
 
-static void
-i915_gem_shrinker_unlock_uninterruptible(struct drm_i915_private *dev_priv,
-					 struct shrinker_lock_uninterruptible *slu)
-{
-	dev_priv->mm.interruptible = slu->was_interruptible;
-	i915_gem_shrinker_unlock(&dev_priv->drm, slu->unlock);
-}
-
 static int
 i915_gem_shrinker_oom(struct notifier_block *nb, unsigned long event, void *ptr)
 {
 	struct drm_i915_private *dev_priv =
 		container_of(nb, struct drm_i915_private, mm.oom_notifier);
-	struct shrinker_lock_uninterruptible slu;
 	struct drm_i915_gem_object *obj;
 	unsigned long unevictable, bound, unbound, freed_pages;
+	bool unlock;
 
-	if (!i915_gem_shrinker_lock_uninterruptible(dev_priv, &slu, 5000))
+	if (!shrinker_lock_uninterruptible(dev_priv, &unlock, 5000))
 		return NOTIFY_DONE;
 
 	freed_pages = i915_gem_shrink_all(dev_priv);
@@ -415,7 +400,7 @@ i915_gem_shrinker_oom(struct notifier_block *nb, unsigned long event, void *ptr)
 			bound += obj->base.size >> PAGE_SHIFT;
 	}
 
-	i915_gem_shrinker_unlock_uninterruptible(dev_priv, &slu);
+	shrinker_unlock(dev_priv, unlock);
 
 	if (freed_pages || unbound || bound)
 		pr_info("Purging GPU memory, %lu pages freed, "
@@ -435,12 +420,12 @@ i915_gem_shrinker_vmap(struct notifier_block *nb, unsigned long event, void *ptr
 {
 	struct drm_i915_private *dev_priv =
 		container_of(nb, struct drm_i915_private, mm.vmap_notifier);
-	struct shrinker_lock_uninterruptible slu;
 	struct i915_vma *vma, *next;
 	unsigned long freed_pages = 0;
+	bool unlock;
 	int ret;
 
-	if (!i915_gem_shrinker_lock_uninterruptible(dev_priv, &slu, 5000))
+	if (!shrinker_lock_uninterruptible(dev_priv, &unlock, 5000))
 		return NOTIFY_DONE;
 
 	/* Force everything onto the inactive lists */
@@ -465,7 +450,7 @@ i915_gem_shrinker_vmap(struct notifier_block *nb, unsigned long event, void *ptr
 	}
 
 out:
-	i915_gem_shrinker_unlock_uninterruptible(dev_priv, &slu);
+	shrinker_unlock(dev_priv, unlock);
 
 	*(unsigned long *)ptr += freed_pages;
 	return NOTIFY_DONE;
diff --git a/drivers/gpu/drm/i915/i915_gem_stolen.c b/drivers/gpu/drm/i915/i915_gem_stolen.c
index f3abdc27c5dd..681db6083f4d 100644
--- a/drivers/gpu/drm/i915/i915_gem_stolen.c
+++ b/drivers/gpu/drm/i915/i915_gem_stolen.c
@@ -414,12 +414,10 @@ int i915_gem_init_stolen(struct drm_i915_private *dev_priv)
 		return 0;
 	}
 
-#ifdef CONFIG_INTEL_IOMMU
-	if (intel_iommu_gfx_mapped && INTEL_GEN(dev_priv) < 8) {
+	if (intel_vtd_active() && INTEL_GEN(dev_priv) < 8) {
 		DRM_INFO("DMAR active, disabling use of stolen memory\n");
 		return 0;
 	}
-#endif
 
 	if (ggtt->stolen_size == 0)
 		return 0;
diff --git a/drivers/gpu/drm/i915/i915_gem_timeline.c b/drivers/gpu/drm/i915/i915_gem_timeline.c
index b596ca7ee058..c597ce277a04 100644
--- a/drivers/gpu/drm/i915/i915_gem_timeline.c
+++ b/drivers/gpu/drm/i915/i915_gem_timeline.c
@@ -23,6 +23,32 @@
  */
 
 #include "i915_drv.h"
+#include "i915_syncmap.h"
+
+static void __intel_timeline_init(struct intel_timeline *tl,
+				  struct i915_gem_timeline *parent,
+				  u64 context,
+				  struct lock_class_key *lockclass,
+				  const char *lockname)
+{
+	tl->fence_context = context;
+	tl->common = parent;
+#ifdef CONFIG_DEBUG_SPINLOCK
+	__raw_spin_lock_init(&tl->lock.rlock, lockname, lockclass);
+#else
+	spin_lock_init(&tl->lock);
+#endif
+	init_request_active(&tl->last_request, NULL);
+	INIT_LIST_HEAD(&tl->requests);
+	i915_syncmap_init(&tl->sync);
+}
+
+static void __intel_timeline_fini(struct intel_timeline *tl)
+{
+	GEM_BUG_ON(!list_empty(&tl->requests));
+
+	i915_syncmap_free(&tl->sync);
+}
 
 static int __i915_gem_timeline_init(struct drm_i915_private *i915,
 				    struct i915_gem_timeline *timeline,
@@ -35,6 +61,14 @@ static int __i915_gem_timeline_init(struct drm_i915_private *i915,
 
 	lockdep_assert_held(&i915->drm.struct_mutex);
 
+	/*
+	 * Ideally we want a set of engines on a single leaf as we expect
+	 * to mostly be tracking synchronisation between engines. It is not
+	 * a huge issue if this is not the case, but we may want to mitigate
+	 * any page crossing penalties if they become an issue.
+	 */
+	BUILD_BUG_ON(KSYNCMAP < I915_NUM_ENGINES);
+
 	timeline->i915 = i915;
 	timeline->name = kstrdup(name ?: "[kernel]", GFP_KERNEL);
 	if (!timeline->name)
@@ -44,19 +78,10 @@ static int __i915_gem_timeline_init(struct drm_i915_private *i915,
 
 	/* Called during early_init before we know how many engines there are */
 	fences = dma_fence_context_alloc(ARRAY_SIZE(timeline->engine));
-	for (i = 0; i < ARRAY_SIZE(timeline->engine); i++) {
-		struct intel_timeline *tl = &timeline->engine[i];
-
-		tl->fence_context = fences++;
-		tl->common = timeline;
-#ifdef CONFIG_DEBUG_SPINLOCK
-		__raw_spin_lock_init(&tl->lock.rlock, lockname, lockclass);
-#else
-		spin_lock_init(&tl->lock);
-#endif
-		init_request_active(&tl->last_request, NULL);
-		INIT_LIST_HEAD(&tl->requests);
-	}
+	for (i = 0; i < ARRAY_SIZE(timeline->engine); i++)
+		__intel_timeline_init(&timeline->engine[i],
+				      timeline, fences++,
+				      lockclass, lockname);
 
 	return 0;
 }
@@ -81,18 +106,52 @@ int i915_gem_timeline_init__global(struct drm_i915_private *i915)
 					&class, "&global_timeline->lock");
 }
 
+/**
+ * i915_gem_timelines_mark_idle -- called when the driver idles
+ * @i915 - the drm_i915_private device
+ *
+ * When the driver is completely idle, we know that all of our sync points
+ * have been signaled and our tracking is then entirely redundant. Any request
+ * to wait upon an older sync point will be completed instantly as we know
+ * the fence is signaled and therefore we will not even look them up in the
+ * sync point map.
+ */
+void i915_gem_timelines_mark_idle(struct drm_i915_private *i915)
+{
+	struct i915_gem_timeline *timeline;
+	int i;
+
+	lockdep_assert_held(&i915->drm.struct_mutex);
+
+	list_for_each_entry(timeline, &i915->gt.timelines, link) {
+		for (i = 0; i < ARRAY_SIZE(timeline->engine); i++) {
+			struct intel_timeline *tl = &timeline->engine[i];
+
+			/*
+			 * All known fences are completed so we can scrap
+			 * the current sync point tracking and start afresh,
+			 * any attempt to wait upon a previous sync point
+			 * will be skipped as the fence was signaled.
+			 */
+			i915_syncmap_free(&tl->sync);
+		}
+	}
+}
+
 void i915_gem_timeline_fini(struct i915_gem_timeline *timeline)
 {
 	int i;
 
 	lockdep_assert_held(&timeline->i915->drm.struct_mutex);
 
-	for (i = 0; i < ARRAY_SIZE(timeline->engine); i++) {
-		struct intel_timeline *tl = &timeline->engine[i];
-
-		GEM_BUG_ON(!list_empty(&tl->requests));
-	}
+	for (i = 0; i < ARRAY_SIZE(timeline->engine); i++)
+		__intel_timeline_fini(&timeline->engine[i]);
 
 	list_del(&timeline->link);
 	kfree(timeline->name);
 }
+
+#if IS_ENABLED(CONFIG_DRM_I915_SELFTEST)
+#include "selftests/mock_timeline.c"
+#include "selftests/i915_gem_timeline.c"
+#endif
diff --git a/drivers/gpu/drm/i915/i915_gem_timeline.h b/drivers/gpu/drm/i915/i915_gem_timeline.h
index 6c53e14cab2a..bfb5eb94c64d 100644
--- a/drivers/gpu/drm/i915/i915_gem_timeline.h
+++ b/drivers/gpu/drm/i915/i915_gem_timeline.h
@@ -27,7 +27,9 @@
 
 #include <linux/list.h>
 
+#include "i915_utils.h"
 #include "i915_gem_request.h"
+#include "i915_syncmap.h"
 
 struct i915_gem_timeline;
 
@@ -55,7 +57,25 @@ struct intel_timeline {
 	 * struct_mutex.
 	 */
 	struct i915_gem_active last_request;
-	u32 sync_seqno[I915_NUM_ENGINES];
+
+	/**
+	 * We track the most recent seqno that we wait on in every context so
+	 * that we only have to emit a new await and dependency on a more
+	 * recent sync point. As the contexts may be executed out-of-order, we
+	 * have to track each individually and can not rely on an absolute
+	 * global_seqno. When we know that all tracked fences are completed
+	 * (i.e. when the driver is idle), we know that the syncmap is
+	 * redundant and we can discard it without loss of generality.
+	 */
+	struct i915_syncmap *sync;
+	/**
+	 * Separately to the inter-context seqno map above, we track the last
+	 * barrier (e.g. semaphore wait) to the global engine timelines. Note
+	 * that this tracks global_seqno rather than the context.seqno, and
+	 * so it is subject to the limitations of hw wraparound and that we
+	 * may need to revoke global_seqno (on pre-emption).
+	 */
+	u32 global_sync[I915_NUM_ENGINES];
 
 	struct i915_gem_timeline *common;
 };
@@ -73,6 +93,31 @@ int i915_gem_timeline_init(struct drm_i915_private *i915,
 			   struct i915_gem_timeline *tl,
 			   const char *name);
 int i915_gem_timeline_init__global(struct drm_i915_private *i915);
+void i915_gem_timelines_mark_idle(struct drm_i915_private *i915);
 void i915_gem_timeline_fini(struct i915_gem_timeline *tl);
 
+static inline int __intel_timeline_sync_set(struct intel_timeline *tl,
+					    u64 context, u32 seqno)
+{
+	return i915_syncmap_set(&tl->sync, context, seqno);
+}
+
+static inline int intel_timeline_sync_set(struct intel_timeline *tl,
+					  const struct dma_fence *fence)
+{
+	return __intel_timeline_sync_set(tl, fence->context, fence->seqno);
+}
+
+static inline bool __intel_timeline_sync_is_later(struct intel_timeline *tl,
+						  u64 context, u32 seqno)
+{
+	return i915_syncmap_is_later(&tl->sync, context, seqno);
+}
+
+static inline bool intel_timeline_sync_is_later(struct intel_timeline *tl,
+						const struct dma_fence *fence)
+{
+	return __intel_timeline_sync_is_later(tl, fence->context, fence->seqno);
+}
+
 #endif
diff --git a/drivers/gpu/drm/i915/i915_gpu_error.c b/drivers/gpu/drm/i915/i915_gpu_error.c
index 8effc59f5cb5..e18f350bc364 100644
--- a/drivers/gpu/drm/i915/i915_gpu_error.c
+++ b/drivers/gpu/drm/i915/i915_gpu_error.c
@@ -712,6 +712,10 @@ int i915_error_state_to_str(struct drm_i915_error_state_buf *m,
 			print_error_obj(m, dev_priv->engine[i], NULL, obj);
 		}
 
+		for (j = 0; j < ee->user_bo_count; j++)
+			print_error_obj(m, dev_priv->engine[i],
+					"user", ee->user_bo[j]);
+
 		if (ee->num_requests) {
 			err_printf(m, "%s --- %d requests\n",
 				   dev_priv->engine[i]->name,
@@ -825,11 +829,15 @@ void __i915_gpu_state_free(struct kref *error_ref)
 {
 	struct i915_gpu_state *error =
 		container_of(error_ref, typeof(*error), ref);
-	int i;
+	long i, j;
 
 	for (i = 0; i < ARRAY_SIZE(error->engine); i++) {
 		struct drm_i915_error_engine *ee = &error->engine[i];
 
+		for (j = 0; j < ee->user_bo_count; j++)
+			i915_error_object_free(ee->user_bo[j]);
+		kfree(ee->user_bo);
+
 		i915_error_object_free(ee->batchbuffer);
 		i915_error_object_free(ee->wa_batchbuffer);
 		i915_error_object_free(ee->ringbuffer);
@@ -1316,12 +1324,17 @@ static void engine_record_requests(struct intel_engine_cs *engine,
 static void error_record_engine_execlists(struct intel_engine_cs *engine,
 					  struct drm_i915_error_engine *ee)
 {
+	const struct execlist_port *port = engine->execlist_port;
 	unsigned int n;
 
-	for (n = 0; n < ARRAY_SIZE(engine->execlist_port); n++)
-		if (engine->execlist_port[n].request)
-			record_request(engine->execlist_port[n].request,
-				       &ee->execlist[n]);
+	for (n = 0; n < ARRAY_SIZE(engine->execlist_port); n++) {
+		struct drm_i915_gem_request *rq = port_request(&port[n]);
+
+		if (!rq)
+			break;
+
+		record_request(rq, &ee->execlist[n]);
+	}
 }
 
 static void record_context(struct drm_i915_error_context *e,
@@ -1346,6 +1359,35 @@ static void record_context(struct drm_i915_error_context *e,
 	e->active = ctx->active_count;
 }
 
+static void request_record_user_bo(struct drm_i915_gem_request *request,
+				   struct drm_i915_error_engine *ee)
+{
+	struct i915_gem_capture_list *c;
+	struct drm_i915_error_object **bo;
+	long count;
+
+	count = 0;
+	for (c = request->capture_list; c; c = c->next)
+		count++;
+
+	bo = NULL;
+	if (count)
+		bo = kcalloc(count, sizeof(*bo), GFP_ATOMIC);
+	if (!bo)
+		return;
+
+	count = 0;
+	for (c = request->capture_list; c; c = c->next) {
+		bo[count] = i915_error_object_create(request->i915, c->vma);
+		if (!bo[count])
+			break;
+		count++;
+	}
+
+	ee->user_bo = bo;
+	ee->user_bo_count = count;
+}
+
 static void i915_gem_record_rings(struct drm_i915_private *dev_priv,
 				  struct i915_gpu_state *error)
 {
@@ -1392,6 +1434,7 @@ static void i915_gem_record_rings(struct drm_i915_private *dev_priv,
 				ee->wa_batchbuffer =
 					i915_error_object_create(dev_priv,
 								 engine->scratch);
+			request_record_user_bo(request, ee);
 
 			ee->ctx =
 				i915_error_object_create(dev_priv,
@@ -1560,6 +1603,9 @@ static void i915_capture_reg_state(struct drm_i915_private *dev_priv,
 		error->done_reg = I915_READ(DONE_REG);
 	}
 
+	if (INTEL_GEN(dev_priv) >= 5)
+		error->ccid = I915_READ(CCID);
+
 	/* 3: Feature specific registers */
 	if (IS_GEN6(dev_priv) || IS_GEN7(dev_priv)) {
 		error->gam_ecochk = I915_READ(GAM_ECOCHK);
@@ -1567,9 +1613,6 @@ static void i915_capture_reg_state(struct drm_i915_private *dev_priv,
 	}
 
 	/* 4: Everything else */
-	if (HAS_HW_CONTEXTS(dev_priv))
-		error->ccid = I915_READ(CCID);
-
 	if (INTEL_GEN(dev_priv) >= 8) {
 		error->ier = I915_READ(GEN8_DE_MISC_IER);
 		for (i = 0; i < 4; i++)
diff --git a/drivers/gpu/drm/i915/i915_guc_submission.c b/drivers/gpu/drm/i915/i915_guc_submission.c
index 1642fff9cf13..48a1e9349a2c 100644
--- a/drivers/gpu/drm/i915/i915_guc_submission.c
+++ b/drivers/gpu/drm/i915/i915_guc_submission.c
@@ -105,7 +105,7 @@ static int __reserve_doorbell(struct i915_guc_client *client)
 		end += offset;
 	}
 
-	id = find_next_zero_bit(client->guc->doorbell_bitmap, offset, end);
+	id = find_next_zero_bit(client->guc->doorbell_bitmap, end, offset);
 	if (id == end)
 		return -ENOSPC;
 
@@ -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
@@ -616,12 +614,6 @@ static void __i915_guc_submit(struct drm_i915_gem_request *rq)
 	b_ret = guc_ring_doorbell(client);
 
 	client->submissions[engine_id] += 1;
-	client->retcode = b_ret;
-	if (b_ret)
-		client->b_fail += 1;
-
-	guc->submissions[engine_id] += 1;
-	guc->last_seqno[engine_id] = rq->global_seqno;
 
 	spin_unlock_irqrestore(&client->wq_lock, flags);
 }
@@ -651,47 +643,68 @@ static void nested_enable_signaling(struct drm_i915_gem_request *rq)
 	trace_dma_fence_enable_signal(&rq->fence);
 
 	spin_lock_nested(&rq->lock, SINGLE_DEPTH_NESTING);
-	intel_engine_enable_signaling(rq);
+	intel_engine_enable_signaling(rq, true);
 	spin_unlock(&rq->lock);
 }
 
+static void port_assign(struct execlist_port *port,
+			struct drm_i915_gem_request *rq)
+{
+	GEM_BUG_ON(rq == port_request(port));
+
+	if (port_isset(port))
+		i915_gem_request_put(port_request(port));
+
+	port_set(port, i915_gem_request_get(rq));
+	nested_enable_signaling(rq);
+}
+
 static bool i915_guc_dequeue(struct intel_engine_cs *engine)
 {
 	struct execlist_port *port = engine->execlist_port;
-	struct drm_i915_gem_request *last = port[0].request;
+	struct drm_i915_gem_request *last = port_request(port);
 	struct rb_node *rb;
 	bool submit = false;
 
 	spin_lock_irq(&engine->timeline->lock);
 	rb = engine->execlist_first;
+	GEM_BUG_ON(rb_first(&engine->execlist_queue) != rb);
 	while (rb) {
-		struct drm_i915_gem_request *rq =
-			rb_entry(rb, typeof(*rq), priotree.node);
-
-		if (last && rq->ctx != last->ctx) {
-			if (port != engine->execlist_port)
-				break;
-
-			i915_gem_request_assign(&port->request, last);
-			nested_enable_signaling(last);
-			port++;
+		struct i915_priolist *p = rb_entry(rb, typeof(*p), node);
+		struct drm_i915_gem_request *rq, *rn;
+
+		list_for_each_entry_safe(rq, rn, &p->requests, priotree.link) {
+			if (last && rq->ctx != last->ctx) {
+				if (port != engine->execlist_port) {
+					__list_del_many(&p->requests,
+							&rq->priotree.link);
+					goto done;
+				}
+
+				if (submit)
+					port_assign(port, last);
+				port++;
+			}
+
+			INIT_LIST_HEAD(&rq->priotree.link);
+			rq->priotree.priority = INT_MAX;
+
+			i915_guc_submit(rq);
+			trace_i915_gem_request_in(rq, port_index(port, engine));
+			last = rq;
+			submit = true;
 		}
 
 		rb = rb_next(rb);
-		rb_erase(&rq->priotree.node, &engine->execlist_queue);
-		RB_CLEAR_NODE(&rq->priotree.node);
-		rq->priotree.priority = INT_MAX;
-
-		i915_guc_submit(rq);
-		trace_i915_gem_request_in(rq, port - engine->execlist_port);
-		last = rq;
-		submit = true;
-	}
-	if (submit) {
-		i915_gem_request_assign(&port->request, last);
-		nested_enable_signaling(last);
-		engine->execlist_first = rb;
+		rb_erase(&p->node, &engine->execlist_queue);
+		INIT_LIST_HEAD(&p->requests);
+		if (p->priority != I915_PRIORITY_NORMAL)
+			kmem_cache_free(engine->i915->priorities, p);
 	}
+done:
+	engine->execlist_first = rb;
+	if (submit)
+		port_assign(port, last);
 	spin_unlock_irq(&engine->timeline->lock);
 
 	return submit;
@@ -705,17 +718,19 @@ static void i915_guc_irq_handler(unsigned long data)
 	bool submit;
 
 	do {
-		rq = port[0].request;
+		rq = port_request(&port[0]);
 		while (rq && i915_gem_request_completed(rq)) {
 			trace_i915_gem_request_out(rq);
 			i915_gem_request_put(rq);
-			port[0].request = port[1].request;
-			port[1].request = NULL;
-			rq = port[0].request;
+
+			port[0] = port[1];
+			memset(&port[1], 0, sizeof(port[1]));
+
+			rq = port_request(&port[0]);
 		}
 
 		submit = false;
-		if (!port[1].request)
+		if (!port_count(&port[1]))
 			submit = i915_guc_dequeue(engine);
 	} while (submit);
 }
@@ -1053,8 +1068,7 @@ static int guc_ads_create(struct intel_guc *guc)
 		dev_priv->engine[RCS]->status_page.ggtt_offset;
 
 	for_each_engine(engine, dev_priv, id)
-		blob->ads.eng_state_size[engine->guc_id] =
-			intel_lr_context_size(engine);
+		blob->ads.eng_state_size[engine->guc_id] = engine->context_size;
 
 	base = guc_ggtt_offset(vma);
 	blob->ads.scheduler_policies = base + ptr_offset(blob, policies);
diff --git a/drivers/gpu/drm/i915/i915_irq.c b/drivers/gpu/drm/i915/i915_irq.c
index 04493ef1d2f7..7b7f55a28eec 100644
--- a/drivers/gpu/drm/i915/i915_irq.c
+++ b/drivers/gpu/drm/i915/i915_irq.c
@@ -1200,7 +1200,7 @@ out:
 static void ivybridge_parity_work(struct work_struct *work)
 {
 	struct drm_i915_private *dev_priv =
-		container_of(work, struct drm_i915_private, l3_parity.error_work);
+		container_of(work, typeof(*dev_priv), l3_parity.error_work);
 	u32 error_status, row, bank, subbank;
 	char *parity_event[6];
 	uint32_t misccpctl;
@@ -1317,14 +1317,16 @@ static void snb_gt_irq_handler(struct drm_i915_private *dev_priv,
 		ivybridge_parity_error_irq_handler(dev_priv, gt_iir);
 }
 
-static __always_inline void
+static void
 gen8_cs_irq_handler(struct intel_engine_cs *engine, u32 iir, int test_shift)
 {
 	bool tasklet = false;
 
 	if (iir & (GT_CONTEXT_SWITCH_INTERRUPT << test_shift)) {
-		set_bit(ENGINE_IRQ_EXECLIST, &engine->irq_posted);
-		tasklet = true;
+		if (port_count(&engine->execlist_port[0])) {
+			__set_bit(ENGINE_IRQ_EXECLIST, &engine->irq_posted);
+			tasklet = true;
+		}
 	}
 
 	if (iir & (GT_RENDER_USER_INTERRUPT << test_shift)) {
@@ -2917,7 +2919,6 @@ static void vlv_display_irq_postinstall(struct drm_i915_private *dev_priv)
 	u32 pipestat_mask;
 	u32 enable_mask;
 	enum pipe pipe;
-	u32 val;
 
 	pipestat_mask = PLANE_FLIP_DONE_INT_STATUS_VLV |
 			PIPE_CRC_DONE_INTERRUPT_STATUS;
@@ -2928,18 +2929,16 @@ static void vlv_display_irq_postinstall(struct drm_i915_private *dev_priv)
 
 	enable_mask = I915_DISPLAY_PORT_INTERRUPT |
 		I915_DISPLAY_PIPE_A_EVENT_INTERRUPT |
-		I915_DISPLAY_PIPE_B_EVENT_INTERRUPT;
+		I915_DISPLAY_PIPE_B_EVENT_INTERRUPT |
+		I915_LPE_PIPE_A_INTERRUPT |
+		I915_LPE_PIPE_B_INTERRUPT;
+
 	if (IS_CHERRYVIEW(dev_priv))
-		enable_mask |= I915_DISPLAY_PIPE_C_EVENT_INTERRUPT;
+		enable_mask |= I915_DISPLAY_PIPE_C_EVENT_INTERRUPT |
+			I915_LPE_PIPE_C_INTERRUPT;
 
 	WARN_ON(dev_priv->irq_mask != ~0);
 
-	val = (I915_LPE_PIPE_A_INTERRUPT |
-		I915_LPE_PIPE_B_INTERRUPT |
-		I915_LPE_PIPE_C_INTERRUPT);
-
-	enable_mask |= val;
-
 	dev_priv->irq_mask = ~enable_mask;
 
 	GEN5_IRQ_INIT(VLV_, dev_priv->irq_mask, enable_mask);
@@ -4197,11 +4196,15 @@ static void i965_irq_uninstall(struct drm_device * dev)
 void intel_irq_init(struct drm_i915_private *dev_priv)
 {
 	struct drm_device *dev = &dev_priv->drm;
+	int i;
 
 	intel_hpd_init_work(dev_priv);
 
 	INIT_WORK(&dev_priv->rps.work, gen6_pm_rps_work);
+
 	INIT_WORK(&dev_priv->l3_parity.error_work, ivybridge_parity_work);
+	for (i = 0; i < MAX_L3_SLICES; ++i)
+		dev_priv->l3_parity.remap_info[i] = NULL;
 
 	if (HAS_GUC_SCHED(dev_priv))
 		dev_priv->pm_guc_events = GEN9_GUC_TO_HOST_INT_EVENT;
@@ -4327,6 +4330,20 @@ void intel_irq_init(struct drm_i915_private *dev_priv)
 }
 
 /**
+ * intel_irq_fini - deinitializes IRQ support
+ * @i915: i915 device instance
+ *
+ * This function deinitializes all the IRQ support.
+ */
+void intel_irq_fini(struct drm_i915_private *i915)
+{
+	int i;
+
+	for (i = 0; i < MAX_L3_SLICES; ++i)
+		kfree(i915->l3_parity.remap_info[i]);
+}
+
+/**
  * intel_irq_install - enables the hardware interrupt
  * @dev_priv: i915 device instance
  *
diff --git a/drivers/gpu/drm/i915/i915_pci.c b/drivers/gpu/drm/i915/i915_pci.c
index f87b0c4e564d..cf43dc1d539f 100644
--- a/drivers/gpu/drm/i915/i915_pci.c
+++ b/drivers/gpu/drm/i915/i915_pci.c
@@ -220,7 +220,6 @@ static const struct intel_device_info intel_ironlake_m_info = {
 	.has_rc6 = 1, \
 	.has_rc6p = 1, \
 	.has_gmbus_irq = 1, \
-	.has_hw_contexts = 1, \
 	.has_aliasing_ppgtt = 1, \
 	GEN_DEFAULT_PIPEOFFSETS, \
 	CURSOR_OFFSETS
@@ -245,7 +244,6 @@ static const struct intel_device_info intel_sandybridge_m_info = {
 	.has_rc6 = 1, \
 	.has_rc6p = 1, \
 	.has_gmbus_irq = 1, \
-	.has_hw_contexts = 1, \
 	.has_aliasing_ppgtt = 1, \
 	.has_full_ppgtt = 1, \
 	GEN_DEFAULT_PIPEOFFSETS, \
@@ -280,7 +278,6 @@ static const struct intel_device_info intel_valleyview_info = {
 	.has_runtime_pm = 1,
 	.has_rc6 = 1,
 	.has_gmbus_irq = 1,
-	.has_hw_contexts = 1,
 	.has_gmch_display = 1,
 	.has_hotplug = 1,
 	.has_aliasing_ppgtt = 1,
@@ -340,7 +337,6 @@ static const struct intel_device_info intel_cherryview_info = {
 	.has_resource_streamer = 1,
 	.has_rc6 = 1,
 	.has_gmbus_irq = 1,
-	.has_hw_contexts = 1,
 	.has_logical_ring_contexts = 1,
 	.has_gmch_display = 1,
 	.has_aliasing_ppgtt = 1,
@@ -387,10 +383,8 @@ static const struct intel_device_info intel_skylake_gt3_info = {
 	.has_rc6 = 1, \
 	.has_dp_mst = 1, \
 	.has_gmbus_irq = 1, \
-	.has_hw_contexts = 1, \
 	.has_logical_ring_contexts = 1, \
 	.has_guc = 1, \
-	.has_decoupled_mmio = 1, \
 	.has_aliasing_ppgtt = 1, \
 	.has_full_ppgtt = 1, \
 	.has_full_48bit_ppgtt = 1, \
diff --git a/drivers/gpu/drm/i915/i915_perf.c b/drivers/gpu/drm/i915/i915_perf.c
index 060b171480d5..85269bcc8372 100644
--- a/drivers/gpu/drm/i915/i915_perf.c
+++ b/drivers/gpu/drm/i915/i915_perf.c
@@ -205,25 +205,49 @@
 
 #define OA_TAKEN(tail, head)	((tail - head) & (OA_BUFFER_SIZE - 1))
 
-/* There's a HW race condition between OA unit tail pointer register updates and
+/**
+ * DOC: OA Tail Pointer Race
+ *
+ * There's a HW race condition between OA unit tail pointer register updates and
  * writes to memory whereby the tail pointer can sometimes get ahead of what's
- * been written out to the OA buffer so far.
+ * been written out to the OA buffer so far (in terms of what's visible to the
+ * CPU).
+ *
+ * Although this can be observed explicitly while copying reports to userspace
+ * by checking for a zeroed report-id field in tail reports, we want to account
+ * for this earlier, as part of the _oa_buffer_check to avoid lots of redundant
+ * read() attempts.
+ *
+ * In effect we define a tail pointer for reading that lags the real tail
+ * pointer by at least %OA_TAIL_MARGIN_NSEC nanoseconds, which gives enough
+ * time for the corresponding reports to become visible to the CPU.
+ *
+ * To manage this we actually track two tail pointers:
+ *  1) An 'aging' tail with an associated timestamp that is tracked until we
+ *     can trust the corresponding data is visible to the CPU; at which point
+ *     it is considered 'aged'.
+ *  2) An 'aged' tail that can be used for read()ing.
+ *
+ * The two separate pointers let us decouple read()s from tail pointer aging.
+ *
+ * The tail pointers are checked and updated at a limited rate within a hrtimer
+ * callback (the same callback that is used for delivering POLLIN events)
  *
- * Although this can be observed explicitly by checking for a zeroed report-id
- * field in tail reports, it seems preferable to account for this earlier e.g.
- * as part of the _oa_buffer_is_empty checks to minimize -EAGAIN polling cycles
- * in this situation.
+ * Initially the tails are marked invalid with %INVALID_TAIL_PTR which
+ * indicates that an updated tail pointer is needed.
  *
- * To give time for the most recent reports to land before they may be copied to
- * userspace, the driver operates as if the tail pointer effectively lags behind
- * the HW tail pointer by 'tail_margin' bytes. The margin in bytes is calculated
- * based on this constant in nanoseconds, the current OA sampling exponent
- * and current report size.
+ * Most of the implementation details for this workaround are in
+ * gen7_oa_buffer_check_unlocked() and gen7_appand_oa_reports()
  *
- * There is also a fallback check while reading to simply skip over reports with
- * a zeroed report-id.
+ * Note for posterity: previously the driver used to define an effective tail
+ * pointer that lagged the real pointer by a 'tail margin' measured in bytes
+ * derived from %OA_TAIL_MARGIN_NSEC and the configured sampling frequency.
+ * This was flawed considering that the OA unit may also automatically generate
+ * non-periodic reports (such as on context switch) or the OA unit may be
+ * enabled without any periodic sampling.
  */
 #define OA_TAIL_MARGIN_NSEC	100000ULL
+#define INVALID_TAIL_PTR	0xffffffff
 
 /* frequency for checking whether the OA unit has written new reports to the
  * circular OA buffer...
@@ -308,27 +332,121 @@ struct perf_open_properties {
 	int oa_period_exponent;
 };
 
-/* NB: This is either called via fops or the poll check hrtimer (atomic ctx)
+/**
+ * gen7_oa_buffer_check_unlocked - check for data and update tail ptr state
+ * @dev_priv: i915 device instance
+ *
+ * This is either called via fops (for blocking reads in user ctx) or the poll
+ * check hrtimer (atomic ctx) to check the OA buffer tail pointer and check
+ * if there is data available for userspace to read.
  *
- * It's safe to read OA config state here unlocked, assuming that this is only
- * called while the stream is enabled, while the global OA configuration can't
- * be modified.
+ * This function is central to providing a workaround for the OA unit tail
+ * pointer having a race with respect to what data is visible to the CPU.
+ * It is responsible for reading tail pointers from the hardware and giving
+ * the pointers time to 'age' before they are made available for reading.
+ * (See description of OA_TAIL_MARGIN_NSEC above for further details.)
  *
- * Note: we don't lock around the head/tail reads even though there's the slim
- * possibility of read() fop errors forcing a re-init of the OA buffer
- * pointers.  A race here could result in a false positive !empty status which
- * is acceptable.
+ * Besides returning true when there is data available to read() this function
+ * also has the side effect of updating the oa_buffer.tails[], .aging_timestamp
+ * and .aged_tail_idx state used for reading.
+ *
+ * Note: It's safe to read OA config state here unlocked, assuming that this is
+ * only called while the stream is enabled, while the global OA configuration
+ * can't be modified.
+ *
+ * Returns: %true if the OA buffer contains data, else %false
  */
-static bool gen7_oa_buffer_is_empty_fop_unlocked(struct drm_i915_private *dev_priv)
+static bool gen7_oa_buffer_check_unlocked(struct drm_i915_private *dev_priv)
 {
 	int report_size = dev_priv->perf.oa.oa_buffer.format_size;
-	u32 oastatus2 = I915_READ(GEN7_OASTATUS2);
-	u32 oastatus1 = I915_READ(GEN7_OASTATUS1);
-	u32 head = oastatus2 & GEN7_OASTATUS2_HEAD_MASK;
-	u32 tail = oastatus1 & GEN7_OASTATUS1_TAIL_MASK;
+	unsigned long flags;
+	unsigned int aged_idx;
+	u32 oastatus1;
+	u32 head, hw_tail, aged_tail, aging_tail;
+	u64 now;
+
+	/* We have to consider the (unlikely) possibility that read() errors
+	 * could result in an OA buffer reset which might reset the head,
+	 * tails[] and aged_tail state.
+	 */
+	spin_lock_irqsave(&dev_priv->perf.oa.oa_buffer.ptr_lock, flags);
+
+	/* NB: The head we observe here might effectively be a little out of
+	 * date (between head and tails[aged_idx].offset if there is currently
+	 * a read() in progress.
+	 */
+	head = dev_priv->perf.oa.oa_buffer.head;
+
+	aged_idx = dev_priv->perf.oa.oa_buffer.aged_tail_idx;
+	aged_tail = dev_priv->perf.oa.oa_buffer.tails[aged_idx].offset;
+	aging_tail = dev_priv->perf.oa.oa_buffer.tails[!aged_idx].offset;
 
-	return OA_TAKEN(tail, head) <
-		dev_priv->perf.oa.tail_margin + report_size;
+	oastatus1 = I915_READ(GEN7_OASTATUS1);
+	hw_tail = oastatus1 & GEN7_OASTATUS1_TAIL_MASK;
+
+	/* The tail pointer increases in 64 byte increments,
+	 * not in report_size steps...
+	 */
+	hw_tail &= ~(report_size - 1);
+
+	now = ktime_get_mono_fast_ns();
+
+	/* Update the aged tail
+	 *
+	 * Flip the tail pointer available for read()s once the aging tail is
+	 * old enough to trust that the corresponding data will be visible to
+	 * the CPU...
+	 *
+	 * Do this before updating the aging pointer in case we may be able to
+	 * immediately start aging a new pointer too (if new data has become
+	 * available) without needing to wait for a later hrtimer callback.
+	 */
+	if (aging_tail != INVALID_TAIL_PTR &&
+	    ((now - dev_priv->perf.oa.oa_buffer.aging_timestamp) >
+	     OA_TAIL_MARGIN_NSEC)) {
+		aged_idx ^= 1;
+		dev_priv->perf.oa.oa_buffer.aged_tail_idx = aged_idx;
+
+		aged_tail = aging_tail;
+
+		/* Mark that we need a new pointer to start aging... */
+		dev_priv->perf.oa.oa_buffer.tails[!aged_idx].offset = INVALID_TAIL_PTR;
+		aging_tail = INVALID_TAIL_PTR;
+	}
+
+	/* Update the aging tail
+	 *
+	 * We throttle aging tail updates until we have a new tail that
+	 * represents >= one report more data than is already available for
+	 * reading. This ensures there will be enough data for a successful
+	 * read once this new pointer has aged and ensures we will give the new
+	 * pointer time to age.
+	 */
+	if (aging_tail == INVALID_TAIL_PTR &&
+	    (aged_tail == INVALID_TAIL_PTR ||
+	     OA_TAKEN(hw_tail, aged_tail) >= report_size)) {
+		struct i915_vma *vma = dev_priv->perf.oa.oa_buffer.vma;
+		u32 gtt_offset = i915_ggtt_offset(vma);
+
+		/* Be paranoid and do a bounds check on the pointer read back
+		 * from hardware, just in case some spurious hardware condition
+		 * could put the tail out of bounds...
+		 */
+		if (hw_tail >= gtt_offset &&
+		    hw_tail < (gtt_offset + OA_BUFFER_SIZE)) {
+			dev_priv->perf.oa.oa_buffer.tails[!aged_idx].offset =
+				aging_tail = hw_tail;
+			dev_priv->perf.oa.oa_buffer.aging_timestamp = now;
+		} else {
+			DRM_ERROR("Ignoring spurious out of range OA buffer tail pointer = %u\n",
+				  hw_tail);
+		}
+	}
+
+	spin_unlock_irqrestore(&dev_priv->perf.oa.oa_buffer.ptr_lock, flags);
+
+	return aged_tail == INVALID_TAIL_PTR ?
+		false : OA_TAKEN(aged_tail, head) >= report_size;
 }
 
 /**
@@ -421,8 +539,6 @@ static int append_oa_sample(struct i915_perf_stream *stream,
  * @buf: destination buffer given by userspace
  * @count: the number of bytes userspace wants to read
  * @offset: (inout): the current position for writing into @buf
- * @head_ptr: (inout): the current oa buffer cpu read position
- * @tail: the current oa buffer gpu write position
  *
  * Notably any error condition resulting in a short read (-%ENOSPC or
  * -%EFAULT) will be returned even though one or more records may
@@ -431,7 +547,7 @@ static int append_oa_sample(struct i915_perf_stream *stream,
  * userspace.
  *
  * Note: reports are consumed from the head, and appended to the
- * tail, so the head chases the tail?... If you think that's mad
+ * tail, so the tail chases the head?... If you think that's mad
  * and back-to-front you're not alone, but this follows the
  * Gen PRM naming convention.
  *
@@ -440,57 +556,55 @@ static int append_oa_sample(struct i915_perf_stream *stream,
 static int gen7_append_oa_reports(struct i915_perf_stream *stream,
 				  char __user *buf,
 				  size_t count,
-				  size_t *offset,
-				  u32 *head_ptr,
-				  u32 tail)
+				  size_t *offset)
 {
 	struct drm_i915_private *dev_priv = stream->dev_priv;
 	int report_size = dev_priv->perf.oa.oa_buffer.format_size;
 	u8 *oa_buf_base = dev_priv->perf.oa.oa_buffer.vaddr;
-	int tail_margin = dev_priv->perf.oa.tail_margin;
 	u32 gtt_offset = i915_ggtt_offset(dev_priv->perf.oa.oa_buffer.vma);
 	u32 mask = (OA_BUFFER_SIZE - 1);
-	u32 head;
+	size_t start_offset = *offset;
+	unsigned long flags;
+	unsigned int aged_tail_idx;
+	u32 head, tail;
 	u32 taken;
 	int ret = 0;
 
 	if (WARN_ON(!stream->enabled))
 		return -EIO;
 
-	head = *head_ptr - gtt_offset;
-	tail -= gtt_offset;
+	spin_lock_irqsave(&dev_priv->perf.oa.oa_buffer.ptr_lock, flags);
 
-	/* The OA unit is expected to wrap the tail pointer according to the OA
-	 * buffer size and since we should never write a misaligned head
-	 * pointer we don't expect to read one back either...
-	 */
-	if (tail > OA_BUFFER_SIZE || head > OA_BUFFER_SIZE ||
-	    head % report_size) {
-		DRM_ERROR("Inconsistent OA buffer pointer (head = %u, tail = %u): force restart\n",
-			  head, tail);
-		dev_priv->perf.oa.ops.oa_disable(dev_priv);
-		dev_priv->perf.oa.ops.oa_enable(dev_priv);
-		*head_ptr = I915_READ(GEN7_OASTATUS2) &
-			GEN7_OASTATUS2_HEAD_MASK;
-		return -EIO;
-	}
+	head = dev_priv->perf.oa.oa_buffer.head;
+	aged_tail_idx = dev_priv->perf.oa.oa_buffer.aged_tail_idx;
+	tail = dev_priv->perf.oa.oa_buffer.tails[aged_tail_idx].offset;
 
+	spin_unlock_irqrestore(&dev_priv->perf.oa.oa_buffer.ptr_lock, flags);
 
-	/* The tail pointer increases in 64 byte increments, not in report_size
-	 * steps...
+	/* An invalid tail pointer here means we're still waiting for the poll
+	 * hrtimer callback to give us a pointer
 	 */
-	tail &= ~(report_size - 1);
+	if (tail == INVALID_TAIL_PTR)
+		return -EAGAIN;
 
-	/* Move the tail pointer back by the current tail_margin to account for
-	 * the possibility that the latest reports may not have really landed
-	 * in memory yet...
+	/* NB: oa_buffer.head/tail include the gtt_offset which we don't want
+	 * while indexing relative to oa_buf_base.
 	 */
+	head -= gtt_offset;
+	tail -= gtt_offset;
 
-	if (OA_TAKEN(tail, head) < report_size + tail_margin)
-		return -EAGAIN;
+	/* An out of bounds or misaligned head or tail pointer implies a driver
+	 * bug since we validate + align the tail pointers we read from the
+	 * hardware and we are in full control of the head pointer which should
+	 * only be incremented by multiples of the report size (notably also
+	 * all a power of two).
+	 */
+	if (WARN_ONCE(head > OA_BUFFER_SIZE || head % report_size ||
+		      tail > OA_BUFFER_SIZE || tail % report_size,
+		      "Inconsistent OA buffer pointers: head = %u, tail = %u\n",
+		      head, tail))
+		return -EIO;
 
-	tail -= tail_margin;
-	tail &= mask;
 
 	for (/* none */;
 	     (taken = OA_TAKEN(tail, head));
@@ -518,7 +632,8 @@ static int gen7_append_oa_reports(struct i915_perf_stream *stream,
 		 * copying it to userspace...
 		 */
 		if (report32[0] == 0) {
-			DRM_NOTE("Skipping spurious, invalid OA report\n");
+			if (__ratelimit(&dev_priv->perf.oa.spurious_report_rs))
+				DRM_NOTE("Skipping spurious, invalid OA report\n");
 			continue;
 		}
 
@@ -535,7 +650,21 @@ static int gen7_append_oa_reports(struct i915_perf_stream *stream,
 		report32[0] = 0;
 	}
 
-	*head_ptr = gtt_offset + head;
+	if (start_offset != *offset) {
+		spin_lock_irqsave(&dev_priv->perf.oa.oa_buffer.ptr_lock, flags);
+
+		/* We removed the gtt_offset for the copy loop above, indexing
+		 * relative to oa_buf_base so put back here...
+		 */
+		head += gtt_offset;
+
+		I915_WRITE(GEN7_OASTATUS2,
+			   ((head & GEN7_OASTATUS2_HEAD_MASK) |
+			    OA_MEM_SELECT_GGTT));
+		dev_priv->perf.oa.oa_buffer.head = head;
+
+		spin_unlock_irqrestore(&dev_priv->perf.oa.oa_buffer.ptr_lock, flags);
+	}
 
 	return ret;
 }
@@ -562,22 +691,14 @@ static int gen7_oa_read(struct i915_perf_stream *stream,
 			size_t *offset)
 {
 	struct drm_i915_private *dev_priv = stream->dev_priv;
-	int report_size = dev_priv->perf.oa.oa_buffer.format_size;
-	u32 oastatus2;
 	u32 oastatus1;
-	u32 head;
-	u32 tail;
 	int ret;
 
 	if (WARN_ON(!dev_priv->perf.oa.oa_buffer.vaddr))
 		return -EIO;
 
-	oastatus2 = I915_READ(GEN7_OASTATUS2);
 	oastatus1 = I915_READ(GEN7_OASTATUS1);
 
-	head = oastatus2 & GEN7_OASTATUS2_HEAD_MASK;
-	tail = oastatus1 & GEN7_OASTATUS1_TAIL_MASK;
-
 	/* XXX: On Haswell we don't have a safe way to clear oastatus1
 	 * bits while the OA unit is enabled (while the tail pointer
 	 * may be updated asynchronously) so we ignore status bits
@@ -616,11 +737,7 @@ static int gen7_oa_read(struct i915_perf_stream *stream,
 		dev_priv->perf.oa.ops.oa_disable(dev_priv);
 		dev_priv->perf.oa.ops.oa_enable(dev_priv);
 
-		oastatus2 = I915_READ(GEN7_OASTATUS2);
 		oastatus1 = I915_READ(GEN7_OASTATUS1);
-
-		head = oastatus2 & GEN7_OASTATUS2_HEAD_MASK;
-		tail = oastatus1 & GEN7_OASTATUS1_TAIL_MASK;
 	}
 
 	if (unlikely(oastatus1 & GEN7_OASTATUS1_REPORT_LOST)) {
@@ -632,29 +749,7 @@ static int gen7_oa_read(struct i915_perf_stream *stream,
 			GEN7_OASTATUS1_REPORT_LOST;
 	}
 
-	ret = gen7_append_oa_reports(stream, buf, count, offset,
-				     &head, tail);
-
-	/* All the report sizes are a power of two and the
-	 * head should always be incremented by some multiple
-	 * of the report size.
-	 *
-	 * A warning here, but notably if we later read back a
-	 * misaligned pointer we will treat that as a bug since
-	 * it could lead to a buffer overrun.
-	 */
-	WARN_ONCE(head & (report_size - 1),
-		  "i915: Writing misaligned OA head pointer");
-
-	/* Note: we update the head pointer here even if an error
-	 * was returned since the error may represent a short read
-	 * where some some reports were successfully copied.
-	 */
-	I915_WRITE(GEN7_OASTATUS2,
-		   ((head & GEN7_OASTATUS2_HEAD_MASK) |
-		    OA_MEM_SELECT_GGTT));
-
-	return ret;
+	return gen7_append_oa_reports(stream, buf, count, offset);
 }
 
 /**
@@ -679,14 +774,8 @@ static int i915_oa_wait_unlocked(struct i915_perf_stream *stream)
 	if (!dev_priv->perf.oa.periodic)
 		return -EIO;
 
-	/* Note: the oa_buffer_is_empty() condition is ok to run unlocked as it
-	 * just performs mmio reads of the OA buffer head + tail pointers and
-	 * it's assumed we're handling some operation that implies the stream
-	 * can't be destroyed until completion (such as a read()) that ensures
-	 * the device + OA buffer can't disappear
-	 */
 	return wait_event_interruptible(dev_priv->perf.oa.poll_wq,
-					!dev_priv->perf.oa.ops.oa_buffer_is_empty(dev_priv));
+					dev_priv->perf.oa.ops.oa_buffer_check(dev_priv));
 }
 
 /**
@@ -744,6 +833,7 @@ static int oa_get_render_ctx_id(struct i915_perf_stream *stream)
 {
 	struct drm_i915_private *dev_priv = stream->dev_priv;
 	struct intel_engine_cs *engine = dev_priv->engine[RCS];
+	struct intel_ring *ring;
 	int ret;
 
 	ret = i915_mutex_lock_interruptible(&dev_priv->drm);
@@ -755,9 +845,10 @@ static int oa_get_render_ctx_id(struct i915_perf_stream *stream)
 	 *
 	 * NB: implied RCS engine...
 	 */
-	ret = engine->context_pin(engine, stream->ctx);
-	if (ret)
-		goto unlock;
+	ring = engine->context_pin(engine, stream->ctx);
+	mutex_unlock(&dev_priv->drm.struct_mutex);
+	if (IS_ERR(ring))
+		return PTR_ERR(ring);
 
 	/* Explicitly track the ID (instead of calling i915_ggtt_offset()
 	 * on the fly) considering the difference with gen8+ and
@@ -766,10 +857,7 @@ static int oa_get_render_ctx_id(struct i915_perf_stream *stream)
 	dev_priv->perf.oa.specific_ctx_id =
 		i915_ggtt_offset(stream->ctx->engine[engine->id].state);
 
-unlock:
-	mutex_unlock(&dev_priv->drm.struct_mutex);
-
-	return ret;
+	return 0;
 }
 
 /**
@@ -824,19 +912,36 @@ static void i915_oa_stream_destroy(struct i915_perf_stream *stream)
 		oa_put_render_ctx_id(stream);
 
 	dev_priv->perf.oa.exclusive_stream = NULL;
+
+	if (dev_priv->perf.oa.spurious_report_rs.missed) {
+		DRM_NOTE("%d spurious OA report notices suppressed due to ratelimiting\n",
+			 dev_priv->perf.oa.spurious_report_rs.missed);
+	}
 }
 
 static void gen7_init_oa_buffer(struct drm_i915_private *dev_priv)
 {
 	u32 gtt_offset = i915_ggtt_offset(dev_priv->perf.oa.oa_buffer.vma);
+	unsigned long flags;
+
+	spin_lock_irqsave(&dev_priv->perf.oa.oa_buffer.ptr_lock, flags);
 
 	/* Pre-DevBDW: OABUFFER must be set with counters off,
 	 * before OASTATUS1, but after OASTATUS2
 	 */
 	I915_WRITE(GEN7_OASTATUS2, gtt_offset | OA_MEM_SELECT_GGTT); /* head */
+	dev_priv->perf.oa.oa_buffer.head = gtt_offset;
+
 	I915_WRITE(GEN7_OABUFFER, gtt_offset);
+
 	I915_WRITE(GEN7_OASTATUS1, gtt_offset | OABUFFER_SIZE_16M); /* tail */
 
+	/* Mark that we need updated tail pointers to read from... */
+	dev_priv->perf.oa.oa_buffer.tails[0].offset = INVALID_TAIL_PTR;
+	dev_priv->perf.oa.oa_buffer.tails[1].offset = INVALID_TAIL_PTR;
+
+	spin_unlock_irqrestore(&dev_priv->perf.oa.oa_buffer.ptr_lock, flags);
+
 	/* On Haswell we have to track which OASTATUS1 flags we've
 	 * already seen since they can't be cleared while periodic
 	 * sampling is enabled.
@@ -1094,12 +1199,6 @@ static void i915_oa_stream_disable(struct i915_perf_stream *stream)
 		hrtimer_cancel(&dev_priv->perf.oa.poll_check_timer);
 }
 
-static u64 oa_exponent_to_ns(struct drm_i915_private *dev_priv, int exponent)
-{
-	return div_u64(1000000000ULL * (2ULL << exponent),
-		       dev_priv->perf.oa.timestamp_frequency);
-}
-
 static const struct i915_perf_stream_ops i915_oa_stream_ops = {
 	.destroy = i915_oa_stream_destroy,
 	.enable = i915_oa_stream_enable,
@@ -1173,6 +1272,26 @@ static int i915_oa_stream_init(struct i915_perf_stream *stream,
 		return -EINVAL;
 	}
 
+	/* We set up some ratelimit state to potentially throttle any _NOTES
+	 * about spurious, invalid OA reports which we don't forward to
+	 * userspace.
+	 *
+	 * The initialization is associated with opening the stream (not driver
+	 * init) considering we print a _NOTE about any throttling when closing
+	 * the stream instead of waiting until driver _fini which no one would
+	 * ever see.
+	 *
+	 * Using the same limiting factors as printk_ratelimit()
+	 */
+	ratelimit_state_init(&dev_priv->perf.oa.spurious_report_rs,
+			     5 * HZ, 10);
+	/* Since we use a DRM_NOTE for spurious reports it would be
+	 * inconsistent to let __ratelimit() automatically print a warning for
+	 * throttling.
+	 */
+	ratelimit_set_flags(&dev_priv->perf.oa.spurious_report_rs,
+			    RATELIMIT_MSG_ON_RELEASE);
+
 	stream->sample_size = sizeof(struct drm_i915_perf_record_header);
 
 	format_size = dev_priv->perf.oa.oa_formats[props->oa_format].size;
@@ -1190,20 +1309,9 @@ static int i915_oa_stream_init(struct i915_perf_stream *stream,
 	dev_priv->perf.oa.metrics_set = props->metrics_set;
 
 	dev_priv->perf.oa.periodic = props->oa_periodic;
-	if (dev_priv->perf.oa.periodic) {
-		u32 tail;
-
+	if (dev_priv->perf.oa.periodic)
 		dev_priv->perf.oa.period_exponent = props->oa_period_exponent;
 
-		/* See comment for OA_TAIL_MARGIN_NSEC for details
-		 * about this tail_margin...
-		 */
-		tail = div64_u64(OA_TAIL_MARGIN_NSEC,
-				 oa_exponent_to_ns(dev_priv,
-						   props->oa_period_exponent));
-		dev_priv->perf.oa.tail_margin = (tail + 1) * format_size;
-	}
-
 	if (stream->ctx) {
 		ret = oa_get_render_ctx_id(stream);
 		if (ret)
@@ -1352,7 +1460,15 @@ static ssize_t i915_perf_read(struct file *file,
 		mutex_unlock(&dev_priv->perf.lock);
 	}
 
-	if (ret >= 0) {
+	/* We allow the poll checking to sometimes report false positive POLLIN
+	 * events where we might actually report EAGAIN on read() if there's
+	 * not really any data available. In this situation though we don't
+	 * want to enter a busy loop between poll() reporting a POLLIN event
+	 * and read() returning -EAGAIN. Clearing the oa.pollin state here
+	 * effectively ensures we back off until the next hrtimer callback
+	 * before reporting another POLLIN event.
+	 */
+	if (ret >= 0 || ret == -EAGAIN) {
 		/* Maybe make ->pollin per-stream state if we support multiple
 		 * concurrent streams in the future.
 		 */
@@ -1368,7 +1484,7 @@ static enum hrtimer_restart oa_poll_check_timer_cb(struct hrtimer *hrtimer)
 		container_of(hrtimer, typeof(*dev_priv),
 			     perf.oa.poll_check_timer);
 
-	if (!dev_priv->perf.oa.ops.oa_buffer_is_empty(dev_priv)) {
+	if (dev_priv->perf.oa.ops.oa_buffer_check(dev_priv)) {
 		dev_priv->perf.oa.pollin = true;
 		wake_up(&dev_priv->perf.oa.poll_wq);
 	}
@@ -1817,11 +1933,13 @@ static int read_properties_unlocked(struct drm_i915_private *dev_priv,
 			break;
 		case DRM_I915_PERF_PROP_OA_FORMAT:
 			if (value == 0 || value >= I915_OA_FORMAT_MAX) {
-				DRM_DEBUG("Invalid OA report format\n");
+				DRM_DEBUG("Out-of-range OA report format %llu\n",
+					  value);
 				return -EINVAL;
 			}
 			if (!dev_priv->perf.oa.oa_formats[value].size) {
-				DRM_DEBUG("Invalid OA report format\n");
+				DRM_DEBUG("Unsupported OA report format %llu\n",
+					  value);
 				return -EINVAL;
 			}
 			props->oa_format = value;
@@ -2063,6 +2181,7 @@ void i915_perf_init(struct drm_i915_private *dev_priv)
 	INIT_LIST_HEAD(&dev_priv->perf.streams);
 	mutex_init(&dev_priv->perf.lock);
 	spin_lock_init(&dev_priv->perf.hook_lock);
+	spin_lock_init(&dev_priv->perf.oa.oa_buffer.ptr_lock);
 
 	dev_priv->perf.oa.ops.init_oa_buffer = gen7_init_oa_buffer;
 	dev_priv->perf.oa.ops.enable_metric_set = hsw_enable_metric_set;
@@ -2070,10 +2189,8 @@ void i915_perf_init(struct drm_i915_private *dev_priv)
 	dev_priv->perf.oa.ops.oa_enable = gen7_oa_enable;
 	dev_priv->perf.oa.ops.oa_disable = gen7_oa_disable;
 	dev_priv->perf.oa.ops.read = gen7_oa_read;
-	dev_priv->perf.oa.ops.oa_buffer_is_empty =
-		gen7_oa_buffer_is_empty_fop_unlocked;
-
-	dev_priv->perf.oa.timestamp_frequency = 12500000;
+	dev_priv->perf.oa.ops.oa_buffer_check =
+		gen7_oa_buffer_check_unlocked;
 
 	dev_priv->perf.oa.oa_formats = hsw_oa_formats;
 
diff --git a/drivers/gpu/drm/i915/i915_reg.h b/drivers/gpu/drm/i915/i915_reg.h
index 11b12f412492..231ee86625cd 100644
--- a/drivers/gpu/drm/i915/i915_reg.h
+++ b/drivers/gpu/drm/i915/i915_reg.h
@@ -85,6 +85,14 @@ static inline bool i915_mmio_reg_valid(i915_reg_t reg)
 #define VECS_HW		3
 #define VCS2_HW		4
 
+/* Engine class */
+
+#define RENDER_CLASS		0
+#define VIDEO_DECODE_CLASS	1
+#define VIDEO_ENHANCEMENT_CLASS	2
+#define COPY_ENGINE_CLASS	3
+#define OTHER_CLASS		4
+
 /* PCI config space */
 
 #define MCHBAR_I915 0x44
@@ -3051,10 +3059,14 @@ enum skl_disp_power_wells {
 #define CLKCFG_FSB_667					(3 << 0)	/* hrawclk 166 */
 #define CLKCFG_FSB_800					(2 << 0)	/* hrawclk 200 */
 #define CLKCFG_FSB_1067					(6 << 0)	/* hrawclk 266 */
+#define CLKCFG_FSB_1067_ALT				(0 << 0)	/* hrawclk 266 */
 #define CLKCFG_FSB_1333					(7 << 0)	/* hrawclk 333 */
-/* Note, below two are guess */
-#define CLKCFG_FSB_1600					(4 << 0)	/* hrawclk 400 */
-#define CLKCFG_FSB_1600_ALT				(0 << 0)	/* hrawclk 400 */
+/*
+ * Note that on at least on ELK the below value is reported for both
+ * 333 and 400 MHz BIOS FSB setting, but given that the gmch datasheet
+ * lists only 200/266/333 MHz FSB as supported let's decode it as 333 MHz.
+ */
+#define CLKCFG_FSB_1333_ALT				(4 << 0)	/* hrawclk 333 */
 #define CLKCFG_FSB_MASK					(7 << 0)
 #define CLKCFG_MEM_533					(1 << 4)
 #define CLKCFG_MEM_667					(2 << 4)
@@ -3362,16 +3374,6 @@ enum skl_disp_power_wells {
 #define GEN7_CXT_VFSTATE_SIZE(ctx_reg)	(((ctx_reg) >> 0) & 0x3f)
 #define GEN7_CXT_TOTAL_SIZE(ctx_reg)	(GEN7_CXT_EXTENDED_SIZE(ctx_reg) + \
 					 GEN7_CXT_VFSTATE_SIZE(ctx_reg))
-/* Haswell does have the CXT_SIZE register however it does not appear to be
- * valid. Now, docs explain in dwords what is in the context object. The full
- * size is 70720 bytes, however, the power context and execlist context will
- * never be saved (power context is stored elsewhere, and execlists don't work
- * on HSW) - so the final size, including the extra state required for the
- * Resource Streamer, is 66944 bytes, which rounds to 17 pages.
- */
-#define HSW_CXT_TOTAL_SIZE		(17 * PAGE_SIZE)
-/* Same as Haswell, but 72064 bytes now. */
-#define GEN8_CXT_TOTAL_SIZE		(18 * PAGE_SIZE)
 
 enum {
 	INTEL_ADVANCED_CONTEXT = 0,
@@ -5437,9 +5439,7 @@ enum {
 #define   CURSOR_MODE_128_ARGB_AX ((1 << 5) | CURSOR_MODE_128_32B_AX)
 #define   CURSOR_MODE_256_ARGB_AX ((1 << 5) | CURSOR_MODE_256_32B_AX)
 #define   CURSOR_MODE_64_ARGB_AX ((1 << 5) | CURSOR_MODE_64_32B_AX)
-#define   MCURSOR_PIPE_SELECT	(1 << 28)
-#define   MCURSOR_PIPE_A	0x00
-#define   MCURSOR_PIPE_B	(1 << 28)
+#define   MCURSOR_PIPE_SELECT(pipe)	((pipe) << 28)
 #define   MCURSOR_GAMMA_ENABLE  (1 << 26)
 #define   CURSOR_ROTATE_180	(1<<15)
 #define   CURSOR_TRICKLE_FEED_DISABLE	(1 << 14)
@@ -5449,7 +5449,9 @@ enum {
 #define   CURSOR_POS_SIGN       0x8000
 #define   CURSOR_X_SHIFT        0
 #define   CURSOR_Y_SHIFT        16
-#define CURSIZE			_MMIO(0x700a0)
+#define CURSIZE			_MMIO(0x700a0) /* 845/865 */
+#define _CUR_FBC_CTL_A		0x700a0 /* ivb+ */
+#define   CUR_FBC_CTL_EN	(1 << 31)
 #define _CURBCNTR		0x700c0
 #define _CURBBASE		0x700c4
 #define _CURBPOS		0x700c8
@@ -5465,6 +5467,7 @@ enum {
 #define CURCNTR(pipe) _CURSOR2(pipe, _CURACNTR)
 #define CURBASE(pipe) _CURSOR2(pipe, _CURABASE)
 #define CURPOS(pipe) _CURSOR2(pipe, _CURAPOS)
+#define CUR_FBC_CTL(pipe) _CURSOR2(pipe, _CUR_FBC_CTL_A)
 
 #define CURSOR_A_OFFSET 0x70080
 #define CURSOR_B_OFFSET 0x700c0
@@ -5497,8 +5500,7 @@ enum {
 #define   DISPPLANE_PIPE_CSC_ENABLE		(1<<24)
 #define   DISPPLANE_SEL_PIPE_SHIFT		24
 #define   DISPPLANE_SEL_PIPE_MASK		(3<<DISPPLANE_SEL_PIPE_SHIFT)
-#define   DISPPLANE_SEL_PIPE_A			0
-#define   DISPPLANE_SEL_PIPE_B			(1<<DISPPLANE_SEL_PIPE_SHIFT)
+#define   DISPPLANE_SEL_PIPE(pipe)		((pipe)<<DISPPLANE_SEL_PIPE_SHIFT)
 #define   DISPPLANE_SRC_KEY_ENABLE		(1<<22)
 #define   DISPPLANE_SRC_KEY_DISABLE		0
 #define   DISPPLANE_LINE_DOUBLE			(1<<20)
@@ -7790,13 +7792,6 @@ enum {
 #define  SKL_FUSE_PG1_DIST_STATUS              (1<<26)
 #define  SKL_FUSE_PG2_DIST_STATUS              (1<<25)
 
-/* Decoupled MMIO register pair for kernel driver */
-#define GEN9_DECOUPLED_REG0_DW0			_MMIO(0xF00)
-#define GEN9_DECOUPLED_REG0_DW1			_MMIO(0xF04)
-#define GEN9_DECOUPLED_DW1_GO			(1<<31)
-#define GEN9_DECOUPLED_PD_SHIFT			28
-#define GEN9_DECOUPLED_OP_SHIFT			24
-
 /* Per-pipe DDI Function Control */
 #define _TRANS_DDI_FUNC_CTL_A		0x60400
 #define _TRANS_DDI_FUNC_CTL_B		0x61400
@@ -8276,7 +8271,7 @@ enum {
 
 /* MIPI DSI registers */
 
-#define _MIPI_PORT(port, a, c)	((port) ? c : a)	/* ports A and C only */
+#define _MIPI_PORT(port, a, c)	(((port) == PORT_A) ? a : c)	/* ports A and C only */
 #define _MMIO_MIPI(port, a, c)	_MMIO(_MIPI_PORT(port, a, c))
 
 #define MIPIO_TXESC_CLK_DIV1			_MMIO(0x160004)
diff --git a/drivers/gpu/drm/i915/i915_sw_fence.c b/drivers/gpu/drm/i915/i915_sw_fence.c
index a277f8eb7beb..474d23c0c0ce 100644
--- a/drivers/gpu/drm/i915/i915_sw_fence.c
+++ b/drivers/gpu/drm/i915/i915_sw_fence.c
@@ -12,6 +12,7 @@
 #include <linux/reservation.h>
 
 #include "i915_sw_fence.h"
+#include "i915_selftest.h"
 
 #define I915_SW_FENCE_FLAG_ALLOC BIT(3) /* after WQ_FLAG_* for safety */
 
@@ -120,34 +121,6 @@ void i915_sw_fence_fini(struct i915_sw_fence *fence)
 }
 #endif
 
-static void i915_sw_fence_release(struct kref *kref)
-{
-	struct i915_sw_fence *fence = container_of(kref, typeof(*fence), kref);
-
-	WARN_ON(atomic_read(&fence->pending) > 0);
-	debug_fence_destroy(fence);
-
-	if (fence->flags & I915_SW_FENCE_MASK) {
-		__i915_sw_fence_notify(fence, FENCE_FREE);
-	} else {
-		i915_sw_fence_fini(fence);
-		kfree(fence);
-	}
-}
-
-static void i915_sw_fence_put(struct i915_sw_fence *fence)
-{
-	debug_fence_assert(fence);
-	kref_put(&fence->kref, i915_sw_fence_release);
-}
-
-static struct i915_sw_fence *i915_sw_fence_get(struct i915_sw_fence *fence)
-{
-	debug_fence_assert(fence);
-	kref_get(&fence->kref);
-	return fence;
-}
-
 static void __i915_sw_fence_wake_up_all(struct i915_sw_fence *fence,
 					struct list_head *continuation)
 {
@@ -202,13 +175,15 @@ static void __i915_sw_fence_complete(struct i915_sw_fence *fence,
 
 	debug_fence_set_state(fence, DEBUG_FENCE_IDLE, DEBUG_FENCE_NOTIFY);
 
-	if (fence->flags & I915_SW_FENCE_MASK &&
-	    __i915_sw_fence_notify(fence, FENCE_COMPLETE) != NOTIFY_DONE)
+	if (__i915_sw_fence_notify(fence, FENCE_COMPLETE) != NOTIFY_DONE)
 		return;
 
 	debug_fence_set_state(fence, DEBUG_FENCE_NOTIFY, DEBUG_FENCE_IDLE);
 
 	__i915_sw_fence_wake_up_all(fence, continuation);
+
+	debug_fence_destroy(fence);
+	__i915_sw_fence_notify(fence, FENCE_FREE);
 }
 
 static void i915_sw_fence_complete(struct i915_sw_fence *fence)
@@ -232,33 +207,26 @@ void __i915_sw_fence_init(struct i915_sw_fence *fence,
 			  const char *name,
 			  struct lock_class_key *key)
 {
-	BUG_ON((unsigned long)fn & ~I915_SW_FENCE_MASK);
+	BUG_ON(!fn || (unsigned long)fn & ~I915_SW_FENCE_MASK);
 
 	debug_fence_init(fence);
 
 	__init_waitqueue_head(&fence->wait, name, key);
-	kref_init(&fence->kref);
 	atomic_set(&fence->pending, 1);
 	fence->flags = (unsigned long)fn;
 }
 
-static void __i915_sw_fence_commit(struct i915_sw_fence *fence)
-{
-	i915_sw_fence_complete(fence);
-	i915_sw_fence_put(fence);
-}
-
 void i915_sw_fence_commit(struct i915_sw_fence *fence)
 {
 	debug_fence_activate(fence);
-	__i915_sw_fence_commit(fence);
+	i915_sw_fence_complete(fence);
 }
 
 static int i915_sw_fence_wake(wait_queue_t *wq, unsigned mode, int flags, void *key)
 {
 	list_del(&wq->task_list);
 	__i915_sw_fence_complete(wq->private, key);
-	i915_sw_fence_put(wq->private);
+
 	if (wq->flags & I915_SW_FENCE_FLAG_ALLOC)
 		kfree(wq);
 	return 0;
@@ -307,7 +275,7 @@ static bool i915_sw_fence_check_if_after(struct i915_sw_fence *fence,
 	unsigned long flags;
 	bool err;
 
-	if (!IS_ENABLED(CONFIG_I915_SW_FENCE_CHECK_DAG))
+	if (!IS_ENABLED(CONFIG_DRM_I915_SW_FENCE_CHECK_DAG))
 		return false;
 
 	spin_lock_irqsave(&i915_sw_fence_lock, flags);
@@ -353,7 +321,7 @@ static int __i915_sw_fence_await_sw_fence(struct i915_sw_fence *fence,
 	INIT_LIST_HEAD(&wq->task_list);
 	wq->flags = pending;
 	wq->func = i915_sw_fence_wake;
-	wq->private = i915_sw_fence_get(fence);
+	wq->private = fence;
 
 	i915_sw_fence_await(fence);
 
@@ -402,7 +370,7 @@ static void timer_i915_sw_fence_wake(unsigned long data)
 	dma_fence_put(cb->dma);
 	cb->dma = NULL;
 
-	__i915_sw_fence_commit(cb->fence);
+	i915_sw_fence_complete(cb->fence);
 	cb->timer.function = NULL;
 }
 
@@ -413,7 +381,7 @@ static void dma_i915_sw_fence_wake(struct dma_fence *dma,
 
 	del_timer_sync(&cb->timer);
 	if (cb->timer.function)
-		__i915_sw_fence_commit(cb->fence);
+		i915_sw_fence_complete(cb->fence);
 	dma_fence_put(cb->dma);
 
 	kfree(cb);
@@ -440,7 +408,7 @@ int i915_sw_fence_await_dma_fence(struct i915_sw_fence *fence,
 		return dma_fence_wait(dma, false);
 	}
 
-	cb->fence = i915_sw_fence_get(fence);
+	cb->fence = fence;
 	i915_sw_fence_await(fence);
 
 	cb->dma = NULL;
@@ -523,3 +491,7 @@ int i915_sw_fence_await_reservation(struct i915_sw_fence *fence,
 
 	return ret;
 }
+
+#if IS_ENABLED(CONFIG_DRM_I915_SELFTEST)
+#include "selftests/i915_sw_fence.c"
+#endif
diff --git a/drivers/gpu/drm/i915/i915_sw_fence.h b/drivers/gpu/drm/i915/i915_sw_fence.h
index d31cefbbcc04..1d3b6051daaf 100644
--- a/drivers/gpu/drm/i915/i915_sw_fence.h
+++ b/drivers/gpu/drm/i915/i915_sw_fence.h
@@ -23,7 +23,6 @@ struct reservation_object;
 struct i915_sw_fence {
 	wait_queue_head_t wait;
 	unsigned long flags;
-	struct kref kref;
 	atomic_t pending;
 };
 
diff --git a/drivers/gpu/drm/i915/i915_syncmap.c b/drivers/gpu/drm/i915/i915_syncmap.c
new file mode 100644
index 000000000000..0087acf731a8
--- /dev/null
+++ b/drivers/gpu/drm/i915/i915_syncmap.c
@@ -0,0 +1,412 @@
+/*
+ * Copyright © 2017 Intel Corporation
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
+ * IN THE SOFTWARE.
+ *
+ */
+
+#include <linux/slab.h>
+
+#include "i915_syncmap.h"
+
+#include "i915_gem.h" /* GEM_BUG_ON() */
+#include "i915_selftest.h"
+
+#define SHIFT ilog2(KSYNCMAP)
+#define MASK (KSYNCMAP - 1)
+
+/*
+ * struct i915_syncmap is a layer of a radixtree that maps a u64 fence
+ * context id to the last u32 fence seqno waited upon from that context.
+ * Unlike lib/radixtree it uses a parent pointer that allows traversal back to
+ * the root. This allows us to access the whole tree via a single pointer
+ * to the most recently used layer. We expect fence contexts to be dense
+ * and most reuse to be on the same i915_gem_context but on neighbouring
+ * engines (i.e. on adjacent contexts) and reuse the same leaf, a very
+ * effective lookup cache. If the new lookup is not on the same leaf, we
+ * expect it to be on the neighbouring branch.
+ *
+ * A leaf holds an array of u32 seqno, and has height 0. The bitmap field
+ * allows us to store whether a particular seqno is valid (i.e. allows us
+ * to distinguish unset from 0).
+ *
+ * A branch holds an array of layer pointers, and has height > 0, and always
+ * has at least 2 layers (either branches or leaves) below it.
+ *
+ * For example,
+ *	for x in
+ *	  0 1 2 0x10 0x11 0x200 0x201
+ *	  0x500000 0x500001 0x503000 0x503001
+ *	  0xE<<60:
+ *		i915_syncmap_set(&sync, x, lower_32_bits(x));
+ * will build a tree like:
+ *	0xXXXXXXXXXXXXXXXX
+ *	0-> 0x0000000000XXXXXX
+ *	|   0-> 0x0000000000000XXX
+ *	|   |   0-> 0x00000000000000XX
+ *	|   |   |   0-> 0x000000000000000X 0:0, 1:1, 2:2
+ *	|   |   |   1-> 0x000000000000001X 0:10, 1:11
+ *	|   |   2-> 0x000000000000020X 0:200, 1:201
+ *	|   5-> 0x000000000050XXXX
+ *	|       0-> 0x000000000050000X 0:500000, 1:500001
+ *	|       3-> 0x000000000050300X 0:503000, 1:503001
+ *	e-> 0xe00000000000000X e:e
+ */
+
+struct i915_syncmap {
+	u64 prefix;
+	unsigned int height;
+	unsigned int bitmap;
+	struct i915_syncmap *parent;
+	/*
+	 * Following this header is an array of either seqno or child pointers:
+	 * union {
+	 *	u32 seqno[KSYNCMAP];
+	 *	struct i915_syncmap *child[KSYNCMAP];
+	 * };
+	 */
+};
+
+/**
+ * i915_syncmap_init -- initialise the #i915_syncmap
+ * @root - pointer to the #i915_syncmap
+ */
+void i915_syncmap_init(struct i915_syncmap **root)
+{
+	BUILD_BUG_ON_NOT_POWER_OF_2(KSYNCMAP);
+	BUILD_BUG_ON_NOT_POWER_OF_2(SHIFT);
+	BUILD_BUG_ON(KSYNCMAP > BITS_PER_BYTE * sizeof((*root)->bitmap));
+	*root = NULL;
+}
+
+static inline u32 *__sync_seqno(struct i915_syncmap *p)
+{
+	GEM_BUG_ON(p->height);
+	return (u32 *)(p + 1);
+}
+
+static inline struct i915_syncmap **__sync_child(struct i915_syncmap *p)
+{
+	GEM_BUG_ON(!p->height);
+	return (struct i915_syncmap **)(p + 1);
+}
+
+static inline unsigned int
+__sync_branch_idx(const struct i915_syncmap *p, u64 id)
+{
+	return (id >> p->height) & MASK;
+}
+
+static inline unsigned int
+__sync_leaf_idx(const struct i915_syncmap *p, u64 id)
+{
+	GEM_BUG_ON(p->height);
+	return id & MASK;
+}
+
+static inline u64 __sync_branch_prefix(const struct i915_syncmap *p, u64 id)
+{
+	return id >> p->height >> SHIFT;
+}
+
+static inline u64 __sync_leaf_prefix(const struct i915_syncmap *p, u64 id)
+{
+	GEM_BUG_ON(p->height);
+	return id >> SHIFT;
+}
+
+static inline bool seqno_later(u32 a, u32 b)
+{
+	return (s32)(a - b) >= 0;
+}
+
+/**
+ * i915_syncmap_is_later -- compare against the last know sync point
+ * @root - pointer to the #i915_syncmap
+ * @id - the context id (other timeline) we are synchronising to
+ * @seqno - the sequence number along the other timeline
+ *
+ * If we have already synchronised this @root timeline with another (@id) then
+ * we can omit any repeated or earlier synchronisation requests. If the two
+ * timelines are already coupled, we can also omit the dependency between the
+ * two as that is already known via the timeline.
+ *
+ * Returns true if the two timelines are already synchronised wrt to @seqno,
+ * false if not and the synchronisation must be emitted.
+ */
+bool i915_syncmap_is_later(struct i915_syncmap **root, u64 id, u32 seqno)
+{
+	struct i915_syncmap *p;
+	unsigned int idx;
+
+	p = *root;
+	if (!p)
+		return false;
+
+	if (likely(__sync_leaf_prefix(p, id) == p->prefix))
+		goto found;
+
+	/* First climb the tree back to a parent branch */
+	do {
+		p = p->parent;
+		if (!p)
+			return false;
+
+		if (__sync_branch_prefix(p, id) == p->prefix)
+			break;
+	} while (1);
+
+	/* And then descend again until we find our leaf */
+	do {
+		if (!p->height)
+			break;
+
+		p = __sync_child(p)[__sync_branch_idx(p, id)];
+		if (!p)
+			return false;
+
+		if (__sync_branch_prefix(p, id) != p->prefix)
+			return false;
+	} while (1);
+
+	*root = p;
+found:
+	idx = __sync_leaf_idx(p, id);
+	if (!(p->bitmap & BIT(idx)))
+		return false;
+
+	return seqno_later(__sync_seqno(p)[idx], seqno);
+}
+
+static struct i915_syncmap *
+__sync_alloc_leaf(struct i915_syncmap *parent, u64 id)
+{
+	struct i915_syncmap *p;
+
+	p = kmalloc(sizeof(*p) + KSYNCMAP * sizeof(u32), GFP_KERNEL);
+	if (unlikely(!p))
+		return NULL;
+
+	p->parent = parent;
+	p->height = 0;
+	p->bitmap = 0;
+	p->prefix = __sync_leaf_prefix(p, id);
+	return p;
+}
+
+static inline void __sync_set_seqno(struct i915_syncmap *p, u64 id, u32 seqno)
+{
+	unsigned int idx = __sync_leaf_idx(p, id);
+
+	p->bitmap |= BIT(idx);
+	__sync_seqno(p)[idx] = seqno;
+}
+
+static inline void __sync_set_child(struct i915_syncmap *p,
+				    unsigned int idx,
+				    struct i915_syncmap *child)
+{
+	p->bitmap |= BIT(idx);
+	__sync_child(p)[idx] = child;
+}
+
+static noinline int __sync_set(struct i915_syncmap **root, u64 id, u32 seqno)
+{
+	struct i915_syncmap *p = *root;
+	unsigned int idx;
+
+	if (!p) {
+		p = __sync_alloc_leaf(NULL, id);
+		if (unlikely(!p))
+			return -ENOMEM;
+
+		goto found;
+	}
+
+	/* Caller handled the likely cached case */
+	GEM_BUG_ON(__sync_leaf_prefix(p, id) == p->prefix);
+
+	/* Climb back up the tree until we find a common prefix */
+	do {
+		if (!p->parent)
+			break;
+
+		p = p->parent;
+
+		if (__sync_branch_prefix(p, id) == p->prefix)
+			break;
+	} while (1);
+
+	/*
+	 * No shortcut, we have to descend the tree to find the right layer
+	 * containing this fence.
+	 *
+	 * Each layer in the tree holds 16 (KSYNCMAP) pointers, either fences
+	 * or lower layers. Leaf nodes (height = 0) contain the fences, all
+	 * other nodes (height > 0) are internal layers that point to a lower
+	 * node. Each internal layer has at least 2 descendents.
+	 *
+	 * Starting at the top, we check whether the current prefix matches. If
+	 * it doesn't, we have gone past our target and need to insert a join
+	 * into the tree, and a new leaf node for the target as a descendent
+	 * of the join, as well as the original layer.
+	 *
+	 * The matching prefix means we are still following the right branch
+	 * of the tree. If it has height 0, we have found our leaf and just
+	 * need to replace the fence slot with ourselves. If the height is
+	 * not zero, our slot contains the next layer in the tree (unless
+	 * it is empty, in which case we can add ourselves as a new leaf).
+	 * As descend the tree the prefix grows (and height decreases).
+	 */
+	do {
+		struct i915_syncmap *next;
+
+		if (__sync_branch_prefix(p, id) != p->prefix) {
+			unsigned int above;
+
+			/* Insert a join above the current layer */
+			next = kzalloc(sizeof(*next) + KSYNCMAP * sizeof(next),
+				       GFP_KERNEL);
+			if (unlikely(!next))
+				return -ENOMEM;
+
+			/* Compute the height at which these two diverge */
+			above = fls64(__sync_branch_prefix(p, id) ^ p->prefix);
+			above = round_up(above, SHIFT);
+			next->height = above + p->height;
+			next->prefix = __sync_branch_prefix(next, id);
+
+			/* Insert the join into the parent */
+			if (p->parent) {
+				idx = __sync_branch_idx(p->parent, id);
+				__sync_child(p->parent)[idx] = next;
+				GEM_BUG_ON(!(p->parent->bitmap & BIT(idx)));
+			}
+			next->parent = p->parent;
+
+			/* Compute the idx of the other branch, not our id! */
+			idx = p->prefix >> (above - SHIFT) & MASK;
+			__sync_set_child(next, idx, p);
+			p->parent = next;
+
+			/* Ascend to the join */
+			p = next;
+		} else {
+			if (!p->height)
+				break;
+		}
+
+		/* Descend into the next layer */
+		GEM_BUG_ON(!p->height);
+		idx = __sync_branch_idx(p, id);
+		next = __sync_child(p)[idx];
+		if (!next) {
+			next = __sync_alloc_leaf(p, id);
+			if (unlikely(!next))
+				return -ENOMEM;
+
+			__sync_set_child(p, idx, next);
+			p = next;
+			break;
+		}
+
+		p = next;
+	} while (1);
+
+found:
+	GEM_BUG_ON(p->prefix != __sync_leaf_prefix(p, id));
+	__sync_set_seqno(p, id, seqno);
+	*root = p;
+	return 0;
+}
+
+/**
+ * i915_syncmap_set -- mark the most recent syncpoint between contexts
+ * @root - pointer to the #i915_syncmap
+ * @id - the context id (other timeline) we have synchronised to
+ * @seqno - the sequence number along the other timeline
+ *
+ * When we synchronise this @root timeline with another (@id), we also know
+ * that we have synchronized with all previous seqno along that timeline. If
+ * we then have a request to synchronise with the same seqno or older, we can
+ * omit it, see i915_syncmap_is_later()
+ *
+ * Returns 0 on success, or a negative error code.
+ */
+int i915_syncmap_set(struct i915_syncmap **root, u64 id, u32 seqno)
+{
+	struct i915_syncmap *p = *root;
+
+	/*
+	 * We expect to be called in sequence following is_later(id), which
+	 * should have preloaded the root for us.
+	 */
+	if (likely(p && __sync_leaf_prefix(p, id) == p->prefix)) {
+		__sync_set_seqno(p, id, seqno);
+		return 0;
+	}
+
+	return __sync_set(root, id, seqno);
+}
+
+static void __sync_free(struct i915_syncmap *p)
+{
+	if (p->height) {
+		unsigned int i;
+
+		while ((i = ffs(p->bitmap))) {
+			p->bitmap &= ~0u << i;
+			__sync_free(__sync_child(p)[i - 1]);
+		}
+	}
+
+	kfree(p);
+}
+
+/**
+ * i915_syncmap_free -- free all memory associated with the syncmap
+ * @root - pointer to the #i915_syncmap
+ *
+ * Either when the timeline is to be freed and we no longer need the sync
+ * point tracking, or when the fences are all known to be signaled and the
+ * sync point tracking is redundant, we can free the #i915_syncmap to recover
+ * its allocations.
+ *
+ * Will reinitialise the @root pointer so that the #i915_syncmap is ready for
+ * reuse.
+ */
+void i915_syncmap_free(struct i915_syncmap **root)
+{
+	struct i915_syncmap *p;
+
+	p = *root;
+	if (!p)
+		return;
+
+	while (p->parent)
+		p = p->parent;
+
+	__sync_free(p);
+	*root = NULL;
+}
+
+#if IS_ENABLED(CONFIG_DRM_I915_SELFTEST)
+#include "selftests/i915_syncmap.c"
+#endif
diff --git a/drivers/gpu/drm/i915/i915_syncmap.h b/drivers/gpu/drm/i915/i915_syncmap.h
new file mode 100644
index 000000000000..0653f70bee82
--- /dev/null
+++ b/drivers/gpu/drm/i915/i915_syncmap.h
@@ -0,0 +1,38 @@
+/*
+ * Copyright © 2017 Intel Corporation
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
+ * IN THE SOFTWARE.
+ *
+ */
+
+#ifndef __I915_SYNCMAP_H__
+#define __I915_SYNCMAP_H__
+
+#include <linux/types.h>
+
+struct i915_syncmap;
+#define KSYNCMAP 16 /* radix of the tree, how many slots in each layer */
+
+void i915_syncmap_init(struct i915_syncmap **root);
+int i915_syncmap_set(struct i915_syncmap **root, u64 id, u32 seqno);
+bool i915_syncmap_is_later(struct i915_syncmap **root, u64 id, u32 seqno);
+void i915_syncmap_free(struct i915_syncmap **root);
+
+#endif /* __I915_SYNCMAP_H__ */
diff --git a/drivers/gpu/drm/i915/i915_sysfs.c b/drivers/gpu/drm/i915/i915_sysfs.c
index f3fdfda5e558..1eef3fae4db3 100644
--- a/drivers/gpu/drm/i915/i915_sysfs.c
+++ b/drivers/gpu/drm/i915/i915_sysfs.c
@@ -181,13 +181,10 @@ i915_l3_write(struct file *filp, struct kobject *kobj,
 	struct drm_i915_private *dev_priv = kdev_minor_to_i915(kdev);
 	struct drm_device *dev = &dev_priv->drm;
 	struct i915_gem_context *ctx;
-	u32 *temp = NULL; /* Just here to make handling failures easy */
 	int slice = (int)(uintptr_t)attr->private;
+	u32 **remap_info;
 	int ret;
 
-	if (!HAS_HW_CONTEXTS(dev_priv))
-		return -ENXIO;
-
 	ret = l3_access_valid(dev_priv, offset);
 	if (ret)
 		return ret;
@@ -196,11 +193,12 @@ i915_l3_write(struct file *filp, struct kobject *kobj,
 	if (ret)
 		return ret;
 
-	if (!dev_priv->l3_parity.remap_info[slice]) {
-		temp = kzalloc(GEN7_L3LOG_SIZE, GFP_KERNEL);
-		if (!temp) {
-			mutex_unlock(&dev->struct_mutex);
-			return -ENOMEM;
+	remap_info = &dev_priv->l3_parity.remap_info[slice];
+	if (!*remap_info) {
+		*remap_info = kzalloc(GEN7_L3LOG_SIZE, GFP_KERNEL);
+		if (!*remap_info) {
+			ret = -ENOMEM;
+			goto out;
 		}
 	}
 
@@ -208,18 +206,18 @@ i915_l3_write(struct file *filp, struct kobject *kobj,
 	 * aren't propagated. Since I cannot find a stable way to reset the GPU
 	 * at this point it is left as a TODO.
 	*/
-	if (temp)
-		dev_priv->l3_parity.remap_info[slice] = temp;
-
-	memcpy(dev_priv->l3_parity.remap_info[slice] + (offset/4), buf, count);
+	memcpy(*remap_info + (offset/4), buf, count);
 
 	/* NB: We defer the remapping until we switch to the context */
 	list_for_each_entry(ctx, &dev_priv->context_list, link)
 		ctx->remap_slice |= (1<<slice);
 
+	ret = count;
+
+out:
 	mutex_unlock(&dev->struct_mutex);
 
-	return count;
+	return ret;
 }
 
 static struct bin_attribute dpf_attrs = {
diff --git a/drivers/gpu/drm/i915/i915_trace.h b/drivers/gpu/drm/i915/i915_trace.h
index 66404c5aee82..b24a83d43559 100644
--- a/drivers/gpu/drm/i915/i915_trace.h
+++ b/drivers/gpu/drm/i915/i915_trace.h
@@ -89,6 +89,55 @@ TRACE_EVENT(intel_memory_cxsr,
 		      __entry->frame[PIPE_C], __entry->scanline[PIPE_C])
 );
 
+TRACE_EVENT(g4x_wm,
+	    TP_PROTO(struct intel_crtc *crtc, const struct g4x_wm_values *wm),
+	    TP_ARGS(crtc, wm),
+
+	    TP_STRUCT__entry(
+			     __field(enum pipe, pipe)
+			     __field(u32, frame)
+			     __field(u32, scanline)
+			     __field(u16, primary)
+			     __field(u16, sprite)
+			     __field(u16, cursor)
+			     __field(u16, sr_plane)
+			     __field(u16, sr_cursor)
+			     __field(u16, sr_fbc)
+			     __field(u16, hpll_plane)
+			     __field(u16, hpll_cursor)
+			     __field(u16, hpll_fbc)
+			     __field(bool, cxsr)
+			     __field(bool, hpll)
+			     __field(bool, fbc)
+			     ),
+
+	    TP_fast_assign(
+			   __entry->pipe = crtc->pipe;
+			   __entry->frame = crtc->base.dev->driver->get_vblank_counter(crtc->base.dev,
+										       crtc->pipe);
+			   __entry->scanline = intel_get_crtc_scanline(crtc);
+			   __entry->primary = wm->pipe[crtc->pipe].plane[PLANE_PRIMARY];
+			   __entry->sprite = wm->pipe[crtc->pipe].plane[PLANE_SPRITE0];
+			   __entry->cursor = wm->pipe[crtc->pipe].plane[PLANE_CURSOR];
+			   __entry->sr_plane = wm->sr.plane;
+			   __entry->sr_cursor = wm->sr.cursor;
+			   __entry->sr_fbc = wm->sr.fbc;
+			   __entry->hpll_plane = wm->hpll.plane;
+			   __entry->hpll_cursor = wm->hpll.cursor;
+			   __entry->hpll_fbc = wm->hpll.fbc;
+			   __entry->cxsr = wm->cxsr;
+			   __entry->hpll = wm->hpll_en;
+			   __entry->fbc = wm->fbc_en;
+			   ),
+
+	    TP_printk("pipe %c, frame=%u, scanline=%u, wm %d/%d/%d, sr %s/%d/%d/%d, hpll %s/%d/%d/%d, fbc %s",
+		      pipe_name(__entry->pipe), __entry->frame, __entry->scanline,
+		      __entry->primary, __entry->sprite, __entry->cursor,
+		      yesno(__entry->cxsr), __entry->sr_plane, __entry->sr_cursor, __entry->sr_fbc,
+		      yesno(__entry->hpll), __entry->hpll_plane, __entry->hpll_cursor, __entry->hpll_fbc,
+		      yesno(__entry->fbc))
+);
+
 TRACE_EVENT(vlv_wm,
 	    TP_PROTO(struct intel_crtc *crtc, const struct vlv_wm_values *wm),
 	    TP_ARGS(crtc, wm),
diff --git a/drivers/gpu/drm/i915/i915_utils.h b/drivers/gpu/drm/i915/i915_utils.h
index c5455d36b617..16ecd1ab108d 100644
--- a/drivers/gpu/drm/i915/i915_utils.h
+++ b/drivers/gpu/drm/i915/i915_utils.h
@@ -70,20 +70,27 @@
 #define overflows_type(x, T) \
 	(sizeof(x) > sizeof(T) && (x) >> (sizeof(T) * BITS_PER_BYTE))
 
-#define ptr_mask_bits(ptr) ({						\
+#define ptr_mask_bits(ptr, n) ({					\
 	unsigned long __v = (unsigned long)(ptr);			\
-	(typeof(ptr))(__v & PAGE_MASK);					\
+	(typeof(ptr))(__v & -BIT(n));					\
 })
 
-#define ptr_unpack_bits(ptr, bits) ({					\
+#define ptr_unmask_bits(ptr, n) ((unsigned long)(ptr) & (BIT(n) - 1))
+
+#define ptr_unpack_bits(ptr, bits, n) ({				\
 	unsigned long __v = (unsigned long)(ptr);			\
-	(bits) = __v & ~PAGE_MASK;					\
-	(typeof(ptr))(__v & PAGE_MASK);					\
+	*(bits) = __v & (BIT(n) - 1);					\
+	(typeof(ptr))(__v & -BIT(n));					\
 })
 
-#define ptr_pack_bits(ptr, bits)					\
+#define ptr_pack_bits(ptr, bits, n)					\
 	((typeof(ptr))((unsigned long)(ptr) | (bits)))
 
+#define page_mask_bits(ptr) ptr_mask_bits(ptr, PAGE_SHIFT)
+#define page_unmask_bits(ptr) ptr_unmask_bits(ptr, PAGE_SHIFT)
+#define page_pack_bits(ptr, bits) ptr_pack_bits(ptr, bits, PAGE_SHIFT)
+#define page_unpack_bits(ptr, bits) ptr_unpack_bits(ptr, bits, PAGE_SHIFT)
+
 #define ptr_offset(ptr, member) offsetof(typeof(*(ptr)), member)
 
 #define fetch_and_zero(ptr) ({						\
@@ -92,4 +99,19 @@
 	__T;								\
 })
 
+#define __mask_next_bit(mask) ({					\
+	int __idx = ffs(mask) - 1;					\
+	mask &= ~BIT(__idx);						\
+	__idx;								\
+})
+
+#include <linux/list.h>
+
+static inline void __list_del_many(struct list_head *head,
+				   struct list_head *first)
+{
+	first->prev = head;
+	WRITE_ONCE(head->next, first);
+}
+
 #endif /* !__I915_UTILS_H */
diff --git a/drivers/gpu/drm/i915/intel_atomic.c b/drivers/gpu/drm/i915/intel_atomic.c
index 50fb1f76cc5f..d791b3ef89b5 100644
--- a/drivers/gpu/drm/i915/intel_atomic.c
+++ b/drivers/gpu/drm/i915/intel_atomic.c
@@ -36,44 +36,121 @@
 #include "intel_drv.h"
 
 /**
- * intel_connector_atomic_get_property - fetch connector property value
- * @connector: connector to fetch property for
- * @state: state containing the property value
- * @property: property to look up
- * @val: pointer to write property value into
+ * intel_digital_connector_atomic_get_property - hook for connector->atomic_get_property.
+ * @connector: Connector to get the property for.
+ * @state: Connector state to retrieve the property from.
+ * @property: Property to retrieve.
+ * @val: Return value for the property.
  *
- * The DRM core does not store shadow copies of properties for
- * atomic-capable drivers.  This entrypoint is used to fetch
- * the current value of a driver-specific connector property.
+ * Returns the atomic property value for a digital connector.
  */
-int
-intel_connector_atomic_get_property(struct drm_connector *connector,
-				    const struct drm_connector_state *state,
-				    struct drm_property *property,
-				    uint64_t *val)
+int intel_digital_connector_atomic_get_property(struct drm_connector *connector,
+						const struct drm_connector_state *state,
+						struct drm_property *property,
+						uint64_t *val)
 {
-	int i;
+	struct drm_device *dev = connector->dev;
+	struct drm_i915_private *dev_priv = to_i915(dev);
+	struct intel_digital_connector_state *intel_conn_state =
+		to_intel_digital_connector_state(state);
+
+	if (property == dev_priv->force_audio_property)
+		*val = intel_conn_state->force_audio;
+	else if (property == dev_priv->broadcast_rgb_property)
+		*val = intel_conn_state->broadcast_rgb;
+	else {
+		DRM_DEBUG_ATOMIC("Unknown property %s\n", property->name);
+		return -EINVAL;
+	}
 
-	/*
-	 * TODO: We only have atomic modeset for planes at the moment, so the
-	 * crtc/connector code isn't quite ready yet.  Until it's ready,
-	 * continue to look up all property values in the DRM's shadow copy
-	 * in obj->properties->values[].
-	 *
-	 * When the crtc/connector state work matures, this function should
-	 * be updated to read the values out of the state structure instead.
-	 */
-	for (i = 0; i < connector->base.properties->count; i++) {
-		if (connector->base.properties->properties[i] == property) {
-			*val = connector->base.properties->values[i];
-			return 0;
-		}
+	return 0;
+}
+
+/**
+ * intel_digital_connector_atomic_set_property - hook for connector->atomic_set_property.
+ * @connector: Connector to set the property for.
+ * @state: Connector state to set the property on.
+ * @property: Property to set.
+ * @val: New value for the property.
+ *
+ * Sets the atomic property value for a digital connector.
+ */
+int intel_digital_connector_atomic_set_property(struct drm_connector *connector,
+						struct drm_connector_state *state,
+						struct drm_property *property,
+						uint64_t val)
+{
+	struct drm_device *dev = connector->dev;
+	struct drm_i915_private *dev_priv = to_i915(dev);
+	struct intel_digital_connector_state *intel_conn_state =
+		to_intel_digital_connector_state(state);
+
+	if (property == dev_priv->force_audio_property) {
+		intel_conn_state->force_audio = val;
+		return 0;
 	}
 
+	if (property == dev_priv->broadcast_rgb_property) {
+		intel_conn_state->broadcast_rgb = val;
+		return 0;
+	}
+
+	DRM_DEBUG_ATOMIC("Unknown property %s\n", property->name);
 	return -EINVAL;
 }
 
-/*
+int intel_digital_connector_atomic_check(struct drm_connector *conn,
+					 struct drm_connector_state *new_state)
+{
+	struct intel_digital_connector_state *new_conn_state =
+		to_intel_digital_connector_state(new_state);
+	struct drm_connector_state *old_state =
+		drm_atomic_get_old_connector_state(new_state->state, conn);
+	struct intel_digital_connector_state *old_conn_state =
+		to_intel_digital_connector_state(old_state);
+	struct drm_crtc_state *crtc_state;
+
+	if (!new_state->crtc)
+		return 0;
+
+	crtc_state = drm_atomic_get_new_crtc_state(new_state->state, new_state->crtc);
+
+	/*
+	 * These properties are handled by fastset, and might not end
+	 * up in a modeset.
+	 */
+	if (new_conn_state->force_audio != old_conn_state->force_audio ||
+	    new_conn_state->broadcast_rgb != old_conn_state->broadcast_rgb ||
+	    new_conn_state->base.picture_aspect_ratio != old_conn_state->base.picture_aspect_ratio ||
+	    new_conn_state->base.scaling_mode != old_conn_state->base.scaling_mode)
+		crtc_state->mode_changed = true;
+
+	return 0;
+}
+
+/**
+ * intel_digital_connector_duplicate_state - duplicate connector state
+ * @connector: digital connector
+ *
+ * Allocates and returns a copy of the connector state (both common and
+ * digital connector specific) for the specified connector.
+ *
+ * Returns: The newly allocated connector state, or NULL on failure.
+ */
+struct drm_connector_state *
+intel_digital_connector_duplicate_state(struct drm_connector *connector)
+{
+	struct intel_digital_connector_state *state;
+
+	state = kmemdup(connector->state, sizeof(*state), GFP_KERNEL);
+	if (!state)
+		return NULL;
+
+	__drm_atomic_helper_connector_duplicate_state(connector, &state->base);
+	return &state->base;
+}
+
+/**
  * intel_crtc_duplicate_state - duplicate crtc state
  * @crtc: drm crtc
  *
diff --git a/drivers/gpu/drm/i915/intel_atomic_plane.c b/drivers/gpu/drm/i915/intel_atomic_plane.c
index a40c82c65450..4325cb0a04f5 100644
--- a/drivers/gpu/drm/i915/intel_atomic_plane.c
+++ b/drivers/gpu/drm/i915/intel_atomic_plane.c
@@ -102,23 +102,7 @@ void
 intel_plane_destroy_state(struct drm_plane *plane,
 			  struct drm_plane_state *state)
 {
-	struct i915_vma *vma;
-
-	vma = fetch_and_zero(&to_intel_plane_state(state)->vma);
-
-	/*
-	 * FIXME: Normally intel_cleanup_plane_fb handles destruction of vma.
-	 * We currently don't clear all planes during driver unload, so we have
-	 * to be able to unpin vma here for now.
-	 *
-	 * Normally this can only happen during unload when kmscon is disabled
-	 * and userspace doesn't attempt to set a framebuffer at all.
-	 */
-	if (vma) {
-		mutex_lock(&plane->dev->struct_mutex);
-		intel_unpin_fb_vma(vma);
-		mutex_unlock(&plane->dev->struct_mutex);
-	}
+	WARN_ON(to_intel_plane_state(state)->vma);
 
 	drm_atomic_helper_plane_destroy_state(plane, state);
 }
@@ -185,7 +169,7 @@ int intel_plane_atomic_check_with_state(struct intel_crtc_state *crtc_state,
 	}
 
 	intel_state->base.visible = false;
-	ret = intel_plane->check_plane(plane, crtc_state, intel_state);
+	ret = intel_plane->check_plane(intel_plane, crtc_state, intel_state);
 	if (ret)
 		return ret;
 
@@ -235,14 +219,14 @@ static void intel_plane_atomic_update(struct drm_plane *plane,
 		trace_intel_update_plane(plane,
 					 to_intel_crtc(crtc));
 
-		intel_plane->update_plane(plane,
+		intel_plane->update_plane(intel_plane,
 					  to_intel_crtc_state(crtc->state),
 					  intel_state);
 	} else {
 		trace_intel_disable_plane(plane,
 					  to_intel_crtc(crtc));
 
-		intel_plane->disable_plane(plane, crtc);
+		intel_plane->disable_plane(intel_plane, to_intel_crtc(crtc));
 	}
 }
 
diff --git a/drivers/gpu/drm/i915/intel_audio.c b/drivers/gpu/drm/i915/intel_audio.c
index 52c207e81f41..d805b6e6fe71 100644
--- a/drivers/gpu/drm/i915/intel_audio.c
+++ b/drivers/gpu/drm/i915/intel_audio.c
@@ -632,20 +632,9 @@ void intel_audio_codec_enable(struct intel_encoder *intel_encoder,
 						 (int) port, (int) pipe);
 	}
 
-	switch (intel_encoder->type) {
-	case INTEL_OUTPUT_HDMI:
-		intel_lpe_audio_notify(dev_priv, connector->eld, port, pipe,
-				       crtc_state->port_clock,
-				       false, 0);
-		break;
-	case INTEL_OUTPUT_DP:
-		intel_lpe_audio_notify(dev_priv, connector->eld, port, pipe,
-				       adjusted_mode->crtc_clock,
-				       true, crtc_state->port_clock);
-		break;
-	default:
-		break;
-	}
+	intel_lpe_audio_notify(dev_priv, pipe, port, connector->eld,
+			       crtc_state->port_clock,
+			       intel_encoder->type == INTEL_OUTPUT_DP);
 }
 
 /**
@@ -680,7 +669,7 @@ void intel_audio_codec_disable(struct intel_encoder *intel_encoder)
 						 (int) port, (int) pipe);
 	}
 
-	intel_lpe_audio_notify(dev_priv, NULL, port, pipe, 0, false, 0);
+	intel_lpe_audio_notify(dev_priv, pipe, port, NULL, 0, false);
 }
 
 /**
diff --git a/drivers/gpu/drm/i915/intel_breadcrumbs.c b/drivers/gpu/drm/i915/intel_breadcrumbs.c
index 9ccbf26124c6..183afcb036aa 100644
--- a/drivers/gpu/drm/i915/intel_breadcrumbs.c
+++ b/drivers/gpu/drm/i915/intel_breadcrumbs.c
@@ -64,10 +64,12 @@ static unsigned long wait_timeout(void)
 
 static noinline void missed_breadcrumb(struct intel_engine_cs *engine)
 {
-	DRM_DEBUG_DRIVER("%s missed breadcrumb at %pF, irq posted? %s\n",
+	DRM_DEBUG_DRIVER("%s missed breadcrumb at %pF, irq posted? %s, current seqno=%x, last=%x\n",
 			 engine->name, __builtin_return_address(0),
 			 yesno(test_bit(ENGINE_IRQ_BREADCRUMB,
-					&engine->irq_posted)));
+					&engine->irq_posted)),
+			 intel_engine_get_seqno(engine),
+			 intel_engine_last_submit(engine));
 
 	set_bit(engine->id, &engine->i915->gpu_error.missed_irq_rings);
 }
@@ -665,12 +667,13 @@ static int intel_breadcrumbs_signaler(void *arg)
 	return 0;
 }
 
-void intel_engine_enable_signaling(struct drm_i915_gem_request *request)
+void intel_engine_enable_signaling(struct drm_i915_gem_request *request,
+				   bool wakeup)
 {
 	struct intel_engine_cs *engine = request->engine;
 	struct intel_breadcrumbs *b = &engine->breadcrumbs;
 	struct rb_node *parent, **p;
-	bool first, wakeup;
+	bool first;
 	u32 seqno;
 
 	/* Note that we may be called from an interrupt handler on another
@@ -703,7 +706,7 @@ void intel_engine_enable_signaling(struct drm_i915_gem_request *request)
 	 * If we are the oldest waiter, enable the irq (after which we
 	 * must double check that the seqno did not complete).
 	 */
-	wakeup = __intel_engine_add_wait(engine, &request->signaling.wait);
+	wakeup &= __intel_engine_add_wait(engine, &request->signaling.wait);
 
 	/* Now insert ourselves into the retirement ordered list of signals
 	 * on this engine. We track the oldest seqno as that will be the
diff --git a/drivers/gpu/drm/i915/intel_cdclk.c b/drivers/gpu/drm/i915/intel_cdclk.c
index dd3ad52b7dfe..29792972d55d 100644
--- a/drivers/gpu/drm/i915/intel_cdclk.c
+++ b/drivers/gpu/drm/i915/intel_cdclk.c
@@ -1071,9 +1071,15 @@ static int bxt_calc_cdclk(int max_pixclk)
 
 static int glk_calc_cdclk(int max_pixclk)
 {
-	if (max_pixclk > 2 * 158400)
+	/*
+	 * FIXME: Avoid using a pixel clock that is more than 99% of the cdclk
+	 * as a temporary workaround. Use a higher cdclk instead. (Note that
+	 * intel_compute_max_dotclk() limits the max pixel clock to 99% of max
+	 * cdclk.)
+	 */
+	if (max_pixclk > DIV_ROUND_UP(2 * 158400 * 99, 100))
 		return 316800;
-	else if (max_pixclk > 2 * 79200)
+	else if (max_pixclk > DIV_ROUND_UP(2 * 79200 * 99, 100))
 		return 158400;
 	else
 		return 79200;
@@ -1664,7 +1670,11 @@ static int intel_compute_max_dotclk(struct drm_i915_private *dev_priv)
 	int max_cdclk_freq = dev_priv->max_cdclk_freq;
 
 	if (IS_GEMINILAKE(dev_priv))
-		return 2 * max_cdclk_freq;
+		/*
+		 * FIXME: Limiting to 99% as a temporary workaround. See
+		 * glk_calc_cdclk() for details.
+		 */
+		return 2 * max_cdclk_freq * 99 / 100;
 	else if (INTEL_INFO(dev_priv)->gen >= 9 ||
 		 IS_HASWELL(dev_priv) || IS_BROADWELL(dev_priv))
 		return max_cdclk_freq;
@@ -1798,13 +1808,11 @@ static int g4x_hrawclk(struct drm_i915_private *dev_priv)
 	case CLKCFG_FSB_800:
 		return 200000;
 	case CLKCFG_FSB_1067:
+	case CLKCFG_FSB_1067_ALT:
 		return 266667;
 	case CLKCFG_FSB_1333:
+	case CLKCFG_FSB_1333_ALT:
 		return 333333;
-	/* these two are just a guess; one of them might be right */
-	case CLKCFG_FSB_1600:
-	case CLKCFG_FSB_1600_ALT:
-		return 400000;
 	default:
 		return 133333;
 	}
diff --git a/drivers/gpu/drm/i915/intel_crt.c b/drivers/gpu/drm/i915/intel_crt.c
index 2797bf37c3ac..84a1f5e85153 100644
--- a/drivers/gpu/drm/i915/intel_crt.c
+++ b/drivers/gpu/drm/i915/intel_crt.c
@@ -777,13 +777,6 @@ out:
 	return ret;
 }
 
-static int intel_crt_set_property(struct drm_connector *connector,
-				  struct drm_property *property,
-				  uint64_t value)
-{
-	return 0;
-}
-
 void intel_crt_reset(struct drm_encoder *encoder)
 {
 	struct drm_i915_private *dev_priv = to_i915(encoder->dev);
@@ -814,10 +807,9 @@ static const struct drm_connector_funcs intel_crt_connector_funcs = {
 	.late_register = intel_connector_register,
 	.early_unregister = intel_connector_unregister,
 	.destroy = intel_crt_destroy,
-	.set_property = intel_crt_set_property,
+	.set_property = drm_atomic_helper_connector_set_property,
 	.atomic_destroy_state = drm_atomic_helper_connector_destroy_state,
 	.atomic_duplicate_state = drm_atomic_helper_connector_duplicate_state,
-	.atomic_get_property = intel_connector_atomic_get_property,
 };
 
 static const struct drm_connector_helper_funcs intel_crt_connector_helper_funcs = {
diff --git a/drivers/gpu/drm/i915/intel_device_info.c b/drivers/gpu/drm/i915/intel_device_info.c
index 7d01dfe7faac..3718341662c2 100644
--- a/drivers/gpu/drm/i915/intel_device_info.c
+++ b/drivers/gpu/drm/i915/intel_device_info.c
@@ -337,7 +337,7 @@ void intel_device_info_runtime_init(struct drm_i915_private *dev_priv)
 	} else if (IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv)) {
 		for_each_pipe(dev_priv, pipe)
 			info->num_sprites[pipe] = 2;
-	} else if (INTEL_GEN(dev_priv) >= 5) {
+	} else if (INTEL_GEN(dev_priv) >= 5 || IS_G4X(dev_priv)) {
 		for_each_pipe(dev_priv, pipe)
 			info->num_sprites[pipe] = 1;
 	}
diff --git a/drivers/gpu/drm/i915/intel_display.c b/drivers/gpu/drm/i915/intel_display.c
index e6e26705c138..85da43e53a58 100644
--- a/drivers/gpu/drm/i915/intel_display.c
+++ b/drivers/gpu/drm/i915/intel_display.c
@@ -1277,7 +1277,7 @@ static void assert_sprites_disabled(struct drm_i915_private *dev_priv,
 		I915_STATE_WARN(val & SPRITE_ENABLE,
 		     "sprite %c assertion failure, should be off on pipe %c but is still active\n",
 		     plane_name(pipe), pipe_name(pipe));
-	} else if (INTEL_GEN(dev_priv) >= 5) {
+	} else if (INTEL_GEN(dev_priv) >= 5 || IS_G4X(dev_priv)) {
 		u32 val = I915_READ(DVSCNTR(pipe));
 		I915_STATE_WARN(val & DVS_ENABLE,
 		     "sprite %c assertion failure, should be off on pipe %c but is still active\n",
@@ -2084,6 +2084,18 @@ intel_fill_fb_ggtt_view(struct i915_ggtt_view *view,
 	}
 }
 
+static unsigned int intel_cursor_alignment(const struct drm_i915_private *dev_priv)
+{
+	if (IS_I830(dev_priv))
+		return 16 * 1024;
+	else if (IS_I85X(dev_priv))
+		return 256;
+	else if (IS_I845G(dev_priv) || IS_I865G(dev_priv))
+		return 32;
+	else
+		return 4 * 1024;
+}
+
 static unsigned int intel_linear_alignment(const struct drm_i915_private *dev_priv)
 {
 	if (INTEL_INFO(dev_priv)->gen >= 9)
@@ -2386,11 +2398,17 @@ u32 intel_compute_tile_offset(int *x, int *y,
 			      const struct intel_plane_state *state,
 			      int plane)
 {
-	const struct drm_i915_private *dev_priv = to_i915(state->base.plane->dev);
+	struct intel_plane *intel_plane = to_intel_plane(state->base.plane);
+	struct drm_i915_private *dev_priv = to_i915(intel_plane->base.dev);
 	const struct drm_framebuffer *fb = state->base.fb;
 	unsigned int rotation = state->base.rotation;
 	int pitch = intel_fb_pitch(fb, plane, rotation);
-	u32 alignment = intel_surf_alignment(fb, plane);
+	u32 alignment;
+
+	if (intel_plane->id == PLANE_CURSOR)
+		alignment = intel_cursor_alignment(dev_priv);
+	else
+		alignment = intel_surf_alignment(fb, plane);
 
 	return _intel_compute_tile_offset(dev_priv, x, y, fb, plane, pitch,
 					  rotation, alignment);
@@ -2750,7 +2768,7 @@ intel_find_initial_plane_obj(struct intel_crtc *intel_crtc,
 				false);
 	intel_pre_disable_primary_noatomic(&intel_crtc->base);
 	trace_intel_disable_plane(primary, intel_crtc);
-	intel_plane->disable_plane(primary, &intel_crtc->base);
+	intel_plane->disable_plane(intel_plane, intel_crtc);
 
 	return;
 
@@ -2981,10 +2999,8 @@ static u32 i9xx_plane_ctl(const struct intel_crtc_state *crtc_state,
 	if (IS_HASWELL(dev_priv) || IS_BROADWELL(dev_priv))
 		dspcntr |= DISPPLANE_PIPE_CSC_ENABLE;
 
-	if (INTEL_GEN(dev_priv) < 4) {
-		if (crtc->pipe == PIPE_B)
-			dspcntr |= DISPPLANE_SEL_PIPE_B;
-	}
+	if (INTEL_GEN(dev_priv) < 4)
+		dspcntr |= DISPPLANE_SEL_PIPE(crtc->pipe);
 
 	switch (fb->format->format) {
 	case DRM_FORMAT_C8:
@@ -3063,14 +3079,14 @@ int i9xx_check_plane_surface(struct intel_plane_state *plane_state)
 	return 0;
 }
 
-static void i9xx_update_primary_plane(struct drm_plane *primary,
+static void i9xx_update_primary_plane(struct intel_plane *primary,
 				      const struct intel_crtc_state *crtc_state,
 				      const struct intel_plane_state *plane_state)
 {
-	struct drm_i915_private *dev_priv = to_i915(primary->dev);
-	struct intel_crtc *intel_crtc = to_intel_crtc(crtc_state->base.crtc);
-	struct drm_framebuffer *fb = plane_state->base.fb;
-	int plane = intel_crtc->plane;
+	struct drm_i915_private *dev_priv = to_i915(primary->base.dev);
+	struct intel_crtc *crtc = to_intel_crtc(crtc_state->base.crtc);
+	const struct drm_framebuffer *fb = plane_state->base.fb;
+	enum plane plane = primary->plane;
 	u32 linear_offset;
 	u32 dspcntr = plane_state->ctl;
 	i915_reg_t reg = DSPCNTR(plane);
@@ -3081,12 +3097,12 @@ static void i9xx_update_primary_plane(struct drm_plane *primary,
 	linear_offset = intel_fb_xy_to_linear(x, y, plane_state, 0);
 
 	if (INTEL_GEN(dev_priv) >= 4)
-		intel_crtc->dspaddr_offset = plane_state->main.offset;
+		crtc->dspaddr_offset = plane_state->main.offset;
 	else
-		intel_crtc->dspaddr_offset = linear_offset;
+		crtc->dspaddr_offset = linear_offset;
 
-	intel_crtc->adjusted_x = x;
-	intel_crtc->adjusted_y = y;
+	crtc->adjusted_x = x;
+	crtc->adjusted_y = y;
 
 	spin_lock_irqsave(&dev_priv->uncore.lock, irqflags);
 
@@ -3112,31 +3128,29 @@ static void i9xx_update_primary_plane(struct drm_plane *primary,
 	if (IS_HASWELL(dev_priv) || IS_BROADWELL(dev_priv)) {
 		I915_WRITE_FW(DSPSURF(plane),
 			      intel_plane_ggtt_offset(plane_state) +
-			      intel_crtc->dspaddr_offset);
+			      crtc->dspaddr_offset);
 		I915_WRITE_FW(DSPOFFSET(plane), (y << 16) | x);
 	} else if (INTEL_GEN(dev_priv) >= 4) {
 		I915_WRITE_FW(DSPSURF(plane),
 			      intel_plane_ggtt_offset(plane_state) +
-			      intel_crtc->dspaddr_offset);
+			      crtc->dspaddr_offset);
 		I915_WRITE_FW(DSPTILEOFF(plane), (y << 16) | x);
 		I915_WRITE_FW(DSPLINOFF(plane), linear_offset);
 	} else {
 		I915_WRITE_FW(DSPADDR(plane),
 			      intel_plane_ggtt_offset(plane_state) +
-			      intel_crtc->dspaddr_offset);
+			      crtc->dspaddr_offset);
 	}
 	POSTING_READ_FW(reg);
 
 	spin_unlock_irqrestore(&dev_priv->uncore.lock, irqflags);
 }
 
-static void i9xx_disable_primary_plane(struct drm_plane *primary,
-				       struct drm_crtc *crtc)
+static void i9xx_disable_primary_plane(struct intel_plane *primary,
+				       struct intel_crtc *crtc)
 {
-	struct drm_device *dev = crtc->dev;
-	struct drm_i915_private *dev_priv = to_i915(dev);
-	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
-	int plane = intel_crtc->plane;
+	struct drm_i915_private *dev_priv = to_i915(primary->base.dev);
+	enum plane plane = primary->plane;
 	unsigned long irqflags;
 
 	spin_lock_irqsave(&dev_priv->uncore.lock, irqflags);
@@ -3321,16 +3335,15 @@ u32 skl_plane_ctl(const struct intel_crtc_state *crtc_state,
 	return plane_ctl;
 }
 
-static void skylake_update_primary_plane(struct drm_plane *plane,
+static void skylake_update_primary_plane(struct intel_plane *plane,
 					 const struct intel_crtc_state *crtc_state,
 					 const struct intel_plane_state *plane_state)
 {
-	struct drm_device *dev = plane->dev;
-	struct drm_i915_private *dev_priv = to_i915(dev);
-	struct intel_crtc *intel_crtc = to_intel_crtc(crtc_state->base.crtc);
-	struct drm_framebuffer *fb = plane_state->base.fb;
-	enum plane_id plane_id = to_intel_plane(plane)->id;
-	enum pipe pipe = to_intel_plane(plane)->pipe;
+	struct drm_i915_private *dev_priv = to_i915(plane->base.dev);
+	struct intel_crtc *crtc = to_intel_crtc(crtc_state->base.crtc);
+	const struct drm_framebuffer *fb = plane_state->base.fb;
+	enum plane_id plane_id = plane->id;
+	enum pipe pipe = plane->pipe;
 	u32 plane_ctl = plane_state->ctl;
 	unsigned int rotation = plane_state->base.rotation;
 	u32 stride = skl_plane_stride(fb, 0, rotation);
@@ -3352,10 +3365,10 @@ static void skylake_update_primary_plane(struct drm_plane *plane,
 	dst_w--;
 	dst_h--;
 
-	intel_crtc->dspaddr_offset = surf_addr;
+	crtc->dspaddr_offset = surf_addr;
 
-	intel_crtc->adjusted_x = src_x;
-	intel_crtc->adjusted_y = src_y;
+	crtc->adjusted_x = src_x;
+	crtc->adjusted_y = src_y;
 
 	spin_lock_irqsave(&dev_priv->uncore.lock, irqflags);
 
@@ -3394,13 +3407,12 @@ static void skylake_update_primary_plane(struct drm_plane *plane,
 	spin_unlock_irqrestore(&dev_priv->uncore.lock, irqflags);
 }
 
-static void skylake_disable_primary_plane(struct drm_plane *primary,
-					  struct drm_crtc *crtc)
+static void skylake_disable_primary_plane(struct intel_plane *primary,
+					  struct intel_crtc *crtc)
 {
-	struct drm_device *dev = crtc->dev;
-	struct drm_i915_private *dev_priv = to_i915(dev);
-	enum plane_id plane_id = to_intel_plane(primary)->id;
-	enum pipe pipe = to_intel_plane(primary)->pipe;
+	struct drm_i915_private *dev_priv = to_i915(primary->base.dev);
+	enum plane_id plane_id = primary->id;
+	enum pipe pipe = primary->pipe;
 	unsigned long irqflags;
 
 	spin_lock_irqsave(&dev_priv->uncore.lock, irqflags);
@@ -3433,7 +3445,7 @@ static void intel_update_primary_planes(struct drm_device *dev)
 			trace_intel_update_plane(&plane->base,
 						 to_intel_crtc(crtc));
 
-			plane->update_plane(&plane->base,
+			plane->update_plane(plane,
 					    to_intel_crtc_state(crtc->state),
 					    plane_state);
 		}
@@ -4861,12 +4873,9 @@ static void intel_crtc_dpms_overlay_disable(struct intel_crtc *intel_crtc)
 {
 	if (intel_crtc->overlay) {
 		struct drm_device *dev = intel_crtc->base.dev;
-		struct drm_i915_private *dev_priv = to_i915(dev);
 
 		mutex_lock(&dev->struct_mutex);
-		dev_priv->mm.interruptible = false;
 		(void) intel_overlay_switch_off(intel_crtc->overlay);
-		dev_priv->mm.interruptible = true;
 		mutex_unlock(&dev->struct_mutex);
 	}
 
@@ -5086,7 +5095,7 @@ static void intel_crtc_disable_planes(struct drm_crtc *crtc, unsigned plane_mask
 	intel_crtc_dpms_overlay_disable(intel_crtc);
 
 	drm_for_each_plane_mask(p, dev, plane_mask)
-		to_intel_plane(p)->disable_plane(p, crtc);
+		to_intel_plane(p)->disable_plane(to_intel_plane(p), intel_crtc);
 
 	/*
 	 * FIXME: Once we grow proper nuclear flip support out of this we need
@@ -5722,6 +5731,8 @@ static void i9xx_set_pll_dividers(struct intel_crtc *crtc)
 static void i9xx_crtc_enable(struct intel_crtc_state *pipe_config,
 			     struct drm_atomic_state *old_state)
 {
+	struct intel_atomic_state *old_intel_state =
+		to_intel_atomic_state(old_state);
 	struct drm_crtc *crtc = pipe_config->base.crtc;
 	struct drm_device *dev = crtc->dev;
 	struct drm_i915_private *dev_priv = to_i915(dev);
@@ -5754,7 +5765,11 @@ static void i9xx_crtc_enable(struct intel_crtc_state *pipe_config,
 
 	intel_color_load_luts(&pipe_config->base);
 
-	intel_update_watermarks(intel_crtc);
+	if (dev_priv->display.initial_watermarks != NULL)
+		dev_priv->display.initial_watermarks(old_intel_state,
+						     intel_crtc->config);
+	else
+		intel_update_watermarks(intel_crtc);
 	intel_enable_pipe(intel_crtc);
 
 	assert_vblank_disabled(crtc);
@@ -5920,9 +5935,10 @@ void intel_encoder_destroy(struct drm_encoder *encoder)
 
 /* Cross check the actual hw state with our own modeset state tracking (and it's
  * internal consistency). */
-static void intel_connector_verify_state(struct intel_connector *connector)
+static void intel_connector_verify_state(struct drm_crtc_state *crtc_state,
+					 struct drm_connector_state *conn_state)
 {
-	struct drm_crtc *crtc = connector->base.state->crtc;
+	struct intel_connector *connector = to_intel_connector(conn_state->connector);
 
 	DRM_DEBUG_KMS("[CONNECTOR:%d:%s]\n",
 		      connector->base.base.id,
@@ -5930,15 +5946,14 @@ static void intel_connector_verify_state(struct intel_connector *connector)
 
 	if (connector->get_hw_state(connector)) {
 		struct intel_encoder *encoder = connector->encoder;
-		struct drm_connector_state *conn_state = connector->base.state;
 
-		I915_STATE_WARN(!crtc,
+		I915_STATE_WARN(!crtc_state,
 			 "connector enabled without attached crtc\n");
 
-		if (!crtc)
+		if (!crtc_state)
 			return;
 
-		I915_STATE_WARN(!crtc->state->active,
+		I915_STATE_WARN(!crtc_state->active,
 		      "connector is active, but attached crtc isn't\n");
 
 		if (!encoder || encoder->type == INTEL_OUTPUT_DP_MST)
@@ -5950,20 +5965,30 @@ static void intel_connector_verify_state(struct intel_connector *connector)
 		I915_STATE_WARN(conn_state->crtc != encoder->base.crtc,
 			"attached encoder crtc differs from connector crtc\n");
 	} else {
-		I915_STATE_WARN(crtc && crtc->state->active,
+		I915_STATE_WARN(crtc_state && crtc_state->active,
 			"attached crtc is active, but connector isn't\n");
-		I915_STATE_WARN(!crtc && connector->base.state->best_encoder,
+		I915_STATE_WARN(!crtc_state && conn_state->best_encoder,
 			"best encoder set without crtc!\n");
 	}
 }
 
 int intel_connector_init(struct intel_connector *connector)
 {
-	drm_atomic_helper_connector_reset(&connector->base);
+	struct intel_digital_connector_state *conn_state;
 
-	if (!connector->base.state)
+	/*
+	 * Allocate enough memory to hold intel_digital_connector_state,
+	 * This might be a few bytes too many, but for connectors that don't
+	 * need it we'll free the state and allocate a smaller one on the first
+	 * succesful commit anyway.
+	 */
+	conn_state = kzalloc(sizeof(*conn_state), GFP_KERNEL);
+	if (!conn_state)
 		return -ENOMEM;
 
+	__drm_atomic_helper_connector_reset(&connector->base,
+					    &conn_state->base);
+
 	return 0;
 }
 
@@ -6372,8 +6397,8 @@ static void vlv_pllb_recal_opamp(struct drm_i915_private *dev_priv, enum pipe
 	vlv_dpio_write(dev_priv, pipe, VLV_PLL_DW9(1), reg_val);
 
 	reg_val = vlv_dpio_read(dev_priv, pipe, VLV_REF_DW13);
-	reg_val &= 0x8cffffff;
-	reg_val = 0x8c000000;
+	reg_val &= 0x00ffffff;
+	reg_val |= 0x8c000000;
 	vlv_dpio_write(dev_priv, pipe, VLV_REF_DW13, reg_val);
 
 	reg_val = vlv_dpio_read(dev_priv, pipe, VLV_PLL_DW9(1));
@@ -8177,9 +8202,6 @@ static int ironlake_crtc_compute_clock(struct intel_crtc *crtc,
 {
 	struct drm_device *dev = crtc->base.dev;
 	struct drm_i915_private *dev_priv = to_i915(dev);
-	struct dpll reduced_clock;
-	bool has_reduced_clock = false;
-	struct intel_shared_dpll *pll;
 	const struct intel_limit *limit;
 	int refclk = 120000;
 
@@ -8221,20 +8243,14 @@ static int ironlake_crtc_compute_clock(struct intel_crtc *crtc,
 		return -EINVAL;
 	}
 
-	ironlake_compute_dpll(crtc, crtc_state,
-			      has_reduced_clock ? &reduced_clock : NULL);
+	ironlake_compute_dpll(crtc, crtc_state, NULL);
 
-	pll = intel_get_shared_dpll(crtc, crtc_state, NULL);
-	if (pll == NULL) {
+	if (!intel_get_shared_dpll(crtc, crtc_state, NULL)) {
 		DRM_DEBUG_DRIVER("failed to find PLL for pipe %c\n",
 				 pipe_name(crtc->pipe));
 		return -EINVAL;
 	}
 
-	if (intel_crtc_has_type(crtc_state, INTEL_OUTPUT_LVDS) &&
-	    has_reduced_clock)
-		crtc->lowfreq_avail = true;
-
 	return 0;
 }
 
@@ -9138,38 +9154,171 @@ out:
 	return active;
 }
 
+static u32 intel_cursor_base(const struct intel_plane_state *plane_state)
+{
+	struct drm_i915_private *dev_priv =
+		to_i915(plane_state->base.plane->dev);
+	const struct drm_framebuffer *fb = plane_state->base.fb;
+	const struct drm_i915_gem_object *obj = intel_fb_obj(fb);
+	u32 base;
+
+	if (INTEL_INFO(dev_priv)->cursor_needs_physical)
+		base = obj->phys_handle->busaddr;
+	else
+		base = intel_plane_ggtt_offset(plane_state);
+
+	base += plane_state->main.offset;
+
+	/* ILK+ do this automagically */
+	if (HAS_GMCH_DISPLAY(dev_priv) &&
+	    plane_state->base.rotation & DRM_MODE_ROTATE_180)
+		base += (plane_state->base.crtc_h *
+			 plane_state->base.crtc_w - 1) * fb->format->cpp[0];
+
+	return base;
+}
+
+static u32 intel_cursor_position(const struct intel_plane_state *plane_state)
+{
+	int x = plane_state->base.crtc_x;
+	int y = plane_state->base.crtc_y;
+	u32 pos = 0;
+
+	if (x < 0) {
+		pos |= CURSOR_POS_SIGN << CURSOR_X_SHIFT;
+		x = -x;
+	}
+	pos |= x << CURSOR_X_SHIFT;
+
+	if (y < 0) {
+		pos |= CURSOR_POS_SIGN << CURSOR_Y_SHIFT;
+		y = -y;
+	}
+	pos |= y << CURSOR_Y_SHIFT;
+
+	return pos;
+}
+
+static bool intel_cursor_size_ok(const struct intel_plane_state *plane_state)
+{
+	const struct drm_mode_config *config =
+		&plane_state->base.plane->dev->mode_config;
+	int width = plane_state->base.crtc_w;
+	int height = plane_state->base.crtc_h;
+
+	return width > 0 && width <= config->cursor_width &&
+		height > 0 && height <= config->cursor_height;
+}
+
+static int intel_check_cursor(struct intel_crtc_state *crtc_state,
+			      struct intel_plane_state *plane_state)
+{
+	const struct drm_framebuffer *fb = plane_state->base.fb;
+	int src_x, src_y;
+	u32 offset;
+	int ret;
+
+	ret = drm_plane_helper_check_state(&plane_state->base,
+					   &plane_state->clip,
+					   DRM_PLANE_HELPER_NO_SCALING,
+					   DRM_PLANE_HELPER_NO_SCALING,
+					   true, true);
+	if (ret)
+		return ret;
+
+	if (!fb)
+		return 0;
+
+	if (fb->modifier != DRM_FORMAT_MOD_LINEAR) {
+		DRM_DEBUG_KMS("cursor cannot be tiled\n");
+		return -EINVAL;
+	}
+
+	src_x = plane_state->base.src_x >> 16;
+	src_y = plane_state->base.src_y >> 16;
+
+	intel_add_fb_offsets(&src_x, &src_y, plane_state, 0);
+	offset = intel_compute_tile_offset(&src_x, &src_y, plane_state, 0);
+
+	if (src_x != 0 || src_y != 0) {
+		DRM_DEBUG_KMS("Arbitrary cursor panning not supported\n");
+		return -EINVAL;
+	}
+
+	plane_state->main.offset = offset;
+
+	return 0;
+}
+
 static u32 i845_cursor_ctl(const struct intel_crtc_state *crtc_state,
 			   const struct intel_plane_state *plane_state)
 {
-	unsigned int width = plane_state->base.crtc_w;
-	unsigned int stride = roundup_pow_of_two(width) * 4;
+	const struct drm_framebuffer *fb = plane_state->base.fb;
 
-	switch (stride) {
-	default:
-		WARN_ONCE(1, "Invalid cursor width/stride, width=%u, stride=%u\n",
-			  width, stride);
-		stride = 256;
-		/* fallthrough */
+	return CURSOR_ENABLE |
+		CURSOR_GAMMA_ENABLE |
+		CURSOR_FORMAT_ARGB |
+		CURSOR_STRIDE(fb->pitches[0]);
+}
+
+static bool i845_cursor_size_ok(const struct intel_plane_state *plane_state)
+{
+	int width = plane_state->base.crtc_w;
+
+	/*
+	 * 845g/865g are only limited by the width of their cursors,
+	 * the height is arbitrary up to the precision of the register.
+	 */
+	return intel_cursor_size_ok(plane_state) && IS_ALIGNED(width, 64);
+}
+
+static int i845_check_cursor(struct intel_plane *plane,
+			     struct intel_crtc_state *crtc_state,
+			     struct intel_plane_state *plane_state)
+{
+	const struct drm_framebuffer *fb = plane_state->base.fb;
+	int ret;
+
+	ret = intel_check_cursor(crtc_state, plane_state);
+	if (ret)
+		return ret;
+
+	/* if we want to turn off the cursor ignore width and height */
+	if (!fb)
+		return 0;
+
+	/* Check for which cursor types we support */
+	if (!i845_cursor_size_ok(plane_state)) {
+		DRM_DEBUG("Cursor dimension %dx%d not supported\n",
+			  plane_state->base.crtc_w,
+			  plane_state->base.crtc_h);
+		return -EINVAL;
+	}
+
+	switch (fb->pitches[0]) {
 	case 256:
 	case 512:
 	case 1024:
 	case 2048:
 		break;
+	default:
+		DRM_DEBUG_KMS("Invalid cursor stride (%u)\n",
+			      fb->pitches[0]);
+		return -EINVAL;
 	}
 
-	return CURSOR_ENABLE |
-		CURSOR_GAMMA_ENABLE |
-		CURSOR_FORMAT_ARGB |
-		CURSOR_STRIDE(stride);
+	plane_state->ctl = i845_cursor_ctl(crtc_state, plane_state);
+
+	return 0;
 }
 
-static void i845_update_cursor(struct drm_crtc *crtc, u32 base,
+static void i845_update_cursor(struct intel_plane *plane,
+			       const struct intel_crtc_state *crtc_state,
 			       const struct intel_plane_state *plane_state)
 {
-	struct drm_device *dev = crtc->dev;
-	struct drm_i915_private *dev_priv = to_i915(dev);
-	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
-	uint32_t cntl = 0, size = 0;
+	struct drm_i915_private *dev_priv = to_i915(plane->base.dev);
+	u32 cntl = 0, base = 0, pos = 0, size = 0;
+	unsigned long irqflags;
 
 	if (plane_state && plane_state->base.visible) {
 		unsigned int width = plane_state->base.crtc_w;
@@ -9177,35 +9326,41 @@ static void i845_update_cursor(struct drm_crtc *crtc, u32 base,
 
 		cntl = plane_state->ctl;
 		size = (height << 12) | width;
-	}
 
-	if (intel_crtc->cursor_cntl != 0 &&
-	    (intel_crtc->cursor_base != base ||
-	     intel_crtc->cursor_size != size ||
-	     intel_crtc->cursor_cntl != cntl)) {
-		/* On these chipsets we can only modify the base/size/stride
-		 * whilst the cursor is disabled.
-		 */
-		I915_WRITE_FW(CURCNTR(PIPE_A), 0);
-		POSTING_READ_FW(CURCNTR(PIPE_A));
-		intel_crtc->cursor_cntl = 0;
+		base = intel_cursor_base(plane_state);
+		pos = intel_cursor_position(plane_state);
 	}
 
-	if (intel_crtc->cursor_base != base) {
-		I915_WRITE_FW(CURBASE(PIPE_A), base);
-		intel_crtc->cursor_base = base;
-	}
+	spin_lock_irqsave(&dev_priv->uncore.lock, irqflags);
 
-	if (intel_crtc->cursor_size != size) {
+	/* On these chipsets we can only modify the base/size/stride
+	 * whilst the cursor is disabled.
+	 */
+	if (plane->cursor.base != base ||
+	    plane->cursor.size != size ||
+	    plane->cursor.cntl != cntl) {
+		I915_WRITE_FW(CURCNTR(PIPE_A), 0);
+		I915_WRITE_FW(CURBASE(PIPE_A), base);
 		I915_WRITE_FW(CURSIZE, size);
-		intel_crtc->cursor_size = size;
-	}
-
-	if (intel_crtc->cursor_cntl != cntl) {
+		I915_WRITE_FW(CURPOS(PIPE_A), pos);
 		I915_WRITE_FW(CURCNTR(PIPE_A), cntl);
-		POSTING_READ_FW(CURCNTR(PIPE_A));
-		intel_crtc->cursor_cntl = cntl;
+
+		plane->cursor.base = base;
+		plane->cursor.size = size;
+		plane->cursor.cntl = cntl;
+	} else {
+		I915_WRITE_FW(CURPOS(PIPE_A), pos);
 	}
+
+	POSTING_READ_FW(CURCNTR(PIPE_A));
+
+	spin_unlock_irqrestore(&dev_priv->uncore.lock, irqflags);
+}
+
+static void i845_disable_cursor(struct intel_plane *plane,
+				struct intel_crtc *crtc)
+{
+	i845_update_cursor(plane, NULL, NULL);
 }
 
 static u32 i9xx_cursor_ctl(const struct intel_crtc_state *crtc_state,
@@ -9214,7 +9369,6 @@ static u32 i9xx_cursor_ctl(const struct intel_crtc_state *crtc_state,
 	struct drm_i915_private *dev_priv =
 		to_i915(plane_state->base.plane->dev);
 	struct intel_crtc *crtc = to_intel_crtc(crtc_state->base.crtc);
-	enum pipe pipe = crtc->pipe;
 	u32 cntl;
 
 	cntl = MCURSOR_GAMMA_ENABLE;
@@ -9222,7 +9376,7 @@ static u32 i9xx_cursor_ctl(const struct intel_crtc_state *crtc_state,
 	if (HAS_DDI(dev_priv))
 		cntl |= CURSOR_PIPE_CSC_ENABLE;
 
-	cntl |= pipe << 28; /* Connect to correct pipe */
+	cntl |= MCURSOR_PIPE_SELECT(crtc->pipe);
 
 	switch (plane_state->base.crtc_w) {
 	case 64:
@@ -9245,116 +9399,154 @@ static u32 i9xx_cursor_ctl(const struct intel_crtc_state *crtc_state,
 	return cntl;
 }
 
-static void i9xx_update_cursor(struct drm_crtc *crtc, u32 base,
-			       const struct intel_plane_state *plane_state)
+static bool i9xx_cursor_size_ok(const struct intel_plane_state *plane_state)
 {
-	struct drm_device *dev = crtc->dev;
-	struct drm_i915_private *dev_priv = to_i915(dev);
-	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
-	int pipe = intel_crtc->pipe;
-	uint32_t cntl = 0;
+	struct drm_i915_private *dev_priv =
+		to_i915(plane_state->base.plane->dev);
+	int width = plane_state->base.crtc_w;
+	int height = plane_state->base.crtc_h;
 
-	if (plane_state && plane_state->base.visible)
-		cntl = plane_state->ctl;
+	if (!intel_cursor_size_ok(plane_state))
+		return false;
 
-	if (intel_crtc->cursor_cntl != cntl) {
-		I915_WRITE_FW(CURCNTR(pipe), cntl);
-		POSTING_READ_FW(CURCNTR(pipe));
-		intel_crtc->cursor_cntl = cntl;
+	/* Cursor width is limited to a few power-of-two sizes */
+	switch (width) {
+	case 256:
+	case 128:
+	case 64:
+		break;
+	default:
+		return false;
 	}
 
-	/* and commit changes on next vblank */
-	I915_WRITE_FW(CURBASE(pipe), base);
-	POSTING_READ_FW(CURBASE(pipe));
+	/*
+	 * IVB+ have CUR_FBC_CTL which allows an arbitrary cursor
+	 * height from 8 lines up to the cursor width, when the
+	 * cursor is not rotated. Everything else requires square
+	 * cursors.
+	 */
+	if (HAS_CUR_FBC(dev_priv) &&
+	    plane_state->base.rotation & DRM_MODE_ROTATE_0) {
+		if (height < 8 || height > width)
+			return false;
+	} else {
+		if (height != width)
+			return false;
+	}
 
-	intel_crtc->cursor_base = base;
+	return true;
 }
 
-/* If no-part of the cursor is visible on the framebuffer, then the GPU may hang... */
-static void intel_crtc_update_cursor(struct drm_crtc *crtc,
-				     const struct intel_plane_state *plane_state)
+static int i9xx_check_cursor(struct intel_plane *plane,
+			     struct intel_crtc_state *crtc_state,
+			     struct intel_plane_state *plane_state)
 {
-	struct drm_device *dev = crtc->dev;
-	struct drm_i915_private *dev_priv = to_i915(dev);
-	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
-	int pipe = intel_crtc->pipe;
-	u32 base = intel_crtc->cursor_addr;
-	unsigned long irqflags;
-	u32 pos = 0;
-
-	if (plane_state) {
-		int x = plane_state->base.crtc_x;
-		int y = plane_state->base.crtc_y;
+	struct drm_i915_private *dev_priv = to_i915(plane->base.dev);
+	const struct drm_framebuffer *fb = plane_state->base.fb;
+	enum pipe pipe = plane->pipe;
+	int ret;
 
-		if (x < 0) {
-			pos |= CURSOR_POS_SIGN << CURSOR_X_SHIFT;
-			x = -x;
-		}
-		pos |= x << CURSOR_X_SHIFT;
+	ret = intel_check_cursor(crtc_state, plane_state);
+	if (ret)
+		return ret;
 
-		if (y < 0) {
-			pos |= CURSOR_POS_SIGN << CURSOR_Y_SHIFT;
-			y = -y;
-		}
-		pos |= y << CURSOR_Y_SHIFT;
+	/* if we want to turn off the cursor ignore width and height */
+	if (!fb)
+		return 0;
 
-		/* ILK+ do this automagically */
-		if (HAS_GMCH_DISPLAY(dev_priv) &&
-		    plane_state->base.rotation & DRM_MODE_ROTATE_180) {
-			base += (plane_state->base.crtc_h *
-				 plane_state->base.crtc_w - 1) * 4;
-		}
+	/* Check for which cursor types we support */
+	if (!i9xx_cursor_size_ok(plane_state)) {
+		DRM_DEBUG("Cursor dimension %dx%d not supported\n",
+			  plane_state->base.crtc_w,
+			  plane_state->base.crtc_h);
+		return -EINVAL;
 	}
 
-	spin_lock_irqsave(&dev_priv->uncore.lock, irqflags);
+	if (fb->pitches[0] != plane_state->base.crtc_w * fb->format->cpp[0]) {
+		DRM_DEBUG_KMS("Invalid cursor stride (%u) (cursor width %d)\n",
+			      fb->pitches[0], plane_state->base.crtc_w);
+		return -EINVAL;
+	}
 
-	I915_WRITE_FW(CURPOS(pipe), pos);
+	/*
+	 * There's something wrong with the cursor on CHV pipe C.
+	 * If it straddles the left edge of the screen then
+	 * moving it away from the edge or disabling it often
+	 * results in a pipe underrun, and often that can lead to
+	 * dead pipe (constant underrun reported, and it scans
+	 * out just a solid color). To recover from that, the
+	 * display power well must be turned off and on again.
+	 * Refuse the put the cursor into that compromised position.
+	 */
+	if (IS_CHERRYVIEW(dev_priv) && pipe == PIPE_C &&
+	    plane_state->base.visible && plane_state->base.crtc_x < 0) {
+		DRM_DEBUG_KMS("CHV cursor C not allowed to straddle the left screen edge\n");
+		return -EINVAL;
+	}
 
-	if (IS_I845G(dev_priv) || IS_I865G(dev_priv))
-		i845_update_cursor(crtc, base, plane_state);
-	else
-		i9xx_update_cursor(crtc, base, plane_state);
+	plane_state->ctl = i9xx_cursor_ctl(crtc_state, plane_state);
 
-	spin_unlock_irqrestore(&dev_priv->uncore.lock, irqflags);
+	return 0;
 }
 
-static bool cursor_size_ok(struct drm_i915_private *dev_priv,
-			   uint32_t width, uint32_t height)
+static void i9xx_update_cursor(struct intel_plane *plane,
+			       const struct intel_crtc_state *crtc_state,
+			       const struct intel_plane_state *plane_state)
 {
-	if (width == 0 || height == 0)
-		return false;
+	struct drm_i915_private *dev_priv = to_i915(plane->base.dev);
+	enum pipe pipe = plane->pipe;
+	u32 cntl = 0, base = 0, pos = 0, fbc_ctl = 0;
+	unsigned long irqflags;
 
-	/*
-	 * 845g/865g are special in that they are only limited by
-	 * the width of their cursors, the height is arbitrary up to
-	 * the precision of the register. Everything else requires
-	 * square cursors, limited to a few power-of-two sizes.
-	 */
-	if (IS_I845G(dev_priv) || IS_I865G(dev_priv)) {
-		if ((width & 63) != 0)
-			return false;
+	if (plane_state && plane_state->base.visible) {
+		cntl = plane_state->ctl;
 
-		if (width > (IS_I845G(dev_priv) ? 64 : 512))
-			return false;
+		if (plane_state->base.crtc_h != plane_state->base.crtc_w)
+			fbc_ctl = CUR_FBC_CTL_EN | (plane_state->base.crtc_h - 1);
 
-		if (height > 1023)
-			return false;
+		base = intel_cursor_base(plane_state);
+		pos = intel_cursor_position(plane_state);
+	}
+
+	spin_lock_irqsave(&dev_priv->uncore.lock, irqflags);
+
+	/*
+	 * On some platforms writing CURCNTR first will also
+	 * cause CURPOS to be armed by the CURBASE write.
+	 * Without the CURCNTR write the CURPOS write would
+	 * arm itself.
+	 *
+	 * CURCNTR and CUR_FBC_CTL are always
+	 * armed by the CURBASE write only.
+	 */
+	if (plane->cursor.base != base ||
+	    plane->cursor.size != fbc_ctl ||
+	    plane->cursor.cntl != cntl) {
+		I915_WRITE_FW(CURCNTR(pipe), cntl);
+		if (HAS_CUR_FBC(dev_priv))
+			I915_WRITE_FW(CUR_FBC_CTL(pipe), fbc_ctl);
+		I915_WRITE_FW(CURPOS(pipe), pos);
+		I915_WRITE_FW(CURBASE(pipe), base);
+
+		plane->cursor.base = base;
+		plane->cursor.size = fbc_ctl;
+		plane->cursor.cntl = cntl;
 	} else {
-		switch (width | height) {
-		case 256:
-		case 128:
-			if (IS_GEN2(dev_priv))
-				return false;
-		case 64:
-			break;
-		default:
-			return false;
-		}
+		I915_WRITE_FW(CURPOS(pipe), pos);
 	}
 
-	return true;
+	POSTING_READ_FW(CURBASE(pipe));
+
+	spin_unlock_irqrestore(&dev_priv->uncore.lock, irqflags);
 }
 
+static void i9xx_disable_cursor(struct intel_plane *plane,
+				struct intel_crtc *crtc)
+{
+	i9xx_update_cursor(plane, NULL, NULL);
+}
+
+
 /* VESA 640x480x72Hz mode to set on the pipe */
 static struct drm_display_mode load_detect_mode = {
 	DRM_MODE("640x480", DRM_MODE_TYPE_DEFAULT, 31500, 640, 664,
@@ -9566,6 +9758,7 @@ int intel_get_load_detect_pipe(struct drm_connector *connector,
 	 */
 	if (!crtc) {
 		DRM_DEBUG_KMS("no pipe available for load-detect\n");
+		ret = -ENODEV;
 		goto fail;
 	}
 
@@ -9622,6 +9815,7 @@ found:
 		DRM_DEBUG_KMS("reusing fbdev for load-detection framebuffer\n");
 	if (IS_ERR(fb)) {
 		DRM_DEBUG_KMS("failed to allocate framebuffer for load-detection\n");
+		ret = PTR_ERR(fb);
 		goto fail;
 	}
 
@@ -10853,21 +11047,21 @@ int intel_plane_atomic_calc_changes(struct drm_crtc_state *crtc_state,
 			 turn_off, turn_on, mode_changed);
 
 	if (turn_on) {
-		if (INTEL_GEN(dev_priv) < 5)
+		if (INTEL_GEN(dev_priv) < 5 && !IS_G4X(dev_priv))
 			pipe_config->update_wm_pre = true;
 
 		/* must disable cxsr around plane enable/disable */
 		if (plane->id != PLANE_CURSOR)
 			pipe_config->disable_cxsr = true;
 	} else if (turn_off) {
-		if (INTEL_GEN(dev_priv) < 5)
+		if (INTEL_GEN(dev_priv) < 5 && !IS_G4X(dev_priv))
 			pipe_config->update_wm_post = true;
 
 		/* must disable cxsr around plane enable/disable */
 		if (plane->id != PLANE_CURSOR)
 			pipe_config->disable_cxsr = true;
 	} else if (intel_wm_need_update(&plane->base, plane_state)) {
-		if (INTEL_GEN(dev_priv) < 5) {
+		if (INTEL_GEN(dev_priv) < 5 && !IS_G4X(dev_priv)) {
 			/* FIXME bollocks */
 			pipe_config->update_wm_pre = true;
 			pipe_config->update_wm_post = true;
@@ -11294,7 +11488,8 @@ clear_intel_crtc_state(struct intel_crtc_state *crtc_state)
 	shared_dpll = crtc_state->shared_dpll;
 	dpll_hw_state = crtc_state->dpll_hw_state;
 	force_thru = crtc_state->pch_pfit.force_thru;
-	if (IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv))
+	if (IS_G4X(dev_priv) ||
+	    IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv))
 		wm_state = crtc_state->wm;
 
 	/* Keep base drm_crtc_state intact, only clear our extended struct */
@@ -11306,7 +11501,8 @@ clear_intel_crtc_state(struct intel_crtc_state *crtc_state)
 	crtc_state->shared_dpll = shared_dpll;
 	crtc_state->dpll_hw_state = dpll_hw_state;
 	crtc_state->pch_pfit.force_thru = force_thru;
-	if (IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv))
+	if (IS_G4X(dev_priv) ||
+	    IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv))
 		crtc_state->wm = wm_state;
 }
 
@@ -11868,7 +12064,7 @@ static void verify_wm_state(struct drm_crtc *crtc,
 	 * allocation. In that case since the ddb allocation will be updated
 	 * once the plane becomes visible, we can skip this check
 	 */
-	if (intel_crtc->cursor_addr) {
+	if (1) {
 		hw_plane_wm = &hw_wm.planes[PLANE_CURSOR];
 		sw_plane_wm = &sw_wm->planes[PLANE_CURSOR];
 
@@ -11924,11 +12120,15 @@ verify_connector_state(struct drm_device *dev,
 
 	for_each_new_connector_in_state(state, connector, new_conn_state, i) {
 		struct drm_encoder *encoder = connector->encoder;
+		struct drm_crtc_state *crtc_state = NULL;
 
 		if (new_conn_state->crtc != crtc)
 			continue;
 
-		intel_connector_verify_state(to_intel_connector(connector));
+		if (crtc)
+			crtc_state = drm_atomic_get_new_crtc_state(state, new_conn_state->crtc);
+
+		intel_connector_verify_state(crtc_state, new_conn_state);
 
 		I915_STATE_WARN(new_conn_state->best_encoder != encoder,
 		     "connector's atomic encoder doesn't match legacy encoder\n");
@@ -12046,7 +12246,7 @@ verify_crtc_state(struct drm_crtc *crtc,
 
 	intel_pipe_config_sanity_check(dev_priv, pipe_config);
 
-	sw_config = to_intel_crtc_state(crtc->state);
+	sw_config = to_intel_crtc_state(new_crtc_state);
 	if (!intel_pipe_config_compare(dev_priv, sw_config,
 				       pipe_config, false)) {
 		I915_STATE_WARN(1, "pipe state doesn't match!\n");
@@ -13065,43 +13265,6 @@ static int intel_atomic_commit(struct drm_device *dev,
 	return 0;
 }
 
-void intel_crtc_restore_mode(struct drm_crtc *crtc)
-{
-	struct drm_device *dev = crtc->dev;
-	struct drm_atomic_state *state;
-	struct drm_crtc_state *crtc_state;
-	int ret;
-
-	state = drm_atomic_state_alloc(dev);
-	if (!state) {
-		DRM_DEBUG_KMS("[CRTC:%d:%s] crtc restore failed, out of memory",
-			      crtc->base.id, crtc->name);
-		return;
-	}
-
-	state->acquire_ctx = crtc->dev->mode_config.acquire_ctx;
-
-retry:
-	crtc_state = drm_atomic_get_crtc_state(state, crtc);
-	ret = PTR_ERR_OR_ZERO(crtc_state);
-	if (!ret) {
-		if (!crtc_state->active)
-			goto out;
-
-		crtc_state->mode_changed = true;
-		ret = drm_atomic_commit(state);
-	}
-
-	if (ret == -EDEADLK) {
-		drm_atomic_state_clear(state);
-		drm_modeset_backoff(state->acquire_ctx);
-		goto retry;
-	}
-
-out:
-	drm_atomic_state_put(state);
-}
-
 static const struct drm_crtc_funcs intel_crtc_funcs = {
 	.gamma_set = drm_atomic_helper_legacy_gamma_set,
 	.set_config = drm_atomic_helper_set_config,
@@ -13142,7 +13305,7 @@ intel_prepare_plane_fb(struct drm_plane *plane,
 	if (obj) {
 		if (plane->type == DRM_PLANE_TYPE_CURSOR &&
 		    INTEL_INFO(dev_priv)->cursor_needs_physical) {
-			const int align = IS_I830(dev_priv) ? 16 * 1024 : 256;
+			const int align = intel_cursor_alignment(dev_priv);
 
 			ret = i915_gem_object_attach_phys(obj, align);
 			if (ret) {
@@ -13272,11 +13435,11 @@ skl_max_scale(struct intel_crtc *intel_crtc, struct intel_crtc_state *crtc_state
 }
 
 static int
-intel_check_primary_plane(struct drm_plane *plane,
+intel_check_primary_plane(struct intel_plane *plane,
 			  struct intel_crtc_state *crtc_state,
 			  struct intel_plane_state *state)
 {
-	struct drm_i915_private *dev_priv = to_i915(plane->dev);
+	struct drm_i915_private *dev_priv = to_i915(plane->base.dev);
 	struct drm_crtc *crtc = state->base.crtc;
 	int min_scale = DRM_PLANE_HELPER_NO_SCALING;
 	int max_scale = DRM_PLANE_HELPER_NO_SCALING;
@@ -13455,7 +13618,7 @@ intel_legacy_cursor_update(struct drm_plane *plane,
 		goto out_free;
 
 	if (INTEL_INFO(dev_priv)->cursor_needs_physical) {
-		int align = IS_I830(dev_priv) ? 16 * 1024 : 256;
+		int align = intel_cursor_alignment(dev_priv);
 
 		ret = i915_gem_object_attach_phys(intel_fb_obj(fb), align);
 		if (ret) {
@@ -13491,12 +13654,12 @@ intel_legacy_cursor_update(struct drm_plane *plane,
 
 	if (plane->state->visible) {
 		trace_intel_update_plane(plane, to_intel_crtc(crtc));
-		intel_plane->update_plane(plane,
+		intel_plane->update_plane(intel_plane,
 					  to_intel_crtc_state(crtc->state),
 					  to_intel_plane_state(plane->state));
 	} else {
 		trace_intel_disable_plane(plane, to_intel_crtc(crtc));
-		intel_plane->disable_plane(plane, crtc);
+		intel_plane->disable_plane(intel_plane, to_intel_crtc(crtc));
 	}
 
 	intel_cleanup_plane_fb(plane, new_plane_state);
@@ -13639,107 +13802,9 @@ fail:
 	return ERR_PTR(ret);
 }
 
-static int
-intel_check_cursor_plane(struct drm_plane *plane,
-			 struct intel_crtc_state *crtc_state,
-			 struct intel_plane_state *state)
-{
-	struct drm_i915_private *dev_priv = to_i915(plane->dev);
-	struct drm_framebuffer *fb = state->base.fb;
-	struct drm_i915_gem_object *obj = intel_fb_obj(fb);
-	enum pipe pipe = to_intel_plane(plane)->pipe;
-	unsigned stride;
-	int ret;
-
-	ret = drm_plane_helper_check_state(&state->base,
-					   &state->clip,
-					   DRM_PLANE_HELPER_NO_SCALING,
-					   DRM_PLANE_HELPER_NO_SCALING,
-					   true, true);
-	if (ret)
-		return ret;
-
-	/* if we want to turn off the cursor ignore width and height */
-	if (!obj)
-		return 0;
-
-	/* Check for which cursor types we support */
-	if (!cursor_size_ok(dev_priv, state->base.crtc_w,
-			    state->base.crtc_h)) {
-		DRM_DEBUG("Cursor dimension %dx%d not supported\n",
-			  state->base.crtc_w, state->base.crtc_h);
-		return -EINVAL;
-	}
-
-	stride = roundup_pow_of_two(state->base.crtc_w) * 4;
-	if (obj->base.size < stride * state->base.crtc_h) {
-		DRM_DEBUG_KMS("buffer is too small\n");
-		return -ENOMEM;
-	}
-
-	if (fb->modifier != DRM_FORMAT_MOD_LINEAR) {
-		DRM_DEBUG_KMS("cursor cannot be tiled\n");
-		return -EINVAL;
-	}
-
-	/*
-	 * There's something wrong with the cursor on CHV pipe C.
-	 * If it straddles the left edge of the screen then
-	 * moving it away from the edge or disabling it often
-	 * results in a pipe underrun, and often that can lead to
-	 * dead pipe (constant underrun reported, and it scans
-	 * out just a solid color). To recover from that, the
-	 * display power well must be turned off and on again.
-	 * Refuse the put the cursor into that compromised position.
-	 */
-	if (IS_CHERRYVIEW(dev_priv) && pipe == PIPE_C &&
-	    state->base.visible && state->base.crtc_x < 0) {
-		DRM_DEBUG_KMS("CHV cursor C not allowed to straddle the left screen edge\n");
-		return -EINVAL;
-	}
-
-	if (IS_I845G(dev_priv) || IS_I865G(dev_priv))
-		state->ctl = i845_cursor_ctl(crtc_state, state);
-	else
-		state->ctl = i9xx_cursor_ctl(crtc_state, state);
-
-	return 0;
-}
-
-static void
-intel_disable_cursor_plane(struct drm_plane *plane,
-			   struct drm_crtc *crtc)
-{
-	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
-
-	intel_crtc->cursor_addr = 0;
-	intel_crtc_update_cursor(crtc, NULL);
-}
-
-static void
-intel_update_cursor_plane(struct drm_plane *plane,
-			  const struct intel_crtc_state *crtc_state,
-			  const struct intel_plane_state *state)
-{
-	struct drm_crtc *crtc = crtc_state->base.crtc;
-	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
-	struct drm_i915_private *dev_priv = to_i915(plane->dev);
-	struct drm_i915_gem_object *obj = intel_fb_obj(state->base.fb);
-	uint32_t addr;
-
-	if (!obj)
-		addr = 0;
-	else if (!INTEL_INFO(dev_priv)->cursor_needs_physical)
-		addr = intel_plane_ggtt_offset(state);
-	else
-		addr = obj->phys_handle->busaddr;
-
-	intel_crtc->cursor_addr = addr;
-	intel_crtc_update_cursor(crtc, state);
-}
-
 static struct intel_plane *
-intel_cursor_plane_create(struct drm_i915_private *dev_priv, enum pipe pipe)
+intel_cursor_plane_create(struct drm_i915_private *dev_priv,
+			  enum pipe pipe)
 {
 	struct intel_plane *cursor = NULL;
 	struct intel_plane_state *state = NULL;
@@ -13765,9 +13830,22 @@ intel_cursor_plane_create(struct drm_i915_private *dev_priv, enum pipe pipe)
 	cursor->plane = pipe;
 	cursor->id = PLANE_CURSOR;
 	cursor->frontbuffer_bit = INTEL_FRONTBUFFER_CURSOR(pipe);
-	cursor->check_plane = intel_check_cursor_plane;
-	cursor->update_plane = intel_update_cursor_plane;
-	cursor->disable_plane = intel_disable_cursor_plane;
+
+	if (IS_I845G(dev_priv) || IS_I865G(dev_priv)) {
+		cursor->update_plane = i845_update_cursor;
+		cursor->disable_plane = i845_disable_cursor;
+		cursor->check_plane = i845_check_cursor;
+	} else {
+		cursor->update_plane = i9xx_update_cursor;
+		cursor->disable_plane = i9xx_disable_cursor;
+		cursor->check_plane = i9xx_check_cursor;
+	}
+
+	cursor->cursor.base = ~0;
+	cursor->cursor.cntl = ~0;
+
+	if (IS_I845G(dev_priv) || IS_I865G(dev_priv) || HAS_CUR_FBC(dev_priv))
+		cursor->cursor.size = ~0;
 
 	ret = drm_universal_plane_init(&dev_priv->drm, &cursor->base,
 				       0, &intel_cursor_plane_funcs,
@@ -13876,10 +13954,6 @@ static int intel_crtc_init(struct drm_i915_private *dev_priv, enum pipe pipe)
 	intel_crtc->pipe = pipe;
 	intel_crtc->plane = primary->plane;
 
-	intel_crtc->cursor_base = ~0;
-	intel_crtc->cursor_cntl = ~0;
-	intel_crtc->cursor_size = ~0;
-
 	/* initialize shared scalers */
 	intel_crtc_init_scalers(intel_crtc, crtc_state);
 
@@ -14419,7 +14493,7 @@ static int intel_framebuffer_init(struct intel_framebuffer *intel_fb,
 	case DRM_FORMAT_UYVY:
 	case DRM_FORMAT_YVYU:
 	case DRM_FORMAT_VYUY:
-		if (INTEL_GEN(dev_priv) < 5) {
+		if (INTEL_GEN(dev_priv) < 5 && !IS_G4X(dev_priv)) {
 			DRM_DEBUG_KMS("unsupported pixel format: %s\n",
 				      drm_get_format_name(mode_cmd->pixel_format, &format_name));
 			goto err;
@@ -14931,6 +15005,7 @@ int intel_modeset_init(struct drm_device *dev)
 
 	dev->mode_config.funcs = &intel_mode_funcs;
 
+	init_llist_head(&dev_priv->atomic_helper.free_list);
 	INIT_WORK(&dev_priv->atomic_helper.free_work,
 		  intel_atomic_helper_free_state_worker);
 
@@ -15152,7 +15227,7 @@ static void intel_sanitize_crtc(struct intel_crtc *crtc)
 				continue;
 
 			trace_intel_disable_plane(&plane->base, crtc);
-			plane->disable_plane(&plane->base, &crtc->base);
+			plane->disable_plane(plane, crtc);
 		}
 	}
 
@@ -15523,7 +15598,10 @@ intel_modeset_setup_hw_state(struct drm_device *dev)
 		pll->on = false;
 	}
 
-	if (IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv)) {
+	if (IS_G4X(dev_priv)) {
+		g4x_wm_get_hw_state(dev);
+		g4x_wm_sanitize(dev_priv);
+	} else if (IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv)) {
 		vlv_wm_get_hw_state(dev);
 		vlv_wm_sanitize(dev_priv);
 	} else if (IS_GEN9(dev_priv)) {
@@ -15557,13 +15635,6 @@ void intel_display_resume(struct drm_device *dev)
 	if (state)
 		state->acquire_ctx = &ctx;
 
-	/*
-	 * This is a cludge because with real atomic modeset mode_config.mutex
-	 * won't be taken. Unfortunately some probed state like
-	 * audio_codec_enable is still protected by mode_config.mutex, so lock
-	 * it here for now.
-	 */
-	mutex_lock(&dev->mode_config.mutex);
 	drm_modeset_acquire_init(&ctx, 0);
 
 	while (1) {
@@ -15579,7 +15650,6 @@ void intel_display_resume(struct drm_device *dev)
 
 	drm_modeset_drop_locks(&ctx);
 	drm_modeset_acquire_fini(&ctx);
-	mutex_unlock(&dev->mode_config.mutex);
 
 	if (ret)
 		DRM_ERROR("Restoring old state failed with %i\n", ret);
diff --git a/drivers/gpu/drm/i915/intel_dp.c b/drivers/gpu/drm/i915/intel_dp.c
index ee77b519835c..49a1db3787c5 100644
--- a/drivers/gpu/drm/i915/intel_dp.c
+++ b/drivers/gpu/drm/i915/intel_dp.c
@@ -133,36 +133,55 @@ static void vlv_steal_power_sequencer(struct drm_device *dev,
 				      enum pipe pipe);
 static void intel_dp_unset_edid(struct intel_dp *intel_dp);
 
-static int
-intel_dp_max_link_bw(struct intel_dp  *intel_dp)
+static int intel_dp_num_rates(u8 link_bw_code)
 {
-	int max_link_bw = intel_dp->dpcd[DP_MAX_LINK_RATE];
-
-	switch (max_link_bw) {
+	switch (link_bw_code) {
+	default:
+		WARN(1, "invalid max DP link bw val %x, using 1.62Gbps\n",
+		     link_bw_code);
 	case DP_LINK_BW_1_62:
+		return 1;
 	case DP_LINK_BW_2_7:
+		return 2;
 	case DP_LINK_BW_5_4:
-		break;
-	default:
-		WARN(1, "invalid max DP link bw val %x, using 1.62Gbps\n",
-		     max_link_bw);
-		max_link_bw = DP_LINK_BW_1_62;
-		break;
+		return 3;
 	}
-	return max_link_bw;
 }
 
-static u8 intel_dp_max_lane_count(struct intel_dp *intel_dp)
+/* update sink rates from dpcd */
+static void intel_dp_set_sink_rates(struct intel_dp *intel_dp)
 {
-	struct intel_digital_port *intel_dig_port = dp_to_dig_port(intel_dp);
-	u8 source_max, sink_max;
+	int i, num_rates;
+
+	num_rates = intel_dp_num_rates(intel_dp->dpcd[DP_MAX_LINK_RATE]);
 
-	source_max = intel_dig_port->max_lanes;
-	sink_max = intel_dp->max_sink_lane_count;
+	for (i = 0; i < num_rates; i++)
+		intel_dp->sink_rates[i] = default_rates[i];
+
+	intel_dp->num_sink_rates = num_rates;
+}
+
+/* Theoretical max between source and sink */
+static int intel_dp_max_common_rate(struct intel_dp *intel_dp)
+{
+	return intel_dp->common_rates[intel_dp->num_common_rates - 1];
+}
+
+/* Theoretical max between source and sink */
+static int intel_dp_max_common_lane_count(struct intel_dp *intel_dp)
+{
+	struct intel_digital_port *intel_dig_port = dp_to_dig_port(intel_dp);
+	int source_max = intel_dig_port->max_lanes;
+	int sink_max = drm_dp_max_lane_count(intel_dp->dpcd);
 
 	return min(source_max, sink_max);
 }
 
+int intel_dp_max_lane_count(struct intel_dp *intel_dp)
+{
+	return intel_dp->max_link_lane_count;
+}
+
 int
 intel_dp_link_required(int pixel_clock, int bpp)
 {
@@ -205,34 +224,25 @@ intel_dp_downstream_max_dotclock(struct intel_dp *intel_dp)
 	return max_dotclk;
 }
 
-static int
-intel_dp_sink_rates(struct intel_dp *intel_dp, const int **sink_rates)
-{
-	if (intel_dp->num_sink_rates) {
-		*sink_rates = intel_dp->sink_rates;
-		return intel_dp->num_sink_rates;
-	}
-
-	*sink_rates = default_rates;
-
-	return (intel_dp->max_sink_link_bw >> 3) + 1;
-}
-
-static int
-intel_dp_source_rates(struct intel_dp *intel_dp, const int **source_rates)
+static void
+intel_dp_set_source_rates(struct intel_dp *intel_dp)
 {
 	struct intel_digital_port *dig_port = dp_to_dig_port(intel_dp);
 	struct drm_i915_private *dev_priv = to_i915(dig_port->base.base.dev);
+	const int *source_rates;
 	int size;
 
+	/* This should only be done once */
+	WARN_ON(intel_dp->source_rates || intel_dp->num_source_rates);
+
 	if (IS_GEN9_LP(dev_priv)) {
-		*source_rates = bxt_rates;
+		source_rates = bxt_rates;
 		size = ARRAY_SIZE(bxt_rates);
 	} else if (IS_GEN9_BC(dev_priv)) {
-		*source_rates = skl_rates;
+		source_rates = skl_rates;
 		size = ARRAY_SIZE(skl_rates);
 	} else {
-		*source_rates = default_rates;
+		source_rates = default_rates;
 		size = ARRAY_SIZE(default_rates);
 	}
 
@@ -240,7 +250,8 @@ intel_dp_source_rates(struct intel_dp *intel_dp, const int **source_rates)
 	if (!intel_dp_source_supports_hbr2(intel_dp))
 		size--;
 
-	return size;
+	intel_dp->source_rates = source_rates;
+	intel_dp->num_source_rates = size;
 }
 
 static int intersect_rates(const int *source_rates, int source_len,
@@ -266,50 +277,83 @@ static int intersect_rates(const int *source_rates, int source_len,
 	return k;
 }
 
-static int intel_dp_common_rates(struct intel_dp *intel_dp,
-				 int *common_rates)
+/* return index of rate in rates array, or -1 if not found */
+static int intel_dp_rate_index(const int *rates, int len, int rate)
 {
-	const int *source_rates, *sink_rates;
-	int source_len, sink_len;
+	int i;
 
-	sink_len = intel_dp_sink_rates(intel_dp, &sink_rates);
-	source_len = intel_dp_source_rates(intel_dp, &source_rates);
+	for (i = 0; i < len; i++)
+		if (rate == rates[i])
+			return i;
 
-	return intersect_rates(source_rates, source_len,
-			       sink_rates, sink_len,
-			       common_rates);
+	return -1;
 }
 
-static int intel_dp_link_rate_index(struct intel_dp *intel_dp,
-				    int *common_rates, int link_rate)
+static void intel_dp_set_common_rates(struct intel_dp *intel_dp)
 {
-	int common_len;
-	int index;
+	WARN_ON(!intel_dp->num_source_rates || !intel_dp->num_sink_rates);
+
+	intel_dp->num_common_rates = intersect_rates(intel_dp->source_rates,
+						     intel_dp->num_source_rates,
+						     intel_dp->sink_rates,
+						     intel_dp->num_sink_rates,
+						     intel_dp->common_rates);
+
+	/* Paranoia, there should always be something in common. */
+	if (WARN_ON(intel_dp->num_common_rates == 0)) {
+		intel_dp->common_rates[0] = default_rates[0];
+		intel_dp->num_common_rates = 1;
+	}
+}
 
-	common_len = intel_dp_common_rates(intel_dp, common_rates);
-	for (index = 0; index < common_len; index++) {
-		if (link_rate == common_rates[common_len - index - 1])
-			return common_len - index - 1;
+/* get length of common rates potentially limited by max_rate */
+static int intel_dp_common_len_rate_limit(struct intel_dp *intel_dp,
+					  int max_rate)
+{
+	const int *common_rates = intel_dp->common_rates;
+	int i, common_len = intel_dp->num_common_rates;
+
+	/* Limit results by potentially reduced max rate */
+	for (i = 0; i < common_len; i++) {
+		if (common_rates[common_len - i - 1] <= max_rate)
+			return common_len - i;
 	}
 
-	return -1;
+	return 0;
+}
+
+static bool intel_dp_link_params_valid(struct intel_dp *intel_dp)
+{
+	/*
+	 * FIXME: we need to synchronize the current link parameters with
+	 * hardware readout. Currently fast link training doesn't work on
+	 * boot-up.
+	 */
+	if (intel_dp->link_rate == 0 ||
+	    intel_dp->link_rate > intel_dp->max_link_rate)
+		return false;
+
+	if (intel_dp->lane_count == 0 ||
+	    intel_dp->lane_count > intel_dp_max_lane_count(intel_dp))
+		return false;
+
+	return true;
 }
 
 int intel_dp_get_link_train_fallback_values(struct intel_dp *intel_dp,
 					    int link_rate, uint8_t lane_count)
 {
-	int common_rates[DP_MAX_SUPPORTED_RATES];
-	int link_rate_index;
+	int index;
 
-	link_rate_index = intel_dp_link_rate_index(intel_dp,
-						   common_rates,
-						   link_rate);
-	if (link_rate_index > 0) {
-		intel_dp->max_sink_link_bw = drm_dp_link_rate_to_bw_code(common_rates[link_rate_index - 1]);
-		intel_dp->max_sink_lane_count = lane_count;
+	index = intel_dp_rate_index(intel_dp->common_rates,
+				    intel_dp->num_common_rates,
+				    link_rate);
+	if (index > 0) {
+		intel_dp->max_link_rate = intel_dp->common_rates[index - 1];
+		intel_dp->max_link_lane_count = lane_count;
 	} else if (lane_count > 1) {
-		intel_dp->max_sink_link_bw = intel_dp_max_link_bw(intel_dp);
-		intel_dp->max_sink_lane_count = lane_count >> 1;
+		intel_dp->max_link_rate = intel_dp_max_common_rate(intel_dp);
+		intel_dp->max_link_lane_count = lane_count >> 1;
 	} else {
 		DRM_ERROR("Link Training Unsuccessful\n");
 		return -1;
@@ -1486,24 +1530,21 @@ static void snprintf_int_array(char *str, size_t len,
 
 static void intel_dp_print_rates(struct intel_dp *intel_dp)
 {
-	const int *source_rates, *sink_rates;
-	int source_len, sink_len, common_len;
-	int common_rates[DP_MAX_SUPPORTED_RATES];
 	char str[128]; /* FIXME: too big for stack? */
 
 	if ((drm_debug & DRM_UT_KMS) == 0)
 		return;
 
-	source_len = intel_dp_source_rates(intel_dp, &source_rates);
-	snprintf_int_array(str, sizeof(str), source_rates, source_len);
+	snprintf_int_array(str, sizeof(str),
+			   intel_dp->source_rates, intel_dp->num_source_rates);
 	DRM_DEBUG_KMS("source rates: %s\n", str);
 
-	sink_len = intel_dp_sink_rates(intel_dp, &sink_rates);
-	snprintf_int_array(str, sizeof(str), sink_rates, sink_len);
+	snprintf_int_array(str, sizeof(str),
+			   intel_dp->sink_rates, intel_dp->num_sink_rates);
 	DRM_DEBUG_KMS("sink rates: %s\n", str);
 
-	common_len = intel_dp_common_rates(intel_dp, common_rates);
-	snprintf_int_array(str, sizeof(str), common_rates, common_len);
+	snprintf_int_array(str, sizeof(str),
+			   intel_dp->common_rates, intel_dp->num_common_rates);
 	DRM_DEBUG_KMS("common rates: %s\n", str);
 }
 
@@ -1538,39 +1579,34 @@ bool intel_dp_read_desc(struct intel_dp *intel_dp)
 	return true;
 }
 
-static int rate_to_index(int find, const int *rates)
-{
-	int i = 0;
-
-	for (i = 0; i < DP_MAX_SUPPORTED_RATES; ++i)
-		if (find == rates[i])
-			break;
-
-	return i;
-}
-
 int
 intel_dp_max_link_rate(struct intel_dp *intel_dp)
 {
-	int rates[DP_MAX_SUPPORTED_RATES] = {};
 	int len;
 
-	len = intel_dp_common_rates(intel_dp, rates);
+	len = intel_dp_common_len_rate_limit(intel_dp, intel_dp->max_link_rate);
 	if (WARN_ON(len <= 0))
 		return 162000;
 
-	return rates[len - 1];
+	return intel_dp->common_rates[len - 1];
 }
 
 int intel_dp_rate_select(struct intel_dp *intel_dp, int rate)
 {
-	return rate_to_index(rate, intel_dp->sink_rates);
+	int i = intel_dp_rate_index(intel_dp->sink_rates,
+				    intel_dp->num_sink_rates, rate);
+
+	if (WARN_ON(i < 0))
+		i = 0;
+
+	return i;
 }
 
 void intel_dp_compute_rate(struct intel_dp *intel_dp, int port_clock,
 			   uint8_t *link_bw, uint8_t *rate_select)
 {
-	if (intel_dp->num_sink_rates) {
+	/* eDP 1.4 rate select method. */
+	if (intel_dp->use_rate_select) {
 		*link_bw = 0;
 		*rate_select =
 			intel_dp_rate_select(intel_dp, port_clock);
@@ -1612,20 +1648,21 @@ intel_dp_compute_config(struct intel_encoder *encoder,
 	enum port port = dp_to_dig_port(intel_dp)->port;
 	struct intel_crtc *intel_crtc = to_intel_crtc(pipe_config->base.crtc);
 	struct intel_connector *intel_connector = intel_dp->attached_connector;
+	struct intel_digital_connector_state *intel_conn_state =
+		to_intel_digital_connector_state(conn_state);
 	int lane_count, clock;
 	int min_lane_count = 1;
 	int max_lane_count = intel_dp_max_lane_count(intel_dp);
 	/* Conveniently, the link BW constants become indices with a shift...*/
 	int min_clock = 0;
 	int max_clock;
-	int link_rate_index;
 	int bpp, mode_rate;
 	int link_avail, link_clock;
-	int common_rates[DP_MAX_SUPPORTED_RATES] = {};
 	int common_len;
 	uint8_t link_bw, rate_select;
 
-	common_len = intel_dp_common_rates(intel_dp, common_rates);
+	common_len = intel_dp_common_len_rate_limit(intel_dp,
+						    intel_dp->max_link_rate);
 
 	/* No common link rates between source and sink */
 	WARN_ON(common_len <= 0);
@@ -1636,7 +1673,12 @@ intel_dp_compute_config(struct intel_encoder *encoder,
 		pipe_config->has_pch_encoder = true;
 
 	pipe_config->has_drrs = false;
-	pipe_config->has_audio = intel_dp->has_audio && port != PORT_A;
+	if (port == PORT_A)
+		pipe_config->has_audio = false;
+	else if (intel_conn_state->force_audio == HDMI_AUDIO_AUTO)
+		pipe_config->has_audio = intel_dp->has_audio;
+	else
+		pipe_config->has_audio = intel_conn_state->force_audio == HDMI_AUDIO_ON;
 
 	if (is_edp(intel_dp) && intel_connector->panel.fixed_mode) {
 		intel_fixed_panel_mode(intel_connector->panel.fixed_mode,
@@ -1651,10 +1693,10 @@ intel_dp_compute_config(struct intel_encoder *encoder,
 
 		if (HAS_GMCH_DISPLAY(dev_priv))
 			intel_gmch_panel_fitting(intel_crtc, pipe_config,
-						 intel_connector->panel.fitting_mode);
+						 conn_state->scaling_mode);
 		else
 			intel_pch_panel_fitting(intel_crtc, pipe_config,
-						intel_connector->panel.fitting_mode);
+						conn_state->scaling_mode);
 	}
 
 	if (adjusted_mode->flags & DRM_MODE_FLAG_DBLCLK)
@@ -1662,16 +1704,18 @@ intel_dp_compute_config(struct intel_encoder *encoder,
 
 	/* Use values requested by Compliance Test Request */
 	if (intel_dp->compliance.test_type == DP_TEST_LINK_TRAINING) {
-		link_rate_index = intel_dp_link_rate_index(intel_dp,
-							   common_rates,
-							   intel_dp->compliance.test_link_rate);
-		if (link_rate_index >= 0)
-			min_clock = max_clock = link_rate_index;
+		int index;
+
+		index = intel_dp_rate_index(intel_dp->common_rates,
+					    intel_dp->num_common_rates,
+					    intel_dp->compliance.test_link_rate);
+		if (index >= 0)
+			min_clock = max_clock = index;
 		min_lane_count = max_lane_count = intel_dp->compliance.test_lane_count;
 	}
 	DRM_DEBUG_KMS("DP link computation with max lane count %i "
 		      "max bw %d pixel clock %iKHz\n",
-		      max_lane_count, common_rates[max_clock],
+		      max_lane_count, intel_dp->common_rates[max_clock],
 		      adjusted_mode->crtc_clock);
 
 	/* Walk through all bpp values. Luckily they're all nicely spaced with 2
@@ -1707,7 +1751,7 @@ intel_dp_compute_config(struct intel_encoder *encoder,
 				lane_count <= max_lane_count;
 				lane_count <<= 1) {
 
-				link_clock = common_rates[clock];
+				link_clock = intel_dp->common_rates[clock];
 				link_avail = intel_dp_max_data_rate(link_clock,
 								    lane_count);
 
@@ -1721,7 +1765,7 @@ intel_dp_compute_config(struct intel_encoder *encoder,
 	return false;
 
 found:
-	if (intel_dp->color_range_auto) {
+	if (intel_conn_state->broadcast_rgb == INTEL_BROADCAST_RGB_AUTO) {
 		/*
 		 * See:
 		 * CEA-861-E - 5.1 Default Encoding Parameters
@@ -1733,13 +1777,13 @@ found:
 			HDMI_QUANTIZATION_RANGE_LIMITED;
 	} else {
 		pipe_config->limited_color_range =
-			intel_dp->limited_color_range;
+			intel_conn_state->broadcast_rgb == INTEL_BROADCAST_RGB_LIMITED;
 	}
 
 	pipe_config->lane_count = lane_count;
 
 	pipe_config->pipe_bpp = bpp;
-	pipe_config->port_clock = common_rates[clock];
+	pipe_config->port_clock = intel_dp->common_rates[clock];
 
 	intel_dp_compute_rate(intel_dp, pipe_config->port_clock,
 			      &link_bw, &rate_select);
@@ -3051,7 +3095,8 @@ static bool intel_dp_get_y_cord_status(struct intel_dp *intel_dp)
 {
 	uint8_t psr_caps = 0;
 
-	drm_dp_dpcd_readb(&intel_dp->aux, DP_PSR_CAPS, &psr_caps);
+	if (drm_dp_dpcd_readb(&intel_dp->aux, DP_PSR_CAPS, &psr_caps) != 1)
+		return false;
 	return psr_caps & DP_PSR2_SU_Y_COORDINATE_REQUIRED;
 }
 
@@ -3059,9 +3104,9 @@ static bool intel_dp_get_colorimetry_status(struct intel_dp *intel_dp)
 {
 	uint8_t dprx = 0;
 
-	drm_dp_dpcd_readb(&intel_dp->aux,
-			DP_DPRX_FEATURE_ENUMERATION_LIST,
-			&dprx);
+	if (drm_dp_dpcd_readb(&intel_dp->aux, DP_DPRX_FEATURE_ENUMERATION_LIST,
+			      &dprx) != 1)
+		return false;
 	return dprx & DP_VSC_SDP_EXT_FOR_COLORIMETRY_SUPPORTED;
 }
 
@@ -3069,7 +3114,9 @@ static bool intel_dp_get_alpm_status(struct intel_dp *intel_dp)
 {
 	uint8_t alpm_caps = 0;
 
-	drm_dp_dpcd_readb(&intel_dp->aux, DP_RECEIVER_ALPM_CAP, &alpm_caps);
+	if (drm_dp_dpcd_readb(&intel_dp->aux, DP_RECEIVER_ALPM_CAP,
+			      &alpm_caps) != 1)
+		return false;
 	return alpm_caps & DP_ALPM_CAP;
 }
 
@@ -3642,9 +3689,10 @@ intel_edp_init_dpcd(struct intel_dp *intel_dp)
 		uint8_t frame_sync_cap;
 
 		dev_priv->psr.sink_support = true;
-		drm_dp_dpcd_read(&intel_dp->aux,
-				 DP_SINK_DEVICE_AUX_FRAME_SYNC_CAP,
-				 &frame_sync_cap, 1);
+		if (drm_dp_dpcd_readb(&intel_dp->aux,
+				      DP_SINK_DEVICE_AUX_FRAME_SYNC_CAP,
+				      &frame_sync_cap) != 1)
+			frame_sync_cap = 0;
 		dev_priv->psr.aux_frame_sync = frame_sync_cap ? true : false;
 		/* PSR2 needs frame sync as well */
 		dev_priv->psr.psr2_support = dev_priv->psr.aux_frame_sync;
@@ -3695,6 +3743,13 @@ intel_edp_init_dpcd(struct intel_dp *intel_dp)
 		intel_dp->num_sink_rates = i;
 	}
 
+	if (intel_dp->num_sink_rates)
+		intel_dp->use_rate_select = true;
+	else
+		intel_dp_set_sink_rates(intel_dp);
+
+	intel_dp_set_common_rates(intel_dp);
+
 	return true;
 }
 
@@ -3702,11 +3757,18 @@ intel_edp_init_dpcd(struct intel_dp *intel_dp)
 static bool
 intel_dp_get_dpcd(struct intel_dp *intel_dp)
 {
+	u8 sink_count;
+
 	if (!intel_dp_read_dpcd(intel_dp))
 		return false;
 
-	if (drm_dp_dpcd_read(&intel_dp->aux, DP_SINK_COUNT,
-			     &intel_dp->sink_count, 1) < 0)
+	/* Don't clobber cached eDP rates. */
+	if (!is_edp(intel_dp)) {
+		intel_dp_set_sink_rates(intel_dp);
+		intel_dp_set_common_rates(intel_dp);
+	}
+
+	if (drm_dp_dpcd_readb(&intel_dp->aux, DP_SINK_COUNT, &sink_count) <= 0)
 		return false;
 
 	/*
@@ -3714,7 +3776,7 @@ intel_dp_get_dpcd(struct intel_dp *intel_dp)
 	 * a member variable in intel_dp will track any changes
 	 * between short pulse interrupts.
 	 */
-	intel_dp->sink_count = DP_GET_SINK_COUNT(intel_dp->sink_count);
+	intel_dp->sink_count = DP_GET_SINK_COUNT(sink_count);
 
 	/*
 	 * SINK_COUNT == 0 and DOWNSTREAM_PORT_PRESENT == 1 implies that
@@ -3743,7 +3805,7 @@ intel_dp_get_dpcd(struct intel_dp *intel_dp)
 static bool
 intel_dp_can_mst(struct intel_dp *intel_dp)
 {
-	u8 buf[1];
+	u8 mstm_cap;
 
 	if (!i915.enable_dp_mst)
 		return false;
@@ -3754,10 +3816,10 @@ intel_dp_can_mst(struct intel_dp *intel_dp)
 	if (intel_dp->dpcd[DP_DPCD_REV] < 0x12)
 		return false;
 
-	if (drm_dp_dpcd_read(&intel_dp->aux, DP_MSTM_CAP, buf, 1) != 1)
+	if (drm_dp_dpcd_readb(&intel_dp->aux, DP_MSTM_CAP, &mstm_cap) != 1)
 		return false;
 
-	return buf[0] & DP_MST_CAP;
+	return mstm_cap & DP_MST_CAP;
 }
 
 static void
@@ -3903,9 +3965,8 @@ stop:
 static bool
 intel_dp_get_sink_irq(struct intel_dp *intel_dp, u8 *sink_irq_vector)
 {
-	return drm_dp_dpcd_read(&intel_dp->aux,
-				       DP_DEVICE_SERVICE_IRQ_VECTOR,
-				       sink_irq_vector, 1) == 1;
+	return drm_dp_dpcd_readb(&intel_dp->aux, DP_DEVICE_SERVICE_IRQ_VECTOR,
+				 sink_irq_vector) == 1;
 }
 
 static bool
@@ -3926,7 +3987,6 @@ static uint8_t intel_dp_autotest_link_training(struct intel_dp *intel_dp)
 {
 	int status = 0;
 	int min_lane_count = 1;
-	int common_rates[DP_MAX_SUPPORTED_RATES] = {};
 	int link_rate_index, test_link_rate;
 	uint8_t test_lane_count, test_link_bw;
 	/* (DP CTS 1.2)
@@ -3943,7 +4003,7 @@ static uint8_t intel_dp_autotest_link_training(struct intel_dp *intel_dp)
 	test_lane_count &= DP_MAX_LANE_COUNT_MASK;
 	/* Validate the requested lane count */
 	if (test_lane_count < min_lane_count ||
-	    test_lane_count > intel_dp->max_sink_lane_count)
+	    test_lane_count > intel_dp->max_link_lane_count)
 		return DP_TEST_NAK;
 
 	status = drm_dp_dpcd_readb(&intel_dp->aux, DP_TEST_LINK_RATE,
@@ -3954,9 +4014,9 @@ static uint8_t intel_dp_autotest_link_training(struct intel_dp *intel_dp)
 	}
 	/* Validate the requested link rate */
 	test_link_rate = drm_dp_bw_code_to_link_rate(test_link_bw);
-	link_rate_index = intel_dp_link_rate_index(intel_dp,
-						   common_rates,
-						   test_link_rate);
+	link_rate_index = intel_dp_rate_index(intel_dp->common_rates,
+					      intel_dp->num_common_rates,
+					      test_link_rate);
 	if (link_rate_index < 0)
 		return DP_TEST_NAK;
 
@@ -3969,13 +4029,13 @@ static uint8_t intel_dp_autotest_link_training(struct intel_dp *intel_dp)
 static uint8_t intel_dp_autotest_video_pattern(struct intel_dp *intel_dp)
 {
 	uint8_t test_pattern;
-	uint16_t test_misc;
+	uint8_t test_misc;
 	__be16 h_width, v_height;
 	int status = 0;
 
 	/* Read the TEST_PATTERN (DP CTS 3.1.5) */
-	status = drm_dp_dpcd_read(&intel_dp->aux, DP_TEST_PATTERN,
-				  &test_pattern, 1);
+	status = drm_dp_dpcd_readb(&intel_dp->aux, DP_TEST_PATTERN,
+				   &test_pattern);
 	if (status <= 0) {
 		DRM_DEBUG_KMS("Test pattern read failed\n");
 		return DP_TEST_NAK;
@@ -3997,8 +4057,8 @@ static uint8_t intel_dp_autotest_video_pattern(struct intel_dp *intel_dp)
 		return DP_TEST_NAK;
 	}
 
-	status = drm_dp_dpcd_read(&intel_dp->aux, DP_TEST_MISC0,
-				  &test_misc, 1);
+	status = drm_dp_dpcd_readb(&intel_dp->aux, DP_TEST_MISC0,
+				   &test_misc);
 	if (status <= 0) {
 		DRM_DEBUG_KMS("TEST MISC read failed\n");
 		return DP_TEST_NAK;
@@ -4057,10 +4117,8 @@ static uint8_t intel_dp_autotest_edid(struct intel_dp *intel_dp)
 		 */
 		block += intel_connector->detect_edid->extensions;
 
-		if (!drm_dp_dpcd_write(&intel_dp->aux,
-					DP_TEST_EDID_CHECKSUM,
-					&block->checksum,
-					1))
+		if (drm_dp_dpcd_writeb(&intel_dp->aux, DP_TEST_EDID_CHECKSUM,
+				       block->checksum) <= 0)
 			DRM_DEBUG_KMS("Failed to write EDID checksum\n");
 
 		test_result = DP_TEST_ACK | DP_TEST_EDID_CHECKSUM_WRITE;
@@ -4224,9 +4282,11 @@ intel_dp_check_link_status(struct intel_dp *intel_dp)
 	if (!to_intel_crtc(intel_encoder->base.crtc)->active)
 		return;
 
-	/* FIXME: we need to synchronize this sort of stuff with hardware
-	 * readout. Currently fast link training doesn't work on boot-up. */
-	if (!intel_dp->lane_count)
+	/*
+	 * Validate the cached values of intel_dp->link_rate and
+	 * intel_dp->lane_count before attempting to retrain.
+	 */
+	if (!intel_dp_link_params_valid(intel_dp))
 		return;
 
 	/* Retrain if Channel EQ or CR not ok */
@@ -4549,10 +4609,7 @@ intel_dp_set_edid(struct intel_dp *intel_dp)
 	edid = intel_dp_get_edid(intel_dp);
 	intel_connector->detect_edid = edid;
 
-	if (intel_dp->force_audio != HDMI_AUDIO_AUTO)
-		intel_dp->has_audio = intel_dp->force_audio == HDMI_AUDIO_ON;
-	else
-		intel_dp->has_audio = drm_detect_monitor_audio(edid);
+	intel_dp->has_audio = drm_detect_monitor_audio(edid);
 }
 
 static void
@@ -4613,11 +4670,11 @@ intel_dp_long_pulse(struct intel_connector *intel_connector)
 		      yesno(drm_dp_tps3_supported(intel_dp->dpcd)));
 
 	if (intel_dp->reset_link_params) {
-		/* Set the max lane count for sink */
-		intel_dp->max_sink_lane_count = drm_dp_max_lane_count(intel_dp->dpcd);
+		/* Initial max link lane count */
+		intel_dp->max_link_lane_count = intel_dp_max_common_lane_count(intel_dp);
 
-		/* Set the max link BW for sink */
-		intel_dp->max_sink_link_bw = intel_dp_max_link_bw(intel_dp);
+		/* Initial max link rate */
+		intel_dp->max_link_rate = intel_dp_max_common_rate(intel_dp);
 
 		intel_dp->reset_link_params = false;
 	}
@@ -4760,112 +4817,6 @@ static int intel_dp_get_modes(struct drm_connector *connector)
 	return 0;
 }
 
-static bool
-intel_dp_detect_audio(struct drm_connector *connector)
-{
-	bool has_audio = false;
-	struct edid *edid;
-
-	edid = to_intel_connector(connector)->detect_edid;
-	if (edid)
-		has_audio = drm_detect_monitor_audio(edid);
-
-	return has_audio;
-}
-
-static int
-intel_dp_set_property(struct drm_connector *connector,
-		      struct drm_property *property,
-		      uint64_t val)
-{
-	struct drm_i915_private *dev_priv = to_i915(connector->dev);
-	struct intel_connector *intel_connector = to_intel_connector(connector);
-	struct intel_encoder *intel_encoder = intel_attached_encoder(connector);
-	struct intel_dp *intel_dp = enc_to_intel_dp(&intel_encoder->base);
-	int ret;
-
-	ret = drm_object_property_set_value(&connector->base, property, val);
-	if (ret)
-		return ret;
-
-	if (property == dev_priv->force_audio_property) {
-		int i = val;
-		bool has_audio;
-
-		if (i == intel_dp->force_audio)
-			return 0;
-
-		intel_dp->force_audio = i;
-
-		if (i == HDMI_AUDIO_AUTO)
-			has_audio = intel_dp_detect_audio(connector);
-		else
-			has_audio = (i == HDMI_AUDIO_ON);
-
-		if (has_audio == intel_dp->has_audio)
-			return 0;
-
-		intel_dp->has_audio = has_audio;
-		goto done;
-	}
-
-	if (property == dev_priv->broadcast_rgb_property) {
-		bool old_auto = intel_dp->color_range_auto;
-		bool old_range = intel_dp->limited_color_range;
-
-		switch (val) {
-		case INTEL_BROADCAST_RGB_AUTO:
-			intel_dp->color_range_auto = true;
-			break;
-		case INTEL_BROADCAST_RGB_FULL:
-			intel_dp->color_range_auto = false;
-			intel_dp->limited_color_range = false;
-			break;
-		case INTEL_BROADCAST_RGB_LIMITED:
-			intel_dp->color_range_auto = false;
-			intel_dp->limited_color_range = true;
-			break;
-		default:
-			return -EINVAL;
-		}
-
-		if (old_auto == intel_dp->color_range_auto &&
-		    old_range == intel_dp->limited_color_range)
-			return 0;
-
-		goto done;
-	}
-
-	if (is_edp(intel_dp) &&
-	    property == connector->dev->mode_config.scaling_mode_property) {
-		if (val == DRM_MODE_SCALE_NONE) {
-			DRM_DEBUG_KMS("no scaling not supported\n");
-			return -EINVAL;
-		}
-		if (HAS_GMCH_DISPLAY(dev_priv) &&
-		    val == DRM_MODE_SCALE_CENTER) {
-			DRM_DEBUG_KMS("centering not supported\n");
-			return -EINVAL;
-		}
-
-		if (intel_connector->panel.fitting_mode == val) {
-			/* the eDP scaling property is not changed */
-			return 0;
-		}
-		intel_connector->panel.fitting_mode = val;
-
-		goto done;
-	}
-
-	return -EINVAL;
-
-done:
-	if (intel_encoder->base.crtc)
-		intel_crtc_restore_mode(intel_encoder->base.crtc);
-
-	return 0;
-}
-
 static int
 intel_dp_connector_register(struct drm_connector *connector)
 {
@@ -5024,19 +4975,21 @@ static const struct drm_connector_funcs intel_dp_connector_funcs = {
 	.dpms = drm_atomic_helper_connector_dpms,
 	.force = intel_dp_force,
 	.fill_modes = drm_helper_probe_single_connector_modes,
-	.set_property = intel_dp_set_property,
-	.atomic_get_property = intel_connector_atomic_get_property,
+	.set_property = drm_atomic_helper_connector_set_property,
+	.atomic_get_property = intel_digital_connector_atomic_get_property,
+	.atomic_set_property = intel_digital_connector_atomic_set_property,
 	.late_register = intel_dp_connector_register,
 	.early_unregister = intel_dp_connector_unregister,
 	.destroy = intel_dp_connector_destroy,
 	.atomic_destroy_state = drm_atomic_helper_connector_destroy_state,
-	.atomic_duplicate_state = drm_atomic_helper_connector_duplicate_state,
+	.atomic_duplicate_state = intel_digital_connector_duplicate_state,
 };
 
 static const struct drm_connector_helper_funcs intel_dp_connector_helper_funcs = {
 	.detect_ctx = intel_dp_detect,
 	.get_modes = intel_dp_get_modes,
 	.mode_valid = intel_dp_mode_valid,
+	.atomic_check = intel_digital_connector_atomic_check,
 };
 
 static const struct drm_encoder_funcs intel_dp_enc_funcs = {
@@ -5127,22 +5080,25 @@ bool intel_dp_is_edp(struct drm_i915_private *dev_priv, enum port port)
 	return intel_bios_is_port_edp(dev_priv, port);
 }
 
-void
+static void
 intel_dp_add_properties(struct intel_dp *intel_dp, struct drm_connector *connector)
 {
-	struct intel_connector *intel_connector = to_intel_connector(connector);
+	struct drm_i915_private *dev_priv = to_i915(connector->dev);
 
 	intel_attach_force_audio_property(connector);
 	intel_attach_broadcast_rgb_property(connector);
-	intel_dp->color_range_auto = true;
 
 	if (is_edp(intel_dp)) {
-		drm_mode_create_scaling_mode_property(connector->dev);
-		drm_object_attach_property(
-			&connector->base,
-			connector->dev->mode_config.scaling_mode_property,
-			DRM_MODE_SCALE_ASPECT);
-		intel_connector->panel.fitting_mode = DRM_MODE_SCALE_ASPECT;
+		u32 allowed_scalers;
+
+		allowed_scalers = BIT(DRM_MODE_SCALE_ASPECT) | BIT(DRM_MODE_SCALE_FULLSCREEN);
+		if (!HAS_GMCH_DISPLAY(dev_priv))
+			allowed_scalers |= BIT(DRM_MODE_SCALE_CENTER);
+
+		drm_connector_attach_scaling_mode_property(connector, allowed_scalers);
+
+		connector->state->scaling_mode = DRM_MODE_SCALE_ASPECT;
+
 	}
 }
 
@@ -5932,6 +5888,29 @@ intel_dp_init_connector_port_info(struct intel_digital_port *intel_dig_port)
 	}
 }
 
+static void intel_dp_modeset_retry_work_fn(struct work_struct *work)
+{
+	struct intel_connector *intel_connector;
+	struct drm_connector *connector;
+
+	intel_connector = container_of(work, typeof(*intel_connector),
+				       modeset_retry_work);
+	connector = &intel_connector->base;
+	DRM_DEBUG_KMS("[CONNECTOR:%d:%s]\n", connector->base.id,
+		      connector->name);
+
+	/* Grab the locks before changing connector property*/
+	mutex_lock(&connector->dev->mode_config.mutex);
+	/* Set connector link status to BAD and send a Uevent to notify
+	 * userspace to do a modeset.
+	 */
+	drm_mode_connector_set_link_status_property(connector,
+						    DRM_MODE_LINK_STATUS_BAD);
+	mutex_unlock(&connector->dev->mode_config.mutex);
+	/* Send Hotplug uevent so userspace can reprobe */
+	drm_kms_helper_hotplug_event(connector->dev);
+}
+
 bool
 intel_dp_init_connector(struct intel_digital_port *intel_dig_port,
 			struct intel_connector *intel_connector)
@@ -5944,11 +5923,17 @@ intel_dp_init_connector(struct intel_digital_port *intel_dig_port,
 	enum port port = intel_dig_port->port;
 	int type;
 
+	/* Initialize the work for modeset in case of link train failure */
+	INIT_WORK(&intel_connector->modeset_retry_work,
+		  intel_dp_modeset_retry_work_fn);
+
 	if (WARN(intel_dig_port->max_lanes < 1,
 		 "Not enough lanes (%d) for DP on port %c\n",
 		 intel_dig_port->max_lanes, port_name(port)))
 		return false;
 
+	intel_dp_set_source_rates(intel_dp);
+
 	intel_dp->reset_link_params = true;
 	intel_dp->pps_pipe = INVALID_PIPE;
 	intel_dp->active_pipe = INVALID_PIPE;
diff --git a/drivers/gpu/drm/i915/intel_dp_aux_backlight.c b/drivers/gpu/drm/i915/intel_dp_aux_backlight.c
index 6532e226db29..a0995c00fc84 100644
--- a/drivers/gpu/drm/i915/intel_dp_aux_backlight.c
+++ b/drivers/gpu/drm/i915/intel_dp_aux_backlight.c
@@ -28,6 +28,10 @@ static void set_aux_backlight_enable(struct intel_dp *intel_dp, bool enable)
 {
 	uint8_t reg_val = 0;
 
+	/* Early return when display use other mechanism to enable backlight. */
+	if (!(intel_dp->edp_dpcd[1] & DP_EDP_BACKLIGHT_AUX_ENABLE_CAP))
+		return;
+
 	if (drm_dp_dpcd_readb(&intel_dp->aux, DP_EDP_DISPLAY_CONTROL_REGISTER,
 			      &reg_val) < 0) {
 		DRM_DEBUG_KMS("Failed to read DPCD register 0x%x\n",
@@ -97,15 +101,37 @@ static void intel_dp_aux_enable_backlight(struct intel_connector *connector)
 {
 	struct intel_dp *intel_dp = enc_to_intel_dp(&connector->encoder->base);
 	uint8_t dpcd_buf = 0;
+	uint8_t edp_backlight_mode = 0;
 
-	set_aux_backlight_enable(intel_dp, true);
+	if (drm_dp_dpcd_readb(&intel_dp->aux,
+			DP_EDP_BACKLIGHT_MODE_SET_REGISTER, &dpcd_buf) != 1) {
+		DRM_DEBUG_KMS("Failed to read DPCD register 0x%x\n",
+			      DP_EDP_BACKLIGHT_MODE_SET_REGISTER);
+		return;
+	}
+
+	edp_backlight_mode = dpcd_buf & DP_EDP_BACKLIGHT_CONTROL_MODE_MASK;
+
+	switch (edp_backlight_mode) {
+	case DP_EDP_BACKLIGHT_CONTROL_MODE_PWM:
+	case DP_EDP_BACKLIGHT_CONTROL_MODE_PRESET:
+	case DP_EDP_BACKLIGHT_CONTROL_MODE_PRODUCT:
+		dpcd_buf &= ~DP_EDP_BACKLIGHT_CONTROL_MODE_MASK;
+		dpcd_buf |= DP_EDP_BACKLIGHT_CONTROL_MODE_DPCD;
+		if (drm_dp_dpcd_writeb(&intel_dp->aux,
+			DP_EDP_BACKLIGHT_MODE_SET_REGISTER, dpcd_buf) < 0) {
+			DRM_DEBUG_KMS("Failed to write aux backlight mode\n");
+		}
+		break;
+
+	/* Do nothing when it is already DPCD mode */
+	case DP_EDP_BACKLIGHT_CONTROL_MODE_DPCD:
+	default:
+		break;
+	}
 
-	if ((drm_dp_dpcd_readb(&intel_dp->aux,
-			       DP_EDP_BACKLIGHT_MODE_SET_REGISTER, &dpcd_buf) == 1) &&
-	    ((dpcd_buf & DP_EDP_BACKLIGHT_CONTROL_MODE_MASK) ==
-	     DP_EDP_BACKLIGHT_CONTROL_MODE_PRESET))
-		drm_dp_dpcd_writeb(&intel_dp->aux, DP_EDP_BACKLIGHT_MODE_SET_REGISTER,
-				   (dpcd_buf | DP_EDP_BACKLIGHT_CONTROL_MODE_DPCD));
+	set_aux_backlight_enable(intel_dp, true);
+	intel_dp_aux_set_backlight(connector, connector->panel.backlight.level);
 }
 
 static void intel_dp_aux_disable_backlight(struct intel_connector *connector)
@@ -143,9 +169,8 @@ intel_dp_aux_display_control_capable(struct intel_connector *connector)
 	 * the panel can support backlight control over the aux channel
 	 */
 	if (intel_dp->edp_dpcd[1] & DP_EDP_TCON_BACKLIGHT_ADJUSTMENT_CAP &&
-	    (intel_dp->edp_dpcd[1] & DP_EDP_BACKLIGHT_AUX_ENABLE_CAP) &&
-	    !((intel_dp->edp_dpcd[1] & DP_EDP_BACKLIGHT_PIN_ENABLE_CAP) ||
-	      (intel_dp->edp_dpcd[2] & DP_EDP_BACKLIGHT_BRIGHTNESS_PWM_PIN_CAP))) {
+	    (intel_dp->edp_dpcd[2] & DP_EDP_BACKLIGHT_BRIGHTNESS_AUX_SET_CAP) &&
+	    !(intel_dp->edp_dpcd[2] & DP_EDP_BACKLIGHT_BRIGHTNESS_PWM_PIN_CAP)) {
 		DRM_DEBUG_KMS("AUX Backlight Control Supported!\n");
 		return true;
 	}
diff --git a/drivers/gpu/drm/i915/intel_dp_link_training.c b/drivers/gpu/drm/i915/intel_dp_link_training.c
index 0048b520baf7..b79c1c0e404c 100644
--- a/drivers/gpu/drm/i915/intel_dp_link_training.c
+++ b/drivers/gpu/drm/i915/intel_dp_link_training.c
@@ -146,7 +146,8 @@ intel_dp_link_training_clock_recovery(struct intel_dp *intel_dp)
 		link_config[1] |= DP_LANE_COUNT_ENHANCED_FRAME_EN;
 	drm_dp_dpcd_write(&intel_dp->aux, DP_LINK_BW_SET, link_config, 2);
 
-	if (intel_dp->num_sink_rates)
+	/* eDP 1.4 rate select method. */
+	if (!link_bw)
 		drm_dp_dpcd_write(&intel_dp->aux, DP_LINK_RATE_SET,
 				  &rate_select, 1);
 
@@ -313,6 +314,24 @@ void intel_dp_stop_link_train(struct intel_dp *intel_dp)
 void
 intel_dp_start_link_train(struct intel_dp *intel_dp)
 {
-	intel_dp_link_training_clock_recovery(intel_dp);
-	intel_dp_link_training_channel_equalization(intel_dp);
+	struct intel_connector *intel_connector = intel_dp->attached_connector;
+
+	if (!intel_dp_link_training_clock_recovery(intel_dp))
+		goto failure_handling;
+	if (!intel_dp_link_training_channel_equalization(intel_dp))
+		goto failure_handling;
+
+	DRM_DEBUG_KMS("Link Training Passed at Link Rate = %d, Lane count = %d",
+		      intel_dp->link_rate, intel_dp->lane_count);
+	return;
+
+ failure_handling:
+	DRM_DEBUG_KMS("Link Training failed at link rate = %d, lane count = %d",
+		      intel_dp->link_rate, intel_dp->lane_count);
+	if (!intel_dp_get_link_train_fallback_values(intel_dp,
+						     intel_dp->link_rate,
+						     intel_dp->lane_count))
+		/* Schedule a Hotplug Uevent to userspace to start modeset */
+		schedule_work(&intel_connector->modeset_retry_work);
+	return;
 }
diff --git a/drivers/gpu/drm/i915/intel_dp_mst.c b/drivers/gpu/drm/i915/intel_dp_mst.c
index 1dee9933005f..3715386e4272 100644
--- a/drivers/gpu/drm/i915/intel_dp_mst.c
+++ b/drivers/gpu/drm/i915/intel_dp_mst.c
@@ -56,7 +56,8 @@ static bool intel_dp_mst_compute_config(struct intel_encoder *encoder,
 	 * for MST we always configure max link bw - the spec doesn't
 	 * seem to suggest we should do otherwise.
 	 */
-	lane_count = drm_dp_max_lane_count(intel_dp->dpcd);
+	lane_count = intel_dp_max_lane_count(intel_dp);
+
 	pipe_config->lane_count = lane_count;
 
 	pipe_config->pipe_bpp = bpp;
@@ -329,14 +330,6 @@ intel_dp_mst_detect(struct drm_connector *connector, bool force)
 	return drm_dp_mst_detect_port(connector, &intel_dp->mst_mgr, intel_connector->port);
 }
 
-static int
-intel_dp_mst_set_property(struct drm_connector *connector,
-			  struct drm_property *property,
-			  uint64_t val)
-{
-	return 0;
-}
-
 static void
 intel_dp_mst_connector_destroy(struct drm_connector *connector)
 {
@@ -353,8 +346,7 @@ static const struct drm_connector_funcs intel_dp_mst_connector_funcs = {
 	.dpms = drm_atomic_helper_connector_dpms,
 	.detect = intel_dp_mst_detect,
 	.fill_modes = drm_helper_probe_single_connector_modes,
-	.set_property = intel_dp_mst_set_property,
-	.atomic_get_property = intel_connector_atomic_get_property,
+	.set_property = drm_atomic_helper_connector_set_property,
 	.late_register = intel_connector_register,
 	.early_unregister = intel_connector_unregister,
 	.destroy = intel_dp_mst_connector_destroy,
@@ -378,7 +370,7 @@ intel_dp_mst_mode_valid(struct drm_connector *connector,
 	int max_rate, mode_rate, max_lanes, max_link_clock;
 
 	max_link_clock = intel_dp_max_link_rate(intel_dp);
-	max_lanes = drm_dp_max_lane_count(intel_dp->dpcd);
+	max_lanes = intel_dp_max_lane_count(intel_dp);
 
 	max_rate = intel_dp_max_data_rate(max_link_clock, max_lanes);
 	mode_rate = intel_dp_link_required(mode->clock, bpp);
@@ -495,7 +487,6 @@ static struct drm_connector *intel_dp_add_mst_connector(struct drm_dp_mst_topolo
 		drm_mode_connector_attach_encoder(&intel_connector->base,
 						  &intel_dp->mst_encoders[i]->base.base);
 	}
-	intel_dp_add_properties(intel_dp, connector);
 
 	drm_object_attach_property(&connector->base, dev->mode_config.path_property, 0);
 	drm_object_attach_property(&connector->base, dev->mode_config.tile_property, 0);
diff --git a/drivers/gpu/drm/i915/intel_drv.h b/drivers/gpu/drm/i915/intel_drv.h
index 48ea8d9d49fe..f63e8aa76e9a 100644
--- a/drivers/gpu/drm/i915/intel_drv.h
+++ b/drivers/gpu/drm/i915/intel_drv.h
@@ -88,7 +88,6 @@
 	int cpu, ret, timeout = (US) * 1000; \
 	u64 base; \
 	_WAIT_FOR_ATOMIC_CHECK(ATOMIC); \
-	BUILD_BUG_ON((US) > 50000); \
 	if (!(ATOMIC)) { \
 		preempt_disable(); \
 		cpu = smp_processor_id(); \
@@ -130,8 +129,14 @@
 	ret__; \
 })
 
-#define wait_for_atomic(COND, MS)	_wait_for_atomic((COND), (MS) * 1000, 1)
-#define wait_for_atomic_us(COND, US)	_wait_for_atomic((COND), (US), 1)
+#define wait_for_atomic_us(COND, US) \
+({ \
+	BUILD_BUG_ON(!__builtin_constant_p(US)); \
+	BUILD_BUG_ON((US) > 50000); \
+	_wait_for_atomic((COND), (US), 1); \
+})
+
+#define wait_for_atomic(COND, MS) wait_for_atomic_us((COND), (MS) * 1000)
 
 #define KHz(x) (1000 * (x))
 #define MHz(x) KHz(1000 * (x))
@@ -261,7 +266,6 @@ struct intel_encoder {
 struct intel_panel {
 	struct drm_display_mode *fixed_mode;
 	struct drm_display_mode *downclock_mode;
-	int fitting_mode;
 
 	/* backlight */
 	struct {
@@ -321,8 +325,20 @@ struct intel_connector {
 	void *port; /* store this opaque as its illegal to dereference it */
 
 	struct intel_dp *mst_port;
+
+	/* Work struct to schedule a uevent on link train failure */
+	struct work_struct modeset_retry_work;
 };
 
+struct intel_digital_connector_state {
+	struct drm_connector_state base;
+
+	enum hdmi_force_audio force_audio;
+	int broadcast_rgb;
+};
+
+#define to_intel_digital_connector_state(x) container_of(x, struct intel_digital_connector_state, base)
+
 struct dpll {
 	/* given values */
 	int n;
@@ -504,8 +520,8 @@ enum vlv_wm_level {
 };
 
 struct vlv_wm_state {
-	struct vlv_pipe_wm wm[NUM_VLV_WM_LEVELS];
-	struct vlv_sr_wm sr[NUM_VLV_WM_LEVELS];
+	struct g4x_pipe_wm wm[NUM_VLV_WM_LEVELS];
+	struct g4x_sr_wm sr[NUM_VLV_WM_LEVELS];
 	uint8_t num_levels;
 	bool cxsr;
 };
@@ -514,6 +530,22 @@ struct vlv_fifo_state {
 	u16 plane[I915_MAX_PLANES];
 };
 
+enum g4x_wm_level {
+	G4X_WM_LEVEL_NORMAL,
+	G4X_WM_LEVEL_SR,
+	G4X_WM_LEVEL_HPLL,
+	NUM_G4X_WM_LEVELS,
+};
+
+struct g4x_wm_state {
+	struct g4x_pipe_wm wm;
+	struct g4x_sr_wm sr;
+	struct g4x_sr_wm hpll;
+	bool cxsr;
+	bool hpll_en;
+	bool fbc_en;
+};
+
 struct intel_crtc_wm_state {
 	union {
 		struct {
@@ -541,7 +573,7 @@ struct intel_crtc_wm_state {
 
 		struct {
 			/* "raw" watermarks (not inverted) */
-			struct vlv_pipe_wm raw[NUM_VLV_WM_LEVELS];
+			struct g4x_pipe_wm raw[NUM_VLV_WM_LEVELS];
 			/* intermediate watermarks (inverted) */
 			struct vlv_wm_state intermediate;
 			/* optimal watermarks (inverted) */
@@ -549,6 +581,15 @@ struct intel_crtc_wm_state {
 			/* display FIFO split */
 			struct vlv_fifo_state fifo_state;
 		} vlv;
+
+		struct {
+			/* "raw" watermarks */
+			struct g4x_pipe_wm raw[NUM_G4X_WM_LEVELS];
+			/* intermediate watermarks */
+			struct g4x_wm_state intermediate;
+			/* optimal watermarks */
+			struct g4x_wm_state optimal;
+		} g4x;
 	};
 
 	/*
@@ -766,11 +807,6 @@ struct intel_crtc {
 	int adjusted_x;
 	int adjusted_y;
 
-	uint32_t cursor_addr;
-	uint32_t cursor_cntl;
-	uint32_t cursor_size;
-	uint32_t cursor_base;
-
 	struct intel_crtc_state *config;
 
 	/* global reset count when the last flip was submitted */
@@ -786,6 +822,7 @@ struct intel_crtc {
 		union {
 			struct intel_pipe_wm ilk;
 			struct vlv_wm_state vlv;
+			struct g4x_wm_state g4x;
 		} active;
 	} wm;
 
@@ -811,18 +848,22 @@ struct intel_plane {
 	int max_downscale;
 	uint32_t frontbuffer_bit;
 
+	struct {
+		u32 base, cntl, size;
+	} cursor;
+
 	/*
 	 * NOTE: Do not place new plane state fields here (e.g., when adding
 	 * new plane properties).  New runtime state should now be placed in
 	 * the intel_plane_state structure and accessed via plane_state.
 	 */
 
-	void (*update_plane)(struct drm_plane *plane,
+	void (*update_plane)(struct intel_plane *plane,
 			     const struct intel_crtc_state *crtc_state,
 			     const struct intel_plane_state *plane_state);
-	void (*disable_plane)(struct drm_plane *plane,
-			      struct drm_crtc *crtc);
-	int (*check_plane)(struct drm_plane *plane,
+	void (*disable_plane)(struct intel_plane *plane,
+			      struct intel_crtc *crtc);
+	int (*check_plane)(struct intel_plane *plane,
 			   struct intel_crtc_state *crtc_state,
 			   struct intel_plane_state *state);
 };
@@ -863,11 +904,8 @@ struct intel_hdmi {
 		enum drm_dp_dual_mode_type type;
 		int max_tmds_clock;
 	} dp_dual_mode;
-	bool limited_color_range;
-	bool color_range_auto;
 	bool has_hdmi_sink;
 	bool has_audio;
-	enum hdmi_force_audio force_audio;
 	bool rgb_quant_range_selectable;
 	struct intel_connector *attached_connector;
 	void (*write_infoframe)(struct drm_encoder *encoder,
@@ -941,20 +979,24 @@ struct intel_dp {
 	bool detect_done;
 	bool channel_eq_status;
 	bool reset_link_params;
-	enum hdmi_force_audio force_audio;
-	bool limited_color_range;
-	bool color_range_auto;
 	uint8_t dpcd[DP_RECEIVER_CAP_SIZE];
 	uint8_t psr_dpcd[EDP_PSR_RECEIVER_CAP_SIZE];
 	uint8_t downstream_ports[DP_MAX_DOWNSTREAM_PORTS];
 	uint8_t edp_dpcd[EDP_DISPLAY_CTL_CAP_SIZE];
-	/* sink rates as reported by DP_SUPPORTED_LINK_RATES */
-	uint8_t num_sink_rates;
+	/* source rates */
+	int num_source_rates;
+	const int *source_rates;
+	/* sink rates as reported by DP_MAX_LINK_RATE/DP_SUPPORTED_LINK_RATES */
+	int num_sink_rates;
 	int sink_rates[DP_MAX_SUPPORTED_RATES];
-	/* Max lane count for the sink as per DPCD registers */
-	uint8_t max_sink_lane_count;
-	/* Max link BW for the sink as per DPCD registers */
-	int max_sink_link_bw;
+	bool use_rate_select;
+	/* intersection of source and sink rates */
+	int num_common_rates;
+	int common_rates[DP_MAX_SUPPORTED_RATES];
+	/* Max lane count for the current link */
+	int max_link_lane_count;
+	/* Max rate for the current link */
+	int max_link_rate;
 	/* sink or branch descriptor */
 	struct intel_dp_desc desc;
 	struct drm_dp_aux aux;
@@ -1307,7 +1349,6 @@ unsigned int intel_rotation_info_size(const struct intel_rotation_info *rot_info
 bool intel_has_pending_fb_unpin(struct drm_i915_private *dev_priv);
 void intel_mark_busy(struct drm_i915_private *dev_priv);
 void intel_mark_idle(struct drm_i915_private *dev_priv);
-void intel_crtc_restore_mode(struct drm_crtc *crtc);
 int intel_display_suspend(struct drm_device *dev);
 void intel_pps_unlock_regs_wa(struct drm_i915_private *dev_priv);
 void intel_encoder_destroy(struct drm_encoder *encoder);
@@ -1491,10 +1532,10 @@ void intel_edp_backlight_off(struct intel_dp *intel_dp);
 void intel_edp_panel_vdd_on(struct intel_dp *intel_dp);
 void intel_edp_panel_on(struct intel_dp *intel_dp);
 void intel_edp_panel_off(struct intel_dp *intel_dp);
-void intel_dp_add_properties(struct intel_dp *intel_dp, struct drm_connector *connector);
 void intel_dp_mst_suspend(struct drm_device *dev);
 void intel_dp_mst_resume(struct drm_device *dev);
 int intel_dp_max_link_rate(struct intel_dp *intel_dp);
+int intel_dp_max_lane_count(struct intel_dp *intel_dp);
 int intel_dp_rate_select(struct intel_dp *intel_dp, int rate);
 void intel_dp_hot_plug(struct intel_encoder *intel_encoder);
 void intel_power_sequencer_reset(struct drm_i915_private *dev_priv);
@@ -1825,6 +1866,7 @@ void gen6_rps_boost(struct drm_i915_private *dev_priv,
 		    struct intel_rps_client *rps,
 		    unsigned long submitted);
 void intel_queue_rps_boost_for_request(struct drm_i915_gem_request *req);
+void g4x_wm_get_hw_state(struct drm_device *dev);
 void vlv_wm_get_hw_state(struct drm_device *dev);
 void ilk_wm_get_hw_state(struct drm_device *dev);
 void skl_wm_get_hw_state(struct drm_device *dev);
@@ -1832,6 +1874,7 @@ void skl_ddb_get_hw_state(struct drm_i915_private *dev_priv,
 			  struct skl_ddb_allocation *ddb /* out */);
 void skl_pipe_wm_get_hw_state(struct drm_crtc *crtc,
 			      struct skl_pipe_wm *out);
+void g4x_wm_sanitize(struct drm_i915_private *dev_priv);
 void vlv_wm_sanitize(struct drm_i915_private *dev_priv);
 bool intel_can_enable_sagv(struct drm_atomic_state *state);
 int intel_enable_sagv(struct drm_i915_private *dev_priv);
@@ -1867,10 +1910,19 @@ void intel_pipe_update_end(struct intel_crtc *crtc, struct intel_flip_work *work
 void intel_tv_init(struct drm_i915_private *dev_priv);
 
 /* intel_atomic.c */
-int intel_connector_atomic_get_property(struct drm_connector *connector,
-					const struct drm_connector_state *state,
-					struct drm_property *property,
-					uint64_t *val);
+int intel_digital_connector_atomic_get_property(struct drm_connector *connector,
+						const struct drm_connector_state *state,
+						struct drm_property *property,
+						uint64_t *val);
+int intel_digital_connector_atomic_set_property(struct drm_connector *connector,
+						struct drm_connector_state *state,
+						struct drm_property *property,
+						uint64_t val);
+int intel_digital_connector_atomic_check(struct drm_connector *conn,
+					 struct drm_connector_state *new_state);
+struct drm_connector_state *
+intel_digital_connector_duplicate_state(struct drm_connector *connector);
+
 struct drm_crtc_state *intel_crtc_duplicate_state(struct drm_crtc *crtc);
 void intel_crtc_destroy_state(struct drm_crtc *crtc,
 			       struct drm_crtc_state *state);
diff --git a/drivers/gpu/drm/i915/intel_dsi.c b/drivers/gpu/drm/i915/intel_dsi.c
index 3ffe8b1f1d48..54030b68406a 100644
--- a/drivers/gpu/drm/i915/intel_dsi.c
+++ b/drivers/gpu/drm/i915/intel_dsi.c
@@ -320,10 +320,10 @@ static bool intel_dsi_compute_config(struct intel_encoder *encoder,
 
 		if (HAS_GMCH_DISPLAY(dev_priv))
 			intel_gmch_panel_fitting(crtc, pipe_config,
-						 intel_connector->panel.fitting_mode);
+						 conn_state->scaling_mode);
 		else
 			intel_pch_panel_fitting(crtc, pipe_config,
-						intel_connector->panel.fitting_mode);
+						conn_state->scaling_mode);
 	}
 
 	/* DSI uses short packets for sync events, so clear mode flags for DSI */
@@ -410,11 +410,10 @@ static void glk_dsi_device_ready(struct intel_encoder *encoder)
 		val |= (ULPS_STATE_ENTER | DEVICE_READY);
 		I915_WRITE(MIPI_DEVICE_READY(port), val);
 
-		/* Wait for ULPS Not active */
+		/* Wait for ULPS active */
 		if (intel_wait_for_register(dev_priv,
-				MIPI_CTRL(port), GLK_ULPS_NOT_ACTIVE,
-				GLK_ULPS_NOT_ACTIVE, 20))
-			DRM_ERROR("ULPS is still active\n");
+				MIPI_CTRL(port), GLK_ULPS_NOT_ACTIVE, 0, 20))
+			DRM_ERROR("ULPS not active\n");
 
 		/* Exit ULPS */
 		val = I915_READ(MIPI_DEVICE_READY(port));
@@ -1588,48 +1587,6 @@ static int intel_dsi_get_modes(struct drm_connector *connector)
 	return 1;
 }
 
-static int intel_dsi_set_property(struct drm_connector *connector,
-				  struct drm_property *property,
-				  uint64_t val)
-{
-	struct drm_device *dev = connector->dev;
-	struct intel_connector *intel_connector = to_intel_connector(connector);
-	struct drm_crtc *crtc;
-	int ret;
-
-	ret = drm_object_property_set_value(&connector->base, property, val);
-	if (ret)
-		return ret;
-
-	if (property == dev->mode_config.scaling_mode_property) {
-		if (val == DRM_MODE_SCALE_NONE) {
-			DRM_DEBUG_KMS("no scaling not supported\n");
-			return -EINVAL;
-		}
-		if (HAS_GMCH_DISPLAY(to_i915(dev)) &&
-		    val == DRM_MODE_SCALE_CENTER) {
-			DRM_DEBUG_KMS("centering not supported\n");
-			return -EINVAL;
-		}
-
-		if (intel_connector->panel.fitting_mode == val)
-			return 0;
-
-		intel_connector->panel.fitting_mode = val;
-	}
-
-	crtc = connector->state->crtc;
-	if (crtc && crtc->state->enable) {
-		/*
-		 * If the CRTC is enabled, the display will be changed
-		 * according to the new panel fitting mode.
-		 */
-		intel_crtc_restore_mode(crtc);
-	}
-
-	return 0;
-}
-
 static void intel_dsi_connector_destroy(struct drm_connector *connector)
 {
 	struct intel_connector *intel_connector = to_intel_connector(connector);
@@ -1658,6 +1615,7 @@ static const struct drm_encoder_funcs intel_dsi_funcs = {
 static const struct drm_connector_helper_funcs intel_dsi_connector_helper_funcs = {
 	.get_modes = intel_dsi_get_modes,
 	.mode_valid = intel_dsi_mode_valid,
+	.atomic_check = intel_digital_connector_atomic_check,
 };
 
 static const struct drm_connector_funcs intel_dsi_connector_funcs = {
@@ -1666,22 +1624,28 @@ static const struct drm_connector_funcs intel_dsi_connector_funcs = {
 	.early_unregister = intel_connector_unregister,
 	.destroy = intel_dsi_connector_destroy,
 	.fill_modes = drm_helper_probe_single_connector_modes,
-	.set_property = intel_dsi_set_property,
-	.atomic_get_property = intel_connector_atomic_get_property,
+	.set_property = drm_atomic_helper_connector_set_property,
+	.atomic_get_property = intel_digital_connector_atomic_get_property,
+	.atomic_set_property = intel_digital_connector_atomic_set_property,
 	.atomic_destroy_state = drm_atomic_helper_connector_destroy_state,
-	.atomic_duplicate_state = drm_atomic_helper_connector_duplicate_state,
+	.atomic_duplicate_state = intel_digital_connector_duplicate_state,
 };
 
 static void intel_dsi_add_properties(struct intel_connector *connector)
 {
-	struct drm_device *dev = connector->base.dev;
+	struct drm_i915_private *dev_priv = to_i915(connector->base.dev);
 
 	if (connector->panel.fixed_mode) {
-		drm_mode_create_scaling_mode_property(dev);
-		drm_object_attach_property(&connector->base.base,
-					   dev->mode_config.scaling_mode_property,
-					   DRM_MODE_SCALE_ASPECT);
-		connector->panel.fitting_mode = DRM_MODE_SCALE_ASPECT;
+		u32 allowed_scalers;
+
+		allowed_scalers = BIT(DRM_MODE_SCALE_ASPECT) | BIT(DRM_MODE_SCALE_FULLSCREEN);
+		if (!HAS_GMCH_DISPLAY(dev_priv))
+			allowed_scalers |= BIT(DRM_MODE_SCALE_CENTER);
+
+		drm_connector_attach_scaling_mode_property(&connector->base,
+								allowed_scalers);
+
+		connector->base.state->scaling_mode = DRM_MODE_SCALE_ASPECT;
 	}
 }
 
diff --git a/drivers/gpu/drm/i915/intel_dsi_vbt.c b/drivers/gpu/drm/i915/intel_dsi_vbt.c
index 0dce7792643a..7158c7ce9c09 100644
--- a/drivers/gpu/drm/i915/intel_dsi_vbt.c
+++ b/drivers/gpu/drm/i915/intel_dsi_vbt.c
@@ -694,8 +694,8 @@ bool intel_dsi_vbt_init(struct intel_dsi *intel_dsi, u16 panel_id)
 						clk_zero_cnt << 8 | prepare_cnt;
 
 	/*
-	 * LP to HS switch count = 4TLPX + PREP_COUNT * 2 + EXIT_ZERO_COUNT * 2
-	 *					+ 10UI + Extra Byte Count
+	 * LP to HS switch count = 4TLPX + PREP_COUNT * mul + EXIT_ZERO_COUNT *
+	 *					mul + 10UI + Extra Byte Count
 	 *
 	 * HS to LP switch count = THS-TRAIL + 2TLPX + Extra Byte Count
 	 * Extra Byte Count is calculated according to number of lanes.
@@ -708,8 +708,8 @@ bool intel_dsi_vbt_init(struct intel_dsi *intel_dsi, u16 panel_id)
 	/* B044 */
 	/* FIXME:
 	 * The comment above does not match with the code */
-	lp_to_hs_switch = DIV_ROUND_UP(4 * tlpx_ui + prepare_cnt * 2 +
-						exit_zero_cnt * 2 + 10, 8);
+	lp_to_hs_switch = DIV_ROUND_UP(4 * tlpx_ui + prepare_cnt * mul +
+						exit_zero_cnt * mul + 10, 8);
 
 	hs_to_lp_switch = DIV_ROUND_UP(mipi_config->ths_trail + 2 * tlpx_ui, 8);
 
diff --git a/drivers/gpu/drm/i915/intel_dvo.c b/drivers/gpu/drm/i915/intel_dvo.c
index 6025839ed3b7..c1544a53095d 100644
--- a/drivers/gpu/drm/i915/intel_dvo.c
+++ b/drivers/gpu/drm/i915/intel_dvo.c
@@ -350,7 +350,7 @@ static const struct drm_connector_funcs intel_dvo_connector_funcs = {
 	.early_unregister = intel_connector_unregister,
 	.destroy = intel_dvo_destroy,
 	.fill_modes = drm_helper_probe_single_connector_modes,
-	.atomic_get_property = intel_connector_atomic_get_property,
+	.set_property = drm_atomic_helper_connector_set_property,
 	.atomic_destroy_state = drm_atomic_helper_connector_destroy_state,
 	.atomic_duplicate_state = drm_atomic_helper_connector_duplicate_state,
 };
diff --git a/drivers/gpu/drm/i915/intel_engine_cs.c b/drivers/gpu/drm/i915/intel_engine_cs.c
index 854e8e0c836b..699f2d3861c7 100644
--- a/drivers/gpu/drm/i915/intel_engine_cs.c
+++ b/drivers/gpu/drm/i915/intel_engine_cs.c
@@ -26,69 +26,177 @@
 #include "intel_ringbuffer.h"
 #include "intel_lrc.h"
 
-static const struct engine_info {
+/* Haswell does have the CXT_SIZE register however it does not appear to be
+ * valid. Now, docs explain in dwords what is in the context object. The full
+ * size is 70720 bytes, however, the power context and execlist context will
+ * never be saved (power context is stored elsewhere, and execlists don't work
+ * on HSW) - so the final size, including the extra state required for the
+ * Resource Streamer, is 66944 bytes, which rounds to 17 pages.
+ */
+#define HSW_CXT_TOTAL_SIZE		(17 * PAGE_SIZE)
+/* Same as Haswell, but 72064 bytes now. */
+#define GEN8_CXT_TOTAL_SIZE		(18 * PAGE_SIZE)
+
+#define GEN8_LR_CONTEXT_RENDER_SIZE	(20 * PAGE_SIZE)
+#define GEN9_LR_CONTEXT_RENDER_SIZE	(22 * PAGE_SIZE)
+
+#define GEN8_LR_CONTEXT_OTHER_SIZE	( 2 * PAGE_SIZE)
+
+struct engine_class_info {
 	const char *name;
-	unsigned int exec_id;
+	int (*init_legacy)(struct intel_engine_cs *engine);
+	int (*init_execlists)(struct intel_engine_cs *engine);
+};
+
+static const struct engine_class_info intel_engine_classes[] = {
+	[RENDER_CLASS] = {
+		.name = "rcs",
+		.init_execlists = logical_render_ring_init,
+		.init_legacy = intel_init_render_ring_buffer,
+	},
+	[COPY_ENGINE_CLASS] = {
+		.name = "bcs",
+		.init_execlists = logical_xcs_ring_init,
+		.init_legacy = intel_init_blt_ring_buffer,
+	},
+	[VIDEO_DECODE_CLASS] = {
+		.name = "vcs",
+		.init_execlists = logical_xcs_ring_init,
+		.init_legacy = intel_init_bsd_ring_buffer,
+	},
+	[VIDEO_ENHANCEMENT_CLASS] = {
+		.name = "vecs",
+		.init_execlists = logical_xcs_ring_init,
+		.init_legacy = intel_init_vebox_ring_buffer,
+	},
+};
+
+struct engine_info {
 	unsigned int hw_id;
+	unsigned int uabi_id;
+	u8 class;
+	u8 instance;
 	u32 mmio_base;
 	unsigned irq_shift;
-	int (*init_legacy)(struct intel_engine_cs *engine);
-	int (*init_execlists)(struct intel_engine_cs *engine);
-} intel_engines[] = {
+};
+
+static const struct engine_info intel_engines[] = {
 	[RCS] = {
-		.name = "rcs",
 		.hw_id = RCS_HW,
-		.exec_id = I915_EXEC_RENDER,
+		.uabi_id = I915_EXEC_RENDER,
+		.class = RENDER_CLASS,
+		.instance = 0,
 		.mmio_base = RENDER_RING_BASE,
 		.irq_shift = GEN8_RCS_IRQ_SHIFT,
-		.init_execlists = logical_render_ring_init,
-		.init_legacy = intel_init_render_ring_buffer,
 	},
 	[BCS] = {
-		.name = "bcs",
 		.hw_id = BCS_HW,
-		.exec_id = I915_EXEC_BLT,
+		.uabi_id = I915_EXEC_BLT,
+		.class = COPY_ENGINE_CLASS,
+		.instance = 0,
 		.mmio_base = BLT_RING_BASE,
 		.irq_shift = GEN8_BCS_IRQ_SHIFT,
-		.init_execlists = logical_xcs_ring_init,
-		.init_legacy = intel_init_blt_ring_buffer,
 	},
 	[VCS] = {
-		.name = "vcs",
 		.hw_id = VCS_HW,
-		.exec_id = I915_EXEC_BSD,
+		.uabi_id = I915_EXEC_BSD,
+		.class = VIDEO_DECODE_CLASS,
+		.instance = 0,
 		.mmio_base = GEN6_BSD_RING_BASE,
 		.irq_shift = GEN8_VCS1_IRQ_SHIFT,
-		.init_execlists = logical_xcs_ring_init,
-		.init_legacy = intel_init_bsd_ring_buffer,
 	},
 	[VCS2] = {
-		.name = "vcs2",
 		.hw_id = VCS2_HW,
-		.exec_id = I915_EXEC_BSD,
+		.uabi_id = I915_EXEC_BSD,
+		.class = VIDEO_DECODE_CLASS,
+		.instance = 1,
 		.mmio_base = GEN8_BSD2_RING_BASE,
 		.irq_shift = GEN8_VCS2_IRQ_SHIFT,
-		.init_execlists = logical_xcs_ring_init,
-		.init_legacy = intel_init_bsd2_ring_buffer,
 	},
 	[VECS] = {
-		.name = "vecs",
 		.hw_id = VECS_HW,
-		.exec_id = I915_EXEC_VEBOX,
+		.uabi_id = I915_EXEC_VEBOX,
+		.class = VIDEO_ENHANCEMENT_CLASS,
+		.instance = 0,
 		.mmio_base = VEBOX_RING_BASE,
 		.irq_shift = GEN8_VECS_IRQ_SHIFT,
-		.init_execlists = logical_xcs_ring_init,
-		.init_legacy = intel_init_vebox_ring_buffer,
 	},
 };
 
+/**
+ * ___intel_engine_context_size() - return the size of the context for an engine
+ * @dev_priv: i915 device private
+ * @class: engine class
+ *
+ * Each engine class may require a different amount of space for a context
+ * image.
+ *
+ * Return: size (in bytes) of an engine class specific context image
+ *
+ * Note: this size includes the HWSP, which is part of the context image
+ * in LRC mode, but does not include the "shared data page" used with
+ * GuC submission. The caller should account for this if using the GuC.
+ */
+static u32
+__intel_engine_context_size(struct drm_i915_private *dev_priv, u8 class)
+{
+	u32 cxt_size;
+
+	BUILD_BUG_ON(I915_GTT_PAGE_SIZE != PAGE_SIZE);
+
+	switch (class) {
+	case RENDER_CLASS:
+		switch (INTEL_GEN(dev_priv)) {
+		default:
+			MISSING_CASE(INTEL_GEN(dev_priv));
+		case 9:
+			return GEN9_LR_CONTEXT_RENDER_SIZE;
+		case 8:
+			return i915.enable_execlists ?
+			       GEN8_LR_CONTEXT_RENDER_SIZE :
+			       GEN8_CXT_TOTAL_SIZE;
+		case 7:
+			if (IS_HASWELL(dev_priv))
+				return HSW_CXT_TOTAL_SIZE;
+
+			cxt_size = I915_READ(GEN7_CXT_SIZE);
+			return round_up(GEN7_CXT_TOTAL_SIZE(cxt_size) * 64,
+					PAGE_SIZE);
+		case 6:
+			cxt_size = I915_READ(CXT_SIZE);
+			return round_up(GEN6_CXT_TOTAL_SIZE(cxt_size) * 64,
+					PAGE_SIZE);
+		case 5:
+		case 4:
+		case 3:
+		case 2:
+		/* For the special day when i810 gets merged. */
+		case 1:
+			return 0;
+		}
+		break;
+	default:
+		MISSING_CASE(class);
+	case VIDEO_DECODE_CLASS:
+	case VIDEO_ENHANCEMENT_CLASS:
+	case COPY_ENGINE_CLASS:
+		if (INTEL_GEN(dev_priv) < 8)
+			return 0;
+		return GEN8_LR_CONTEXT_OTHER_SIZE;
+	}
+}
+
 static int
 intel_engine_setup(struct drm_i915_private *dev_priv,
 		   enum intel_engine_id id)
 {
 	const struct engine_info *info = &intel_engines[id];
+	const struct engine_class_info *class_info;
 	struct intel_engine_cs *engine;
 
+	GEM_BUG_ON(info->class >= ARRAY_SIZE(intel_engine_classes));
+	class_info = &intel_engine_classes[info->class];
+
 	GEM_BUG_ON(dev_priv->engine[id]);
 	engine = kzalloc(sizeof(*engine), GFP_KERNEL);
 	if (!engine)
@@ -96,11 +204,20 @@ intel_engine_setup(struct drm_i915_private *dev_priv,
 
 	engine->id = id;
 	engine->i915 = dev_priv;
-	engine->name = info->name;
-	engine->exec_id = info->exec_id;
+	WARN_ON(snprintf(engine->name, sizeof(engine->name), "%s%u",
+			 class_info->name, info->instance) >=
+		sizeof(engine->name));
+	engine->uabi_id = info->uabi_id;
 	engine->hw_id = engine->guc_id = info->hw_id;
 	engine->mmio_base = info->mmio_base;
 	engine->irq_shift = info->irq_shift;
+	engine->class = info->class;
+	engine->instance = info->instance;
+
+	engine->context_size = __intel_engine_context_size(dev_priv,
+							   engine->class);
+	if (WARN_ON(engine->context_size > BIT(20)))
+		engine->context_size = 0;
 
 	/* Nothing to do here, execute in order of dependencies */
 	engine->schedule = NULL;
@@ -112,18 +229,18 @@ intel_engine_setup(struct drm_i915_private *dev_priv,
 }
 
 /**
- * intel_engines_init_early() - allocate the Engine Command Streamers
+ * intel_engines_init_mmio() - allocate and prepare the Engine Command Streamers
  * @dev_priv: i915 device private
  *
  * Return: non-zero if the initialization failed.
  */
-int intel_engines_init_early(struct drm_i915_private *dev_priv)
+int intel_engines_init_mmio(struct drm_i915_private *dev_priv)
 {
 	struct intel_device_info *device_info = mkwrite_device_info(dev_priv);
-	unsigned int ring_mask = INTEL_INFO(dev_priv)->ring_mask;
-	unsigned int mask = 0;
+	const unsigned int ring_mask = INTEL_INFO(dev_priv)->ring_mask;
 	struct intel_engine_cs *engine;
 	enum intel_engine_id id;
+	unsigned int mask = 0;
 	unsigned int i;
 	int err;
 
@@ -150,6 +267,12 @@ int intel_engines_init_early(struct drm_i915_private *dev_priv)
 	if (WARN_ON(mask != ring_mask))
 		device_info->ring_mask = mask;
 
+	/* We always presume we have at least RCS available for later probing */
+	if (WARN_ON(!HAS_ENGINE(dev_priv, RCS))) {
+		err = -ENODEV;
+		goto cleanup;
+	}
+
 	device_info->num_rings = hweight32(mask);
 
 	return 0;
@@ -161,7 +284,7 @@ cleanup:
 }
 
 /**
- * intel_engines_init() - allocate, populate and init the Engine Command Streamers
+ * intel_engines_init() - init the Engine Command Streamers
  * @dev_priv: i915 device private
  *
  * Return: non-zero if the initialization failed.
@@ -175,12 +298,14 @@ int intel_engines_init(struct drm_i915_private *dev_priv)
 	int err = 0;
 
 	for_each_engine(engine, dev_priv, id) {
+		const struct engine_class_info *class_info =
+			&intel_engine_classes[engine->class];
 		int (*init)(struct intel_engine_cs *engine);
 
 		if (i915.enable_execlists)
-			init = intel_engines[id].init_execlists;
+			init = class_info->init_execlists;
 		else
-			init = intel_engines[id].init_legacy;
+			init = class_info->init_legacy;
 		if (!init) {
 			kfree(engine);
 			dev_priv->engine[id] = NULL;
@@ -223,6 +348,9 @@ void intel_engine_init_global_seqno(struct intel_engine_cs *engine, u32 seqno)
 {
 	struct drm_i915_private *dev_priv = engine->i915;
 
+	GEM_BUG_ON(!intel_engine_is_idle(engine));
+	GEM_BUG_ON(i915_gem_active_isset(&engine->timeline->last_request));
+
 	/* Our semaphore implementation is strictly monotonic (i.e. we proceed
 	 * so long as the semaphore value in the register/page is greater
 	 * than the sync value), so whenever we reset the seqno,
@@ -253,13 +381,12 @@ void intel_engine_init_global_seqno(struct intel_engine_cs *engine, u32 seqno)
 	intel_write_status_page(engine, I915_GEM_HWS_INDEX, seqno);
 	clear_bit(ENGINE_IRQ_BREADCRUMB, &engine->irq_posted);
 
-	GEM_BUG_ON(i915_gem_active_isset(&engine->timeline->last_request));
-	engine->hangcheck.seqno = seqno;
-
 	/* After manually advancing the seqno, fake the interrupt in case
 	 * there are any waiters for that seqno.
 	 */
 	intel_engine_wakeup(engine);
+
+	GEM_BUG_ON(intel_engine_get_seqno(engine) != seqno);
 }
 
 static void intel_engine_init_timeline(struct intel_engine_cs *engine)
@@ -342,6 +469,7 @@ static void intel_engine_cleanup_scratch(struct intel_engine_cs *engine)
  */
 int intel_engine_init_common(struct intel_engine_cs *engine)
 {
+	struct intel_ring *ring;
 	int ret;
 
 	engine->set_default_submission(engine);
@@ -353,9 +481,9 @@ int intel_engine_init_common(struct intel_engine_cs *engine)
 	 * be available. To avoid this we always pin the default
 	 * context.
 	 */
-	ret = engine->context_pin(engine, engine->i915->kernel_context);
-	if (ret)
-		return ret;
+	ring = engine->context_pin(engine, engine->i915->kernel_context);
+	if (IS_ERR(ring))
+		return PTR_ERR(ring);
 
 	ret = intel_engine_init_breadcrumbs(engine);
 	if (ret)
@@ -723,8 +851,10 @@ static int gen9_init_workarounds(struct intel_engine_cs *engine)
 		 */
 	}
 
+	/* WaEnableYV12BugFixInHalfSliceChicken7:skl,bxt,kbl,glk */
 	/* WaEnableSamplerGPGPUPreemptionSupport:skl,bxt,kbl */
 	WA_SET_BIT_MASKED(GEN9_HALF_SLICE_CHICKEN7,
+			  GEN9_ENABLE_YV12_BUGFIX |
 			  GEN9_ENABLE_GPGPU_PREEMPTION);
 
 	/* Wa4x4STCOptimizationDisable:skl,bxt,kbl,glk */
@@ -1075,6 +1205,22 @@ int intel_ring_workarounds_emit(struct drm_i915_gem_request *req)
 	return 0;
 }
 
+static bool ring_is_idle(struct intel_engine_cs *engine)
+{
+	struct drm_i915_private *dev_priv = engine->i915;
+	bool idle = true;
+
+	intel_runtime_pm_get(dev_priv);
+
+	/* No bit for gen2, so assume the CS parser is idle */
+	if (INTEL_GEN(dev_priv) > 2 && !(I915_READ_MODE(engine) & MODE_IDLE))
+		idle = false;
+
+	intel_runtime_pm_put(dev_priv);
+
+	return idle;
+}
+
 /**
  * intel_engine_is_idle() - Report if the engine has finished process all work
  * @engine: the intel_engine_cs
@@ -1086,21 +1232,28 @@ bool intel_engine_is_idle(struct intel_engine_cs *engine)
 {
 	struct drm_i915_private *dev_priv = engine->i915;
 
+	/* More white lies, if wedged, hw state is inconsistent */
+	if (i915_terminally_wedged(&dev_priv->gpu_error))
+		return true;
+
 	/* Any inflight/incomplete requests? */
 	if (!i915_seqno_passed(intel_engine_get_seqno(engine),
 			       intel_engine_last_submit(engine)))
 		return false;
 
+	if (I915_SELFTEST_ONLY(engine->breadcrumbs.mock))
+		return true;
+
 	/* Interrupt/tasklet pending? */
 	if (test_bit(ENGINE_IRQ_EXECLIST, &engine->irq_posted))
 		return false;
 
 	/* Both ports drained, no more ELSP submission? */
-	if (engine->execlist_port[0].request)
+	if (port_request(&engine->execlist_port[0]))
 		return false;
 
 	/* Ring stopped? */
-	if (INTEL_GEN(dev_priv) > 2 && !(I915_READ_MODE(engine) & MODE_IDLE))
+	if (!ring_is_idle(engine))
 		return false;
 
 	return true;
@@ -1137,6 +1290,18 @@ void intel_engines_reset_default_submission(struct drm_i915_private *i915)
 		engine->set_default_submission(engine);
 }
 
+void intel_engines_mark_idle(struct drm_i915_private *i915)
+{
+	struct intel_engine_cs *engine;
+	enum intel_engine_id id;
+
+	for_each_engine(engine, i915, id) {
+		intel_engine_disarm_breadcrumbs(engine);
+		i915_gem_batch_pool_fini(&engine->batch_pool);
+		engine->no_priolist = false;
+	}
+}
+
 #if IS_ENABLED(CONFIG_DRM_I915_SELFTEST)
 #include "selftests/mock_engine.c"
 #endif
diff --git a/drivers/gpu/drm/i915/intel_fbc.c b/drivers/gpu/drm/i915/intel_fbc.c
index db7f8f0a1f36..ff2fc5bc4af4 100644
--- a/drivers/gpu/drm/i915/intel_fbc.c
+++ b/drivers/gpu/drm/i915/intel_fbc.c
@@ -1312,14 +1312,12 @@ static int intel_sanitize_fbc_option(struct drm_i915_private *dev_priv)
 
 static bool need_fbc_vtd_wa(struct drm_i915_private *dev_priv)
 {
-#ifdef CONFIG_INTEL_IOMMU
 	/* WaFbcTurnOffFbcWhenHyperVisorIsUsed:skl,bxt */
-	if (intel_iommu_gfx_mapped &&
+	if (intel_vtd_active() &&
 	    (IS_SKYLAKE(dev_priv) || IS_BROXTON(dev_priv))) {
 		DRM_INFO("Disabling framebuffer compression (FBC) to prevent screen flicker with VT-d enabled\n");
 		return true;
 	}
-#endif
 
 	return false;
 }
diff --git a/drivers/gpu/drm/i915/intel_guc_ct.c b/drivers/gpu/drm/i915/intel_guc_ct.c
new file mode 100644
index 000000000000..c4cbec140101
--- /dev/null
+++ b/drivers/gpu/drm/i915/intel_guc_ct.c
@@ -0,0 +1,461 @@
+/*
+ * Copyright © 2016-2017 Intel Corporation
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
+ * IN THE SOFTWARE.
+ */
+
+#include "i915_drv.h"
+#include "intel_guc_ct.h"
+
+enum { CTB_SEND = 0, CTB_RECV = 1 };
+
+enum { CTB_OWNER_HOST = 0 };
+
+void intel_guc_ct_init_early(struct intel_guc_ct *ct)
+{
+	/* we're using static channel owners */
+	ct->host_channel.owner = CTB_OWNER_HOST;
+}
+
+static inline const char *guc_ct_buffer_type_to_str(u32 type)
+{
+	switch (type) {
+	case INTEL_GUC_CT_BUFFER_TYPE_SEND:
+		return "SEND";
+	case INTEL_GUC_CT_BUFFER_TYPE_RECV:
+		return "RECV";
+	default:
+		return "<invalid>";
+	}
+}
+
+static void guc_ct_buffer_desc_init(struct guc_ct_buffer_desc *desc,
+				    u32 cmds_addr, u32 size, u32 owner)
+{
+	DRM_DEBUG_DRIVER("CT: desc %p init addr=%#x size=%u owner=%u\n",
+			 desc, cmds_addr, size, owner);
+	memset(desc, 0, sizeof(*desc));
+	desc->addr = cmds_addr;
+	desc->size = size;
+	desc->owner = owner;
+}
+
+static void guc_ct_buffer_desc_reset(struct guc_ct_buffer_desc *desc)
+{
+	DRM_DEBUG_DRIVER("CT: desc %p reset head=%u tail=%u\n",
+			 desc, desc->head, desc->tail);
+	desc->head = 0;
+	desc->tail = 0;
+	desc->is_in_error = 0;
+}
+
+static int guc_action_register_ct_buffer(struct intel_guc *guc,
+					 u32 desc_addr,
+					 u32 type)
+{
+	u32 action[] = {
+		INTEL_GUC_ACTION_REGISTER_COMMAND_TRANSPORT_BUFFER,
+		desc_addr,
+		sizeof(struct guc_ct_buffer_desc),
+		type
+	};
+	int err;
+
+	/* Can't use generic send(), CT registration must go over MMIO */
+	err = intel_guc_send_mmio(guc, action, ARRAY_SIZE(action));
+	if (err)
+		DRM_ERROR("CT: register %s buffer failed; err=%d\n",
+			  guc_ct_buffer_type_to_str(type), err);
+	return err;
+}
+
+static int guc_action_deregister_ct_buffer(struct intel_guc *guc,
+					   u32 owner,
+					   u32 type)
+{
+	u32 action[] = {
+		INTEL_GUC_ACTION_DEREGISTER_COMMAND_TRANSPORT_BUFFER,
+		owner,
+		type
+	};
+	int err;
+
+	/* Can't use generic send(), CT deregistration must go over MMIO */
+	err = intel_guc_send_mmio(guc, action, ARRAY_SIZE(action));
+	if (err)
+		DRM_ERROR("CT: deregister %s buffer failed; owner=%d err=%d\n",
+			  guc_ct_buffer_type_to_str(type), owner, err);
+	return err;
+}
+
+static bool ctch_is_open(struct intel_guc_ct_channel *ctch)
+{
+	return ctch->vma != NULL;
+}
+
+static int ctch_init(struct intel_guc *guc,
+		     struct intel_guc_ct_channel *ctch)
+{
+	struct i915_vma *vma;
+	void *blob;
+	int err;
+	int i;
+
+	GEM_BUG_ON(ctch->vma);
+
+	/* We allocate 1 page to hold both descriptors and both buffers.
+	 *       ___________.....................
+	 *      |desc (SEND)|                   :
+	 *      |___________|                   PAGE/4
+	 *      :___________....................:
+	 *      |desc (RECV)|                   :
+	 *      |___________|                   PAGE/4
+	 *      :_______________________________:
+	 *      |cmds (SEND)                    |
+	 *      |                               PAGE/4
+	 *      |_______________________________|
+	 *      |cmds (RECV)                    |
+	 *      |                               PAGE/4
+	 *      |_______________________________|
+	 *
+	 * Each message can use a maximum of 32 dwords and we don't expect to
+	 * have more than 1 in flight at any time, so we have enough space.
+	 * Some logic further ahead will rely on the fact that there is only 1
+	 * page and that it is always mapped, so if the size is changed the
+	 * other code will need updating as well.
+	 */
+
+	/* allocate vma */
+	vma = intel_guc_allocate_vma(guc, PAGE_SIZE);
+	if (IS_ERR(vma)) {
+		err = PTR_ERR(vma);
+		goto err_out;
+	}
+	ctch->vma = vma;
+
+	/* map first page */
+	blob = i915_gem_object_pin_map(vma->obj, I915_MAP_WB);
+	if (IS_ERR(blob)) {
+		err = PTR_ERR(blob);
+		goto err_vma;
+	}
+	DRM_DEBUG_DRIVER("CT: vma base=%#x\n", guc_ggtt_offset(ctch->vma));
+
+	/* store pointers to desc and cmds */
+	for (i = 0; i < ARRAY_SIZE(ctch->ctbs); i++) {
+		GEM_BUG_ON((i != CTB_SEND) && (i != CTB_RECV));
+		ctch->ctbs[i].desc = blob + PAGE_SIZE/4 * i;
+		ctch->ctbs[i].cmds = blob + PAGE_SIZE/4 * i + PAGE_SIZE/2;
+	}
+
+	return 0;
+
+err_vma:
+	i915_vma_unpin_and_release(&ctch->vma);
+err_out:
+	DRM_DEBUG_DRIVER("CT: channel %d initialization failed; err=%d\n",
+			 ctch->owner, err);
+	return err;
+}
+
+static void ctch_fini(struct intel_guc *guc,
+		      struct intel_guc_ct_channel *ctch)
+{
+	GEM_BUG_ON(!ctch->vma);
+
+	i915_gem_object_unpin_map(ctch->vma->obj);
+	i915_vma_unpin_and_release(&ctch->vma);
+}
+
+static int ctch_open(struct intel_guc *guc,
+		     struct intel_guc_ct_channel *ctch)
+{
+	u32 base;
+	int err;
+	int i;
+
+	DRM_DEBUG_DRIVER("CT: channel %d reopen=%s\n",
+			 ctch->owner, yesno(ctch_is_open(ctch)));
+
+	if (!ctch->vma) {
+		err = ctch_init(guc, ctch);
+		if (unlikely(err))
+			goto err_out;
+	}
+
+	/* vma should be already allocated and map'ed */
+	base = guc_ggtt_offset(ctch->vma);
+
+	/* (re)initialize descriptors
+	 * cmds buffers are in the second half of the blob page
+	 */
+	for (i = 0; i < ARRAY_SIZE(ctch->ctbs); i++) {
+		GEM_BUG_ON((i != CTB_SEND) && (i != CTB_RECV));
+		guc_ct_buffer_desc_init(ctch->ctbs[i].desc,
+					base + PAGE_SIZE/4 * i + PAGE_SIZE/2,
+					PAGE_SIZE/4,
+					ctch->owner);
+	}
+
+	/* register buffers, starting wirh RECV buffer
+	 * descriptors are in first half of the blob
+	 */
+	err = guc_action_register_ct_buffer(guc,
+					    base + PAGE_SIZE/4 * CTB_RECV,
+					    INTEL_GUC_CT_BUFFER_TYPE_RECV);
+	if (unlikely(err))
+		goto err_fini;
+
+	err = guc_action_register_ct_buffer(guc,
+					    base + PAGE_SIZE/4 * CTB_SEND,
+					    INTEL_GUC_CT_BUFFER_TYPE_SEND);
+	if (unlikely(err))
+		goto err_deregister;
+
+	return 0;
+
+err_deregister:
+	guc_action_deregister_ct_buffer(guc,
+					ctch->owner,
+					INTEL_GUC_CT_BUFFER_TYPE_RECV);
+err_fini:
+	ctch_fini(guc, ctch);
+err_out:
+	DRM_ERROR("CT: can't open channel %d; err=%d\n", ctch->owner, err);
+	return err;
+}
+
+static void ctch_close(struct intel_guc *guc,
+		       struct intel_guc_ct_channel *ctch)
+{
+	GEM_BUG_ON(!ctch_is_open(ctch));
+
+	guc_action_deregister_ct_buffer(guc,
+					ctch->owner,
+					INTEL_GUC_CT_BUFFER_TYPE_SEND);
+	guc_action_deregister_ct_buffer(guc,
+					ctch->owner,
+					INTEL_GUC_CT_BUFFER_TYPE_RECV);
+	ctch_fini(guc, ctch);
+}
+
+static u32 ctch_get_next_fence(struct intel_guc_ct_channel *ctch)
+{
+	/* For now it's trivial */
+	return ++ctch->next_fence;
+}
+
+static int ctb_write(struct intel_guc_ct_buffer *ctb,
+		     const u32 *action,
+		     u32 len /* in dwords */,
+		     u32 fence)
+{
+	struct guc_ct_buffer_desc *desc = ctb->desc;
+	u32 head = desc->head / 4;	/* in dwords */
+	u32 tail = desc->tail / 4;	/* in dwords */
+	u32 size = desc->size / 4;	/* in dwords */
+	u32 used;			/* in dwords */
+	u32 header;
+	u32 *cmds = ctb->cmds;
+	unsigned int i;
+
+	GEM_BUG_ON(desc->size % 4);
+	GEM_BUG_ON(desc->head % 4);
+	GEM_BUG_ON(desc->tail % 4);
+	GEM_BUG_ON(tail >= size);
+
+	/*
+	 * tail == head condition indicates empty. GuC FW does not support
+	 * using up the entire buffer to get tail == head meaning full.
+	 */
+	if (tail < head)
+		used = (size - head) + tail;
+	else
+		used = tail - head;
+
+	/* make sure there is a space including extra dw for the fence */
+	if (unlikely(used + len + 1 >= size))
+		return -ENOSPC;
+
+	/* Write the message. The format is the following:
+	 * DW0: header (including action code)
+	 * DW1: fence
+	 * DW2+: action data
+	 */
+	header = (len << GUC_CT_MSG_LEN_SHIFT) |
+		 (GUC_CT_MSG_WRITE_FENCE_TO_DESC) |
+		 (action[0] << GUC_CT_MSG_ACTION_SHIFT);
+
+	cmds[tail] = header;
+	tail = (tail + 1) % size;
+
+	cmds[tail] = fence;
+	tail = (tail + 1) % size;
+
+	for (i = 1; i < len; i++) {
+		cmds[tail] = action[i];
+		tail = (tail + 1) % size;
+	}
+
+	/* now update desc tail (back in bytes) */
+	desc->tail = tail * 4;
+	GEM_BUG_ON(desc->tail > desc->size);
+
+	return 0;
+}
+
+/* Wait for the response from the GuC.
+ * @fence:	response fence
+ * @status:	placeholder for status
+ * return:	0 response received (status is valid)
+ *		-ETIMEDOUT no response within hardcoded timeout
+ *		-EPROTO no response, ct buffer was in error
+ */
+static int wait_for_response(struct guc_ct_buffer_desc *desc,
+			     u32 fence,
+			     u32 *status)
+{
+	int err;
+
+	/*
+	 * Fast commands should complete in less than 10us, so sample quickly
+	 * up to that length of time, then switch to a slower sleep-wait loop.
+	 * No GuC command should ever take longer than 10ms.
+	 */
+#define done (READ_ONCE(desc->fence) == fence)
+	err = wait_for_us(done, 10);
+	if (err)
+		err = wait_for(done, 10);
+#undef done
+
+	if (unlikely(err)) {
+		DRM_ERROR("CT: fence %u failed; reported fence=%u\n",
+			  fence, desc->fence);
+
+		if (WARN_ON(desc->is_in_error)) {
+			/* Something went wrong with the messaging, try to reset
+			 * the buffer and hope for the best
+			 */
+			guc_ct_buffer_desc_reset(desc);
+			err = -EPROTO;
+		}
+	}
+
+	*status = desc->status;
+	return err;
+}
+
+static int ctch_send(struct intel_guc *guc,
+		     struct intel_guc_ct_channel *ctch,
+		     const u32 *action,
+		     u32 len,
+		     u32 *status)
+{
+	struct intel_guc_ct_buffer *ctb = &ctch->ctbs[CTB_SEND];
+	struct guc_ct_buffer_desc *desc = ctb->desc;
+	u32 fence;
+	int err;
+
+	GEM_BUG_ON(!ctch_is_open(ctch));
+	GEM_BUG_ON(!len);
+	GEM_BUG_ON(len & ~GUC_CT_MSG_LEN_MASK);
+
+	fence = ctch_get_next_fence(ctch);
+	err = ctb_write(ctb, action, len, fence);
+	if (unlikely(err))
+		return err;
+
+	intel_guc_notify(guc);
+
+	err = wait_for_response(desc, fence, status);
+	if (unlikely(err))
+		return err;
+	if (*status != INTEL_GUC_STATUS_SUCCESS)
+		return -EIO;
+	return 0;
+}
+
+/*
+ * Command Transport (CT) buffer based GuC send function.
+ */
+static int intel_guc_send_ct(struct intel_guc *guc, const u32 *action, u32 len)
+{
+	struct intel_guc_ct_channel *ctch = &guc->ct.host_channel;
+	u32 status = ~0; /* undefined */
+	int err;
+
+	mutex_lock(&guc->send_mutex);
+
+	err = ctch_send(guc, ctch, action, len, &status);
+	if (unlikely(err)) {
+		DRM_ERROR("CT: send action %#X failed; err=%d status=%#X\n",
+			  action[0], err, status);
+	}
+
+	mutex_unlock(&guc->send_mutex);
+	return err;
+}
+
+/**
+ * Enable buffer based command transport
+ * Shall only be called for platforms with HAS_GUC_CT.
+ * @guc:	the guc
+ * return:	0 on success
+ *		non-zero on failure
+ */
+int intel_guc_enable_ct(struct intel_guc *guc)
+{
+	struct drm_i915_private *dev_priv = guc_to_i915(guc);
+	struct intel_guc_ct_channel *ctch = &guc->ct.host_channel;
+	int err;
+
+	GEM_BUG_ON(!HAS_GUC_CT(dev_priv));
+
+	err = ctch_open(guc, ctch);
+	if (unlikely(err))
+		return err;
+
+	/* Switch into cmd transport buffer based send() */
+	guc->send = intel_guc_send_ct;
+	DRM_INFO("CT: %s\n", enableddisabled(true));
+	return 0;
+}
+
+/**
+ * Disable buffer based command transport.
+ * Shall only be called for platforms with HAS_GUC_CT.
+ * @guc: the guc
+ */
+void intel_guc_disable_ct(struct intel_guc *guc)
+{
+	struct drm_i915_private *dev_priv = guc_to_i915(guc);
+	struct intel_guc_ct_channel *ctch = &guc->ct.host_channel;
+
+	GEM_BUG_ON(!HAS_GUC_CT(dev_priv));
+
+	if (!ctch_is_open(ctch))
+		return;
+
+	ctch_close(guc, ctch);
+
+	/* Disable send */
+	guc->send = intel_guc_send_nop;
+	DRM_INFO("CT: %s\n", enableddisabled(false));
+}
diff --git a/drivers/gpu/drm/i915/intel_guc_ct.h b/drivers/gpu/drm/i915/intel_guc_ct.h
new file mode 100644
index 000000000000..6d97f36fcc62
--- /dev/null
+++ b/drivers/gpu/drm/i915/intel_guc_ct.h
@@ -0,0 +1,86 @@
+/*
+ * Copyright © 2016-2017 Intel Corporation
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
+ * IN THE SOFTWARE.
+ */
+
+#ifndef _INTEL_GUC_CT_H_
+#define _INTEL_GUC_CT_H_
+
+struct intel_guc;
+struct i915_vma;
+
+#include "intel_guc_fwif.h"
+
+/**
+ * DOC: Command Transport (CT).
+ *
+ * Buffer based command transport is a replacement for MMIO based mechanism.
+ * It can be used to perform both host-2-guc and guc-to-host communication.
+ */
+
+/** Represents single command transport buffer.
+ *
+ * A single command transport buffer consists of two parts, the header
+ * record (command transport buffer descriptor) and the actual buffer which
+ * holds the commands.
+ *
+ * @desc: pointer to the buffer descriptor
+ * @cmds: pointer to the commands buffer
+ */
+struct intel_guc_ct_buffer {
+	struct guc_ct_buffer_desc *desc;
+	u32 *cmds;
+};
+
+/** Represents pair of command transport buffers.
+ *
+ * Buffers go in pairs to allow bi-directional communication.
+ * To simplify the code we place both of them in the same vma.
+ * Buffers from the same pair must share unique owner id.
+ *
+ * @vma: pointer to the vma with pair of CT buffers
+ * @ctbs: buffers for sending(0) and receiving(1) commands
+ * @owner: unique identifier
+ * @next_fence: fence to be used with next send command
+ */
+struct intel_guc_ct_channel {
+	struct i915_vma *vma;
+	struct intel_guc_ct_buffer ctbs[2];
+	u32 owner;
+	u32 next_fence;
+};
+
+/** Holds all command transport channels.
+ *
+ * @host_channel: main channel used by the host
+ */
+struct intel_guc_ct {
+	struct intel_guc_ct_channel host_channel;
+	/* other channels are tbd */
+};
+
+void intel_guc_ct_init_early(struct intel_guc_ct *ct);
+
+/* XXX: move to intel_uc.h ? don't fit there either */
+int intel_guc_enable_ct(struct intel_guc *guc);
+void intel_guc_disable_ct(struct intel_guc *guc);
+
+#endif /* _INTEL_GUC_CT_H_ */
diff --git a/drivers/gpu/drm/i915/intel_guc_fwif.h b/drivers/gpu/drm/i915/intel_guc_fwif.h
index cb36cbf3818f..5fa286074811 100644
--- a/drivers/gpu/drm/i915/intel_guc_fwif.h
+++ b/drivers/gpu/drm/i915/intel_guc_fwif.h
@@ -23,8 +23,8 @@
 #ifndef _INTEL_GUC_FWIF_H
 #define _INTEL_GUC_FWIF_H
 
-#define GFXCORE_FAMILY_GEN9		12
-#define GFXCORE_FAMILY_UNKNOWN		0x7fffffff
+#define GUC_CORE_FAMILY_GEN9		12
+#define GUC_CORE_FAMILY_UNKNOWN		0x7fffffff
 
 #define GUC_CLIENT_PRIORITY_KMD_HIGH	0
 #define GUC_CLIENT_PRIORITY_HIGH	1
@@ -331,6 +331,47 @@ struct guc_stage_desc {
 	u64 desc_private;
 } __packed;
 
+/*
+ * Describes single command transport buffer.
+ * Used by both guc-master and clients.
+ */
+struct guc_ct_buffer_desc {
+	u32 addr;		/* gfx address */
+	u64 host_private;	/* host private data */
+	u32 size;		/* size in bytes */
+	u32 head;		/* offset updated by GuC*/
+	u32 tail;		/* offset updated by owner */
+	u32 is_in_error;	/* error indicator */
+	u32 fence;		/* fence updated by GuC */
+	u32 status;		/* status updated by GuC */
+	u32 owner;		/* id of the channel owner */
+	u32 owner_sub_id;	/* owner-defined field for extra tracking */
+	u32 reserved[5];
+} __packed;
+
+/* Type of command transport buffer */
+#define INTEL_GUC_CT_BUFFER_TYPE_SEND	0x0u
+#define INTEL_GUC_CT_BUFFER_TYPE_RECV	0x1u
+
+/*
+ * Definition of the command transport message header (DW0)
+ *
+ * bit[4..0]	message len (in dwords)
+ * bit[7..5]	reserved
+ * bit[8]	write fence to desc
+ * bit[9]	write status to H2G buff
+ * bit[10]	send status (via G2H)
+ * bit[15..11]	reserved
+ * bit[31..16]	action code
+ */
+#define GUC_CT_MSG_LEN_SHIFT			0
+#define GUC_CT_MSG_LEN_MASK			0x1F
+#define GUC_CT_MSG_WRITE_FENCE_TO_DESC		(1 << 8)
+#define GUC_CT_MSG_WRITE_STATUS_TO_BUFF		(1 << 9)
+#define GUC_CT_MSG_SEND_STATUS			(1 << 10)
+#define GUC_CT_MSG_ACTION_SHIFT			16
+#define GUC_CT_MSG_ACTION_MASK			0xFFFF
+
 #define GUC_FORCEWAKE_RENDER	(1 << 0)
 #define GUC_FORCEWAKE_MEDIA	(1 << 1)
 
@@ -515,6 +556,8 @@ enum intel_guc_action {
 	INTEL_GUC_ACTION_EXIT_S_STATE = 0x502,
 	INTEL_GUC_ACTION_SLPC_REQUEST = 0x3003,
 	INTEL_GUC_ACTION_AUTHENTICATE_HUC = 0x4000,
+	INTEL_GUC_ACTION_REGISTER_COMMAND_TRANSPORT_BUFFER = 0x4505,
+	INTEL_GUC_ACTION_DEREGISTER_COMMAND_TRANSPORT_BUFFER = 0x4506,
 	INTEL_GUC_ACTION_UK_LOG_ENABLE_LOGGING = 0x0E000,
 	INTEL_GUC_ACTION_LIMIT
 };
diff --git a/drivers/gpu/drm/i915/intel_guc_loader.c b/drivers/gpu/drm/i915/intel_guc_loader.c
index 8a1a023e48b2..d9045b6e897b 100644
--- a/drivers/gpu/drm/i915/intel_guc_loader.c
+++ b/drivers/gpu/drm/i915/intel_guc_loader.c
@@ -61,6 +61,9 @@
 #define KBL_FW_MAJOR 9
 #define KBL_FW_MINOR 14
 
+#define GLK_FW_MAJOR 10
+#define GLK_FW_MINOR 56
+
 #define GUC_FW_PATH(platform, major, minor) \
        "i915/" __stringify(platform) "_guc_ver" __stringify(major) "_" __stringify(minor) ".bin"
 
@@ -73,6 +76,8 @@ MODULE_FIRMWARE(I915_BXT_GUC_UCODE);
 #define I915_KBL_GUC_UCODE GUC_FW_PATH(kbl, KBL_FW_MAJOR, KBL_FW_MINOR)
 MODULE_FIRMWARE(I915_KBL_GUC_UCODE);
 
+#define I915_GLK_GUC_UCODE GUC_FW_PATH(glk, GLK_FW_MAJOR, GLK_FW_MINOR)
+
 
 static u32 get_gttype(struct drm_i915_private *dev_priv)
 {
@@ -86,11 +91,11 @@ static u32 get_core_family(struct drm_i915_private *dev_priv)
 
 	switch (gen) {
 	case 9:
-		return GFXCORE_FAMILY_GEN9;
+		return GUC_CORE_FAMILY_GEN9;
 
 	default:
-		WARN(1, "GEN%d does not support GuC operation!\n", gen);
-		return GFXCORE_FAMILY_UNKNOWN;
+		MISSING_CASE(gen);
+		return GUC_CORE_FAMILY_UNKNOWN;
 	}
 }
 
@@ -280,10 +285,6 @@ static int guc_ucode_xfer(struct drm_i915_private *dev_priv)
 
 	intel_uncore_forcewake_get(dev_priv, FORCEWAKE_ALL);
 
-	/* init WOPCM */
-	I915_WRITE(GUC_WOPCM_SIZE, intel_guc_wopcm_size(dev_priv));
-	I915_WRITE(DMA_GUC_WOPCM_OFFSET, GUC_WOPCM_OFFSET_VALUE);
-
 	/* Enable MIA caching. GuC clock gating is disabled. */
 	I915_WRITE(GUC_SHIM_CONTROL, GUC_SHIM_CONTROL_VALUE);
 
@@ -405,6 +406,10 @@ int intel_guc_select_fw(struct intel_guc *guc)
 		guc->fw.path = I915_KBL_GUC_UCODE;
 		guc->fw.major_ver_wanted = KBL_FW_MAJOR;
 		guc->fw.minor_ver_wanted = KBL_FW_MINOR;
+	} else if (IS_GEMINILAKE(dev_priv)) {
+		guc->fw.path = I915_GLK_GUC_UCODE;
+		guc->fw.major_ver_wanted = GLK_FW_MAJOR;
+		guc->fw.minor_ver_wanted = GLK_FW_MINOR;
 	} else {
 		DRM_ERROR("No GuC firmware known for platform with GuC!\n");
 		return -ENOENT;
diff --git a/drivers/gpu/drm/i915/intel_guc_log.c b/drivers/gpu/drm/i915/intel_guc_log.c
index 6fb63a3c65b0..16d3b8719cab 100644
--- a/drivers/gpu/drm/i915/intel_guc_log.c
+++ b/drivers/gpu/drm/i915/intel_guc_log.c
@@ -359,12 +359,16 @@ static int guc_log_runtime_create(struct intel_guc *guc)
 	void *vaddr;
 	struct rchan *guc_log_relay_chan;
 	size_t n_subbufs, subbuf_size;
-	int ret = 0;
+	int ret;
 
 	lockdep_assert_held(&dev_priv->drm.struct_mutex);
 
 	GEM_BUG_ON(guc_log_has_runtime(guc));
 
+	ret = i915_gem_object_set_to_wc_domain(guc->log.vma->obj, true);
+	if (ret)
+		return ret;
+
 	/* Create a WC (Uncached for read) vmalloc mapping of log
 	 * buffer pages, so that we can directly get the data
 	 * (up-to-date) from memory.
diff --git a/drivers/gpu/drm/i915/intel_gvt.c b/drivers/gpu/drm/i915/intel_gvt.c
index e1ab6432a914..52d5b82790d9 100644
--- a/drivers/gpu/drm/i915/intel_gvt.c
+++ b/drivers/gpu/drm/i915/intel_gvt.c
@@ -51,6 +51,32 @@ static bool is_supported_device(struct drm_i915_private *dev_priv)
 }
 
 /**
+ * intel_gvt_sanitize_options - sanitize GVT related options
+ * @dev_priv: drm i915 private data
+ *
+ * This function is called at the i915 options sanitize stage.
+ */
+void intel_gvt_sanitize_options(struct drm_i915_private *dev_priv)
+{
+	if (!i915.enable_gvt)
+		return;
+
+	if (intel_vgpu_active(dev_priv)) {
+		DRM_INFO("GVT-g is disabled for guest\n");
+		goto bail;
+	}
+
+	if (!is_supported_device(dev_priv)) {
+		DRM_INFO("Unsupported device. GVT-g is disabled\n");
+		goto bail;
+	}
+
+	return;
+bail:
+	i915.enable_gvt = 0;
+}
+
+/**
  * intel_gvt_init - initialize GVT components
  * @dev_priv: drm i915 private data
  *
@@ -69,19 +95,14 @@ int intel_gvt_init(struct drm_i915_private *dev_priv)
 		return 0;
 	}
 
-	if (intel_vgpu_active(dev_priv)) {
-		DRM_DEBUG_DRIVER("GVT-g is disabled for guest\n");
-		goto bail;
-	}
-
-	if (!is_supported_device(dev_priv)) {
-		DRM_DEBUG_DRIVER("Unsupported device. GVT-g is disabled\n");
-		goto bail;
+	if (!i915.enable_execlists) {
+		DRM_ERROR("i915 GVT-g loading failed due to disabled execlists mode\n");
+		return -EIO;
 	}
 
-	if (!i915.enable_execlists) {
-		DRM_INFO("GPU guest virtualisation [GVT-g] disabled due to disabled execlist submission [i915.enable_execlists module parameter]\n");
-		goto bail;
+	if (i915.enable_guc_submission) {
+		DRM_ERROR("i915 GVT-g loading failed due to Graphics virtualization is not yet supported with GuC submission\n");
+		return -EIO;
 	}
 
 	/*
diff --git a/drivers/gpu/drm/i915/intel_gvt.h b/drivers/gpu/drm/i915/intel_gvt.h
index 25df2d65b985..61b246470282 100644
--- a/drivers/gpu/drm/i915/intel_gvt.h
+++ b/drivers/gpu/drm/i915/intel_gvt.h
@@ -32,6 +32,7 @@ void intel_gvt_cleanup(struct drm_i915_private *dev_priv);
 int intel_gvt_init_device(struct drm_i915_private *dev_priv);
 void intel_gvt_clean_device(struct drm_i915_private *dev_priv);
 int intel_gvt_init_host(void);
+void intel_gvt_sanitize_options(struct drm_i915_private *dev_priv);
 #else
 static inline int intel_gvt_init(struct drm_i915_private *dev_priv)
 {
@@ -40,6 +41,10 @@ static inline int intel_gvt_init(struct drm_i915_private *dev_priv)
 static inline void intel_gvt_cleanup(struct drm_i915_private *dev_priv)
 {
 }
+
+static inline void intel_gvt_sanitize_options(struct drm_i915_private *dev_priv)
+{
+}
 #endif
 
 #endif /* _INTEL_GVT_H_ */
diff --git a/drivers/gpu/drm/i915/intel_hangcheck.c b/drivers/gpu/drm/i915/intel_hangcheck.c
index dce742243ba6..9b0ece427bdc 100644
--- a/drivers/gpu/drm/i915/intel_hangcheck.c
+++ b/drivers/gpu/drm/i915/intel_hangcheck.c
@@ -407,7 +407,7 @@ static void hangcheck_declare_hang(struct drm_i915_private *i915,
 				 "%s, ", engine->name);
 	msg[len-2] = '\0';
 
-	return i915_handle_error(i915, hung, msg);
+	return i915_handle_error(i915, hung, "%s", msg);
 }
 
 /*
diff --git a/drivers/gpu/drm/i915/intel_hdmi.c b/drivers/gpu/drm/i915/intel_hdmi.c
index c6b8207724fa..41267ffb3624 100644
--- a/drivers/gpu/drm/i915/intel_hdmi.c
+++ b/drivers/gpu/drm/i915/intel_hdmi.c
@@ -1218,7 +1218,8 @@ static int intel_hdmi_source_max_tmds_clock(struct drm_i915_private *dev_priv)
 }
 
 static int hdmi_port_clock_limit(struct intel_hdmi *hdmi,
-				 bool respect_downstream_limits)
+				 bool respect_downstream_limits,
+				 bool force_dvi)
 {
 	struct drm_device *dev = intel_hdmi_to_dev(hdmi);
 	int max_tmds_clock = intel_hdmi_source_max_tmds_clock(to_i915(dev));
@@ -1234,7 +1235,7 @@ static int hdmi_port_clock_limit(struct intel_hdmi *hdmi,
 		if (info->max_tmds_clock)
 			max_tmds_clock = min(max_tmds_clock,
 					     info->max_tmds_clock);
-		else if (!hdmi->has_hdmi_sink)
+		else if (!hdmi->has_hdmi_sink || force_dvi)
 			max_tmds_clock = min(max_tmds_clock, 165000);
 	}
 
@@ -1243,13 +1244,14 @@ static int hdmi_port_clock_limit(struct intel_hdmi *hdmi,
 
 static enum drm_mode_status
 hdmi_port_clock_valid(struct intel_hdmi *hdmi,
-		      int clock, bool respect_downstream_limits)
+		      int clock, bool respect_downstream_limits,
+		      bool force_dvi)
 {
 	struct drm_i915_private *dev_priv = to_i915(intel_hdmi_to_dev(hdmi));
 
 	if (clock < 25000)
 		return MODE_CLOCK_LOW;
-	if (clock > hdmi_port_clock_limit(hdmi, respect_downstream_limits))
+	if (clock > hdmi_port_clock_limit(hdmi, respect_downstream_limits, force_dvi))
 		return MODE_CLOCK_HIGH;
 
 	/* BXT DPLL can't generate 223-240 MHz */
@@ -1273,6 +1275,8 @@ intel_hdmi_mode_valid(struct drm_connector *connector,
 	enum drm_mode_status status;
 	int clock;
 	int max_dotclk = to_i915(connector->dev)->max_dotclk_freq;
+	bool force_dvi =
+		READ_ONCE(to_intel_digital_connector_state(connector->state)->force_audio) == HDMI_AUDIO_OFF_DVI;
 
 	if (mode->flags & DRM_MODE_FLAG_DBLSCAN)
 		return MODE_NO_DBLESCAN;
@@ -1289,11 +1293,11 @@ intel_hdmi_mode_valid(struct drm_connector *connector,
 		clock *= 2;
 
 	/* check if we can do 8bpc */
-	status = hdmi_port_clock_valid(hdmi, clock, true);
+	status = hdmi_port_clock_valid(hdmi, clock, true, force_dvi);
 
 	/* if we can't do 8bpc we may still be able to do 12bpc */
-	if (!HAS_GMCH_DISPLAY(dev_priv) && status != MODE_OK)
-		status = hdmi_port_clock_valid(hdmi, clock * 3 / 2, true);
+	if (!HAS_GMCH_DISPLAY(dev_priv) && status != MODE_OK && hdmi->has_hdmi_sink && !force_dvi)
+		status = hdmi_port_clock_valid(hdmi, clock * 3 / 2, true, force_dvi);
 
 	return status;
 }
@@ -1327,6 +1331,11 @@ static bool hdmi_12bpc_possible(struct intel_crtc_state *crtc_state)
 			return false;
 	}
 
+	/* Display Wa #1139 */
+	if (IS_GLK_REVID(dev_priv, 0, GLK_REVID_A1) &&
+	    crtc_state->base.adjusted_mode.htotal > 5460)
+		return false;
+
 	return true;
 }
 
@@ -1338,16 +1347,19 @@ bool intel_hdmi_compute_config(struct intel_encoder *encoder,
 	struct drm_i915_private *dev_priv = to_i915(encoder->base.dev);
 	struct drm_display_mode *adjusted_mode = &pipe_config->base.adjusted_mode;
 	struct drm_scdc *scdc = &conn_state->connector->display_info.hdmi.scdc;
+	struct intel_digital_connector_state *intel_conn_state =
+		to_intel_digital_connector_state(conn_state);
 	int clock_8bpc = pipe_config->base.adjusted_mode.crtc_clock;
 	int clock_12bpc = clock_8bpc * 3 / 2;
 	int desired_bpp;
+	bool force_dvi = intel_conn_state->force_audio == HDMI_AUDIO_OFF_DVI;
 
-	pipe_config->has_hdmi_sink = intel_hdmi->has_hdmi_sink;
+	pipe_config->has_hdmi_sink = !force_dvi && intel_hdmi->has_hdmi_sink;
 
 	if (pipe_config->has_hdmi_sink)
 		pipe_config->has_infoframe = true;
 
-	if (intel_hdmi->color_range_auto) {
+	if (intel_conn_state->broadcast_rgb == INTEL_BROADCAST_RGB_AUTO) {
 		/* See CEA-861-E - 5.1 Default Encoding Parameters */
 		pipe_config->limited_color_range =
 			pipe_config->has_hdmi_sink &&
@@ -1355,7 +1367,7 @@ bool intel_hdmi_compute_config(struct intel_encoder *encoder,
 			HDMI_QUANTIZATION_RANGE_LIMITED;
 	} else {
 		pipe_config->limited_color_range =
-			intel_hdmi->limited_color_range;
+			intel_conn_state->broadcast_rgb == INTEL_BROADCAST_RGB_LIMITED;
 	}
 
 	if (adjusted_mode->flags & DRM_MODE_FLAG_DBLCLK) {
@@ -1367,8 +1379,13 @@ bool intel_hdmi_compute_config(struct intel_encoder *encoder,
 	if (HAS_PCH_SPLIT(dev_priv) && !HAS_DDI(dev_priv))
 		pipe_config->has_pch_encoder = true;
 
-	if (pipe_config->has_hdmi_sink && intel_hdmi->has_audio)
-		pipe_config->has_audio = true;
+	if (pipe_config->has_hdmi_sink) {
+		if (intel_conn_state->force_audio == HDMI_AUDIO_AUTO)
+			pipe_config->has_audio = intel_hdmi->has_audio;
+		else
+			pipe_config->has_audio =
+				intel_conn_state->force_audio == HDMI_AUDIO_ON;
+	}
 
 	/*
 	 * HDMI is either 12 or 8, so if the display lets 10bpc sneak
@@ -1376,8 +1393,8 @@ bool intel_hdmi_compute_config(struct intel_encoder *encoder,
 	 * outputs. We also need to check that the higher clock still fits
 	 * within limits.
 	 */
-	if (pipe_config->pipe_bpp > 8*3 && pipe_config->has_hdmi_sink &&
-	    hdmi_port_clock_valid(intel_hdmi, clock_12bpc, true) == MODE_OK &&
+	if (pipe_config->pipe_bpp > 8*3 && pipe_config->has_hdmi_sink && !force_dvi &&
+	    hdmi_port_clock_valid(intel_hdmi, clock_12bpc, true, force_dvi) == MODE_OK &&
 	    hdmi_12bpc_possible(pipe_config)) {
 		DRM_DEBUG_KMS("picking bpc to 12 for HDMI output\n");
 		desired_bpp = 12*3;
@@ -1392,12 +1409,12 @@ bool intel_hdmi_compute_config(struct intel_encoder *encoder,
 	}
 
 	if (!pipe_config->bw_constrained) {
-		DRM_DEBUG_KMS("forcing pipe bpc to %i for HDMI\n", desired_bpp);
+		DRM_DEBUG_KMS("forcing pipe bpp to %i for HDMI\n", desired_bpp);
 		pipe_config->pipe_bpp = desired_bpp;
 	}
 
 	if (hdmi_port_clock_valid(intel_hdmi, pipe_config->port_clock,
-				  false) != MODE_OK) {
+				  false, force_dvi) != MODE_OK) {
 		DRM_DEBUG_KMS("unsupported HDMI clock, rejecting mode\n");
 		return false;
 	}
@@ -1504,13 +1521,7 @@ intel_hdmi_set_edid(struct drm_connector *connector)
 			drm_rgb_quant_range_selectable(edid);
 
 		intel_hdmi->has_audio = drm_detect_monitor_audio(edid);
-		if (intel_hdmi->force_audio != HDMI_AUDIO_AUTO)
-			intel_hdmi->has_audio =
-				intel_hdmi->force_audio == HDMI_AUDIO_ON;
-
-		if (intel_hdmi->force_audio != HDMI_AUDIO_OFF_DVI)
-			intel_hdmi->has_hdmi_sink =
-				drm_detect_hdmi_monitor(edid);
+		intel_hdmi->has_hdmi_sink = drm_detect_hdmi_monitor(edid);
 
 		connected = true;
 	}
@@ -1572,96 +1583,6 @@ static int intel_hdmi_get_modes(struct drm_connector *connector)
 	return intel_connector_update_modes(connector, edid);
 }
 
-static bool
-intel_hdmi_detect_audio(struct drm_connector *connector)
-{
-	bool has_audio = false;
-	struct edid *edid;
-
-	edid = to_intel_connector(connector)->detect_edid;
-	if (edid && edid->input & DRM_EDID_INPUT_DIGITAL)
-		has_audio = drm_detect_monitor_audio(edid);
-
-	return has_audio;
-}
-
-static int
-intel_hdmi_set_property(struct drm_connector *connector,
-			struct drm_property *property,
-			uint64_t val)
-{
-	struct intel_hdmi *intel_hdmi = intel_attached_hdmi(connector);
-	struct intel_digital_port *intel_dig_port =
-		hdmi_to_dig_port(intel_hdmi);
-	struct drm_i915_private *dev_priv = to_i915(connector->dev);
-	int ret;
-
-	ret = drm_object_property_set_value(&connector->base, property, val);
-	if (ret)
-		return ret;
-
-	if (property == dev_priv->force_audio_property) {
-		enum hdmi_force_audio i = val;
-		bool has_audio;
-
-		if (i == intel_hdmi->force_audio)
-			return 0;
-
-		intel_hdmi->force_audio = i;
-
-		if (i == HDMI_AUDIO_AUTO)
-			has_audio = intel_hdmi_detect_audio(connector);
-		else
-			has_audio = (i == HDMI_AUDIO_ON);
-
-		if (i == HDMI_AUDIO_OFF_DVI)
-			intel_hdmi->has_hdmi_sink = 0;
-
-		intel_hdmi->has_audio = has_audio;
-		goto done;
-	}
-
-	if (property == dev_priv->broadcast_rgb_property) {
-		bool old_auto = intel_hdmi->color_range_auto;
-		bool old_range = intel_hdmi->limited_color_range;
-
-		switch (val) {
-		case INTEL_BROADCAST_RGB_AUTO:
-			intel_hdmi->color_range_auto = true;
-			break;
-		case INTEL_BROADCAST_RGB_FULL:
-			intel_hdmi->color_range_auto = false;
-			intel_hdmi->limited_color_range = false;
-			break;
-		case INTEL_BROADCAST_RGB_LIMITED:
-			intel_hdmi->color_range_auto = false;
-			intel_hdmi->limited_color_range = true;
-			break;
-		default:
-			return -EINVAL;
-		}
-
-		if (old_auto == intel_hdmi->color_range_auto &&
-		    old_range == intel_hdmi->limited_color_range)
-			return 0;
-
-		goto done;
-	}
-
-	if (property == connector->dev->mode_config.aspect_ratio_property) {
-		connector->state->picture_aspect_ratio = val;
-		goto done;
-	}
-
-	return -EINVAL;
-
-done:
-	if (intel_dig_port->base.base.crtc)
-		intel_crtc_restore_mode(intel_dig_port->base.base.crtc);
-
-	return 0;
-}
-
 static void intel_hdmi_pre_enable(struct intel_encoder *encoder,
 				  struct intel_crtc_state *pipe_config,
 				  struct drm_connector_state *conn_state)
@@ -1786,18 +1707,20 @@ static const struct drm_connector_funcs intel_hdmi_connector_funcs = {
 	.detect = intel_hdmi_detect,
 	.force = intel_hdmi_force,
 	.fill_modes = drm_helper_probe_single_connector_modes,
-	.set_property = intel_hdmi_set_property,
-	.atomic_get_property = intel_connector_atomic_get_property,
+	.set_property = drm_atomic_helper_connector_set_property,
+	.atomic_get_property = intel_digital_connector_atomic_get_property,
+	.atomic_set_property = intel_digital_connector_atomic_set_property,
 	.late_register = intel_connector_register,
 	.early_unregister = intel_connector_unregister,
 	.destroy = intel_hdmi_destroy,
 	.atomic_destroy_state = drm_atomic_helper_connector_destroy_state,
-	.atomic_duplicate_state = drm_atomic_helper_connector_duplicate_state,
+	.atomic_duplicate_state = intel_digital_connector_duplicate_state,
 };
 
 static const struct drm_connector_helper_funcs intel_hdmi_connector_helper_funcs = {
 	.get_modes = intel_hdmi_get_modes,
 	.mode_valid = intel_hdmi_mode_valid,
+	.atomic_check = intel_digital_connector_atomic_check,
 };
 
 static const struct drm_encoder_funcs intel_hdmi_enc_funcs = {
@@ -1809,7 +1732,6 @@ intel_hdmi_add_properties(struct intel_hdmi *intel_hdmi, struct drm_connector *c
 {
 	intel_attach_force_audio_property(connector);
 	intel_attach_broadcast_rgb_property(connector);
-	intel_hdmi->color_range_auto = true;
 	intel_attach_aspect_ratio_property(connector);
 	connector->state->picture_aspect_ratio = HDMI_PICTURE_ASPECT_NONE;
 }
diff --git a/drivers/gpu/drm/i915/intel_huc.c b/drivers/gpu/drm/i915/intel_huc.c
index 9ee819666a4c..f5eb18d0e2d1 100644
--- a/drivers/gpu/drm/i915/intel_huc.c
+++ b/drivers/gpu/drm/i915/intel_huc.c
@@ -52,6 +52,10 @@
 #define KBL_HUC_FW_MINOR 00
 #define KBL_BLD_NUM 1810
 
+#define GLK_HUC_FW_MAJOR 02
+#define GLK_HUC_FW_MINOR 00
+#define GLK_BLD_NUM 1748
+
 #define HUC_FW_PATH(platform, major, minor, bld_num) \
 	"i915/" __stringify(platform) "_huc_ver" __stringify(major) "_" \
 	__stringify(minor) "_" __stringify(bld_num) ".bin"
@@ -68,6 +72,9 @@ MODULE_FIRMWARE(I915_BXT_HUC_UCODE);
 	KBL_HUC_FW_MINOR, KBL_BLD_NUM)
 MODULE_FIRMWARE(I915_KBL_HUC_UCODE);
 
+#define I915_GLK_HUC_UCODE HUC_FW_PATH(glk, GLK_HUC_FW_MAJOR, \
+	GLK_HUC_FW_MINOR, GLK_BLD_NUM)
+
 /**
  * huc_ucode_xfer() - DMA's the firmware
  * @dev_priv: the drm_i915_private device
@@ -99,11 +106,6 @@ static int huc_ucode_xfer(struct drm_i915_private *dev_priv)
 
 	intel_uncore_forcewake_get(dev_priv, FORCEWAKE_ALL);
 
-	/* init WOPCM */
-	I915_WRITE(GUC_WOPCM_SIZE, intel_guc_wopcm_size(dev_priv));
-	I915_WRITE(DMA_GUC_WOPCM_OFFSET, GUC_WOPCM_OFFSET_VALUE |
-			HUC_LOADING_AGENT_GUC);
-
 	/* Set the source address for the uCode */
 	offset = guc_ggtt_offset(vma) + huc_fw->header_offset;
 	I915_WRITE(DMA_ADDR_0_LOW, lower_32_bits(offset));
@@ -169,6 +171,10 @@ void intel_huc_select_fw(struct intel_huc *huc)
 		huc->fw.path = I915_KBL_HUC_UCODE;
 		huc->fw.major_ver_wanted = KBL_HUC_FW_MAJOR;
 		huc->fw.minor_ver_wanted = KBL_HUC_FW_MINOR;
+	} else if (IS_GEMINILAKE(dev_priv)) {
+		huc->fw.path = I915_GLK_HUC_UCODE;
+		huc->fw.major_ver_wanted = GLK_HUC_FW_MAJOR;
+		huc->fw.minor_ver_wanted = GLK_HUC_FW_MINOR;
 	} else {
 		DRM_ERROR("No HuC firmware known for platform with HuC!\n");
 		return;
@@ -186,68 +192,36 @@ void intel_huc_select_fw(struct intel_huc *huc)
  * earlier call to intel_huc_init(), so here we need only check that
  * is succeeded, and then transfer the image to the h/w.
  *
- * Return:	non-zero code on error
  */
-int intel_huc_init_hw(struct intel_huc *huc)
+void intel_huc_init_hw(struct intel_huc *huc)
 {
 	struct drm_i915_private *dev_priv = huc_to_i915(huc);
 	int err;
 
-	if (huc->fw.fetch_status == INTEL_UC_FIRMWARE_NONE)
-		return 0;
-
 	DRM_DEBUG_DRIVER("%s fw status: fetch %s, load %s\n",
 		huc->fw.path,
 		intel_uc_fw_status_repr(huc->fw.fetch_status),
 		intel_uc_fw_status_repr(huc->fw.load_status));
 
-	if (huc->fw.fetch_status == INTEL_UC_FIRMWARE_SUCCESS &&
-	    huc->fw.load_status == INTEL_UC_FIRMWARE_FAIL)
-		return -ENOEXEC;
+	if (huc->fw.fetch_status != INTEL_UC_FIRMWARE_SUCCESS)
+		return;
 
 	huc->fw.load_status = INTEL_UC_FIRMWARE_PENDING;
 
-	switch (huc->fw.fetch_status) {
-	case INTEL_UC_FIRMWARE_FAIL:
-		/* something went wrong :( */
-		err = -EIO;
-		goto fail;
-
-	case INTEL_UC_FIRMWARE_NONE:
-	case INTEL_UC_FIRMWARE_PENDING:
-	default:
-		/* "can't happen" */
-		WARN_ONCE(1, "HuC fw %s invalid fetch_status %s [%d]\n",
-			huc->fw.path,
-			intel_uc_fw_status_repr(huc->fw.fetch_status),
-			huc->fw.fetch_status);
-		err = -ENXIO;
-		goto fail;
-
-	case INTEL_UC_FIRMWARE_SUCCESS:
-		break;
-	}
-
 	err = huc_ucode_xfer(dev_priv);
-	if (err)
-		goto fail;
 
-	huc->fw.load_status = INTEL_UC_FIRMWARE_SUCCESS;
+	huc->fw.load_status = err ?
+		INTEL_UC_FIRMWARE_FAIL : INTEL_UC_FIRMWARE_SUCCESS;
 
 	DRM_DEBUG_DRIVER("%s fw status: fetch %s, load %s\n",
 		huc->fw.path,
 		intel_uc_fw_status_repr(huc->fw.fetch_status),
 		intel_uc_fw_status_repr(huc->fw.load_status));
 
-	return 0;
-
-fail:
-	if (huc->fw.load_status == INTEL_UC_FIRMWARE_PENDING)
-		huc->fw.load_status = INTEL_UC_FIRMWARE_FAIL;
-
-	DRM_ERROR("Failed to complete HuC uCode load with ret %d\n", err);
+	if (huc->fw.load_status != INTEL_UC_FIRMWARE_SUCCESS)
+		DRM_ERROR("Failed to complete HuC uCode load with ret %d\n", err);
 
-	return err;
+	return;
 }
 
 /**
diff --git a/drivers/gpu/drm/i915/intel_lpe_audio.c b/drivers/gpu/drm/i915/intel_lpe_audio.c
index 25d8e76489e4..3bf65288ffff 100644
--- a/drivers/gpu/drm/i915/intel_lpe_audio.c
+++ b/drivers/gpu/drm/i915/intel_lpe_audio.c
@@ -63,6 +63,7 @@
 #include <linux/acpi.h>
 #include <linux/device.h>
 #include <linux/pci.h>
+#include <linux/pm_runtime.h>
 
 #include "i915_drv.h"
 #include <linux/delay.h>
@@ -110,6 +111,11 @@ lpe_audio_platdev_create(struct drm_i915_private *dev_priv)
 	pinfo.size_data = sizeof(*pdata);
 	pinfo.dma_mask = DMA_BIT_MASK(32);
 
+	pdata->num_pipes = INTEL_INFO(dev_priv)->num_pipes;
+	pdata->num_ports = IS_CHERRYVIEW(dev_priv) ? 3 : 2; /* B,C,D or B,C */
+	pdata->port[0].pipe = -1;
+	pdata->port[1].pipe = -1;
+	pdata->port[2].pipe = -1;
 	spin_lock_init(&pdata->lpe_audio_slock);
 
 	platdev = platform_device_register_full(&pinfo);
@@ -121,6 +127,10 @@ lpe_audio_platdev_create(struct drm_i915_private *dev_priv)
 
 	kfree(rsc);
 
+	pm_runtime_forbid(&platdev->dev);
+	pm_runtime_set_active(&platdev->dev);
+	pm_runtime_enable(&platdev->dev);
+
 	return platdev;
 
 err:
@@ -144,44 +154,10 @@ static void lpe_audio_platdev_destroy(struct drm_i915_private *dev_priv)
 
 static void lpe_audio_irq_unmask(struct irq_data *d)
 {
-	struct drm_i915_private *dev_priv = d->chip_data;
-	unsigned long irqflags;
-	u32 val = (I915_LPE_PIPE_A_INTERRUPT |
-		I915_LPE_PIPE_B_INTERRUPT);
-
-	if (IS_CHERRYVIEW(dev_priv))
-		val |= I915_LPE_PIPE_C_INTERRUPT;
-
-	spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
-
-	dev_priv->irq_mask &= ~val;
-	I915_WRITE(VLV_IIR, val);
-	I915_WRITE(VLV_IIR, val);
-	I915_WRITE(VLV_IMR, dev_priv->irq_mask);
-	POSTING_READ(VLV_IMR);
-
-	spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
 }
 
 static void lpe_audio_irq_mask(struct irq_data *d)
 {
-	struct drm_i915_private *dev_priv = d->chip_data;
-	unsigned long irqflags;
-	u32 val = (I915_LPE_PIPE_A_INTERRUPT |
-		I915_LPE_PIPE_B_INTERRUPT);
-
-	if (IS_CHERRYVIEW(dev_priv))
-		val |= I915_LPE_PIPE_C_INTERRUPT;
-
-	spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
-
-	dev_priv->irq_mask |= val;
-	I915_WRITE(VLV_IMR, dev_priv->irq_mask);
-	I915_WRITE(VLV_IIR, val);
-	I915_WRITE(VLV_IIR, val);
-	POSTING_READ(VLV_IIR);
-
-	spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
 }
 
 static struct irq_chip lpe_audio_irqchip = {
@@ -325,8 +301,6 @@ void intel_lpe_audio_teardown(struct drm_i915_private *dev_priv)
 
 	desc = irq_to_desc(dev_priv->lpe_audio.irq);
 
-	lpe_audio_irq_mask(&desc->irq_data);
-
 	lpe_audio_platdev_destroy(dev_priv);
 
 	irq_free_desc(dev_priv->lpe_audio.irq);
@@ -337,53 +311,47 @@ void intel_lpe_audio_teardown(struct drm_i915_private *dev_priv)
  * intel_lpe_audio_notify() - notify lpe audio event
  * audio driver and i915
  * @dev_priv: the i915 drm device private data
+ * @pipe: pipe
+ * @port: port
  * @eld : ELD data
- * @pipe: pipe id
- * @port: port id
- * @tmds_clk_speed: tmds clock frequency in Hz
+ * @ls_clock: Link symbol clock in kHz
+ * @dp_output: Driving a DP output?
  *
  * Notify lpe audio driver of eld change.
  */
 void intel_lpe_audio_notify(struct drm_i915_private *dev_priv,
-			    void *eld, int port, int pipe, int tmds_clk_speed,
-			    bool dp_output, int link_rate)
+			    enum pipe pipe, enum port port,
+			    const void *eld, int ls_clock, bool dp_output)
 {
-	unsigned long irq_flags;
-	struct intel_hdmi_lpe_audio_pdata *pdata = NULL;
+	unsigned long irqflags;
+	struct intel_hdmi_lpe_audio_pdata *pdata;
+	struct intel_hdmi_lpe_audio_port_pdata *ppdata;
 	u32 audio_enable;
 
 	if (!HAS_LPE_AUDIO(dev_priv))
 		return;
 
-	pdata = dev_get_platdata(
-		&(dev_priv->lpe_audio.platdev->dev));
+	pdata = dev_get_platdata(&dev_priv->lpe_audio.platdev->dev);
+	ppdata = &pdata->port[port - PORT_B];
 
-	spin_lock_irqsave(&pdata->lpe_audio_slock, irq_flags);
+	spin_lock_irqsave(&pdata->lpe_audio_slock, irqflags);
 
 	audio_enable = I915_READ(VLV_AUD_PORT_EN_DBG(port));
 
 	if (eld != NULL) {
-		memcpy(pdata->eld.eld_data, eld,
-			HDMI_MAX_ELD_BYTES);
-		pdata->eld.port_id = port;
-		pdata->eld.pipe_id = pipe;
-		pdata->hdmi_connected = true;
-
-		pdata->dp_output = dp_output;
-		if (tmds_clk_speed)
-			pdata->tmds_clock_speed = tmds_clk_speed;
-		if (link_rate)
-			pdata->link_rate = link_rate;
+		memcpy(ppdata->eld, eld, HDMI_MAX_ELD_BYTES);
+		ppdata->pipe = pipe;
+		ppdata->ls_clock = ls_clock;
+		ppdata->dp_output = dp_output;
 
 		/* Unmute the amp for both DP and HDMI */
 		I915_WRITE(VLV_AUD_PORT_EN_DBG(port),
 			   audio_enable & ~VLV_AMP_MUTE);
-
 	} else {
-		memset(pdata->eld.eld_data, 0,
-			HDMI_MAX_ELD_BYTES);
-		pdata->hdmi_connected = false;
-		pdata->dp_output = false;
+		memset(ppdata->eld, 0, HDMI_MAX_ELD_BYTES);
+		ppdata->pipe = -1;
+		ppdata->ls_clock = 0;
+		ppdata->dp_output = false;
 
 		/* Mute the amp for both DP and HDMI */
 		I915_WRITE(VLV_AUD_PORT_EN_DBG(port),
@@ -391,10 +359,7 @@ void intel_lpe_audio_notify(struct drm_i915_private *dev_priv,
 	}
 
 	if (pdata->notify_audio_lpe)
-		pdata->notify_audio_lpe(dev_priv->lpe_audio.platdev);
-	else
-		pdata->notify_pending = true;
+		pdata->notify_audio_lpe(dev_priv->lpe_audio.platdev, port - PORT_B);
 
-	spin_unlock_irqrestore(&pdata->lpe_audio_slock,
-			irq_flags);
+	spin_unlock_irqrestore(&pdata->lpe_audio_slock, irqflags);
 }
diff --git a/drivers/gpu/drm/i915/intel_lrc.c b/drivers/gpu/drm/i915/intel_lrc.c
index c8f7c631fc1f..014b30ace8a0 100644
--- a/drivers/gpu/drm/i915/intel_lrc.c
+++ b/drivers/gpu/drm/i915/intel_lrc.c
@@ -138,10 +138,6 @@
 #include "i915_drv.h"
 #include "intel_mocs.h"
 
-#define GEN9_LR_CONTEXT_RENDER_SIZE (22 * PAGE_SIZE)
-#define GEN8_LR_CONTEXT_RENDER_SIZE (20 * PAGE_SIZE)
-#define GEN8_LR_CONTEXT_OTHER_SIZE (2 * PAGE_SIZE)
-
 #define RING_EXECLIST_QFULL		(1 << 0x2)
 #define RING_EXECLIST1_VALID		(1 << 0x3)
 #define RING_EXECLIST0_VALID		(1 << 0x4)
@@ -326,8 +322,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.
@@ -342,39 +337,32 @@ static u64 execlists_update_context(struct drm_i915_gem_request *rq)
 
 static void execlists_submit_ports(struct intel_engine_cs *engine)
 {
-	struct drm_i915_private *dev_priv = engine->i915;
 	struct execlist_port *port = engine->execlist_port;
 	u32 __iomem *elsp =
-		dev_priv->regs + i915_mmio_reg_offset(RING_ELSP(engine));
-	u64 desc[2];
-
-	GEM_BUG_ON(port[0].count > 1);
-	if (!port[0].count)
-		execlists_context_status_change(port[0].request,
-						INTEL_CONTEXT_SCHEDULE_IN);
-	desc[0] = execlists_update_context(port[0].request);
-	GEM_DEBUG_EXEC(port[0].context_id = upper_32_bits(desc[0]));
-	port[0].count++;
-
-	if (port[1].request) {
-		GEM_BUG_ON(port[1].count);
-		execlists_context_status_change(port[1].request,
-						INTEL_CONTEXT_SCHEDULE_IN);
-		desc[1] = execlists_update_context(port[1].request);
-		GEM_DEBUG_EXEC(port[1].context_id = upper_32_bits(desc[1]));
-		port[1].count = 1;
-	} else {
-		desc[1] = 0;
-	}
-	GEM_BUG_ON(desc[0] == desc[1]);
-
-	/* You must always write both descriptors in the order below. */
-	writel(upper_32_bits(desc[1]), elsp);
-	writel(lower_32_bits(desc[1]), elsp);
+		engine->i915->regs + i915_mmio_reg_offset(RING_ELSP(engine));
+	unsigned int n;
+
+	for (n = ARRAY_SIZE(engine->execlist_port); n--; ) {
+		struct drm_i915_gem_request *rq;
+		unsigned int count;
+		u64 desc;
+
+		rq = port_unpack(&port[n], &count);
+		if (rq) {
+			GEM_BUG_ON(count > !n);
+			if (!count++)
+				execlists_context_status_change(rq, INTEL_CONTEXT_SCHEDULE_IN);
+			port_set(&port[n], port_pack(rq, count));
+			desc = execlists_update_context(rq);
+			GEM_DEBUG_EXEC(port[n].context_id = upper_32_bits(desc));
+		} else {
+			GEM_BUG_ON(!n);
+			desc = 0;
+		}
 
-	writel(upper_32_bits(desc[0]), elsp);
-	/* The context is automatically loaded after the following */
-	writel(lower_32_bits(desc[0]), elsp);
+		writel(upper_32_bits(desc), elsp);
+		writel(lower_32_bits(desc), elsp);
+	}
 }
 
 static bool ctx_single_port_submission(const struct i915_gem_context *ctx)
@@ -395,6 +383,17 @@ static bool can_merge_ctx(const struct i915_gem_context *prev,
 	return true;
 }
 
+static void port_assign(struct execlist_port *port,
+			struct drm_i915_gem_request *rq)
+{
+	GEM_BUG_ON(rq == port_request(port));
+
+	if (port_isset(port))
+		i915_gem_request_put(port_request(port));
+
+	port_set(port, port_pack(i915_gem_request_get(rq), port_count(port)));
+}
+
 static void execlists_dequeue(struct intel_engine_cs *engine)
 {
 	struct drm_i915_gem_request *last;
@@ -402,7 +401,7 @@ static void execlists_dequeue(struct intel_engine_cs *engine)
 	struct rb_node *rb;
 	bool submit = false;
 
-	last = port->request;
+	last = port_request(port);
 	if (last)
 		/* WaIdleLiteRestore:bdw,skl
 		 * Apply the wa NOOPs to prevent ring:HEAD == req:TAIL
@@ -412,7 +411,7 @@ static void execlists_dequeue(struct intel_engine_cs *engine)
 		 */
 		last->tail = last->wa_tail;
 
-	GEM_BUG_ON(port[1].request);
+	GEM_BUG_ON(port_isset(&port[1]));
 
 	/* Hardware submission is through 2 ports. Conceptually each port
 	 * has a (RING_START, RING_HEAD, RING_TAIL) tuple. RING_START is
@@ -437,72 +436,86 @@ static void execlists_dequeue(struct intel_engine_cs *engine)
 
 	spin_lock_irq(&engine->timeline->lock);
 	rb = engine->execlist_first;
+	GEM_BUG_ON(rb_first(&engine->execlist_queue) != rb);
 	while (rb) {
-		struct drm_i915_gem_request *cursor =
-			rb_entry(rb, typeof(*cursor), priotree.node);
-
-		/* Can we combine this request with the current port? It has to
-		 * be the same context/ringbuffer and not have any exceptions
-		 * (e.g. GVT saying never to combine contexts).
-		 *
-		 * If we can combine the requests, we can execute both by
-		 * updating the RING_TAIL to point to the end of the second
-		 * request, and so we never need to tell the hardware about
-		 * the first.
-		 */
-		if (last && !can_merge_ctx(cursor->ctx, last->ctx)) {
-			/* If we are on the second port and cannot combine
-			 * this request with the last, then we are done.
-			 */
-			if (port != engine->execlist_port)
-				break;
-
-			/* If GVT overrides us we only ever submit port[0],
-			 * leaving port[1] empty. Note that we also have
-			 * to be careful that we don't queue the same
-			 * context (even though a different request) to
-			 * the second port.
+		struct i915_priolist *p = rb_entry(rb, typeof(*p), node);
+		struct drm_i915_gem_request *rq, *rn;
+
+		list_for_each_entry_safe(rq, rn, &p->requests, priotree.link) {
+			/*
+			 * Can we combine this request with the current port?
+			 * It has to be the same context/ringbuffer and not
+			 * have any exceptions (e.g. GVT saying never to
+			 * combine contexts).
+			 *
+			 * If we can combine the requests, we can execute both
+			 * by updating the RING_TAIL to point to the end of the
+			 * second request, and so we never need to tell the
+			 * hardware about the first.
 			 */
-			if (ctx_single_port_submission(last->ctx) ||
-			    ctx_single_port_submission(cursor->ctx))
-				break;
+			if (last && !can_merge_ctx(rq->ctx, last->ctx)) {
+				/*
+				 * If we are on the second port and cannot
+				 * combine this request with the last, then we
+				 * are done.
+				 */
+				if (port != engine->execlist_port) {
+					__list_del_many(&p->requests,
+							&rq->priotree.link);
+					goto done;
+				}
+
+				/*
+				 * If GVT overrides us we only ever submit
+				 * port[0], leaving port[1] empty. Note that we
+				 * also have to be careful that we don't queue
+				 * the same context (even though a different
+				 * request) to the second port.
+				 */
+				if (ctx_single_port_submission(last->ctx) ||
+				    ctx_single_port_submission(rq->ctx)) {
+					__list_del_many(&p->requests,
+							&rq->priotree.link);
+					goto done;
+				}
+
+				GEM_BUG_ON(last->ctx == rq->ctx);
+
+				if (submit)
+					port_assign(port, last);
+				port++;
+			}
 
-			GEM_BUG_ON(last->ctx == cursor->ctx);
+			INIT_LIST_HEAD(&rq->priotree.link);
+			rq->priotree.priority = INT_MAX;
 
-			i915_gem_request_assign(&port->request, last);
-			port++;
+			__i915_gem_request_submit(rq);
+			trace_i915_gem_request_in(rq, port_index(port, engine));
+			last = rq;
+			submit = true;
 		}
 
 		rb = rb_next(rb);
-		rb_erase(&cursor->priotree.node, &engine->execlist_queue);
-		RB_CLEAR_NODE(&cursor->priotree.node);
-		cursor->priotree.priority = INT_MAX;
-
-		__i915_gem_request_submit(cursor);
-		trace_i915_gem_request_in(cursor, port - engine->execlist_port);
-		last = cursor;
-		submit = true;
-	}
-	if (submit) {
-		i915_gem_request_assign(&port->request, last);
-		engine->execlist_first = rb;
+		rb_erase(&p->node, &engine->execlist_queue);
+		INIT_LIST_HEAD(&p->requests);
+		if (p->priority != I915_PRIORITY_NORMAL)
+			kmem_cache_free(engine->i915->priorities, p);
 	}
+done:
+	engine->execlist_first = rb;
+	if (submit)
+		port_assign(port, last);
 	spin_unlock_irq(&engine->timeline->lock);
 
 	if (submit)
 		execlists_submit_ports(engine);
 }
 
-static bool execlists_elsp_idle(struct intel_engine_cs *engine)
-{
-	return !engine->execlist_port[0].request;
-}
-
 static bool execlists_elsp_ready(const struct intel_engine_cs *engine)
 {
 	const struct execlist_port *port = engine->execlist_port;
 
-	return port[0].count + port[1].count < 2;
+	return port_count(&port[0]) + port_count(&port[1]) < 2;
 }
 
 /*
@@ -515,6 +528,15 @@ static void intel_lrc_irq_handler(unsigned long data)
 	struct execlist_port *port = engine->execlist_port;
 	struct drm_i915_private *dev_priv = engine->i915;
 
+	/* We can skip acquiring intel_runtime_pm_get() here as it was taken
+	 * on our behalf by the request (see i915_gem_mark_busy()) and it will
+	 * not be relinquished until the device is idle (see
+	 * i915_gem_idle_work_handler()). As a precaution, we make sure
+	 * that all ELSP are drained i.e. we have processed the CSB,
+	 * before allowing ourselves to idle and calling intel_runtime_pm_put().
+	 */
+	GEM_BUG_ON(!dev_priv->gt.awake);
+
 	intel_uncore_forcewake_get(dev_priv, engine->fw_domains);
 
 	/* Prefer doing test_and_clear_bit() as a two stage operation to avoid
@@ -543,7 +565,9 @@ static void intel_lrc_irq_handler(unsigned long data)
 		tail = GEN8_CSB_WRITE_PTR(head);
 		head = GEN8_CSB_READ_PTR(head);
 		while (head != tail) {
+			struct drm_i915_gem_request *rq;
 			unsigned int status;
+			unsigned int count;
 
 			if (++head == GEN8_CSB_ENTRIES)
 				head = 0;
@@ -571,22 +595,26 @@ static void intel_lrc_irq_handler(unsigned long data)
 
 			/* Check the context/desc id for this event matches */
 			GEM_DEBUG_BUG_ON(readl(buf + 2 * head + 1) !=
-					 port[0].context_id);
+					 port->context_id);
 
-			GEM_BUG_ON(port[0].count == 0);
-			if (--port[0].count == 0) {
+			rq = port_unpack(port, &count);
+			GEM_BUG_ON(count == 0);
+			if (--count == 0) {
 				GEM_BUG_ON(status & GEN8_CTX_STATUS_PREEMPTED);
-				GEM_BUG_ON(!i915_gem_request_completed(port[0].request));
-				execlists_context_status_change(port[0].request,
-								INTEL_CONTEXT_SCHEDULE_OUT);
+				GEM_BUG_ON(!i915_gem_request_completed(rq));
+				execlists_context_status_change(rq, INTEL_CONTEXT_SCHEDULE_OUT);
+
+				trace_i915_gem_request_out(rq);
+				i915_gem_request_put(rq);
 
-				trace_i915_gem_request_out(port[0].request);
-				i915_gem_request_put(port[0].request);
 				port[0] = port[1];
 				memset(&port[1], 0, sizeof(port[1]));
+			} else {
+				port_set(port, port_pack(rq, count));
 			}
 
-			GEM_BUG_ON(port[0].count == 0 &&
+			/* After the final element, the hw should be idle */
+			GEM_BUG_ON(port_count(port) == 0 &&
 				   !(status & GEN8_CTX_STATUS_ACTIVE_IDLE));
 		}
 
@@ -600,28 +628,66 @@ static void intel_lrc_irq_handler(unsigned long data)
 	intel_uncore_forcewake_put(dev_priv, engine->fw_domains);
 }
 
-static bool insert_request(struct i915_priotree *pt, struct rb_root *root)
+static bool
+insert_request(struct intel_engine_cs *engine,
+	       struct i915_priotree *pt,
+	       int prio)
 {
-	struct rb_node **p, *rb;
+	struct i915_priolist *p;
+	struct rb_node **parent, *rb;
 	bool first = true;
 
+	if (unlikely(engine->no_priolist))
+		prio = I915_PRIORITY_NORMAL;
+
+find_priolist:
 	/* most positive priority is scheduled first, equal priorities fifo */
 	rb = NULL;
-	p = &root->rb_node;
-	while (*p) {
-		struct i915_priotree *pos;
-
-		rb = *p;
-		pos = rb_entry(rb, typeof(*pos), node);
-		if (pt->priority > pos->priority) {
-			p = &rb->rb_left;
-		} else {
-			p = &rb->rb_right;
+	parent = &engine->execlist_queue.rb_node;
+	while (*parent) {
+		rb = *parent;
+		p = rb_entry(rb, typeof(*p), node);
+		if (prio > p->priority) {
+			parent = &rb->rb_left;
+		} else if (prio < p->priority) {
+			parent = &rb->rb_right;
 			first = false;
+		} else {
+			list_add_tail(&pt->link, &p->requests);
+			return false;
 		}
 	}
-	rb_link_node(&pt->node, rb, p);
-	rb_insert_color(&pt->node, root);
+
+	if (prio == I915_PRIORITY_NORMAL) {
+		p = &engine->default_priolist;
+	} else {
+		p = kmem_cache_alloc(engine->i915->priorities, GFP_ATOMIC);
+		/* Convert an allocation failure to a priority bump */
+		if (unlikely(!p)) {
+			prio = I915_PRIORITY_NORMAL; /* recurses just once */
+
+			/* To maintain ordering with all rendering, after an
+			 * allocation failure we have to disable all scheduling.
+			 * Requests will then be executed in fifo, and schedule
+			 * will ensure that dependencies are emitted in fifo.
+			 * There will be still some reordering with existing
+			 * requests, so if userspace lied about their
+			 * dependencies that reordering may be visible.
+			 */
+			engine->no_priolist = true;
+			goto find_priolist;
+		}
+	}
+
+	p->priority = prio;
+	rb_link_node(&p->node, rb, parent);
+	rb_insert_color(&p->node, &engine->execlist_queue);
+
+	INIT_LIST_HEAD(&p->requests);
+	list_add_tail(&pt->link, &p->requests);
+
+	if (first)
+		engine->execlist_first = &p->node;
 
 	return first;
 }
@@ -634,12 +700,16 @@ static void execlists_submit_request(struct drm_i915_gem_request *request)
 	/* Will be called from irq-context when using foreign fences. */
 	spin_lock_irqsave(&engine->timeline->lock, flags);
 
-	if (insert_request(&request->priotree, &engine->execlist_queue)) {
-		engine->execlist_first = &request->priotree.node;
+	if (insert_request(engine,
+			   &request->priotree,
+			   request->priotree.priority)) {
 		if (execlists_elsp_ready(engine))
 			tasklet_hi_schedule(&engine->irq_tasklet);
 	}
 
+	GEM_BUG_ON(!engine->execlist_first);
+	GEM_BUG_ON(list_empty(&request->priotree.link));
+
 	spin_unlock_irqrestore(&engine->timeline->lock, flags);
 }
 
@@ -709,6 +779,19 @@ static void execlists_schedule(struct drm_i915_gem_request *request, int prio)
 		list_safe_reset_next(dep, p, dfs_link);
 	}
 
+	/* If we didn't need to bump any existing priorities, and we haven't
+	 * yet submitted this request (i.e. there is no potential race with
+	 * execlists_submit_request()), we can set our own priority and skip
+	 * acquiring the engine locks.
+	 */
+	if (request->priotree.priority == INT_MIN) {
+		GEM_BUG_ON(!list_empty(&request->priotree.link));
+		request->priotree.priority = prio;
+		if (stack.dfs_link.next == stack.dfs_link.prev)
+			return;
+		__list_del_entry(&stack.dfs_link);
+	}
+
 	engine = request->engine;
 	spin_lock_irq(&engine->timeline->lock);
 
@@ -724,10 +807,9 @@ static void execlists_schedule(struct drm_i915_gem_request *request, int prio)
 			continue;
 
 		pt->priority = prio;
-		if (!RB_EMPTY_NODE(&pt->node)) {
-			rb_erase(&pt->node, &engine->execlist_queue);
-			if (insert_request(pt, &engine->execlist_queue))
-				engine->execlist_first = &pt->node;
+		if (!list_empty(&pt->link)) {
+			__list_del_entry(&pt->link);
+			insert_request(engine, pt, prio);
 		}
 	}
 
@@ -736,8 +818,9 @@ static void execlists_schedule(struct drm_i915_gem_request *request, int prio)
 	/* XXX Do we need to preempt to make room for us and our deps? */
 }
 
-static int execlists_context_pin(struct intel_engine_cs *engine,
-				 struct i915_gem_context *ctx)
+static struct intel_ring *
+execlists_context_pin(struct intel_engine_cs *engine,
+		      struct i915_gem_context *ctx)
 {
 	struct intel_context *ce = &ctx->engine[engine->id];
 	unsigned int flags;
@@ -746,8 +829,8 @@ static int execlists_context_pin(struct intel_engine_cs *engine,
 
 	lockdep_assert_held(&ctx->i915->drm.struct_mutex);
 
-	if (ce->pin_count++)
-		return 0;
+	if (likely(ce->pin_count++))
+		goto out;
 	GEM_BUG_ON(!ce->pin_count); /* no overflow please! */
 
 	if (!ce->state) {
@@ -771,7 +854,7 @@ static int execlists_context_pin(struct intel_engine_cs *engine,
 		goto unpin_vma;
 	}
 
-	ret = intel_ring_pin(ce->ring, ctx->ggtt_offset_bias);
+	ret = intel_ring_pin(ce->ring, ctx->i915, ctx->ggtt_offset_bias);
 	if (ret)
 		goto unpin_map;
 
@@ -784,7 +867,8 @@ static int execlists_context_pin(struct intel_engine_cs *engine,
 	ce->state->obj->mm.dirty = true;
 
 	i915_gem_context_get(ctx);
-	return 0;
+out:
+	return ce->ring;
 
 unpin_map:
 	i915_gem_object_unpin_map(ce->state->obj);
@@ -792,7 +876,7 @@ unpin_vma:
 	__i915_vma_unpin(ce->state);
 err:
 	ce->pin_count = 0;
-	return ret;
+	return ERR_PTR(ret);
 }
 
 static void execlists_context_unpin(struct intel_engine_cs *engine,
@@ -829,9 +913,6 @@ static int execlists_request_alloc(struct drm_i915_gem_request *request)
 	 */
 	request->reserved_space += EXECLISTS_REQUEST_SIZE;
 
-	GEM_BUG_ON(!ce->ring);
-	request->ring = ce->ring;
-
 	if (i915.enable_guc_submission) {
 		/*
 		 * Check that the GuC has space for the request before
@@ -1139,14 +1220,12 @@ static int intel_init_workaround_bb(struct intel_engine_cs *engine)
 	return ret;
 }
 
-static u32 port_seqno(struct execlist_port *port)
-{
-	return port->request ? port->request->global_seqno : 0;
-}
-
 static int gen8_init_common_ring(struct intel_engine_cs *engine)
 {
 	struct drm_i915_private *dev_priv = engine->i915;
+	struct execlist_port *port = engine->execlist_port;
+	unsigned int n;
+	bool submit;
 	int ret;
 
 	ret = intel_mocs_init_engine(engine);
@@ -1167,16 +1246,24 @@ static int gen8_init_common_ring(struct intel_engine_cs *engine)
 
 	/* After a GPU reset, we may have requests to replay */
 	clear_bit(ENGINE_IRQ_EXECLIST, &engine->irq_posted);
-	if (!i915.enable_guc_submission && !execlists_elsp_idle(engine)) {
-		DRM_DEBUG_DRIVER("Restarting %s from requests [0x%x, 0x%x]\n",
-				 engine->name,
-				 port_seqno(&engine->execlist_port[0]),
-				 port_seqno(&engine->execlist_port[1]));
-		engine->execlist_port[0].count = 0;
-		engine->execlist_port[1].count = 0;
-		execlists_submit_ports(engine);
+
+	submit = false;
+	for (n = 0; n < ARRAY_SIZE(engine->execlist_port); n++) {
+		if (!port_isset(&port[n]))
+			break;
+
+		DRM_DEBUG_DRIVER("Restarting %s:%d from 0x%x\n",
+				 engine->name, n,
+				 port_request(&port[n])->global_seqno);
+
+		/* Discard the current inflight count */
+		port_set(&port[n], port_request(&port[n]));
+		submit = true;
 	}
 
+	if (submit && !i915.enable_guc_submission)
+		execlists_submit_ports(engine);
+
 	return 0;
 }
 
@@ -1252,13 +1339,13 @@ static void reset_common_ring(struct intel_engine_cs *engine,
 	intel_ring_update_space(request->ring);
 
 	/* Catch up with any missed context-switch interrupts */
-	if (request->ctx != port[0].request->ctx) {
-		i915_gem_request_put(port[0].request);
+	if (request->ctx != port_request(port)->ctx) {
+		i915_gem_request_put(port_request(port));
 		port[0] = port[1];
 		memset(&port[1], 0, sizeof(port[1]));
 	}
 
-	GEM_BUG_ON(request->ctx != port[0].request->ctx);
+	GEM_BUG_ON(request->ctx != port_request(port)->ctx);
 
 	/* Reset WaIdleLiteRestore:bdw,skl as well */
 	request->tail =
@@ -1907,44 +1994,6 @@ populate_lr_context(struct i915_gem_context *ctx,
 	return 0;
 }
 
-/**
- * intel_lr_context_size() - return the size of the context for an engine
- * @engine: which engine to find the context size for
- *
- * Each engine may require a different amount of space for a context image,
- * so when allocating (or copying) an image, this function can be used to
- * find the right size for the specific engine.
- *
- * Return: size (in bytes) of an engine-specific context image
- *
- * Note: this size includes the HWSP, which is part of the context image
- * in LRC mode, but does not include the "shared data page" used with
- * GuC submission. The caller should account for this if using the GuC.
- */
-uint32_t intel_lr_context_size(struct intel_engine_cs *engine)
-{
-	int ret = 0;
-
-	WARN_ON(INTEL_GEN(engine->i915) < 8);
-
-	switch (engine->id) {
-	case RCS:
-		if (INTEL_GEN(engine->i915) >= 9)
-			ret = GEN9_LR_CONTEXT_RENDER_SIZE;
-		else
-			ret = GEN8_LR_CONTEXT_RENDER_SIZE;
-		break;
-	case VCS:
-	case BCS:
-	case VECS:
-	case VCS2:
-		ret = GEN8_LR_CONTEXT_OTHER_SIZE;
-		break;
-	}
-
-	return ret;
-}
-
 static int execlists_context_deferred_alloc(struct i915_gem_context *ctx,
 					    struct intel_engine_cs *engine)
 {
@@ -1957,8 +2006,7 @@ static int execlists_context_deferred_alloc(struct i915_gem_context *ctx,
 
 	WARN_ON(ce->state);
 
-	context_size = round_up(intel_lr_context_size(engine),
-				I915_GTT_PAGE_SIZE);
+	context_size = round_up(engine->context_size, I915_GTT_PAGE_SIZE);
 
 	/* One extra page as the sharing data between driver and GuC */
 	context_size += PAGE_SIZE * LRC_PPHWSP_PN;
@@ -1989,7 +2037,7 @@ static int execlists_context_deferred_alloc(struct i915_gem_context *ctx,
 
 	ce->ring = ring;
 	ce->state = vma;
-	ce->initialised = engine->init_context == NULL;
+	ce->initialised |= engine->init_context == NULL;
 
 	return 0;
 
@@ -2036,8 +2084,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_lrc.h b/drivers/gpu/drm/i915/intel_lrc.h
index e8015e7bf4e9..52b3a1fd4059 100644
--- a/drivers/gpu/drm/i915/intel_lrc.h
+++ b/drivers/gpu/drm/i915/intel_lrc.h
@@ -78,8 +78,6 @@ int logical_xcs_ring_init(struct intel_engine_cs *engine);
 struct drm_i915_private;
 struct i915_gem_context;
 
-uint32_t intel_lr_context_size(struct intel_engine_cs *engine);
-
 void intel_lr_context_resume(struct drm_i915_private *dev_priv);
 uint64_t intel_lr_context_descriptor(struct i915_gem_context *ctx,
 				     struct intel_engine_cs *engine);
diff --git a/drivers/gpu/drm/i915/intel_lvds.c b/drivers/gpu/drm/i915/intel_lvds.c
index 8b942ef2b3ec..d2c2bca1b327 100644
--- a/drivers/gpu/drm/i915/intel_lvds.c
+++ b/drivers/gpu/drm/i915/intel_lvds.c
@@ -433,10 +433,10 @@ static bool intel_lvds_compute_config(struct intel_encoder *intel_encoder,
 		pipe_config->has_pch_encoder = true;
 
 		intel_pch_panel_fitting(intel_crtc, pipe_config,
-					intel_connector->panel.fitting_mode);
+					conn_state->scaling_mode);
 	} else {
 		intel_gmch_panel_fitting(intel_crtc, pipe_config,
-					 intel_connector->panel.fitting_mode);
+					 conn_state->scaling_mode);
 
 	}
 
@@ -598,56 +598,24 @@ static void intel_lvds_destroy(struct drm_connector *connector)
 	kfree(connector);
 }
 
-static int intel_lvds_set_property(struct drm_connector *connector,
-				   struct drm_property *property,
-				   uint64_t value)
-{
-	struct intel_connector *intel_connector = to_intel_connector(connector);
-	struct drm_device *dev = connector->dev;
-
-	if (property == dev->mode_config.scaling_mode_property) {
-		struct drm_crtc *crtc;
-
-		if (value == DRM_MODE_SCALE_NONE) {
-			DRM_DEBUG_KMS("no scaling not supported\n");
-			return -EINVAL;
-		}
-
-		if (intel_connector->panel.fitting_mode == value) {
-			/* the LVDS scaling property is not changed */
-			return 0;
-		}
-		intel_connector->panel.fitting_mode = value;
-
-		crtc = intel_attached_encoder(connector)->base.crtc;
-		if (crtc && crtc->state->enable) {
-			/*
-			 * If the CRTC is enabled, the display will be changed
-			 * according to the new panel fitting mode.
-			 */
-			intel_crtc_restore_mode(crtc);
-		}
-	}
-
-	return 0;
-}
-
 static const struct drm_connector_helper_funcs intel_lvds_connector_helper_funcs = {
 	.get_modes = intel_lvds_get_modes,
 	.mode_valid = intel_lvds_mode_valid,
+	.atomic_check = intel_digital_connector_atomic_check,
 };
 
 static const struct drm_connector_funcs intel_lvds_connector_funcs = {
 	.dpms = drm_atomic_helper_connector_dpms,
 	.detect = intel_lvds_detect,
 	.fill_modes = drm_helper_probe_single_connector_modes,
-	.set_property = intel_lvds_set_property,
-	.atomic_get_property = intel_connector_atomic_get_property,
+	.set_property = drm_atomic_helper_connector_set_property,
+	.atomic_get_property = intel_digital_connector_atomic_get_property,
+	.atomic_set_property = intel_digital_connector_atomic_set_property,
 	.late_register = intel_connector_register,
 	.early_unregister = intel_connector_unregister,
 	.destroy = intel_lvds_destroy,
 	.atomic_destroy_state = drm_atomic_helper_connector_destroy_state,
-	.atomic_duplicate_state = drm_atomic_helper_connector_duplicate_state,
+	.atomic_duplicate_state = intel_digital_connector_duplicate_state,
 };
 
 static const struct drm_encoder_funcs intel_lvds_enc_funcs = {
@@ -988,6 +956,7 @@ void intel_lvds_init(struct drm_i915_private *dev_priv)
 	u32 lvds;
 	int pipe;
 	u8 pin;
+	u32 allowed_scalers;
 
 	if (!intel_lvds_supported(dev_priv))
 		return;
@@ -1083,11 +1052,11 @@ void intel_lvds_init(struct drm_i915_private *dev_priv)
 	lvds_encoder->reg = lvds_reg;
 
 	/* create the scaling mode property */
-	drm_mode_create_scaling_mode_property(dev);
-	drm_object_attach_property(&connector->base,
-				      dev->mode_config.scaling_mode_property,
-				      DRM_MODE_SCALE_ASPECT);
-	intel_connector->panel.fitting_mode = DRM_MODE_SCALE_ASPECT;
+	allowed_scalers = BIT(DRM_MODE_SCALE_ASPECT);
+	allowed_scalers |= BIT(DRM_MODE_SCALE_FULLSCREEN);
+	allowed_scalers |= BIT(DRM_MODE_SCALE_CENTER);
+	drm_connector_attach_scaling_mode_property(connector, allowed_scalers);
+	connector->state->scaling_mode = DRM_MODE_SCALE_ASPECT;
 
 	intel_lvds_pps_get_hw_state(dev_priv, &lvds_encoder->init_pps);
 	lvds_encoder->init_lvds_val = lvds;
diff --git a/drivers/gpu/drm/i915/intel_panel.c b/drivers/gpu/drm/i915/intel_panel.c
index cb50c527401f..c8103f8d4dfa 100644
--- a/drivers/gpu/drm/i915/intel_panel.c
+++ b/drivers/gpu/drm/i915/intel_panel.c
@@ -888,10 +888,14 @@ static void pch_enable_backlight(struct intel_connector *connector)
 	struct drm_i915_private *dev_priv = to_i915(connector->base.dev);
 	struct intel_panel *panel = &connector->panel;
 	enum pipe pipe = intel_get_pipe_from_connector(connector);
-	enum transcoder cpu_transcoder =
-		intel_pipe_to_cpu_transcoder(dev_priv, pipe);
+	enum transcoder cpu_transcoder;
 	u32 cpu_ctl2, pch_ctl1, pch_ctl2;
 
+	if (!WARN_ON_ONCE(pipe == INVALID_PIPE))
+		cpu_transcoder = intel_pipe_to_cpu_transcoder(dev_priv, pipe);
+	else
+		cpu_transcoder = TRANSCODER_EDP;
+
 	cpu_ctl2 = I915_READ(BLC_PWM_CPU_CTL2);
 	if (cpu_ctl2 & BLM_PWM_ENABLE) {
 		DRM_DEBUG_KMS("cpu backlight already enabled\n");
@@ -973,6 +977,9 @@ static void i965_enable_backlight(struct intel_connector *connector)
 	enum pipe pipe = intel_get_pipe_from_connector(connector);
 	u32 ctl, ctl2, freq;
 
+	if (WARN_ON_ONCE(pipe == INVALID_PIPE))
+		pipe = PIPE_A;
+
 	ctl2 = I915_READ(BLC_PWM_CTL2);
 	if (ctl2 & BLM_PWM_ENABLE) {
 		DRM_DEBUG_KMS("backlight already enabled\n");
@@ -1037,6 +1044,9 @@ static void bxt_enable_backlight(struct intel_connector *connector)
 	enum pipe pipe = intel_get_pipe_from_connector(connector);
 	u32 pwm_ctl, val;
 
+	if (WARN_ON_ONCE(pipe == INVALID_PIPE))
+		pipe = PIPE_A;
+
 	/* Controller 1 uses the utility pin. */
 	if (panel->backlight.controller == 1) {
 		val = I915_READ(UTIL_PIN_CTL);
@@ -1093,7 +1103,8 @@ void intel_panel_enable_backlight(struct intel_connector *connector)
 	if (!panel->backlight.present)
 		return;
 
-	DRM_DEBUG_KMS("pipe %c\n", pipe_name(pipe));
+	if (!WARN_ON_ONCE(pipe == INVALID_PIPE))
+		DRM_DEBUG_KMS("pipe %c\n", pipe_name(pipe));
 
 	mutex_lock(&dev_priv->backlight_lock);
 
diff --git a/drivers/gpu/drm/i915/intel_pipe_crc.c b/drivers/gpu/drm/i915/intel_pipe_crc.c
index 206ee4f0150e..8fbd2bd0877f 100644
--- a/drivers/gpu/drm/i915/intel_pipe_crc.c
+++ b/drivers/gpu/drm/i915/intel_pipe_crc.c
@@ -513,16 +513,20 @@ static void hsw_trans_edp_pipe_A_crc_wa(struct drm_i915_private *dev_priv,
 	struct intel_crtc *crtc = intel_get_crtc_for_pipe(dev_priv, PIPE_A);
 	struct intel_crtc_state *pipe_config;
 	struct drm_atomic_state *state;
+	struct drm_modeset_acquire_ctx ctx;
 	int ret = 0;
 
-	drm_modeset_lock_all(dev);
+	drm_modeset_acquire_init(&ctx, 0);
+
 	state = drm_atomic_state_alloc(dev);
 	if (!state) {
 		ret = -ENOMEM;
 		goto unlock;
 	}
 
-	state->acquire_ctx = crtc->base.dev->mode_config.acquire_ctx;
+	state->acquire_ctx = &ctx;
+
+retry:
 	pipe_config = intel_atomic_get_crtc_state(state, crtc);
 	if (IS_ERR(pipe_config)) {
 		ret = PTR_ERR(pipe_config);
@@ -537,10 +541,17 @@ static void hsw_trans_edp_pipe_A_crc_wa(struct drm_i915_private *dev_priv,
 	ret = drm_atomic_commit(state);
 
 put_state:
+	if (ret == -EDEADLK) {
+		drm_atomic_state_clear(state);
+		drm_modeset_backoff(&ctx);
+		goto retry;
+	}
+
 	drm_atomic_state_put(state);
 unlock:
 	WARN(ret, "Toggling workaround to %i returns %i\n", enable, ret);
-	drm_modeset_unlock_all(dev);
+	drm_modeset_drop_locks(&ctx);
+	drm_modeset_acquire_fini(&ctx);
 }
 
 static int ivb_pipe_crc_ctl_reg(struct drm_i915_private *dev_priv,
@@ -842,19 +853,12 @@ static ssize_t display_crc_ctl_write(struct file *file, const char __user *ubuf,
 		return -E2BIG;
 	}
 
-	tmpbuf = kmalloc(len + 1, GFP_KERNEL);
-	if (!tmpbuf)
-		return -ENOMEM;
-
-	if (copy_from_user(tmpbuf, ubuf, len)) {
-		ret = -EFAULT;
-		goto out;
-	}
-	tmpbuf[len] = '\0';
+	tmpbuf = memdup_user_nul(ubuf, len);
+	if (IS_ERR(tmpbuf))
+		return PTR_ERR(tmpbuf);
 
 	ret = display_crc_ctl_parse(dev_priv, tmpbuf, len);
 
-out:
 	kfree(tmpbuf);
 	if (ret < 0)
 		return ret;
diff --git a/drivers/gpu/drm/i915/intel_pm.c b/drivers/gpu/drm/i915/intel_pm.c
index 570bd603f401..936eef1634c7 100644
--- a/drivers/gpu/drm/i915/intel_pm.c
+++ b/drivers/gpu/drm/i915/intel_pm.c
@@ -386,13 +386,53 @@ static bool _intel_set_memory_cxsr(struct drm_i915_private *dev_priv, bool enabl
 	return was_enabled;
 }
 
+/**
+ * intel_set_memory_cxsr - Configure CxSR state
+ * @dev_priv: i915 device
+ * @enable: Allow vs. disallow CxSR
+ *
+ * Allow or disallow the system to enter a special CxSR
+ * (C-state self refresh) state. What typically happens in CxSR mode
+ * is that several display FIFOs may get combined into a single larger
+ * FIFO for a particular plane (so called max FIFO mode) to allow the
+ * system to defer memory fetches longer, and the memory will enter
+ * self refresh.
+ *
+ * Note that enabling CxSR does not guarantee that the system enter
+ * this special mode, nor does it guarantee that the system stays
+ * in that mode once entered. So this just allows/disallows the system
+ * to autonomously utilize the CxSR mode. Other factors such as core
+ * C-states will affect when/if the system actually enters/exits the
+ * CxSR mode.
+ *
+ * Note that on VLV/CHV this actually only controls the max FIFO mode,
+ * and the system is free to enter/exit memory self refresh at any time
+ * even when the use of CxSR has been disallowed.
+ *
+ * While the system is actually in the CxSR/max FIFO mode, some plane
+ * control registers will not get latched on vblank. Thus in order to
+ * guarantee the system will respond to changes in the plane registers
+ * we must always disallow CxSR prior to making changes to those registers.
+ * Unfortunately the system will re-evaluate the CxSR conditions at
+ * frame start which happens after vblank start (which is when the plane
+ * registers would get latched), so we can't proceed with the plane update
+ * during the same frame where we disallowed CxSR.
+ *
+ * Certain platforms also have a deeper HPLL SR mode. Fortunately the
+ * HPLL SR mode depends on CxSR itself, so we don't have to hand hold
+ * the hardware w.r.t. HPLL SR when writing to plane registers.
+ * Disallowing just CxSR is sufficient.
+ */
 bool intel_set_memory_cxsr(struct drm_i915_private *dev_priv, bool enable)
 {
 	bool ret;
 
 	mutex_lock(&dev_priv->wm.wm_mutex);
 	ret = _intel_set_memory_cxsr(dev_priv, enable);
-	dev_priv->wm.vlv.cxsr = enable;
+	if (IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv))
+		dev_priv->wm.vlv.cxsr = enable;
+	else if (IS_G4X(dev_priv))
+		dev_priv->wm.g4x.cxsr = enable;
 	mutex_unlock(&dev_priv->wm.wm_mutex);
 
 	return ret;
@@ -454,13 +494,6 @@ static void vlv_get_fifo_size(struct intel_crtc_state *crtc_state)
 	fifo_state->plane[PLANE_SPRITE0] = sprite1_start - sprite0_start;
 	fifo_state->plane[PLANE_SPRITE1] = 511 - sprite1_start;
 	fifo_state->plane[PLANE_CURSOR] = 63;
-
-	DRM_DEBUG_KMS("Pipe %c FIFO size: %d/%d/%d/%d\n",
-		      pipe_name(pipe),
-		      fifo_state->plane[PLANE_PRIMARY],
-		      fifo_state->plane[PLANE_SPRITE0],
-		      fifo_state->plane[PLANE_SPRITE1],
-		      fifo_state->plane[PLANE_CURSOR]);
 }
 
 static int i9xx_get_fifo_size(struct drm_i915_private *dev_priv, int plane)
@@ -538,20 +571,6 @@ static const struct intel_watermark_params pineview_cursor_hplloff_wm = {
 	.guard_size = PINEVIEW_CURSOR_GUARD_WM,
 	.cacheline_size = PINEVIEW_FIFO_LINE_SIZE,
 };
-static const struct intel_watermark_params g4x_wm_info = {
-	.fifo_size = G4X_FIFO_SIZE,
-	.max_wm = G4X_MAX_WM,
-	.default_wm = G4X_MAX_WM,
-	.guard_size = 2,
-	.cacheline_size = G4X_FIFO_LINE_SIZE,
-};
-static const struct intel_watermark_params g4x_cursor_wm_info = {
-	.fifo_size = I965_CURSOR_FIFO,
-	.max_wm = I965_CURSOR_MAX_WM,
-	.default_wm = I965_CURSOR_DFT_WM,
-	.guard_size = 2,
-	.cacheline_size = G4X_FIFO_LINE_SIZE,
-};
 static const struct intel_watermark_params i965_cursor_wm_info = {
 	.fifo_size = I965_CURSOR_FIFO,
 	.max_wm = I965_CURSOR_MAX_WM,
@@ -596,8 +615,104 @@ static const struct intel_watermark_params i845_wm_info = {
 };
 
 /**
+ * intel_wm_method1 - Method 1 / "small buffer" watermark formula
+ * @pixel_rate: Pipe pixel rate in kHz
+ * @cpp: Plane bytes per pixel
+ * @latency: Memory wakeup latency in 0.1us units
+ *
+ * Compute the watermark using the method 1 or "small buffer"
+ * formula. The caller may additonally add extra cachelines
+ * to account for TLB misses and clock crossings.
+ *
+ * This method is concerned with the short term drain rate
+ * of the FIFO, ie. it does not account for blanking periods
+ * which would effectively reduce the average drain rate across
+ * a longer period. The name "small" refers to the fact the
+ * FIFO is relatively small compared to the amount of data
+ * fetched.
+ *
+ * The FIFO level vs. time graph might look something like:
+ *
+ *   |\   |\
+ *   | \  | \
+ * __---__---__ (- plane active, _ blanking)
+ * -> time
+ *
+ * or perhaps like this:
+ *
+ *   |\|\  |\|\
+ * __----__----__ (- plane active, _ blanking)
+ * -> time
+ *
+ * Returns:
+ * The watermark in bytes
+ */
+static unsigned int intel_wm_method1(unsigned int pixel_rate,
+				     unsigned int cpp,
+				     unsigned int latency)
+{
+	uint64_t ret;
+
+	ret = (uint64_t) pixel_rate * cpp * latency;
+	ret = DIV_ROUND_UP_ULL(ret, 10000);
+
+	return ret;
+}
+
+/**
+ * intel_wm_method2 - Method 2 / "large buffer" watermark formula
+ * @pixel_rate: Pipe pixel rate in kHz
+ * @htotal: Pipe horizontal total
+ * @width: Plane width in pixels
+ * @cpp: Plane bytes per pixel
+ * @latency: Memory wakeup latency in 0.1us units
+ *
+ * Compute the watermark using the method 2 or "large buffer"
+ * formula. The caller may additonally add extra cachelines
+ * to account for TLB misses and clock crossings.
+ *
+ * This method is concerned with the long term drain rate
+ * of the FIFO, ie. it does account for blanking periods
+ * which effectively reduce the average drain rate across
+ * a longer period. The name "large" refers to the fact the
+ * FIFO is relatively large compared to the amount of data
+ * fetched.
+ *
+ * The FIFO level vs. time graph might look something like:
+ *
+ *    |\___       |\___
+ *    |    \___   |    \___
+ *    |        \  |        \
+ * __ --__--__--__--__--__--__ (- plane active, _ blanking)
+ * -> time
+ *
+ * Returns:
+ * The watermark in bytes
+ */
+static unsigned int intel_wm_method2(unsigned int pixel_rate,
+				     unsigned int htotal,
+				     unsigned int width,
+				     unsigned int cpp,
+				     unsigned int latency)
+{
+	unsigned int ret;
+
+	/*
+	 * FIXME remove once all users are computing
+	 * watermarks in the correct place.
+	 */
+	if (WARN_ON_ONCE(htotal == 0))
+		htotal = 1;
+
+	ret = (latency * pixel_rate) / (htotal * 10000);
+	ret = (ret + 1) * width * cpp;
+
+	return ret;
+}
+
+/**
  * intel_calculate_wm - calculate watermark level
- * @clock_in_khz: pixel clock
+ * @pixel_rate: pixel clock
  * @wm: chip FIFO params
  * @cpp: bytes per pixel
  * @latency_ns: memory latency for the platform
@@ -613,12 +728,12 @@ static const struct intel_watermark_params i845_wm_info = {
  * past the watermark point.  If the FIFO drains completely, a FIFO underrun
  * will occur, and a display engine hang could result.
  */
-static unsigned long intel_calculate_wm(unsigned long clock_in_khz,
-					const struct intel_watermark_params *wm,
-					int fifo_size, int cpp,
-					unsigned long latency_ns)
+static unsigned int intel_calculate_wm(int pixel_rate,
+				       const struct intel_watermark_params *wm,
+				       int fifo_size, int cpp,
+				       unsigned int latency_ns)
 {
-	long entries_required, wm_size;
+	int entries, wm_size;
 
 	/*
 	 * Note: we need to make sure we don't overflow for various clock &
@@ -626,18 +741,17 @@ static unsigned long intel_calculate_wm(unsigned long clock_in_khz,
 	 * clocks go from a few thousand to several hundred thousand.
 	 * latency is usually a few thousand
 	 */
-	entries_required = ((clock_in_khz / 1000) * cpp * latency_ns) /
-		1000;
-	entries_required = DIV_ROUND_UP(entries_required, wm->cacheline_size);
-
-	DRM_DEBUG_KMS("FIFO entries required for mode: %ld\n", entries_required);
-
-	wm_size = fifo_size - (entries_required + wm->guard_size);
+	entries = intel_wm_method1(pixel_rate, cpp,
+				   latency_ns / 100);
+	entries = DIV_ROUND_UP(entries, wm->cacheline_size) +
+		wm->guard_size;
+	DRM_DEBUG_KMS("FIFO entries required for mode: %d\n", entries);
 
-	DRM_DEBUG_KMS("FIFO watermark level: %ld\n", wm_size);
+	wm_size = fifo_size - entries;
+	DRM_DEBUG_KMS("FIFO watermark level: %d\n", wm_size);
 
 	/* Don't promote wm_size to unsigned... */
-	if (wm_size > (long)wm->max_wm)
+	if (wm_size > wm->max_wm)
 		wm_size = wm->max_wm;
 	if (wm_size <= 0)
 		wm_size = wm->default_wm;
@@ -655,6 +769,21 @@ static unsigned long intel_calculate_wm(unsigned long clock_in_khz,
 	return wm_size;
 }
 
+static bool is_disabling(int old, int new, int threshold)
+{
+	return old >= threshold && new < threshold;
+}
+
+static bool is_enabling(int old, int new, int threshold)
+{
+	return old < threshold && new >= threshold;
+}
+
+static int intel_wm_num_levels(struct drm_i915_private *dev_priv)
+{
+	return dev_priv->wm.max_level + 1;
+}
+
 static bool intel_wm_plane_visible(const struct intel_crtc_state *crtc_state,
 				   const struct intel_plane_state *plane_state)
 {
@@ -699,7 +828,7 @@ static void pineview_update_wm(struct intel_crtc *unused_crtc)
 	struct intel_crtc *crtc;
 	const struct cxsr_latency *latency;
 	u32 reg;
-	unsigned long wm;
+	unsigned int wm;
 
 	latency = intel_get_cxsr_latency(IS_PINEVIEW_G(dev_priv),
 					 dev_priv->is_ddr3,
@@ -733,7 +862,7 @@ static void pineview_update_wm(struct intel_crtc *unused_crtc)
 		/* cursor SR */
 		wm = intel_calculate_wm(clock, &pineview_cursor_wm,
 					pineview_display_wm.fifo_size,
-					cpp, latency->cursor_sr);
+					4, latency->cursor_sr);
 		reg = I915_READ(DSPFW3);
 		reg &= ~DSPFW_CURSOR_SR_MASK;
 		reg |= FW_WM(wm, CURSOR_SR);
@@ -751,7 +880,7 @@ static void pineview_update_wm(struct intel_crtc *unused_crtc)
 		/* cursor HPLL off SR */
 		wm = intel_calculate_wm(clock, &pineview_cursor_hplloff_wm,
 					pineview_display_hplloff_wm.fifo_size,
-					cpp, latency->cursor_hpll_disable);
+					4, latency->cursor_hpll_disable);
 		reg = I915_READ(DSPFW3);
 		reg &= ~DSPFW_HPLL_CURSOR_MASK;
 		reg |= FW_WM(wm, HPLL_CURSOR);
@@ -764,144 +893,50 @@ static void pineview_update_wm(struct intel_crtc *unused_crtc)
 	}
 }
 
-static bool g4x_compute_wm0(struct drm_i915_private *dev_priv,
-			    int plane,
-			    const struct intel_watermark_params *display,
-			    int display_latency_ns,
-			    const struct intel_watermark_params *cursor,
-			    int cursor_latency_ns,
-			    int *plane_wm,
-			    int *cursor_wm)
-{
-	struct intel_crtc *crtc;
-	const struct drm_display_mode *adjusted_mode;
-	const struct drm_framebuffer *fb;
-	int htotal, hdisplay, clock, cpp;
-	int line_time_us, line_count;
-	int entries, tlb_miss;
-
-	crtc = intel_get_crtc_for_plane(dev_priv, plane);
-	if (!intel_crtc_active(crtc)) {
-		*cursor_wm = cursor->guard_size;
-		*plane_wm = display->guard_size;
-		return false;
-	}
-
-	adjusted_mode = &crtc->config->base.adjusted_mode;
-	fb = crtc->base.primary->state->fb;
-	clock = adjusted_mode->crtc_clock;
-	htotal = adjusted_mode->crtc_htotal;
-	hdisplay = crtc->config->pipe_src_w;
-	cpp = fb->format->cpp[0];
-
-	/* Use the small buffer method to calculate plane watermark */
-	entries = ((clock * cpp / 1000) * display_latency_ns) / 1000;
-	tlb_miss = display->fifo_size*display->cacheline_size - hdisplay * 8;
-	if (tlb_miss > 0)
-		entries += tlb_miss;
-	entries = DIV_ROUND_UP(entries, display->cacheline_size);
-	*plane_wm = entries + display->guard_size;
-	if (*plane_wm > (int)display->max_wm)
-		*plane_wm = display->max_wm;
-
-	/* Use the large buffer method to calculate cursor watermark */
-	line_time_us = max(htotal * 1000 / clock, 1);
-	line_count = (cursor_latency_ns / line_time_us + 1000) / 1000;
-	entries = line_count * crtc->base.cursor->state->crtc_w * cpp;
-	tlb_miss = cursor->fifo_size*cursor->cacheline_size - hdisplay * 8;
-	if (tlb_miss > 0)
-		entries += tlb_miss;
-	entries = DIV_ROUND_UP(entries, cursor->cacheline_size);
-	*cursor_wm = entries + cursor->guard_size;
-	if (*cursor_wm > (int)cursor->max_wm)
-		*cursor_wm = (int)cursor->max_wm;
-
-	return true;
-}
-
 /*
- * Check the wm result.
- *
- * If any calculated watermark values is larger than the maximum value that
- * can be programmed into the associated watermark register, that watermark
- * must be disabled.
+ * Documentation says:
+ * "If the line size is small, the TLB fetches can get in the way of the
+ *  data fetches, causing some lag in the pixel data return which is not
+ *  accounted for in the above formulas. The following adjustment only
+ *  needs to be applied if eight whole lines fit in the buffer at once.
+ *  The WM is adjusted upwards by the difference between the FIFO size
+ *  and the size of 8 whole lines. This adjustment is always performed
+ *  in the actual pixel depth regardless of whether FBC is enabled or not."
  */
-static bool g4x_check_srwm(struct drm_i915_private *dev_priv,
-			   int display_wm, int cursor_wm,
-			   const struct intel_watermark_params *display,
-			   const struct intel_watermark_params *cursor)
+static int g4x_tlb_miss_wa(int fifo_size, int width, int cpp)
 {
-	DRM_DEBUG_KMS("SR watermark: display plane %d, cursor %d\n",
-		      display_wm, cursor_wm);
-
-	if (display_wm > display->max_wm) {
-		DRM_DEBUG_KMS("display watermark is too large(%d/%u), disabling\n",
-			      display_wm, display->max_wm);
-		return false;
-	}
+	int tlb_miss = fifo_size * 64 - width * cpp * 8;
 
-	if (cursor_wm > cursor->max_wm) {
-		DRM_DEBUG_KMS("cursor watermark is too large(%d/%u), disabling\n",
-			      cursor_wm, cursor->max_wm);
-		return false;
-	}
-
-	if (!(display_wm || cursor_wm)) {
-		DRM_DEBUG_KMS("SR latency is 0, disabling\n");
-		return false;
-	}
-
-	return true;
+	return max(0, tlb_miss);
 }
 
-static bool g4x_compute_srwm(struct drm_i915_private *dev_priv,
-			     int plane,
-			     int latency_ns,
-			     const struct intel_watermark_params *display,
-			     const struct intel_watermark_params *cursor,
-			     int *display_wm, int *cursor_wm)
+static void g4x_write_wm_values(struct drm_i915_private *dev_priv,
+				const struct g4x_wm_values *wm)
 {
-	struct intel_crtc *crtc;
-	const struct drm_display_mode *adjusted_mode;
-	const struct drm_framebuffer *fb;
-	int hdisplay, htotal, cpp, clock;
-	unsigned long line_time_us;
-	int line_count, line_size;
-	int small, large;
-	int entries;
-
-	if (!latency_ns) {
-		*display_wm = *cursor_wm = 0;
-		return false;
-	}
-
-	crtc = intel_get_crtc_for_plane(dev_priv, plane);
-	adjusted_mode = &crtc->config->base.adjusted_mode;
-	fb = crtc->base.primary->state->fb;
-	clock = adjusted_mode->crtc_clock;
-	htotal = adjusted_mode->crtc_htotal;
-	hdisplay = crtc->config->pipe_src_w;
-	cpp = fb->format->cpp[0];
-
-	line_time_us = max(htotal * 1000 / clock, 1);
-	line_count = (latency_ns / line_time_us + 1000) / 1000;
-	line_size = hdisplay * cpp;
-
-	/* Use the minimum of the small and large buffer method for primary */
-	small = ((clock * cpp / 1000) * latency_ns) / 1000;
-	large = line_count * line_size;
+	enum pipe pipe;
 
-	entries = DIV_ROUND_UP(min(small, large), display->cacheline_size);
-	*display_wm = entries + display->guard_size;
+	for_each_pipe(dev_priv, pipe)
+		trace_g4x_wm(intel_get_crtc_for_pipe(dev_priv, pipe), wm);
 
-	/* calculate the self-refresh watermark for display cursor */
-	entries = line_count * cpp * crtc->base.cursor->state->crtc_w;
-	entries = DIV_ROUND_UP(entries, cursor->cacheline_size);
-	*cursor_wm = entries + cursor->guard_size;
+	I915_WRITE(DSPFW1,
+		   FW_WM(wm->sr.plane, SR) |
+		   FW_WM(wm->pipe[PIPE_B].plane[PLANE_CURSOR], CURSORB) |
+		   FW_WM(wm->pipe[PIPE_B].plane[PLANE_PRIMARY], PLANEB) |
+		   FW_WM(wm->pipe[PIPE_A].plane[PLANE_PRIMARY], PLANEA));
+	I915_WRITE(DSPFW2,
+		   (wm->fbc_en ? DSPFW_FBC_SR_EN : 0) |
+		   FW_WM(wm->sr.fbc, FBC_SR) |
+		   FW_WM(wm->hpll.fbc, FBC_HPLL_SR) |
+		   FW_WM(wm->pipe[PIPE_B].plane[PLANE_SPRITE0], SPRITEB) |
+		   FW_WM(wm->pipe[PIPE_A].plane[PLANE_CURSOR], CURSORA) |
+		   FW_WM(wm->pipe[PIPE_A].plane[PLANE_SPRITE0], SPRITEA));
+	I915_WRITE(DSPFW3,
+		   (wm->hpll_en ? DSPFW_HPLL_SR_EN : 0) |
+		   FW_WM(wm->sr.cursor, CURSOR_SR) |
+		   FW_WM(wm->hpll.cursor, HPLL_CURSOR) |
+		   FW_WM(wm->hpll.plane, HPLL_SR));
 
-	return g4x_check_srwm(dev_priv,
-			      *display_wm, *cursor_wm,
-			      display, cursor);
+	POSTING_READ(DSPFW1);
 }
 
 #define FW_WM_VLV(value, plane) \
@@ -985,17 +1020,535 @@ static void vlv_write_wm_values(struct drm_i915_private *dev_priv,
 
 #undef FW_WM_VLV
 
+static void g4x_setup_wm_latency(struct drm_i915_private *dev_priv)
+{
+	/* all latencies in usec */
+	dev_priv->wm.pri_latency[G4X_WM_LEVEL_NORMAL] = 5;
+	dev_priv->wm.pri_latency[G4X_WM_LEVEL_SR] = 12;
+	dev_priv->wm.pri_latency[G4X_WM_LEVEL_HPLL] = 35;
+
+	dev_priv->wm.max_level = G4X_WM_LEVEL_HPLL;
+}
+
+static int g4x_plane_fifo_size(enum plane_id plane_id, int level)
+{
+	/*
+	 * DSPCNTR[13] supposedly controls whether the
+	 * primary plane can use the FIFO space otherwise
+	 * reserved for the sprite plane. It's not 100% clear
+	 * what the actual FIFO size is, but it looks like we
+	 * can happily set both primary and sprite watermarks
+	 * up to 127 cachelines. So that would seem to mean
+	 * that either DSPCNTR[13] doesn't do anything, or that
+	 * the total FIFO is >= 256 cachelines in size. Either
+	 * way, we don't seem to have to worry about this
+	 * repartitioning as the maximum watermark value the
+	 * register can hold for each plane is lower than the
+	 * minimum FIFO size.
+	 */
+	switch (plane_id) {
+	case PLANE_CURSOR:
+		return 63;
+	case PLANE_PRIMARY:
+		return level == G4X_WM_LEVEL_NORMAL ? 127 : 511;
+	case PLANE_SPRITE0:
+		return level == G4X_WM_LEVEL_NORMAL ? 127 : 0;
+	default:
+		MISSING_CASE(plane_id);
+		return 0;
+	}
+}
+
+static int g4x_fbc_fifo_size(int level)
+{
+	switch (level) {
+	case G4X_WM_LEVEL_SR:
+		return 7;
+	case G4X_WM_LEVEL_HPLL:
+		return 15;
+	default:
+		MISSING_CASE(level);
+		return 0;
+	}
+}
+
+static uint16_t g4x_compute_wm(const struct intel_crtc_state *crtc_state,
+			       const struct intel_plane_state *plane_state,
+			       int level)
+{
+	struct intel_plane *plane = to_intel_plane(plane_state->base.plane);
+	struct drm_i915_private *dev_priv = to_i915(plane->base.dev);
+	const struct drm_display_mode *adjusted_mode =
+		&crtc_state->base.adjusted_mode;
+	int clock, htotal, cpp, width, wm;
+	int latency = dev_priv->wm.pri_latency[level] * 10;
+
+	if (latency == 0)
+		return USHRT_MAX;
+
+	if (!intel_wm_plane_visible(crtc_state, plane_state))
+		return 0;
+
+	/*
+	 * Not 100% sure which way ELK should go here as the
+	 * spec only says CL/CTG should assume 32bpp and BW
+	 * doesn't need to. But as these things followed the
+	 * mobile vs. desktop lines on gen3 as well, let's
+	 * assume ELK doesn't need this.
+	 *
+	 * The spec also fails to list such a restriction for
+	 * the HPLL watermark, which seems a little strange.
+	 * Let's use 32bpp for the HPLL watermark as well.
+	 */
+	if (IS_GM45(dev_priv) && plane->id == PLANE_PRIMARY &&
+	    level != G4X_WM_LEVEL_NORMAL)
+		cpp = 4;
+	else
+		cpp = plane_state->base.fb->format->cpp[0];
+
+	clock = adjusted_mode->crtc_clock;
+	htotal = adjusted_mode->crtc_htotal;
+
+	if (plane->id == PLANE_CURSOR)
+		width = plane_state->base.crtc_w;
+	else
+		width = drm_rect_width(&plane_state->base.dst);
+
+	if (plane->id == PLANE_CURSOR) {
+		wm = intel_wm_method2(clock, htotal, width, cpp, latency);
+	} else if (plane->id == PLANE_PRIMARY &&
+		   level == G4X_WM_LEVEL_NORMAL) {
+		wm = intel_wm_method1(clock, cpp, latency);
+	} else {
+		int small, large;
+
+		small = intel_wm_method1(clock, cpp, latency);
+		large = intel_wm_method2(clock, htotal, width, cpp, latency);
+
+		wm = min(small, large);
+	}
+
+	wm += g4x_tlb_miss_wa(g4x_plane_fifo_size(plane->id, level),
+			      width, cpp);
+
+	wm = DIV_ROUND_UP(wm, 64) + 2;
+
+	return min_t(int, wm, USHRT_MAX);
+}
+
+static bool g4x_raw_plane_wm_set(struct intel_crtc_state *crtc_state,
+				 int level, enum plane_id plane_id, u16 value)
+{
+	struct drm_i915_private *dev_priv = to_i915(crtc_state->base.crtc->dev);
+	bool dirty = false;
+
+	for (; level < intel_wm_num_levels(dev_priv); level++) {
+		struct g4x_pipe_wm *raw = &crtc_state->wm.g4x.raw[level];
+
+		dirty |= raw->plane[plane_id] != value;
+		raw->plane[plane_id] = value;
+	}
+
+	return dirty;
+}
+
+static bool g4x_raw_fbc_wm_set(struct intel_crtc_state *crtc_state,
+			       int level, u16 value)
+{
+	struct drm_i915_private *dev_priv = to_i915(crtc_state->base.crtc->dev);
+	bool dirty = false;
+
+	/* NORMAL level doesn't have an FBC watermark */
+	level = max(level, G4X_WM_LEVEL_SR);
+
+	for (; level < intel_wm_num_levels(dev_priv); level++) {
+		struct g4x_pipe_wm *raw = &crtc_state->wm.g4x.raw[level];
+
+		dirty |= raw->fbc != value;
+		raw->fbc = value;
+	}
+
+	return dirty;
+}
+
+static uint32_t ilk_compute_fbc_wm(const struct intel_crtc_state *cstate,
+				   const struct intel_plane_state *pstate,
+				   uint32_t pri_val);
+
+static bool g4x_raw_plane_wm_compute(struct intel_crtc_state *crtc_state,
+				     const struct intel_plane_state *plane_state)
+{
+	struct intel_plane *plane = to_intel_plane(plane_state->base.plane);
+	int num_levels = intel_wm_num_levels(to_i915(plane->base.dev));
+	enum plane_id plane_id = plane->id;
+	bool dirty = false;
+	int level;
+
+	if (!intel_wm_plane_visible(crtc_state, plane_state)) {
+		dirty |= g4x_raw_plane_wm_set(crtc_state, 0, plane_id, 0);
+		if (plane_id == PLANE_PRIMARY)
+			dirty |= g4x_raw_fbc_wm_set(crtc_state, 0, 0);
+		goto out;
+	}
+
+	for (level = 0; level < num_levels; level++) {
+		struct g4x_pipe_wm *raw = &crtc_state->wm.g4x.raw[level];
+		int wm, max_wm;
+
+		wm = g4x_compute_wm(crtc_state, plane_state, level);
+		max_wm = g4x_plane_fifo_size(plane_id, level);
+
+		if (wm > max_wm)
+			break;
+
+		dirty |= raw->plane[plane_id] != wm;
+		raw->plane[plane_id] = wm;
+
+		if (plane_id != PLANE_PRIMARY ||
+		    level == G4X_WM_LEVEL_NORMAL)
+			continue;
+
+		wm = ilk_compute_fbc_wm(crtc_state, plane_state,
+					raw->plane[plane_id]);
+		max_wm = g4x_fbc_fifo_size(level);
+
+		/*
+		 * FBC wm is not mandatory as we
+		 * can always just disable its use.
+		 */
+		if (wm > max_wm)
+			wm = USHRT_MAX;
+
+		dirty |= raw->fbc != wm;
+		raw->fbc = wm;
+	}
+
+	/* mark watermarks as invalid */
+	dirty |= g4x_raw_plane_wm_set(crtc_state, level, plane_id, USHRT_MAX);
+
+	if (plane_id == PLANE_PRIMARY)
+		dirty |= g4x_raw_fbc_wm_set(crtc_state, level, USHRT_MAX);
+
+ out:
+	if (dirty) {
+		DRM_DEBUG_KMS("%s watermarks: normal=%d, SR=%d, HPLL=%d\n",
+			      plane->base.name,
+			      crtc_state->wm.g4x.raw[G4X_WM_LEVEL_NORMAL].plane[plane_id],
+			      crtc_state->wm.g4x.raw[G4X_WM_LEVEL_SR].plane[plane_id],
+			      crtc_state->wm.g4x.raw[G4X_WM_LEVEL_HPLL].plane[plane_id]);
+
+		if (plane_id == PLANE_PRIMARY)
+			DRM_DEBUG_KMS("FBC watermarks: SR=%d, HPLL=%d\n",
+				      crtc_state->wm.g4x.raw[G4X_WM_LEVEL_SR].fbc,
+				      crtc_state->wm.g4x.raw[G4X_WM_LEVEL_HPLL].fbc);
+	}
+
+	return dirty;
+}
+
+static bool g4x_raw_plane_wm_is_valid(const struct intel_crtc_state *crtc_state,
+				      enum plane_id plane_id, int level)
+{
+	const struct g4x_pipe_wm *raw = &crtc_state->wm.g4x.raw[level];
+
+	return raw->plane[plane_id] <= g4x_plane_fifo_size(plane_id, level);
+}
+
+static bool g4x_raw_crtc_wm_is_valid(const struct intel_crtc_state *crtc_state,
+				     int level)
+{
+	struct drm_i915_private *dev_priv = to_i915(crtc_state->base.crtc->dev);
+
+	if (level > dev_priv->wm.max_level)
+		return false;
+
+	return g4x_raw_plane_wm_is_valid(crtc_state, PLANE_PRIMARY, level) &&
+		g4x_raw_plane_wm_is_valid(crtc_state, PLANE_SPRITE0, level) &&
+		g4x_raw_plane_wm_is_valid(crtc_state, PLANE_CURSOR, level);
+}
+
+/* mark all levels starting from 'level' as invalid */
+static void g4x_invalidate_wms(struct intel_crtc *crtc,
+			       struct g4x_wm_state *wm_state, int level)
+{
+	if (level <= G4X_WM_LEVEL_NORMAL) {
+		enum plane_id plane_id;
+
+		for_each_plane_id_on_crtc(crtc, plane_id)
+			wm_state->wm.plane[plane_id] = USHRT_MAX;
+	}
+
+	if (level <= G4X_WM_LEVEL_SR) {
+		wm_state->cxsr = false;
+		wm_state->sr.cursor = USHRT_MAX;
+		wm_state->sr.plane = USHRT_MAX;
+		wm_state->sr.fbc = USHRT_MAX;
+	}
+
+	if (level <= G4X_WM_LEVEL_HPLL) {
+		wm_state->hpll_en = false;
+		wm_state->hpll.cursor = USHRT_MAX;
+		wm_state->hpll.plane = USHRT_MAX;
+		wm_state->hpll.fbc = USHRT_MAX;
+	}
+}
+
+static int g4x_compute_pipe_wm(struct intel_crtc_state *crtc_state)
+{
+	struct intel_crtc *crtc = to_intel_crtc(crtc_state->base.crtc);
+	struct intel_atomic_state *state =
+		to_intel_atomic_state(crtc_state->base.state);
+	struct g4x_wm_state *wm_state = &crtc_state->wm.g4x.optimal;
+	int num_active_planes = hweight32(crtc_state->active_planes &
+					  ~BIT(PLANE_CURSOR));
+	const struct g4x_pipe_wm *raw;
+	struct intel_plane_state *plane_state;
+	struct intel_plane *plane;
+	enum plane_id plane_id;
+	int i, level;
+	unsigned int dirty = 0;
+
+	for_each_intel_plane_in_state(state, plane, plane_state, i) {
+		const struct intel_plane_state *old_plane_state =
+			to_intel_plane_state(plane->base.state);
+
+		if (plane_state->base.crtc != &crtc->base &&
+		    old_plane_state->base.crtc != &crtc->base)
+			continue;
+
+		if (g4x_raw_plane_wm_compute(crtc_state, plane_state))
+			dirty |= BIT(plane->id);
+	}
+
+	if (!dirty)
+		return 0;
+
+	level = G4X_WM_LEVEL_NORMAL;
+	if (!g4x_raw_crtc_wm_is_valid(crtc_state, level))
+		goto out;
+
+	raw = &crtc_state->wm.g4x.raw[level];
+	for_each_plane_id_on_crtc(crtc, plane_id)
+		wm_state->wm.plane[plane_id] = raw->plane[plane_id];
+
+	level = G4X_WM_LEVEL_SR;
+
+	if (!g4x_raw_crtc_wm_is_valid(crtc_state, level))
+		goto out;
+
+	raw = &crtc_state->wm.g4x.raw[level];
+	wm_state->sr.plane = raw->plane[PLANE_PRIMARY];
+	wm_state->sr.cursor = raw->plane[PLANE_CURSOR];
+	wm_state->sr.fbc = raw->fbc;
+
+	wm_state->cxsr = num_active_planes == BIT(PLANE_PRIMARY);
+
+	level = G4X_WM_LEVEL_HPLL;
+
+	if (!g4x_raw_crtc_wm_is_valid(crtc_state, level))
+		goto out;
+
+	raw = &crtc_state->wm.g4x.raw[level];
+	wm_state->hpll.plane = raw->plane[PLANE_PRIMARY];
+	wm_state->hpll.cursor = raw->plane[PLANE_CURSOR];
+	wm_state->hpll.fbc = raw->fbc;
+
+	wm_state->hpll_en = wm_state->cxsr;
+
+	level++;
+
+ out:
+	if (level == G4X_WM_LEVEL_NORMAL)
+		return -EINVAL;
+
+	/* invalidate the higher levels */
+	g4x_invalidate_wms(crtc, wm_state, level);
+
+	/*
+	 * Determine if the FBC watermark(s) can be used. IF
+	 * this isn't the case we prefer to disable the FBC
+	 ( watermark(s) rather than disable the SR/HPLL
+	 * level(s) entirely.
+	 */
+	wm_state->fbc_en = level > G4X_WM_LEVEL_NORMAL;
+
+	if (level >= G4X_WM_LEVEL_SR &&
+	    wm_state->sr.fbc > g4x_fbc_fifo_size(G4X_WM_LEVEL_SR))
+		wm_state->fbc_en = false;
+	else if (level >= G4X_WM_LEVEL_HPLL &&
+		 wm_state->hpll.fbc > g4x_fbc_fifo_size(G4X_WM_LEVEL_HPLL))
+		wm_state->fbc_en = false;
+
+	return 0;
+}
+
+static int g4x_compute_intermediate_wm(struct drm_device *dev,
+				       struct intel_crtc *crtc,
+				       struct intel_crtc_state *crtc_state)
+{
+	struct g4x_wm_state *intermediate = &crtc_state->wm.g4x.intermediate;
+	const struct g4x_wm_state *optimal = &crtc_state->wm.g4x.optimal;
+	const struct g4x_wm_state *active = &crtc->wm.active.g4x;
+	enum plane_id plane_id;
+
+	intermediate->cxsr = optimal->cxsr && active->cxsr &&
+		!crtc_state->disable_cxsr;
+	intermediate->hpll_en = optimal->hpll_en && active->hpll_en &&
+		!crtc_state->disable_cxsr;
+	intermediate->fbc_en = optimal->fbc_en && active->fbc_en;
+
+	for_each_plane_id_on_crtc(crtc, plane_id) {
+		intermediate->wm.plane[plane_id] =
+			max(optimal->wm.plane[plane_id],
+			    active->wm.plane[plane_id]);
+
+		WARN_ON(intermediate->wm.plane[plane_id] >
+			g4x_plane_fifo_size(plane_id, G4X_WM_LEVEL_NORMAL));
+	}
+
+	intermediate->sr.plane = max(optimal->sr.plane,
+				     active->sr.plane);
+	intermediate->sr.cursor = max(optimal->sr.cursor,
+				      active->sr.cursor);
+	intermediate->sr.fbc = max(optimal->sr.fbc,
+				   active->sr.fbc);
+
+	intermediate->hpll.plane = max(optimal->hpll.plane,
+				       active->hpll.plane);
+	intermediate->hpll.cursor = max(optimal->hpll.cursor,
+					active->hpll.cursor);
+	intermediate->hpll.fbc = max(optimal->hpll.fbc,
+				     active->hpll.fbc);
+
+	WARN_ON((intermediate->sr.plane >
+		 g4x_plane_fifo_size(PLANE_PRIMARY, G4X_WM_LEVEL_SR) ||
+		 intermediate->sr.cursor >
+		 g4x_plane_fifo_size(PLANE_CURSOR, G4X_WM_LEVEL_SR)) &&
+		intermediate->cxsr);
+	WARN_ON((intermediate->sr.plane >
+		 g4x_plane_fifo_size(PLANE_PRIMARY, G4X_WM_LEVEL_HPLL) ||
+		 intermediate->sr.cursor >
+		 g4x_plane_fifo_size(PLANE_CURSOR, G4X_WM_LEVEL_HPLL)) &&
+		intermediate->hpll_en);
+
+	WARN_ON(intermediate->sr.fbc > g4x_fbc_fifo_size(1) &&
+		intermediate->fbc_en && intermediate->cxsr);
+	WARN_ON(intermediate->hpll.fbc > g4x_fbc_fifo_size(2) &&
+		intermediate->fbc_en && intermediate->hpll_en);
+
+	/*
+	 * If our intermediate WM are identical to the final WM, then we can
+	 * omit the post-vblank programming; only update if it's different.
+	 */
+	if (memcmp(intermediate, optimal, sizeof(*intermediate)) != 0)
+		crtc_state->wm.need_postvbl_update = true;
+
+	return 0;
+}
+
+static void g4x_merge_wm(struct drm_i915_private *dev_priv,
+			 struct g4x_wm_values *wm)
+{
+	struct intel_crtc *crtc;
+	int num_active_crtcs = 0;
+
+	wm->cxsr = true;
+	wm->hpll_en = true;
+	wm->fbc_en = true;
+
+	for_each_intel_crtc(&dev_priv->drm, crtc) {
+		const struct g4x_wm_state *wm_state = &crtc->wm.active.g4x;
+
+		if (!crtc->active)
+			continue;
+
+		if (!wm_state->cxsr)
+			wm->cxsr = false;
+		if (!wm_state->hpll_en)
+			wm->hpll_en = false;
+		if (!wm_state->fbc_en)
+			wm->fbc_en = false;
+
+		num_active_crtcs++;
+	}
+
+	if (num_active_crtcs != 1) {
+		wm->cxsr = false;
+		wm->hpll_en = false;
+		wm->fbc_en = false;
+	}
+
+	for_each_intel_crtc(&dev_priv->drm, crtc) {
+		const struct g4x_wm_state *wm_state = &crtc->wm.active.g4x;
+		enum pipe pipe = crtc->pipe;
+
+		wm->pipe[pipe] = wm_state->wm;
+		if (crtc->active && wm->cxsr)
+			wm->sr = wm_state->sr;
+		if (crtc->active && wm->hpll_en)
+			wm->hpll = wm_state->hpll;
+	}
+}
+
+static void g4x_program_watermarks(struct drm_i915_private *dev_priv)
+{
+	struct g4x_wm_values *old_wm = &dev_priv->wm.g4x;
+	struct g4x_wm_values new_wm = {};
+
+	g4x_merge_wm(dev_priv, &new_wm);
+
+	if (memcmp(old_wm, &new_wm, sizeof(new_wm)) == 0)
+		return;
+
+	if (is_disabling(old_wm->cxsr, new_wm.cxsr, true))
+		_intel_set_memory_cxsr(dev_priv, false);
+
+	g4x_write_wm_values(dev_priv, &new_wm);
+
+	if (is_enabling(old_wm->cxsr, new_wm.cxsr, true))
+		_intel_set_memory_cxsr(dev_priv, true);
+
+	*old_wm = new_wm;
+}
+
+static void g4x_initial_watermarks(struct intel_atomic_state *state,
+				   struct intel_crtc_state *crtc_state)
+{
+	struct drm_i915_private *dev_priv = to_i915(crtc_state->base.crtc->dev);
+	struct intel_crtc *crtc = to_intel_crtc(crtc_state->base.crtc);
+
+	mutex_lock(&dev_priv->wm.wm_mutex);
+	crtc->wm.active.g4x = crtc_state->wm.g4x.intermediate;
+	g4x_program_watermarks(dev_priv);
+	mutex_unlock(&dev_priv->wm.wm_mutex);
+}
+
+static void g4x_optimize_watermarks(struct intel_atomic_state *state,
+				    struct intel_crtc_state *crtc_state)
+{
+	struct drm_i915_private *dev_priv = to_i915(crtc_state->base.crtc->dev);
+	struct intel_crtc *intel_crtc = to_intel_crtc(crtc_state->base.crtc);
+
+	if (!crtc_state->wm.need_postvbl_update)
+		return;
+
+	mutex_lock(&dev_priv->wm.wm_mutex);
+	intel_crtc->wm.active.g4x = crtc_state->wm.g4x.optimal;
+	g4x_program_watermarks(dev_priv);
+	mutex_unlock(&dev_priv->wm.wm_mutex);
+}
+
 /* latency must be in 0.1us units. */
 static unsigned int vlv_wm_method2(unsigned int pixel_rate,
-				   unsigned int pipe_htotal,
-				   unsigned int horiz_pixels,
+				   unsigned int htotal,
+				   unsigned int width,
 				   unsigned int cpp,
 				   unsigned int latency)
 {
 	unsigned int ret;
 
-	ret = (latency * pixel_rate) / (pipe_htotal * 10000);
-	ret = (ret + 1) * horiz_pixels * cpp;
+	ret = intel_wm_method2(pixel_rate, htotal,
+			       width, cpp, latency);
 	ret = DIV_ROUND_UP(ret, 64);
 
 	return ret;
@@ -1029,17 +1582,15 @@ static uint16_t vlv_compute_wm_level(const struct intel_crtc_state *crtc_state,
 	if (dev_priv->wm.pri_latency[level] == 0)
 		return USHRT_MAX;
 
-	if (!plane_state->base.visible)
+	if (!intel_wm_plane_visible(crtc_state, plane_state))
 		return 0;
 
 	cpp = plane_state->base.fb->format->cpp[0];
 	clock = adjusted_mode->crtc_clock;
 	htotal = adjusted_mode->crtc_htotal;
 	width = crtc_state->pipe_src_w;
-	if (WARN_ON(htotal == 0))
-		htotal = 1;
 
-	if (plane->base.type == DRM_PLANE_TYPE_CURSOR) {
+	if (plane->id == PLANE_CURSOR) {
 		/*
 		 * FIXME the formula gives values that are
 		 * too big for the cursor FIFO, and hence we
@@ -1064,7 +1615,7 @@ static bool vlv_need_sprite0_fifo_workaround(unsigned int active_planes)
 static int vlv_compute_fifo(struct intel_crtc_state *crtc_state)
 {
 	struct intel_crtc *crtc = to_intel_crtc(crtc_state->base.crtc);
-	const struct vlv_pipe_wm *raw =
+	const struct g4x_pipe_wm *raw =
 		&crtc_state->wm.vlv.raw[VLV_WM_LEVEL_PM2];
 	struct vlv_fifo_state *fifo_state = &crtc_state->wm.vlv.fifo_state;
 	unsigned int active_planes = crtc_state->active_planes & ~BIT(PLANE_CURSOR);
@@ -1143,18 +1694,13 @@ static int vlv_compute_fifo(struct intel_crtc_state *crtc_state)
 	return 0;
 }
 
-static int vlv_num_wm_levels(struct drm_i915_private *dev_priv)
-{
-	return dev_priv->wm.max_level + 1;
-}
-
 /* mark all levels starting from 'level' as invalid */
 static void vlv_invalidate_wms(struct intel_crtc *crtc,
 			       struct vlv_wm_state *wm_state, int level)
 {
 	struct drm_i915_private *dev_priv = to_i915(crtc->base.dev);
 
-	for (; level < vlv_num_wm_levels(dev_priv); level++) {
+	for (; level < intel_wm_num_levels(dev_priv); level++) {
 		enum plane_id plane_id;
 
 		for_each_plane_id_on_crtc(crtc, plane_id)
@@ -1181,11 +1727,11 @@ static bool vlv_raw_plane_wm_set(struct intel_crtc_state *crtc_state,
 				 int level, enum plane_id plane_id, u16 value)
 {
 	struct drm_i915_private *dev_priv = to_i915(crtc_state->base.crtc->dev);
-	int num_levels = vlv_num_wm_levels(dev_priv);
+	int num_levels = intel_wm_num_levels(dev_priv);
 	bool dirty = false;
 
 	for (; level < num_levels; level++) {
-		struct vlv_pipe_wm *raw = &crtc_state->wm.vlv.raw[level];
+		struct g4x_pipe_wm *raw = &crtc_state->wm.vlv.raw[level];
 
 		dirty |= raw->plane[plane_id] != value;
 		raw->plane[plane_id] = value;
@@ -1194,22 +1740,22 @@ static bool vlv_raw_plane_wm_set(struct intel_crtc_state *crtc_state,
 	return dirty;
 }
 
-static bool vlv_plane_wm_compute(struct intel_crtc_state *crtc_state,
-				 const struct intel_plane_state *plane_state)
+static bool vlv_raw_plane_wm_compute(struct intel_crtc_state *crtc_state,
+				     const struct intel_plane_state *plane_state)
 {
 	struct intel_plane *plane = to_intel_plane(plane_state->base.plane);
 	enum plane_id plane_id = plane->id;
-	int num_levels = vlv_num_wm_levels(to_i915(plane->base.dev));
+	int num_levels = intel_wm_num_levels(to_i915(plane->base.dev));
 	int level;
 	bool dirty = false;
 
-	if (!plane_state->base.visible) {
+	if (!intel_wm_plane_visible(crtc_state, plane_state)) {
 		dirty |= vlv_raw_plane_wm_set(crtc_state, 0, plane_id, 0);
 		goto out;
 	}
 
 	for (level = 0; level < num_levels; level++) {
-		struct vlv_pipe_wm *raw = &crtc_state->wm.vlv.raw[level];
+		struct g4x_pipe_wm *raw = &crtc_state->wm.vlv.raw[level];
 		int wm = vlv_compute_wm_level(crtc_state, plane_state, level);
 		int max_wm = plane_id == PLANE_CURSOR ? 63 : 511;
 
@@ -1225,7 +1771,7 @@ static bool vlv_plane_wm_compute(struct intel_crtc_state *crtc_state,
 
 out:
 	if (dirty)
-		DRM_DEBUG_KMS("%s wms: [0]=%d,[1]=%d,[2]=%d\n",
+		DRM_DEBUG_KMS("%s watermarks: PM2=%d, PM5=%d, DDR DVFS=%d\n",
 			      plane->base.name,
 			      crtc_state->wm.vlv.raw[VLV_WM_LEVEL_PM2].plane[plane_id],
 			      crtc_state->wm.vlv.raw[VLV_WM_LEVEL_PM5].plane[plane_id],
@@ -1234,10 +1780,10 @@ out:
 	return dirty;
 }
 
-static bool vlv_plane_wm_is_valid(const struct intel_crtc_state *crtc_state,
-				  enum plane_id plane_id, int level)
+static bool vlv_raw_plane_wm_is_valid(const struct intel_crtc_state *crtc_state,
+				      enum plane_id plane_id, int level)
 {
-	const struct vlv_pipe_wm *raw =
+	const struct g4x_pipe_wm *raw =
 		&crtc_state->wm.vlv.raw[level];
 	const struct vlv_fifo_state *fifo_state =
 		&crtc_state->wm.vlv.fifo_state;
@@ -1245,12 +1791,12 @@ static bool vlv_plane_wm_is_valid(const struct intel_crtc_state *crtc_state,
 	return raw->plane[plane_id] <= fifo_state->plane[plane_id];
 }
 
-static bool vlv_crtc_wm_is_valid(const struct intel_crtc_state *crtc_state, int level)
+static bool vlv_raw_crtc_wm_is_valid(const struct intel_crtc_state *crtc_state, int level)
 {
-	return vlv_plane_wm_is_valid(crtc_state, PLANE_PRIMARY, level) &&
-		vlv_plane_wm_is_valid(crtc_state, PLANE_SPRITE0, level) &&
-		vlv_plane_wm_is_valid(crtc_state, PLANE_SPRITE1, level) &&
-		vlv_plane_wm_is_valid(crtc_state, PLANE_CURSOR, level);
+	return vlv_raw_plane_wm_is_valid(crtc_state, PLANE_PRIMARY, level) &&
+		vlv_raw_plane_wm_is_valid(crtc_state, PLANE_SPRITE0, level) &&
+		vlv_raw_plane_wm_is_valid(crtc_state, PLANE_SPRITE1, level) &&
+		vlv_raw_plane_wm_is_valid(crtc_state, PLANE_CURSOR, level);
 }
 
 static int vlv_compute_pipe_wm(struct intel_crtc_state *crtc_state)
@@ -1279,7 +1825,7 @@ static int vlv_compute_pipe_wm(struct intel_crtc_state *crtc_state)
 		    old_plane_state->base.crtc != &crtc->base)
 			continue;
 
-		if (vlv_plane_wm_compute(crtc_state, plane_state))
+		if (vlv_raw_plane_wm_compute(crtc_state, plane_state))
 			dirty |= BIT(plane->id);
 	}
 
@@ -1313,7 +1859,7 @@ static int vlv_compute_pipe_wm(struct intel_crtc_state *crtc_state)
 	}
 
 	/* initially allow all levels */
-	wm_state->num_levels = vlv_num_wm_levels(dev_priv);
+	wm_state->num_levels = intel_wm_num_levels(dev_priv);
 	/*
 	 * Note that enabling cxsr with no primary/sprite planes
 	 * enabled can wedge the pipe. Hence we only allow cxsr
@@ -1322,10 +1868,10 @@ static int vlv_compute_pipe_wm(struct intel_crtc_state *crtc_state)
 	wm_state->cxsr = crtc->pipe != PIPE_C && num_active_planes == 1;
 
 	for (level = 0; level < wm_state->num_levels; level++) {
-		const struct vlv_pipe_wm *raw = &crtc_state->wm.vlv.raw[level];
+		const struct g4x_pipe_wm *raw = &crtc_state->wm.vlv.raw[level];
 		const int sr_fifo_size = INTEL_INFO(dev_priv)->num_pipes * 512 - 1;
 
-		if (!vlv_crtc_wm_is_valid(crtc_state, level))
+		if (!vlv_raw_crtc_wm_is_valid(crtc_state, level))
 			break;
 
 		for_each_plane_id_on_crtc(crtc, plane_id) {
@@ -1539,16 +2085,6 @@ static void vlv_merge_wm(struct drm_i915_private *dev_priv,
 	}
 }
 
-static bool is_disabling(int old, int new, int threshold)
-{
-	return old >= threshold && new < threshold;
-}
-
-static bool is_enabling(int old, int new, int threshold)
-{
-	return old < threshold && new >= threshold;
-}
-
 static void vlv_program_watermarks(struct drm_i915_private *dev_priv)
 {
 	struct vlv_wm_values *old_wm = &dev_priv->wm.vlv;
@@ -1609,65 +2145,6 @@ static void vlv_optimize_watermarks(struct intel_atomic_state *state,
 	mutex_unlock(&dev_priv->wm.wm_mutex);
 }
 
-#define single_plane_enabled(mask) is_power_of_2(mask)
-
-static void g4x_update_wm(struct intel_crtc *crtc)
-{
-	struct drm_i915_private *dev_priv = to_i915(crtc->base.dev);
-	static const int sr_latency_ns = 12000;
-	int planea_wm, planeb_wm, cursora_wm, cursorb_wm;
-	int plane_sr, cursor_sr;
-	unsigned int enabled = 0;
-	bool cxsr_enabled;
-
-	if (g4x_compute_wm0(dev_priv, PIPE_A,
-			    &g4x_wm_info, pessimal_latency_ns,
-			    &g4x_cursor_wm_info, pessimal_latency_ns,
-			    &planea_wm, &cursora_wm))
-		enabled |= 1 << PIPE_A;
-
-	if (g4x_compute_wm0(dev_priv, PIPE_B,
-			    &g4x_wm_info, pessimal_latency_ns,
-			    &g4x_cursor_wm_info, pessimal_latency_ns,
-			    &planeb_wm, &cursorb_wm))
-		enabled |= 1 << PIPE_B;
-
-	if (single_plane_enabled(enabled) &&
-	    g4x_compute_srwm(dev_priv, ffs(enabled) - 1,
-			     sr_latency_ns,
-			     &g4x_wm_info,
-			     &g4x_cursor_wm_info,
-			     &plane_sr, &cursor_sr)) {
-		cxsr_enabled = true;
-	} else {
-		cxsr_enabled = false;
-		intel_set_memory_cxsr(dev_priv, false);
-		plane_sr = cursor_sr = 0;
-	}
-
-	DRM_DEBUG_KMS("Setting FIFO watermarks - A: plane=%d, cursor=%d, "
-		      "B: plane=%d, cursor=%d, SR: plane=%d, cursor=%d\n",
-		      planea_wm, cursora_wm,
-		      planeb_wm, cursorb_wm,
-		      plane_sr, cursor_sr);
-
-	I915_WRITE(DSPFW1,
-		   FW_WM(plane_sr, SR) |
-		   FW_WM(cursorb_wm, CURSORB) |
-		   FW_WM(planeb_wm, PLANEB) |
-		   FW_WM(planea_wm, PLANEA));
-	I915_WRITE(DSPFW2,
-		   (I915_READ(DSPFW2) & ~DSPFW_CURSORA_MASK) |
-		   FW_WM(cursora_wm, CURSORA));
-	/* HPLL off in SR has some issues on G4x... disable it */
-	I915_WRITE(DSPFW3,
-		   (I915_READ(DSPFW3) & ~(DSPFW_HPLL_SR_EN | DSPFW_CURSOR_SR_MASK)) |
-		   FW_WM(cursor_sr, CURSOR_SR));
-
-	if (cxsr_enabled)
-		intel_set_memory_cxsr(dev_priv, true);
-}
-
 static void i965_update_wm(struct intel_crtc *unused_crtc)
 {
 	struct drm_i915_private *dev_priv = to_i915(unused_crtc->base.dev);
@@ -1689,14 +2166,10 @@ static void i965_update_wm(struct intel_crtc *unused_crtc)
 		int htotal = adjusted_mode->crtc_htotal;
 		int hdisplay = crtc->config->pipe_src_w;
 		int cpp = fb->format->cpp[0];
-		unsigned long line_time_us;
 		int entries;
 
-		line_time_us = max(htotal * 1000 / clock, 1);
-
-		/* Use ns/us then divide to preserve precision */
-		entries = (((sr_latency_ns / line_time_us) + 1000) / 1000) *
-			cpp * hdisplay;
+		entries = intel_wm_method2(clock, htotal,
+					   hdisplay, cpp, sr_latency_ns / 100);
 		entries = DIV_ROUND_UP(entries, I915_FIFO_LINE_SIZE);
 		srwm = I965_FIFO_SIZE - entries;
 		if (srwm < 0)
@@ -1705,13 +2178,14 @@ static void i965_update_wm(struct intel_crtc *unused_crtc)
 		DRM_DEBUG_KMS("self-refresh entries: %d, wm: %d\n",
 			      entries, srwm);
 
-		entries = (((sr_latency_ns / line_time_us) + 1000) / 1000) *
-			cpp * crtc->base.cursor->state->crtc_w;
+		entries = intel_wm_method2(clock, htotal,
+					   crtc->base.cursor->state->crtc_w, 4,
+					   sr_latency_ns / 100);
 		entries = DIV_ROUND_UP(entries,
-					  i965_cursor_wm_info.cacheline_size);
-		cursor_sr = i965_cursor_wm_info.fifo_size -
-			(entries + i965_cursor_wm_info.guard_size);
+				       i965_cursor_wm_info.cacheline_size) +
+			i965_cursor_wm_info.guard_size;
 
+		cursor_sr = i965_cursor_wm_info.fifo_size - entries;
 		if (cursor_sr > i965_cursor_wm_info.max_wm)
 			cursor_sr = i965_cursor_wm_info.max_wm;
 
@@ -1848,7 +2322,6 @@ static void i9xx_update_wm(struct intel_crtc *unused_crtc)
 		int htotal = adjusted_mode->crtc_htotal;
 		int hdisplay = enabled->config->pipe_src_w;
 		int cpp;
-		unsigned long line_time_us;
 		int entries;
 
 		if (IS_I915GM(dev_priv) || IS_I945GM(dev_priv))
@@ -1856,11 +2329,8 @@ static void i9xx_update_wm(struct intel_crtc *unused_crtc)
 		else
 			cpp = fb->format->cpp[0];
 
-		line_time_us = max(htotal * 1000 / clock, 1);
-
-		/* Use ns/us then divide to preserve precision */
-		entries = (((sr_latency_ns / line_time_us) + 1000) / 1000) *
-			cpp * hdisplay;
+		entries = intel_wm_method2(clock, htotal, hdisplay, cpp,
+					   sr_latency_ns / 100);
 		entries = DIV_ROUND_UP(entries, wm_info->cacheline_size);
 		DRM_DEBUG_KMS("self-refresh entries: %d\n", entries);
 		srwm = wm_info->fifo_size - entries;
@@ -1917,34 +2387,31 @@ static void i845_update_wm(struct intel_crtc *unused_crtc)
 }
 
 /* latency must be in 0.1us units. */
-static uint32_t ilk_wm_method1(uint32_t pixel_rate, uint8_t cpp, uint32_t latency)
+static unsigned int ilk_wm_method1(unsigned int pixel_rate,
+				   unsigned int cpp,
+				   unsigned int latency)
 {
-	uint64_t ret;
-
-	if (WARN(latency == 0, "Latency value missing\n"))
-		return UINT_MAX;
+	unsigned int ret;
 
-	ret = (uint64_t) pixel_rate * cpp * latency;
-	ret = DIV_ROUND_UP_ULL(ret, 64 * 10000) + 2;
+	ret = intel_wm_method1(pixel_rate, cpp, latency);
+	ret = DIV_ROUND_UP(ret, 64) + 2;
 
 	return ret;
 }
 
 /* latency must be in 0.1us units. */
-static uint32_t ilk_wm_method2(uint32_t pixel_rate, uint32_t pipe_htotal,
-			       uint32_t horiz_pixels, uint8_t cpp,
-			       uint32_t latency)
+static unsigned int ilk_wm_method2(unsigned int pixel_rate,
+				   unsigned int htotal,
+				   unsigned int width,
+				   unsigned int cpp,
+				   unsigned int latency)
 {
-	uint32_t ret;
-
-	if (WARN(latency == 0, "Latency value missing\n"))
-		return UINT_MAX;
-	if (WARN_ON(!pipe_htotal))
-		return UINT_MAX;
+	unsigned int ret;
 
-	ret = (latency * pixel_rate) / (pipe_htotal * 10000);
-	ret = (ret + 1) * horiz_pixels * cpp;
+	ret = intel_wm_method2(pixel_rate, htotal,
+			       width, cpp, latency);
 	ret = DIV_ROUND_UP(ret, 64) + 2;
+
 	return ret;
 }
 
@@ -3360,26 +3827,27 @@ void skl_ddb_get_hw_state(struct drm_i915_private *dev_priv,
  * Return value is provided in 16.16 fixed point form to retain fractional part.
  * Caller should take care of dividing & rounding off the value.
  */
-static uint32_t
+static uint_fixed_16_16_t
 skl_plane_downscale_amount(const struct intel_crtc_state *cstate,
 			   const struct intel_plane_state *pstate)
 {
 	struct intel_plane *plane = to_intel_plane(pstate->base.plane);
-	uint32_t downscale_h, downscale_w;
 	uint32_t src_w, src_h, dst_w, dst_h;
+	uint_fixed_16_16_t fp_w_ratio, fp_h_ratio;
+	uint_fixed_16_16_t downscale_h, downscale_w;
 
 	if (WARN_ON(!intel_wm_plane_visible(cstate, pstate)))
-		return DRM_PLANE_HELPER_NO_SCALING;
+		return u32_to_fixed_16_16(0);
 
 	/* n.b., src is 16.16 fixed point, dst is whole integer */
 	if (plane->id == PLANE_CURSOR) {
-		src_w = pstate->base.src_w;
-		src_h = pstate->base.src_h;
+		src_w = pstate->base.src_w >> 16;
+		src_h = pstate->base.src_h >> 16;
 		dst_w = pstate->base.crtc_w;
 		dst_h = pstate->base.crtc_h;
 	} else {
-		src_w = drm_rect_width(&pstate->base.src);
-		src_h = drm_rect_height(&pstate->base.src);
+		src_w = drm_rect_width(&pstate->base.src) >> 16;
+		src_h = drm_rect_height(&pstate->base.src) >> 16;
 		dst_w = drm_rect_width(&pstate->base.dst);
 		dst_h = drm_rect_height(&pstate->base.dst);
 	}
@@ -3387,11 +3855,12 @@ skl_plane_downscale_amount(const struct intel_crtc_state *cstate,
 	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);
+	fp_w_ratio = fixed_16_16_div(src_w, dst_w);
+	fp_h_ratio = fixed_16_16_div(src_h, dst_h);
+	downscale_w = max_fixed_16_16(fp_w_ratio, u32_to_fixed_16_16(1));
+	downscale_h = max_fixed_16_16(fp_h_ratio, u32_to_fixed_16_16(1));
 
-	/* Provide result in 16.16 fixed point */
-	return (uint64_t)downscale_w * downscale_h >> 16;
+	return mul_fixed16(downscale_w, downscale_h);
 }
 
 static unsigned int
@@ -3401,10 +3870,11 @@ skl_plane_relative_data_rate(const struct intel_crtc_state *cstate,
 {
 	struct intel_plane *plane = to_intel_plane(pstate->plane);
 	struct intel_plane_state *intel_pstate = to_intel_plane_state(pstate);
-	uint32_t down_scale_amount, data_rate;
+	uint32_t data_rate;
 	uint32_t width = 0, height = 0;
 	struct drm_framebuffer *fb;
 	u32 format;
+	uint_fixed_16_16_t down_scale_amount;
 
 	if (!intel_pstate->base.visible)
 		return 0;
@@ -3438,7 +3908,7 @@ skl_plane_relative_data_rate(const struct intel_crtc_state *cstate,
 
 	down_scale_amount = skl_plane_downscale_amount(cstate, intel_pstate);
 
-	return (uint64_t)data_rate * down_scale_amount >> 16;
+	return mul_round_up_u32_fixed16(data_rate, down_scale_amount);
 }
 
 /*
@@ -3587,6 +4057,7 @@ skl_allocate_pipe_ddb(struct intel_crtc_state *cstate,
 	int num_active;
 	unsigned plane_data_rate[I915_MAX_PLANES] = {};
 	unsigned plane_y_data_rate[I915_MAX_PLANES] = {};
+	uint16_t total_min_blocks = 0;
 
 	/* Clear the partitioning for disabled planes. */
 	memset(ddb->plane[pipe], 0, sizeof(ddb->plane[pipe]));
@@ -3602,10 +4073,8 @@ skl_allocate_pipe_ddb(struct intel_crtc_state *cstate,
 
 	skl_ddb_get_pipe_allocation_limits(dev, cstate, alloc, &num_active);
 	alloc_size = skl_ddb_entry_size(alloc);
-	if (alloc_size == 0) {
-		memset(ddb->plane[pipe], 0, sizeof(ddb->plane[pipe]));
+	if (alloc_size == 0)
 		return 0;
-	}
 
 	skl_ddb_calc_min(cstate, num_active, minimum, y_minimum);
 
@@ -3616,10 +4085,18 @@ skl_allocate_pipe_ddb(struct intel_crtc_state *cstate,
 	 */
 
 	for_each_plane_id_on_crtc(intel_crtc, plane_id) {
-		alloc_size -= minimum[plane_id];
-		alloc_size -= y_minimum[plane_id];
+		total_min_blocks += minimum[plane_id];
+		total_min_blocks += y_minimum[plane_id];
 	}
 
+	if (total_min_blocks > alloc_size) {
+		DRM_DEBUG_KMS("Requested display configuration exceeds system DDB limitations");
+		DRM_DEBUG_KMS("minimum required %d/%d\n", total_min_blocks,
+							alloc_size);
+		return -EINVAL;
+	}
+
+	alloc_size -= total_min_blocks;
 	ddb->plane[pipe][PLANE_CURSOR].start = alloc->end - minimum[PLANE_CURSOR];
 	ddb->plane[pipe][PLANE_CURSOR].end = alloc->end;
 
@@ -3698,7 +4175,7 @@ static uint_fixed_16_16_t skl_wm_method1(uint32_t pixel_rate, uint8_t cpp,
 		return FP_16_16_MAX;
 
 	wm_intermediate_val = latency * pixel_rate * cpp;
-	ret = fixed_16_16_div_round_up_u64(wm_intermediate_val, 1000 * 512);
+	ret = fixed_16_16_div_u64(wm_intermediate_val, 1000 * 512);
 	return ret;
 }
 
@@ -3720,12 +4197,33 @@ static uint_fixed_16_16_t skl_wm_method2(uint32_t pixel_rate,
 	return ret;
 }
 
-static uint32_t skl_adjusted_plane_pixel_rate(const struct intel_crtc_state *cstate,
-					      struct intel_plane_state *pstate)
+static uint_fixed_16_16_t
+intel_get_linetime_us(struct intel_crtc_state *cstate)
+{
+	uint32_t pixel_rate;
+	uint32_t crtc_htotal;
+	uint_fixed_16_16_t linetime_us;
+
+	if (!cstate->base.active)
+		return u32_to_fixed_16_16(0);
+
+	pixel_rate = cstate->pixel_rate;
+
+	if (WARN_ON(pixel_rate == 0))
+		return u32_to_fixed_16_16(0);
+
+	crtc_htotal = cstate->base.adjusted_mode.crtc_htotal;
+	linetime_us = fixed_16_16_div_u64(crtc_htotal * 1000, pixel_rate);
+
+	return linetime_us;
+}
+
+static uint32_t
+skl_adjusted_plane_pixel_rate(const struct intel_crtc_state *cstate,
+			      const struct intel_plane_state *pstate)
 {
 	uint64_t adjusted_pixel_rate;
-	uint64_t downscale_amount;
-	uint64_t pixel_rate;
+	uint_fixed_16_16_t downscale_amount;
 
 	/* Shouldn't reach here on disabled planes... */
 	if (WARN_ON(!intel_wm_plane_visible(cstate, pstate)))
@@ -3738,15 +4236,13 @@ static uint32_t skl_adjusted_plane_pixel_rate(const struct intel_crtc_state *cst
 	adjusted_pixel_rate = cstate->pixel_rate;
 	downscale_amount = skl_plane_downscale_amount(cstate, pstate);
 
-	pixel_rate = adjusted_pixel_rate * downscale_amount >> 16;
-	WARN_ON(pixel_rate != clamp_t(uint32_t, pixel_rate, 0, ~0));
-
-	return pixel_rate;
+	return mul_round_up_u32_fixed16(adjusted_pixel_rate,
+					    downscale_amount);
 }
 
 static int skl_compute_plane_wm(const struct drm_i915_private *dev_priv,
 				struct intel_crtc_state *cstate,
-				struct intel_plane_state *intel_pstate,
+				const struct intel_plane_state *intel_pstate,
 				uint16_t ddb_allocation,
 				int level,
 				uint16_t *out_blocks, /* out */
@@ -3754,8 +4250,8 @@ static int skl_compute_plane_wm(const struct drm_i915_private *dev_priv,
 				bool *enabled /* out */)
 {
 	struct intel_plane *plane = to_intel_plane(intel_pstate->base.plane);
-	struct drm_plane_state *pstate = &intel_pstate->base;
-	struct drm_framebuffer *fb = pstate->fb;
+	const struct drm_plane_state *pstate = &intel_pstate->base;
+	const struct drm_framebuffer *fb = pstate->fb;
 	uint32_t latency = dev_priv->wm.skl_latency[level];
 	uint_fixed_16_16_t method1, method2;
 	uint_fixed_16_16_t plane_blocks_per_line;
@@ -3834,8 +4330,8 @@ static int skl_compute_plane_wm(const struct drm_i915_private *dev_priv,
 	if (y_tiled) {
 		interm_pbpl = DIV_ROUND_UP(plane_bytes_per_line *
 					   y_min_scanlines, 512);
-		plane_blocks_per_line =
-		      fixed_16_16_div_round_up(interm_pbpl, y_min_scanlines);
+		plane_blocks_per_line = fixed_16_16_div(interm_pbpl,
+							y_min_scanlines);
 	} else if (x_tiled) {
 		interm_pbpl = DIV_ROUND_UP(plane_bytes_per_line, 512);
 		plane_blocks_per_line = u32_to_fixed_16_16(interm_pbpl);
@@ -3856,19 +4352,25 @@ static int skl_compute_plane_wm(const struct drm_i915_private *dev_priv,
 	if (y_tiled) {
 		selected_result = max_fixed_16_16(method2, y_tile_minimum);
 	} else {
+		uint32_t linetime_us;
+
+		linetime_us = fixed_16_16_to_u32_round_up(
+				intel_get_linetime_us(cstate));
 		if ((cpp * cstate->base.adjusted_mode.crtc_htotal / 512 < 1) &&
 		    (plane_bytes_per_line / 512 < 1))
 			selected_result = method2;
-		else if ((ddb_allocation /
+		else if ((ddb_allocation && ddb_allocation /
 			fixed_16_16_to_u32_round_up(plane_blocks_per_line)) >= 1)
 			selected_result = min_fixed_16_16(method1, method2);
+		else if (latency >= linetime_us)
+			selected_result = min_fixed_16_16(method1, method2);
 		else
 			selected_result = method1;
 	}
 
 	res_blocks = fixed_16_16_to_u32_round_up(selected_result) + 1;
-	res_lines = DIV_ROUND_UP(selected_result.val,
-				 plane_blocks_per_line.val);
+	res_lines = div_round_up_fixed16(selected_result,
+					 plane_blocks_per_line);
 
 	if (level >= 1 && level <= 7) {
 		if (y_tiled) {
@@ -3907,54 +4409,39 @@ static int skl_compute_plane_wm(const struct drm_i915_private *dev_priv,
 }
 
 static int
-skl_compute_wm_level(const struct drm_i915_private *dev_priv,
-		     struct skl_ddb_allocation *ddb,
-		     struct intel_crtc_state *cstate,
-		     struct intel_plane *intel_plane,
-		     int level,
-		     struct skl_wm_level *result)
+skl_compute_wm_levels(const struct drm_i915_private *dev_priv,
+		      struct skl_ddb_allocation *ddb,
+		      struct intel_crtc_state *cstate,
+		      const struct intel_plane_state *intel_pstate,
+		      struct skl_plane_wm *wm)
 {
-	struct drm_atomic_state *state = cstate->base.state;
 	struct intel_crtc *intel_crtc = to_intel_crtc(cstate->base.crtc);
-	struct drm_plane *plane = &intel_plane->base;
-	struct intel_plane_state *intel_pstate = NULL;
+	struct drm_plane *plane = intel_pstate->base.plane;
+	struct intel_plane *intel_plane = to_intel_plane(plane);
 	uint16_t ddb_blocks;
 	enum pipe pipe = intel_crtc->pipe;
+	int level, max_level = ilk_wm_max_level(dev_priv);
 	int ret;
 
-	if (state)
-		intel_pstate =
-			intel_atomic_get_existing_plane_state(state,
-							      intel_plane);
-
-	/*
-	 * Note: If we start supporting multiple pending atomic commits against
-	 * the same planes/CRTC's in the future, plane->state will no longer be
-	 * the correct pre-state to use for the calculations here and we'll
-	 * need to change where we get the 'unchanged' plane data from.
-	 *
-	 * For now this is fine because we only allow one queued commit against
-	 * a CRTC.  Even if the plane isn't modified by this transaction and we
-	 * don't have a plane lock, we still have the CRTC's lock, so we know
-	 * that no other transactions are racing with us to update it.
-	 */
-	if (!intel_pstate)
-		intel_pstate = to_intel_plane_state(plane->state);
-
-	WARN_ON(!intel_pstate->base.fb);
+	if (WARN_ON(!intel_pstate->base.fb))
+		return -EINVAL;
 
 	ddb_blocks = skl_ddb_entry_size(&ddb->plane[pipe][intel_plane->id]);
 
-	ret = skl_compute_plane_wm(dev_priv,
-				   cstate,
-				   intel_pstate,
-				   ddb_blocks,
-				   level,
-				   &result->plane_res_b,
-				   &result->plane_res_l,
-				   &result->plane_en);
-	if (ret)
-		return ret;
+	for (level = 0; level <= max_level; level++) {
+		struct skl_wm_level *result = &wm->wm[level];
+
+		ret = skl_compute_plane_wm(dev_priv,
+					   cstate,
+					   intel_pstate,
+					   ddb_blocks,
+					   level,
+					   &result->plane_res_b,
+					   &result->plane_res_l,
+					   &result->plane_en);
+		if (ret)
+			return ret;
+	}
 
 	return 0;
 }
@@ -3964,19 +4451,16 @@ skl_compute_linetime_wm(struct intel_crtc_state *cstate)
 {
 	struct drm_atomic_state *state = cstate->base.state;
 	struct drm_i915_private *dev_priv = to_i915(state->dev);
-	uint32_t pixel_rate;
+	uint_fixed_16_16_t linetime_us;
 	uint32_t linetime_wm;
 
-	if (!cstate->base.active)
-		return 0;
+	linetime_us = intel_get_linetime_us(cstate);
 
-	pixel_rate = cstate->pixel_rate;
-
-	if (WARN_ON(pixel_rate == 0))
+	if (is_fixed16_zero(linetime_us))
 		return 0;
 
-	linetime_wm = DIV_ROUND_UP(8 * cstate->base.adjusted_mode.crtc_htotal *
-				   1000, pixel_rate);
+	linetime_wm = fixed_16_16_to_u32_round_up(mul_u32_fixed_16_16(8,
+				linetime_us));
 
 	/* Display WA #1135: bxt. */
 	if (IS_BROXTON(dev_priv) && dev_priv->ipc_enabled)
@@ -4000,10 +4484,11 @@ static int skl_build_pipe_wm(struct intel_crtc_state *cstate,
 			     struct skl_pipe_wm *pipe_wm)
 {
 	struct drm_device *dev = cstate->base.crtc->dev;
+	struct drm_crtc_state *crtc_state = &cstate->base;
 	const struct drm_i915_private *dev_priv = to_i915(dev);
-	struct intel_plane *intel_plane;
+	struct drm_plane *plane;
+	const struct drm_plane_state *pstate;
 	struct skl_plane_wm *wm;
-	int level, max_level = ilk_wm_max_level(dev_priv);
 	int ret;
 
 	/*
@@ -4012,18 +4497,17 @@ static int skl_build_pipe_wm(struct intel_crtc_state *cstate,
 	 */
 	memset(pipe_wm->planes, 0, sizeof(pipe_wm->planes));
 
-	for_each_intel_plane_mask(&dev_priv->drm,
-				  intel_plane,
-				  cstate->base.plane_mask) {
-		wm = &pipe_wm->planes[intel_plane->id];
+	drm_atomic_crtc_state_for_each_plane_state(plane, pstate, crtc_state) {
+		const struct intel_plane_state *intel_pstate =
+						to_intel_plane_state(pstate);
+		enum plane_id plane_id = to_intel_plane(plane)->id;
 
-		for (level = 0; level <= max_level; level++) {
-			ret = skl_compute_wm_level(dev_priv, ddb, cstate,
-						   intel_plane, level,
-						   &wm->wm[level]);
-			if (ret)
-				return ret;
-		}
+		wm = &pipe_wm->planes[plane_id];
+
+		ret = skl_compute_wm_levels(dev_priv, ddb, cstate,
+					    intel_pstate, wm);
+		if (ret)
+			return ret;
 		skl_compute_transition_wm(cstate, &wm->trans_wm);
 	}
 	pipe_wm->linetime = skl_compute_linetime_wm(cstate);
@@ -4654,6 +5138,32 @@ static void ilk_pipe_wm_get_hw_state(struct drm_crtc *crtc)
 #define _FW_WM_VLV(value, plane) \
 	(((value) & DSPFW_ ## plane ## _MASK_VLV) >> DSPFW_ ## plane ## _SHIFT)
 
+static void g4x_read_wm_values(struct drm_i915_private *dev_priv,
+			       struct g4x_wm_values *wm)
+{
+	uint32_t tmp;
+
+	tmp = I915_READ(DSPFW1);
+	wm->sr.plane = _FW_WM(tmp, SR);
+	wm->pipe[PIPE_B].plane[PLANE_CURSOR] = _FW_WM(tmp, CURSORB);
+	wm->pipe[PIPE_B].plane[PLANE_PRIMARY] = _FW_WM(tmp, PLANEB);
+	wm->pipe[PIPE_A].plane[PLANE_PRIMARY] = _FW_WM(tmp, PLANEA);
+
+	tmp = I915_READ(DSPFW2);
+	wm->fbc_en = tmp & DSPFW_FBC_SR_EN;
+	wm->sr.fbc = _FW_WM(tmp, FBC_SR);
+	wm->hpll.fbc = _FW_WM(tmp, FBC_HPLL_SR);
+	wm->pipe[PIPE_B].plane[PLANE_SPRITE0] = _FW_WM(tmp, SPRITEB);
+	wm->pipe[PIPE_A].plane[PLANE_CURSOR] = _FW_WM(tmp, CURSORA);
+	wm->pipe[PIPE_A].plane[PLANE_SPRITE0] = _FW_WM(tmp, SPRITEA);
+
+	tmp = I915_READ(DSPFW3);
+	wm->hpll_en = tmp & DSPFW_HPLL_SR_EN;
+	wm->sr.cursor = _FW_WM(tmp, CURSOR_SR);
+	wm->hpll.cursor = _FW_WM(tmp, HPLL_CURSOR);
+	wm->hpll.plane = _FW_WM(tmp, HPLL_SR);
+}
+
 static void vlv_read_wm_values(struct drm_i915_private *dev_priv,
 			       struct vlv_wm_values *wm)
 {
@@ -4730,6 +5240,147 @@ static void vlv_read_wm_values(struct drm_i915_private *dev_priv,
 #undef _FW_WM
 #undef _FW_WM_VLV
 
+void g4x_wm_get_hw_state(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = to_i915(dev);
+	struct g4x_wm_values *wm = &dev_priv->wm.g4x;
+	struct intel_crtc *crtc;
+
+	g4x_read_wm_values(dev_priv, wm);
+
+	wm->cxsr = I915_READ(FW_BLC_SELF) & FW_BLC_SELF_EN;
+
+	for_each_intel_crtc(dev, crtc) {
+		struct intel_crtc_state *crtc_state =
+			to_intel_crtc_state(crtc->base.state);
+		struct g4x_wm_state *active = &crtc->wm.active.g4x;
+		struct g4x_pipe_wm *raw;
+		enum pipe pipe = crtc->pipe;
+		enum plane_id plane_id;
+		int level, max_level;
+
+		active->cxsr = wm->cxsr;
+		active->hpll_en = wm->hpll_en;
+		active->fbc_en = wm->fbc_en;
+
+		active->sr = wm->sr;
+		active->hpll = wm->hpll;
+
+		for_each_plane_id_on_crtc(crtc, plane_id) {
+			active->wm.plane[plane_id] =
+				wm->pipe[pipe].plane[plane_id];
+		}
+
+		if (wm->cxsr && wm->hpll_en)
+			max_level = G4X_WM_LEVEL_HPLL;
+		else if (wm->cxsr)
+			max_level = G4X_WM_LEVEL_SR;
+		else
+			max_level = G4X_WM_LEVEL_NORMAL;
+
+		level = G4X_WM_LEVEL_NORMAL;
+		raw = &crtc_state->wm.g4x.raw[level];
+		for_each_plane_id_on_crtc(crtc, plane_id)
+			raw->plane[plane_id] = active->wm.plane[plane_id];
+
+		if (++level > max_level)
+			goto out;
+
+		raw = &crtc_state->wm.g4x.raw[level];
+		raw->plane[PLANE_PRIMARY] = active->sr.plane;
+		raw->plane[PLANE_CURSOR] = active->sr.cursor;
+		raw->plane[PLANE_SPRITE0] = 0;
+		raw->fbc = active->sr.fbc;
+
+		if (++level > max_level)
+			goto out;
+
+		raw = &crtc_state->wm.g4x.raw[level];
+		raw->plane[PLANE_PRIMARY] = active->hpll.plane;
+		raw->plane[PLANE_CURSOR] = active->hpll.cursor;
+		raw->plane[PLANE_SPRITE0] = 0;
+		raw->fbc = active->hpll.fbc;
+
+	out:
+		for_each_plane_id_on_crtc(crtc, plane_id)
+			g4x_raw_plane_wm_set(crtc_state, level,
+					     plane_id, USHRT_MAX);
+		g4x_raw_fbc_wm_set(crtc_state, level, USHRT_MAX);
+
+		crtc_state->wm.g4x.optimal = *active;
+		crtc_state->wm.g4x.intermediate = *active;
+
+		DRM_DEBUG_KMS("Initial watermarks: pipe %c, plane=%d, cursor=%d, sprite=%d\n",
+			      pipe_name(pipe),
+			      wm->pipe[pipe].plane[PLANE_PRIMARY],
+			      wm->pipe[pipe].plane[PLANE_CURSOR],
+			      wm->pipe[pipe].plane[PLANE_SPRITE0]);
+	}
+
+	DRM_DEBUG_KMS("Initial SR watermarks: plane=%d, cursor=%d fbc=%d\n",
+		      wm->sr.plane, wm->sr.cursor, wm->sr.fbc);
+	DRM_DEBUG_KMS("Initial HPLL watermarks: plane=%d, SR cursor=%d fbc=%d\n",
+		      wm->hpll.plane, wm->hpll.cursor, wm->hpll.fbc);
+	DRM_DEBUG_KMS("Initial SR=%s HPLL=%s FBC=%s\n",
+		      yesno(wm->cxsr), yesno(wm->hpll_en), yesno(wm->fbc_en));
+}
+
+void g4x_wm_sanitize(struct drm_i915_private *dev_priv)
+{
+	struct intel_plane *plane;
+	struct intel_crtc *crtc;
+
+	mutex_lock(&dev_priv->wm.wm_mutex);
+
+	for_each_intel_plane(&dev_priv->drm, plane) {
+		struct intel_crtc *crtc =
+			intel_get_crtc_for_pipe(dev_priv, plane->pipe);
+		struct intel_crtc_state *crtc_state =
+			to_intel_crtc_state(crtc->base.state);
+		struct intel_plane_state *plane_state =
+			to_intel_plane_state(plane->base.state);
+		struct g4x_wm_state *wm_state = &crtc_state->wm.g4x.optimal;
+		enum plane_id plane_id = plane->id;
+		int level;
+
+		if (plane_state->base.visible)
+			continue;
+
+		for (level = 0; level < 3; level++) {
+			struct g4x_pipe_wm *raw =
+				&crtc_state->wm.g4x.raw[level];
+
+			raw->plane[plane_id] = 0;
+			wm_state->wm.plane[plane_id] = 0;
+		}
+
+		if (plane_id == PLANE_PRIMARY) {
+			for (level = 0; level < 3; level++) {
+				struct g4x_pipe_wm *raw =
+					&crtc_state->wm.g4x.raw[level];
+				raw->fbc = 0;
+			}
+
+			wm_state->sr.fbc = 0;
+			wm_state->hpll.fbc = 0;
+			wm_state->fbc_en = false;
+		}
+	}
+
+	for_each_intel_crtc(&dev_priv->drm, crtc) {
+		struct intel_crtc_state *crtc_state =
+			to_intel_crtc_state(crtc->base.state);
+
+		crtc_state->wm.g4x.intermediate =
+			crtc_state->wm.g4x.optimal;
+		crtc->wm.active.g4x = crtc_state->wm.g4x.optimal;
+	}
+
+	g4x_program_watermarks(dev_priv);
+
+	mutex_unlock(&dev_priv->wm.wm_mutex);
+}
+
 void vlv_wm_get_hw_state(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = to_i915(dev);
@@ -4792,7 +5443,7 @@ void vlv_wm_get_hw_state(struct drm_device *dev)
 		active->cxsr = wm->cxsr;
 
 		for (level = 0; level < active->num_levels; level++) {
-			struct vlv_pipe_wm *raw =
+			struct g4x_pipe_wm *raw =
 				&crtc_state->wm.vlv.raw[level];
 
 			active->sr[level].plane = wm->sr.plane;
@@ -4852,7 +5503,7 @@ void vlv_wm_sanitize(struct drm_i915_private *dev_priv)
 			continue;
 
 		for (level = 0; level < wm_state->num_levels; level++) {
-			struct vlv_pipe_wm *raw =
+			struct g4x_pipe_wm *raw =
 				&crtc_state->wm.vlv.raw[level];
 
 			raw->plane[plane_id] = 0;
@@ -8036,6 +8687,12 @@ void intel_init_pm(struct drm_i915_private *dev_priv)
 		dev_priv->display.initial_watermarks = vlv_initial_watermarks;
 		dev_priv->display.optimize_watermarks = vlv_optimize_watermarks;
 		dev_priv->display.atomic_update_watermarks = vlv_atomic_update_fifo;
+	} else if (IS_G4X(dev_priv)) {
+		g4x_setup_wm_latency(dev_priv);
+		dev_priv->display.compute_pipe_wm = g4x_compute_pipe_wm;
+		dev_priv->display.compute_intermediate_wm = g4x_compute_intermediate_wm;
+		dev_priv->display.initial_watermarks = g4x_initial_watermarks;
+		dev_priv->display.optimize_watermarks = g4x_optimize_watermarks;
 	} else if (IS_PINEVIEW(dev_priv)) {
 		if (!intel_get_cxsr_latency(IS_PINEVIEW_G(dev_priv),
 					    dev_priv->is_ddr3,
@@ -8051,8 +8708,6 @@ void intel_init_pm(struct drm_i915_private *dev_priv)
 			dev_priv->display.update_wm = NULL;
 		} else
 			dev_priv->display.update_wm = pineview_update_wm;
-	} else if (IS_G4X(dev_priv)) {
-		dev_priv->display.update_wm = g4x_update_wm;
 	} else if (IS_GEN4(dev_priv)) {
 		dev_priv->display.update_wm = i965_update_wm;
 	} else if (IS_GEN3(dev_priv)) {
@@ -8135,9 +8790,9 @@ int sandybridge_pcode_read(struct drm_i915_private *dev_priv, u32 mbox, u32 *val
 	I915_WRITE_FW(GEN6_PCODE_DATA1, 0);
 	I915_WRITE_FW(GEN6_PCODE_MAILBOX, GEN6_PCODE_READY | mbox);
 
-	if (intel_wait_for_register_fw(dev_priv,
-				       GEN6_PCODE_MAILBOX, GEN6_PCODE_READY, 0,
-				       500)) {
+	if (__intel_wait_for_register_fw(dev_priv,
+					 GEN6_PCODE_MAILBOX, GEN6_PCODE_READY, 0,
+					 500, 0, NULL)) {
 		DRM_ERROR("timeout waiting for pcode read (%d) to finish\n", mbox);
 		return -ETIMEDOUT;
 	}
@@ -8180,9 +8835,9 @@ int sandybridge_pcode_write(struct drm_i915_private *dev_priv,
 	I915_WRITE_FW(GEN6_PCODE_DATA1, 0);
 	I915_WRITE_FW(GEN6_PCODE_MAILBOX, GEN6_PCODE_READY | mbox);
 
-	if (intel_wait_for_register_fw(dev_priv,
-				       GEN6_PCODE_MAILBOX, GEN6_PCODE_READY, 0,
-				       500)) {
+	if (__intel_wait_for_register_fw(dev_priv,
+					 GEN6_PCODE_MAILBOX, GEN6_PCODE_READY, 0,
+					 500, 0, NULL)) {
 		DRM_ERROR("timeout waiting for pcode write (%d) to finish\n", mbox);
 		return -ETIMEDOUT;
 	}
diff --git a/drivers/gpu/drm/i915/intel_psr.c b/drivers/gpu/drm/i915/intel_psr.c
index c3780d0d2baf..559f1ab42bfc 100644
--- a/drivers/gpu/drm/i915/intel_psr.c
+++ b/drivers/gpu/drm/i915/intel_psr.c
@@ -435,8 +435,9 @@ static bool intel_psr_match_conditions(struct intel_dp *intel_dp)
 	}
 
 	/* PSR2 is restricted to work with panel resolutions upto 3200x2000 */
-	if (intel_crtc->config->pipe_src_w > 3200 ||
-				intel_crtc->config->pipe_src_h > 2000) {
+	if (dev_priv->psr.psr2_support &&
+	    (intel_crtc->config->pipe_src_w > 3200 ||
+	     intel_crtc->config->pipe_src_h > 2000)) {
 		dev_priv->psr.psr2_support = false;
 		return false;
 	}
diff --git a/drivers/gpu/drm/i915/intel_ringbuffer.c b/drivers/gpu/drm/i915/intel_ringbuffer.c
index 66a2b8b83972..acd1da9b62a3 100644
--- a/drivers/gpu/drm/i915/intel_ringbuffer.c
+++ b/drivers/gpu/drm/i915/intel_ringbuffer.c
@@ -39,17 +39,27 @@
  */
 #define LEGACY_REQUEST_SIZE 200
 
-static int __intel_ring_space(int head, int tail, int size)
+static unsigned int __intel_ring_space(unsigned int head,
+				       unsigned int tail,
+				       unsigned int size)
 {
-	int space = head - tail;
-	if (space <= 0)
-		space += size;
-	return space - I915_RING_FREE_SPACE;
+	/*
+	 * "If the Ring Buffer Head Pointer and the Tail Pointer are on the
+	 * same cacheline, the Head Pointer must not be greater than the Tail
+	 * Pointer."
+	 */
+	GEM_BUG_ON(!is_power_of_2(size));
+	return (head - tail - CACHELINE_BYTES) & (size - 1);
 }
 
-void intel_ring_update_space(struct intel_ring *ring)
+unsigned int intel_ring_update_space(struct intel_ring *ring)
 {
-	ring->space = __intel_ring_space(ring->head, ring->tail, ring->size);
+	unsigned int space;
+
+	space = __intel_ring_space(ring->head, ring->emit, ring->size);
+
+	ring->space = space;
+	return space;
 }
 
 static int
@@ -538,9 +548,9 @@ static int init_ring_common(struct intel_engine_cs *engine)
 	I915_WRITE_CTL(engine, RING_CTL_SIZE(ring->size) | RING_VALID);
 
 	/* If the head is still not zero, the ring is dead */
-	if (intel_wait_for_register_fw(dev_priv, RING_CTL(engine->mmio_base),
-				       RING_VALID, RING_VALID,
-				       50)) {
+	if (intel_wait_for_register(dev_priv, RING_CTL(engine->mmio_base),
+				    RING_VALID, RING_VALID,
+				    50)) {
 		DRM_ERROR("%s initialization failed "
 			  "ctl %08x (valid? %d) head %08x [%08x] tail %08x [%08x] start %08x [expected %08x]\n",
 			  engine->name,
@@ -774,8 +784,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)
@@ -1259,6 +1269,8 @@ static int init_phys_status_page(struct intel_engine_cs *engine)
 {
 	struct drm_i915_private *dev_priv = engine->i915;
 
+	GEM_BUG_ON(engine->id != RCS);
+
 	dev_priv->status_page_dmah =
 		drm_pci_alloc(&dev_priv->drm, PAGE_SIZE, PAGE_SIZE);
 	if (!dev_priv->status_page_dmah)
@@ -1270,17 +1282,18 @@ static int init_phys_status_page(struct intel_engine_cs *engine)
 	return 0;
 }
 
-int intel_ring_pin(struct intel_ring *ring, unsigned int offset_bias)
+int intel_ring_pin(struct intel_ring *ring,
+		   struct drm_i915_private *i915,
+		   unsigned int offset_bias)
 {
-	unsigned int flags;
-	enum i915_map_type map;
+	enum i915_map_type map = HAS_LLC(i915) ? I915_MAP_WB : I915_MAP_WC;
 	struct i915_vma *vma = ring->vma;
+	unsigned int flags;
 	void *addr;
 	int ret;
 
 	GEM_BUG_ON(ring->vaddr);
 
-	map = HAS_LLC(ring->engine->i915) ? I915_MAP_WB : I915_MAP_WC;
 
 	flags = PIN_GLOBAL;
 	if (offset_bias)
@@ -1316,11 +1329,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
@@ -1338,7 +1363,7 @@ intel_ring_create_vma(struct drm_i915_private *dev_priv, int size)
 
 	obj = i915_gem_object_create_stolen(dev_priv, size);
 	if (!obj)
-		obj = i915_gem_object_create(dev_priv, size);
+		obj = i915_gem_object_create_internal(dev_priv, size);
 	if (IS_ERR(obj))
 		return ERR_CAST(obj);
 
@@ -1369,8 +1394,6 @@ intel_engine_create_ring(struct intel_engine_cs *engine, int size)
 	if (!ring)
 		return ERR_PTR(-ENOMEM);
 
-	ring->engine = engine;
-
 	INIT_LIST_HEAD(&ring->request_list);
 
 	ring->size = size;
@@ -1424,22 +1447,73 @@ static int context_pin(struct i915_gem_context *ctx)
 			    PIN_GLOBAL | PIN_HIGH);
 }
 
-static int intel_ring_context_pin(struct intel_engine_cs *engine,
-				  struct i915_gem_context *ctx)
+static struct i915_vma *
+alloc_context_vma(struct intel_engine_cs *engine)
+{
+	struct drm_i915_private *i915 = engine->i915;
+	struct drm_i915_gem_object *obj;
+	struct i915_vma *vma;
+
+	obj = i915_gem_object_create(i915, engine->context_size);
+	if (IS_ERR(obj))
+		return ERR_CAST(obj);
+
+	/*
+	 * Try to make the context utilize L3 as well as LLC.
+	 *
+	 * On VLV we don't have L3 controls in the PTEs so we
+	 * shouldn't touch the cache level, especially as that
+	 * would make the object snooped which might have a
+	 * negative performance impact.
+	 *
+	 * Snooping is required on non-llc platforms in execlist
+	 * mode, but since all GGTT accesses use PAT entry 0 we
+	 * get snooping anyway regardless of cache_level.
+	 *
+	 * This is only applicable for Ivy Bridge devices since
+	 * later platforms don't have L3 control bits in the PTE.
+	 */
+	if (IS_IVYBRIDGE(i915)) {
+		/* Ignore any error, regard it as a simple optimisation */
+		i915_gem_object_set_cache_level(obj, I915_CACHE_L3_LLC);
+	}
+
+	vma = i915_vma_instance(obj, &i915->ggtt.base, NULL);
+	if (IS_ERR(vma))
+		i915_gem_object_put(obj);
+
+	return vma;
+}
+
+static struct intel_ring *
+intel_ring_context_pin(struct intel_engine_cs *engine,
+		       struct i915_gem_context *ctx)
 {
 	struct intel_context *ce = &ctx->engine[engine->id];
 	int ret;
 
 	lockdep_assert_held(&ctx->i915->drm.struct_mutex);
 
-	if (ce->pin_count++)
-		return 0;
+	if (likely(ce->pin_count++))
+		goto out;
 	GEM_BUG_ON(!ce->pin_count); /* no overflow please! */
 
+	if (!ce->state && engine->context_size) {
+		struct i915_vma *vma;
+
+		vma = alloc_context_vma(engine);
+		if (IS_ERR(vma)) {
+			ret = PTR_ERR(vma);
+			goto err;
+		}
+
+		ce->state = vma;
+	}
+
 	if (ce->state) {
 		ret = context_pin(ctx);
 		if (ret)
-			goto error;
+			goto err;
 
 		ce->state->obj->mm.dirty = true;
 	}
@@ -1455,11 +1529,14 @@ static int intel_ring_context_pin(struct intel_engine_cs *engine,
 		ce->initialised = true;
 
 	i915_gem_context_get(ctx);
-	return 0;
 
-error:
+out:
+	/* One ringbuffer to rule them all */
+	return engine->buffer;
+
+err:
 	ce->pin_count = 0;
-	return ret;
+	return ERR_PTR(ret);
 }
 
 static void intel_ring_context_unpin(struct intel_engine_cs *engine,
@@ -1481,78 +1558,70 @@ static void intel_ring_context_unpin(struct intel_engine_cs *engine,
 
 static int intel_init_ring_buffer(struct intel_engine_cs *engine)
 {
-	struct drm_i915_private *dev_priv = engine->i915;
 	struct intel_ring *ring;
-	int ret;
-
-	WARN_ON(engine->buffer);
+	int err;
 
 	intel_engine_setup_common(engine);
 
-	ret = intel_engine_init_common(engine);
-	if (ret)
-		goto error;
+	err = intel_engine_init_common(engine);
+	if (err)
+		goto err;
+
+	if (HWS_NEEDS_PHYSICAL(engine->i915))
+		err = init_phys_status_page(engine);
+	else
+		err = init_status_page(engine);
+	if (err)
+		goto err;
 
 	ring = intel_engine_create_ring(engine, 32 * PAGE_SIZE);
 	if (IS_ERR(ring)) {
-		ret = PTR_ERR(ring);
-		goto error;
-	}
-
-	if (HWS_NEEDS_PHYSICAL(dev_priv)) {
-		WARN_ON(engine->id != RCS);
-		ret = init_phys_status_page(engine);
-		if (ret)
-			goto error;
-	} else {
-		ret = init_status_page(engine);
-		if (ret)
-			goto error;
+		err = PTR_ERR(ring);
+		goto err_hws;
 	}
 
 	/* Ring wraparound at offset 0 sometimes hangs. No idea why. */
-	ret = intel_ring_pin(ring, I915_GTT_PAGE_SIZE);
-	if (ret) {
-		intel_ring_free(ring);
-		goto error;
-	}
+	err = intel_ring_pin(ring, engine->i915, I915_GTT_PAGE_SIZE);
+	if (err)
+		goto err_ring;
+
+	GEM_BUG_ON(engine->buffer);
 	engine->buffer = ring;
 
 	return 0;
 
-error:
-	intel_engine_cleanup(engine);
-	return ret;
+err_ring:
+	intel_ring_free(ring);
+err_hws:
+	if (HWS_NEEDS_PHYSICAL(engine->i915))
+		cleanup_phys_status_page(engine);
+	else
+		cleanup_status_page(engine);
+err:
+	intel_engine_cleanup_common(engine);
+	return err;
 }
 
 void intel_engine_cleanup(struct intel_engine_cs *engine)
 {
-	struct drm_i915_private *dev_priv;
-
-	dev_priv = engine->i915;
+	struct drm_i915_private *dev_priv = engine->i915;
 
-	if (engine->buffer) {
-		WARN_ON(INTEL_GEN(dev_priv) > 2 &&
-			(I915_READ_MODE(engine) & MODE_IDLE) == 0);
+	WARN_ON(INTEL_GEN(dev_priv) > 2 &&
+		(I915_READ_MODE(engine) & MODE_IDLE) == 0);
 
-		intel_ring_unpin(engine->buffer);
-		intel_ring_free(engine->buffer);
-		engine->buffer = NULL;
-	}
+	intel_ring_unpin(engine->buffer);
+	intel_ring_free(engine->buffer);
 
 	if (engine->cleanup)
 		engine->cleanup(engine);
 
-	if (HWS_NEEDS_PHYSICAL(dev_priv)) {
-		WARN_ON(engine->id != RCS);
+	if (HWS_NEEDS_PHYSICAL(dev_priv))
 		cleanup_phys_status_page(engine);
-	} else {
+	else
 		cleanup_status_page(engine);
-	}
 
 	intel_engine_cleanup_common(engine);
 
-	engine->i915 = NULL;
 	dev_priv->engine[engine->id] = NULL;
 	kfree(engine);
 }
@@ -1562,8 +1631,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)
@@ -1578,9 +1648,6 @@ static int ring_request_alloc(struct drm_i915_gem_request *request)
 	 */
 	request->reserved_space += LEGACY_REQUEST_SIZE;
 
-	GEM_BUG_ON(!request->engine->buffer);
-	request->ring = request->engine->buffer;
-
 	cs = intel_ring_begin(request, 0);
 	if (IS_ERR(cs))
 		return PTR_ERR(cs);
@@ -1589,7 +1656,8 @@ static int ring_request_alloc(struct drm_i915_gem_request *request)
 	return 0;
 }
 
-static int wait_for_space(struct drm_i915_gem_request *req, int bytes)
+static noinline int wait_for_space(struct drm_i915_gem_request *req,
+				   unsigned int bytes)
 {
 	struct intel_ring *ring = req->ring;
 	struct drm_i915_gem_request *target;
@@ -1597,8 +1665,7 @@ static int wait_for_space(struct drm_i915_gem_request *req, int bytes)
 
 	lockdep_assert_held(&req->i915->drm.struct_mutex);
 
-	intel_ring_update_space(ring);
-	if (ring->space >= bytes)
+	if (intel_ring_update_space(ring) >= bytes)
 		return 0;
 
 	/*
@@ -1613,12 +1680,9 @@ static int wait_for_space(struct drm_i915_gem_request *req, int bytes)
 	GEM_BUG_ON(!req->reserved_space);
 
 	list_for_each_entry(target, &ring->request_list, ring_link) {
-		unsigned space;
-
 		/* Would completion of this request free enough space? */
-		space = __intel_ring_space(target->postfix, ring->tail,
-					   ring->size);
-		if (space >= bytes)
+		if (bytes <= __intel_ring_space(target->postfix,
+						ring->emit, ring->size))
 			break;
 	}
 
@@ -1638,59 +1702,64 @@ static int wait_for_space(struct drm_i915_gem_request *req, int bytes)
 	return 0;
 }
 
-u32 *intel_ring_begin(struct drm_i915_gem_request *req, int num_dwords)
+u32 *intel_ring_begin(struct drm_i915_gem_request *req,
+		      unsigned 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 bytes = num_dwords * sizeof(u32);
-	int total_bytes, wait_bytes;
-	bool need_wrap = false;
+	const unsigned int remain_usable = ring->effective_size - ring->emit;
+	const unsigned int bytes = num_dwords * sizeof(u32);
+	unsigned int need_wrap = 0;
+	unsigned int total_bytes;
 	u32 *cs;
 
 	total_bytes = bytes + req->reserved_space;
+	GEM_BUG_ON(total_bytes > ring->effective_size);
 
-	if (unlikely(bytes > remain_usable)) {
-		/*
-		 * Not enough space for the basic request. So need to flush
-		 * out the remainder and then wait for base + reserved.
-		 */
-		wait_bytes = remain_actual + total_bytes;
-		need_wrap = true;
-	} else if (unlikely(total_bytes > remain_usable)) {
-		/*
-		 * The base request will fit but the reserved space
-		 * falls off the end. So we don't need an immediate wrap
-		 * and only need to effectively wait for the reserved
-		 * size space from the start of ringbuffer.
-		 */
-		wait_bytes = remain_actual + req->reserved_space;
-	} else {
-		/* No wrapping required, just waiting. */
-		wait_bytes = total_bytes;
+	if (unlikely(total_bytes > remain_usable)) {
+		const int remain_actual = ring->size - ring->emit;
+
+		if (bytes > remain_usable) {
+			/*
+			 * Not enough space for the basic request. So need to
+			 * flush out the remainder and then wait for
+			 * base + reserved.
+			 */
+			total_bytes += remain_actual;
+			need_wrap = remain_actual | 1;
+		} else  {
+			/*
+			 * The base request will fit but the reserved space
+			 * falls off the end. So we don't need an immediate
+			 * wrap and only need to effectively wait for the
+			 * reserved size from the start of ringbuffer.
+			 */
+			total_bytes = req->reserved_space + remain_actual;
+		}
 	}
 
-	if (wait_bytes > ring->space) {
-		int ret = wait_for_space(req, wait_bytes);
+	if (unlikely(total_bytes > ring->space)) {
+		int ret = wait_for_space(req, total_bytes);
 		if (unlikely(ret))
 			return ERR_PTR(ret);
 	}
 
 	if (unlikely(need_wrap)) {
-		GEM_BUG_ON(remain_actual > ring->space);
-		GEM_BUG_ON(ring->tail + remain_actual > ring->size);
+		need_wrap &= ~1;
+		GEM_BUG_ON(need_wrap > ring->space);
+		GEM_BUG_ON(ring->emit + need_wrap > ring->size);
 
 		/* Fill the tail with MI_NOOP */
-		memset(ring->vaddr + ring->tail, 0, remain_actual);
-		ring->tail = 0;
-		ring->space -= remain_actual;
+		memset(ring->vaddr + ring->emit, 0, need_wrap);
+		ring->emit = 0;
+		ring->space -= need_wrap;
 	}
 
-	GEM_BUG_ON(ring->tail > ring->size - bytes);
-	cs = ring->vaddr + ring->tail;
-	ring->tail += bytes;
+	GEM_BUG_ON(ring->emit > ring->size - bytes);
+	GEM_BUG_ON(ring->space < bytes);
+	cs = ring->vaddr + ring->emit;
+	GEM_DEBUG_EXEC(memset(cs, POISON_INUSE, bytes));
+	ring->emit += bytes;
 	ring->space -= bytes;
-	GEM_BUG_ON(ring->space < 0);
 
 	return cs;
 }
@@ -1699,7 +1768,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)
@@ -1736,11 +1805,11 @@ static void gen6_bsd_submit_request(struct drm_i915_gem_request *request)
 	I915_WRITE64_FW(GEN6_BSD_RNCID, 0x0);
 
 	/* Wait for the ring not to be idle, i.e. for it to wake up. */
-	if (intel_wait_for_register_fw(dev_priv,
-				       GEN6_BSD_SLEEP_PSMI_CONTROL,
-				       GEN6_BSD_SLEEP_INDICATOR,
-				       0,
-				       50))
+	if (__intel_wait_for_register_fw(dev_priv,
+					 GEN6_BSD_SLEEP_PSMI_CONTROL,
+					 GEN6_BSD_SLEEP_INDICATOR,
+					 0,
+					 1000, 0, NULL))
 		DRM_ERROR("timed out waiting for the BSD ring to wake up\n");
 
 	/* Now that the ring is fully powered up, update the tail */
@@ -2182,20 +2251,6 @@ int intel_init_bsd_ring_buffer(struct intel_engine_cs *engine)
 	return intel_init_ring_buffer(engine);
 }
 
-/**
- * Initialize the second BSD ring (eg. Broadwell GT3, Skylake GT3)
- */
-int intel_init_bsd2_ring_buffer(struct intel_engine_cs *engine)
-{
-	struct drm_i915_private *dev_priv = engine->i915;
-
-	intel_ring_default_vfuncs(dev_priv, engine);
-
-	engine->emit_flush = gen6_bsd_ring_flush;
-
-	return intel_init_ring_buffer(engine);
-}
-
 int intel_init_blt_ring_buffer(struct intel_engine_cs *engine)
 {
 	struct drm_i915_private *dev_priv = engine->i915;
diff --git a/drivers/gpu/drm/i915/intel_ringbuffer.h b/drivers/gpu/drm/i915/intel_ringbuffer.h
index a82a0807f64d..6aa20ac8cde3 100644
--- a/drivers/gpu/drm/i915/intel_ringbuffer.h
+++ b/drivers/gpu/drm/i915/intel_ringbuffer.h
@@ -17,17 +17,6 @@
 #define CACHELINE_BYTES 64
 #define CACHELINE_DWORDS (CACHELINE_BYTES / sizeof(uint32_t))
 
-/*
- * Gen2 BSpec "1. Programming Environment" / 1.4.4.6 "Ring Buffer Use"
- * Gen3 BSpec "vol1c Memory Interface Functions" / 2.3.4.5 "Ring Buffer Use"
- * Gen4+ BSpec "vol1c Memory Interface and Command Stream" / 5.3.4.5 "Ring Buffer Use"
- *
- * "If the Ring Buffer Head Pointer and the Tail Pointer are on the same
- * cacheline, the Head Pointer must not be greater than the Tail
- * Pointer."
- */
-#define I915_RING_FREE_SPACE 64
-
 struct intel_hw_status_page {
 	struct i915_vma *vma;
 	u32 *page_addr;
@@ -139,16 +128,15 @@ struct intel_ring {
 	struct i915_vma *vma;
 	void *vaddr;
 
-	struct intel_engine_cs *engine;
-
 	struct list_head request_list;
 
 	u32 head;
 	u32 tail;
+	u32 emit;
 
-	int space;
-	int size;
-	int effective_size;
+	u32 space;
+	u32 size;
+	u32 effective_size;
 };
 
 struct i915_gem_context;
@@ -189,15 +177,28 @@ enum intel_engine_id {
 	VECS
 };
 
+struct i915_priolist {
+	struct rb_node node;
+	struct list_head requests;
+	int priority;
+};
+
+#define INTEL_ENGINE_CS_MAX_NAME 8
+
 struct intel_engine_cs {
 	struct drm_i915_private *i915;
-	const char	*name;
+	char name[INTEL_ENGINE_CS_MAX_NAME];
 	enum intel_engine_id id;
-	unsigned int exec_id;
+	unsigned int uabi_id;
 	unsigned int hw_id;
 	unsigned int guc_id;
-	u32		mmio_base;
+
+	u8 class;
+	u8 instance;
+	u32 context_size;
+	u32 mmio_base;
 	unsigned int irq_shift;
+
 	struct intel_ring *buffer;
 	struct intel_timeline *timeline;
 
@@ -265,8 +266,8 @@ struct intel_engine_cs {
 
 	void		(*set_default_submission)(struct intel_engine_cs *engine);
 
-	int		(*context_pin)(struct intel_engine_cs *engine,
-				       struct i915_gem_context *ctx);
+	struct intel_ring *(*context_pin)(struct intel_engine_cs *engine,
+					  struct i915_gem_context *ctx);
 	void		(*context_unpin)(struct intel_engine_cs *engine,
 					 struct i915_gem_context *ctx);
 	int		(*request_alloc)(struct drm_i915_gem_request *req);
@@ -372,9 +373,18 @@ struct intel_engine_cs {
 
 	/* Execlists */
 	struct tasklet_struct irq_tasklet;
+	struct i915_priolist default_priolist;
+	bool no_priolist;
 	struct execlist_port {
-		struct drm_i915_gem_request *request;
-		unsigned int count;
+		struct drm_i915_gem_request *request_count;
+#define EXECLIST_COUNT_BITS 2
+#define port_request(p) ptr_mask_bits((p)->request_count, EXECLIST_COUNT_BITS)
+#define port_count(p) ptr_unmask_bits((p)->request_count, EXECLIST_COUNT_BITS)
+#define port_pack(rq, count) ptr_pack_bits(rq, count, EXECLIST_COUNT_BITS)
+#define port_unpack(p, count) ptr_unpack_bits((p)->request_count, count, EXECLIST_COUNT_BITS)
+#define port_set(p, packed) ((p)->request_count = (packed))
+#define port_isset(p) ((p)->request_count)
+#define port_index(p, e) ((p) - (e)->execlist_port)
 		GEM_DEBUG_DECL(u32 context_id);
 	} execlist_port[2];
 	struct rb_root execlist_queue;
@@ -487,7 +497,11 @@ 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);
+int intel_ring_pin(struct intel_ring *ring,
+		   struct drm_i915_private *i915,
+		   unsigned int offset_bias);
+void intel_ring_reset(struct intel_ring *ring, u32 tail);
+unsigned int intel_ring_update_space(struct intel_ring *ring);
 void intel_ring_unpin(struct intel_ring *ring);
 void intel_ring_free(struct intel_ring *ring);
 
@@ -498,7 +512,8 @@ void intel_legacy_submission_resume(struct drm_i915_private *dev_priv);
 
 int __must_check intel_ring_cacheline_align(struct drm_i915_gem_request *req);
 
-u32 __must_check *intel_ring_begin(struct drm_i915_gem_request *req, int n);
+u32 __must_check *intel_ring_begin(struct drm_i915_gem_request *req,
+				   unsigned int n);
 
 static inline void
 intel_ring_advance(struct drm_i915_gem_request *req, u32 *cs)
@@ -511,7 +526,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
@@ -538,9 +553,40 @@ assert_ring_tail_valid(const struct intel_ring *ring, unsigned int tail)
 	 */
 	GEM_BUG_ON(!IS_ALIGNED(tail, 8));
 	GEM_BUG_ON(tail >= ring->size);
+
+	/*
+	 * "Ring Buffer Use"
+	 *	Gen2 BSpec "1. Programming Environment" / 1.4.4.6
+	 *	Gen3 BSpec "1c Memory Interface Functions" / 2.3.4.5
+	 *	Gen4+ BSpec "1c Memory Interface and Command Stream" / 5.3.4.5
+	 * "If the Ring Buffer Head Pointer and the Tail Pointer are on the
+	 * same cacheline, the Head Pointer must not be greater than the Tail
+	 * Pointer."
+	 *
+	 * We use ring->head as the last known location of the actual RING_HEAD,
+	 * it may have advanced but in the worst case it is equally the same
+	 * as ring->head and so we should never program RING_TAIL to advance
+	 * into the same cacheline as ring->head.
+	 */
+#define cacheline(a) round_down(a, CACHELINE_BYTES)
+	GEM_BUG_ON(cacheline(tail) == cacheline(ring->head) &&
+		   tail < ring->head);
+#undef cacheline
 }
 
-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);
 
@@ -551,7 +597,6 @@ void intel_engine_cleanup_common(struct intel_engine_cs *engine);
 
 int intel_init_render_ring_buffer(struct intel_engine_cs *engine);
 int intel_init_bsd_ring_buffer(struct intel_engine_cs *engine);
-int intel_init_bsd2_ring_buffer(struct intel_engine_cs *engine);
 int intel_init_blt_ring_buffer(struct intel_engine_cs *engine);
 int intel_init_vebox_ring_buffer(struct intel_engine_cs *engine);
 
@@ -652,7 +697,8 @@ bool intel_engine_add_wait(struct intel_engine_cs *engine,
 			   struct intel_wait *wait);
 void intel_engine_remove_wait(struct intel_engine_cs *engine,
 			      struct intel_wait *wait);
-void intel_engine_enable_signaling(struct drm_i915_gem_request *request);
+void intel_engine_enable_signaling(struct drm_i915_gem_request *request,
+				   bool wakeup);
 void intel_engine_cancel_signaling(struct drm_i915_gem_request *request);
 
 static inline bool intel_engine_has_waiter(const struct intel_engine_cs *engine)
@@ -685,6 +731,7 @@ static inline u32 *gen8_emit_pipe_control(u32 *batch, u32 flags, u32 offset)
 bool intel_engine_is_idle(struct intel_engine_cs *engine);
 bool intel_engines_are_idle(struct drm_i915_private *dev_priv);
 
+void intel_engines_mark_idle(struct drm_i915_private *i915);
 void intel_engines_reset_default_submission(struct drm_i915_private *i915);
 
 #endif /* _INTEL_RINGBUFFER_H_ */
diff --git a/drivers/gpu/drm/i915/intel_sdvo.c b/drivers/gpu/drm/i915/intel_sdvo.c
index ef6fa87b2f8a..f4329d20b6f6 100644
--- a/drivers/gpu/drm/i915/intel_sdvo.c
+++ b/drivers/gpu/drm/i915/intel_sdvo.c
@@ -100,13 +100,6 @@ struct intel_sdvo {
 	uint16_t hotplug_active;
 
 	/**
-	 * This is used to select the color range of RBG outputs in HDMI mode.
-	 * It is only valid when using TMDS encoding and 8 bit per color mode.
-	 */
-	uint32_t color_range;
-	bool color_range_auto;
-
-	/**
 	 * This is set if we're going to treat the device as TV-out.
 	 *
 	 * While we have these nice friendly flags for output types that ought
@@ -117,9 +110,6 @@ struct intel_sdvo {
 
 	enum port port;
 
-	/* This is for current tv format name */
-	int tv_format_index;
-
 	/**
 	 * This is set if we treat the device as HDMI, instead of DVI.
 	 */
@@ -154,8 +144,6 @@ struct intel_sdvo_connector {
 	/* Mark the type of connector */
 	uint16_t output_flag;
 
-	enum hdmi_force_audio force_audio;
-
 	/* This contains all current supported TV format */
 	u8 tv_format_supported[TV_FORMAT_NUM];
 	int   format_supported_num;
@@ -182,24 +170,19 @@ struct intel_sdvo_connector {
 	/* add the property for the SDVO-TV/LVDS */
 	struct drm_property *brightness;
 
-	/* Add variable to record current setting for the above property */
-	u32	left_margin, right_margin, top_margin, bottom_margin;
-
 	/* this is to get the range of margin.*/
-	u32	max_hscan,  max_vscan;
-	u32	max_hpos, cur_hpos;
-	u32	max_vpos, cur_vpos;
-	u32	cur_brightness, max_brightness;
-	u32	cur_contrast,	max_contrast;
-	u32	cur_saturation, max_saturation;
-	u32	cur_hue,	max_hue;
-	u32	cur_sharpness,	max_sharpness;
-	u32	cur_flicker_filter,		max_flicker_filter;
-	u32	cur_flicker_filter_adaptive,	max_flicker_filter_adaptive;
-	u32	cur_flicker_filter_2d,		max_flicker_filter_2d;
-	u32	cur_tv_chroma_filter,	max_tv_chroma_filter;
-	u32	cur_tv_luma_filter,	max_tv_luma_filter;
-	u32	cur_dot_crawl,	max_dot_crawl;
+	u32 max_hscan, max_vscan;
+};
+
+struct intel_sdvo_connector_state {
+	/* base.base: tv.saturation/contrast/hue/brightness */
+	struct intel_digital_connector_state base;
+
+	struct {
+		unsigned overscan_h, overscan_v, hpos, vpos, sharpness;
+		unsigned flicker_filter, flicker_filter_2d, flicker_filter_adaptive;
+		unsigned chroma_filter, luma_filter, dot_crawl;
+	} tv;
 };
 
 static struct intel_sdvo *to_sdvo(struct intel_encoder *encoder)
@@ -212,9 +195,16 @@ static struct intel_sdvo *intel_attached_sdvo(struct drm_connector *connector)
 	return to_sdvo(intel_attached_encoder(connector));
 }
 
-static struct intel_sdvo_connector *to_intel_sdvo_connector(struct drm_connector *connector)
+static struct intel_sdvo_connector *
+to_intel_sdvo_connector(struct drm_connector *connector)
 {
-	return container_of(to_intel_connector(connector), struct intel_sdvo_connector, base);
+	return container_of(connector, struct intel_sdvo_connector, base.base);
+}
+
+static struct intel_sdvo_connector_state *
+to_intel_sdvo_connector_state(struct drm_connector_state *conn_state)
+{
+	return container_of(conn_state, struct intel_sdvo_connector_state, base.base);
 }
 
 static bool
@@ -1030,12 +1020,13 @@ static bool intel_sdvo_set_avi_infoframe(struct intel_sdvo *intel_sdvo,
 					  sdvo_data, sizeof(sdvo_data));
 }
 
-static bool intel_sdvo_set_tv_format(struct intel_sdvo *intel_sdvo)
+static bool intel_sdvo_set_tv_format(struct intel_sdvo *intel_sdvo,
+				     struct drm_connector_state *conn_state)
 {
 	struct intel_sdvo_tv_format format;
 	uint32_t format_map;
 
-	format_map = 1 << intel_sdvo->tv_format_index;
+	format_map = 1 << conn_state->tv.mode;
 	memset(&format, 0, sizeof(format));
 	memcpy(&format, &format_map, min(sizeof(format), sizeof(format_map)));
 
@@ -1122,6 +1113,8 @@ static bool intel_sdvo_compute_config(struct intel_encoder *encoder,
 				      struct drm_connector_state *conn_state)
 {
 	struct intel_sdvo *intel_sdvo = to_sdvo(encoder);
+	struct intel_sdvo_connector_state *intel_sdvo_state =
+		to_intel_sdvo_connector_state(conn_state);
 	struct drm_display_mode *adjusted_mode = &pipe_config->base.adjusted_mode;
 	struct drm_display_mode *mode = &pipe_config->base.mode;
 
@@ -1160,9 +1153,14 @@ static bool intel_sdvo_compute_config(struct intel_encoder *encoder,
 	pipe_config->pixel_multiplier =
 		intel_sdvo_get_pixel_multiplier(adjusted_mode);
 
-	pipe_config->has_hdmi_sink = intel_sdvo->has_hdmi_monitor;
+	if (intel_sdvo_state->base.force_audio != HDMI_AUDIO_OFF_DVI)
+		pipe_config->has_hdmi_sink = intel_sdvo->has_hdmi_monitor;
+
+	if (intel_sdvo_state->base.force_audio == HDMI_AUDIO_ON ||
+	    (intel_sdvo_state->base.force_audio == HDMI_AUDIO_AUTO && intel_sdvo->has_hdmi_audio))
+		pipe_config->has_audio = true;
 
-	if (intel_sdvo->color_range_auto) {
+	if (intel_sdvo_state->base.broadcast_rgb == INTEL_BROADCAST_RGB_AUTO) {
 		/* See CEA-861-E - 5.1 Default Encoding Parameters */
 		/* FIXME: This bit is only valid when using TMDS encoding and 8
 		 * bit per color mode. */
@@ -1171,7 +1169,7 @@ static bool intel_sdvo_compute_config(struct intel_encoder *encoder,
 			pipe_config->limited_color_range = true;
 	} else {
 		if (pipe_config->has_hdmi_sink &&
-		    intel_sdvo->color_range == HDMI_COLOR_RANGE_16_235)
+		    intel_sdvo_state->base.broadcast_rgb == INTEL_BROADCAST_RGB_LIMITED)
 			pipe_config->limited_color_range = true;
 	}
 
@@ -1186,6 +1184,68 @@ static bool intel_sdvo_compute_config(struct intel_encoder *encoder,
 	return true;
 }
 
+#define UPDATE_PROPERTY(input, NAME) \
+	do { \
+		val = input; \
+		intel_sdvo_set_value(intel_sdvo, SDVO_CMD_SET_##NAME, &val, sizeof(val)); \
+	} while (0)
+
+static void intel_sdvo_update_props(struct intel_sdvo *intel_sdvo,
+				    struct intel_sdvo_connector_state *sdvo_state)
+{
+	struct drm_connector_state *conn_state = &sdvo_state->base.base;
+	struct intel_sdvo_connector *intel_sdvo_conn =
+		to_intel_sdvo_connector(conn_state->connector);
+	uint16_t val;
+
+	if (intel_sdvo_conn->left)
+		UPDATE_PROPERTY(sdvo_state->tv.overscan_h, OVERSCAN_H);
+
+	if (intel_sdvo_conn->top)
+		UPDATE_PROPERTY(sdvo_state->tv.overscan_v, OVERSCAN_V);
+
+	if (intel_sdvo_conn->hpos)
+		UPDATE_PROPERTY(sdvo_state->tv.hpos, HPOS);
+
+	if (intel_sdvo_conn->vpos)
+		UPDATE_PROPERTY(sdvo_state->tv.vpos, VPOS);
+
+	if (intel_sdvo_conn->saturation)
+		UPDATE_PROPERTY(conn_state->tv.saturation, SATURATION);
+
+	if (intel_sdvo_conn->contrast)
+		UPDATE_PROPERTY(conn_state->tv.contrast, CONTRAST);
+
+	if (intel_sdvo_conn->hue)
+		UPDATE_PROPERTY(conn_state->tv.hue, HUE);
+
+	if (intel_sdvo_conn->brightness)
+		UPDATE_PROPERTY(conn_state->tv.brightness, BRIGHTNESS);
+
+	if (intel_sdvo_conn->sharpness)
+		UPDATE_PROPERTY(sdvo_state->tv.sharpness, SHARPNESS);
+
+	if (intel_sdvo_conn->flicker_filter)
+		UPDATE_PROPERTY(sdvo_state->tv.flicker_filter, FLICKER_FILTER);
+
+	if (intel_sdvo_conn->flicker_filter_2d)
+		UPDATE_PROPERTY(sdvo_state->tv.flicker_filter_2d, FLICKER_FILTER_2D);
+
+	if (intel_sdvo_conn->flicker_filter_adaptive)
+		UPDATE_PROPERTY(sdvo_state->tv.flicker_filter_adaptive, FLICKER_FILTER_ADAPTIVE);
+
+	if (intel_sdvo_conn->tv_chroma_filter)
+		UPDATE_PROPERTY(sdvo_state->tv.chroma_filter, TV_CHROMA_FILTER);
+
+	if (intel_sdvo_conn->tv_luma_filter)
+		UPDATE_PROPERTY(sdvo_state->tv.luma_filter, TV_LUMA_FILTER);
+
+	if (intel_sdvo_conn->dot_crawl)
+		UPDATE_PROPERTY(sdvo_state->tv.dot_crawl, DOT_CRAWL);
+
+#undef UPDATE_PROPERTY
+}
+
 static void intel_sdvo_pre_enable(struct intel_encoder *intel_encoder,
 				  struct intel_crtc_state *crtc_state,
 				  struct drm_connector_state *conn_state)
@@ -1193,6 +1253,7 @@ static void intel_sdvo_pre_enable(struct intel_encoder *intel_encoder,
 	struct drm_i915_private *dev_priv = to_i915(intel_encoder->base.dev);
 	struct intel_crtc *crtc = to_intel_crtc(crtc_state->base.crtc);
 	const struct drm_display_mode *adjusted_mode = &crtc_state->base.adjusted_mode;
+	struct intel_sdvo_connector_state *sdvo_state = to_intel_sdvo_connector_state(conn_state);
 	struct drm_display_mode *mode = &crtc_state->base.mode;
 	struct intel_sdvo *intel_sdvo = to_sdvo(intel_encoder);
 	u32 sdvox;
@@ -1200,6 +1261,8 @@ static void intel_sdvo_pre_enable(struct intel_encoder *intel_encoder,
 	struct intel_sdvo_dtd input_dtd, output_dtd;
 	int rate;
 
+	intel_sdvo_update_props(intel_sdvo, sdvo_state);
+
 	/* First, set the input mapping for the first input to our controlled
 	 * output. This is only correct if we're a single-input device, in
 	 * which case the first input is the output from the appropriate SDVO
@@ -1241,7 +1304,7 @@ static void intel_sdvo_pre_enable(struct intel_encoder *intel_encoder,
 		intel_sdvo_set_encode(intel_sdvo, SDVO_ENCODE_DVI);
 
 	if (intel_sdvo->is_tv &&
-	    !intel_sdvo_set_tv_format(intel_sdvo))
+	    !intel_sdvo_set_tv_format(intel_sdvo, conn_state))
 		return;
 
 	intel_sdvo_get_dtd_from_mode(&input_dtd, adjusted_mode);
@@ -1285,7 +1348,7 @@ static void intel_sdvo_pre_enable(struct intel_encoder *intel_encoder,
 	else
 		sdvox |= SDVO_PIPE_SEL(crtc->pipe);
 
-	if (intel_sdvo->has_hdmi_audio)
+	if (crtc_state->has_audio)
 		sdvox |= SDVO_AUDIO_ENABLE;
 
 	if (INTEL_GEN(dev_priv) >= 4) {
@@ -1694,12 +1757,6 @@ intel_sdvo_tmds_sink_detect(struct drm_connector *connector)
 		kfree(edid);
 	}
 
-	if (status == connector_status_connected) {
-		struct intel_sdvo_connector *intel_sdvo_connector = to_intel_sdvo_connector(connector);
-		if (intel_sdvo_connector->force_audio != HDMI_AUDIO_AUTO)
-			intel_sdvo->has_hdmi_audio = (intel_sdvo_connector->force_audio == HDMI_AUDIO_ON);
-	}
-
 	return status;
 }
 
@@ -1879,6 +1936,7 @@ static const struct drm_display_mode sdvo_tv_modes[] = {
 static void intel_sdvo_get_tv_modes(struct drm_connector *connector)
 {
 	struct intel_sdvo *intel_sdvo = intel_attached_sdvo(connector);
+	const struct drm_connector_state *conn_state = connector->state;
 	struct intel_sdvo_sdtv_resolution_request tv_res;
 	uint32_t reply = 0, format_map = 0;
 	int i;
@@ -1889,7 +1947,7 @@ static void intel_sdvo_get_tv_modes(struct drm_connector *connector)
 	/* Read the list of supported input resolutions for the selected TV
 	 * format.
 	 */
-	format_map = 1 << intel_sdvo->tv_format_index;
+	format_map = 1 << conn_state->tv.mode;
 	memcpy(&tv_res, &format_map,
 	       min(sizeof(format_map), sizeof(struct intel_sdvo_sdtv_resolution_request)));
 
@@ -1978,192 +2036,121 @@ static void intel_sdvo_destroy(struct drm_connector *connector)
 	kfree(intel_sdvo_connector);
 }
 
-static bool intel_sdvo_detect_hdmi_audio(struct drm_connector *connector)
-{
-	struct intel_sdvo *intel_sdvo = intel_attached_sdvo(connector);
-	struct edid *edid;
-	bool has_audio = false;
-
-	if (!intel_sdvo->is_hdmi)
-		return false;
-
-	edid = intel_sdvo_get_edid(connector);
-	if (edid != NULL && edid->input & DRM_EDID_INPUT_DIGITAL)
-		has_audio = drm_detect_monitor_audio(edid);
-	kfree(edid);
-
-	return has_audio;
-}
-
 static int
-intel_sdvo_set_property(struct drm_connector *connector,
-			struct drm_property *property,
-			uint64_t val)
+intel_sdvo_connector_atomic_get_property(struct drm_connector *connector,
+					 const struct drm_connector_state *state,
+					 struct drm_property *property,
+					 uint64_t *val)
 {
-	struct intel_sdvo *intel_sdvo = intel_attached_sdvo(connector);
 	struct intel_sdvo_connector *intel_sdvo_connector = to_intel_sdvo_connector(connector);
-	struct drm_i915_private *dev_priv = to_i915(connector->dev);
-	uint16_t temp_value;
-	uint8_t cmd;
-	int ret;
-
-	ret = drm_object_property_set_value(&connector->base, property, val);
-	if (ret)
-		return ret;
-
-	if (property == dev_priv->force_audio_property) {
-		int i = val;
-		bool has_audio;
-
-		if (i == intel_sdvo_connector->force_audio)
-			return 0;
-
-		intel_sdvo_connector->force_audio = i;
-
-		if (i == HDMI_AUDIO_AUTO)
-			has_audio = intel_sdvo_detect_hdmi_audio(connector);
-		else
-			has_audio = (i == HDMI_AUDIO_ON);
-
-		if (has_audio == intel_sdvo->has_hdmi_audio)
-			return 0;
-
-		intel_sdvo->has_hdmi_audio = has_audio;
-		goto done;
-	}
-
-	if (property == dev_priv->broadcast_rgb_property) {
-		bool old_auto = intel_sdvo->color_range_auto;
-		uint32_t old_range = intel_sdvo->color_range;
-
-		switch (val) {
-		case INTEL_BROADCAST_RGB_AUTO:
-			intel_sdvo->color_range_auto = true;
-			break;
-		case INTEL_BROADCAST_RGB_FULL:
-			intel_sdvo->color_range_auto = false;
-			intel_sdvo->color_range = 0;
-			break;
-		case INTEL_BROADCAST_RGB_LIMITED:
-			intel_sdvo->color_range_auto = false;
-			/* FIXME: this bit is only valid when using TMDS
-			 * encoding and 8 bit per color mode. */
-			intel_sdvo->color_range = HDMI_COLOR_RANGE_16_235;
-			break;
-		default:
-			return -EINVAL;
-		}
-
-		if (old_auto == intel_sdvo->color_range_auto &&
-		    old_range == intel_sdvo->color_range)
-			return 0;
-
-		goto done;
-	}
-
-	if (property == connector->dev->mode_config.aspect_ratio_property) {
-		connector->state->picture_aspect_ratio = val;
-		goto done;
-	}
-
-#define CHECK_PROPERTY(name, NAME) \
-	if (intel_sdvo_connector->name == property) { \
-		if (intel_sdvo_connector->cur_##name == temp_value) return 0; \
-		if (intel_sdvo_connector->max_##name < temp_value) return -EINVAL; \
-		cmd = SDVO_CMD_SET_##NAME; \
-		intel_sdvo_connector->cur_##name = temp_value; \
-		goto set_value; \
-	}
+	const struct intel_sdvo_connector_state *sdvo_state = to_intel_sdvo_connector_state((void *)state);
 
 	if (property == intel_sdvo_connector->tv_format) {
-		if (val >= TV_FORMAT_NUM)
-			return -EINVAL;
-
-		if (intel_sdvo->tv_format_index ==
-		    intel_sdvo_connector->tv_format_supported[val])
-			return 0;
-
-		intel_sdvo->tv_format_index = intel_sdvo_connector->tv_format_supported[val];
-		goto done;
-	} else if (IS_TV_OR_LVDS(intel_sdvo_connector)) {
-		temp_value = val;
-		if (intel_sdvo_connector->left == property) {
-			drm_object_property_set_value(&connector->base,
-							 intel_sdvo_connector->right, val);
-			if (intel_sdvo_connector->left_margin == temp_value)
-				return 0;
+		int i;
 
-			intel_sdvo_connector->left_margin = temp_value;
-			intel_sdvo_connector->right_margin = temp_value;
-			temp_value = intel_sdvo_connector->max_hscan -
-				intel_sdvo_connector->left_margin;
-			cmd = SDVO_CMD_SET_OVERSCAN_H;
-			goto set_value;
-		} else if (intel_sdvo_connector->right == property) {
-			drm_object_property_set_value(&connector->base,
-							 intel_sdvo_connector->left, val);
-			if (intel_sdvo_connector->right_margin == temp_value)
-				return 0;
+		for (i = 0; i < intel_sdvo_connector->format_supported_num; i++)
+			if (state->tv.mode == intel_sdvo_connector->tv_format_supported[i]) {
+				*val = i;
 
-			intel_sdvo_connector->left_margin = temp_value;
-			intel_sdvo_connector->right_margin = temp_value;
-			temp_value = intel_sdvo_connector->max_hscan -
-				intel_sdvo_connector->left_margin;
-			cmd = SDVO_CMD_SET_OVERSCAN_H;
-			goto set_value;
-		} else if (intel_sdvo_connector->top == property) {
-			drm_object_property_set_value(&connector->base,
-							 intel_sdvo_connector->bottom, val);
-			if (intel_sdvo_connector->top_margin == temp_value)
 				return 0;
+			}
 
-			intel_sdvo_connector->top_margin = temp_value;
-			intel_sdvo_connector->bottom_margin = temp_value;
-			temp_value = intel_sdvo_connector->max_vscan -
-				intel_sdvo_connector->top_margin;
-			cmd = SDVO_CMD_SET_OVERSCAN_V;
-			goto set_value;
-		} else if (intel_sdvo_connector->bottom == property) {
-			drm_object_property_set_value(&connector->base,
-							 intel_sdvo_connector->top, val);
-			if (intel_sdvo_connector->bottom_margin == temp_value)
-				return 0;
+		WARN_ON(1);
+		*val = 0;
+	} else if (property == intel_sdvo_connector->top ||
+		   property == intel_sdvo_connector->bottom)
+		*val = intel_sdvo_connector->max_vscan - sdvo_state->tv.overscan_v;
+	else if (property == intel_sdvo_connector->left ||
+		 property == intel_sdvo_connector->right)
+		*val = intel_sdvo_connector->max_hscan - sdvo_state->tv.overscan_h;
+	else if (property == intel_sdvo_connector->hpos)
+		*val = sdvo_state->tv.hpos;
+	else if (property == intel_sdvo_connector->vpos)
+		*val = sdvo_state->tv.vpos;
+	else if (property == intel_sdvo_connector->saturation)
+		*val = state->tv.saturation;
+	else if (property == intel_sdvo_connector->contrast)
+		*val = state->tv.contrast;
+	else if (property == intel_sdvo_connector->hue)
+		*val = state->tv.hue;
+	else if (property == intel_sdvo_connector->brightness)
+		*val = state->tv.brightness;
+	else if (property == intel_sdvo_connector->sharpness)
+		*val = sdvo_state->tv.sharpness;
+	else if (property == intel_sdvo_connector->flicker_filter)
+		*val = sdvo_state->tv.flicker_filter;
+	else if (property == intel_sdvo_connector->flicker_filter_2d)
+		*val = sdvo_state->tv.flicker_filter_2d;
+	else if (property == intel_sdvo_connector->flicker_filter_adaptive)
+		*val = sdvo_state->tv.flicker_filter_adaptive;
+	else if (property == intel_sdvo_connector->tv_chroma_filter)
+		*val = sdvo_state->tv.chroma_filter;
+	else if (property == intel_sdvo_connector->tv_luma_filter)
+		*val = sdvo_state->tv.luma_filter;
+	else if (property == intel_sdvo_connector->dot_crawl)
+		*val = sdvo_state->tv.dot_crawl;
+	else
+		return intel_digital_connector_atomic_get_property(connector, state, property, val);
 
-			intel_sdvo_connector->top_margin = temp_value;
-			intel_sdvo_connector->bottom_margin = temp_value;
-			temp_value = intel_sdvo_connector->max_vscan -
-				intel_sdvo_connector->top_margin;
-			cmd = SDVO_CMD_SET_OVERSCAN_V;
-			goto set_value;
-		}
-		CHECK_PROPERTY(hpos, HPOS)
-		CHECK_PROPERTY(vpos, VPOS)
-		CHECK_PROPERTY(saturation, SATURATION)
-		CHECK_PROPERTY(contrast, CONTRAST)
-		CHECK_PROPERTY(hue, HUE)
-		CHECK_PROPERTY(brightness, BRIGHTNESS)
-		CHECK_PROPERTY(sharpness, SHARPNESS)
-		CHECK_PROPERTY(flicker_filter, FLICKER_FILTER)
-		CHECK_PROPERTY(flicker_filter_2d, FLICKER_FILTER_2D)
-		CHECK_PROPERTY(flicker_filter_adaptive, FLICKER_FILTER_ADAPTIVE)
-		CHECK_PROPERTY(tv_chroma_filter, TV_CHROMA_FILTER)
-		CHECK_PROPERTY(tv_luma_filter, TV_LUMA_FILTER)
-		CHECK_PROPERTY(dot_crawl, DOT_CRAWL)
-	}
+	return 0;
+}
 
-	return -EINVAL; /* unknown property */
+static int
+intel_sdvo_connector_atomic_set_property(struct drm_connector *connector,
+					 struct drm_connector_state *state,
+					 struct drm_property *property,
+					 uint64_t val)
+{
+	struct intel_sdvo_connector *intel_sdvo_connector = to_intel_sdvo_connector(connector);
+	struct intel_sdvo_connector_state *sdvo_state = to_intel_sdvo_connector_state(state);
 
-set_value:
-	if (!intel_sdvo_set_value(intel_sdvo, cmd, &temp_value, 2))
-		return -EIO;
+	if (property == intel_sdvo_connector->tv_format) {
+		state->tv.mode = intel_sdvo_connector->tv_format_supported[val];
 
+		if (state->crtc) {
+			struct drm_crtc_state *crtc_state =
+				drm_atomic_get_new_crtc_state(state->state, state->crtc);
 
-done:
-	if (intel_sdvo->base.base.crtc)
-		intel_crtc_restore_mode(intel_sdvo->base.base.crtc);
+			crtc_state->connectors_changed = true;
+		}
+	} else if (property == intel_sdvo_connector->top ||
+		   property == intel_sdvo_connector->bottom)
+		/* Cannot set these independent from each other */
+		sdvo_state->tv.overscan_v = intel_sdvo_connector->max_vscan - val;
+	else if (property == intel_sdvo_connector->left ||
+		 property == intel_sdvo_connector->right)
+		/* Cannot set these independent from each other */
+		sdvo_state->tv.overscan_h = intel_sdvo_connector->max_hscan - val;
+	else if (property == intel_sdvo_connector->hpos)
+		sdvo_state->tv.hpos = val;
+	else if (property == intel_sdvo_connector->vpos)
+		sdvo_state->tv.vpos = val;
+	else if (property == intel_sdvo_connector->saturation)
+		state->tv.saturation = val;
+	else if (property == intel_sdvo_connector->contrast)
+		state->tv.contrast = val;
+	else if (property == intel_sdvo_connector->hue)
+		state->tv.hue = val;
+	else if (property == intel_sdvo_connector->brightness)
+		state->tv.brightness = val;
+	else if (property == intel_sdvo_connector->sharpness)
+		sdvo_state->tv.sharpness = val;
+	else if (property == intel_sdvo_connector->flicker_filter)
+		sdvo_state->tv.flicker_filter = val;
+	else if (property == intel_sdvo_connector->flicker_filter_2d)
+		sdvo_state->tv.flicker_filter_2d = val;
+	else if (property == intel_sdvo_connector->flicker_filter_adaptive)
+		sdvo_state->tv.flicker_filter_adaptive = val;
+	else if (property == intel_sdvo_connector->tv_chroma_filter)
+		sdvo_state->tv.chroma_filter = val;
+	else if (property == intel_sdvo_connector->tv_luma_filter)
+		sdvo_state->tv.luma_filter = val;
+	else if (property == intel_sdvo_connector->dot_crawl)
+		sdvo_state->tv.dot_crawl = val;
+	else
+		return intel_digital_connector_atomic_set_property(connector, state, property, val);
 
 	return 0;
-#undef CHECK_PROPERTY
 }
 
 static int
@@ -2191,22 +2178,61 @@ intel_sdvo_connector_unregister(struct drm_connector *connector)
 	intel_connector_unregister(connector);
 }
 
+static struct drm_connector_state *
+intel_sdvo_connector_duplicate_state(struct drm_connector *connector)
+{
+	struct intel_sdvo_connector_state *state;
+
+	state = kmemdup(connector->state, sizeof(*state), GFP_KERNEL);
+	if (!state)
+		return NULL;
+
+	__drm_atomic_helper_connector_duplicate_state(connector, &state->base.base);
+	return &state->base.base;
+}
+
 static const struct drm_connector_funcs intel_sdvo_connector_funcs = {
 	.dpms = drm_atomic_helper_connector_dpms,
 	.detect = intel_sdvo_detect,
 	.fill_modes = drm_helper_probe_single_connector_modes,
-	.set_property = intel_sdvo_set_property,
-	.atomic_get_property = intel_connector_atomic_get_property,
+	.set_property = drm_atomic_helper_connector_set_property,
+	.atomic_get_property = intel_sdvo_connector_atomic_get_property,
+	.atomic_set_property = intel_sdvo_connector_atomic_set_property,
 	.late_register = intel_sdvo_connector_register,
 	.early_unregister = intel_sdvo_connector_unregister,
 	.destroy = intel_sdvo_destroy,
 	.atomic_destroy_state = drm_atomic_helper_connector_destroy_state,
-	.atomic_duplicate_state = drm_atomic_helper_connector_duplicate_state,
+	.atomic_duplicate_state = intel_sdvo_connector_duplicate_state,
 };
 
+static int intel_sdvo_atomic_check(struct drm_connector *conn,
+				   struct drm_connector_state *new_conn_state)
+{
+	struct drm_atomic_state *state = new_conn_state->state;
+	struct drm_connector_state *old_conn_state =
+		drm_atomic_get_old_connector_state(state, conn);
+	struct intel_sdvo_connector_state *old_state =
+		to_intel_sdvo_connector_state(old_conn_state);
+	struct intel_sdvo_connector_state *new_state =
+		to_intel_sdvo_connector_state(new_conn_state);
+
+	if (new_conn_state->crtc &&
+	    (memcmp(&old_state->tv, &new_state->tv, sizeof(old_state->tv)) ||
+	     memcmp(&old_conn_state->tv, &new_conn_state->tv, sizeof(old_conn_state->tv)))) {
+		struct drm_crtc_state *crtc_state =
+			drm_atomic_get_new_crtc_state(new_conn_state->state,
+						      new_conn_state->crtc);
+
+		crtc_state->connectors_changed = true;
+	}
+
+	return intel_digital_connector_atomic_check(conn, new_conn_state);
+}
+
 static const struct drm_connector_helper_funcs intel_sdvo_connector_helper_funcs = {
 	.get_modes = intel_sdvo_get_modes,
 	.mode_valid = intel_sdvo_mode_valid,
+	.atomic_check = intel_sdvo_atomic_check,
 };
 
 static void intel_sdvo_enc_destroy(struct drm_encoder *encoder)
@@ -2398,7 +2424,6 @@ intel_sdvo_add_hdmi_properties(struct intel_sdvo *intel_sdvo,
 	intel_attach_force_audio_property(&connector->base.base);
 	if (INTEL_GEN(dev_priv) >= 4 && IS_MOBILE(dev_priv)) {
 		intel_attach_broadcast_rgb_property(&connector->base.base);
-		intel_sdvo->color_range_auto = true;
 	}
 	intel_attach_aspect_ratio_property(&connector->base.base);
 	connector->base.base.state->picture_aspect_ratio = HDMI_PICTURE_ASPECT_NONE;
@@ -2407,16 +2432,21 @@ intel_sdvo_add_hdmi_properties(struct intel_sdvo *intel_sdvo,
 static struct intel_sdvo_connector *intel_sdvo_connector_alloc(void)
 {
 	struct intel_sdvo_connector *sdvo_connector;
+	struct intel_sdvo_connector_state *conn_state;
 
 	sdvo_connector = kzalloc(sizeof(*sdvo_connector), GFP_KERNEL);
 	if (!sdvo_connector)
 		return NULL;
 
-	if (intel_connector_init(&sdvo_connector->base) < 0) {
+	conn_state = kzalloc(sizeof(*conn_state), GFP_KERNEL);
+	if (!conn_state) {
 		kfree(sdvo_connector);
 		return NULL;
 	}
 
+	__drm_atomic_helper_connector_reset(&sdvo_connector->base.base,
+					    &conn_state->base.base);
+
 	return sdvo_connector;
 }
 
@@ -2708,31 +2738,30 @@ static bool intel_sdvo_tv_create_property(struct intel_sdvo *intel_sdvo,
 				intel_sdvo_connector->tv_format, i,
 				i, tv_format_names[intel_sdvo_connector->tv_format_supported[i]]);
 
-	intel_sdvo->tv_format_index = intel_sdvo_connector->tv_format_supported[0];
-	drm_object_attach_property(&intel_sdvo_connector->base.base.base,
-				      intel_sdvo_connector->tv_format, 0);
+	intel_sdvo_connector->base.base.state->tv.mode = intel_sdvo_connector->tv_format_supported[0];
+	drm_object_attach_property(&intel_sdvo_connector->base.base.base, 0, 0);
 	return true;
 
 }
 
-#define ENHANCEMENT(name, NAME) do { \
+#define _ENHANCEMENT(state_assignment, name, NAME) do { \
 	if (enhancements.name) { \
 		if (!intel_sdvo_get_value(intel_sdvo, SDVO_CMD_GET_MAX_##NAME, &data_value, 4) || \
 		    !intel_sdvo_get_value(intel_sdvo, SDVO_CMD_GET_##NAME, &response, 2)) \
 			return false; \
-		intel_sdvo_connector->max_##name = data_value[0]; \
-		intel_sdvo_connector->cur_##name = response; \
 		intel_sdvo_connector->name = \
 			drm_property_create_range(dev, 0, #name, 0, data_value[0]); \
 		if (!intel_sdvo_connector->name) return false; \
+		state_assignment = response; \
 		drm_object_attach_property(&connector->base, \
-					      intel_sdvo_connector->name, \
-					      intel_sdvo_connector->cur_##name); \
+					   intel_sdvo_connector->name, 0); \
 		DRM_DEBUG_KMS(#name ": max %d, default %d, current %d\n", \
 			      data_value[0], data_value[1], response); \
 	} \
 } while (0)
 
+#define ENHANCEMENT(state, name, NAME) _ENHANCEMENT((state)->name, name, NAME)
+
 static bool
 intel_sdvo_create_enhance_property_tv(struct intel_sdvo *intel_sdvo,
 				      struct intel_sdvo_connector *intel_sdvo_connector,
@@ -2740,6 +2769,9 @@ intel_sdvo_create_enhance_property_tv(struct intel_sdvo *intel_sdvo,
 {
 	struct drm_device *dev = intel_sdvo->base.base.dev;
 	struct drm_connector *connector = &intel_sdvo_connector->base.base;
+	struct drm_connector_state *conn_state = connector->state;
+	struct intel_sdvo_connector_state *sdvo_state =
+		to_intel_sdvo_connector_state(conn_state);
 	uint16_t response, data_value[2];
 
 	/* when horizontal overscan is supported, Add the left/right  property */
@@ -2754,17 +2786,16 @@ intel_sdvo_create_enhance_property_tv(struct intel_sdvo *intel_sdvo,
 					  &response, 2))
 			return false;
 
+		sdvo_state->tv.overscan_h = response;
+
 		intel_sdvo_connector->max_hscan = data_value[0];
-		intel_sdvo_connector->left_margin = data_value[0] - response;
-		intel_sdvo_connector->right_margin = intel_sdvo_connector->left_margin;
 		intel_sdvo_connector->left =
 			drm_property_create_range(dev, 0, "left_margin", 0, data_value[0]);
 		if (!intel_sdvo_connector->left)
 			return false;
 
 		drm_object_attach_property(&connector->base,
-					      intel_sdvo_connector->left,
-					      intel_sdvo_connector->left_margin);
+					   intel_sdvo_connector->left, 0);
 
 		intel_sdvo_connector->right =
 			drm_property_create_range(dev, 0, "right_margin", 0, data_value[0]);
@@ -2772,8 +2803,7 @@ intel_sdvo_create_enhance_property_tv(struct intel_sdvo *intel_sdvo,
 			return false;
 
 		drm_object_attach_property(&connector->base,
-					      intel_sdvo_connector->right,
-					      intel_sdvo_connector->right_margin);
+					      intel_sdvo_connector->right, 0);
 		DRM_DEBUG_KMS("h_overscan: max %d, "
 			      "default %d, current %d\n",
 			      data_value[0], data_value[1], response);
@@ -2790,9 +2820,9 @@ intel_sdvo_create_enhance_property_tv(struct intel_sdvo *intel_sdvo,
 					  &response, 2))
 			return false;
 
+		sdvo_state->tv.overscan_v = response;
+
 		intel_sdvo_connector->max_vscan = data_value[0];
-		intel_sdvo_connector->top_margin = data_value[0] - response;
-		intel_sdvo_connector->bottom_margin = intel_sdvo_connector->top_margin;
 		intel_sdvo_connector->top =
 			drm_property_create_range(dev, 0,
 					    "top_margin", 0, data_value[0]);
@@ -2800,8 +2830,7 @@ intel_sdvo_create_enhance_property_tv(struct intel_sdvo *intel_sdvo,
 			return false;
 
 		drm_object_attach_property(&connector->base,
-					      intel_sdvo_connector->top,
-					      intel_sdvo_connector->top_margin);
+					   intel_sdvo_connector->top, 0);
 
 		intel_sdvo_connector->bottom =
 			drm_property_create_range(dev, 0,
@@ -2810,40 +2839,37 @@ intel_sdvo_create_enhance_property_tv(struct intel_sdvo *intel_sdvo,
 			return false;
 
 		drm_object_attach_property(&connector->base,
-					      intel_sdvo_connector->bottom,
-					      intel_sdvo_connector->bottom_margin);
+					      intel_sdvo_connector->bottom, 0);
 		DRM_DEBUG_KMS("v_overscan: max %d, "
 			      "default %d, current %d\n",
 			      data_value[0], data_value[1], response);
 	}
 
-	ENHANCEMENT(hpos, HPOS);
-	ENHANCEMENT(vpos, VPOS);
-	ENHANCEMENT(saturation, SATURATION);
-	ENHANCEMENT(contrast, CONTRAST);
-	ENHANCEMENT(hue, HUE);
-	ENHANCEMENT(sharpness, SHARPNESS);
-	ENHANCEMENT(brightness, BRIGHTNESS);
-	ENHANCEMENT(flicker_filter, FLICKER_FILTER);
-	ENHANCEMENT(flicker_filter_adaptive, FLICKER_FILTER_ADAPTIVE);
-	ENHANCEMENT(flicker_filter_2d, FLICKER_FILTER_2D);
-	ENHANCEMENT(tv_chroma_filter, TV_CHROMA_FILTER);
-	ENHANCEMENT(tv_luma_filter, TV_LUMA_FILTER);
+	ENHANCEMENT(&sdvo_state->tv, hpos, HPOS);
+	ENHANCEMENT(&sdvo_state->tv, vpos, VPOS);
+	ENHANCEMENT(&conn_state->tv, saturation, SATURATION);
+	ENHANCEMENT(&conn_state->tv, contrast, CONTRAST);
+	ENHANCEMENT(&conn_state->tv, hue, HUE);
+	ENHANCEMENT(&conn_state->tv, brightness, BRIGHTNESS);
+	ENHANCEMENT(&sdvo_state->tv, sharpness, SHARPNESS);
+	ENHANCEMENT(&sdvo_state->tv, flicker_filter, FLICKER_FILTER);
+	ENHANCEMENT(&sdvo_state->tv, flicker_filter_adaptive, FLICKER_FILTER_ADAPTIVE);
+	ENHANCEMENT(&sdvo_state->tv, flicker_filter_2d, FLICKER_FILTER_2D);
+	_ENHANCEMENT(sdvo_state->tv.chroma_filter, tv_chroma_filter, TV_CHROMA_FILTER);
+	_ENHANCEMENT(sdvo_state->tv.luma_filter, tv_luma_filter, TV_LUMA_FILTER);
 
 	if (enhancements.dot_crawl) {
 		if (!intel_sdvo_get_value(intel_sdvo, SDVO_CMD_GET_DOT_CRAWL, &response, 2))
 			return false;
 
-		intel_sdvo_connector->max_dot_crawl = 1;
-		intel_sdvo_connector->cur_dot_crawl = response & 0x1;
+		sdvo_state->tv.dot_crawl = response & 0x1;
 		intel_sdvo_connector->dot_crawl =
 			drm_property_create_range(dev, 0, "dot_crawl", 0, 1);
 		if (!intel_sdvo_connector->dot_crawl)
 			return false;
 
 		drm_object_attach_property(&connector->base,
-					      intel_sdvo_connector->dot_crawl,
-					      intel_sdvo_connector->cur_dot_crawl);
+					   intel_sdvo_connector->dot_crawl, 0);
 		DRM_DEBUG_KMS("dot crawl: current %d\n", response);
 	}
 
@@ -2859,11 +2885,12 @@ intel_sdvo_create_enhance_property_lvds(struct intel_sdvo *intel_sdvo,
 	struct drm_connector *connector = &intel_sdvo_connector->base.base;
 	uint16_t response, data_value[2];
 
-	ENHANCEMENT(brightness, BRIGHTNESS);
+	ENHANCEMENT(&connector->state->tv, brightness, BRIGHTNESS);
 
 	return true;
 }
 #undef ENHANCEMENT
+#undef _ENHANCEMENT
 
 static bool intel_sdvo_create_enhance_property(struct intel_sdvo *intel_sdvo,
 					       struct intel_sdvo_connector *intel_sdvo_connector)
@@ -2875,11 +2902,10 @@ static bool intel_sdvo_create_enhance_property(struct intel_sdvo *intel_sdvo,
 
 	BUILD_BUG_ON(sizeof(enhancements) != 2);
 
-	enhancements.response = 0;
-	intel_sdvo_get_value(intel_sdvo,
-			     SDVO_CMD_GET_SUPPORTED_ENHANCEMENTS,
-			     &enhancements, sizeof(enhancements));
-	if (enhancements.response == 0) {
+	if (!intel_sdvo_get_value(intel_sdvo,
+				  SDVO_CMD_GET_SUPPORTED_ENHANCEMENTS,
+				  &enhancements, sizeof(enhancements)) ||
+	    enhancements.response == 0) {
 		DRM_DEBUG_KMS("No enhancement is supported\n");
 		return true;
 	}
diff --git a/drivers/gpu/drm/i915/intel_sprite.c b/drivers/gpu/drm/i915/intel_sprite.c
index 191e14ddde0c..c4bf19364e49 100644
--- a/drivers/gpu/drm/i915/intel_sprite.c
+++ b/drivers/gpu/drm/i915/intel_sprite.c
@@ -210,16 +210,14 @@ void intel_pipe_update_end(struct intel_crtc *crtc, struct intel_flip_work *work
 }
 
 static void
-skl_update_plane(struct drm_plane *drm_plane,
+skl_update_plane(struct intel_plane *plane,
 		 const struct intel_crtc_state *crtc_state,
 		 const struct intel_plane_state *plane_state)
 {
-	struct drm_device *dev = drm_plane->dev;
-	struct drm_i915_private *dev_priv = to_i915(dev);
-	struct intel_plane *intel_plane = to_intel_plane(drm_plane);
-	struct drm_framebuffer *fb = plane_state->base.fb;
-	enum plane_id plane_id = intel_plane->id;
-	enum pipe pipe = intel_plane->pipe;
+	struct drm_i915_private *dev_priv = to_i915(plane->base.dev);
+	const struct drm_framebuffer *fb = plane_state->base.fb;
+	enum plane_id plane_id = plane->id;
+	enum pipe pipe = plane->pipe;
 	u32 plane_ctl = plane_state->ctl;
 	const struct drm_intel_sprite_colorkey *key = &plane_state->ckey;
 	u32 surf_addr = plane_state->main.offset;
@@ -288,13 +286,11 @@ skl_update_plane(struct drm_plane *drm_plane,
 }
 
 static void
-skl_disable_plane(struct drm_plane *dplane, struct drm_crtc *crtc)
+skl_disable_plane(struct intel_plane *plane, struct intel_crtc *crtc)
 {
-	struct drm_device *dev = dplane->dev;
-	struct drm_i915_private *dev_priv = to_i915(dev);
-	struct intel_plane *intel_plane = to_intel_plane(dplane);
-	enum plane_id plane_id = intel_plane->id;
-	enum pipe pipe = intel_plane->pipe;
+	struct drm_i915_private *dev_priv = to_i915(plane->base.dev);
+	enum plane_id plane_id = plane->id;
+	enum pipe pipe = plane->pipe;
 	unsigned long irqflags;
 
 	spin_lock_irqsave(&dev_priv->uncore.lock, irqflags);
@@ -308,10 +304,10 @@ skl_disable_plane(struct drm_plane *dplane, struct drm_crtc *crtc)
 }
 
 static void
-chv_update_csc(struct intel_plane *intel_plane, uint32_t format)
+chv_update_csc(struct intel_plane *plane, uint32_t format)
 {
-	struct drm_i915_private *dev_priv = to_i915(intel_plane->base.dev);
-	enum plane_id plane_id = intel_plane->id;
+	struct drm_i915_private *dev_priv = to_i915(plane->base.dev);
+	enum plane_id plane_id = plane->id;
 
 	/* Seems RGB data bypasses the CSC always */
 	if (!format_is_yuv(format))
@@ -411,16 +407,14 @@ static u32 vlv_sprite_ctl(const struct intel_crtc_state *crtc_state,
 }
 
 static void
-vlv_update_plane(struct drm_plane *dplane,
+vlv_update_plane(struct intel_plane *plane,
 		 const struct intel_crtc_state *crtc_state,
 		 const struct intel_plane_state *plane_state)
 {
-	struct drm_device *dev = dplane->dev;
-	struct drm_i915_private *dev_priv = to_i915(dev);
-	struct intel_plane *intel_plane = to_intel_plane(dplane);
-	struct drm_framebuffer *fb = plane_state->base.fb;
-	enum pipe pipe = intel_plane->pipe;
-	enum plane_id plane_id = intel_plane->id;
+	struct drm_i915_private *dev_priv = to_i915(plane->base.dev);
+	const struct drm_framebuffer *fb = plane_state->base.fb;
+	enum pipe pipe = plane->pipe;
+	enum plane_id plane_id = plane->id;
 	u32 sprctl = plane_state->ctl;
 	u32 sprsurf_offset = plane_state->main.offset;
 	u32 linear_offset;
@@ -442,7 +436,7 @@ vlv_update_plane(struct drm_plane *dplane,
 	spin_lock_irqsave(&dev_priv->uncore.lock, irqflags);
 
 	if (IS_CHERRYVIEW(dev_priv) && pipe == PIPE_B)
-		chv_update_csc(intel_plane, fb->format->format);
+		chv_update_csc(plane, fb->format->format);
 
 	if (key->flags) {
 		I915_WRITE_FW(SPKEYMINVAL(pipe, plane_id), key->min_value);
@@ -469,13 +463,11 @@ vlv_update_plane(struct drm_plane *dplane,
 }
 
 static void
-vlv_disable_plane(struct drm_plane *dplane, struct drm_crtc *crtc)
+vlv_disable_plane(struct intel_plane *plane, struct intel_crtc *crtc)
 {
-	struct drm_device *dev = dplane->dev;
-	struct drm_i915_private *dev_priv = to_i915(dev);
-	struct intel_plane *intel_plane = to_intel_plane(dplane);
-	enum pipe pipe = intel_plane->pipe;
-	enum plane_id plane_id = intel_plane->id;
+	struct drm_i915_private *dev_priv = to_i915(plane->base.dev);
+	enum pipe pipe = plane->pipe;
+	enum plane_id plane_id = plane->id;
 	unsigned long irqflags;
 
 	spin_lock_irqsave(&dev_priv->uncore.lock, irqflags);
@@ -545,15 +537,13 @@ static u32 ivb_sprite_ctl(const struct intel_crtc_state *crtc_state,
 }
 
 static void
-ivb_update_plane(struct drm_plane *plane,
+ivb_update_plane(struct intel_plane *plane,
 		 const struct intel_crtc_state *crtc_state,
 		 const struct intel_plane_state *plane_state)
 {
-	struct drm_device *dev = plane->dev;
-	struct drm_i915_private *dev_priv = to_i915(dev);
-	struct intel_plane *intel_plane = to_intel_plane(plane);
-	struct drm_framebuffer *fb = plane_state->base.fb;
-	enum pipe pipe = intel_plane->pipe;
+	struct drm_i915_private *dev_priv = to_i915(plane->base.dev);
+	const struct drm_framebuffer *fb = plane_state->base.fb;
+	enum pipe pipe = plane->pipe;
 	u32 sprctl = plane_state->ctl, sprscale = 0;
 	u32 sprsurf_offset = plane_state->main.offset;
 	u32 linear_offset;
@@ -600,7 +590,7 @@ ivb_update_plane(struct drm_plane *plane,
 		I915_WRITE_FW(SPRLINOFF(pipe), linear_offset);
 
 	I915_WRITE_FW(SPRSIZE(pipe), (crtc_h << 16) | crtc_w);
-	if (intel_plane->can_scale)
+	if (plane->can_scale)
 		I915_WRITE_FW(SPRSCALE(pipe), sprscale);
 	I915_WRITE_FW(SPRCTL(pipe), sprctl);
 	I915_WRITE_FW(SPRSURF(pipe),
@@ -611,19 +601,17 @@ ivb_update_plane(struct drm_plane *plane,
 }
 
 static void
-ivb_disable_plane(struct drm_plane *plane, struct drm_crtc *crtc)
+ivb_disable_plane(struct intel_plane *plane, struct intel_crtc *crtc)
 {
-	struct drm_device *dev = plane->dev;
-	struct drm_i915_private *dev_priv = to_i915(dev);
-	struct intel_plane *intel_plane = to_intel_plane(plane);
-	int pipe = intel_plane->pipe;
+	struct drm_i915_private *dev_priv = to_i915(plane->base.dev);
+	enum pipe pipe = plane->pipe;
 	unsigned long irqflags;
 
 	spin_lock_irqsave(&dev_priv->uncore.lock, irqflags);
 
 	I915_WRITE_FW(SPRCTL(pipe), 0);
 	/* Can't leave the scaler enabled... */
-	if (intel_plane->can_scale)
+	if (plane->can_scale)
 		I915_WRITE_FW(SPRSCALE(pipe), 0);
 
 	I915_WRITE_FW(SPRSURF(pipe), 0);
@@ -632,7 +620,7 @@ ivb_disable_plane(struct drm_plane *plane, struct drm_crtc *crtc)
 	spin_unlock_irqrestore(&dev_priv->uncore.lock, irqflags);
 }
 
-static u32 ilk_sprite_ctl(const struct intel_crtc_state *crtc_state,
+static u32 g4x_sprite_ctl(const struct intel_crtc_state *crtc_state,
 			  const struct intel_plane_state *plane_state)
 {
 	struct drm_i915_private *dev_priv =
@@ -686,15 +674,13 @@ static u32 ilk_sprite_ctl(const struct intel_crtc_state *crtc_state,
 }
 
 static void
-ilk_update_plane(struct drm_plane *plane,
+g4x_update_plane(struct intel_plane *plane,
 		 const struct intel_crtc_state *crtc_state,
 		 const struct intel_plane_state *plane_state)
 {
-	struct drm_device *dev = plane->dev;
-	struct drm_i915_private *dev_priv = to_i915(dev);
-	struct intel_plane *intel_plane = to_intel_plane(plane);
-	struct drm_framebuffer *fb = plane_state->base.fb;
-	int pipe = intel_plane->pipe;
+	struct drm_i915_private *dev_priv = to_i915(plane->base.dev);
+	const struct drm_framebuffer *fb = plane_state->base.fb;
+	enum pipe pipe = plane->pipe;
 	u32 dvscntr = plane_state->ctl, dvsscale = 0;
 	u32 dvssurf_offset = plane_state->main.offset;
 	u32 linear_offset;
@@ -747,12 +733,10 @@ ilk_update_plane(struct drm_plane *plane,
 }
 
 static void
-ilk_disable_plane(struct drm_plane *plane, struct drm_crtc *crtc)
+g4x_disable_plane(struct intel_plane *plane, struct intel_crtc *crtc)
 {
-	struct drm_device *dev = plane->dev;
-	struct drm_i915_private *dev_priv = to_i915(dev);
-	struct intel_plane *intel_plane = to_intel_plane(plane);
-	int pipe = intel_plane->pipe;
+	struct drm_i915_private *dev_priv = to_i915(plane->base.dev);
+	enum pipe pipe = plane->pipe;
 	unsigned long irqflags;
 
 	spin_lock_irqsave(&dev_priv->uncore.lock, irqflags);
@@ -768,14 +752,12 @@ ilk_disable_plane(struct drm_plane *plane, struct drm_crtc *crtc)
 }
 
 static int
-intel_check_sprite_plane(struct drm_plane *plane,
+intel_check_sprite_plane(struct intel_plane *plane,
 			 struct intel_crtc_state *crtc_state,
 			 struct intel_plane_state *state)
 {
-	struct drm_i915_private *dev_priv = to_i915(plane->dev);
-	struct drm_crtc *crtc = state->base.crtc;
-	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
-	struct intel_plane *intel_plane = to_intel_plane(plane);
+	struct drm_i915_private *dev_priv = to_i915(plane->base.dev);
+	struct intel_crtc *crtc = to_intel_crtc(crtc_state->base.crtc);
 	struct drm_framebuffer *fb = state->base.fb;
 	int crtc_x, crtc_y;
 	unsigned int crtc_w, crtc_h;
@@ -797,7 +779,7 @@ intel_check_sprite_plane(struct drm_plane *plane,
 	}
 
 	/* Don't modify another pipe's plane */
-	if (intel_plane->pipe != intel_crtc->pipe) {
+	if (plane->pipe != crtc->pipe) {
 		DRM_DEBUG_KMS("Wrong plane <-> crtc mapping\n");
 		return -EINVAL;
 	}
@@ -814,16 +796,16 @@ intel_check_sprite_plane(struct drm_plane *plane,
 		if (state->ckey.flags == I915_SET_COLORKEY_NONE) {
 			can_scale = 1;
 			min_scale = 1;
-			max_scale = skl_max_scale(intel_crtc, crtc_state);
+			max_scale = skl_max_scale(crtc, crtc_state);
 		} else {
 			can_scale = 0;
 			min_scale = DRM_PLANE_HELPER_NO_SCALING;
 			max_scale = DRM_PLANE_HELPER_NO_SCALING;
 		}
 	} else {
-		can_scale = intel_plane->can_scale;
-		max_scale = intel_plane->max_downscale << 16;
-		min_scale = intel_plane->can_scale ? 1 : (1 << 16);
+		can_scale = plane->can_scale;
+		max_scale = plane->max_downscale << 16;
+		min_scale = plane->can_scale ? 1 : (1 << 16);
 	}
 
 	/*
@@ -967,7 +949,7 @@ intel_check_sprite_plane(struct drm_plane *plane,
 		if (ret)
 			return ret;
 
-		state->ctl = ilk_sprite_ctl(crtc_state, state);
+		state->ctl = g4x_sprite_ctl(crtc_state, state);
 	}
 
 	return 0;
@@ -1027,7 +1009,7 @@ out:
 	return ret;
 }
 
-static const uint32_t ilk_plane_formats[] = {
+static const uint32_t g4x_plane_formats[] = {
 	DRM_FORMAT_XRGB8888,
 	DRM_FORMAT_YUYV,
 	DRM_FORMAT_YVYU,
@@ -1131,15 +1113,15 @@ intel_sprite_plane_create(struct drm_i915_private *dev_priv,
 		intel_plane->can_scale = true;
 		intel_plane->max_downscale = 16;
 
-		intel_plane->update_plane = ilk_update_plane;
-		intel_plane->disable_plane = ilk_disable_plane;
+		intel_plane->update_plane = g4x_update_plane;
+		intel_plane->disable_plane = g4x_disable_plane;
 
 		if (IS_GEN6(dev_priv)) {
 			plane_formats = snb_plane_formats;
 			num_plane_formats = ARRAY_SIZE(snb_plane_formats);
 		} else {
-			plane_formats = ilk_plane_formats;
-			num_plane_formats = ARRAY_SIZE(ilk_plane_formats);
+			plane_formats = g4x_plane_formats;
+			num_plane_formats = ARRAY_SIZE(g4x_plane_formats);
 		}
 	}
 
diff --git a/drivers/gpu/drm/i915/intel_tv.c b/drivers/gpu/drm/i915/intel_tv.c
index e077c2a9e694..784df024e230 100644
--- a/drivers/gpu/drm/i915/intel_tv.c
+++ b/drivers/gpu/drm/i915/intel_tv.c
@@ -48,41 +48,6 @@ struct intel_tv {
 	struct intel_encoder base;
 
 	int type;
-	const char *tv_format;
-	int margin[4];
-	u32 save_TV_H_CTL_1;
-	u32 save_TV_H_CTL_2;
-	u32 save_TV_H_CTL_3;
-	u32 save_TV_V_CTL_1;
-	u32 save_TV_V_CTL_2;
-	u32 save_TV_V_CTL_3;
-	u32 save_TV_V_CTL_4;
-	u32 save_TV_V_CTL_5;
-	u32 save_TV_V_CTL_6;
-	u32 save_TV_V_CTL_7;
-	u32 save_TV_SC_CTL_1, save_TV_SC_CTL_2, save_TV_SC_CTL_3;
-
-	u32 save_TV_CSC_Y;
-	u32 save_TV_CSC_Y2;
-	u32 save_TV_CSC_U;
-	u32 save_TV_CSC_U2;
-	u32 save_TV_CSC_V;
-	u32 save_TV_CSC_V2;
-	u32 save_TV_CLR_KNOBS;
-	u32 save_TV_CLR_LEVEL;
-	u32 save_TV_WIN_POS;
-	u32 save_TV_WIN_SIZE;
-	u32 save_TV_FILTER_CTL_1;
-	u32 save_TV_FILTER_CTL_2;
-	u32 save_TV_FILTER_CTL_3;
-
-	u32 save_TV_H_LUMA[60];
-	u32 save_TV_H_CHROMA[60];
-	u32 save_TV_V_LUMA[43];
-	u32 save_TV_V_CHROMA[43];
-
-	u32 save_TV_DAC;
-	u32 save_TV_CTL;
 };
 
 struct video_levels {
@@ -873,32 +838,18 @@ intel_disable_tv(struct intel_encoder *encoder,
 	I915_WRITE(TV_CTL, I915_READ(TV_CTL) & ~TV_ENC_ENABLE);
 }
 
-static const struct tv_mode *
-intel_tv_mode_lookup(const char *tv_format)
+static const struct tv_mode *intel_tv_mode_find(struct drm_connector_state *conn_state)
 {
-	int i;
-
-	for (i = 0; i < ARRAY_SIZE(tv_modes); i++) {
-		const struct tv_mode *tv_mode = &tv_modes[i];
+	int format = conn_state->tv.mode;
 
-		if (!strcmp(tv_format, tv_mode->name))
-			return tv_mode;
-	}
-	return NULL;
-}
-
-static const struct tv_mode *
-intel_tv_mode_find(struct intel_tv *intel_tv)
-{
-	return intel_tv_mode_lookup(intel_tv->tv_format);
+	return &tv_modes[format];
 }
 
 static enum drm_mode_status
 intel_tv_mode_valid(struct drm_connector *connector,
 		    struct drm_display_mode *mode)
 {
-	struct intel_tv *intel_tv = intel_attached_tv(connector);
-	const struct tv_mode *tv_mode = intel_tv_mode_find(intel_tv);
+	const struct tv_mode *tv_mode = intel_tv_mode_find(connector->state);
 	int max_dotclk = to_i915(connector->dev)->max_dotclk_freq;
 
 	if (mode->clock > max_dotclk)
@@ -925,8 +876,7 @@ intel_tv_compute_config(struct intel_encoder *encoder,
 			struct intel_crtc_state *pipe_config,
 			struct drm_connector_state *conn_state)
 {
-	struct intel_tv *intel_tv = enc_to_tv(encoder);
-	const struct tv_mode *tv_mode = intel_tv_mode_find(intel_tv);
+	const struct tv_mode *tv_mode = intel_tv_mode_find(conn_state);
 
 	if (!tv_mode)
 		return false;
@@ -1032,7 +982,7 @@ static void intel_tv_pre_enable(struct intel_encoder *encoder,
 	struct drm_i915_private *dev_priv = to_i915(encoder->base.dev);
 	struct intel_crtc *intel_crtc = to_intel_crtc(encoder->base.crtc);
 	struct intel_tv *intel_tv = enc_to_tv(encoder);
-	const struct tv_mode *tv_mode = intel_tv_mode_find(intel_tv);
+	const struct tv_mode *tv_mode = intel_tv_mode_find(conn_state);
 	u32 tv_ctl;
 	u32 scctl1, scctl2, scctl3;
 	int i, j;
@@ -1135,12 +1085,12 @@ static void intel_tv_pre_enable(struct intel_encoder *encoder,
 	else
 		ysize = 2*tv_mode->nbr_end + 1;
 
-	xpos += intel_tv->margin[TV_MARGIN_LEFT];
-	ypos += intel_tv->margin[TV_MARGIN_TOP];
-	xsize -= (intel_tv->margin[TV_MARGIN_LEFT] +
-		  intel_tv->margin[TV_MARGIN_RIGHT]);
-	ysize -= (intel_tv->margin[TV_MARGIN_TOP] +
-		  intel_tv->margin[TV_MARGIN_BOTTOM]);
+	xpos += conn_state->tv.margins.left;
+	ypos += conn_state->tv.margins.top;
+	xsize -= (conn_state->tv.margins.left +
+		  conn_state->tv.margins.right);
+	ysize -= (conn_state->tv.margins.top +
+		  conn_state->tv.margins.bottom);
 	I915_WRITE(TV_WIN_POS, (xpos<<16)|ypos);
 	I915_WRITE(TV_WIN_SIZE, (xsize<<16)|ysize);
 
@@ -1288,7 +1238,7 @@ intel_tv_detect_type(struct intel_tv *intel_tv,
 static void intel_tv_find_better_format(struct drm_connector *connector)
 {
 	struct intel_tv *intel_tv = intel_attached_tv(connector);
-	const struct tv_mode *tv_mode = intel_tv_mode_find(intel_tv);
+	const struct tv_mode *tv_mode = intel_tv_mode_find(connector->state);
 	int i;
 
 	if ((intel_tv->type == DRM_MODE_CONNECTOR_Component) ==
@@ -1304,9 +1254,7 @@ static void intel_tv_find_better_format(struct drm_connector *connector)
 			break;
 	}
 
-	intel_tv->tv_format = tv_mode->name;
-	drm_object_property_set_value(&connector->base,
-		connector->dev->mode_config.tv_mode_property, i);
+	connector->state->tv.mode = i;
 }
 
 /**
@@ -1347,16 +1295,15 @@ intel_tv_detect(struct drm_connector *connector,
 				connector_status_connected;
 		} else
 			status = connector_status_unknown;
-	} else
-		return connector->status;
 
-	if (status != connector_status_connected)
-		return status;
-
-	intel_tv->type = type;
-	intel_tv_find_better_format(connector);
+		if (status == connector_status_connected) {
+			intel_tv->type = type;
+			intel_tv_find_better_format(connector);
+		}
 
-	return connector_status_connected;
+		return status;
+	} else
+		return connector->status;
 }
 
 static const struct input_res {
@@ -1376,12 +1323,9 @@ static const struct input_res {
  * Chose preferred mode  according to line number of TV format
  */
 static void
-intel_tv_chose_preferred_modes(struct drm_connector *connector,
+intel_tv_choose_preferred_modes(const struct tv_mode *tv_mode,
 			       struct drm_display_mode *mode_ptr)
 {
-	struct intel_tv *intel_tv = intel_attached_tv(connector);
-	const struct tv_mode *tv_mode = intel_tv_mode_find(intel_tv);
-
 	if (tv_mode->nbr_end < 480 && mode_ptr->vdisplay == 480)
 		mode_ptr->type |= DRM_MODE_TYPE_PREFERRED;
 	else if (tv_mode->nbr_end > 480) {
@@ -1404,8 +1348,7 @@ static int
 intel_tv_get_modes(struct drm_connector *connector)
 {
 	struct drm_display_mode *mode_ptr;
-	struct intel_tv *intel_tv = intel_attached_tv(connector);
-	const struct tv_mode *tv_mode = intel_tv_mode_find(intel_tv);
+	const struct tv_mode *tv_mode = intel_tv_mode_find(connector->state);
 	int j, count = 0;
 	u64 tmp;
 
@@ -1448,7 +1391,7 @@ intel_tv_get_modes(struct drm_connector *connector)
 		mode_ptr->clock = (int) tmp;
 
 		mode_ptr->type = DRM_MODE_TYPE_DRIVER;
-		intel_tv_chose_preferred_modes(connector, mode_ptr);
+		intel_tv_choose_preferred_modes(tv_mode, mode_ptr);
 		drm_mode_probed_add(connector, mode_ptr);
 		count++;
 	}
@@ -1463,74 +1406,47 @@ intel_tv_destroy(struct drm_connector *connector)
 	kfree(connector);
 }
 
-
-static int
-intel_tv_set_property(struct drm_connector *connector, struct drm_property *property,
-		      uint64_t val)
-{
-	struct drm_device *dev = connector->dev;
-	struct intel_tv *intel_tv = intel_attached_tv(connector);
-	struct drm_crtc *crtc = intel_tv->base.base.crtc;
-	int ret = 0;
-	bool changed = false;
-
-	ret = drm_object_property_set_value(&connector->base, property, val);
-	if (ret < 0)
-		goto out;
-
-	if (property == dev->mode_config.tv_left_margin_property &&
-		intel_tv->margin[TV_MARGIN_LEFT] != val) {
-		intel_tv->margin[TV_MARGIN_LEFT] = val;
-		changed = true;
-	} else if (property == dev->mode_config.tv_right_margin_property &&
-		intel_tv->margin[TV_MARGIN_RIGHT] != val) {
-		intel_tv->margin[TV_MARGIN_RIGHT] = val;
-		changed = true;
-	} else if (property == dev->mode_config.tv_top_margin_property &&
-		intel_tv->margin[TV_MARGIN_TOP] != val) {
-		intel_tv->margin[TV_MARGIN_TOP] = val;
-		changed = true;
-	} else if (property == dev->mode_config.tv_bottom_margin_property &&
-		intel_tv->margin[TV_MARGIN_BOTTOM] != val) {
-		intel_tv->margin[TV_MARGIN_BOTTOM] = val;
-		changed = true;
-	} else if (property == dev->mode_config.tv_mode_property) {
-		if (val >= ARRAY_SIZE(tv_modes)) {
-			ret = -EINVAL;
-			goto out;
-		}
-		if (!strcmp(intel_tv->tv_format, tv_modes[val].name))
-			goto out;
-
-		intel_tv->tv_format = tv_modes[val].name;
-		changed = true;
-	} else {
-		ret = -EINVAL;
-		goto out;
-	}
-
-	if (changed && crtc)
-		intel_crtc_restore_mode(crtc);
-out:
-	return ret;
-}
-
 static const struct drm_connector_funcs intel_tv_connector_funcs = {
 	.dpms = drm_atomic_helper_connector_dpms,
 	.late_register = intel_connector_register,
 	.early_unregister = intel_connector_unregister,
 	.destroy = intel_tv_destroy,
-	.set_property = intel_tv_set_property,
-	.atomic_get_property = intel_connector_atomic_get_property,
+	.set_property = drm_atomic_helper_connector_set_property,
 	.fill_modes = drm_helper_probe_single_connector_modes,
 	.atomic_destroy_state = drm_atomic_helper_connector_destroy_state,
 	.atomic_duplicate_state = drm_atomic_helper_connector_duplicate_state,
 };
 
+static int intel_tv_atomic_check(struct drm_connector *connector,
+				 struct drm_connector_state *new_state)
+{
+	struct drm_crtc_state *new_crtc_state;
+	struct drm_connector_state *old_state;
+
+	if (!new_state->crtc)
+		return 0;
+
+	old_state = drm_atomic_get_old_connector_state(new_state->state, connector);
+	new_crtc_state = drm_atomic_get_new_crtc_state(new_state->state, new_state->crtc);
+
+	if (old_state->tv.mode != new_state->tv.mode ||
+	    old_state->tv.margins.left != new_state->tv.margins.left ||
+	    old_state->tv.margins.right != new_state->tv.margins.right ||
+	    old_state->tv.margins.top != new_state->tv.margins.top ||
+	    old_state->tv.margins.bottom != new_state->tv.margins.bottom) {
+		/* Force a modeset. */
+
+		new_crtc_state->connectors_changed = true;
+	}
+
+	return 0;
+}
+
 static const struct drm_connector_helper_funcs intel_tv_connector_helper_funcs = {
 	.detect_ctx = intel_tv_detect,
 	.mode_valid = intel_tv_mode_valid,
 	.get_modes = intel_tv_get_modes,
+	.atomic_check = intel_tv_atomic_check,
 };
 
 static const struct drm_encoder_funcs intel_tv_enc_funcs = {
@@ -1548,6 +1464,7 @@ intel_tv_init(struct drm_i915_private *dev_priv)
 	u32 tv_dac_on, tv_dac_off, save_tv_dac;
 	const char *tv_format_names[ARRAY_SIZE(tv_modes)];
 	int i, initial_mode = 0;
+	struct drm_connector_state *state;
 
 	if ((I915_READ(TV_CTL) & TV_FUSE_STATE_MASK) == TV_FUSE_STATE_DISABLED)
 		return;
@@ -1593,6 +1510,7 @@ intel_tv_init(struct drm_i915_private *dev_priv)
 
 	intel_encoder = &intel_tv->base;
 	connector = &intel_connector->base;
+	state = connector->state;
 
 	/* The documentation, for the older chipsets at least, recommend
 	 * using a polling method rather than hotplug detection for TVs.
@@ -1630,12 +1548,12 @@ intel_tv_init(struct drm_i915_private *dev_priv)
 	intel_tv->type = DRM_MODE_CONNECTOR_Unknown;
 
 	/* BIOS margin values */
-	intel_tv->margin[TV_MARGIN_LEFT] = 54;
-	intel_tv->margin[TV_MARGIN_TOP] = 36;
-	intel_tv->margin[TV_MARGIN_RIGHT] = 46;
-	intel_tv->margin[TV_MARGIN_BOTTOM] = 37;
+	state->tv.margins.left = 54;
+	state->tv.margins.top = 36;
+	state->tv.margins.right = 46;
+	state->tv.margins.bottom = 37;
 
-	intel_tv->tv_format = tv_modes[initial_mode].name;
+	state->tv.mode = initial_mode;
 
 	drm_connector_helper_add(connector, &intel_tv_connector_helper_funcs);
 	connector->interlace_allowed = false;
@@ -1649,17 +1567,17 @@ intel_tv_init(struct drm_i915_private *dev_priv)
 				      tv_format_names);
 
 	drm_object_attach_property(&connector->base, dev->mode_config.tv_mode_property,
-				   initial_mode);
+				   state->tv.mode);
 	drm_object_attach_property(&connector->base,
 				   dev->mode_config.tv_left_margin_property,
-				   intel_tv->margin[TV_MARGIN_LEFT]);
+				   state->tv.margins.left);
 	drm_object_attach_property(&connector->base,
 				   dev->mode_config.tv_top_margin_property,
-				   intel_tv->margin[TV_MARGIN_TOP]);
+				   state->tv.margins.top);
 	drm_object_attach_property(&connector->base,
 				   dev->mode_config.tv_right_margin_property,
-				   intel_tv->margin[TV_MARGIN_RIGHT]);
+				   state->tv.margins.right);
 	drm_object_attach_property(&connector->base,
 				   dev->mode_config.tv_bottom_margin_property,
-				   intel_tv->margin[TV_MARGIN_BOTTOM]);
+				   state->tv.margins.bottom);
 }
diff --git a/drivers/gpu/drm/i915/intel_uc.c b/drivers/gpu/drm/i915/intel_uc.c
index c117424f1f50..7a7b07de28a3 100644
--- a/drivers/gpu/drm/i915/intel_uc.c
+++ b/drivers/gpu/drm/i915/intel_uc.c
@@ -94,12 +94,22 @@ void intel_uc_sanitize_options(struct drm_i915_private *dev_priv)
 		i915.enable_guc_submission = HAS_GUC_SCHED(dev_priv);
 }
 
+static void guc_write_irq_trigger(struct intel_guc *guc)
+{
+	struct drm_i915_private *dev_priv = guc_to_i915(guc);
+
+	I915_WRITE(GUC_SEND_INTERRUPT, GUC_SEND_TRIGGER);
+}
+
 void intel_uc_init_early(struct drm_i915_private *dev_priv)
 {
 	struct intel_guc *guc = &dev_priv->guc;
 
+	intel_guc_ct_init_early(&guc->ct);
+
 	mutex_init(&guc->send_mutex);
-	guc->send = intel_guc_send_mmio;
+	guc->send = intel_guc_send_nop;
+	guc->notify = guc_write_irq_trigger;
 }
 
 static void fetch_uc_fw(struct drm_i915_private *dev_priv,
@@ -252,13 +262,81 @@ void intel_uc_fini_fw(struct drm_i915_private *dev_priv)
 	__intel_uc_fw_fini(&dev_priv->huc.fw);
 }
 
+static inline i915_reg_t guc_send_reg(struct intel_guc *guc, u32 i)
+{
+	GEM_BUG_ON(!guc->send_regs.base);
+	GEM_BUG_ON(!guc->send_regs.count);
+	GEM_BUG_ON(i >= guc->send_regs.count);
+
+	return _MMIO(guc->send_regs.base + 4 * i);
+}
+
+static void guc_init_send_regs(struct intel_guc *guc)
+{
+	struct drm_i915_private *dev_priv = guc_to_i915(guc);
+	enum forcewake_domains fw_domains = 0;
+	unsigned int i;
+
+	guc->send_regs.base = i915_mmio_reg_offset(SOFT_SCRATCH(0));
+	guc->send_regs.count = SOFT_SCRATCH_COUNT - 1;
+
+	for (i = 0; i < guc->send_regs.count; i++) {
+		fw_domains |= intel_uncore_forcewake_for_reg(dev_priv,
+					guc_send_reg(guc, i),
+					FW_REG_READ | FW_REG_WRITE);
+	}
+	guc->send_regs.fw_domains = fw_domains;
+}
+
+static void guc_capture_load_err_log(struct intel_guc *guc)
+{
+	if (!guc->log.vma || i915.guc_log_level < 0)
+		return;
+
+	if (!guc->load_err_log)
+		guc->load_err_log = i915_gem_object_get(guc->log.vma->obj);
+
+	return;
+}
+
+static void guc_free_load_err_log(struct intel_guc *guc)
+{
+	if (guc->load_err_log)
+		i915_gem_object_put(guc->load_err_log);
+}
+
+static int guc_enable_communication(struct intel_guc *guc)
+{
+	struct drm_i915_private *dev_priv = guc_to_i915(guc);
+
+	guc_init_send_regs(guc);
+
+	if (HAS_GUC_CT(dev_priv))
+		return intel_guc_enable_ct(guc);
+
+	guc->send = intel_guc_send_mmio;
+	return 0;
+}
+
+static void guc_disable_communication(struct intel_guc *guc)
+{
+	struct drm_i915_private *dev_priv = guc_to_i915(guc);
+
+	if (HAS_GUC_CT(dev_priv))
+		intel_guc_disable_ct(guc);
+
+	guc->send = intel_guc_send_nop;
+}
+
 int intel_uc_init_hw(struct drm_i915_private *dev_priv)
 {
+	struct intel_guc *guc = &dev_priv->guc;
 	int ret, attempts;
 
 	if (!i915.enable_guc_loading)
 		return 0;
 
+	guc_disable_communication(guc);
 	gen9_reset_guc_interrupts(dev_priv);
 
 	/* We need to notify the guc whenever we change the GGTT */
@@ -274,6 +352,11 @@ int intel_uc_init_hw(struct drm_i915_private *dev_priv)
 			goto err_guc;
 	}
 
+	/* init WOPCM */
+	I915_WRITE(GUC_WOPCM_SIZE, intel_guc_wopcm_size(dev_priv));
+	I915_WRITE(DMA_GUC_WOPCM_OFFSET,
+		   GUC_WOPCM_OFFSET_VALUE | HUC_LOADING_AGENT_GUC);
+
 	/* WaEnableuKernelHeaderValidFix:skl */
 	/* WaEnableGuCBootHashCheckNotSet:skl,bxt,kbl */
 	if (IS_GEN9(dev_priv))
@@ -301,7 +384,11 @@ int intel_uc_init_hw(struct drm_i915_private *dev_priv)
 
 	/* Did we succeded or run out of retries? */
 	if (ret)
-		goto err_submission;
+		goto err_log_capture;
+
+	ret = guc_enable_communication(guc);
+	if (ret)
+		goto err_log_capture;
 
 	intel_guc_auth_huc(dev_priv);
 	if (i915.enable_guc_submission) {
@@ -325,7 +412,10 @@ int intel_uc_init_hw(struct drm_i915_private *dev_priv)
 	 * marks the GPU as wedged until reset).
 	 */
 err_interrupts:
+	guc_disable_communication(guc);
 	gen9_disable_guc_interrupts(dev_priv);
+err_log_capture:
+	guc_capture_load_err_log(guc);
 err_submission:
 	if (i915.enable_guc_submission)
 		i915_guc_submission_fini(dev_priv);
@@ -351,25 +441,25 @@ void intel_uc_fini_hw(struct drm_i915_private *dev_priv)
 	if (!i915.enable_guc_loading)
 		return;
 
-	if (i915.enable_guc_submission) {
+	guc_free_load_err_log(&dev_priv->guc);
+
+	if (i915.enable_guc_submission)
 		i915_guc_submission_disable(dev_priv);
+
+	guc_disable_communication(&dev_priv->guc);
+
+	if (i915.enable_guc_submission) {
 		gen9_disable_guc_interrupts(dev_priv);
 		i915_guc_submission_fini(dev_priv);
 	}
+
 	i915_ggtt_disable_guc(dev_priv);
 }
 
-/*
- * Read GuC command/status register (SOFT_SCRATCH_0)
- * Return true if it contains a response rather than a command
- */
-static bool guc_recv(struct intel_guc *guc, u32 *status)
+int intel_guc_send_nop(struct intel_guc *guc, const u32 *action, u32 len)
 {
-	struct drm_i915_private *dev_priv = guc_to_i915(guc);
-
-	u32 val = I915_READ(SOFT_SCRATCH(0));
-	*status = val;
-	return INTEL_GUC_RECV_IS_RESPONSE(val);
+	WARN(1, "Unexpected send: action=%#x\n", *action);
+	return -ENODEV;
 }
 
 /*
@@ -382,30 +472,33 @@ int intel_guc_send_mmio(struct intel_guc *guc, const u32 *action, u32 len)
 	int i;
 	int ret;
 
-	if (WARN_ON(len < 1 || len > 15))
-		return -EINVAL;
+	GEM_BUG_ON(!len);
+	GEM_BUG_ON(len > guc->send_regs.count);
 
-	mutex_lock(&guc->send_mutex);
-	intel_uncore_forcewake_get(dev_priv, FORCEWAKE_BLITTER);
+	/* If CT is available, we expect to use MMIO only during init/fini */
+	GEM_BUG_ON(HAS_GUC_CT(dev_priv) &&
+		*action != INTEL_GUC_ACTION_REGISTER_COMMAND_TRANSPORT_BUFFER &&
+		*action != INTEL_GUC_ACTION_DEREGISTER_COMMAND_TRANSPORT_BUFFER);
 
-	dev_priv->guc.action_count += 1;
-	dev_priv->guc.action_cmd = action[0];
+	mutex_lock(&guc->send_mutex);
+	intel_uncore_forcewake_get(dev_priv, guc->send_regs.fw_domains);
 
 	for (i = 0; i < len; i++)
-		I915_WRITE(SOFT_SCRATCH(i), action[i]);
+		I915_WRITE(guc_send_reg(guc, i), action[i]);
 
-	POSTING_READ(SOFT_SCRATCH(i - 1));
+	POSTING_READ(guc_send_reg(guc, i - 1));
 
-	I915_WRITE(GUC_SEND_INTERRUPT, GUC_SEND_TRIGGER);
+	intel_guc_notify(guc);
 
 	/*
-	 * Fast commands should complete in less than 10us, so sample quickly
-	 * up to that length of time, then switch to a slower sleep-wait loop.
-	 * No inte_guc_send command should ever take longer than 10ms.
+	 * No GuC command should ever take longer than 10ms.
+	 * Fast commands should still complete in 10us.
 	 */
-	ret = wait_for_us(guc_recv(guc, &status), 10);
-	if (ret)
-		ret = wait_for(guc_recv(guc, &status), 10);
+	ret = __intel_wait_for_register_fw(dev_priv,
+					   guc_send_reg(guc, 0),
+					   INTEL_GUC_RECV_MASK,
+					   INTEL_GUC_RECV_MASK,
+					   10, 10, &status);
 	if (status != INTEL_GUC_STATUS_SUCCESS) {
 		/*
 		 * Either the GuC explicitly returned an error (which
@@ -418,13 +511,9 @@ int intel_guc_send_mmio(struct intel_guc *guc, const u32 *action, u32 len)
 		DRM_WARN("INTEL_GUC_SEND: Action 0x%X failed;"
 			 " ret=%d status=0x%08X response=0x%08X\n",
 			 action[0], ret, status, I915_READ(SOFT_SCRATCH(15)));
-
-		dev_priv->guc.action_fail += 1;
-		dev_priv->guc.action_err = ret;
 	}
-	dev_priv->guc.action_status = status;
 
-	intel_uncore_forcewake_put(dev_priv, FORCEWAKE_BLITTER);
+	intel_uncore_forcewake_put(dev_priv, guc->send_regs.fw_domains);
 	mutex_unlock(&guc->send_mutex);
 
 	return ret;
diff --git a/drivers/gpu/drm/i915/intel_uc.h b/drivers/gpu/drm/i915/intel_uc.h
index 4b7f73aeddac..69daf4c01cd0 100644
--- a/drivers/gpu/drm/i915/intel_uc.h
+++ b/drivers/gpu/drm/i915/intel_uc.h
@@ -27,7 +27,7 @@
 #include "intel_guc_fwif.h"
 #include "i915_guc_reg.h"
 #include "intel_ringbuffer.h"
-
+#include "intel_guc_ct.h"
 #include "i915_vma.h"
 
 struct drm_i915_gem_request;
@@ -59,12 +59,6 @@ struct drm_i915_gem_request;
  *                available in the work queue (note, the queue is shared,
  *                not per-engine). It is OK for this to be nonzero, but
  *                it should not be huge!
- *   q_fail: failed to enqueue a work item. This should never happen,
- *           because we check for space beforehand.
- *   b_fail: failed to ring the doorbell. This should never happen, unless
- *           somehow the hardware misbehaves, or maybe if the GuC firmware
- *           crashes? We probably need to reset the GPU to recover.
- *   retcode: errno from last guc_submit()
  */
 struct i915_guc_client {
 	struct i915_vma *vma;
@@ -87,8 +81,6 @@ struct i915_guc_client {
 	uint32_t wq_tail;
 	uint32_t wq_rsvd;
 	uint32_t no_wq_space;
-	uint32_t b_fail;
-	int retcode;
 
 	/* Per-engine counts of GuC submissions */
 	uint64_t submissions[I915_NUM_ENGINES];
@@ -181,6 +173,10 @@ struct intel_guc_log {
 struct intel_guc {
 	struct intel_uc_fw fw;
 	struct intel_guc_log log;
+	struct intel_guc_ct ct;
+
+	/* Log snapshot if GuC errors during load */
+	struct drm_i915_gem_object *load_err_log;
 
 	/* intel_guc_recv interrupt related state */
 	bool interrupts_enabled;
@@ -195,21 +191,21 @@ struct intel_guc {
 	DECLARE_BITMAP(doorbell_bitmap, GUC_NUM_DOORBELLS);
 	uint32_t db_cacheline;		/* Cyclic counter mod pagesize	*/
 
-	/* Action status & statistics */
-	uint64_t action_count;		/* Total commands issued	*/
-	uint32_t action_cmd;		/* Last command word		*/
-	uint32_t action_status;		/* Last return status		*/
-	uint32_t action_fail;		/* Total number of failures	*/
-	int32_t action_err;		/* Last error code		*/
-
-	uint64_t submissions[I915_NUM_ENGINES];
-	uint32_t last_seqno[I915_NUM_ENGINES];
+	/* GuC's FW specific registers used in MMIO send */
+	struct {
+		u32 base;
+		unsigned int count;
+		enum forcewake_domains fw_domains;
+	} send_regs;
 
 	/* To serialize the intel_guc_send actions */
 	struct mutex send_mutex;
 
 	/* GuC's FW specific send function */
 	int (*send)(struct intel_guc *guc, const u32 *data, u32 len);
+
+	/* GuC's FW specific notify function */
+	void (*notify)(struct intel_guc *guc);
 };
 
 struct intel_huc {
@@ -227,12 +223,19 @@ void intel_uc_fini_fw(struct drm_i915_private *dev_priv);
 int intel_uc_init_hw(struct drm_i915_private *dev_priv);
 void intel_uc_fini_hw(struct drm_i915_private *dev_priv);
 int intel_guc_sample_forcewake(struct intel_guc *guc);
+int intel_guc_send_nop(struct intel_guc *guc, const u32 *action, u32 len);
 int intel_guc_send_mmio(struct intel_guc *guc, const u32 *action, u32 len);
+
 static inline int intel_guc_send(struct intel_guc *guc, const u32 *action, u32 len)
 {
 	return guc->send(guc, action, len);
 }
 
+static inline void intel_guc_notify(struct intel_guc *guc)
+{
+	guc->notify(guc);
+}
+
 /* intel_guc_loader.c */
 int intel_guc_select_fw(struct intel_guc *guc);
 int intel_guc_init_hw(struct intel_guc *guc);
@@ -266,7 +269,7 @@ static inline u32 guc_ggtt_offset(struct i915_vma *vma)
 
 /* intel_huc.c */
 void intel_huc_select_fw(struct intel_huc *huc);
-int intel_huc_init_hw(struct intel_huc *huc);
+void intel_huc_init_hw(struct intel_huc *huc);
 void intel_guc_auth_huc(struct drm_i915_private *dev_priv);
 
 #endif
diff --git a/drivers/gpu/drm/i915/intel_uncore.c b/drivers/gpu/drm/i915/intel_uncore.c
index 6d1ea26b2493..9882724bc2b6 100644
--- a/drivers/gpu/drm/i915/intel_uncore.c
+++ b/drivers/gpu/drm/i915/intel_uncore.c
@@ -29,6 +29,7 @@
 #include <linux/pm_runtime.h>
 
 #define FORCEWAKE_ACK_TIMEOUT_MS 50
+#define GT_FIFO_TIMEOUT_MS	 10
 
 #define __raw_posting_read(dev_priv__, reg__) (void)__raw_i915_read32((dev_priv__), (reg__))
 
@@ -172,22 +173,6 @@ static void fw_domains_get_with_thread_status(struct drm_i915_private *dev_priv,
 	__gen6_gt_wait_for_thread_c0(dev_priv);
 }
 
-static void gen6_gt_check_fifodbg(struct drm_i915_private *dev_priv)
-{
-	u32 gtfifodbg;
-
-	gtfifodbg = __raw_i915_read32(dev_priv, GTFIFODBG);
-	if (WARN(gtfifodbg, "GT wake FIFO error 0x%x\n", gtfifodbg))
-		__raw_i915_write32(dev_priv, GTFIFODBG, gtfifodbg);
-}
-
-static void fw_domains_put_with_fifo(struct drm_i915_private *dev_priv,
-				     enum forcewake_domains fw_domains)
-{
-	fw_domains_put(dev_priv, fw_domains);
-	gen6_gt_check_fifodbg(dev_priv);
-}
-
 static inline u32 fifo_free_entries(struct drm_i915_private *dev_priv)
 {
 	u32 count = __raw_i915_read32(dev_priv, GTFIFOCTL);
@@ -195,30 +180,27 @@ static inline u32 fifo_free_entries(struct drm_i915_private *dev_priv)
 	return count & GT_FIFO_FREE_ENTRIES_MASK;
 }
 
-static int __gen6_gt_wait_for_fifo(struct drm_i915_private *dev_priv)
+static void __gen6_gt_wait_for_fifo(struct drm_i915_private *dev_priv)
 {
-	int ret = 0;
+	u32 n;
 
 	/* On VLV, FIFO will be shared by both SW and HW.
 	 * So, we need to read the FREE_ENTRIES everytime */
 	if (IS_VALLEYVIEW(dev_priv))
-		dev_priv->uncore.fifo_count = fifo_free_entries(dev_priv);
-
-	if (dev_priv->uncore.fifo_count < GT_FIFO_NUM_RESERVED_ENTRIES) {
-		int loop = 500;
-		u32 fifo = fifo_free_entries(dev_priv);
-
-		while (fifo <= GT_FIFO_NUM_RESERVED_ENTRIES && loop--) {
-			udelay(10);
-			fifo = fifo_free_entries(dev_priv);
+		n = fifo_free_entries(dev_priv);
+	else
+		n = dev_priv->uncore.fifo_count;
+
+	if (n <= GT_FIFO_NUM_RESERVED_ENTRIES) {
+		if (wait_for_atomic((n = fifo_free_entries(dev_priv)) >
+				    GT_FIFO_NUM_RESERVED_ENTRIES,
+				    GT_FIFO_TIMEOUT_MS)) {
+			DRM_DEBUG("GT_FIFO timeout, entries: %u\n", n);
+			return;
 		}
-		if (WARN_ON(loop < 0 && fifo <= GT_FIFO_NUM_RESERVED_ENTRIES))
-			++ret;
-		dev_priv->uncore.fifo_count = fifo;
 	}
-	dev_priv->uncore.fifo_count--;
 
-	return ret;
+	dev_priv->uncore.fifo_count = n - 1;
 }
 
 static enum hrtimer_restart
@@ -232,6 +214,9 @@ intel_uncore_fw_release_timer(struct hrtimer *timer)
 
 	assert_rpm_device_not_suspended(dev_priv);
 
+	if (xchg(&domain->active, false))
+		return HRTIMER_RESTART;
+
 	spin_lock_irqsave(&dev_priv->uncore.lock, irqflags);
 	if (WARN_ON(domain->wake_count == 0))
 		domain->wake_count++;
@@ -262,6 +247,7 @@ static void intel_uncore_forcewake_reset(struct drm_i915_private *dev_priv,
 		active_domains = 0;
 
 		for_each_fw_domain(domain, dev_priv, tmp) {
+			smp_store_mb(domain->active, false);
 			if (hrtimer_cancel(&domain->timer) == 0)
 				continue;
 
@@ -384,31 +370,44 @@ vlv_check_for_unclaimed_mmio(struct drm_i915_private *dev_priv)
 }
 
 static bool
+gen6_check_for_fifo_debug(struct drm_i915_private *dev_priv)
+{
+	u32 fifodbg;
+
+	fifodbg = __raw_i915_read32(dev_priv, GTFIFODBG);
+
+	if (unlikely(fifodbg)) {
+		DRM_DEBUG_DRIVER("GTFIFODBG = 0x08%x\n", fifodbg);
+		__raw_i915_write32(dev_priv, GTFIFODBG, fifodbg);
+	}
+
+	return fifodbg;
+}
+
+static bool
 check_for_unclaimed_mmio(struct drm_i915_private *dev_priv)
 {
+	bool ret = false;
+
 	if (HAS_FPGA_DBG_UNCLAIMED(dev_priv))
-		return fpga_check_for_unclaimed_mmio(dev_priv);
+		ret |= fpga_check_for_unclaimed_mmio(dev_priv);
 
 	if (IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv))
-		return vlv_check_for_unclaimed_mmio(dev_priv);
+		ret |= vlv_check_for_unclaimed_mmio(dev_priv);
 
-	return false;
+	if (IS_GEN6(dev_priv) || IS_GEN7(dev_priv))
+		ret |= gen6_check_for_fifo_debug(dev_priv);
+
+	return ret;
 }
 
 static void __intel_uncore_early_sanitize(struct drm_i915_private *dev_priv,
 					  bool restore_forcewake)
 {
-	struct intel_device_info *info = mkwrite_device_info(dev_priv);
-
 	/* clear out unclaimed reg detection bit */
 	if (check_for_unclaimed_mmio(dev_priv))
 		DRM_DEBUG("unclaimed mmio detected on uncore init, clearing\n");
 
-	/* clear out old GT FIFO errors */
-	if (IS_GEN6(dev_priv) || IS_GEN7(dev_priv))
-		__raw_i915_write32(dev_priv, GTFIFODBG,
-				   __raw_i915_read32(dev_priv, GTFIFODBG));
-
 	/* WaDisableShadowRegForCpd:chv */
 	if (IS_CHERRYVIEW(dev_priv)) {
 		__raw_i915_write32(dev_priv, GTFIFOCTL,
@@ -417,9 +416,6 @@ static void __intel_uncore_early_sanitize(struct drm_i915_private *dev_priv,
 				   GT_FIFO_CTL_RC6_POLICY_STALL);
 	}
 
-	if (IS_BXT_REVID(dev_priv, 0, BXT_REVID_B_LAST))
-		info->has_decoupled_mmio = false;
-
 	intel_uncore_forcewake_reset(dev_priv, restore_forcewake);
 }
 
@@ -454,9 +450,12 @@ static void __intel_uncore_forcewake_get(struct drm_i915_private *dev_priv,
 
 	fw_domains &= dev_priv->uncore.fw_domains;
 
-	for_each_fw_domain_masked(domain, fw_domains, dev_priv, tmp)
-		if (domain->wake_count++)
+	for_each_fw_domain_masked(domain, fw_domains, dev_priv, tmp) {
+		if (domain->wake_count++) {
 			fw_domains &= ~domain->mask;
+			domain->active = true;
+		}
+	}
 
 	if (fw_domains)
 		dev_priv->uncore.funcs.force_wake_get(dev_priv, fw_domains);
@@ -521,8 +520,10 @@ static void __intel_uncore_forcewake_put(struct drm_i915_private *dev_priv,
 		if (WARN_ON(domain->wake_count == 0))
 			continue;
 
-		if (--domain->wake_count)
+		if (--domain->wake_count) {
+			domain->active = true;
 			continue;
+		}
 
 		fw_domain_arm_timer(domain);
 	}
@@ -804,66 +805,6 @@ unclaimed_reg_debug(struct drm_i915_private *dev_priv,
 	__unclaimed_reg_debug(dev_priv, reg, read, before);
 }
 
-static const enum decoupled_power_domain fw2dpd_domain[] = {
-	GEN9_DECOUPLED_PD_RENDER,
-	GEN9_DECOUPLED_PD_BLITTER,
-	GEN9_DECOUPLED_PD_ALL,
-	GEN9_DECOUPLED_PD_MEDIA,
-	GEN9_DECOUPLED_PD_ALL,
-	GEN9_DECOUPLED_PD_ALL,
-	GEN9_DECOUPLED_PD_ALL
-};
-
-/*
- * Decoupled MMIO access for only 1 DWORD
- */
-static void __gen9_decoupled_mmio_access(struct drm_i915_private *dev_priv,
-					 u32 reg,
-					 enum forcewake_domains fw_domain,
-					 enum decoupled_ops operation)
-{
-	enum decoupled_power_domain dp_domain;
-	u32 ctrl_reg_data = 0;
-
-	dp_domain = fw2dpd_domain[fw_domain - 1];
-
-	ctrl_reg_data |= reg;
-	ctrl_reg_data |= (operation << GEN9_DECOUPLED_OP_SHIFT);
-	ctrl_reg_data |= (dp_domain << GEN9_DECOUPLED_PD_SHIFT);
-	ctrl_reg_data |= GEN9_DECOUPLED_DW1_GO;
-	__raw_i915_write32(dev_priv, GEN9_DECOUPLED_REG0_DW1, ctrl_reg_data);
-
-	if (wait_for_atomic((__raw_i915_read32(dev_priv,
-			    GEN9_DECOUPLED_REG0_DW1) &
-			    GEN9_DECOUPLED_DW1_GO) == 0,
-			    FORCEWAKE_ACK_TIMEOUT_MS))
-		DRM_ERROR("Decoupled MMIO wait timed out\n");
-}
-
-static inline u32
-__gen9_decoupled_mmio_read32(struct drm_i915_private *dev_priv,
-			     u32 reg,
-			     enum forcewake_domains fw_domain)
-{
-	__gen9_decoupled_mmio_access(dev_priv, reg, fw_domain,
-				     GEN9_DECOUPLED_OP_READ);
-
-	return __raw_i915_read32(dev_priv, GEN9_DECOUPLED_REG0_DW0);
-}
-
-static inline void
-__gen9_decoupled_mmio_write(struct drm_i915_private *dev_priv,
-			    u32 reg, u32 data,
-			    enum forcewake_domains fw_domain)
-{
-
-	__raw_i915_write32(dev_priv, GEN9_DECOUPLED_REG0_DW0, data);
-
-	__gen9_decoupled_mmio_access(dev_priv, reg, fw_domain,
-				     GEN9_DECOUPLED_OP_WRITE);
-}
-
-
 #define GEN2_READ_HEADER(x) \
 	u##x val = 0; \
 	assert_rpm_wakelock_held(dev_priv);
@@ -960,28 +901,6 @@ func##_read##x(struct drm_i915_private *dev_priv, i915_reg_t reg, bool trace) {
 #define __gen6_read(x) __gen_read(gen6, x)
 #define __fwtable_read(x) __gen_read(fwtable, x)
 
-#define __gen9_decoupled_read(x) \
-static u##x \
-gen9_decoupled_read##x(struct drm_i915_private *dev_priv, \
-		       i915_reg_t reg, bool trace) { \
-	enum forcewake_domains fw_engine; \
-	GEN6_READ_HEADER(x); \
-	fw_engine = __fwtable_reg_read_fw_domains(offset); \
-	if (fw_engine & ~dev_priv->uncore.fw_domains_active) { \
-		unsigned i; \
-		u32 *ptr_data = (u32 *) &val; \
-		for (i = 0; i < x/32; i++, offset += sizeof(u32), ptr_data++) \
-			*ptr_data = __gen9_decoupled_mmio_read32(dev_priv, \
-								 offset, \
-								 fw_engine); \
-	} else { \
-		val = __raw_i915_read##x(dev_priv, reg); \
-	} \
-	GEN6_READ_FOOTER; \
-}
-
-__gen9_decoupled_read(32)
-__gen9_decoupled_read(64)
 __fwtable_read(8)
 __fwtable_read(16)
 __fwtable_read(32)
@@ -1047,15 +966,10 @@ __gen2_write(32)
 #define __gen6_write(x) \
 static void \
 gen6_write##x(struct drm_i915_private *dev_priv, i915_reg_t reg, u##x val, bool trace) { \
-	u32 __fifo_ret = 0; \
 	GEN6_WRITE_HEADER; \
-	if (NEEDS_FORCE_WAKE(offset)) { \
-		__fifo_ret = __gen6_gt_wait_for_fifo(dev_priv); \
-	} \
+	if (NEEDS_FORCE_WAKE(offset)) \
+		__gen6_gt_wait_for_fifo(dev_priv); \
 	__raw_i915_write##x(dev_priv, reg, val); \
-	if (unlikely(__fifo_ret)) { \
-		gen6_gt_check_fifodbg(dev_priv); \
-	} \
 	GEN6_WRITE_FOOTER; \
 }
 
@@ -1073,25 +987,6 @@ func##_write##x(struct drm_i915_private *dev_priv, i915_reg_t reg, u##x val, boo
 #define __gen8_write(x) __gen_write(gen8, x)
 #define __fwtable_write(x) __gen_write(fwtable, x)
 
-#define __gen9_decoupled_write(x) \
-static void \
-gen9_decoupled_write##x(struct drm_i915_private *dev_priv, \
-			i915_reg_t reg, u##x val, \
-		bool trace) { \
-	enum forcewake_domains fw_engine; \
-	GEN6_WRITE_HEADER; \
-	fw_engine = __fwtable_reg_write_fw_domains(offset); \
-	if (fw_engine & ~dev_priv->uncore.fw_domains_active) \
-		__gen9_decoupled_mmio_write(dev_priv, \
-					    offset, \
-					    val, \
-					    fw_engine); \
-	else \
-		__raw_i915_write##x(dev_priv, reg, val); \
-	GEN6_WRITE_FOOTER; \
-}
-
-__gen9_decoupled_write(32)
 __fwtable_write(8)
 __fwtable_write(16)
 __fwtable_write(32)
@@ -1108,19 +1003,19 @@ __gen6_write(32)
 #undef GEN6_WRITE_FOOTER
 #undef GEN6_WRITE_HEADER
 
-#define ASSIGN_WRITE_MMIO_VFUNCS(x) \
+#define ASSIGN_WRITE_MMIO_VFUNCS(i915, x) \
 do { \
-	dev_priv->uncore.funcs.mmio_writeb = x##_write8; \
-	dev_priv->uncore.funcs.mmio_writew = x##_write16; \
-	dev_priv->uncore.funcs.mmio_writel = x##_write32; \
+	(i915)->uncore.funcs.mmio_writeb = x##_write8; \
+	(i915)->uncore.funcs.mmio_writew = x##_write16; \
+	(i915)->uncore.funcs.mmio_writel = x##_write32; \
 } while (0)
 
-#define ASSIGN_READ_MMIO_VFUNCS(x) \
+#define ASSIGN_READ_MMIO_VFUNCS(i915, x) \
 do { \
-	dev_priv->uncore.funcs.mmio_readb = x##_read8; \
-	dev_priv->uncore.funcs.mmio_readw = x##_read16; \
-	dev_priv->uncore.funcs.mmio_readl = x##_read32; \
-	dev_priv->uncore.funcs.mmio_readq = x##_read64; \
+	(i915)->uncore.funcs.mmio_readb = x##_read8; \
+	(i915)->uncore.funcs.mmio_readw = x##_read16; \
+	(i915)->uncore.funcs.mmio_readl = x##_read32; \
+	(i915)->uncore.funcs.mmio_readq = x##_read64; \
 } while (0)
 
 
@@ -1190,11 +1085,7 @@ static void intel_uncore_fw_domains_init(struct drm_i915_private *dev_priv)
 			       FORCEWAKE_MEDIA_GEN9, FORCEWAKE_ACK_MEDIA_GEN9);
 	} else if (IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv)) {
 		dev_priv->uncore.funcs.force_wake_get = fw_domains_get;
-		if (!IS_CHERRYVIEW(dev_priv))
-			dev_priv->uncore.funcs.force_wake_put =
-				fw_domains_put_with_fifo;
-		else
-			dev_priv->uncore.funcs.force_wake_put = fw_domains_put;
+		dev_priv->uncore.funcs.force_wake_put = fw_domains_put;
 		fw_domain_init(dev_priv, FW_DOMAIN_ID_RENDER,
 			       FORCEWAKE_VLV, FORCEWAKE_ACK_VLV);
 		fw_domain_init(dev_priv, FW_DOMAIN_ID_MEDIA,
@@ -1202,11 +1093,7 @@ static void intel_uncore_fw_domains_init(struct drm_i915_private *dev_priv)
 	} else if (IS_HASWELL(dev_priv) || IS_BROADWELL(dev_priv)) {
 		dev_priv->uncore.funcs.force_wake_get =
 			fw_domains_get_with_thread_status;
-		if (IS_HASWELL(dev_priv))
-			dev_priv->uncore.funcs.force_wake_put =
-				fw_domains_put_with_fifo;
-		else
-			dev_priv->uncore.funcs.force_wake_put = fw_domains_put;
+		dev_priv->uncore.funcs.force_wake_put = fw_domains_put;
 		fw_domain_init(dev_priv, FW_DOMAIN_ID_RENDER,
 			       FORCEWAKE_MT, FORCEWAKE_ACK_HSW);
 	} else if (IS_IVYBRIDGE(dev_priv)) {
@@ -1223,8 +1110,7 @@ static void intel_uncore_fw_domains_init(struct drm_i915_private *dev_priv)
 		 */
 		dev_priv->uncore.funcs.force_wake_get =
 			fw_domains_get_with_thread_status;
-		dev_priv->uncore.funcs.force_wake_put =
-			fw_domains_put_with_fifo;
+		dev_priv->uncore.funcs.force_wake_put = fw_domains_put;
 
 		/* We need to init first for ECOBUS access and then
 		 * determine later if we want to reinit, in case of MT access is
@@ -1242,7 +1128,7 @@ static void intel_uncore_fw_domains_init(struct drm_i915_private *dev_priv)
 		spin_lock_irq(&dev_priv->uncore.lock);
 		fw_domains_get_with_thread_status(dev_priv, FORCEWAKE_RENDER);
 		ecobus = __raw_i915_read32(dev_priv, ECOBUS);
-		fw_domains_put_with_fifo(dev_priv, FORCEWAKE_RENDER);
+		fw_domains_put(dev_priv, FORCEWAKE_RENDER);
 		spin_unlock_irq(&dev_priv->uncore.lock);
 
 		if (!(ecobus & FORCEWAKE_MT_ENABLE)) {
@@ -1254,8 +1140,7 @@ static void intel_uncore_fw_domains_init(struct drm_i915_private *dev_priv)
 	} else if (IS_GEN6(dev_priv)) {
 		dev_priv->uncore.funcs.force_wake_get =
 			fw_domains_get_with_thread_status;
-		dev_priv->uncore.funcs.force_wake_put =
-			fw_domains_put_with_fifo;
+		dev_priv->uncore.funcs.force_wake_put = fw_domains_put;
 		fw_domain_init(dev_priv, FW_DOMAIN_ID_RENDER,
 			       FORCEWAKE, FORCEWAKE_ACK);
 	}
@@ -1310,42 +1195,34 @@ void intel_uncore_init(struct drm_i915_private *dev_priv)
 		i915_pmic_bus_access_notifier;
 
 	if (IS_GEN(dev_priv, 2, 4) || intel_vgpu_active(dev_priv)) {
-		ASSIGN_WRITE_MMIO_VFUNCS(gen2);
-		ASSIGN_READ_MMIO_VFUNCS(gen2);
+		ASSIGN_WRITE_MMIO_VFUNCS(dev_priv, gen2);
+		ASSIGN_READ_MMIO_VFUNCS(dev_priv, gen2);
 	} else if (IS_GEN5(dev_priv)) {
-		ASSIGN_WRITE_MMIO_VFUNCS(gen5);
-		ASSIGN_READ_MMIO_VFUNCS(gen5);
+		ASSIGN_WRITE_MMIO_VFUNCS(dev_priv, gen5);
+		ASSIGN_READ_MMIO_VFUNCS(dev_priv, gen5);
 	} else if (IS_GEN(dev_priv, 6, 7)) {
-		ASSIGN_WRITE_MMIO_VFUNCS(gen6);
+		ASSIGN_WRITE_MMIO_VFUNCS(dev_priv, gen6);
 
 		if (IS_VALLEYVIEW(dev_priv)) {
 			ASSIGN_FW_DOMAINS_TABLE(__vlv_fw_ranges);
-			ASSIGN_READ_MMIO_VFUNCS(fwtable);
+			ASSIGN_READ_MMIO_VFUNCS(dev_priv, fwtable);
 		} else {
-			ASSIGN_READ_MMIO_VFUNCS(gen6);
+			ASSIGN_READ_MMIO_VFUNCS(dev_priv, gen6);
 		}
 	} else if (IS_GEN8(dev_priv)) {
 		if (IS_CHERRYVIEW(dev_priv)) {
 			ASSIGN_FW_DOMAINS_TABLE(__chv_fw_ranges);
-			ASSIGN_WRITE_MMIO_VFUNCS(fwtable);
-			ASSIGN_READ_MMIO_VFUNCS(fwtable);
+			ASSIGN_WRITE_MMIO_VFUNCS(dev_priv, fwtable);
+			ASSIGN_READ_MMIO_VFUNCS(dev_priv, fwtable);
 
 		} else {
-			ASSIGN_WRITE_MMIO_VFUNCS(gen8);
-			ASSIGN_READ_MMIO_VFUNCS(gen6);
+			ASSIGN_WRITE_MMIO_VFUNCS(dev_priv, gen8);
+			ASSIGN_READ_MMIO_VFUNCS(dev_priv, gen6);
 		}
 	} else {
 		ASSIGN_FW_DOMAINS_TABLE(__gen9_fw_ranges);
-		ASSIGN_WRITE_MMIO_VFUNCS(fwtable);
-		ASSIGN_READ_MMIO_VFUNCS(fwtable);
-		if (HAS_DECOUPLED_MMIO(dev_priv)) {
-			dev_priv->uncore.funcs.mmio_readl =
-						gen9_decoupled_read32;
-			dev_priv->uncore.funcs.mmio_readq =
-						gen9_decoupled_read64;
-			dev_priv->uncore.funcs.mmio_writel =
-						gen9_decoupled_write32;
-		}
+		ASSIGN_WRITE_MMIO_VFUNCS(dev_priv, fwtable);
+		ASSIGN_READ_MMIO_VFUNCS(dev_priv, fwtable);
 	}
 
 	iosf_mbi_register_pmic_bus_access_notifier(
@@ -1353,8 +1230,6 @@ void intel_uncore_init(struct drm_i915_private *dev_priv)
 
 	i915_check_and_clear_faults(dev_priv);
 }
-#undef ASSIGN_WRITE_MMIO_VFUNCS
-#undef ASSIGN_READ_MMIO_VFUNCS
 
 void intel_uncore_fini(struct drm_i915_private *dev_priv)
 {
@@ -1435,9 +1310,39 @@ out:
 	return ret;
 }
 
-static int i915_reset_complete(struct pci_dev *pdev)
+static void gen3_stop_rings(struct drm_i915_private *dev_priv)
+{
+	struct intel_engine_cs *engine;
+	enum intel_engine_id id;
+
+	for_each_engine(engine, dev_priv, id) {
+		const u32 base = engine->mmio_base;
+		const i915_reg_t mode = RING_MI_MODE(base);
+
+		I915_WRITE_FW(mode, _MASKED_BIT_ENABLE(STOP_RING));
+		if (intel_wait_for_register_fw(dev_priv,
+					       mode,
+					       MODE_IDLE,
+					       MODE_IDLE,
+					       500))
+			DRM_DEBUG_DRIVER("%s: timed out on STOP_RING\n",
+					 engine->name);
+
+		I915_WRITE_FW(RING_CTL(base), 0);
+		I915_WRITE_FW(RING_HEAD(base), 0);
+		I915_WRITE_FW(RING_TAIL(base), 0);
+
+		/* Check acts as a post */
+		if (I915_READ_FW(RING_HEAD(base)) != 0)
+			DRM_DEBUG_DRIVER("%s: ring head not parked\n",
+					 engine->name);
+	}
+}
+
+static bool i915_reset_complete(struct pci_dev *pdev)
 {
 	u8 gdrst;
+
 	pci_read_config_byte(pdev, I915_GDRST, &gdrst);
 	return (gdrst & GRDOM_RESET_STATUS) == 0;
 }
@@ -1448,15 +1353,16 @@ static int i915_do_reset(struct drm_i915_private *dev_priv, unsigned engine_mask
 
 	/* assert reset for at least 20 usec */
 	pci_write_config_byte(pdev, I915_GDRST, GRDOM_RESET_ENABLE);
-	udelay(20);
+	usleep_range(50, 200);
 	pci_write_config_byte(pdev, I915_GDRST, 0);
 
 	return wait_for(i915_reset_complete(pdev), 500);
 }
 
-static int g4x_reset_complete(struct pci_dev *pdev)
+static bool g4x_reset_complete(struct pci_dev *pdev)
 {
 	u8 gdrst;
+
 	pci_read_config_byte(pdev, I915_GDRST, &gdrst);
 	return (gdrst & GRDOM_RESET_ENABLE) == 0;
 }
@@ -1464,6 +1370,10 @@ static int g4x_reset_complete(struct pci_dev *pdev)
 static int g33_do_reset(struct drm_i915_private *dev_priv, unsigned engine_mask)
 {
 	struct pci_dev *pdev = dev_priv->drm.pdev;
+
+	/* Stop engines before we reset; see g4x_do_reset() below for why. */
+	gen3_stop_rings(dev_priv);
+
 	pci_write_config_byte(pdev, I915_GDRST, GRDOM_RESET_ENABLE);
 	return wait_for(g4x_reset_complete(pdev), 500);
 }
@@ -1473,29 +1383,41 @@ static int g4x_do_reset(struct drm_i915_private *dev_priv, unsigned engine_mask)
 	struct pci_dev *pdev = dev_priv->drm.pdev;
 	int ret;
 
-	pci_write_config_byte(pdev, I915_GDRST,
-			      GRDOM_RENDER | GRDOM_RESET_ENABLE);
-	ret =  wait_for(g4x_reset_complete(pdev), 500);
-	if (ret)
-		return ret;
-
 	/* WaVcpClkGateDisableForMediaReset:ctg,elk */
-	I915_WRITE(VDECCLK_GATE_D, I915_READ(VDECCLK_GATE_D) | VCP_UNIT_CLOCK_GATE_DISABLE);
+	I915_WRITE(VDECCLK_GATE_D,
+		   I915_READ(VDECCLK_GATE_D) | VCP_UNIT_CLOCK_GATE_DISABLE);
 	POSTING_READ(VDECCLK_GATE_D);
 
+	/* We stop engines, otherwise we might get failed reset and a
+	 * dead gpu (on elk).
+	 * WaMediaResetMainRingCleanup:ctg,elk (presumably)
+	 */
+	gen3_stop_rings(dev_priv);
+
 	pci_write_config_byte(pdev, I915_GDRST,
 			      GRDOM_MEDIA | GRDOM_RESET_ENABLE);
 	ret =  wait_for(g4x_reset_complete(pdev), 500);
-	if (ret)
-		return ret;
+	if (ret) {
+		DRM_DEBUG_DRIVER("Wait for media reset failed\n");
+		goto out;
+	}
 
-	/* WaVcpClkGateDisableForMediaReset:ctg,elk */
-	I915_WRITE(VDECCLK_GATE_D, I915_READ(VDECCLK_GATE_D) & ~VCP_UNIT_CLOCK_GATE_DISABLE);
-	POSTING_READ(VDECCLK_GATE_D);
+	pci_write_config_byte(pdev, I915_GDRST,
+			      GRDOM_RENDER | GRDOM_RESET_ENABLE);
+	ret =  wait_for(g4x_reset_complete(pdev), 500);
+	if (ret) {
+		DRM_DEBUG_DRIVER("Wait for render reset failed\n");
+		goto out;
+	}
 
+out:
 	pci_write_config_byte(pdev, I915_GDRST, 0);
 
-	return 0;
+	I915_WRITE(VDECCLK_GATE_D,
+		   I915_READ(VDECCLK_GATE_D) & ~VCP_UNIT_CLOCK_GATE_DISABLE);
+	POSTING_READ(VDECCLK_GATE_D);
+
+	return ret;
 }
 
 static int ironlake_do_reset(struct drm_i915_private *dev_priv,
@@ -1503,41 +1425,51 @@ static int ironlake_do_reset(struct drm_i915_private *dev_priv,
 {
 	int ret;
 
-	I915_WRITE(ILK_GDSR,
-		   ILK_GRDOM_RENDER | ILK_GRDOM_RESET_ENABLE);
+	I915_WRITE(ILK_GDSR, ILK_GRDOM_RENDER | ILK_GRDOM_RESET_ENABLE);
 	ret = intel_wait_for_register(dev_priv,
 				      ILK_GDSR, ILK_GRDOM_RESET_ENABLE, 0,
 				      500);
-	if (ret)
-		return ret;
+	if (ret) {
+		DRM_DEBUG_DRIVER("Wait for render reset failed\n");
+		goto out;
+	}
 
-	I915_WRITE(ILK_GDSR,
-		   ILK_GRDOM_MEDIA | ILK_GRDOM_RESET_ENABLE);
+	I915_WRITE(ILK_GDSR, ILK_GRDOM_MEDIA | ILK_GRDOM_RESET_ENABLE);
 	ret = intel_wait_for_register(dev_priv,
 				      ILK_GDSR, ILK_GRDOM_RESET_ENABLE, 0,
 				      500);
-	if (ret)
-		return ret;
+	if (ret) {
+		DRM_DEBUG_DRIVER("Wait for media reset failed\n");
+		goto out;
+	}
 
+out:
 	I915_WRITE(ILK_GDSR, 0);
-
-	return 0;
+	POSTING_READ(ILK_GDSR);
+	return ret;
 }
 
 /* Reset the hardware domains (GENX_GRDOM_*) specified by mask */
 static int gen6_hw_domain_reset(struct drm_i915_private *dev_priv,
 				u32 hw_domain_mask)
 {
+	int err;
+
 	/* GEN6_GDRST is not in the gt power well, no need to check
 	 * for fifo space for the write or forcewake the chip for
 	 * the read
 	 */
 	__raw_i915_write32(dev_priv, GEN6_GDRST, hw_domain_mask);
 
-	/* Spin waiting for the device to ack the reset requests */
-	return intel_wait_for_register_fw(dev_priv,
+	/* Wait for the device to ack the reset requests */
+	err = intel_wait_for_register_fw(dev_priv,
 					  GEN6_GDRST, hw_domain_mask, 0,
 					  500);
+	if (err)
+		DRM_DEBUG_DRIVER("Wait for 0x%08x engines reset failed\n",
+				 hw_domain_mask);
+
+	return err;
 }
 
 /**
@@ -1585,19 +1517,23 @@ static int gen6_reset_engines(struct drm_i915_private *dev_priv,
 }
 
 /**
- * intel_wait_for_register_fw - wait until register matches expected state
+ * __intel_wait_for_register_fw - wait until register matches expected state
  * @dev_priv: the i915 device
  * @reg: the register to read
  * @mask: mask to apply to register value
  * @value: expected value
- * @timeout_ms: timeout in millisecond
+ * @fast_timeout_us: fast timeout in microsecond for atomic/tight wait
+ * @slow_timeout_ms: slow timeout in millisecond
+ * @out_value: optional placeholder to hold registry value
  *
  * This routine waits until the target register @reg contains the expected
  * @value after applying the @mask, i.e. it waits until ::
  *
  *     (I915_READ_FW(reg) & mask) == value
  *
- * Otherwise, the wait will timeout after @timeout_ms milliseconds.
+ * Otherwise, the wait will timeout after @slow_timeout_ms milliseconds.
+ * For atomic context @slow_timeout_ms must be zero and @fast_timeout_us
+ * must be not larger than 20,0000 microseconds.
  *
  * Note that this routine assumes the caller holds forcewake asserted, it is
  * not suitable for very long waits. See intel_wait_for_register() if you
@@ -1606,16 +1542,31 @@ static int gen6_reset_engines(struct drm_i915_private *dev_priv,
  *
  * Returns 0 if the register matches the desired condition, or -ETIMEOUT.
  */
-int intel_wait_for_register_fw(struct drm_i915_private *dev_priv,
-			       i915_reg_t reg,
-			       const u32 mask,
-			       const u32 value,
-			       const unsigned long timeout_ms)
-{
-#define done ((I915_READ_FW(reg) & mask) == value)
-	int ret = wait_for_us(done, 2);
-	if (ret)
-		ret = wait_for(done, timeout_ms);
+int __intel_wait_for_register_fw(struct drm_i915_private *dev_priv,
+				 i915_reg_t reg,
+				 u32 mask,
+				 u32 value,
+				 unsigned int fast_timeout_us,
+				 unsigned int slow_timeout_ms,
+				 u32 *out_value)
+{
+	u32 uninitialized_var(reg_value);
+#define done (((reg_value = I915_READ_FW(reg)) & mask) == value)
+	int ret;
+
+	/* Catch any overuse of this function */
+	might_sleep_if(slow_timeout_ms);
+	GEM_BUG_ON(fast_timeout_us > 20000);
+
+	ret = -ETIMEDOUT;
+	if (fast_timeout_us && fast_timeout_us <= 20000)
+		ret = _wait_for_atomic(done, fast_timeout_us, 0);
+	if (ret && slow_timeout_ms)
+		ret = wait_for(done, slow_timeout_ms);
+
+	if (out_value)
+		*out_value = reg_value;
+
 	return ret;
 #undef done
 }
@@ -1639,18 +1590,26 @@ int intel_wait_for_register_fw(struct drm_i915_private *dev_priv,
  */
 int intel_wait_for_register(struct drm_i915_private *dev_priv,
 			    i915_reg_t reg,
-			    const u32 mask,
-			    const u32 value,
-			    const unsigned long timeout_ms)
+			    u32 mask,
+			    u32 value,
+			    unsigned int timeout_ms)
 {
-
 	unsigned fw =
 		intel_uncore_forcewake_for_reg(dev_priv, reg, FW_REG_READ);
 	int ret;
 
-	intel_uncore_forcewake_get(dev_priv, fw);
-	ret = wait_for_us((I915_READ_FW(reg) & mask) == value, 2);
-	intel_uncore_forcewake_put(dev_priv, fw);
+	might_sleep();
+
+	spin_lock_irq(&dev_priv->uncore.lock);
+	intel_uncore_forcewake_get__locked(dev_priv, fw);
+
+	ret = __intel_wait_for_register_fw(dev_priv,
+					   reg, mask, value,
+					   2, 0, NULL);
+
+	intel_uncore_forcewake_put__locked(dev_priv, fw);
+	spin_unlock_irq(&dev_priv->uncore.lock);
+
 	if (ret)
 		ret = wait_for((I915_READ_NOTRACE(reg) & mask) == value,
 			       timeout_ms);
@@ -1658,7 +1617,7 @@ int intel_wait_for_register(struct drm_i915_private *dev_priv,
 	return ret;
 }
 
-static int gen8_request_engine_reset(struct intel_engine_cs *engine)
+static int gen8_reset_engine_start(struct intel_engine_cs *engine)
 {
 	struct drm_i915_private *dev_priv = engine->i915;
 	int ret;
@@ -1677,7 +1636,7 @@ static int gen8_request_engine_reset(struct intel_engine_cs *engine)
 	return ret;
 }
 
-static void gen8_unrequest_engine_reset(struct intel_engine_cs *engine)
+static void gen8_reset_engine_cancel(struct intel_engine_cs *engine)
 {
 	struct drm_i915_private *dev_priv = engine->i915;
 
@@ -1692,14 +1651,14 @@ static int gen8_reset_engines(struct drm_i915_private *dev_priv,
 	unsigned int tmp;
 
 	for_each_engine_masked(engine, dev_priv, engine_mask, tmp)
-		if (gen8_request_engine_reset(engine))
+		if (gen8_reset_engine_start(engine))
 			goto not_ready;
 
 	return gen6_reset_engines(dev_priv, engine_mask);
 
 not_ready:
 	for_each_engine_masked(engine, dev_priv, engine_mask, tmp)
-		gen8_unrequest_engine_reset(engine);
+		gen8_reset_engine_cancel(engine);
 
 	return -EIO;
 }
@@ -1730,8 +1689,11 @@ static reset_func intel_get_gpu_reset(struct drm_i915_private *dev_priv)
 int intel_gpu_reset(struct drm_i915_private *dev_priv, unsigned engine_mask)
 {
 	reset_func reset;
+	int retry;
 	int ret;
 
+	might_sleep();
+
 	reset = intel_get_gpu_reset(dev_priv);
 	if (reset == NULL)
 		return -ENODEV;
@@ -1740,7 +1702,13 @@ int intel_gpu_reset(struct drm_i915_private *dev_priv, unsigned engine_mask)
 	 * request may be dropped and never completes (causing -EIO).
 	 */
 	intel_uncore_forcewake_get(dev_priv, FORCEWAKE_ALL);
-	ret = reset(dev_priv, engine_mask);
+	for (retry = 0; retry < 3; retry++) {
+		ret = reset(dev_priv, engine_mask);
+		if (ret != -ETIMEDOUT)
+			break;
+
+		cond_resched();
+	}
 	intel_uncore_forcewake_put(dev_priv, FORCEWAKE_ALL);
 
 	return ret;
@@ -1754,17 +1722,12 @@ bool intel_has_gpu_reset(struct drm_i915_private *dev_priv)
 int intel_guc_reset(struct drm_i915_private *dev_priv)
 {
 	int ret;
-	unsigned long irqflags;
 
 	if (!HAS_GUC(dev_priv))
 		return -EINVAL;
 
 	intel_uncore_forcewake_get(dev_priv, FORCEWAKE_ALL);
-	spin_lock_irqsave(&dev_priv->uncore.lock, irqflags);
-
 	ret = gen6_hw_domain_reset(dev_priv, GEN9_GRDOM_GUC);
-
-	spin_unlock_irqrestore(&dev_priv->uncore.lock, irqflags);
 	intel_uncore_forcewake_put(dev_priv, FORCEWAKE_ALL);
 
 	return ret;
@@ -1873,5 +1836,6 @@ intel_uncore_forcewake_for_reg(struct drm_i915_private *dev_priv,
 }
 
 #if IS_ENABLED(CONFIG_DRM_I915_SELFTEST)
+#include "selftests/mock_uncore.c"
 #include "selftests/intel_uncore.c"
 #endif
diff --git a/drivers/gpu/drm/i915/intel_uncore.h b/drivers/gpu/drm/i915/intel_uncore.h
new file mode 100644
index 000000000000..5f90278da461
--- /dev/null
+++ b/drivers/gpu/drm/i915/intel_uncore.h
@@ -0,0 +1,170 @@
+/*
+ * Copyright © 2017 Intel Corporation
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
+ * IN THE SOFTWARE.
+ *
+ */
+
+#ifndef __INTEL_UNCORE_H__
+#define __INTEL_UNCORE_H__
+
+struct drm_i915_private;
+
+enum forcewake_domain_id {
+	FW_DOMAIN_ID_RENDER = 0,
+	FW_DOMAIN_ID_BLITTER,
+	FW_DOMAIN_ID_MEDIA,
+
+	FW_DOMAIN_ID_COUNT
+};
+
+enum forcewake_domains {
+	FORCEWAKE_RENDER = BIT(FW_DOMAIN_ID_RENDER),
+	FORCEWAKE_BLITTER = BIT(FW_DOMAIN_ID_BLITTER),
+	FORCEWAKE_MEDIA	= BIT(FW_DOMAIN_ID_MEDIA),
+	FORCEWAKE_ALL = (FORCEWAKE_RENDER |
+			 FORCEWAKE_BLITTER |
+			 FORCEWAKE_MEDIA)
+};
+
+struct intel_uncore_funcs {
+	void (*force_wake_get)(struct drm_i915_private *dev_priv,
+			       enum forcewake_domains domains);
+	void (*force_wake_put)(struct drm_i915_private *dev_priv,
+			       enum forcewake_domains domains);
+
+	uint8_t  (*mmio_readb)(struct drm_i915_private *dev_priv,
+			       i915_reg_t r, bool trace);
+	uint16_t (*mmio_readw)(struct drm_i915_private *dev_priv,
+			       i915_reg_t r, bool trace);
+	uint32_t (*mmio_readl)(struct drm_i915_private *dev_priv,
+			       i915_reg_t r, bool trace);
+	uint64_t (*mmio_readq)(struct drm_i915_private *dev_priv,
+			       i915_reg_t r, bool trace);
+
+	void (*mmio_writeb)(struct drm_i915_private *dev_priv,
+			    i915_reg_t r, uint8_t val, bool trace);
+	void (*mmio_writew)(struct drm_i915_private *dev_priv,
+			    i915_reg_t r, uint16_t val, bool trace);
+	void (*mmio_writel)(struct drm_i915_private *dev_priv,
+			    i915_reg_t r, uint32_t val, bool trace);
+};
+
+struct intel_forcewake_range {
+	u32 start;
+	u32 end;
+
+	enum forcewake_domains domains;
+};
+
+struct intel_uncore {
+	spinlock_t lock; /** lock is also taken in irq contexts. */
+
+	const struct intel_forcewake_range *fw_domains_table;
+	unsigned int fw_domains_table_entries;
+
+	struct notifier_block pmic_bus_access_nb;
+	struct intel_uncore_funcs funcs;
+
+	unsigned int fifo_count;
+
+	enum forcewake_domains fw_domains;
+	enum forcewake_domains fw_domains_active;
+
+	u32 fw_set;
+	u32 fw_clear;
+	u32 fw_reset;
+
+	struct intel_uncore_forcewake_domain {
+		enum forcewake_domain_id id;
+		enum forcewake_domains mask;
+		unsigned int wake_count;
+		bool active;
+		struct hrtimer timer;
+		i915_reg_t reg_set;
+		i915_reg_t reg_ack;
+	} fw_domain[FW_DOMAIN_ID_COUNT];
+
+	int unclaimed_mmio_check;
+};
+
+/* Iterate over initialised fw domains */
+#define for_each_fw_domain_masked(domain__, mask__, dev_priv__, tmp__) \
+	for (tmp__ = (mask__); \
+	     tmp__ ? (domain__ = &(dev_priv__)->uncore.fw_domain[__mask_next_bit(tmp__)]), 1 : 0;)
+
+#define for_each_fw_domain(domain__, dev_priv__, tmp__) \
+	for_each_fw_domain_masked(domain__, (dev_priv__)->uncore.fw_domains, dev_priv__, tmp__)
+
+
+void intel_uncore_sanitize(struct drm_i915_private *dev_priv);
+void intel_uncore_init(struct drm_i915_private *dev_priv);
+bool intel_uncore_unclaimed_mmio(struct drm_i915_private *dev_priv);
+bool intel_uncore_arm_unclaimed_mmio_detection(struct drm_i915_private *dev_priv);
+void intel_uncore_fini(struct drm_i915_private *dev_priv);
+void intel_uncore_suspend(struct drm_i915_private *dev_priv);
+void intel_uncore_resume_early(struct drm_i915_private *dev_priv);
+
+u64 intel_uncore_edram_size(struct drm_i915_private *dev_priv);
+void assert_forcewakes_inactive(struct drm_i915_private *dev_priv);
+const char *intel_uncore_forcewake_domain_to_str(const enum forcewake_domain_id id);
+
+enum forcewake_domains
+intel_uncore_forcewake_for_reg(struct drm_i915_private *dev_priv,
+			       i915_reg_t reg, unsigned int op);
+#define FW_REG_READ  (1)
+#define FW_REG_WRITE (2)
+
+void intel_uncore_forcewake_get(struct drm_i915_private *dev_priv,
+				enum forcewake_domains domains);
+void intel_uncore_forcewake_put(struct drm_i915_private *dev_priv,
+				enum forcewake_domains domains);
+/* Like above but the caller must manage the uncore.lock itself.
+ * Must be used with I915_READ_FW and friends.
+ */
+void intel_uncore_forcewake_get__locked(struct drm_i915_private *dev_priv,
+					enum forcewake_domains domains);
+void intel_uncore_forcewake_put__locked(struct drm_i915_private *dev_priv,
+					enum forcewake_domains domains);
+
+int intel_wait_for_register(struct drm_i915_private *dev_priv,
+			    i915_reg_t reg,
+			    u32 mask,
+			    u32 value,
+			    unsigned int timeout_ms);
+int __intel_wait_for_register_fw(struct drm_i915_private *dev_priv,
+				 i915_reg_t reg,
+				 u32 mask,
+				 u32 value,
+				 unsigned int fast_timeout_us,
+				 unsigned int slow_timeout_ms,
+				 u32 *out_value);
+static inline
+int intel_wait_for_register_fw(struct drm_i915_private *dev_priv,
+			       i915_reg_t reg,
+			       u32 mask,
+			       u32 value,
+			       unsigned int timeout_ms)
+{
+	return __intel_wait_for_register_fw(dev_priv, reg, mask, value,
+					    2, timeout_ms, NULL);
+}
+
+#endif /* !__INTEL_UNCORE_H__ */
diff --git a/drivers/gpu/drm/i915/selftests/i915_gem_coherency.c b/drivers/gpu/drm/i915/selftests/i915_gem_coherency.c
index f08d0179b3df..95d4aebc0181 100644
--- a/drivers/gpu/drm/i915/selftests/i915_gem_coherency.c
+++ b/drivers/gpu/drm/i915/selftests/i915_gem_coherency.c
@@ -138,10 +138,7 @@ static int wc_set(struct drm_i915_gem_object *obj,
 	typeof(v) *map;
 	int err;
 
-	/* XXX GTT write followed by WC write go missing */
-	i915_gem_object_flush_gtt_write_domain(obj);
-
-	err = i915_gem_object_set_to_gtt_domain(obj, true);
+	err = i915_gem_object_set_to_wc_domain(obj, true);
 	if (err)
 		return err;
 
@@ -162,10 +159,7 @@ static int wc_get(struct drm_i915_gem_object *obj,
 	typeof(v) map;
 	int err;
 
-	/* XXX WC write followed by GTT write go missing */
-	i915_gem_object_flush_gtt_write_domain(obj);
-
-	err = i915_gem_object_set_to_gtt_domain(obj, false);
+	err = i915_gem_object_set_to_wc_domain(obj, false);
 	if (err)
 		return err;
 
diff --git a/drivers/gpu/drm/i915/selftests/i915_gem_context.c b/drivers/gpu/drm/i915/selftests/i915_gem_context.c
index 1afb8b06e3e1..12b85b3278cd 100644
--- a/drivers/gpu/drm/i915/selftests/i915_gem_context.c
+++ b/drivers/gpu/drm/i915/selftests/i915_gem_context.c
@@ -320,7 +320,7 @@ static unsigned long max_dwords(struct drm_i915_gem_object *obj)
 static int igt_ctx_exec(void *arg)
 {
 	struct drm_i915_private *i915 = arg;
-	struct drm_i915_gem_object *obj;
+	struct drm_i915_gem_object *obj = NULL;
 	struct drm_file *file;
 	IGT_TIMEOUT(end_time);
 	LIST_HEAD(objects);
@@ -359,7 +359,7 @@ static int igt_ctx_exec(void *arg)
 		}
 
 		for_each_engine(engine, i915, id) {
-			if (dw == 0) {
+			if (!obj) {
 				obj = create_test_object(ctx, file, &objects);
 				if (IS_ERR(obj)) {
 					err = PTR_ERR(obj);
@@ -376,8 +376,10 @@ static int igt_ctx_exec(void *arg)
 				goto out_unlock;
 			}
 
-			if (++dw == max_dwords(obj))
+			if (++dw == max_dwords(obj)) {
+				obj = NULL;
 				dw = 0;
+			}
 			ndwords++;
 		}
 		ncontexts++;
diff --git a/drivers/gpu/drm/i915/selftests/i915_gem_dmabuf.c b/drivers/gpu/drm/i915/selftests/i915_gem_dmabuf.c
index 817bef74bbcb..d15cc9d3a5cd 100644
--- a/drivers/gpu/drm/i915/selftests/i915_gem_dmabuf.c
+++ b/drivers/gpu/drm/i915/selftests/i915_gem_dmabuf.c
@@ -271,6 +271,105 @@ err_obj:
 	return err;
 }
 
+static int igt_dmabuf_export_kmap(void *arg)
+{
+	struct drm_i915_private *i915 = arg;
+	struct drm_i915_gem_object *obj;
+	struct dma_buf *dmabuf;
+	void *ptr;
+	int err;
+
+	obj = i915_gem_object_create(i915, 2*PAGE_SIZE);
+	if (IS_ERR(obj))
+		return PTR_ERR(obj);
+
+	dmabuf = i915_gem_prime_export(&i915->drm, &obj->base, 0);
+	i915_gem_object_put(obj);
+	if (IS_ERR(dmabuf)) {
+		err = PTR_ERR(dmabuf);
+		pr_err("i915_gem_prime_export failed with err=%d\n", err);
+		return err;
+	}
+
+	ptr = dma_buf_kmap(dmabuf, 0);
+	if (!ptr) {
+		pr_err("dma_buf_kmap failed\n");
+		err = -ENOMEM;
+		goto err;
+	}
+
+	if (memchr_inv(ptr, 0, PAGE_SIZE)) {
+		dma_buf_kunmap(dmabuf, 0, ptr);
+		pr_err("Exported page[0] not initialiased to zero!\n");
+		err = -EINVAL;
+		goto err;
+	}
+
+	memset(ptr, 0xc5, PAGE_SIZE);
+	dma_buf_kunmap(dmabuf, 0, ptr);
+
+	ptr = i915_gem_object_pin_map(obj, I915_MAP_WB);
+	if (IS_ERR(ptr)) {
+		err = PTR_ERR(ptr);
+		pr_err("i915_gem_object_pin_map failed with err=%d\n", err);
+		goto err;
+	}
+	memset(ptr + PAGE_SIZE, 0xaa, PAGE_SIZE);
+	i915_gem_object_unpin_map(obj);
+
+	ptr = dma_buf_kmap(dmabuf, 1);
+	if (!ptr) {
+		pr_err("dma_buf_kmap failed\n");
+		err = -ENOMEM;
+		goto err;
+	}
+
+	if (memchr_inv(ptr, 0xaa, PAGE_SIZE)) {
+		dma_buf_kunmap(dmabuf, 1, ptr);
+		pr_err("Exported page[1] not set to 0xaa!\n");
+		err = -EINVAL;
+		goto err;
+	}
+
+	memset(ptr, 0xc5, PAGE_SIZE);
+	dma_buf_kunmap(dmabuf, 1, ptr);
+
+	ptr = dma_buf_kmap(dmabuf, 0);
+	if (!ptr) {
+		pr_err("dma_buf_kmap failed\n");
+		err = -ENOMEM;
+		goto err;
+	}
+	if (memchr_inv(ptr, 0xc5, PAGE_SIZE)) {
+		dma_buf_kunmap(dmabuf, 0, ptr);
+		pr_err("Exported page[0] did not retain 0xc5!\n");
+		err = -EINVAL;
+		goto err;
+	}
+	dma_buf_kunmap(dmabuf, 0, ptr);
+
+	ptr = dma_buf_kmap(dmabuf, 2);
+	if (ptr) {
+		pr_err("Erroneously kmapped beyond the end of the object!\n");
+		dma_buf_kunmap(dmabuf, 2, ptr);
+		err = -EINVAL;
+		goto err;
+	}
+
+	ptr = dma_buf_kmap(dmabuf, -1);
+	if (ptr) {
+		pr_err("Erroneously kmapped before the start of the object!\n");
+		dma_buf_kunmap(dmabuf, -1, ptr);
+		err = -EINVAL;
+		goto err;
+	}
+
+	err = 0;
+err:
+	dma_buf_put(dmabuf);
+	return err;
+}
+
 int i915_gem_dmabuf_mock_selftests(void)
 {
 	static const struct i915_subtest tests[] = {
@@ -279,6 +378,7 @@ int i915_gem_dmabuf_mock_selftests(void)
 		SUBTEST(igt_dmabuf_import),
 		SUBTEST(igt_dmabuf_import_ownership),
 		SUBTEST(igt_dmabuf_export_vmap),
+		SUBTEST(igt_dmabuf_export_kmap),
 	};
 	struct drm_i915_private *i915;
 	int err;
diff --git a/drivers/gpu/drm/i915/selftests/i915_gem_object.c b/drivers/gpu/drm/i915/selftests/i915_gem_object.c
index 67d82bf1407f..8f011c447e41 100644
--- a/drivers/gpu/drm/i915/selftests/i915_gem_object.c
+++ b/drivers/gpu/drm/i915/selftests/i915_gem_object.c
@@ -266,7 +266,7 @@ static int check_partial_mapping(struct drm_i915_gem_object *obj,
 		if (offset >= obj->base.size)
 			continue;
 
-		i915_gem_object_flush_gtt_write_domain(obj);
+		flush_write_domain(obj, ~I915_GEM_DOMAIN_CPU);
 
 		p = i915_gem_object_get_page(obj, offset >> PAGE_SHIFT);
 		cpu = kmap(p) + offset_in_page(offset);
@@ -545,7 +545,9 @@ static int igt_mmap_offset_exhaustion(void *arg)
 		}
 
 		mutex_lock(&i915->drm.struct_mutex);
+		intel_runtime_pm_get(i915);
 		err = make_obj_busy(obj);
+		intel_runtime_pm_put(i915);
 		mutex_unlock(&i915->drm.struct_mutex);
 		if (err) {
 			pr_err("[loop %d] Failed to busy the object\n", loop);
diff --git a/drivers/gpu/drm/i915/selftests/i915_gem_request.c b/drivers/gpu/drm/i915/selftests/i915_gem_request.c
index 98b7aac41eec..6664cb2eb0b8 100644
--- a/drivers/gpu/drm/i915/selftests/i915_gem_request.c
+++ b/drivers/gpu/drm/i915/selftests/i915_gem_request.c
@@ -580,7 +580,7 @@ static struct i915_vma *recursive_batch(struct drm_i915_private *i915)
 	if (err)
 		goto err;
 
-	err = i915_gem_object_set_to_gtt_domain(obj, true);
+	err = i915_gem_object_set_to_wc_domain(obj, true);
 	if (err)
 		goto err;
 
diff --git a/drivers/gpu/drm/i915/selftests/i915_gem_timeline.c b/drivers/gpu/drm/i915/selftests/i915_gem_timeline.c
new file mode 100644
index 000000000000..7a44dab631b8
--- /dev/null
+++ b/drivers/gpu/drm/i915/selftests/i915_gem_timeline.c
@@ -0,0 +1,299 @@
+/*
+ * Copyright © 2017 Intel Corporation
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
+ * IN THE SOFTWARE.
+ *
+ */
+
+#include "../i915_selftest.h"
+#include "i915_random.h"
+
+#include "mock_gem_device.h"
+#include "mock_timeline.h"
+
+struct __igt_sync {
+	const char *name;
+	u32 seqno;
+	bool expected;
+	bool set;
+};
+
+static int __igt_sync(struct intel_timeline *tl,
+		      u64 ctx,
+		      const struct __igt_sync *p,
+		      const char *name)
+{
+	int ret;
+
+	if (__intel_timeline_sync_is_later(tl, ctx, p->seqno) != p->expected) {
+		pr_err("%s: %s(ctx=%llu, seqno=%u) expected passed %s but failed\n",
+		       name, p->name, ctx, p->seqno, yesno(p->expected));
+		return -EINVAL;
+	}
+
+	if (p->set) {
+		ret = __intel_timeline_sync_set(tl, ctx, p->seqno);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+static int igt_sync(void *arg)
+{
+	const struct __igt_sync pass[] = {
+		{ "unset", 0, false, false },
+		{ "new", 0, false, true },
+		{ "0a", 0, true, true },
+		{ "1a", 1, false, true },
+		{ "1b", 1, true, true },
+		{ "0b", 0, true, false },
+		{ "2a", 2, false, true },
+		{ "4", 4, false, true },
+		{ "INT_MAX", INT_MAX, false, true },
+		{ "INT_MAX-1", INT_MAX-1, true, false },
+		{ "INT_MAX+1", (u32)INT_MAX+1, false, true },
+		{ "INT_MAX", INT_MAX, true, false },
+		{ "UINT_MAX", UINT_MAX, false, true },
+		{ "wrap", 0, false, true },
+		{ "unwrap", UINT_MAX, true, false },
+		{},
+	}, *p;
+	struct intel_timeline *tl;
+	int order, offset;
+	int ret;
+
+	tl = mock_timeline(0);
+	if (!tl)
+		return -ENOMEM;
+
+	for (p = pass; p->name; p++) {
+		for (order = 1; order < 64; order++) {
+			for (offset = -1; offset <= (order > 1); offset++) {
+				u64 ctx = BIT_ULL(order) + offset;
+
+				ret = __igt_sync(tl, ctx, p, "1");
+				if (ret)
+					goto out;
+			}
+		}
+	}
+	mock_timeline_destroy(tl);
+
+	tl = mock_timeline(0);
+	if (!tl)
+		return -ENOMEM;
+
+	for (order = 1; order < 64; order++) {
+		for (offset = -1; offset <= (order > 1); offset++) {
+			u64 ctx = BIT_ULL(order) + offset;
+
+			for (p = pass; p->name; p++) {
+				ret = __igt_sync(tl, ctx, p, "2");
+				if (ret)
+					goto out;
+			}
+		}
+	}
+
+out:
+	mock_timeline_destroy(tl);
+	return ret;
+}
+
+static unsigned int random_engine(struct rnd_state *rnd)
+{
+	return ((u64)prandom_u32_state(rnd) * I915_NUM_ENGINES) >> 32;
+}
+
+static int bench_sync(void *arg)
+{
+	struct rnd_state prng;
+	struct intel_timeline *tl;
+	unsigned long end_time, count;
+	u64 prng32_1M;
+	ktime_t kt;
+	int order, last_order;
+
+	tl = mock_timeline(0);
+	if (!tl)
+		return -ENOMEM;
+
+	/* Lookups from cache are very fast and so the random number generation
+	 * and the loop itself becomes a significant factor in the per-iteration
+	 * timings. We try to compensate the results by measuring the overhead
+	 * of the prng and subtract it from the reported results.
+	 */
+	prandom_seed_state(&prng, i915_selftest.random_seed);
+	count = 0;
+	kt = ktime_get();
+	end_time = jiffies + HZ/10;
+	do {
+		u32 x;
+
+		/* Make sure the compiler doesn't optimise away the prng call */
+		WRITE_ONCE(x, prandom_u32_state(&prng));
+
+		count++;
+	} while (!time_after(jiffies, end_time));
+	kt = ktime_sub(ktime_get(), kt);
+	pr_debug("%s: %lu random evaluations, %lluns/prng\n",
+		 __func__, count, (long long)div64_ul(ktime_to_ns(kt), count));
+	prng32_1M = div64_ul(ktime_to_ns(kt) << 20, count);
+
+	/* Benchmark (only) setting random context ids */
+	prandom_seed_state(&prng, i915_selftest.random_seed);
+	count = 0;
+	kt = ktime_get();
+	end_time = jiffies + HZ/10;
+	do {
+		u64 id = i915_prandom_u64_state(&prng);
+
+		__intel_timeline_sync_set(tl, id, 0);
+		count++;
+	} while (!time_after(jiffies, end_time));
+	kt = ktime_sub(ktime_get(), kt);
+	kt = ktime_sub_ns(kt, (count * prng32_1M * 2) >> 20);
+	pr_info("%s: %lu random insertions, %lluns/insert\n",
+		__func__, count, (long long)div64_ul(ktime_to_ns(kt), count));
+
+	/* Benchmark looking up the exact same context ids as we just set */
+	prandom_seed_state(&prng, i915_selftest.random_seed);
+	end_time = count;
+	kt = ktime_get();
+	while (end_time--) {
+		u64 id = i915_prandom_u64_state(&prng);
+
+		if (!__intel_timeline_sync_is_later(tl, id, 0)) {
+			mock_timeline_destroy(tl);
+			pr_err("Lookup of %llu failed\n", id);
+			return -EINVAL;
+		}
+	}
+	kt = ktime_sub(ktime_get(), kt);
+	kt = ktime_sub_ns(kt, (count * prng32_1M * 2) >> 20);
+	pr_info("%s: %lu random lookups, %lluns/lookup\n",
+		__func__, count, (long long)div64_ul(ktime_to_ns(kt), count));
+
+	mock_timeline_destroy(tl);
+	cond_resched();
+
+	tl = mock_timeline(0);
+	if (!tl)
+		return -ENOMEM;
+
+	/* Benchmark setting the first N (in order) contexts */
+	count = 0;
+	kt = ktime_get();
+	end_time = jiffies + HZ/10;
+	do {
+		__intel_timeline_sync_set(tl, count++, 0);
+	} while (!time_after(jiffies, end_time));
+	kt = ktime_sub(ktime_get(), kt);
+	pr_info("%s: %lu in-order insertions, %lluns/insert\n",
+		__func__, count, (long long)div64_ul(ktime_to_ns(kt), count));
+
+	/* Benchmark looking up the exact same context ids as we just set */
+	end_time = count;
+	kt = ktime_get();
+	while (end_time--) {
+		if (!__intel_timeline_sync_is_later(tl, end_time, 0)) {
+			pr_err("Lookup of %lu failed\n", end_time);
+			mock_timeline_destroy(tl);
+			return -EINVAL;
+		}
+	}
+	kt = ktime_sub(ktime_get(), kt);
+	pr_info("%s: %lu in-order lookups, %lluns/lookup\n",
+		__func__, count, (long long)div64_ul(ktime_to_ns(kt), count));
+
+	mock_timeline_destroy(tl);
+	cond_resched();
+
+	tl = mock_timeline(0);
+	if (!tl)
+		return -ENOMEM;
+
+	/* Benchmark searching for a random context id and maybe changing it */
+	prandom_seed_state(&prng, i915_selftest.random_seed);
+	count = 0;
+	kt = ktime_get();
+	end_time = jiffies + HZ/10;
+	do {
+		u32 id = random_engine(&prng);
+		u32 seqno = prandom_u32_state(&prng);
+
+		if (!__intel_timeline_sync_is_later(tl, id, seqno))
+			__intel_timeline_sync_set(tl, id, seqno);
+
+		count++;
+	} while (!time_after(jiffies, end_time));
+	kt = ktime_sub(ktime_get(), kt);
+	kt = ktime_sub_ns(kt, (count * prng32_1M * 2) >> 20);
+	pr_info("%s: %lu repeated insert/lookups, %lluns/op\n",
+		__func__, count, (long long)div64_ul(ktime_to_ns(kt), count));
+	mock_timeline_destroy(tl);
+	cond_resched();
+
+	/* Benchmark searching for a known context id and changing the seqno */
+	for (last_order = 1, order = 1; order < 32;
+	     ({ int tmp = last_order; last_order = order; order += tmp; })) {
+		unsigned int mask = BIT(order) - 1;
+
+		tl = mock_timeline(0);
+		if (!tl)
+			return -ENOMEM;
+
+		count = 0;
+		kt = ktime_get();
+		end_time = jiffies + HZ/10;
+		do {
+			/* Without assuming too many details of the underlying
+			 * implementation, try to identify its phase-changes
+			 * (if any)!
+			 */
+			u64 id = (u64)(count & mask) << order;
+
+			__intel_timeline_sync_is_later(tl, id, 0);
+			__intel_timeline_sync_set(tl, id, 0);
+
+			count++;
+		} while (!time_after(jiffies, end_time));
+		kt = ktime_sub(ktime_get(), kt);
+		pr_info("%s: %lu cyclic/%d insert/lookups, %lluns/op\n",
+			__func__, count, order,
+			(long long)div64_ul(ktime_to_ns(kt), count));
+		mock_timeline_destroy(tl);
+		cond_resched();
+	}
+
+	return 0;
+}
+
+int i915_gem_timeline_mock_selftests(void)
+{
+	static const struct i915_subtest tests[] = {
+		SUBTEST(igt_sync),
+		SUBTEST(bench_sync),
+	};
+
+	return i915_subtests(tests, NULL);
+}
diff --git a/drivers/gpu/drm/i915/selftests/i915_mock_selftests.h b/drivers/gpu/drm/i915/selftests/i915_mock_selftests.h
index be9a9ebf5692..fc74687501ba 100644
--- a/drivers/gpu/drm/i915/selftests/i915_mock_selftests.h
+++ b/drivers/gpu/drm/i915/selftests/i915_mock_selftests.h
@@ -9,9 +9,12 @@
  * Tests are executed in order by igt/drv_selftest
  */
 selftest(sanitycheck, i915_mock_sanitycheck) /* keep first (igt selfcheck) */
+selftest(fence, i915_sw_fence_mock_selftests)
 selftest(scatterlist, scatterlist_mock_selftests)
+selftest(syncmap, i915_syncmap_mock_selftests)
 selftest(uncore, intel_uncore_mock_selftests)
 selftest(breadcrumbs, intel_breadcrumbs_mock_selftests)
+selftest(timelines, i915_gem_timeline_mock_selftests)
 selftest(requests, i915_gem_request_mock_selftests)
 selftest(objects, i915_gem_object_mock_selftests)
 selftest(dmabuf, i915_gem_dmabuf_mock_selftests)
diff --git a/drivers/gpu/drm/i915/selftests/i915_random.c b/drivers/gpu/drm/i915/selftests/i915_random.c
index c17c83c30637..d044bf9a6feb 100644
--- a/drivers/gpu/drm/i915/selftests/i915_random.c
+++ b/drivers/gpu/drm/i915/selftests/i915_random.c
@@ -30,6 +30,17 @@
 
 #include "i915_random.h"
 
+u64 i915_prandom_u64_state(struct rnd_state *rnd)
+{
+	u64 x;
+
+	x = prandom_u32_state(rnd);
+	x <<= 32;
+	x |= prandom_u32_state(rnd);
+
+	return x;
+}
+
 static inline u32 i915_prandom_u32_max_state(u32 ep_ro, struct rnd_state *state)
 {
 	return upper_32_bits((u64)prandom_u32_state(state) * ep_ro);
diff --git a/drivers/gpu/drm/i915/selftests/i915_random.h b/drivers/gpu/drm/i915/selftests/i915_random.h
index b9c334ce6cd9..6c9379871384 100644
--- a/drivers/gpu/drm/i915/selftests/i915_random.h
+++ b/drivers/gpu/drm/i915/selftests/i915_random.h
@@ -41,6 +41,8 @@
 #define I915_RND_SUBSTATE(name__, parent__) \
 	struct rnd_state name__ = I915_RND_STATE_INITIALIZER(prandom_u32_state(&(parent__)))
 
+u64 i915_prandom_u64_state(struct rnd_state *rnd);
+
 unsigned int *i915_random_order(unsigned int count,
 				struct rnd_state *state);
 void i915_random_reorder(unsigned int *order,
diff --git a/drivers/gpu/drm/i915/selftests/i915_sw_fence.c b/drivers/gpu/drm/i915/selftests/i915_sw_fence.c
new file mode 100644
index 000000000000..19d145d6bf52
--- /dev/null
+++ b/drivers/gpu/drm/i915/selftests/i915_sw_fence.c
@@ -0,0 +1,582 @@
+/*
+ * Copyright © 2017 Intel Corporation
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
+ * IN THE SOFTWARE.
+ *
+ */
+
+#include <linux/completion.h>
+#include <linux/delay.h>
+
+#include "../i915_selftest.h"
+
+static int __i915_sw_fence_call
+fence_notify(struct i915_sw_fence *fence, enum i915_sw_fence_notify state)
+{
+	switch (state) {
+	case FENCE_COMPLETE:
+		break;
+
+	case FENCE_FREE:
+		/* Leave the fence for the caller to free it after testing */
+		break;
+	}
+
+	return NOTIFY_DONE;
+}
+
+static struct i915_sw_fence *alloc_fence(void)
+{
+	struct i915_sw_fence *fence;
+
+	fence = kmalloc(sizeof(*fence), GFP_KERNEL);
+	if (!fence)
+		return NULL;
+
+	i915_sw_fence_init(fence, fence_notify);
+	return fence;
+}
+
+static void free_fence(struct i915_sw_fence *fence)
+{
+	i915_sw_fence_fini(fence);
+	kfree(fence);
+}
+
+static int __test_self(struct i915_sw_fence *fence)
+{
+	if (i915_sw_fence_done(fence))
+		return -EINVAL;
+
+	i915_sw_fence_commit(fence);
+	if (!i915_sw_fence_done(fence))
+		return -EINVAL;
+
+	i915_sw_fence_wait(fence);
+	if (!i915_sw_fence_done(fence))
+		return -EINVAL;
+
+	return 0;
+}
+
+static int test_self(void *arg)
+{
+	struct i915_sw_fence *fence;
+	int ret;
+
+	/* Test i915_sw_fence signaling and completion testing */
+	fence = alloc_fence();
+	if (!fence)
+		return -ENOMEM;
+
+	ret = __test_self(fence);
+
+	free_fence(fence);
+	return ret;
+}
+
+static int test_dag(void *arg)
+{
+	struct i915_sw_fence *A, *B, *C;
+	int ret = -EINVAL;
+
+	/* Test detection of cycles within the i915_sw_fence graphs */
+	if (!IS_ENABLED(CONFIG_DRM_I915_SW_FENCE_CHECK_DAG))
+		return 0;
+
+	A = alloc_fence();
+	if (!A)
+		return -ENOMEM;
+
+	if (i915_sw_fence_await_sw_fence_gfp(A, A, GFP_KERNEL) != -EINVAL) {
+		pr_err("recursive cycle not detected (AA)\n");
+		goto err_A;
+	}
+
+	B = alloc_fence();
+	if (!B) {
+		ret = -ENOMEM;
+		goto err_A;
+	}
+
+	i915_sw_fence_await_sw_fence_gfp(A, B, GFP_KERNEL);
+	if (i915_sw_fence_await_sw_fence_gfp(B, A, GFP_KERNEL) != -EINVAL) {
+		pr_err("single depth cycle not detected (BAB)\n");
+		goto err_B;
+	}
+
+	C = alloc_fence();
+	if (!C) {
+		ret = -ENOMEM;
+		goto err_B;
+	}
+
+	if (i915_sw_fence_await_sw_fence_gfp(B, C, GFP_KERNEL) == -EINVAL) {
+		pr_err("invalid cycle detected\n");
+		goto err_C;
+	}
+	if (i915_sw_fence_await_sw_fence_gfp(C, B, GFP_KERNEL) != -EINVAL) {
+		pr_err("single depth cycle not detected (CBC)\n");
+		goto err_C;
+	}
+	if (i915_sw_fence_await_sw_fence_gfp(C, A, GFP_KERNEL) != -EINVAL) {
+		pr_err("cycle not detected (BA, CB, AC)\n");
+		goto err_C;
+	}
+	if (i915_sw_fence_await_sw_fence_gfp(A, C, GFP_KERNEL) == -EINVAL) {
+		pr_err("invalid cycle detected\n");
+		goto err_C;
+	}
+
+	i915_sw_fence_commit(A);
+	i915_sw_fence_commit(B);
+	i915_sw_fence_commit(C);
+
+	ret = 0;
+	if (!i915_sw_fence_done(C)) {
+		pr_err("fence C not done\n");
+		ret = -EINVAL;
+	}
+	if (!i915_sw_fence_done(B)) {
+		pr_err("fence B not done\n");
+		ret = -EINVAL;
+	}
+	if (!i915_sw_fence_done(A)) {
+		pr_err("fence A not done\n");
+		ret = -EINVAL;
+	}
+err_C:
+	free_fence(C);
+err_B:
+	free_fence(B);
+err_A:
+	free_fence(A);
+	return ret;
+}
+
+static int test_AB(void *arg)
+{
+	struct i915_sw_fence *A, *B;
+	int ret;
+
+	/* Test i915_sw_fence (A) waiting on an event source (B) */
+	A = alloc_fence();
+	if (!A)
+		return -ENOMEM;
+	B = alloc_fence();
+	if (!B) {
+		ret = -ENOMEM;
+		goto err_A;
+	}
+
+	ret = i915_sw_fence_await_sw_fence_gfp(A, B, GFP_KERNEL);
+	if (ret < 0)
+		goto err_B;
+	if (ret == 0) {
+		pr_err("Incorrectly reported fence A was complete before await\n");
+		ret = -EINVAL;
+		goto err_B;
+	}
+
+	ret = -EINVAL;
+	i915_sw_fence_commit(A);
+	if (i915_sw_fence_done(A))
+		goto err_B;
+
+	i915_sw_fence_commit(B);
+	if (!i915_sw_fence_done(B)) {
+		pr_err("Fence B is not done\n");
+		goto err_B;
+	}
+
+	if (!i915_sw_fence_done(A)) {
+		pr_err("Fence A is not done\n");
+		goto err_B;
+	}
+
+	ret = 0;
+err_B:
+	free_fence(B);
+err_A:
+	free_fence(A);
+	return ret;
+}
+
+static int test_ABC(void *arg)
+{
+	struct i915_sw_fence *A, *B, *C;
+	int ret;
+
+	/* Test a chain of fences, A waits on B who waits on C */
+	A = alloc_fence();
+	if (!A)
+		return -ENOMEM;
+
+	B = alloc_fence();
+	if (!B) {
+		ret = -ENOMEM;
+		goto err_A;
+	}
+
+	C = alloc_fence();
+	if (!C) {
+		ret = -ENOMEM;
+		goto err_B;
+	}
+
+	ret = i915_sw_fence_await_sw_fence_gfp(A, B, GFP_KERNEL);
+	if (ret < 0)
+		goto err_C;
+	if (ret == 0) {
+		pr_err("Incorrectly reported fence B was complete before await\n");
+		goto err_C;
+	}
+
+	ret = i915_sw_fence_await_sw_fence_gfp(B, C, GFP_KERNEL);
+	if (ret < 0)
+		goto err_C;
+	if (ret == 0) {
+		pr_err("Incorrectly reported fence C was complete before await\n");
+		goto err_C;
+	}
+
+	ret = -EINVAL;
+	i915_sw_fence_commit(A);
+	if (i915_sw_fence_done(A)) {
+		pr_err("Fence A completed early\n");
+		goto err_C;
+	}
+
+	i915_sw_fence_commit(B);
+	if (i915_sw_fence_done(B)) {
+		pr_err("Fence B completed early\n");
+		goto err_C;
+	}
+
+	if (i915_sw_fence_done(A)) {
+		pr_err("Fence A completed early (after signaling B)\n");
+		goto err_C;
+	}
+
+	i915_sw_fence_commit(C);
+
+	ret = 0;
+	if (!i915_sw_fence_done(C)) {
+		pr_err("Fence C not done\n");
+		ret = -EINVAL;
+	}
+	if (!i915_sw_fence_done(B)) {
+		pr_err("Fence B not done\n");
+		ret = -EINVAL;
+	}
+	if (!i915_sw_fence_done(A)) {
+		pr_err("Fence A not done\n");
+		ret = -EINVAL;
+	}
+err_C:
+	free_fence(C);
+err_B:
+	free_fence(B);
+err_A:
+	free_fence(A);
+	return ret;
+}
+
+static int test_AB_C(void *arg)
+{
+	struct i915_sw_fence *A, *B, *C;
+	int ret = -EINVAL;
+
+	/* Test multiple fences (AB) waiting on a single event (C) */
+	A = alloc_fence();
+	if (!A)
+		return -ENOMEM;
+
+	B = alloc_fence();
+	if (!B) {
+		ret = -ENOMEM;
+		goto err_A;
+	}
+
+	C = alloc_fence();
+	if (!C) {
+		ret = -ENOMEM;
+		goto err_B;
+	}
+
+	ret = i915_sw_fence_await_sw_fence_gfp(A, C, GFP_KERNEL);
+	if (ret < 0)
+		goto err_C;
+	if (ret == 0) {
+		ret = -EINVAL;
+		goto err_C;
+	}
+
+	ret = i915_sw_fence_await_sw_fence_gfp(B, C, GFP_KERNEL);
+	if (ret < 0)
+		goto err_C;
+	if (ret == 0) {
+		ret = -EINVAL;
+		goto err_C;
+	}
+
+	i915_sw_fence_commit(A);
+	i915_sw_fence_commit(B);
+
+	ret = 0;
+	if (i915_sw_fence_done(A)) {
+		pr_err("Fence A completed early\n");
+		ret = -EINVAL;
+	}
+
+	if (i915_sw_fence_done(B)) {
+		pr_err("Fence B completed early\n");
+		ret = -EINVAL;
+	}
+
+	i915_sw_fence_commit(C);
+	if (!i915_sw_fence_done(C)) {
+		pr_err("Fence C not done\n");
+		ret = -EINVAL;
+	}
+
+	if (!i915_sw_fence_done(B)) {
+		pr_err("Fence B not done\n");
+		ret = -EINVAL;
+	}
+
+	if (!i915_sw_fence_done(A)) {
+		pr_err("Fence A not done\n");
+		ret = -EINVAL;
+	}
+
+err_C:
+	free_fence(C);
+err_B:
+	free_fence(B);
+err_A:
+	free_fence(A);
+	return ret;
+}
+
+static int test_C_AB(void *arg)
+{
+	struct i915_sw_fence *A, *B, *C;
+	int ret;
+
+	/* Test multiple event sources (A,B) for a single fence (C) */
+	A = alloc_fence();
+	if (!A)
+		return -ENOMEM;
+
+	B = alloc_fence();
+	if (!B) {
+		ret = -ENOMEM;
+		goto err_A;
+	}
+
+	C = alloc_fence();
+	if (!C) {
+		ret = -ENOMEM;
+		goto err_B;
+	}
+
+	ret = i915_sw_fence_await_sw_fence_gfp(C, A, GFP_KERNEL);
+	if (ret < 0)
+		goto err_C;
+	if (ret == 0) {
+		ret = -EINVAL;
+		goto err_C;
+	}
+
+	ret = i915_sw_fence_await_sw_fence_gfp(C, B, GFP_KERNEL);
+	if (ret < 0)
+		goto err_C;
+	if (ret == 0) {
+		ret = -EINVAL;
+		goto err_C;
+	}
+
+	ret = 0;
+	i915_sw_fence_commit(C);
+	if (i915_sw_fence_done(C))
+		ret = -EINVAL;
+
+	i915_sw_fence_commit(A);
+	i915_sw_fence_commit(B);
+
+	if (!i915_sw_fence_done(A)) {
+		pr_err("Fence A not done\n");
+		ret = -EINVAL;
+	}
+
+	if (!i915_sw_fence_done(B)) {
+		pr_err("Fence B not done\n");
+		ret = -EINVAL;
+	}
+
+	if (!i915_sw_fence_done(C)) {
+		pr_err("Fence C not done\n");
+		ret = -EINVAL;
+	}
+
+err_C:
+	free_fence(C);
+err_B:
+	free_fence(B);
+err_A:
+	free_fence(A);
+	return ret;
+}
+
+static int test_chain(void *arg)
+{
+	int nfences = 4096;
+	struct i915_sw_fence **fences;
+	int ret, i;
+
+	/* Test a long chain of fences */
+	fences = kmalloc_array(nfences, sizeof(*fences), GFP_KERNEL);
+	if (!fences)
+		return -ENOMEM;
+
+	for (i = 0; i < nfences; i++) {
+		fences[i] = alloc_fence();
+		if (!fences[i]) {
+			nfences = i;
+			ret = -ENOMEM;
+			goto err;
+		}
+
+		if (i > 0) {
+			ret = i915_sw_fence_await_sw_fence_gfp(fences[i],
+							       fences[i - 1],
+							       GFP_KERNEL);
+			if (ret < 0) {
+				nfences = i + 1;
+				goto err;
+			}
+
+			i915_sw_fence_commit(fences[i]);
+		}
+	}
+
+	ret = 0;
+	for (i = nfences; --i; ) {
+		if (i915_sw_fence_done(fences[i])) {
+			if (ret == 0)
+				pr_err("Fence[%d] completed early\n", i);
+			ret = -EINVAL;
+		}
+	}
+	i915_sw_fence_commit(fences[0]);
+	for (i = 0; ret == 0 && i < nfences; i++) {
+		if (!i915_sw_fence_done(fences[i])) {
+			pr_err("Fence[%d] is not done\n", i);
+			ret = -EINVAL;
+		}
+	}
+
+err:
+	for (i = 0; i < nfences; i++)
+		free_fence(fences[i]);
+	kfree(fences);
+	return ret;
+}
+
+struct task_ipc {
+	struct work_struct work;
+	struct completion started;
+	struct i915_sw_fence *in, *out;
+	int value;
+};
+
+static void task_ipc(struct work_struct *work)
+{
+	struct task_ipc *ipc = container_of(work, typeof(*ipc), work);
+
+	complete(&ipc->started);
+
+	i915_sw_fence_wait(ipc->in);
+	smp_store_mb(ipc->value, 1);
+	i915_sw_fence_commit(ipc->out);
+}
+
+static int test_ipc(void *arg)
+{
+	struct task_ipc ipc;
+	int ret = 0;
+
+	/* Test use of i915_sw_fence as an interprocess signaling mechanism */
+	ipc.in = alloc_fence();
+	if (!ipc.in)
+		return -ENOMEM;
+	ipc.out = alloc_fence();
+	if (!ipc.out) {
+		ret = -ENOMEM;
+		goto err_in;
+	}
+
+	/* use a completion to avoid chicken-and-egg testing */
+	init_completion(&ipc.started);
+
+	ipc.value = 0;
+	INIT_WORK_ONSTACK(&ipc.work, task_ipc);
+	schedule_work(&ipc.work);
+
+	wait_for_completion(&ipc.started);
+
+	usleep_range(1000, 2000);
+	if (READ_ONCE(ipc.value)) {
+		pr_err("worker updated value before i915_sw_fence was signaled\n");
+		ret = -EINVAL;
+	}
+
+	i915_sw_fence_commit(ipc.in);
+	i915_sw_fence_wait(ipc.out);
+
+	if (!READ_ONCE(ipc.value)) {
+		pr_err("worker signaled i915_sw_fence before value was posted\n");
+		ret = -EINVAL;
+	}
+
+	flush_work(&ipc.work);
+	destroy_work_on_stack(&ipc.work);
+	free_fence(ipc.out);
+err_in:
+	free_fence(ipc.in);
+	return ret;
+}
+
+int i915_sw_fence_mock_selftests(void)
+{
+	static const struct i915_subtest tests[] = {
+		SUBTEST(test_self),
+		SUBTEST(test_dag),
+		SUBTEST(test_AB),
+		SUBTEST(test_ABC),
+		SUBTEST(test_AB_C),
+		SUBTEST(test_C_AB),
+		SUBTEST(test_chain),
+		SUBTEST(test_ipc),
+	};
+
+	return i915_subtests(tests, NULL);
+}
diff --git a/drivers/gpu/drm/i915/selftests/i915_syncmap.c b/drivers/gpu/drm/i915/selftests/i915_syncmap.c
new file mode 100644
index 000000000000..bcab3d00a785
--- /dev/null
+++ b/drivers/gpu/drm/i915/selftests/i915_syncmap.c
@@ -0,0 +1,616 @@
+/*
+ * Copyright © 2017 Intel Corporation
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
+ * IN THE SOFTWARE.
+ *
+ */
+
+#include "../i915_selftest.h"
+#include "i915_random.h"
+
+static char *
+__sync_print(struct i915_syncmap *p,
+	     char *buf, unsigned long *sz,
+	     unsigned int depth,
+	     unsigned int last,
+	     unsigned int idx)
+{
+	unsigned long len;
+	unsigned int i, X;
+
+	if (depth) {
+		unsigned int d;
+
+		for (d = 0; d < depth - 1; d++) {
+			if (last & BIT(depth - d - 1))
+				len = scnprintf(buf, *sz, "|   ");
+			else
+				len = scnprintf(buf, *sz, "    ");
+			buf += len;
+			*sz -= len;
+		}
+		len = scnprintf(buf, *sz, "%x-> ", idx);
+		buf += len;
+		*sz -= len;
+	}
+
+	/* We mark bits after the prefix as "X" */
+	len = scnprintf(buf, *sz, "0x%016llx", p->prefix << p->height << SHIFT);
+	buf += len;
+	*sz -= len;
+	X = (p->height + SHIFT) / 4;
+	scnprintf(buf - X, *sz + X, "%*s", X, "XXXXXXXXXXXXXXXXX");
+
+	if (!p->height) {
+		for_each_set_bit(i, (unsigned long *)&p->bitmap, KSYNCMAP) {
+			len = scnprintf(buf, *sz, " %x:%x,",
+					i, __sync_seqno(p)[i]);
+			buf += len;
+			*sz -= len;
+		}
+		buf -= 1;
+		*sz += 1;
+	}
+
+	len = scnprintf(buf, *sz, "\n");
+	buf += len;
+	*sz -= len;
+
+	if (p->height) {
+		for_each_set_bit(i, (unsigned long *)&p->bitmap, KSYNCMAP) {
+			buf = __sync_print(__sync_child(p)[i], buf, sz,
+					   depth + 1,
+					   last << 1 | !!(p->bitmap >> (i + 1)),
+					   i);
+		}
+	}
+
+	return buf;
+}
+
+static bool
+i915_syncmap_print_to_buf(struct i915_syncmap *p, char *buf, unsigned long sz)
+{
+	if (!p)
+		return false;
+
+	while (p->parent)
+		p = p->parent;
+
+	__sync_print(p, buf, &sz, 0, 1, 0);
+	return true;
+}
+
+static int check_syncmap_free(struct i915_syncmap **sync)
+{
+	i915_syncmap_free(sync);
+	if (*sync) {
+		pr_err("sync not cleared after free\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int dump_syncmap(struct i915_syncmap *sync, int err)
+{
+	char *buf;
+
+	if (!err)
+		return check_syncmap_free(&sync);
+
+	buf = kmalloc(PAGE_SIZE, GFP_KERNEL);
+	if (!buf)
+		goto skip;
+
+	if (i915_syncmap_print_to_buf(sync, buf, PAGE_SIZE))
+		pr_err("%s", buf);
+
+	kfree(buf);
+
+skip:
+	i915_syncmap_free(&sync);
+	return err;
+}
+
+static int igt_syncmap_init(void *arg)
+{
+	struct i915_syncmap *sync = (void *)~0ul;
+
+	/*
+	 * Cursory check that we can initialise a random pointer and transform
+	 * it into the root pointer of a syncmap.
+	 */
+
+	i915_syncmap_init(&sync);
+	return check_syncmap_free(&sync);
+}
+
+static int check_seqno(struct i915_syncmap *leaf, unsigned int idx, u32 seqno)
+{
+	if (leaf->height) {
+		pr_err("%s: not a leaf, height is %d\n",
+		       __func__, leaf->height);
+		return -EINVAL;
+	}
+
+	if (__sync_seqno(leaf)[idx] != seqno) {
+		pr_err("%s: seqno[%d], found %x, expected %x\n",
+		       __func__, idx, __sync_seqno(leaf)[idx], seqno);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int check_one(struct i915_syncmap **sync, u64 context, u32 seqno)
+{
+	int err;
+
+	err = i915_syncmap_set(sync, context, seqno);
+	if (err)
+		return err;
+
+	if ((*sync)->height) {
+		pr_err("Inserting first context=%llx did not return leaf (height=%d, prefix=%llx\n",
+		       context, (*sync)->height, (*sync)->prefix);
+		return -EINVAL;
+	}
+
+	if ((*sync)->parent) {
+		pr_err("Inserting first context=%llx created branches!\n",
+		       context);
+		return -EINVAL;
+	}
+
+	if (hweight32((*sync)->bitmap) != 1) {
+		pr_err("First bitmap does not contain a single entry, found %x (count=%d)!\n",
+		       (*sync)->bitmap, hweight32((*sync)->bitmap));
+		return -EINVAL;
+	}
+
+	err = check_seqno((*sync), ilog2((*sync)->bitmap), seqno);
+	if (err)
+		return err;
+
+	if (!i915_syncmap_is_later(sync, context, seqno)) {
+		pr_err("Lookup of first context=%llx/seqno=%x failed!\n",
+		       context, seqno);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int igt_syncmap_one(void *arg)
+{
+	I915_RND_STATE(prng);
+	IGT_TIMEOUT(end_time);
+	struct i915_syncmap *sync;
+	unsigned long max = 1;
+	int err;
+
+	/*
+	 * Check that inserting a new id, creates a leaf and only that leaf.
+	 */
+
+	i915_syncmap_init(&sync);
+
+	do {
+		u64 context = i915_prandom_u64_state(&prng);
+		unsigned long loop;
+
+		err = check_syncmap_free(&sync);
+		if (err)
+			goto out;
+
+		for (loop = 0; loop <= max; loop++) {
+			err = check_one(&sync, context,
+					prandom_u32_state(&prng));
+			if (err)
+				goto out;
+		}
+		max++;
+	} while (!__igt_timeout(end_time, NULL));
+	pr_debug("%s: Completed %lu single insertions\n",
+		 __func__, max * (max - 1) / 2);
+out:
+	return dump_syncmap(sync, err);
+}
+
+static int check_leaf(struct i915_syncmap **sync, u64 context, u32 seqno)
+{
+	int err;
+
+	err = i915_syncmap_set(sync, context, seqno);
+	if (err)
+		return err;
+
+	if ((*sync)->height) {
+		pr_err("Inserting context=%llx did not return leaf (height=%d, prefix=%llx\n",
+		       context, (*sync)->height, (*sync)->prefix);
+		return -EINVAL;
+	}
+
+	if (hweight32((*sync)->bitmap) != 1) {
+		pr_err("First entry into leaf (context=%llx) does not contain a single entry, found %x (count=%d)!\n",
+		       context, (*sync)->bitmap, hweight32((*sync)->bitmap));
+		return -EINVAL;
+	}
+
+	err = check_seqno((*sync), ilog2((*sync)->bitmap), seqno);
+	if (err)
+		return err;
+
+	if (!i915_syncmap_is_later(sync, context, seqno)) {
+		pr_err("Lookup of first entry context=%llx/seqno=%x failed!\n",
+		       context, seqno);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int igt_syncmap_join_above(void *arg)
+{
+	struct i915_syncmap *sync;
+	unsigned int pass, order;
+	int err;
+
+	i915_syncmap_init(&sync);
+
+	/*
+	 * When we have a new id that doesn't fit inside the existing tree,
+	 * we need to add a new layer above.
+	 *
+	 * 1: 0x00000001
+	 * 2: 0x00000010
+	 * 3: 0x00000100
+	 * 4: 0x00001000
+	 * ...
+	 * Each pass the common prefix shrinks and we have to insert a join.
+	 * Each join will only contain two branches, the latest of which
+	 * is always a leaf.
+	 *
+	 * If we then reuse the same set of contexts, we expect to build an
+	 * identical tree.
+	 */
+	for (pass = 0; pass < 3; pass++) {
+		for (order = 0; order < 64; order += SHIFT) {
+			u64 context = BIT_ULL(order);
+			struct i915_syncmap *join;
+
+			err = check_leaf(&sync, context, 0);
+			if (err)
+				goto out;
+
+			join = sync->parent;
+			if (!join) /* very first insert will have no parents */
+				continue;
+
+			if (!join->height) {
+				pr_err("Parent with no height!\n");
+				err = -EINVAL;
+				goto out;
+			}
+
+			if (hweight32(join->bitmap) != 2) {
+				pr_err("Join does not have 2 children: %x (%d)\n",
+				       join->bitmap, hweight32(join->bitmap));
+				err = -EINVAL;
+				goto out;
+			}
+
+			if (__sync_child(join)[__sync_branch_idx(join, context)] != sync) {
+				pr_err("Leaf misplaced in parent!\n");
+				err = -EINVAL;
+				goto out;
+			}
+		}
+	}
+out:
+	return dump_syncmap(sync, err);
+}
+
+static int igt_syncmap_join_below(void *arg)
+{
+	struct i915_syncmap *sync;
+	unsigned int step, order, idx;
+	int err;
+
+	i915_syncmap_init(&sync);
+
+	/*
+	 * Check that we can split a compacted branch by replacing it with
+	 * a join.
+	 */
+	for (step = 0; step < KSYNCMAP; step++) {
+		for (order = 64 - SHIFT; order > 0; order -= SHIFT) {
+			u64 context = step * BIT_ULL(order);
+
+			err = i915_syncmap_set(&sync, context, 0);
+			if (err)
+				goto out;
+
+			if (sync->height) {
+				pr_err("Inserting context=%llx (order=%d, step=%d) did not return leaf (height=%d, prefix=%llx\n",
+				       context, order, step, sync->height, sync->prefix);
+				err = -EINVAL;
+				goto out;
+			}
+		}
+	}
+
+	for (step = 0; step < KSYNCMAP; step++) {
+		for (order = SHIFT; order < 64; order += SHIFT) {
+			u64 context = step * BIT_ULL(order);
+
+			if (!i915_syncmap_is_later(&sync, context, 0)) {
+				pr_err("1: context %llx (order=%d, step=%d) not found\n",
+				       context, order, step);
+				err = -EINVAL;
+				goto out;
+			}
+
+			for (idx = 1; idx < KSYNCMAP; idx++) {
+				if (i915_syncmap_is_later(&sync, context + idx, 0)) {
+					pr_err("1: context %llx (order=%d, step=%d) should not exist\n",
+					       context + idx, order, step);
+					err = -EINVAL;
+					goto out;
+				}
+			}
+		}
+	}
+
+	for (order = SHIFT; order < 64; order += SHIFT) {
+		for (step = 0; step < KSYNCMAP; step++) {
+			u64 context = step * BIT_ULL(order);
+
+			if (!i915_syncmap_is_later(&sync, context, 0)) {
+				pr_err("2: context %llx (order=%d, step=%d) not found\n",
+				       context, order, step);
+				err = -EINVAL;
+				goto out;
+			}
+		}
+	}
+
+out:
+	return dump_syncmap(sync, err);
+}
+
+static int igt_syncmap_neighbours(void *arg)
+{
+	I915_RND_STATE(prng);
+	IGT_TIMEOUT(end_time);
+	struct i915_syncmap *sync;
+	int err;
+
+	/*
+	 * Each leaf holds KSYNCMAP seqno. Check that when we create KSYNCMAP
+	 * neighbouring ids, they all fit into the same leaf.
+	 */
+
+	i915_syncmap_init(&sync);
+	do {
+		u64 context = i915_prandom_u64_state(&prng) & ~MASK;
+		unsigned int idx;
+
+		if (i915_syncmap_is_later(&sync, context, 0)) /* Skip repeats */
+			continue;
+
+		for (idx = 0; idx < KSYNCMAP; idx++) {
+			err = i915_syncmap_set(&sync, context + idx, 0);
+			if (err)
+				goto out;
+
+			if (sync->height) {
+				pr_err("Inserting context=%llx did not return leaf (height=%d, prefix=%llx\n",
+				       context, sync->height, sync->prefix);
+				err = -EINVAL;
+				goto out;
+			}
+
+			if (sync->bitmap != BIT(idx + 1) - 1) {
+				pr_err("Inserting neighbouring context=0x%llx+%d, did not fit into the same leaf bitmap=%x (%d), expected %lx (%d)\n",
+				       context, idx,
+				       sync->bitmap, hweight32(sync->bitmap),
+				       BIT(idx + 1) - 1, idx + 1);
+				err = -EINVAL;
+				goto out;
+			}
+		}
+	} while (!__igt_timeout(end_time, NULL));
+out:
+	return dump_syncmap(sync, err);
+}
+
+static int igt_syncmap_compact(void *arg)
+{
+	struct i915_syncmap *sync;
+	unsigned int idx, order;
+	int err;
+
+	i915_syncmap_init(&sync);
+
+	/*
+	 * The syncmap are "space efficient" compressed radix trees - any
+	 * branch with only one child is skipped and replaced by the child.
+	 *
+	 * If we construct a tree with ids that are neighbouring at a non-zero
+	 * height, we form a join but each child of that join is directly a
+	 * leaf holding the single id.
+	 */
+	for (order = SHIFT; order < 64; order += SHIFT) {
+		err = check_syncmap_free(&sync);
+		if (err)
+			goto out;
+
+		/* Create neighbours in the parent */
+		for (idx = 0; idx < KSYNCMAP; idx++) {
+			u64 context = idx * BIT_ULL(order) + idx;
+
+			err = i915_syncmap_set(&sync, context, 0);
+			if (err)
+				goto out;
+
+			if (sync->height) {
+				pr_err("Inserting context=%llx (order=%d, idx=%d) did not return leaf (height=%d, prefix=%llx\n",
+				       context, order, idx,
+				       sync->height, sync->prefix);
+				err = -EINVAL;
+				goto out;
+			}
+		}
+
+		sync = sync->parent;
+		if (sync->parent) {
+			pr_err("Parent (join) of last leaf was not the sync!\n");
+			err = -EINVAL;
+			goto out;
+		}
+
+		if (sync->height != order) {
+			pr_err("Join does not have the expected height, found %d, expected %d\n",
+			       sync->height, order);
+			err = -EINVAL;
+			goto out;
+		}
+
+		if (sync->bitmap != BIT(KSYNCMAP) - 1) {
+			pr_err("Join is not full!, found %x (%d) expected %lx (%d)\n",
+			       sync->bitmap, hweight32(sync->bitmap),
+			       BIT(KSYNCMAP) - 1, KSYNCMAP);
+			err = -EINVAL;
+			goto out;
+		}
+
+		/* Each of our children should be a leaf */
+		for (idx = 0; idx < KSYNCMAP; idx++) {
+			struct i915_syncmap *leaf = __sync_child(sync)[idx];
+
+			if (leaf->height) {
+				pr_err("Child %d is a not leaf!\n", idx);
+				err = -EINVAL;
+				goto out;
+			}
+
+			if (leaf->parent != sync) {
+				pr_err("Child %d is not attached to us!\n",
+				       idx);
+				err = -EINVAL;
+				goto out;
+			}
+
+			if (!is_power_of_2(leaf->bitmap)) {
+				pr_err("Child %d holds more than one id, found %x (%d)\n",
+				       idx, leaf->bitmap, hweight32(leaf->bitmap));
+				err = -EINVAL;
+				goto out;
+			}
+
+			if (leaf->bitmap != BIT(idx)) {
+				pr_err("Child %d has wrong seqno idx, found %d, expected %d\n",
+				       idx, ilog2(leaf->bitmap), idx);
+				err = -EINVAL;
+				goto out;
+			}
+		}
+	}
+out:
+	return dump_syncmap(sync, err);
+}
+
+static int igt_syncmap_random(void *arg)
+{
+	I915_RND_STATE(prng);
+	IGT_TIMEOUT(end_time);
+	struct i915_syncmap *sync;
+	unsigned long count, phase, i;
+	u32 seqno;
+	int err;
+
+	i915_syncmap_init(&sync);
+
+	/*
+	 * Having tried to test the individual operations within i915_syncmap,
+	 * run a smoketest exploring the entire u64 space with random
+	 * insertions.
+	 */
+
+	count = 0;
+	phase = jiffies + HZ/100 + 1;
+	do {
+		u64 context = i915_prandom_u64_state(&prng);
+
+		err = i915_syncmap_set(&sync, context, 0);
+		if (err)
+			goto out;
+
+		count++;
+	} while (!time_after(jiffies, phase));
+	seqno = 0;
+
+	phase = 0;
+	do {
+		I915_RND_STATE(ctx);
+		u32 last_seqno = seqno;
+		bool expect;
+
+		seqno = prandom_u32_state(&prng);
+		expect = seqno_later(last_seqno, seqno);
+
+		for (i = 0; i < count; i++) {
+			u64 context = i915_prandom_u64_state(&ctx);
+
+			if (i915_syncmap_is_later(&sync, context, seqno) != expect) {
+				pr_err("context=%llu, last=%u this=%u did not match expectation (%d)\n",
+				       context, last_seqno, seqno, expect);
+				err = -EINVAL;
+				goto out;
+			}
+
+			err = i915_syncmap_set(&sync, context, seqno);
+			if (err)
+				goto out;
+		}
+
+		phase++;
+	} while (!__igt_timeout(end_time, NULL));
+	pr_debug("Completed %lu passes, each of %lu contexts\n", phase, count);
+out:
+	return dump_syncmap(sync, err);
+}
+
+int i915_syncmap_mock_selftests(void)
+{
+	static const struct i915_subtest tests[] = {
+		SUBTEST(igt_syncmap_init),
+		SUBTEST(igt_syncmap_one),
+		SUBTEST(igt_syncmap_join_above),
+		SUBTEST(igt_syncmap_join_below),
+		SUBTEST(igt_syncmap_neighbours),
+		SUBTEST(igt_syncmap_compact),
+		SUBTEST(igt_syncmap_random),
+	};
+
+	return i915_subtests(tests, NULL);
+}
diff --git a/drivers/gpu/drm/i915/selftests/mock_engine.c b/drivers/gpu/drm/i915/selftests/mock_engine.c
index 0ad624a1db90..5b18a2dc19a8 100644
--- a/drivers/gpu/drm/i915/selftests/mock_engine.c
+++ b/drivers/gpu/drm/i915/selftests/mock_engine.c
@@ -52,11 +52,12 @@ static void hw_delay_complete(unsigned long data)
 	spin_unlock(&engine->hw_lock);
 }
 
-static int mock_context_pin(struct intel_engine_cs *engine,
-			    struct i915_gem_context *ctx)
+static struct intel_ring *
+mock_context_pin(struct intel_engine_cs *engine,
+		 struct i915_gem_context *ctx)
 {
 	i915_gem_context_get(ctx);
-	return 0;
+	return engine->buffer;
 }
 
 static void mock_context_unpin(struct intel_engine_cs *engine,
@@ -72,7 +73,6 @@ static int mock_request_alloc(struct drm_i915_gem_request *request)
 	INIT_LIST_HEAD(&mock->link);
 	mock->delay = 0;
 
-	request->ring = request->engine->buffer;
 	return 0;
 }
 
@@ -112,7 +112,6 @@ static struct intel_ring *mock_ring(struct intel_engine_cs *engine)
 	if (!ring)
 		return NULL;
 
-	ring->engine = engine;
 	ring->size = sz;
 	ring->effective_size = sz;
 	ring->vaddr = (void *)(ring + 1);
@@ -141,7 +140,7 @@ struct intel_engine_cs *mock_engine(struct drm_i915_private *i915,
 
 	/* minimal engine setup for requests */
 	engine->base.i915 = i915;
-	engine->base.name = name;
+	snprintf(engine->base.name, sizeof(engine->base.name), "%s", name);
 	engine->base.id = id++;
 	engine->base.status_page.page_addr = (void *)(engine + 1);
 
diff --git a/drivers/gpu/drm/i915/selftests/mock_gem_device.c b/drivers/gpu/drm/i915/selftests/mock_gem_device.c
index 9f24c5da3f8d..627e2aa09766 100644
--- a/drivers/gpu/drm/i915/selftests/mock_gem_device.c
+++ b/drivers/gpu/drm/i915/selftests/mock_gem_device.c
@@ -30,6 +30,7 @@
 #include "mock_gem_device.h"
 #include "mock_gem_object.h"
 #include "mock_gtt.h"
+#include "mock_uncore.h"
 
 void mock_device_flush(struct drm_i915_private *i915)
 {
@@ -73,6 +74,7 @@ static void mock_device_release(struct drm_device *dev)
 
 	destroy_workqueue(i915->wq);
 
+	kmem_cache_destroy(i915->priorities);
 	kmem_cache_destroy(i915->dependencies);
 	kmem_cache_destroy(i915->requests);
 	kmem_cache_destroy(i915->vmas);
@@ -119,6 +121,7 @@ struct drm_i915_private *mock_gem_device(void)
 		goto err;
 
 	device_initialize(&pdev->dev);
+	pdev->class = PCI_BASE_CLASS_DISPLAY << 16;
 	pdev->dev.release = release_dev;
 	dev_set_name(&pdev->dev, "mock");
 	dma_coerce_mask_and_coherent(&pdev->dev, DMA_BIT_MASK(32));
@@ -143,6 +146,7 @@ struct drm_i915_private *mock_gem_device(void)
 	mkwrite_device_info(i915)->gen = -1;
 
 	spin_lock_init(&i915->mm.object_stat_lock);
+	mock_uncore_init(i915);
 
 	init_waitqueue_head(&i915->gpu_error.wait_queue);
 	init_waitqueue_head(&i915->gpu_error.reset_queue);
@@ -184,12 +188,16 @@ struct drm_i915_private *mock_gem_device(void)
 	if (!i915->dependencies)
 		goto err_requests;
 
+	i915->priorities = KMEM_CACHE(i915_priolist, SLAB_HWCACHE_ALIGN);
+	if (!i915->priorities)
+		goto err_dependencies;
+
 	mutex_lock(&i915->drm.struct_mutex);
 	INIT_LIST_HEAD(&i915->gt.timelines);
 	err = i915_gem_timeline_init__global(i915);
 	if (err) {
 		mutex_unlock(&i915->drm.struct_mutex);
-		goto err_dependencies;
+		goto err_priorities;
 	}
 
 	mock_init_ggtt(i915);
@@ -209,6 +217,8 @@ struct drm_i915_private *mock_gem_device(void)
 err_engine:
 	for_each_engine(engine, i915, id)
 		mock_engine_free(engine);
+err_priorities:
+	kmem_cache_destroy(i915->priorities);
 err_dependencies:
 	kmem_cache_destroy(i915->dependencies);
 err_requests:
diff --git a/drivers/gpu/drm/i915/selftests/mock_timeline.c b/drivers/gpu/drm/i915/selftests/mock_timeline.c
new file mode 100644
index 000000000000..47b1f47c5812
--- /dev/null
+++ b/drivers/gpu/drm/i915/selftests/mock_timeline.c
@@ -0,0 +1,45 @@
+/*
+ * Copyright © 2017 Intel Corporation
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
+ * IN THE SOFTWARE.
+ *
+ */
+
+#include "mock_timeline.h"
+
+struct intel_timeline *mock_timeline(u64 context)
+{
+	static struct lock_class_key class;
+	struct intel_timeline *tl;
+
+	tl = kzalloc(sizeof(*tl), GFP_KERNEL);
+	if (!tl)
+		return NULL;
+
+	__intel_timeline_init(tl, NULL, context, &class, "mock");
+
+	return tl;
+}
+
+void mock_timeline_destroy(struct intel_timeline *tl)
+{
+	__intel_timeline_fini(tl);
+	kfree(tl);
+}
diff --git a/drivers/gpu/drm/i915/selftests/mock_timeline.h b/drivers/gpu/drm/i915/selftests/mock_timeline.h
new file mode 100644
index 000000000000..c27ff4639b8b
--- /dev/null
+++ b/drivers/gpu/drm/i915/selftests/mock_timeline.h
@@ -0,0 +1,33 @@
+/*
+ * Copyright © 2017 Intel Corporation
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
+ * IN THE SOFTWARE.
+ *
+ */
+
+#ifndef __MOCK_TIMELINE__
+#define __MOCK_TIMELINE__
+
+#include "../i915_gem_timeline.h"
+
+struct intel_timeline *mock_timeline(u64 context);
+void mock_timeline_destroy(struct intel_timeline *tl);
+
+#endif /* !__MOCK_TIMELINE__ */
diff --git a/drivers/gpu/drm/i915/selftests/mock_uncore.c b/drivers/gpu/drm/i915/selftests/mock_uncore.c
new file mode 100644
index 000000000000..8ef14c7e5e38
--- /dev/null
+++ b/drivers/gpu/drm/i915/selftests/mock_uncore.c
@@ -0,0 +1,46 @@
+/*
+ * Copyright © 2017 Intel Corporation
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
+ * IN THE SOFTWARE.
+ *
+ */
+
+#include "mock_uncore.h"
+
+#define __nop_write(x) \
+static void \
+nop_write##x(struct drm_i915_private *dev_priv, i915_reg_t reg, u##x val, bool trace) { }
+__nop_write(8)
+__nop_write(16)
+__nop_write(32)
+
+#define __nop_read(x) \
+static u##x \
+nop_read##x(struct drm_i915_private *dev_priv, i915_reg_t reg, bool trace) { return 0; }
+__nop_read(8)
+__nop_read(16)
+__nop_read(32)
+__nop_read(64)
+
+void mock_uncore_init(struct drm_i915_private *i915)
+{
+	ASSIGN_WRITE_MMIO_VFUNCS(i915, nop);
+	ASSIGN_READ_MMIO_VFUNCS(i915, nop);
+}
diff --git a/drivers/gpu/drm/i915/selftests/mock_uncore.h b/drivers/gpu/drm/i915/selftests/mock_uncore.h
new file mode 100644
index 000000000000..d79aa3ca4d51
--- /dev/null
+++ b/drivers/gpu/drm/i915/selftests/mock_uncore.h
@@ -0,0 +1,30 @@
+/*
+ * Copyright © 2017 Intel Corporation
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
+ * IN THE SOFTWARE.
+ *
+ */
+
+#ifndef __MOCK_UNCORE_H
+#define __MOCK_UNCORE_H
+
+void mock_uncore_init(struct drm_i915_private *i915);
+
+#endif /* !__MOCK_UNCORE_H */
diff --git a/drivers/gpu/drm/nouveau/nouveau_display.c b/drivers/gpu/drm/nouveau/nouveau_display.c
index 6718c84fb862..8d1df5678eaa 100644
--- a/drivers/gpu/drm/nouveau/nouveau_display.c
+++ b/drivers/gpu/drm/nouveau/nouveau_display.c
@@ -337,6 +337,8 @@ nouveau_display_hpd_work(struct work_struct *work)
 	pm_runtime_get_sync(drm->dev->dev);
 
 	drm_helper_hpd_irq_event(drm->dev);
+	/* enable polling for external displays */
+	drm_kms_helper_poll_enable(drm->dev);
 
 	pm_runtime_mark_last_busy(drm->dev->dev);
 	pm_runtime_put_sync(drm->dev->dev);
@@ -390,10 +392,6 @@ nouveau_display_init(struct drm_device *dev)
 	if (ret)
 		return ret;
 
-	/* enable polling for external displays */
-	if (!dev->mode_config.poll_enabled)
-		drm_kms_helper_poll_enable(dev);
-
 	/* enable hotplug interrupts */
 	list_for_each_entry(connector, &dev->mode_config.connector_list, head) {
 		struct nouveau_connector *conn = nouveau_connector(connector);
diff --git a/drivers/gpu/drm/nouveau/nouveau_drm.c b/drivers/gpu/drm/nouveau/nouveau_drm.c
index c3dc75fee700..6844372366d3 100644
--- a/drivers/gpu/drm/nouveau/nouveau_drm.c
+++ b/drivers/gpu/drm/nouveau/nouveau_drm.c
@@ -502,6 +502,9 @@ nouveau_drm_load(struct drm_device *dev, unsigned long flags)
 		pm_runtime_allow(dev->dev);
 		pm_runtime_mark_last_busy(dev->dev);
 		pm_runtime_put(dev->dev);
+	} else {
+		/* enable polling for external displays */
+		drm_kms_helper_poll_enable(dev);
 	}
 	return 0;
 
@@ -774,9 +777,6 @@ nouveau_pmops_runtime_resume(struct device *dev)
 
 	ret = nouveau_do_resume(drm_dev, true);
 
-	if (!drm_dev->mode_config.poll_enabled)
-		drm_kms_helper_poll_enable(drm_dev);
-
 	/* do magic */
 	nvif_mask(&device->object, 0x088488, (1 << 25), (1 << 25));
 	vga_switcheroo_set_dynamic_switch(pdev, VGA_SWITCHEROO_ON);
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/fifo/gk104.c b/drivers/gpu/drm/nouveau/nvkm/engine/fifo/gk104.c
index 3a24788c3185..a7e55c422501 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/fifo/gk104.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/fifo/gk104.c
@@ -148,7 +148,7 @@ gk104_fifo_runlist_commit(struct gk104_fifo *fifo, int runl)
 	case NVKM_MEM_TARGET_NCOH: target = 3; break;
 	default:
 		WARN_ON(1);
-		return;
+		goto unlock;
 	}
 
 	nvkm_wr32(device, 0x002270, (nvkm_memory_addr(mem) >> 12) |
@@ -160,6 +160,7 @@ gk104_fifo_runlist_commit(struct gk104_fifo *fifo, int runl)
 				       & 0x00100000),
 			       msecs_to_jiffies(2000)) == 0)
 		nvkm_error(subdev, "runlist %d update timeout\n", runl);
+unlock:
 	mutex_unlock(&subdev->mutex);
 }
 
diff --git a/drivers/gpu/drm/nouveau/nvkm/subdev/secboot/ls_ucode_gr.c b/drivers/gpu/drm/nouveau/nvkm/subdev/secboot/ls_ucode_gr.c
index d1cf02d22db1..1b0c793c0192 100644
--- a/drivers/gpu/drm/nouveau/nvkm/subdev/secboot/ls_ucode_gr.c
+++ b/drivers/gpu/drm/nouveau/nvkm/subdev/secboot/ls_ucode_gr.c
@@ -116,6 +116,7 @@ ls_ucode_img_load_gr(const struct nvkm_subdev *subdev, struct ls_ucode_img *img,
 	ret = nvkm_firmware_get(subdev->device, f, &sig);
 	if (ret)
 		goto free_data;
+
 	img->sig = kmemdup(sig->data, sig->size, GFP_KERNEL);
 	if (!img->sig) {
 		ret = -ENOMEM;
@@ -126,8 +127,9 @@ ls_ucode_img_load_gr(const struct nvkm_subdev *subdev, struct ls_ucode_img *img,
 	img->ucode_data = ls_ucode_img_build(bl, code, data,
 					     &img->ucode_desc);
 	if (IS_ERR(img->ucode_data)) {
+		kfree(img->sig);
 		ret = PTR_ERR(img->ucode_data);
-		goto free_data;
+		goto free_sig;
 	}
 	img->ucode_size = img->ucode_desc.image_size;
 
diff --git a/drivers/gpu/drm/qxl/qxl_display.c b/drivers/gpu/drm/qxl/qxl_display.c
index f5ef81595f5a..03fe182203ce 100644
--- a/drivers/gpu/drm/qxl/qxl_display.c
+++ b/drivers/gpu/drm/qxl/qxl_display.c
@@ -574,8 +574,6 @@ static void qxl_cursor_atomic_update(struct drm_plane *plane,
 	if (ret)
 		return;
 
-	cmd = (struct qxl_cursor_cmd *) qxl_release_map(qdev, release);
-
 	if (fb != old_state->fb) {
 		obj = to_qxl_framebuffer(fb)->obj;
 		user_bo = gem_to_qxl_bo(obj);
@@ -613,6 +611,7 @@ static void qxl_cursor_atomic_update(struct drm_plane *plane,
 		qxl_bo_kunmap(cursor_bo);
 		qxl_bo_kunmap(user_bo);
 
+		cmd = (struct qxl_cursor_cmd *) qxl_release_map(qdev, release);
 		cmd->u.set.visible = 1;
 		cmd->u.set.shape = qxl_bo_physical_address(qdev,
 							   cursor_bo, 0);
@@ -623,6 +622,7 @@ static void qxl_cursor_atomic_update(struct drm_plane *plane,
 		if (ret)
 			goto out_free_release;
 
+		cmd = (struct qxl_cursor_cmd *) qxl_release_map(qdev, release);
 		cmd->type = QXL_CURSOR_MOVE;
 	}
 
diff --git a/drivers/gpu/drm/radeon/ci_dpm.c b/drivers/gpu/drm/radeon/ci_dpm.c
index f0cf99783c62..c97fbb2ab48b 100644
--- a/drivers/gpu/drm/radeon/ci_dpm.c
+++ b/drivers/gpu/drm/radeon/ci_dpm.c
@@ -776,6 +776,12 @@ bool ci_dpm_vblank_too_short(struct radeon_device *rdev)
 	u32 vblank_time = r600_dpm_get_vblank_time(rdev);
 	u32 switch_limit = pi->mem_gddr5 ? 450 : 300;
 
+	/* disable mclk switching if the refresh is >120Hz, even if the
+        * blanking period would allow it
+        */
+	if (r600_dpm_get_vrefresh(rdev) > 120)
+		return true;
+
 	if (vblank_time < switch_limit)
 		return true;
 	else
diff --git a/drivers/gpu/drm/radeon/cik.c b/drivers/gpu/drm/radeon/cik.c
index e368ce22bcc4..258912132b62 100644
--- a/drivers/gpu/drm/radeon/cik.c
+++ b/drivers/gpu/drm/radeon/cik.c
@@ -7401,7 +7401,7 @@ static inline void cik_irq_ack(struct radeon_device *rdev)
 		WREG32(DC_HPD5_INT_CONTROL, tmp);
 	}
 	if (rdev->irq.stat_regs.cik.disp_int_cont5 & DC_HPD6_INTERRUPT) {
-		tmp = RREG32(DC_HPD5_INT_CONTROL);
+		tmp = RREG32(DC_HPD6_INT_CONTROL);
 		tmp |= DC_HPDx_INT_ACK;
 		WREG32(DC_HPD6_INT_CONTROL, tmp);
 	}
@@ -7431,7 +7431,7 @@ static inline void cik_irq_ack(struct radeon_device *rdev)
 		WREG32(DC_HPD5_INT_CONTROL, tmp);
 	}
 	if (rdev->irq.stat_regs.cik.disp_int_cont5 & DC_HPD6_RX_INTERRUPT) {
-		tmp = RREG32(DC_HPD5_INT_CONTROL);
+		tmp = RREG32(DC_HPD6_INT_CONTROL);
 		tmp |= DC_HPDx_RX_INT_ACK;
 		WREG32(DC_HPD6_INT_CONTROL, tmp);
 	}
diff --git a/drivers/gpu/drm/radeon/evergreen.c b/drivers/gpu/drm/radeon/evergreen.c
index f130ec41ee4b..0bf103536404 100644
--- a/drivers/gpu/drm/radeon/evergreen.c
+++ b/drivers/gpu/drm/radeon/evergreen.c
@@ -4927,7 +4927,7 @@ static void evergreen_irq_ack(struct radeon_device *rdev)
 		WREG32(DC_HPD5_INT_CONTROL, tmp);
 	}
 	if (rdev->irq.stat_regs.evergreen.disp_int_cont5 & DC_HPD6_INTERRUPT) {
-		tmp = RREG32(DC_HPD5_INT_CONTROL);
+		tmp = RREG32(DC_HPD6_INT_CONTROL);
 		tmp |= DC_HPDx_INT_ACK;
 		WREG32(DC_HPD6_INT_CONTROL, tmp);
 	}
@@ -4958,7 +4958,7 @@ static void evergreen_irq_ack(struct radeon_device *rdev)
 		WREG32(DC_HPD5_INT_CONTROL, tmp);
 	}
 	if (rdev->irq.stat_regs.evergreen.disp_int_cont5 & DC_HPD6_RX_INTERRUPT) {
-		tmp = RREG32(DC_HPD5_INT_CONTROL);
+		tmp = RREG32(DC_HPD6_INT_CONTROL);
 		tmp |= DC_HPDx_RX_INT_ACK;
 		WREG32(DC_HPD6_INT_CONTROL, tmp);
 	}
diff --git a/drivers/gpu/drm/radeon/r600.c b/drivers/gpu/drm/radeon/r600.c
index 0a085176e79b..e06e2d8feab3 100644
--- a/drivers/gpu/drm/radeon/r600.c
+++ b/drivers/gpu/drm/radeon/r600.c
@@ -3988,7 +3988,7 @@ static void r600_irq_ack(struct radeon_device *rdev)
 			WREG32(DC_HPD5_INT_CONTROL, tmp);
 		}
 		if (rdev->irq.stat_regs.r600.disp_int_cont2 & DC_HPD6_INTERRUPT) {
-			tmp = RREG32(DC_HPD5_INT_CONTROL);
+			tmp = RREG32(DC_HPD6_INT_CONTROL);
 			tmp |= DC_HPDx_INT_ACK;
 			WREG32(DC_HPD6_INT_CONTROL, tmp);
 		}
diff --git a/drivers/gpu/drm/radeon/radeon_kms.c b/drivers/gpu/drm/radeon/radeon_kms.c
index 6a68d440bc44..d0ad03674250 100644
--- a/drivers/gpu/drm/radeon/radeon_kms.c
+++ b/drivers/gpu/drm/radeon/radeon_kms.c
@@ -116,7 +116,7 @@ int radeon_driver_load_kms(struct drm_device *dev, unsigned long flags)
 	if ((radeon_runtime_pm != 0) &&
 	    radeon_has_atpx() &&
 	    ((flags & RADEON_IS_IGP) == 0) &&
-	    !pci_is_thunderbolt_attached(rdev->pdev))
+	    !pci_is_thunderbolt_attached(dev->pdev))
 		flags |= RADEON_IS_PX;
 
 	/* radeon_device_init should report only fatal error
diff --git a/drivers/gpu/drm/radeon/si.c b/drivers/gpu/drm/radeon/si.c
index ceee87f029d9..76d1888528e6 100644
--- a/drivers/gpu/drm/radeon/si.c
+++ b/drivers/gpu/drm/radeon/si.c
@@ -6317,7 +6317,7 @@ static inline void si_irq_ack(struct radeon_device *rdev)
 		WREG32(DC_HPD5_INT_CONTROL, tmp);
 	}
 	if (rdev->irq.stat_regs.evergreen.disp_int_cont5 & DC_HPD6_INTERRUPT) {
-		tmp = RREG32(DC_HPD5_INT_CONTROL);
+		tmp = RREG32(DC_HPD6_INT_CONTROL);
 		tmp |= DC_HPDx_INT_ACK;
 		WREG32(DC_HPD6_INT_CONTROL, tmp);
 	}
@@ -6348,7 +6348,7 @@ static inline void si_irq_ack(struct radeon_device *rdev)
 		WREG32(DC_HPD5_INT_CONTROL, tmp);
 	}
 	if (rdev->irq.stat_regs.evergreen.disp_int_cont5 & DC_HPD6_RX_INTERRUPT) {
-		tmp = RREG32(DC_HPD5_INT_CONTROL);
+		tmp = RREG32(DC_HPD6_INT_CONTROL);
 		tmp |= DC_HPDx_RX_INT_ACK;
 		WREG32(DC_HPD6_INT_CONTROL, tmp);
 	}
diff --git a/drivers/gpu/drm/rockchip/analogix_dp-rockchip.c b/drivers/gpu/drm/rockchip/analogix_dp-rockchip.c
index 1bccd827d2e4..9606121fa185 100644
--- a/drivers/gpu/drm/rockchip/analogix_dp-rockchip.c
+++ b/drivers/gpu/drm/rockchip/analogix_dp-rockchip.c
@@ -237,8 +237,6 @@ rockchip_dp_drm_encoder_atomic_check(struct drm_encoder *encoder,
 				      struct drm_connector_state *conn_state)
 {
 	struct rockchip_crtc_state *s = to_rockchip_crtc_state(crtc_state);
-	struct rockchip_dp_device *dp = to_dp(encoder);
-	int ret;
 
 	/*
 	 * The hardware IC designed that VOP must output the RGB10 video
@@ -250,16 +248,6 @@ rockchip_dp_drm_encoder_atomic_check(struct drm_encoder *encoder,
 
 	s->output_mode = ROCKCHIP_OUT_MODE_AAAA;
 	s->output_type = DRM_MODE_CONNECTOR_eDP;
-	if (dp->data->chip_type == RK3399_EDP) {
-		/*
-		 * For RK3399, VOP Lit must code the out mode to RGB888,
-		 * VOP Big must code the out mode to RGB10.
-		 */
-		ret = drm_of_encoder_active_endpoint_id(dp->dev->of_node,
-							encoder);
-		if (ret > 0)
-			s->output_mode = ROCKCHIP_OUT_MODE_P888;
-	}
 
 	return 0;
 }
diff --git a/drivers/gpu/drm/rockchip/cdn-dp-core.c b/drivers/gpu/drm/rockchip/cdn-dp-core.c
index a2169dd3d26b..14fa1f8351e8 100644
--- a/drivers/gpu/drm/rockchip/cdn-dp-core.c
+++ b/drivers/gpu/drm/rockchip/cdn-dp-core.c
@@ -615,7 +615,6 @@ static void cdn_dp_encoder_enable(struct drm_encoder *encoder)
 {
 	struct cdn_dp_device *dp = encoder_to_dp(encoder);
 	int ret, val;
-	struct rockchip_crtc_state *state;
 
 	ret = drm_of_encoder_active_endpoint_id(dp->dev->of_node, encoder);
 	if (ret < 0) {
@@ -625,14 +624,10 @@ static void cdn_dp_encoder_enable(struct drm_encoder *encoder)
 
 	DRM_DEV_DEBUG_KMS(dp->dev, "vop %s output to cdn-dp\n",
 			  (ret) ? "LIT" : "BIG");
-	state = to_rockchip_crtc_state(encoder->crtc->state);
-	if (ret) {
+	if (ret)
 		val = DP_SEL_VOP_LIT | (DP_SEL_VOP_LIT << 16);
-		state->output_mode = ROCKCHIP_OUT_MODE_P888;
-	} else {
+	else
 		val = DP_SEL_VOP_LIT << 16;
-		state->output_mode = ROCKCHIP_OUT_MODE_AAAA;
-	}
 
 	ret = cdn_dp_grf_write(dp, GRF_SOC_CON9, val);
 	if (ret)
diff --git a/drivers/gpu/drm/rockchip/rockchip_drm_vop.c b/drivers/gpu/drm/rockchip/rockchip_drm_vop.c
index 9b3525a36969..5d450332c2fd 100644
--- a/drivers/gpu/drm/rockchip/rockchip_drm_vop.c
+++ b/drivers/gpu/drm/rockchip/rockchip_drm_vop.c
@@ -874,6 +874,7 @@ static bool vop_crtc_mode_fixup(struct drm_crtc *crtc,
 static void vop_crtc_enable(struct drm_crtc *crtc)
 {
 	struct vop *vop = to_vop(crtc);
+	const struct vop_data *vop_data = vop->data;
 	struct rockchip_crtc_state *s = to_rockchip_crtc_state(crtc->state);
 	struct drm_display_mode *adjusted_mode = &crtc->state->adjusted_mode;
 	u16 hsync_len = adjusted_mode->hsync_end - adjusted_mode->hsync_start;
@@ -966,6 +967,13 @@ static void vop_crtc_enable(struct drm_crtc *crtc)
 		DRM_DEV_ERROR(vop->dev, "unsupported connector_type [%d]\n",
 			      s->output_type);
 	}
+
+	/*
+	 * if vop is not support RGB10 output, need force RGB10 to RGB888.
+	 */
+	if (s->output_mode == ROCKCHIP_OUT_MODE_AAAA &&
+	    !(vop_data->feature & VOP_FEATURE_OUTPUT_RGB10))
+		s->output_mode = ROCKCHIP_OUT_MODE_P888;
 	VOP_CTRL_SET(vop, out_mode, s->output_mode);
 
 	VOP_CTRL_SET(vop, htotal_pw, (htotal << 16) | hsync_len);
diff --git a/drivers/gpu/drm/rockchip/rockchip_drm_vop.h b/drivers/gpu/drm/rockchip/rockchip_drm_vop.h
index 5a4faa85dbd2..9979fd0c2282 100644
--- a/drivers/gpu/drm/rockchip/rockchip_drm_vop.h
+++ b/drivers/gpu/drm/rockchip/rockchip_drm_vop.h
@@ -142,6 +142,9 @@ struct vop_data {
 	const struct vop_intr *intr;
 	const struct vop_win_data *win;
 	unsigned int win_size;
+
+#define VOP_FEATURE_OUTPUT_RGB10	BIT(0)
+	u64 feature;
 };
 
 /* interrupt define */
diff --git a/drivers/gpu/drm/rockchip/rockchip_vop_reg.c b/drivers/gpu/drm/rockchip/rockchip_vop_reg.c
index 0da44442aab0..bafd698a28b1 100644
--- a/drivers/gpu/drm/rockchip/rockchip_vop_reg.c
+++ b/drivers/gpu/drm/rockchip/rockchip_vop_reg.c
@@ -275,6 +275,7 @@ static const struct vop_intr rk3288_vop_intr = {
 static const struct vop_data rk3288_vop = {
 	.init_table = rk3288_init_reg_table,
 	.table_size = ARRAY_SIZE(rk3288_init_reg_table),
+	.feature = VOP_FEATURE_OUTPUT_RGB10,
 	.intr = &rk3288_vop_intr,
 	.ctrl = &rk3288_ctrl_data,
 	.win = rk3288_vop_win_data,
@@ -343,6 +344,7 @@ static const struct vop_reg_data rk3399_init_reg_table[] = {
 static const struct vop_data rk3399_vop_big = {
 	.init_table = rk3399_init_reg_table,
 	.table_size = ARRAY_SIZE(rk3399_init_reg_table),
+	.feature = VOP_FEATURE_OUTPUT_RGB10,
 	.intr = &rk3399_vop_intr,
 	.ctrl = &rk3399_ctrl_data,
 	/*
diff --git a/drivers/hid/Kconfig b/drivers/hid/Kconfig
index fe40e5e499dd..d575e1852d8c 100644
--- a/drivers/hid/Kconfig
+++ b/drivers/hid/Kconfig
@@ -275,10 +275,12 @@ config HID_EMS_FF
 	 - Trio Linker Plus II
 
 config HID_ELECOM
-	tristate "ELECOM BM084 bluetooth mouse"
+	tristate "ELECOM HID devices"
 	depends on HID
 	---help---
-	Support for the ELECOM BM084 (bluetooth mouse).
+	Support for ELECOM devices:
+	  - BM084 Bluetooth Mouse
+	  - DEFT Trackball (Wired and wireless)
 
 config HID_ELO
 	tristate "ELO USB 4000/4500 touchscreen"
@@ -386,6 +388,13 @@ config HID_ICADE
 	To compile this driver as a module, choose M here: the
 	module will be called hid-icade.
 
+config HID_ITE
+	tristate "ITE devices"
+	depends on HID
+	default !EXPERT
+	---help---
+	Support for ITE devices not fully compliant with HID standard.
+
 config HID_TWINHAN
 	tristate "Twinhan IR remote control"
 	depends on HID
diff --git a/drivers/hid/Makefile b/drivers/hid/Makefile
index fef027bc7fa3..05ac8d375aeb 100644
--- a/drivers/hid/Makefile
+++ b/drivers/hid/Makefile
@@ -50,6 +50,7 @@ obj-$(CONFIG_HID_HOLTEK)	+= hid-holtek-mouse.o
 obj-$(CONFIG_HID_HOLTEK)	+= hid-holtekff.o
 obj-$(CONFIG_HID_HYPERV_MOUSE)	+= hid-hyperv.o
 obj-$(CONFIG_HID_ICADE)		+= hid-icade.o
+obj-$(CONFIG_HID_ITE)		+= hid-ite.o
 obj-$(CONFIG_HID_KENSINGTON)	+= hid-kensington.o
 obj-$(CONFIG_HID_KEYTOUCH)	+= hid-keytouch.o
 obj-$(CONFIG_HID_KYE)		+= hid-kye.o
diff --git a/drivers/hid/hid-asus.c b/drivers/hid/hid-asus.c
index 16df6cc90235..101ab2e63d18 100644
--- a/drivers/hid/hid-asus.c
+++ b/drivers/hid/hid-asus.c
@@ -69,6 +69,7 @@ MODULE_DESCRIPTION("Asus HID Keyboard and TouchPad");
 #define QUIRK_IS_MULTITOUCH		BIT(3)
 #define QUIRK_NO_CONSUMER_USAGES	BIT(4)
 #define QUIRK_USE_KBD_BACKLIGHT		BIT(5)
+#define QUIRK_T100_KEYBOARD		BIT(6)
 
 #define I2C_KEYBOARD_QUIRKS			(QUIRK_FIX_NOTEBOOK_REPORT | \
 						 QUIRK_NO_INIT_REPORTS | \
@@ -548,6 +549,12 @@ static __u8 *asus_report_fixup(struct hid_device *hdev, __u8 *rdesc,
 		hid_info(hdev, "Fixing up Asus notebook report descriptor\n");
 		rdesc[55] = 0xdd;
 	}
+	if (drvdata->quirks & QUIRK_T100_KEYBOARD &&
+		 *rsize == 76 && rdesc[73] == 0x81 && rdesc[74] == 0x01) {
+		hid_info(hdev, "Fixing up Asus T100 keyb report descriptor\n");
+		rdesc[74] &= ~HID_MAIN_ITEM_CONSTANT;
+	}
+
 	return rdesc;
 }
 
@@ -560,6 +567,9 @@ static const struct hid_device_id asus_devices[] = {
 		USB_DEVICE_ID_ASUSTEK_ROG_KEYBOARD1) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ASUSTEK,
 		USB_DEVICE_ID_ASUSTEK_ROG_KEYBOARD2), QUIRK_USE_KBD_BACKLIGHT },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_ASUSTEK,
+		USB_DEVICE_ID_ASUSTEK_T100_KEYBOARD),
+	  QUIRK_T100_KEYBOARD | QUIRK_NO_CONSUMER_USAGES },
 	{ }
 };
 MODULE_DEVICE_TABLE(hid, asus_devices);
diff --git a/drivers/hid/hid-core.c b/drivers/hid/hid-core.c
index 37084b645785..ec214a9117a1 100644
--- a/drivers/hid/hid-core.c
+++ b/drivers/hid/hid-core.c
@@ -1855,6 +1855,7 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ 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) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_AUREAL, USB_DEVICE_ID_AUREAL_W01RN) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_BELKIN, USB_DEVICE_ID_FLIP_KVM) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_BETOP_2185BFM, 0x2208) },
@@ -1891,6 +1892,8 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ 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_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) },
 	{ 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) },
@@ -1913,6 +1916,7 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ 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_ITE, USB_DEVICE_ID_ITE8595) },
 	{ 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) },
 	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_ION, USB_DEVICE_ID_ICADE) },
diff --git a/drivers/hid/hid-elecom.c b/drivers/hid/hid-elecom.c
index 6e3848a8d8dd..e2c7465df69f 100644
--- a/drivers/hid/hid-elecom.c
+++ b/drivers/hid/hid-elecom.c
@@ -1,10 +1,8 @@
 /*
- *  HID driver for Elecom BM084 (bluetooth mouse).
- *  Removes a non-existing horizontal wheel from
- *  the HID descriptor.
- *  (This module is based on "hid-ortek".)
- *
+ *  HID driver for ELECOM devices.
  *  Copyright (c) 2010 Richard Nauber <Richard.Nauber@gmail.com>
+ *  Copyright (c) 2016 Yuxuan Shui <yshuiv7@gmail.com>
+ *  Copyright (c) 2017 Diego Elio Pettenò <flameeyes@flameeyes.eu>
  */
 
 /*
@@ -23,15 +21,61 @@
 static __u8 *elecom_report_fixup(struct hid_device *hdev, __u8 *rdesc,
 		unsigned int *rsize)
 {
-	if (*rsize >= 48 && rdesc[46] == 0x05 && rdesc[47] == 0x0c) {
-		hid_info(hdev, "Fixing up Elecom BM084 report descriptor\n");
-		rdesc[47] = 0x00;
+	switch (hdev->product) {
+	case USB_DEVICE_ID_ELECOM_BM084:
+		/* The BM084 Bluetooth mouse includes a non-existing horizontal
+		 * wheel in the HID descriptor. */
+		if (*rsize >= 48 && rdesc[46] == 0x05 && rdesc[47] == 0x0c) {
+			hid_info(hdev, "Fixing up Elecom BM084 report descriptor\n");
+			rdesc[47] = 0x00;
+		}
+		break;
+	case USB_DEVICE_ID_ELECOM_DEFT_WIRED:
+	case USB_DEVICE_ID_ELECOM_DEFT_WIRELESS:
+		/* The DEFT trackball has eight buttons, but its descriptor only
+		 * reports five, disabling the three Fn buttons on the top of
+		 * the mouse.
+		 *
+		 * Apply the following diff to the descriptor:
+		 *
+		 * Collection (Physical),              Collection (Physical),
+		 *     Report ID (1),                      Report ID (1),
+		 *     Report Count (5),           ->      Report Count (8),
+		 *     Report Size (1),                    Report Size (1),
+		 *     Usage Page (Button),                Usage Page (Button),
+		 *     Usage Minimum (01h),                Usage Minimum (01h),
+		 *     Usage Maximum (05h),        ->      Usage Maximum (08h),
+		 *     Logical Minimum (0),                Logical Minimum (0),
+		 *     Logical Maximum (1),                Logical Maximum (1),
+		 *     Input (Variable),                   Input (Variable),
+		 *     Report Count (1),           ->      Report Count (0),
+		 *     Report Size (3),                    Report Size (3),
+		 *     Input (Constant),                   Input (Constant),
+		 *     Report Size (16),                   Report Size (16),
+		 *     Report Count (2),                   Report Count (2),
+		 *     Usage Page (Desktop),               Usage Page (Desktop),
+		 *     Usage (X),                          Usage (X),
+		 *     Usage (Y),                          Usage (Y),
+		 *     Logical Minimum (-32768),           Logical Minimum (-32768),
+		 *     Logical Maximum (32767),            Logical Maximum (32767),
+		 *     Input (Variable, Relative),         Input (Variable, Relative),
+		 * End Collection,                     End Collection,
+		 */
+		if (*rsize == 213 && rdesc[13] == 5 && rdesc[21] == 5) {
+			hid_info(hdev, "Fixing up Elecom DEFT Fn buttons\n");
+			rdesc[13] = 8; /* Button/Variable Report Count */
+			rdesc[21] = 8; /* Button/Variable Usage Maximum */
+			rdesc[29] = 0; /* Button/Constant Report Count */
+		}
+		break;
 	}
 	return rdesc;
 }
 
 static const struct hid_device_id elecom_devices[] = {
-	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_ELECOM, USB_DEVICE_ID_ELECOM_BM084)},
+	{ 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) },
 	{ }
 };
 MODULE_DEVICE_TABLE(hid, elecom_devices);
diff --git a/drivers/hid/hid-ids.h b/drivers/hid/hid-ids.h
index 643390ba749d..8a027c112d90 100644
--- a/drivers/hid/hid-ids.h
+++ b/drivers/hid/hid-ids.h
@@ -173,6 +173,7 @@
 #define USB_VENDOR_ID_ASUSTEK		0x0b05
 #define USB_DEVICE_ID_ASUSTEK_LCM	0x1726
 #define USB_DEVICE_ID_ASUSTEK_LCM2	0x175b
+#define USB_DEVICE_ID_ASUSTEK_T100_KEYBOARD	0x17e0
 #define USB_DEVICE_ID_ASUSTEK_I2C_KEYBOARD	0x8585
 #define USB_DEVICE_ID_ASUSTEK_I2C_TOUCHPAD	0x0101
 #define USB_DEVICE_ID_ASUSTEK_ROG_KEYBOARD1 0x1854
@@ -358,6 +359,8 @@
 
 #define USB_VENDOR_ID_ELECOM		0x056e
 #define USB_DEVICE_ID_ELECOM_BM084	0x0061
+#define USB_DEVICE_ID_ELECOM_DEFT_WIRED	0x00fe
+#define USB_DEVICE_ID_ELECOM_DEFT_WIRELESS	0x00ff
 
 #define USB_VENDOR_ID_DREAM_CHEEKY	0x1d34
 #define USB_DEVICE_ID_DREAM_CHEEKY_WN	0x0004
@@ -559,6 +562,7 @@
 #define USB_DEVICE_ID_ITE_LENOVO_YOGA   0x8386
 #define USB_DEVICE_ID_ITE_LENOVO_YOGA2  0x8350
 #define USB_DEVICE_ID_ITE_LENOVO_YOGA900	0x8396
+#define USB_DEVICE_ID_ITE8595		0x8595
 
 #define USB_VENDOR_ID_JABRA		0x0b0e
 #define USB_DEVICE_ID_JABRA_SPEAK_410	0x0412
diff --git a/drivers/hid/hid-input.c b/drivers/hid/hid-input.c
index a1ebdd7d4d4d..ccdff1ee1f0c 100644
--- a/drivers/hid/hid-input.c
+++ b/drivers/hid/hid-input.c
@@ -656,6 +656,15 @@ static void hidinput_configure_usage(struct hid_input *hidinput, struct hid_fiel
 		case HID_GD_START:	map_key_clear(BTN_START);	break;
 		case HID_GD_SELECT:	map_key_clear(BTN_SELECT);	break;
 
+		case HID_GD_RFKILL_BTN:
+			/* MS wireless radio ctl extension, also check CA */
+			if (field->application == HID_GD_WIRELESS_RADIO_CTLS) {
+				map_key_clear(KEY_RFKILL);
+				/* We need to simulate the btn release */
+				field->flags |= HID_MAIN_ITEM_RELATIVE;
+				break;
+			}
+
 		default: goto unknown;
 		}
 
diff --git a/drivers/hid/hid-ite.c b/drivers/hid/hid-ite.c
new file mode 100644
index 000000000000..1882a4ab0f29
--- /dev/null
+++ b/drivers/hid/hid-ite.c
@@ -0,0 +1,56 @@
+/*
+ * HID driver for some ITE "special" devices
+ * Copyright (c) 2017 Hans de Goede <hdegoede@redhat.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/device.h>
+#include <linux/input.h>
+#include <linux/hid.h>
+#include <linux/module.h>
+
+#include "hid-ids.h"
+
+static int ite_event(struct hid_device *hdev, struct hid_field *field,
+		     struct hid_usage *usage, __s32 value)
+{
+	struct input_dev *input;
+
+	if (!(hdev->claimed & HID_CLAIMED_INPUT) || !field->hidinput)
+		return 0;
+
+	input = field->hidinput->input;
+
+	/*
+	 * The ITE8595 always reports 0 as value for the rfkill button. Luckily
+	 * it is the only button in its report, and it sends a report on
+	 * release only, so receiving a report means the button was pressed.
+	 */
+	if (usage->hid == HID_GD_RFKILL_BTN) {
+		input_event(input, EV_KEY, KEY_RFKILL, 1);
+		input_sync(input);
+		input_event(input, EV_KEY, KEY_RFKILL, 0);
+		input_sync(input);
+		return 1;
+	}
+
+	return 0;
+}
+
+static const struct hid_device_id ite_devices[] = {
+	{ HID_USB_DEVICE(USB_VENDOR_ID_ITE, USB_DEVICE_ID_ITE8595) },
+	{ }
+};
+MODULE_DEVICE_TABLE(hid, ite_devices);
+
+static struct hid_driver ite_driver = {
+	.name = "itetech",
+	.id_table = ite_devices,
+	.event = ite_event,
+};
+module_hid_driver(ite_driver);
+
+MODULE_LICENSE("GPL");
diff --git a/drivers/hid/hid-magicmouse.c b/drivers/hid/hid-magicmouse.c
index 20b40ad26325..1d6c997b3001 100644
--- a/drivers/hid/hid-magicmouse.c
+++ b/drivers/hid/hid-magicmouse.c
@@ -349,6 +349,7 @@ 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 */
@@ -388,16 +389,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 8daa8ce64ebb..841aa43526eb 100644
--- a/drivers/hid/i2c-hid/i2c-hid.c
+++ b/drivers/hid/i2c-hid/i2c-hid.c
@@ -40,7 +40,7 @@
 #include <linux/of.h>
 #include <linux/regulator/consumer.h>
 
-#include <linux/i2c/i2c-hid.h>
+#include <linux/platform_data/i2c-hid.h>
 
 #include "../hid-ids.h"
 
diff --git a/drivers/hid/intel-ish-hid/Kconfig b/drivers/hid/intel-ish-hid/Kconfig
index ea065b3684a2..519e4c8b53c4 100644
--- a/drivers/hid/intel-ish-hid/Kconfig
+++ b/drivers/hid/intel-ish-hid/Kconfig
@@ -1,5 +1,5 @@
 menu "Intel ISH HID support"
-	depends on X86_64 && PCI
+	depends on (X86_64 || COMPILE_TEST) && PCI
 
 config INTEL_ISH_HID
 	tristate "Intel Integrated Sensor Hub"
diff --git a/drivers/hid/intel-ish-hid/ipc/ipc.c b/drivers/hid/intel-ish-hid/ipc/ipc.c
index 842d8416a7a6..9a60ec13cb10 100644
--- a/drivers/hid/intel-ish-hid/ipc/ipc.c
+++ b/drivers/hid/intel-ish-hid/ipc/ipc.c
@@ -296,17 +296,12 @@ static int write_ipc_from_queue(struct ishtp_device *dev)
 	/* If sending MNG_SYNC_FW_CLOCK, update clock again */
 	if (IPC_HEADER_GET_PROTOCOL(doorbell_val) == IPC_PROTOCOL_MNG &&
 		IPC_HEADER_GET_MNG_CMD(doorbell_val) == MNG_SYNC_FW_CLOCK) {
-		struct timespec ts_system;
-		struct timeval tv_utc;
-		uint64_t        usec_system, usec_utc;
+		uint64_t usec_system, usec_utc;
 		struct ipc_time_update_msg time_update;
 		struct time_sync_format ts_format;
 
-		get_monotonic_boottime(&ts_system);
-		do_gettimeofday(&tv_utc);
-		usec_system = (timespec_to_ns(&ts_system)) / NSEC_PER_USEC;
-		usec_utc = (uint64_t)tv_utc.tv_sec * 1000000 +
-						((uint32_t)tv_utc.tv_usec);
+		usec_system = ktime_to_us(ktime_get_boottime());
+		usec_utc = ktime_to_us(ktime_get_real());
 		ts_format.ts1_source = HOST_SYSTEM_TIME_USEC;
 		ts_format.ts2_source = HOST_UTC_TIME_USEC;
 		ts_format.reserved = 0;
@@ -575,15 +570,13 @@ static void fw_reset_work_fn(struct work_struct *unused)
 static void _ish_sync_fw_clock(struct ishtp_device *dev)
 {
 	static unsigned long	prev_sync;
-	struct timespec	ts;
 	uint64_t	usec;
 
 	if (prev_sync && jiffies - prev_sync < 20 * HZ)
 		return;
 
 	prev_sync = jiffies;
-	get_monotonic_boottime(&ts);
-	usec = (timespec_to_ns(&ts)) / NSEC_PER_USEC;
+	usec = ktime_to_us(ktime_get_boottime());
 	ipc_send_mng_msg(dev, MNG_SYNC_FW_CLOCK, &usec, sizeof(uint64_t));
 }
 
diff --git a/drivers/hid/intel-ish-hid/ishtp-hid-client.c b/drivers/hid/intel-ish-hid/ishtp-hid-client.c
index 5c643d7a07b2..157b44aacdff 100644
--- a/drivers/hid/intel-ish-hid/ishtp-hid-client.c
+++ b/drivers/hid/intel-ish-hid/ishtp-hid-client.c
@@ -136,10 +136,9 @@ static void process_recv(struct ishtp_cl *hid_ishtp_cl, void *recv_buf,
 				if (1 + sizeof(struct device_info) * i >=
 						payload_len) {
 					dev_err(&client_data->cl_device->dev,
-						"[hid-ish]: [ENUM_DEVICES]: content size %lu is bigger than payload_len %u\n",
+						"[hid-ish]: [ENUM_DEVICES]: content size %zu is bigger than payload_len %zu\n",
 						1 + sizeof(struct device_info)
-						* i,
-						(unsigned int)payload_len);
+						* i, payload_len);
 				}
 
 				if (1 + sizeof(struct device_info) * i >=
diff --git a/drivers/hid/intel-ish-hid/ishtp/client.c b/drivers/hid/intel-ish-hid/ishtp/client.c
index aad61328f282..007443ef5fca 100644
--- a/drivers/hid/intel-ish-hid/ishtp/client.c
+++ b/drivers/hid/intel-ish-hid/ishtp/client.c
@@ -803,7 +803,7 @@ void ishtp_cl_send_msg(struct ishtp_device *dev, struct ishtp_cl *cl)
  * @ishtp_hdr: Pointer to message header
  *
  * Receive and dispatch ISHTP client messages. This function executes in ISR
- * context
+ * or work queue context
  */
 void recv_ishtp_cl_msg(struct ishtp_device *dev,
 		       struct ishtp_msg_hdr *ishtp_hdr)
@@ -813,7 +813,6 @@ void recv_ishtp_cl_msg(struct ishtp_device *dev,
 	struct ishtp_cl_rb *new_rb;
 	unsigned char *buffer = NULL;
 	struct ishtp_cl_rb *complete_rb = NULL;
-	unsigned long	dev_flags;
 	unsigned long	flags;
 	int	rb_count;
 
@@ -828,7 +827,7 @@ void recv_ishtp_cl_msg(struct ishtp_device *dev,
 		goto	eoi;
 	}
 
-	spin_lock_irqsave(&dev->read_list_spinlock, dev_flags);
+	spin_lock_irqsave(&dev->read_list_spinlock, flags);
 	rb_count = -1;
 	list_for_each_entry(rb, &dev->read_list.list, list) {
 		++rb_count;
@@ -840,8 +839,7 @@ void recv_ishtp_cl_msg(struct ishtp_device *dev,
 
 		 /* If no Rx buffer is allocated, disband the rb */
 		if (rb->buffer.size == 0 || rb->buffer.data == NULL) {
-			spin_unlock_irqrestore(&dev->read_list_spinlock,
-				dev_flags);
+			spin_unlock_irqrestore(&dev->read_list_spinlock, flags);
 			dev_err(&cl->device->dev,
 				"Rx buffer is not allocated.\n");
 			list_del(&rb->list);
@@ -857,8 +855,7 @@ void recv_ishtp_cl_msg(struct ishtp_device *dev,
 		 * back FC, so communication will be stuck anyway)
 		 */
 		if (rb->buffer.size < ishtp_hdr->length + rb->buf_idx) {
-			spin_unlock_irqrestore(&dev->read_list_spinlock,
-				dev_flags);
+			spin_unlock_irqrestore(&dev->read_list_spinlock, flags);
 			dev_err(&cl->device->dev,
 				"message overflow. size %d len %d idx %ld\n",
 				rb->buffer.size, ishtp_hdr->length,
@@ -884,14 +881,13 @@ void recv_ishtp_cl_msg(struct ishtp_device *dev,
 			 * the whole msg arrived, send a new FC, and add a new
 			 * rb buffer for the next coming msg
 			 */
-			spin_lock_irqsave(&cl->free_list_spinlock, flags);
+			spin_lock(&cl->free_list_spinlock);
 
 			if (!list_empty(&cl->free_rb_list.list)) {
 				new_rb = list_entry(cl->free_rb_list.list.next,
 					struct ishtp_cl_rb, list);
 				list_del_init(&new_rb->list);
-				spin_unlock_irqrestore(&cl->free_list_spinlock,
-					flags);
+				spin_unlock(&cl->free_list_spinlock);
 				new_rb->cl = cl;
 				new_rb->buf_idx = 0;
 				INIT_LIST_HEAD(&new_rb->list);
@@ -900,8 +896,7 @@ void recv_ishtp_cl_msg(struct ishtp_device *dev,
 
 				ishtp_hbm_cl_flow_control_req(dev, cl);
 			} else {
-				spin_unlock_irqrestore(&cl->free_list_spinlock,
-					flags);
+				spin_unlock(&cl->free_list_spinlock);
 			}
 		}
 		/* One more fragment in message (even if this was last) */
@@ -914,7 +909,7 @@ void recv_ishtp_cl_msg(struct ishtp_device *dev,
 		break;
 	}
 
-	spin_unlock_irqrestore(&dev->read_list_spinlock, dev_flags);
+	spin_unlock_irqrestore(&dev->read_list_spinlock, flags);
 	/* If it's nobody's message, just read and discard it */
 	if (!buffer) {
 		uint8_t	rd_msg_buf[ISHTP_RD_MSG_BUF_SIZE];
@@ -925,7 +920,8 @@ void recv_ishtp_cl_msg(struct ishtp_device *dev,
 	}
 
 	if (complete_rb) {
-		getnstimeofday(&cl->ts_rx);
+		cl = complete_rb->cl;
+		cl->ts_rx = ktime_get();
 		++cl->recv_msg_cnt_ipc;
 		ishtp_cl_read_complete(complete_rb);
 	}
@@ -940,7 +936,7 @@ eoi:
  * @hbm: hbm buffer
  *
  * Receive and dispatch ISHTP client messages using DMA. This function executes
- * in ISR context
+ * in ISR or work queue context
  */
 void recv_ishtp_cl_msg_dma(struct ishtp_device *dev, void *msg,
 			   struct dma_xfer_hbm *hbm)
@@ -950,10 +946,10 @@ void recv_ishtp_cl_msg_dma(struct ishtp_device *dev, void *msg,
 	struct ishtp_cl_rb *new_rb;
 	unsigned char *buffer = NULL;
 	struct ishtp_cl_rb *complete_rb = NULL;
-	unsigned long	dev_flags;
 	unsigned long	flags;
 
-	spin_lock_irqsave(&dev->read_list_spinlock, dev_flags);
+	spin_lock_irqsave(&dev->read_list_spinlock, flags);
+
 	list_for_each_entry(rb, &dev->read_list.list, list) {
 		cl = rb->cl;
 		if (!cl || !(cl->host_client_id == hbm->host_client_id &&
@@ -965,8 +961,7 @@ void recv_ishtp_cl_msg_dma(struct ishtp_device *dev, void *msg,
 		 * If no Rx buffer is allocated, disband the rb
 		 */
 		if (rb->buffer.size == 0 || rb->buffer.data == NULL) {
-			spin_unlock_irqrestore(&dev->read_list_spinlock,
-				dev_flags);
+			spin_unlock_irqrestore(&dev->read_list_spinlock, flags);
 			dev_err(&cl->device->dev,
 				"response buffer is not allocated.\n");
 			list_del(&rb->list);
@@ -982,8 +977,7 @@ void recv_ishtp_cl_msg_dma(struct ishtp_device *dev, void *msg,
 		 * back FC, so communication will be stuck anyway)
 		 */
 		if (rb->buffer.size < hbm->msg_length) {
-			spin_unlock_irqrestore(&dev->read_list_spinlock,
-				dev_flags);
+			spin_unlock_irqrestore(&dev->read_list_spinlock, flags);
 			dev_err(&cl->device->dev,
 				"message overflow. size %d len %d idx %ld\n",
 				rb->buffer.size, hbm->msg_length, rb->buf_idx);
@@ -1007,14 +1001,13 @@ void recv_ishtp_cl_msg_dma(struct ishtp_device *dev, void *msg,
 		 * the whole msg arrived, send a new FC, and add a new
 		 * rb buffer for the next coming msg
 		 */
-		spin_lock_irqsave(&cl->free_list_spinlock, flags);
+		spin_lock(&cl->free_list_spinlock);
 
 		if (!list_empty(&cl->free_rb_list.list)) {
 			new_rb = list_entry(cl->free_rb_list.list.next,
 				struct ishtp_cl_rb, list);
 			list_del_init(&new_rb->list);
-			spin_unlock_irqrestore(&cl->free_list_spinlock,
-				flags);
+			spin_unlock(&cl->free_list_spinlock);
 			new_rb->cl = cl;
 			new_rb->buf_idx = 0;
 			INIT_LIST_HEAD(&new_rb->list);
@@ -1023,8 +1016,7 @@ void recv_ishtp_cl_msg_dma(struct ishtp_device *dev, void *msg,
 
 			ishtp_hbm_cl_flow_control_req(dev, cl);
 		} else {
-			spin_unlock_irqrestore(&cl->free_list_spinlock,
-				flags);
+			spin_unlock(&cl->free_list_spinlock);
 		}
 
 		/* One more fragment in message (this is always last) */
@@ -1037,7 +1029,7 @@ void recv_ishtp_cl_msg_dma(struct ishtp_device *dev, void *msg,
 		break;
 	}
 
-	spin_unlock_irqrestore(&dev->read_list_spinlock, dev_flags);
+	spin_unlock_irqrestore(&dev->read_list_spinlock, flags);
 	/* If it's nobody's message, just read and discard it */
 	if (!buffer) {
 		dev_err(dev->devc, "Dropped Rx (DMA) msg - no request\n");
@@ -1045,7 +1037,8 @@ void recv_ishtp_cl_msg_dma(struct ishtp_device *dev, void *msg,
 	}
 
 	if (complete_rb) {
-		getnstimeofday(&cl->ts_rx);
+		cl = complete_rb->cl;
+		cl->ts_rx = ktime_get();
 		++cl->recv_msg_cnt_dma;
 		ishtp_cl_read_complete(complete_rb);
 	}
diff --git a/drivers/hid/intel-ish-hid/ishtp/client.h b/drivers/hid/intel-ish-hid/ishtp/client.h
index 444d069c2ed4..79eade547f5d 100644
--- a/drivers/hid/intel-ish-hid/ishtp/client.h
+++ b/drivers/hid/intel-ish-hid/ishtp/client.h
@@ -118,9 +118,9 @@ struct ishtp_cl {
 	unsigned int	out_flow_ctrl_cnt;
 
 	/* Rx msg ... out FC timing */
-	struct timespec ts_rx;
-	struct timespec ts_out_fc;
-	struct timespec ts_max_fc_delay;
+	ktime_t ts_rx;
+	ktime_t ts_out_fc;
+	ktime_t ts_max_fc_delay;
 	void *client_data;
 };
 
diff --git a/drivers/hid/intel-ish-hid/ishtp/hbm.c b/drivers/hid/intel-ish-hid/ishtp/hbm.c
index b7213608ce43..ae4a69f7f2f4 100644
--- a/drivers/hid/intel-ish-hid/ishtp/hbm.c
+++ b/drivers/hid/intel-ish-hid/ishtp/hbm.c
@@ -321,13 +321,10 @@ int ishtp_hbm_cl_flow_control_req(struct ishtp_device *dev,
 	if (!rv) {
 		++cl->out_flow_ctrl_creds;
 		++cl->out_flow_ctrl_cnt;
-		getnstimeofday(&cl->ts_out_fc);
-		if (cl->ts_rx.tv_sec && cl->ts_rx.tv_nsec) {
-			struct timespec ts_diff;
-
-			ts_diff = timespec_sub(cl->ts_out_fc, cl->ts_rx);
-			if (timespec_compare(&ts_diff, &cl->ts_max_fc_delay)
-					> 0)
+		cl->ts_out_fc = ktime_get();
+		if (cl->ts_rx) {
+			ktime_t ts_diff = ktime_sub(cl->ts_out_fc, cl->ts_rx);
+			if (ktime_after(ts_diff, cl->ts_max_fc_delay))
 				cl->ts_max_fc_delay = ts_diff;
 		}
 	} else {
diff --git a/drivers/hid/wacom.h b/drivers/hid/wacom.h
index c7b9ab1907d8..3c37c3cbf6f1 100644
--- a/drivers/hid/wacom.h
+++ b/drivers/hid/wacom.h
@@ -138,6 +138,7 @@ struct wacom_battery {
 	struct power_supply_desc bat_desc;
 	struct power_supply *battery;
 	char bat_name[WACOM_NAME_MAX];
+	int bat_status;
 	int battery_capacity;
 	int bat_charging;
 	int bat_connected;
diff --git a/drivers/hid/wacom_sys.c b/drivers/hid/wacom_sys.c
index 0022c0dac88a..838c1ebfffa9 100644
--- a/drivers/hid/wacom_sys.c
+++ b/drivers/hid/wacom_sys.c
@@ -1547,7 +1547,9 @@ static int wacom_battery_get_property(struct power_supply *psy,
 			val->intval = battery->battery_capacity;
 			break;
 		case POWER_SUPPLY_PROP_STATUS:
-			if (battery->bat_charging)
+			if (battery->bat_status != WACOM_POWER_SUPPLY_STATUS_AUTO)
+				val->intval = battery->bat_status;
+			else if (battery->bat_charging)
 				val->intval = POWER_SUPPLY_STATUS_CHARGING;
 			else if (battery->battery_capacity == 100 &&
 				    battery->ps_connected)
diff --git a/drivers/hid/wacom_wac.c b/drivers/hid/wacom_wac.c
index 4b225fb19a16..1992bb0732c2 100644
--- a/drivers/hid/wacom_wac.c
+++ b/drivers/hid/wacom_wac.c
@@ -57,15 +57,18 @@ static unsigned short batcap_gr[8] = { 1, 15, 25, 35, 50, 70, 100, 100 };
 static unsigned short batcap_i4[8] = { 1, 15, 30, 45, 60, 70, 85, 100 };
 
 static void __wacom_notify_battery(struct wacom_battery *battery,
-				   int bat_capacity, bool bat_charging,
-				   bool bat_connected, bool ps_connected)
+				   int bat_status, int bat_capacity,
+				   bool bat_charging, bool bat_connected,
+				   bool ps_connected)
 {
-	bool changed = battery->battery_capacity != bat_capacity  ||
+	bool changed = battery->bat_status       != bat_status    ||
+		       battery->battery_capacity != bat_capacity  ||
 		       battery->bat_charging     != bat_charging  ||
 		       battery->bat_connected    != bat_connected ||
 		       battery->ps_connected     != ps_connected;
 
 	if (changed) {
+		battery->bat_status = bat_status;
 		battery->battery_capacity = bat_capacity;
 		battery->bat_charging = bat_charging;
 		battery->bat_connected = bat_connected;
@@ -77,13 +80,13 @@ static void __wacom_notify_battery(struct wacom_battery *battery,
 }
 
 static void wacom_notify_battery(struct wacom_wac *wacom_wac,
-	int bat_capacity, bool bat_charging, bool bat_connected,
-	bool ps_connected)
+	int bat_status, int bat_capacity, bool bat_charging,
+	bool bat_connected, bool ps_connected)
 {
 	struct wacom *wacom = container_of(wacom_wac, struct wacom, wacom_wac);
 
-	__wacom_notify_battery(&wacom->battery, bat_capacity, bat_charging,
-			       bat_connected, ps_connected);
+	__wacom_notify_battery(&wacom->battery, bat_status, bat_capacity,
+			       bat_charging, bat_connected, ps_connected);
 }
 
 static int wacom_penpartner_irq(struct wacom_wac *wacom)
@@ -448,8 +451,9 @@ static int wacom_graphire_irq(struct wacom_wac *wacom)
 		rw = (data[7] >> 2 & 0x07);
 		battery_capacity = batcap_gr[rw];
 		ps_connected = rw == 7;
-		wacom_notify_battery(wacom, battery_capacity, ps_connected,
-				     1, ps_connected);
+		wacom_notify_battery(wacom, WACOM_POWER_SUPPLY_STATUS_AUTO,
+				     battery_capacity, ps_connected, 1,
+				     ps_connected);
 	}
 exit:
 	return retval;
@@ -1071,7 +1075,8 @@ static int wacom_remote_irq(struct wacom_wac *wacom_wac, size_t len)
 			wacom->led.groups[i].select = touch_ring_mode;
 	}
 
-	__wacom_notify_battery(&remote->remotes[index].battery, bat_percent,
+	__wacom_notify_battery(&remote->remotes[index].battery,
+				WACOM_POWER_SUPPLY_STATUS_AUTO, bat_percent,
 				bat_charging, 1, bat_charging);
 
 out:
@@ -1157,7 +1162,8 @@ static int wacom_intuos_bt_irq(struct wacom_wac *wacom, size_t len)
 		bat_charging = (power_raw & 0x08) ? 1 : 0;
 		ps_connected = (power_raw & 0x10) ? 1 : 0;
 		battery_capacity = batcap_i4[power_raw & 0x07];
-		wacom_notify_battery(wacom, battery_capacity, bat_charging,
+		wacom_notify_battery(wacom, WACOM_POWER_SUPPLY_STATUS_AUTO,
+				     battery_capacity, bat_charging,
 				     battery_capacity || bat_charging,
 				     ps_connected);
 		break;
@@ -1334,7 +1340,8 @@ static void wacom_intuos_pro2_bt_battery(struct wacom_wac *wacom)
 	bool chg = data[284] & 0x80;
 	int battery_status = data[284] & 0x7F;
 
-	wacom_notify_battery(wacom, battery_status, chg, 1, chg);
+	wacom_notify_battery(wacom, WACOM_POWER_SUPPLY_STATUS_AUTO,
+			     battery_status, chg, 1, chg);
 }
 
 static int wacom_intuos_pro2_bt_irq(struct wacom_wac *wacom, size_t len)
@@ -1571,37 +1578,38 @@ static int wacom_tpc_irq(struct wacom_wac *wacom, size_t len)
 {
 	unsigned char *data = wacom->data;
 
-	if (wacom->pen_input)
+	if (wacom->pen_input) {
 		dev_dbg(wacom->pen_input->dev.parent,
 			"%s: received report #%d\n", __func__, data[0]);
-	else if (wacom->touch_input)
+
+		if (len == WACOM_PKGLEN_PENABLED ||
+		    data[0] == WACOM_REPORT_PENABLED)
+			return wacom_tpc_pen(wacom);
+	}
+	else if (wacom->touch_input) {
 		dev_dbg(wacom->touch_input->dev.parent,
 			"%s: received report #%d\n", __func__, data[0]);
 
-	switch (len) {
-	case WACOM_PKGLEN_TPC1FG:
-		return wacom_tpc_single_touch(wacom, len);
-
-	case WACOM_PKGLEN_TPC2FG:
-		return wacom_tpc_mt_touch(wacom);
+		switch (len) {
+		case WACOM_PKGLEN_TPC1FG:
+			return wacom_tpc_single_touch(wacom, len);
 
-	case WACOM_PKGLEN_PENABLED:
-		return wacom_tpc_pen(wacom);
+		case WACOM_PKGLEN_TPC2FG:
+			return wacom_tpc_mt_touch(wacom);
 
-	default:
-		switch (data[0]) {
-		case WACOM_REPORT_TPC1FG:
-		case WACOM_REPORT_TPCHID:
-		case WACOM_REPORT_TPCST:
-		case WACOM_REPORT_TPC1FGE:
-			return wacom_tpc_single_touch(wacom, len);
+		default:
+			switch (data[0]) {
+			case WACOM_REPORT_TPC1FG:
+			case WACOM_REPORT_TPCHID:
+			case WACOM_REPORT_TPCST:
+			case WACOM_REPORT_TPC1FGE:
+				return wacom_tpc_single_touch(wacom, len);
 
-		case WACOM_REPORT_TPCMT:
-		case WACOM_REPORT_TPCMT2:
-			return wacom_mt_touch(wacom);
+			case WACOM_REPORT_TPCMT:
+			case WACOM_REPORT_TPCMT2:
+				return wacom_mt_touch(wacom);
 
-		case WACOM_REPORT_PENABLED:
-			return wacom_tpc_pen(wacom);
+			}
 		}
 	}
 
@@ -1695,20 +1703,92 @@ static void wacom_map_usage(struct input_dev *input, struct hid_usage *usage,
 	}
 }
 
-static void wacom_wac_pad_usage_mapping(struct hid_device *hdev,
+static void wacom_wac_battery_usage_mapping(struct hid_device *hdev,
 		struct hid_field *field, struct hid_usage *usage)
 {
 	struct wacom *wacom = hid_get_drvdata(hdev);
 	struct wacom_wac *wacom_wac = &wacom->wacom_wac;
 	struct wacom_features *features = &wacom_wac->features;
-	struct input_dev *input = wacom_wac->pad_input;
 	unsigned equivalent_usage = wacom_equivalent_usage(usage->hid);
 
 	switch (equivalent_usage) {
+	case HID_DG_BATTERYSTRENGTH:
 	case WACOM_HID_WD_BATTERY_LEVEL:
 	case WACOM_HID_WD_BATTERY_CHARGING:
 		features->quirks |= WACOM_QUIRK_BATTERY;
 		break;
+	}
+}
+
+static void wacom_wac_battery_event(struct hid_device *hdev, struct hid_field *field,
+		struct hid_usage *usage, __s32 value)
+{
+	struct wacom *wacom = hid_get_drvdata(hdev);
+	struct wacom_wac *wacom_wac = &wacom->wacom_wac;
+	unsigned equivalent_usage = wacom_equivalent_usage(usage->hid);
+
+	switch (equivalent_usage) {
+	case HID_DG_BATTERYSTRENGTH:
+		if (value == 0) {
+			wacom_wac->hid_data.bat_status = POWER_SUPPLY_STATUS_UNKNOWN;
+		}
+		else {
+			value = value * 100 / (field->logical_maximum - field->logical_minimum);
+			wacom_wac->hid_data.battery_capacity = value;
+			wacom_wac->hid_data.bat_connected = 1;
+			wacom_wac->hid_data.bat_status = WACOM_POWER_SUPPLY_STATUS_AUTO;
+		}
+		break;
+	case WACOM_HID_WD_BATTERY_LEVEL:
+		value = value * 100 / (field->logical_maximum - field->logical_minimum);
+		wacom_wac->hid_data.battery_capacity = value;
+		wacom_wac->hid_data.bat_connected = 1;
+		wacom_wac->hid_data.bat_status = WACOM_POWER_SUPPLY_STATUS_AUTO;
+		break;
+	case WACOM_HID_WD_BATTERY_CHARGING:
+		wacom_wac->hid_data.bat_charging = value;
+		wacom_wac->hid_data.ps_connected = value;
+		wacom_wac->hid_data.bat_connected = 1;
+		wacom_wac->hid_data.bat_status = WACOM_POWER_SUPPLY_STATUS_AUTO;
+		break;
+	}
+}
+
+static void wacom_wac_battery_pre_report(struct hid_device *hdev,
+		struct hid_report *report)
+{
+	return;
+}
+
+static void wacom_wac_battery_report(struct hid_device *hdev,
+		struct hid_report *report)
+{
+	struct wacom *wacom = hid_get_drvdata(hdev);
+	struct wacom_wac *wacom_wac = &wacom->wacom_wac;
+	struct wacom_features *features = &wacom_wac->features;
+
+	if (features->quirks & WACOM_QUIRK_BATTERY) {
+		int status = wacom_wac->hid_data.bat_status;
+		int capacity = wacom_wac->hid_data.battery_capacity;
+		bool charging = wacom_wac->hid_data.bat_charging;
+		bool connected = wacom_wac->hid_data.bat_connected;
+		bool powered = wacom_wac->hid_data.ps_connected;
+
+		wacom_notify_battery(wacom_wac, status, capacity, charging,
+				     connected, powered);
+	}
+}
+
+static void wacom_wac_pad_usage_mapping(struct hid_device *hdev,
+		struct hid_field *field, struct hid_usage *usage)
+{
+	struct wacom *wacom = hid_get_drvdata(hdev);
+	struct wacom_wac *wacom_wac = &wacom->wacom_wac;
+	struct wacom_features *features = &wacom_wac->features;
+	struct input_dev *input = wacom_wac->pad_input;
+	unsigned equivalent_usage = wacom_equivalent_usage(usage->hid);
+
+	switch (equivalent_usage) {
 	case WACOM_HID_WD_ACCELEROMETER_X:
 		__set_bit(INPUT_PROP_ACCELEROMETER, input->propbit);
 		wacom_map_usage(input, usage, field, EV_ABS, ABS_X, 0);
@@ -1802,27 +1882,6 @@ static void wacom_wac_pad_usage_mapping(struct hid_device *hdev,
 	}
 }
 
-static void wacom_wac_pad_battery_event(struct hid_device *hdev, struct hid_field *field,
-		struct hid_usage *usage, __s32 value)
-{
-	struct wacom *wacom = hid_get_drvdata(hdev);
-	struct wacom_wac *wacom_wac = &wacom->wacom_wac;
-	unsigned equivalent_usage = wacom_equivalent_usage(usage->hid);
-
-	switch (equivalent_usage) {
-	case WACOM_HID_WD_BATTERY_LEVEL:
-		wacom_wac->hid_data.battery_capacity = value;
-		wacom_wac->hid_data.bat_connected = 1;
-		break;
-
-	case WACOM_HID_WD_BATTERY_CHARGING:
-		wacom_wac->hid_data.bat_charging = value;
-		wacom_wac->hid_data.ps_connected = value;
-		wacom_wac->hid_data.bat_connected = 1;
-		break;
-	}
-}
-
 static void wacom_wac_pad_event(struct hid_device *hdev, struct hid_field *field,
 		struct hid_usage *usage, __s32 value)
 {
@@ -1896,24 +1955,6 @@ static void wacom_wac_pad_pre_report(struct hid_device *hdev,
 	wacom_wac->hid_data.inrange_state = 0;
 }
 
-static void wacom_wac_pad_battery_report(struct hid_device *hdev,
-		struct hid_report *report)
-{
-	struct wacom *wacom = hid_get_drvdata(hdev);
-	struct wacom_wac *wacom_wac = &wacom->wacom_wac;
-	struct wacom_features *features = &wacom_wac->features;
-
-	if (features->quirks & WACOM_QUIRK_BATTERY) {
-		int capacity = wacom_wac->hid_data.battery_capacity;
-		bool charging = wacom_wac->hid_data.bat_charging;
-		bool connected = wacom_wac->hid_data.bat_connected;
-		bool powered = wacom_wac->hid_data.ps_connected;
-
-		wacom_notify_battery(wacom_wac, capacity, charging,
-				     connected, powered);
-	}
-}
-
 static void wacom_wac_pad_report(struct hid_device *hdev,
 		struct hid_report *report)
 {
@@ -1959,9 +2000,6 @@ static void wacom_wac_pen_usage_mapping(struct hid_device *hdev,
 	case HID_DG_INRANGE:
 		wacom_map_usage(input, usage, field, EV_KEY, BTN_TOOL_PEN, 0);
 		break;
-	case HID_DG_BATTERYSTRENGTH:
-		features->quirks |= WACOM_QUIRK_BATTERY;
-		break;
 	case HID_DG_INVERT:
 		wacom_map_usage(input, usage, field, EV_KEY,
 				BTN_TOOL_RUBBER, 0);
@@ -2034,10 +2072,6 @@ static void wacom_wac_pen_event(struct hid_device *hdev, struct hid_field *field
 		if (!(features->quirks & WACOM_QUIRK_SENSE))
 			wacom_wac->hid_data.sense_state = value;
 		return;
-	case HID_DG_BATTERYSTRENGTH:
-		wacom_wac->hid_data.battery_capacity = value;
-		wacom_wac->hid_data.bat_connected = 1;
-		break;
 	case HID_DG_INVERT:
 		wacom_wac->hid_data.invert_state = value;
 		return;
@@ -2394,7 +2428,10 @@ void wacom_wac_usage_mapping(struct hid_device *hdev,
 	if (WACOM_DIRECT_DEVICE(field))
 		features->device_type |= WACOM_DEVICETYPE_DIRECT;
 
-	if (WACOM_PAD_FIELD(field))
+	/* usage tests must precede field tests */
+	if (WACOM_BATTERY_USAGE(usage))
+		wacom_wac_battery_usage_mapping(hdev, field, usage);
+	else if (WACOM_PAD_FIELD(field))
 		wacom_wac_pad_usage_mapping(hdev, field, usage);
 	else if (WACOM_PEN_FIELD(field))
 		wacom_wac_pen_usage_mapping(hdev, field, usage);
@@ -2413,11 +2450,12 @@ void wacom_wac_event(struct hid_device *hdev, struct hid_field *field,
 	if (value > field->logical_maximum || value < field->logical_minimum)
 		return;
 
-	if (WACOM_PAD_FIELD(field)) {
-		wacom_wac_pad_battery_event(hdev, field, usage, value);
-		if (wacom->wacom_wac.pad_input)
-			wacom_wac_pad_event(hdev, field, usage, value);
-	} else if (WACOM_PEN_FIELD(field) && wacom->wacom_wac.pen_input)
+	/* usage tests must precede field tests */
+	if (WACOM_BATTERY_USAGE(usage))
+		wacom_wac_battery_event(hdev, field, usage, value);
+	else if (WACOM_PAD_FIELD(field) && wacom->wacom_wac.pad_input)
+		wacom_wac_pad_event(hdev, field, usage, value);
+	else if (WACOM_PEN_FIELD(field) && wacom->wacom_wac.pen_input)
 		wacom_wac_pen_event(hdev, field, usage, value);
 	else if (WACOM_FINGER_FIELD(field) && wacom->wacom_wac.touch_input)
 		wacom_wac_finger_event(hdev, field, usage, value);
@@ -2451,6 +2489,8 @@ void wacom_wac_report(struct hid_device *hdev, struct hid_report *report)
 	if (wacom_wac->features.type != HID_GENERIC)
 		return;
 
+	wacom_wac_battery_pre_report(hdev, report);
+
 	if (WACOM_PAD_FIELD(field) && wacom->wacom_wac.pad_input)
 		wacom_wac_pad_pre_report(hdev, report);
 	else if (WACOM_PEN_FIELD(field) && wacom->wacom_wac.pen_input)
@@ -2470,11 +2510,11 @@ void wacom_wac_report(struct hid_device *hdev, struct hid_report *report)
 	if (report->type != HID_INPUT_REPORT)
 		return;
 
-	if (WACOM_PAD_FIELD(field)) {
-		wacom_wac_pad_battery_report(hdev, report);
-		if (wacom->wacom_wac.pad_input)
-			wacom_wac_pad_report(hdev, report);
-	} else if (WACOM_PEN_FIELD(field) && wacom->wacom_wac.pen_input)
+	wacom_wac_battery_report(hdev, report);
+
+	if (WACOM_PAD_FIELD(field) && wacom->wacom_wac.pad_input)
+		wacom_wac_pad_report(hdev, report);
+	else if (WACOM_PEN_FIELD(field) && wacom->wacom_wac.pen_input)
 		wacom_wac_pen_report(hdev, report);
 	else if (WACOM_FINGER_FIELD(field) && wacom->wacom_wac.touch_input)
 		wacom_wac_finger_report(hdev, report);
@@ -2812,13 +2852,14 @@ static int wacom_wireless_irq(struct wacom_wac *wacom, size_t len)
 			wacom_schedule_work(wacom, WACOM_WORKER_WIRELESS);
 		}
 
-		wacom_notify_battery(wacom, battery, charging, 1, 0);
+		wacom_notify_battery(wacom, WACOM_POWER_SUPPLY_STATUS_AUTO,
+				     battery, charging, 1, 0);
 
 	} else if (wacom->pid != 0) {
 		/* disconnected while previously connected */
 		wacom->pid = 0;
 		wacom_schedule_work(wacom, WACOM_WORKER_WIRELESS);
-		wacom_notify_battery(wacom, 0, 0, 0, 0);
+		wacom_notify_battery(wacom, POWER_SUPPLY_STATUS_UNKNOWN, 0, 0, 0, 0);
 	}
 
 	return 0;
@@ -2846,8 +2887,8 @@ static int wacom_status_irq(struct wacom_wac *wacom_wac, size_t len)
 		int battery = (data[8] & 0x3f) * 100 / 31;
 		bool charging = !!(data[8] & 0x80);
 
-		wacom_notify_battery(wacom_wac, battery, charging,
-				     battery || charging, 1);
+		wacom_notify_battery(wacom_wac, WACOM_POWER_SUPPLY_STATUS_AUTO,
+				     battery, charging, battery || charging, 1);
 
 		if (!wacom->battery.battery &&
 		    !(features->quirks & WACOM_QUIRK_BATTERY)) {
@@ -2859,7 +2900,7 @@ static int wacom_status_irq(struct wacom_wac *wacom_wac, size_t len)
 		 wacom->battery.battery) {
 		features->quirks &= ~WACOM_QUIRK_BATTERY;
 		wacom_schedule_work(wacom_wac, WACOM_WORKER_BATTERY);
-		wacom_notify_battery(wacom_wac, 0, 0, 0, 0);
+		wacom_notify_battery(wacom_wac, POWER_SUPPLY_STATUS_UNKNOWN, 0, 0, 0, 0);
 	}
 	return 0;
 }
diff --git a/drivers/hid/wacom_wac.h b/drivers/hid/wacom_wac.h
index 570d29582b82..8a03654048bf 100644
--- a/drivers/hid/wacom_wac.h
+++ b/drivers/hid/wacom_wac.h
@@ -96,6 +96,8 @@
 #define WACOM_DEVICETYPE_WL_MONITOR     0x0008
 #define WACOM_DEVICETYPE_DIRECT         0x0010
 
+#define WACOM_POWER_SUPPLY_STATUS_AUTO  -1
+
 #define WACOM_HID_UP_WACOMDIGITIZER     0xff0d0000
 #define WACOM_HID_SP_PAD                0x00040000
 #define WACOM_HID_SP_BUTTON             0x00090000
@@ -151,6 +153,10 @@
 #define WACOM_HID_WT_X                  (WACOM_HID_UP_WACOMTOUCH | 0x130)
 #define WACOM_HID_WT_Y                  (WACOM_HID_UP_WACOMTOUCH | 0x131)
 
+#define WACOM_BATTERY_USAGE(f)	(((f)->hid == HID_DG_BATTERYSTRENGTH) || \
+				 ((f)->hid == WACOM_HID_WD_BATTERY_CHARGING) || \
+				 ((f)->hid == WACOM_HID_WD_BATTERY_LEVEL))
+
 #define WACOM_PAD_FIELD(f)	(((f)->physical == HID_DG_TABLETFUNCTIONKEY) || \
 				 ((f)->physical == WACOM_HID_WD_DIGITIZERFNKEYS) || \
 				 ((f)->physical == WACOM_HID_WD_DIGITIZERINFO))
@@ -297,6 +303,7 @@ struct hid_data {
 	int last_slot_field;
 	int num_expected;
 	int num_received;
+	int bat_status;
 	int battery_capacity;
 	int bat_charging;
 	int bat_connected;
diff --git a/drivers/hwmon/Kconfig b/drivers/hwmon/Kconfig
index 22d5eafd6815..5ef2814345ef 100644
--- a/drivers/hwmon/Kconfig
+++ b/drivers/hwmon/Kconfig
@@ -343,6 +343,7 @@ config SENSORS_ASB100
 
 config SENSORS_ASPEED
 	tristate "ASPEED AST2400/AST2500 PWM and Fan tach driver"
+	select REGMAP
 	help
 	  This driver provides support for ASPEED AST2400/AST2500 PWM
 	  and Fan Tacho controllers.
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 48403a2115be..12b716b70ead 100644
--- a/drivers/hwmon/aspeed-pwm-tacho.c
+++ b/drivers/hwmon/aspeed-pwm-tacho.c
@@ -7,6 +7,7 @@
  */
 
 #include <linux/clk.h>
+#include <linux/errno.h>
 #include <linux/gpio/consumer.h>
 #include <linux/delay.h>
 #include <linux/hwmon.h>
@@ -494,7 +495,7 @@ static u32 aspeed_get_fan_tach_ch_measure_period(struct aspeed_pwm_tacho_data
 	return clk / (clk_unit * div_h * div_l * tacho_div * tacho_unit);
 }
 
-static u32 aspeed_get_fan_tach_ch_rpm(struct aspeed_pwm_tacho_data *priv,
+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;
@@ -510,6 +511,9 @@ static u32 aspeed_get_fan_tach_ch_rpm(struct aspeed_pwm_tacho_data *priv,
 	msleep(sec);
 
 	regmap_read(priv->regmap, ASPEED_PTCR_RESULT, &val);
+	if (!(val & RESULT_STATUS_MASK))
+		return -ETIMEDOUT;
+
 	raw_data = val & RESULT_VALUE_MASK;
 	tach_div = priv->type_fan_tach_clock_division[type];
 	tach_div = 0x4 << (tach_div * 2);
@@ -561,12 +565,14 @@ static ssize_t show_rpm(struct device *dev, struct device_attribute *attr,
 {
 	struct sensor_device_attribute *sensor_attr = to_sensor_dev_attr(attr);
 	int index = sensor_attr->index;
-	u32 rpm;
+	int rpm;
 	struct aspeed_pwm_tacho_data *priv = dev_get_drvdata(dev);
 
 	rpm = aspeed_get_fan_tach_ch_rpm(priv, index);
+	if (rpm < 0)
+		return rpm;
 
-	return sprintf(buf, "%u\n", rpm);
+	return sprintf(buf, "%d\n", rpm);
 }
 
 static umode_t pwm_is_visible(struct kobject *kobj,
diff --git a/drivers/hwmon/coretemp.c b/drivers/hwmon/coretemp.c
index 3ac4c03ba77b..c13a4fd86b3c 100644
--- a/drivers/hwmon/coretemp.c
+++ b/drivers/hwmon/coretemp.c
@@ -605,6 +605,13 @@ static int coretemp_cpu_online(unsigned int cpu)
 	struct platform_data *pdata;
 
 	/*
+	 * Don't execute this on resume as the offline callback did
+	 * not get executed on suspend.
+	 */
+	if (cpuhp_tasks_frozen)
+		return 0;
+
+	/*
 	 * CPUID.06H.EAX[0] indicates whether the CPU has thermal
 	 * sensors. We check this bit only, all the early CPUs
 	 * without thermal sensors will be filtered out.
@@ -654,6 +661,13 @@ static int coretemp_cpu_offline(unsigned int cpu)
 	struct temp_data *tdata;
 	int indx, target;
 
+	/*
+	 * Don't execute this on suspend as the device remove locks
+	 * up the machine.
+	 */
+	if (cpuhp_tasks_frozen)
+		return 0;
+
 	/* If the physical CPU device does not exist, just return */
 	if (!pdev)
 		return 0;
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..b562323e2c4e 100644
--- a/drivers/hwmon/ibmpowernv.c
+++ b/drivers/hwmon/ibmpowernv.c
@@ -298,10 +298,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 +341,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 +456,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/i2c/Makefile b/drivers/i2c/Makefile
index 45095b3d16a9..7bb65a4369e1 100644
--- a/drivers/i2c/Makefile
+++ b/drivers/i2c/Makefile
@@ -4,6 +4,11 @@
 
 obj-$(CONFIG_I2C_BOARDINFO)	+= i2c-boardinfo.o
 obj-$(CONFIG_I2C)		+= i2c-core.o
+i2c-core-objs 			:= i2c-core-base.o i2c-core-smbus.o
+i2c-core-$(CONFIG_ACPI)		+= i2c-core-acpi.o
+i2c-core-$(CONFIG_I2C_SLAVE) 	+= i2c-core-slave.o
+i2c-core-$(CONFIG_OF) 		+= i2c-core-of.o
+
 obj-$(CONFIG_I2C_SMBUS)		+= i2c-smbus.o
 obj-$(CONFIG_I2C_CHARDEV)	+= i2c-dev.o
 obj-$(CONFIG_I2C_MUX)		+= i2c-mux.o
@@ -12,4 +17,4 @@ obj-$(CONFIG_I2C_STUB)		+= i2c-stub.o
 obj-$(CONFIG_I2C_SLAVE_EEPROM)	+= i2c-slave-eeprom.o
 
 ccflags-$(CONFIG_I2C_DEBUG_CORE) := -DDEBUG
-CFLAGS_i2c-core.o := -Wno-deprecated-declarations
+CFLAGS_i2c-core-base.o := -Wno-deprecated-declarations
diff --git a/drivers/i2c/busses/i2c-designware-core.c b/drivers/i2c/busses/i2c-designware-core.c
index c453717b753b..3c41995634c2 100644
--- a/drivers/i2c/busses/i2c-designware-core.c
+++ b/drivers/i2c/busses/i2c-designware-core.c
@@ -583,7 +583,7 @@ i2c_dw_xfer_msg(struct dw_i2c_dev *dev)
 			 */
 
 			/*
-			 * i2c-core.c always sets the buffer length of
+			 * i2c-core always sets the buffer length of
 			 * I2C_FUNC_SMBUS_BLOCK_DATA to 1. The length will
 			 * be adjusted when receiving the first byte.
 			 * Thus we can't stop the transaction here.
diff --git a/drivers/i2c/busses/i2c-designware-platdrv.c b/drivers/i2c/busses/i2c-designware-platdrv.c
index f2acd4b6bf01..d1263b82d646 100644
--- a/drivers/i2c/busses/i2c-designware-platdrv.c
+++ b/drivers/i2c/busses/i2c-designware-platdrv.c
@@ -94,6 +94,7 @@ static void dw_i2c_acpi_params(struct platform_device *pdev, char method[],
 static int dw_i2c_acpi_configure(struct platform_device *pdev)
 {
 	struct dw_i2c_dev *dev = platform_get_drvdata(pdev);
+	u32 ss_ht = 0, fp_ht = 0, hs_ht = 0, fs_ht = 0;
 	acpi_handle handle = ACPI_HANDLE(&pdev->dev);
 	const struct acpi_device_id *id;
 	struct acpi_device *adev;
@@ -107,23 +108,24 @@ static int dw_i2c_acpi_configure(struct platform_device *pdev)
 	 * Try to get SDA hold time and *CNT values from an ACPI method for
 	 * selected speed modes.
 	 */
+	dw_i2c_acpi_params(pdev, "SSCN", &dev->ss_hcnt, &dev->ss_lcnt, &ss_ht);
+	dw_i2c_acpi_params(pdev, "FPCN", &dev->fp_hcnt, &dev->fp_lcnt, &fp_ht);
+	dw_i2c_acpi_params(pdev, "HSCN", &dev->hs_hcnt, &dev->hs_lcnt, &hs_ht);
+	dw_i2c_acpi_params(pdev, "FMCN", &dev->fs_hcnt, &dev->fs_lcnt, &fs_ht);
+
 	switch (dev->clk_freq) {
 	case 100000:
-		dw_i2c_acpi_params(pdev, "SSCN", &dev->ss_hcnt, &dev->ss_lcnt,
-				   &dev->sda_hold_time);
+		dev->sda_hold_time = ss_ht;
 		break;
 	case 1000000:
-		dw_i2c_acpi_params(pdev, "FPCN", &dev->fp_hcnt, &dev->fp_lcnt,
-				   &dev->sda_hold_time);
+		dev->sda_hold_time = fp_ht;
 		break;
 	case 3400000:
-		dw_i2c_acpi_params(pdev, "HSCN", &dev->hs_hcnt, &dev->hs_lcnt,
-				   &dev->sda_hold_time);
+		dev->sda_hold_time = hs_ht;
 		break;
 	case 400000:
 	default:
-		dw_i2c_acpi_params(pdev, "FMCN", &dev->fs_hcnt, &dev->fs_lcnt,
-				   &dev->sda_hold_time);
+		dev->sda_hold_time = fs_ht;
 		break;
 	}
 
diff --git a/drivers/i2c/busses/i2c-mv64xxx.c b/drivers/i2c/busses/i2c-mv64xxx.c
index cf737ec8563b..5c4db65c5019 100644
--- a/drivers/i2c/busses/i2c-mv64xxx.c
+++ b/drivers/i2c/busses/i2c-mv64xxx.c
@@ -819,7 +819,6 @@ mv64xxx_of_config(struct mv64xxx_i2c_data *drv_data,
 		rc = -EINVAL;
 		goto out;
 	}
-	drv_data->irq = irq_of_parse_and_map(np, 0);
 
 	drv_data->rstc = devm_reset_control_get_optional(dev, NULL);
 	if (IS_ERR(drv_data->rstc)) {
@@ -902,10 +901,11 @@ mv64xxx_i2c_probe(struct platform_device *pd)
 	if (!IS_ERR(drv_data->clk))
 		clk_prepare_enable(drv_data->clk);
 
+	drv_data->irq = platform_get_irq(pd, 0);
+
 	if (pdata) {
 		drv_data->freq_m = pdata->freq_m;
 		drv_data->freq_n = pdata->freq_n;
-		drv_data->irq = platform_get_irq(pd, 0);
 		drv_data->adapter.timeout = msecs_to_jiffies(pdata->timeout);
 		drv_data->offload_enabled = false;
 		memcpy(&drv_data->reg_offsets, &mv64xxx_i2c_regs_mv64xxx, sizeof(drv_data->reg_offsets));
@@ -915,7 +915,7 @@ mv64xxx_i2c_probe(struct platform_device *pd)
 			goto exit_clk;
 	}
 	if (drv_data->irq < 0) {
-		rc = -ENXIO;
+		rc = drv_data->irq;
 		goto exit_reset;
 	}
 
diff --git a/drivers/i2c/busses/i2c-tiny-usb.c b/drivers/i2c/busses/i2c-tiny-usb.c
index 0ed77eeff31e..a2e3dd715380 100644
--- a/drivers/i2c/busses/i2c-tiny-usb.c
+++ b/drivers/i2c/busses/i2c-tiny-usb.c
@@ -178,22 +178,39 @@ static int usb_read(struct i2c_adapter *adapter, int cmd,
 		    int value, int index, void *data, int len)
 {
 	struct i2c_tiny_usb *dev = (struct i2c_tiny_usb *)adapter->algo_data;
+	void *dmadata = kmalloc(len, GFP_KERNEL);
+	int ret;
+
+	if (!dmadata)
+		return -ENOMEM;
 
 	/* do control transfer */
-	return usb_control_msg(dev->usb_dev, usb_rcvctrlpipe(dev->usb_dev, 0),
+	ret = usb_control_msg(dev->usb_dev, usb_rcvctrlpipe(dev->usb_dev, 0),
 			       cmd, USB_TYPE_VENDOR | USB_RECIP_INTERFACE |
-			       USB_DIR_IN, value, index, data, len, 2000);
+			       USB_DIR_IN, value, index, dmadata, len, 2000);
+
+	memcpy(data, dmadata, len);
+	kfree(dmadata);
+	return ret;
 }
 
 static int usb_write(struct i2c_adapter *adapter, int cmd,
 		     int value, int index, void *data, int len)
 {
 	struct i2c_tiny_usb *dev = (struct i2c_tiny_usb *)adapter->algo_data;
+	void *dmadata = kmemdup(data, len, GFP_KERNEL);
+	int ret;
+
+	if (!dmadata)
+		return -ENOMEM;
 
 	/* do control transfer */
-	return usb_control_msg(dev->usb_dev, usb_sndctrlpipe(dev->usb_dev, 0),
+	ret = usb_control_msg(dev->usb_dev, usb_sndctrlpipe(dev->usb_dev, 0),
 			       cmd, USB_TYPE_VENDOR | USB_RECIP_INTERFACE,
-			       value, index, data, len, 2000);
+			       value, index, dmadata, len, 2000);
+
+	kfree(dmadata);
+	return ret;
 }
 
 static void i2c_tiny_usb_free(struct i2c_tiny_usb *dev)
diff --git a/drivers/i2c/busses/i2c-xgene-slimpro.c b/drivers/i2c/busses/i2c-xgene-slimpro.c
index dbe7e44c9321..6ba6c83ca8f1 100644
--- a/drivers/i2c/busses/i2c-xgene-slimpro.c
+++ b/drivers/i2c/busses/i2c-xgene-slimpro.c
@@ -416,6 +416,7 @@ static int xgene_slimpro_i2c_probe(struct platform_device *pdev)
 	adapter->class = I2C_CLASS_HWMON;
 	adapter->dev.parent = &pdev->dev;
 	adapter->dev.of_node = pdev->dev.of_node;
+	ACPI_COMPANION_SET(&adapter->dev, ACPI_COMPANION(&pdev->dev));
 	i2c_set_adapdata(adapter, ctx);
 	rc = i2c_add_adapter(adapter);
 	if (rc) {
diff --git a/drivers/i2c/i2c-core-acpi.c b/drivers/i2c/i2c-core-acpi.c
new file mode 100644
index 000000000000..052005579ed6
--- /dev/null
+++ b/drivers/i2c/i2c-core-acpi.c
@@ -0,0 +1,653 @@
+/*
+ * Linux I2C core ACPI support code
+ *
+ * Copyright (C) 2014 Intel Corp, Author: Lan Tianyu <tianyu.lan@intel.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/acpi.h>
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/i2c.h>
+#include <linux/list.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+
+#include "i2c-core.h"
+
+struct i2c_acpi_handler_data {
+	struct acpi_connection_info info;
+	struct i2c_adapter *adapter;
+};
+
+struct gsb_buffer {
+	u8	status;
+	u8	len;
+	union {
+		u16	wdata;
+		u8	bdata;
+		u8	data[0];
+	};
+} __packed;
+
+struct i2c_acpi_lookup {
+	struct i2c_board_info *info;
+	acpi_handle adapter_handle;
+	acpi_handle device_handle;
+	acpi_handle search_handle;
+	int n;
+	int index;
+	u32 speed;
+	u32 min_speed;
+};
+
+static int i2c_acpi_fill_info(struct acpi_resource *ares, void *data)
+{
+	struct i2c_acpi_lookup *lookup = data;
+	struct i2c_board_info *info = lookup->info;
+	struct acpi_resource_i2c_serialbus *sb;
+	acpi_status status;
+
+	if (info->addr || ares->type != ACPI_RESOURCE_TYPE_SERIAL_BUS)
+		return 1;
+
+	sb = &ares->data.i2c_serial_bus;
+	if (sb->type != ACPI_RESOURCE_SERIAL_TYPE_I2C)
+		return 1;
+
+	if (lookup->index != -1 && lookup->n++ != lookup->index)
+		return 1;
+
+	status = acpi_get_handle(lookup->device_handle,
+				 sb->resource_source.string_ptr,
+				 &lookup->adapter_handle);
+	if (!ACPI_SUCCESS(status))
+		return 1;
+
+	info->addr = sb->slave_address;
+	lookup->speed = sb->connection_speed;
+	if (sb->access_mode == ACPI_I2C_10BIT_MODE)
+		info->flags |= I2C_CLIENT_TEN;
+
+	return 1;
+}
+
+static int i2c_acpi_do_lookup(struct acpi_device *adev,
+			      struct i2c_acpi_lookup *lookup)
+{
+	struct i2c_board_info *info = lookup->info;
+	struct list_head resource_list;
+	int ret;
+
+	if (acpi_bus_get_status(adev) || !adev->status.present ||
+	    acpi_device_enumerated(adev))
+		return -EINVAL;
+
+	memset(info, 0, sizeof(*info));
+	lookup->device_handle = acpi_device_handle(adev);
+
+	/* Look up for I2cSerialBus resource */
+	INIT_LIST_HEAD(&resource_list);
+	ret = acpi_dev_get_resources(adev, &resource_list,
+				     i2c_acpi_fill_info, lookup);
+	acpi_dev_free_resource_list(&resource_list);
+
+	if (ret < 0 || !info->addr)
+		return -EINVAL;
+
+	return 0;
+}
+
+static int i2c_acpi_get_info(struct acpi_device *adev,
+			     struct i2c_board_info *info,
+			     struct i2c_adapter *adapter,
+			     acpi_handle *adapter_handle)
+{
+	struct list_head resource_list;
+	struct resource_entry *entry;
+	struct i2c_acpi_lookup lookup;
+	int ret;
+
+	memset(&lookup, 0, sizeof(lookup));
+	lookup.info = info;
+	lookup.index = -1;
+
+	ret = i2c_acpi_do_lookup(adev, &lookup);
+	if (ret)
+		return ret;
+
+	if (adapter) {
+		/* The adapter must match the one in I2cSerialBus() connector */
+		if (ACPI_HANDLE(&adapter->dev) != lookup.adapter_handle)
+			return -ENODEV;
+	} else {
+		struct acpi_device *adapter_adev;
+
+		/* The adapter must be present */
+		if (acpi_bus_get_device(lookup.adapter_handle, &adapter_adev))
+			return -ENODEV;
+		if (acpi_bus_get_status(adapter_adev) ||
+		    !adapter_adev->status.present)
+			return -ENODEV;
+	}
+
+	info->fwnode = acpi_fwnode_handle(adev);
+	if (adapter_handle)
+		*adapter_handle = lookup.adapter_handle;
+
+	/* Then fill IRQ number if any */
+	INIT_LIST_HEAD(&resource_list);
+	ret = acpi_dev_get_resources(adev, &resource_list, NULL, NULL);
+	if (ret < 0)
+		return -EINVAL;
+
+	resource_list_for_each_entry(entry, &resource_list) {
+		if (resource_type(entry->res) == IORESOURCE_IRQ) {
+			info->irq = entry->res->start;
+			break;
+		}
+	}
+
+	acpi_dev_free_resource_list(&resource_list);
+
+	acpi_set_modalias(adev, dev_name(&adev->dev), info->type,
+			  sizeof(info->type));
+
+	return 0;
+}
+
+static void i2c_acpi_register_device(struct i2c_adapter *adapter,
+				     struct acpi_device *adev,
+				     struct i2c_board_info *info)
+{
+	adev->power.flags.ignore_parent = true;
+	acpi_device_set_enumerated(adev);
+
+	if (!i2c_new_device(adapter, info)) {
+		adev->power.flags.ignore_parent = false;
+		dev_err(&adapter->dev,
+			"failed to add I2C device %s from ACPI\n",
+			dev_name(&adev->dev));
+	}
+}
+
+static acpi_status i2c_acpi_add_device(acpi_handle handle, u32 level,
+				       void *data, void **return_value)
+{
+	struct i2c_adapter *adapter = data;
+	struct acpi_device *adev;
+	struct i2c_board_info info;
+
+	if (acpi_bus_get_device(handle, &adev))
+		return AE_OK;
+
+	if (i2c_acpi_get_info(adev, &info, adapter, NULL))
+		return AE_OK;
+
+	i2c_acpi_register_device(adapter, adev, &info);
+
+	return AE_OK;
+}
+
+#define I2C_ACPI_MAX_SCAN_DEPTH 32
+
+/**
+ * i2c_acpi_register_devices - enumerate I2C slave devices behind adapter
+ * @adap: pointer to adapter
+ *
+ * Enumerate all I2C slave devices behind this adapter by walking the ACPI
+ * namespace. When a device is found it will be added to the Linux device
+ * model and bound to the corresponding ACPI handle.
+ */
+void i2c_acpi_register_devices(struct i2c_adapter *adap)
+{
+	acpi_status status;
+
+	if (!has_acpi_companion(&adap->dev))
+		return;
+
+	status = acpi_walk_namespace(ACPI_TYPE_DEVICE, ACPI_ROOT_OBJECT,
+				     I2C_ACPI_MAX_SCAN_DEPTH,
+				     i2c_acpi_add_device, NULL,
+				     adap, NULL);
+	if (ACPI_FAILURE(status))
+		dev_warn(&adap->dev, "failed to enumerate I2C slaves\n");
+}
+
+static acpi_status i2c_acpi_lookup_speed(acpi_handle handle, u32 level,
+					   void *data, void **return_value)
+{
+	struct i2c_acpi_lookup *lookup = data;
+	struct acpi_device *adev;
+
+	if (acpi_bus_get_device(handle, &adev))
+		return AE_OK;
+
+	if (i2c_acpi_do_lookup(adev, lookup))
+		return AE_OK;
+
+	if (lookup->search_handle != lookup->adapter_handle)
+		return AE_OK;
+
+	if (lookup->speed <= lookup->min_speed)
+		lookup->min_speed = lookup->speed;
+
+	return AE_OK;
+}
+
+/**
+ * i2c_acpi_find_bus_speed - find I2C bus speed from ACPI
+ * @dev: The device owning the bus
+ *
+ * Find the I2C bus speed by walking the ACPI namespace for all I2C slaves
+ * devices connected to this bus and use the speed of slowest device.
+ *
+ * Returns the speed in Hz or zero
+ */
+u32 i2c_acpi_find_bus_speed(struct device *dev)
+{
+	struct i2c_acpi_lookup lookup;
+	struct i2c_board_info dummy;
+	acpi_status status;
+
+	if (!has_acpi_companion(dev))
+		return 0;
+
+	memset(&lookup, 0, sizeof(lookup));
+	lookup.search_handle = ACPI_HANDLE(dev);
+	lookup.min_speed = UINT_MAX;
+	lookup.info = &dummy;
+	lookup.index = -1;
+
+	status = acpi_walk_namespace(ACPI_TYPE_DEVICE, ACPI_ROOT_OBJECT,
+				     I2C_ACPI_MAX_SCAN_DEPTH,
+				     i2c_acpi_lookup_speed, NULL,
+				     &lookup, NULL);
+
+	if (ACPI_FAILURE(status)) {
+		dev_warn(dev, "unable to find I2C bus speed from ACPI\n");
+		return 0;
+	}
+
+	return lookup.min_speed != UINT_MAX ? lookup.min_speed : 0;
+}
+EXPORT_SYMBOL_GPL(i2c_acpi_find_bus_speed);
+
+static int i2c_acpi_match_adapter(struct device *dev, void *data)
+{
+	struct i2c_adapter *adapter = i2c_verify_adapter(dev);
+
+	if (!adapter)
+		return 0;
+
+	return ACPI_HANDLE(dev) == (acpi_handle)data;
+}
+
+static int i2c_acpi_match_device(struct device *dev, void *data)
+{
+	return ACPI_COMPANION(dev) == data;
+}
+
+static struct i2c_adapter *i2c_acpi_find_adapter_by_handle(acpi_handle handle)
+{
+	struct device *dev;
+
+	dev = bus_find_device(&i2c_bus_type, NULL, handle,
+			      i2c_acpi_match_adapter);
+	return dev ? i2c_verify_adapter(dev) : NULL;
+}
+
+static struct i2c_client *i2c_acpi_find_client_by_adev(struct acpi_device *adev)
+{
+	struct device *dev;
+
+	dev = bus_find_device(&i2c_bus_type, NULL, adev, i2c_acpi_match_device);
+	return dev ? i2c_verify_client(dev) : NULL;
+}
+
+static int i2c_acpi_notify(struct notifier_block *nb, unsigned long value,
+			   void *arg)
+{
+	struct acpi_device *adev = arg;
+	struct i2c_board_info info;
+	acpi_handle adapter_handle;
+	struct i2c_adapter *adapter;
+	struct i2c_client *client;
+
+	switch (value) {
+	case ACPI_RECONFIG_DEVICE_ADD:
+		if (i2c_acpi_get_info(adev, &info, NULL, &adapter_handle))
+			break;
+
+		adapter = i2c_acpi_find_adapter_by_handle(adapter_handle);
+		if (!adapter)
+			break;
+
+		i2c_acpi_register_device(adapter, adev, &info);
+		break;
+	case ACPI_RECONFIG_DEVICE_REMOVE:
+		if (!acpi_device_enumerated(adev))
+			break;
+
+		client = i2c_acpi_find_client_by_adev(adev);
+		if (!client)
+			break;
+
+		i2c_unregister_device(client);
+		put_device(&client->dev);
+		break;
+	}
+
+	return NOTIFY_OK;
+}
+
+struct notifier_block i2c_acpi_notifier = {
+	.notifier_call = i2c_acpi_notify,
+};
+
+/**
+ * i2c_acpi_new_device - Create i2c-client for the Nth I2cSerialBus resource
+ * @dev:     Device owning the ACPI resources to get the client from
+ * @index:   Index of ACPI resource to get
+ * @info:    describes the I2C device; note this is modified (addr gets set)
+ * Context: can sleep
+ *
+ * By default the i2c subsys creates an i2c-client for the first I2cSerialBus
+ * resource of an acpi_device, but some acpi_devices have multiple I2cSerialBus
+ * resources, in that case this function can be used to create an i2c-client
+ * for other I2cSerialBus resources in the Current Resource Settings table.
+ *
+ * Also see i2c_new_device, which this function calls to create the i2c-client.
+ *
+ * Returns a pointer to the new i2c-client, or NULL if the adapter is not found.
+ */
+struct i2c_client *i2c_acpi_new_device(struct device *dev, int index,
+				       struct i2c_board_info *info)
+{
+	struct i2c_acpi_lookup lookup;
+	struct i2c_adapter *adapter;
+	struct acpi_device *adev;
+	LIST_HEAD(resource_list);
+	int ret;
+
+	adev = ACPI_COMPANION(dev);
+	if (!adev)
+		return NULL;
+
+	memset(&lookup, 0, sizeof(lookup));
+	lookup.info = info;
+	lookup.device_handle = acpi_device_handle(adev);
+	lookup.index = index;
+
+	ret = acpi_dev_get_resources(adev, &resource_list,
+				     i2c_acpi_fill_info, &lookup);
+	acpi_dev_free_resource_list(&resource_list);
+
+	if (ret < 0 || !info->addr)
+		return NULL;
+
+	adapter = i2c_acpi_find_adapter_by_handle(lookup.adapter_handle);
+	if (!adapter)
+		return NULL;
+
+	return i2c_new_device(adapter, info);
+}
+EXPORT_SYMBOL_GPL(i2c_acpi_new_device);
+
+#ifdef CONFIG_ACPI_I2C_OPREGION
+static int acpi_gsb_i2c_read_bytes(struct i2c_client *client,
+		u8 cmd, u8 *data, u8 data_len)
+{
+
+	struct i2c_msg msgs[2];
+	int ret;
+	u8 *buffer;
+
+	buffer = kzalloc(data_len, GFP_KERNEL);
+	if (!buffer)
+		return AE_NO_MEMORY;
+
+	msgs[0].addr = client->addr;
+	msgs[0].flags = client->flags;
+	msgs[0].len = 1;
+	msgs[0].buf = &cmd;
+
+	msgs[1].addr = client->addr;
+	msgs[1].flags = client->flags | I2C_M_RD;
+	msgs[1].len = data_len;
+	msgs[1].buf = buffer;
+
+	ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
+	if (ret < 0)
+		dev_err(&client->adapter->dev, "i2c read failed\n");
+	else
+		memcpy(data, buffer, data_len);
+
+	kfree(buffer);
+	return ret;
+}
+
+static int acpi_gsb_i2c_write_bytes(struct i2c_client *client,
+		u8 cmd, u8 *data, u8 data_len)
+{
+
+	struct i2c_msg msgs[1];
+	u8 *buffer;
+	int ret = AE_OK;
+
+	buffer = kzalloc(data_len + 1, GFP_KERNEL);
+	if (!buffer)
+		return AE_NO_MEMORY;
+
+	buffer[0] = cmd;
+	memcpy(buffer + 1, data, data_len);
+
+	msgs[0].addr = client->addr;
+	msgs[0].flags = client->flags;
+	msgs[0].len = data_len + 1;
+	msgs[0].buf = buffer;
+
+	ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
+	if (ret < 0)
+		dev_err(&client->adapter->dev, "i2c write failed\n");
+
+	kfree(buffer);
+	return ret;
+}
+
+static acpi_status
+i2c_acpi_space_handler(u32 function, acpi_physical_address command,
+			u32 bits, u64 *value64,
+			void *handler_context, void *region_context)
+{
+	struct gsb_buffer *gsb = (struct gsb_buffer *)value64;
+	struct i2c_acpi_handler_data *data = handler_context;
+	struct acpi_connection_info *info = &data->info;
+	struct acpi_resource_i2c_serialbus *sb;
+	struct i2c_adapter *adapter = data->adapter;
+	struct i2c_client *client;
+	struct acpi_resource *ares;
+	u32 accessor_type = function >> 16;
+	u8 action = function & ACPI_IO_MASK;
+	acpi_status ret;
+	int status;
+
+	ret = acpi_buffer_to_resource(info->connection, info->length, &ares);
+	if (ACPI_FAILURE(ret))
+		return ret;
+
+	client = kzalloc(sizeof(*client), GFP_KERNEL);
+	if (!client) {
+		ret = AE_NO_MEMORY;
+		goto err;
+	}
+
+	if (!value64 || ares->type != ACPI_RESOURCE_TYPE_SERIAL_BUS) {
+		ret = AE_BAD_PARAMETER;
+		goto err;
+	}
+
+	sb = &ares->data.i2c_serial_bus;
+	if (sb->type != ACPI_RESOURCE_SERIAL_TYPE_I2C) {
+		ret = AE_BAD_PARAMETER;
+		goto err;
+	}
+
+	client->adapter = adapter;
+	client->addr = sb->slave_address;
+
+	if (sb->access_mode == ACPI_I2C_10BIT_MODE)
+		client->flags |= I2C_CLIENT_TEN;
+
+	switch (accessor_type) {
+	case ACPI_GSB_ACCESS_ATTRIB_SEND_RCV:
+		if (action == ACPI_READ) {
+			status = i2c_smbus_read_byte(client);
+			if (status >= 0) {
+				gsb->bdata = status;
+				status = 0;
+			}
+		} else {
+			status = i2c_smbus_write_byte(client, gsb->bdata);
+		}
+		break;
+
+	case ACPI_GSB_ACCESS_ATTRIB_BYTE:
+		if (action == ACPI_READ) {
+			status = i2c_smbus_read_byte_data(client, command);
+			if (status >= 0) {
+				gsb->bdata = status;
+				status = 0;
+			}
+		} else {
+			status = i2c_smbus_write_byte_data(client, command,
+					gsb->bdata);
+		}
+		break;
+
+	case ACPI_GSB_ACCESS_ATTRIB_WORD:
+		if (action == ACPI_READ) {
+			status = i2c_smbus_read_word_data(client, command);
+			if (status >= 0) {
+				gsb->wdata = status;
+				status = 0;
+			}
+		} else {
+			status = i2c_smbus_write_word_data(client, command,
+					gsb->wdata);
+		}
+		break;
+
+	case ACPI_GSB_ACCESS_ATTRIB_BLOCK:
+		if (action == ACPI_READ) {
+			status = i2c_smbus_read_block_data(client, command,
+					gsb->data);
+			if (status >= 0) {
+				gsb->len = status;
+				status = 0;
+			}
+		} else {
+			status = i2c_smbus_write_block_data(client, command,
+					gsb->len, gsb->data);
+		}
+		break;
+
+	case ACPI_GSB_ACCESS_ATTRIB_MULTIBYTE:
+		if (action == ACPI_READ) {
+			status = acpi_gsb_i2c_read_bytes(client, command,
+					gsb->data, info->access_length);
+			if (status > 0)
+				status = 0;
+		} else {
+			status = acpi_gsb_i2c_write_bytes(client, command,
+					gsb->data, info->access_length);
+		}
+		break;
+
+	default:
+		dev_warn(&adapter->dev, "protocol 0x%02x not supported for client 0x%02x\n",
+			 accessor_type, client->addr);
+		ret = AE_BAD_PARAMETER;
+		goto err;
+	}
+
+	gsb->status = status;
+
+ err:
+	kfree(client);
+	ACPI_FREE(ares);
+	return ret;
+}
+
+
+int i2c_acpi_install_space_handler(struct i2c_adapter *adapter)
+{
+	acpi_handle handle;
+	struct i2c_acpi_handler_data *data;
+	acpi_status status;
+
+	if (!adapter->dev.parent)
+		return -ENODEV;
+
+	handle = ACPI_HANDLE(adapter->dev.parent);
+
+	if (!handle)
+		return -ENODEV;
+
+	data = kzalloc(sizeof(struct i2c_acpi_handler_data),
+			    GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	data->adapter = adapter;
+	status = acpi_bus_attach_private_data(handle, (void *)data);
+	if (ACPI_FAILURE(status)) {
+		kfree(data);
+		return -ENOMEM;
+	}
+
+	status = acpi_install_address_space_handler(handle,
+				ACPI_ADR_SPACE_GSBUS,
+				&i2c_acpi_space_handler,
+				NULL,
+				data);
+	if (ACPI_FAILURE(status)) {
+		dev_err(&adapter->dev, "Error installing i2c space handler\n");
+		acpi_bus_detach_private_data(handle);
+		kfree(data);
+		return -ENOMEM;
+	}
+
+	acpi_walk_dep_device_list(handle);
+	return 0;
+}
+
+void i2c_acpi_remove_space_handler(struct i2c_adapter *adapter)
+{
+	acpi_handle handle;
+	struct i2c_acpi_handler_data *data;
+	acpi_status status;
+
+	if (!adapter->dev.parent)
+		return;
+
+	handle = ACPI_HANDLE(adapter->dev.parent);
+
+	if (!handle)
+		return;
+
+	acpi_remove_address_space_handler(handle,
+				ACPI_ADR_SPACE_GSBUS,
+				&i2c_acpi_space_handler);
+
+	status = acpi_bus_get_private_data(handle, (void **)&data);
+	if (ACPI_SUCCESS(status))
+		kfree(data);
+
+	acpi_bus_detach_private_data(handle);
+}
+#endif /* CONFIG_ACPI_I2C_OPREGION */
diff --git a/drivers/i2c/i2c-core.c b/drivers/i2c/i2c-core-base.c
index 82576aaccc90..c89dac7fd2e7 100644
--- a/drivers/i2c/i2c-core.c
+++ b/drivers/i2c/i2c-core-base.c
@@ -1,36 +1,26 @@
-/* i2c-core.c - a device driver for the iic-bus interface		     */
-/* ------------------------------------------------------------------------- */
-/*   Copyright (C) 1995-99 Simon G. Vogl
-
-    This program is free software; you can redistribute it and/or modify
-    it under the terms of the 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.			     */
-/* ------------------------------------------------------------------------- */
-
-/* With some changes from Kyösti Mälkki <kmalkki@cc.hut.fi>.
-   All SMBus-related things are written by Frodo Looijaard <frodol@dds.nl>
-   SMBus 2.0 support by Mark Studebaker <mdsxyz123@yahoo.com> and
-   Jean Delvare <jdelvare@suse.de>
-   Mux support by Rodolfo Giometti <giometti@enneenne.com> and
-   Michael Lawnick <michael.lawnick.ext@nsn.com>
-   OF support is copyright (c) 2008 Jochen Friedrich <jochen@scram.de>
-   (based on a previous patch from Jon Smirl <jonsmirl@gmail.com>) and
-   (c) 2013  Wolfram Sang <wsa@the-dreams.de>
-   I2C ACPI code Copyright (C) 2014 Intel Corp
-   Author: Lan Tianyu <tianyu.lan@intel.com>
-   I2C slave support (c) 2014 by Wolfram Sang <wsa@sang-engineering.com>
+/*
+ * Linux I2C core
+ *
+ * Copyright (C) 1995-99 Simon G. Vogl
+ *   With some changes from Kyösti Mälkki <kmalkki@cc.hut.fi>
+ *   Mux support by Rodolfo Giometti <giometti@enneenne.com> and
+ *   Michael Lawnick <michael.lawnick.ext@nsn.com>
+ *
+ * Copyright (C) 2013-2017 Wolfram Sang <wsa@the-dreams.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.
  */
 
 #define pr_fmt(fmt) "i2c-core: " fmt
 
 #include <dt-bindings/i2c/i2c.h>
-#include <linux/uaccess.h>
 #include <linux/acpi.h>
 #include <linux/clk/clk-conf.h>
 #include <linux/completion.h>
@@ -38,7 +28,6 @@
 #include <linux/err.h>
 #include <linux/errno.h>
 #include <linux/gpio.h>
-#include <linux/hardirq.h>
 #include <linux/i2c.h>
 #include <linux/idr.h>
 #include <linux/init.h>
@@ -68,9 +57,10 @@
 #define I2C_ADDR_7BITS_MAX	0x77
 #define I2C_ADDR_7BITS_COUNT	(I2C_ADDR_7BITS_MAX + 1)
 
-/* core_lock protects i2c_adapter_idr, and guarantees
-   that device detection, deletion of detected devices, and attach_adapter
-   calls are serialized */
+/*
+ * core_lock protects i2c_adapter_idr, and guarantees that device detection,
+ * deletion of detected devices, and attach_adapter calls are serialized
+ */
 static DEFINE_MUTEX(core_lock);
 static DEFINE_IDR(i2c_adapter_idr);
 
@@ -90,652 +80,6 @@ void i2c_transfer_trace_unreg(void)
 	static_key_slow_dec(&i2c_trace_msg);
 }
 
-#if defined(CONFIG_ACPI)
-struct i2c_acpi_handler_data {
-	struct acpi_connection_info info;
-	struct i2c_adapter *adapter;
-};
-
-struct gsb_buffer {
-	u8	status;
-	u8	len;
-	union {
-		u16	wdata;
-		u8	bdata;
-		u8	data[0];
-	};
-} __packed;
-
-struct i2c_acpi_lookup {
-	struct i2c_board_info *info;
-	acpi_handle adapter_handle;
-	acpi_handle device_handle;
-	acpi_handle search_handle;
-	int n;
-	int index;
-	u32 speed;
-	u32 min_speed;
-};
-
-static int i2c_acpi_fill_info(struct acpi_resource *ares, void *data)
-{
-	struct i2c_acpi_lookup *lookup = data;
-	struct i2c_board_info *info = lookup->info;
-	struct acpi_resource_i2c_serialbus *sb;
-	acpi_status status;
-
-	if (info->addr || ares->type != ACPI_RESOURCE_TYPE_SERIAL_BUS)
-		return 1;
-
-	sb = &ares->data.i2c_serial_bus;
-	if (sb->type != ACPI_RESOURCE_SERIAL_TYPE_I2C)
-		return 1;
-
-	if (lookup->index != -1 && lookup->n++ != lookup->index)
-		return 1;
-
-	status = acpi_get_handle(lookup->device_handle,
-				 sb->resource_source.string_ptr,
-				 &lookup->adapter_handle);
-	if (!ACPI_SUCCESS(status))
-		return 1;
-
-	info->addr = sb->slave_address;
-	lookup->speed = sb->connection_speed;
-	if (sb->access_mode == ACPI_I2C_10BIT_MODE)
-		info->flags |= I2C_CLIENT_TEN;
-
-	return 1;
-}
-
-static int i2c_acpi_do_lookup(struct acpi_device *adev,
-			      struct i2c_acpi_lookup *lookup)
-{
-	struct i2c_board_info *info = lookup->info;
-	struct list_head resource_list;
-	int ret;
-
-	if (acpi_bus_get_status(adev) || !adev->status.present ||
-	    acpi_device_enumerated(adev))
-		return -EINVAL;
-
-	memset(info, 0, sizeof(*info));
-	lookup->device_handle = acpi_device_handle(adev);
-
-	/* Look up for I2cSerialBus resource */
-	INIT_LIST_HEAD(&resource_list);
-	ret = acpi_dev_get_resources(adev, &resource_list,
-				     i2c_acpi_fill_info, lookup);
-	acpi_dev_free_resource_list(&resource_list);
-
-	if (ret < 0 || !info->addr)
-		return -EINVAL;
-
-	return 0;
-}
-
-static int i2c_acpi_get_info(struct acpi_device *adev,
-			     struct i2c_board_info *info,
-			     struct i2c_adapter *adapter,
-			     acpi_handle *adapter_handle)
-{
-	struct list_head resource_list;
-	struct resource_entry *entry;
-	struct i2c_acpi_lookup lookup;
-	int ret;
-
-	memset(&lookup, 0, sizeof(lookup));
-	lookup.info = info;
-	lookup.index = -1;
-
-	ret = i2c_acpi_do_lookup(adev, &lookup);
-	if (ret)
-		return ret;
-
-	if (adapter) {
-		/* The adapter must match the one in I2cSerialBus() connector */
-		if (ACPI_HANDLE(&adapter->dev) != lookup.adapter_handle)
-			return -ENODEV;
-	} else {
-		struct acpi_device *adapter_adev;
-
-		/* The adapter must be present */
-		if (acpi_bus_get_device(lookup.adapter_handle, &adapter_adev))
-			return -ENODEV;
-		if (acpi_bus_get_status(adapter_adev) ||
-		    !adapter_adev->status.present)
-			return -ENODEV;
-	}
-
-	info->fwnode = acpi_fwnode_handle(adev);
-	if (adapter_handle)
-		*adapter_handle = lookup.adapter_handle;
-
-	/* Then fill IRQ number if any */
-	INIT_LIST_HEAD(&resource_list);
-	ret = acpi_dev_get_resources(adev, &resource_list, NULL, NULL);
-	if (ret < 0)
-		return -EINVAL;
-
-	resource_list_for_each_entry(entry, &resource_list) {
-		if (resource_type(entry->res) == IORESOURCE_IRQ) {
-			info->irq = entry->res->start;
-			break;
-		}
-	}
-
-	acpi_dev_free_resource_list(&resource_list);
-
-	acpi_set_modalias(adev, dev_name(&adev->dev), info->type,
-			  sizeof(info->type));
-
-	return 0;
-}
-
-static void i2c_acpi_register_device(struct i2c_adapter *adapter,
-				     struct acpi_device *adev,
-				     struct i2c_board_info *info)
-{
-	adev->power.flags.ignore_parent = true;
-	acpi_device_set_enumerated(adev);
-
-	if (!i2c_new_device(adapter, info)) {
-		adev->power.flags.ignore_parent = false;
-		dev_err(&adapter->dev,
-			"failed to add I2C device %s from ACPI\n",
-			dev_name(&adev->dev));
-	}
-}
-
-static acpi_status i2c_acpi_add_device(acpi_handle handle, u32 level,
-				       void *data, void **return_value)
-{
-	struct i2c_adapter *adapter = data;
-	struct acpi_device *adev;
-	struct i2c_board_info info;
-
-	if (acpi_bus_get_device(handle, &adev))
-		return AE_OK;
-
-	if (i2c_acpi_get_info(adev, &info, adapter, NULL))
-		return AE_OK;
-
-	i2c_acpi_register_device(adapter, adev, &info);
-
-	return AE_OK;
-}
-
-#define I2C_ACPI_MAX_SCAN_DEPTH 32
-
-/**
- * i2c_acpi_register_devices - enumerate I2C slave devices behind adapter
- * @adap: pointer to adapter
- *
- * Enumerate all I2C slave devices behind this adapter by walking the ACPI
- * namespace. When a device is found it will be added to the Linux device
- * model and bound to the corresponding ACPI handle.
- */
-static void i2c_acpi_register_devices(struct i2c_adapter *adap)
-{
-	acpi_status status;
-
-	if (!has_acpi_companion(&adap->dev))
-		return;
-
-	status = acpi_walk_namespace(ACPI_TYPE_DEVICE, ACPI_ROOT_OBJECT,
-				     I2C_ACPI_MAX_SCAN_DEPTH,
-				     i2c_acpi_add_device, NULL,
-				     adap, NULL);
-	if (ACPI_FAILURE(status))
-		dev_warn(&adap->dev, "failed to enumerate I2C slaves\n");
-}
-
-static acpi_status i2c_acpi_lookup_speed(acpi_handle handle, u32 level,
-					   void *data, void **return_value)
-{
-	struct i2c_acpi_lookup *lookup = data;
-	struct acpi_device *adev;
-
-	if (acpi_bus_get_device(handle, &adev))
-		return AE_OK;
-
-	if (i2c_acpi_do_lookup(adev, lookup))
-		return AE_OK;
-
-	if (lookup->search_handle != lookup->adapter_handle)
-		return AE_OK;
-
-	if (lookup->speed <= lookup->min_speed)
-		lookup->min_speed = lookup->speed;
-
-	return AE_OK;
-}
-
-/**
- * i2c_acpi_find_bus_speed - find I2C bus speed from ACPI
- * @dev: The device owning the bus
- *
- * Find the I2C bus speed by walking the ACPI namespace for all I2C slaves
- * devices connected to this bus and use the speed of slowest device.
- *
- * Returns the speed in Hz or zero
- */
-u32 i2c_acpi_find_bus_speed(struct device *dev)
-{
-	struct i2c_acpi_lookup lookup;
-	struct i2c_board_info dummy;
-	acpi_status status;
-
-	if (!has_acpi_companion(dev))
-		return 0;
-
-	memset(&lookup, 0, sizeof(lookup));
-	lookup.search_handle = ACPI_HANDLE(dev);
-	lookup.min_speed = UINT_MAX;
-	lookup.info = &dummy;
-	lookup.index = -1;
-
-	status = acpi_walk_namespace(ACPI_TYPE_DEVICE, ACPI_ROOT_OBJECT,
-				     I2C_ACPI_MAX_SCAN_DEPTH,
-				     i2c_acpi_lookup_speed, NULL,
-				     &lookup, NULL);
-
-	if (ACPI_FAILURE(status)) {
-		dev_warn(dev, "unable to find I2C bus speed from ACPI\n");
-		return 0;
-	}
-
-	return lookup.min_speed != UINT_MAX ? lookup.min_speed : 0;
-}
-EXPORT_SYMBOL_GPL(i2c_acpi_find_bus_speed);
-
-static int i2c_acpi_match_adapter(struct device *dev, void *data)
-{
-	struct i2c_adapter *adapter = i2c_verify_adapter(dev);
-
-	if (!adapter)
-		return 0;
-
-	return ACPI_HANDLE(dev) == (acpi_handle)data;
-}
-
-static int i2c_acpi_match_device(struct device *dev, void *data)
-{
-	return ACPI_COMPANION(dev) == data;
-}
-
-static struct i2c_adapter *i2c_acpi_find_adapter_by_handle(acpi_handle handle)
-{
-	struct device *dev;
-
-	dev = bus_find_device(&i2c_bus_type, NULL, handle,
-			      i2c_acpi_match_adapter);
-	return dev ? i2c_verify_adapter(dev) : NULL;
-}
-
-static struct i2c_client *i2c_acpi_find_client_by_adev(struct acpi_device *adev)
-{
-	struct device *dev;
-
-	dev = bus_find_device(&i2c_bus_type, NULL, adev, i2c_acpi_match_device);
-	return dev ? i2c_verify_client(dev) : NULL;
-}
-
-static int i2c_acpi_notify(struct notifier_block *nb, unsigned long value,
-			   void *arg)
-{
-	struct acpi_device *adev = arg;
-	struct i2c_board_info info;
-	acpi_handle adapter_handle;
-	struct i2c_adapter *adapter;
-	struct i2c_client *client;
-
-	switch (value) {
-	case ACPI_RECONFIG_DEVICE_ADD:
-		if (i2c_acpi_get_info(adev, &info, NULL, &adapter_handle))
-			break;
-
-		adapter = i2c_acpi_find_adapter_by_handle(adapter_handle);
-		if (!adapter)
-			break;
-
-		i2c_acpi_register_device(adapter, adev, &info);
-		break;
-	case ACPI_RECONFIG_DEVICE_REMOVE:
-		if (!acpi_device_enumerated(adev))
-			break;
-
-		client = i2c_acpi_find_client_by_adev(adev);
-		if (!client)
-			break;
-
-		i2c_unregister_device(client);
-		put_device(&client->dev);
-		break;
-	}
-
-	return NOTIFY_OK;
-}
-
-static struct notifier_block i2c_acpi_notifier = {
-	.notifier_call = i2c_acpi_notify,
-};
-
-/**
- * i2c_acpi_new_device - Create i2c-client for the Nth I2cSerialBus resource
- * @dev:     Device owning the ACPI resources to get the client from
- * @index:   Index of ACPI resource to get
- * @info:    describes the I2C device; note this is modified (addr gets set)
- * Context: can sleep
- *
- * By default the i2c subsys creates an i2c-client for the first I2cSerialBus
- * resource of an acpi_device, but some acpi_devices have multiple I2cSerialBus
- * resources, in that case this function can be used to create an i2c-client
- * for other I2cSerialBus resources in the Current Resource Settings table.
- *
- * Also see i2c_new_device, which this function calls to create the i2c-client.
- *
- * Returns a pointer to the new i2c-client, or NULL if the adapter is not found.
- */
-struct i2c_client *i2c_acpi_new_device(struct device *dev, int index,
-				       struct i2c_board_info *info)
-{
-	struct i2c_acpi_lookup lookup;
-	struct i2c_adapter *adapter;
-	struct acpi_device *adev;
-	LIST_HEAD(resource_list);
-	int ret;
-
-	adev = ACPI_COMPANION(dev);
-	if (!adev)
-		return NULL;
-
-	memset(&lookup, 0, sizeof(lookup));
-	lookup.info = info;
-	lookup.device_handle = acpi_device_handle(adev);
-	lookup.index = index;
-
-	ret = acpi_dev_get_resources(adev, &resource_list,
-				     i2c_acpi_fill_info, &lookup);
-	acpi_dev_free_resource_list(&resource_list);
-
-	if (ret < 0 || !info->addr)
-		return NULL;
-
-	adapter = i2c_acpi_find_adapter_by_handle(lookup.adapter_handle);
-	if (!adapter)
-		return NULL;
-
-	return i2c_new_device(adapter, info);
-}
-EXPORT_SYMBOL_GPL(i2c_acpi_new_device);
-#else /* CONFIG_ACPI */
-static inline void i2c_acpi_register_devices(struct i2c_adapter *adap) { }
-extern struct notifier_block i2c_acpi_notifier;
-#endif /* CONFIG_ACPI */
-
-#ifdef CONFIG_ACPI_I2C_OPREGION
-static int acpi_gsb_i2c_read_bytes(struct i2c_client *client,
-		u8 cmd, u8 *data, u8 data_len)
-{
-
-	struct i2c_msg msgs[2];
-	int ret;
-	u8 *buffer;
-
-	buffer = kzalloc(data_len, GFP_KERNEL);
-	if (!buffer)
-		return AE_NO_MEMORY;
-
-	msgs[0].addr = client->addr;
-	msgs[0].flags = client->flags;
-	msgs[0].len = 1;
-	msgs[0].buf = &cmd;
-
-	msgs[1].addr = client->addr;
-	msgs[1].flags = client->flags | I2C_M_RD;
-	msgs[1].len = data_len;
-	msgs[1].buf = buffer;
-
-	ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
-	if (ret < 0)
-		dev_err(&client->adapter->dev, "i2c read failed\n");
-	else
-		memcpy(data, buffer, data_len);
-
-	kfree(buffer);
-	return ret;
-}
-
-static int acpi_gsb_i2c_write_bytes(struct i2c_client *client,
-		u8 cmd, u8 *data, u8 data_len)
-{
-
-	struct i2c_msg msgs[1];
-	u8 *buffer;
-	int ret = AE_OK;
-
-	buffer = kzalloc(data_len + 1, GFP_KERNEL);
-	if (!buffer)
-		return AE_NO_MEMORY;
-
-	buffer[0] = cmd;
-	memcpy(buffer + 1, data, data_len);
-
-	msgs[0].addr = client->addr;
-	msgs[0].flags = client->flags;
-	msgs[0].len = data_len + 1;
-	msgs[0].buf = buffer;
-
-	ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
-	if (ret < 0)
-		dev_err(&client->adapter->dev, "i2c write failed\n");
-
-	kfree(buffer);
-	return ret;
-}
-
-static acpi_status
-i2c_acpi_space_handler(u32 function, acpi_physical_address command,
-			u32 bits, u64 *value64,
-			void *handler_context, void *region_context)
-{
-	struct gsb_buffer *gsb = (struct gsb_buffer *)value64;
-	struct i2c_acpi_handler_data *data = handler_context;
-	struct acpi_connection_info *info = &data->info;
-	struct acpi_resource_i2c_serialbus *sb;
-	struct i2c_adapter *adapter = data->adapter;
-	struct i2c_client *client;
-	struct acpi_resource *ares;
-	u32 accessor_type = function >> 16;
-	u8 action = function & ACPI_IO_MASK;
-	acpi_status ret;
-	int status;
-
-	ret = acpi_buffer_to_resource(info->connection, info->length, &ares);
-	if (ACPI_FAILURE(ret))
-		return ret;
-
-	client = kzalloc(sizeof(*client), GFP_KERNEL);
-	if (!client) {
-		ret = AE_NO_MEMORY;
-		goto err;
-	}
-
-	if (!value64 || ares->type != ACPI_RESOURCE_TYPE_SERIAL_BUS) {
-		ret = AE_BAD_PARAMETER;
-		goto err;
-	}
-
-	sb = &ares->data.i2c_serial_bus;
-	if (sb->type != ACPI_RESOURCE_SERIAL_TYPE_I2C) {
-		ret = AE_BAD_PARAMETER;
-		goto err;
-	}
-
-	client->adapter = adapter;
-	client->addr = sb->slave_address;
-
-	if (sb->access_mode == ACPI_I2C_10BIT_MODE)
-		client->flags |= I2C_CLIENT_TEN;
-
-	switch (accessor_type) {
-	case ACPI_GSB_ACCESS_ATTRIB_SEND_RCV:
-		if (action == ACPI_READ) {
-			status = i2c_smbus_read_byte(client);
-			if (status >= 0) {
-				gsb->bdata = status;
-				status = 0;
-			}
-		} else {
-			status = i2c_smbus_write_byte(client, gsb->bdata);
-		}
-		break;
-
-	case ACPI_GSB_ACCESS_ATTRIB_BYTE:
-		if (action == ACPI_READ) {
-			status = i2c_smbus_read_byte_data(client, command);
-			if (status >= 0) {
-				gsb->bdata = status;
-				status = 0;
-			}
-		} else {
-			status = i2c_smbus_write_byte_data(client, command,
-					gsb->bdata);
-		}
-		break;
-
-	case ACPI_GSB_ACCESS_ATTRIB_WORD:
-		if (action == ACPI_READ) {
-			status = i2c_smbus_read_word_data(client, command);
-			if (status >= 0) {
-				gsb->wdata = status;
-				status = 0;
-			}
-		} else {
-			status = i2c_smbus_write_word_data(client, command,
-					gsb->wdata);
-		}
-		break;
-
-	case ACPI_GSB_ACCESS_ATTRIB_BLOCK:
-		if (action == ACPI_READ) {
-			status = i2c_smbus_read_block_data(client, command,
-					gsb->data);
-			if (status >= 0) {
-				gsb->len = status;
-				status = 0;
-			}
-		} else {
-			status = i2c_smbus_write_block_data(client, command,
-					gsb->len, gsb->data);
-		}
-		break;
-
-	case ACPI_GSB_ACCESS_ATTRIB_MULTIBYTE:
-		if (action == ACPI_READ) {
-			status = acpi_gsb_i2c_read_bytes(client, command,
-					gsb->data, info->access_length);
-			if (status > 0)
-				status = 0;
-		} else {
-			status = acpi_gsb_i2c_write_bytes(client, command,
-					gsb->data, info->access_length);
-		}
-		break;
-
-	default:
-		dev_warn(&adapter->dev, "protocol 0x%02x not supported for client 0x%02x\n",
-			 accessor_type, client->addr);
-		ret = AE_BAD_PARAMETER;
-		goto err;
-	}
-
-	gsb->status = status;
-
- err:
-	kfree(client);
-	ACPI_FREE(ares);
-	return ret;
-}
-
-
-static int i2c_acpi_install_space_handler(struct i2c_adapter *adapter)
-{
-	acpi_handle handle;
-	struct i2c_acpi_handler_data *data;
-	acpi_status status;
-
-	if (!adapter->dev.parent)
-		return -ENODEV;
-
-	handle = ACPI_HANDLE(adapter->dev.parent);
-
-	if (!handle)
-		return -ENODEV;
-
-	data = kzalloc(sizeof(struct i2c_acpi_handler_data),
-			    GFP_KERNEL);
-	if (!data)
-		return -ENOMEM;
-
-	data->adapter = adapter;
-	status = acpi_bus_attach_private_data(handle, (void *)data);
-	if (ACPI_FAILURE(status)) {
-		kfree(data);
-		return -ENOMEM;
-	}
-
-	status = acpi_install_address_space_handler(handle,
-				ACPI_ADR_SPACE_GSBUS,
-				&i2c_acpi_space_handler,
-				NULL,
-				data);
-	if (ACPI_FAILURE(status)) {
-		dev_err(&adapter->dev, "Error installing i2c space handler\n");
-		acpi_bus_detach_private_data(handle);
-		kfree(data);
-		return -ENOMEM;
-	}
-
-	acpi_walk_dep_device_list(handle);
-	return 0;
-}
-
-static void i2c_acpi_remove_space_handler(struct i2c_adapter *adapter)
-{
-	acpi_handle handle;
-	struct i2c_acpi_handler_data *data;
-	acpi_status status;
-
-	if (!adapter->dev.parent)
-		return;
-
-	handle = ACPI_HANDLE(adapter->dev.parent);
-
-	if (!handle)
-		return;
-
-	acpi_remove_address_space_handler(handle,
-				ACPI_ADR_SPACE_GSBUS,
-				&i2c_acpi_space_handler);
-
-	status = acpi_bus_get_private_data(handle, (void **)&data);
-	if (ACPI_SUCCESS(status))
-		kfree(data);
-
-	acpi_bus_detach_private_data(handle);
-}
-#else /* CONFIG_ACPI_I2C_OPREGION */
-static inline void i2c_acpi_remove_space_handler(struct i2c_adapter *adapter)
-{ }
-
-static inline int i2c_acpi_install_space_handler(struct i2c_adapter *adapter)
-{ return 0; }
-#endif /* CONFIG_ACPI_I2C_OPREGION */
-
-/* ------------------------------------------------------------------------- */
-
 const struct i2c_device_id *i2c_match_id(const struct i2c_device_id *id,
 						const struct i2c_client *client)
 {
@@ -1195,7 +539,7 @@ static unsigned short i2c_encode_flags_to_addr(struct i2c_client *client)
 
 /* This is a permissive address validity check, I2C address map constraints
  * are purposely not enforced, except for the general call address. */
-static int i2c_check_addr_validity(unsigned addr, unsigned short flags)
+int i2c_check_addr_validity(unsigned addr, unsigned short flags)
 {
 	if (flags & I2C_CLIENT_TEN) {
 		/* 10-bit address, all values are valid */
@@ -1213,7 +557,7 @@ static int i2c_check_addr_validity(unsigned addr, unsigned short flags)
  * device uses a reserved address, then it shouldn't be probed. 7-bit
  * addressing is assumed, 10-bit address devices are rare and should be
  * explicitly enumerated. */
-static int i2c_check_7bit_addr_validity_strict(unsigned short addr)
+int i2c_check_7bit_addr_validity_strict(unsigned short addr)
 {
 	/*
 	 * Reserved addresses per I2C specification:
@@ -1764,210 +1108,6 @@ static void i2c_scan_static_board_info(struct i2c_adapter *adapter)
 	up_read(&__i2c_board_lock);
 }
 
-/* OF support code */
-
-#if IS_ENABLED(CONFIG_OF)
-static struct i2c_client *of_i2c_register_device(struct i2c_adapter *adap,
-						 struct device_node *node)
-{
-	struct i2c_client *result;
-	struct i2c_board_info info = {};
-	struct dev_archdata dev_ad = {};
-	const __be32 *addr_be;
-	u32 addr;
-	int len;
-
-	dev_dbg(&adap->dev, "of_i2c: register %s\n", node->full_name);
-
-	if (of_modalias_node(node, info.type, sizeof(info.type)) < 0) {
-		dev_err(&adap->dev, "of_i2c: modalias failure on %s\n",
-			node->full_name);
-		return ERR_PTR(-EINVAL);
-	}
-
-	addr_be = of_get_property(node, "reg", &len);
-	if (!addr_be || (len < sizeof(*addr_be))) {
-		dev_err(&adap->dev, "of_i2c: invalid reg on %s\n",
-			node->full_name);
-		return ERR_PTR(-EINVAL);
-	}
-
-	addr = be32_to_cpup(addr_be);
-	if (addr & I2C_TEN_BIT_ADDRESS) {
-		addr &= ~I2C_TEN_BIT_ADDRESS;
-		info.flags |= I2C_CLIENT_TEN;
-	}
-
-	if (addr & I2C_OWN_SLAVE_ADDRESS) {
-		addr &= ~I2C_OWN_SLAVE_ADDRESS;
-		info.flags |= I2C_CLIENT_SLAVE;
-	}
-
-	if (i2c_check_addr_validity(addr, info.flags)) {
-		dev_err(&adap->dev, "of_i2c: invalid addr=%x on %s\n",
-			addr, node->full_name);
-		return ERR_PTR(-EINVAL);
-	}
-
-	info.addr = addr;
-	info.of_node = of_node_get(node);
-	info.archdata = &dev_ad;
-
-	if (of_property_read_bool(node, "host-notify"))
-		info.flags |= I2C_CLIENT_HOST_NOTIFY;
-
-	if (of_get_property(node, "wakeup-source", NULL))
-		info.flags |= I2C_CLIENT_WAKE;
-
-	result = i2c_new_device(adap, &info);
-	if (result == NULL) {
-		dev_err(&adap->dev, "of_i2c: Failure registering %s\n",
-			node->full_name);
-		of_node_put(node);
-		return ERR_PTR(-EINVAL);
-	}
-	return result;
-}
-
-static void of_i2c_register_devices(struct i2c_adapter *adap)
-{
-	struct device_node *bus, *node;
-	struct i2c_client *client;
-
-	/* Only register child devices if the adapter has a node pointer set */
-	if (!adap->dev.of_node)
-		return;
-
-	dev_dbg(&adap->dev, "of_i2c: walking child nodes\n");
-
-	bus = of_get_child_by_name(adap->dev.of_node, "i2c-bus");
-	if (!bus)
-		bus = of_node_get(adap->dev.of_node);
-
-	for_each_available_child_of_node(bus, node) {
-		if (of_node_test_and_set_flag(node, OF_POPULATED))
-			continue;
-
-		client = of_i2c_register_device(adap, node);
-		if (IS_ERR(client)) {
-			dev_warn(&adap->dev,
-				 "Failed to create I2C device for %s\n",
-				 node->full_name);
-			of_node_clear_flag(node, OF_POPULATED);
-		}
-	}
-
-	of_node_put(bus);
-}
-
-static int of_dev_node_match(struct device *dev, void *data)
-{
-	return dev->of_node == data;
-}
-
-/* must call put_device() when done with returned i2c_client device */
-struct i2c_client *of_find_i2c_device_by_node(struct device_node *node)
-{
-	struct device *dev;
-	struct i2c_client *client;
-
-	dev = bus_find_device(&i2c_bus_type, NULL, node, of_dev_node_match);
-	if (!dev)
-		return NULL;
-
-	client = i2c_verify_client(dev);
-	if (!client)
-		put_device(dev);
-
-	return client;
-}
-EXPORT_SYMBOL(of_find_i2c_device_by_node);
-
-/* must call put_device() when done with returned i2c_adapter device */
-struct i2c_adapter *of_find_i2c_adapter_by_node(struct device_node *node)
-{
-	struct device *dev;
-	struct i2c_adapter *adapter;
-
-	dev = bus_find_device(&i2c_bus_type, NULL, node, of_dev_node_match);
-	if (!dev)
-		return NULL;
-
-	adapter = i2c_verify_adapter(dev);
-	if (!adapter)
-		put_device(dev);
-
-	return adapter;
-}
-EXPORT_SYMBOL(of_find_i2c_adapter_by_node);
-
-/* must call i2c_put_adapter() when done with returned i2c_adapter device */
-struct i2c_adapter *of_get_i2c_adapter_by_node(struct device_node *node)
-{
-	struct i2c_adapter *adapter;
-
-	adapter = of_find_i2c_adapter_by_node(node);
-	if (!adapter)
-		return NULL;
-
-	if (!try_module_get(adapter->owner)) {
-		put_device(&adapter->dev);
-		adapter = NULL;
-	}
-
-	return adapter;
-}
-EXPORT_SYMBOL(of_get_i2c_adapter_by_node);
-
-static const struct of_device_id*
-i2c_of_match_device_sysfs(const struct of_device_id *matches,
-				  struct i2c_client *client)
-{
-	const char *name;
-
-	for (; matches->compatible[0]; matches++) {
-		/*
-		 * Adding devices through the i2c sysfs interface provides us
-		 * a string to match which may be compatible with the device
-		 * tree compatible strings, however with no actual of_node the
-		 * of_match_device() will not match
-		 */
-		if (sysfs_streq(client->name, matches->compatible))
-			return matches;
-
-		name = strchr(matches->compatible, ',');
-		if (!name)
-			name = matches->compatible;
-		else
-			name++;
-
-		if (sysfs_streq(client->name, name))
-			return matches;
-	}
-
-	return NULL;
-}
-
-const struct of_device_id
-*i2c_of_match_device(const struct of_device_id *matches,
-		     struct i2c_client *client)
-{
-	const struct of_device_id *match;
-
-	if (!(client && matches))
-		return NULL;
-
-	match = of_match_device(matches, &client->dev);
-	if (match)
-		return match;
-
-	return i2c_of_match_device_sysfs(matches, client);
-}
-EXPORT_SYMBOL_GPL(i2c_of_match_device);
-#else
-static void of_i2c_register_devices(struct i2c_adapter *adap) { }
-#endif /* CONFIG_OF */
-
 static int i2c_do_add_adapter(struct i2c_driver *driver,
 			      struct i2c_adapter *adap)
 {
@@ -2562,62 +1702,6 @@ void i2c_clients_command(struct i2c_adapter *adap, unsigned int cmd, void *arg)
 }
 EXPORT_SYMBOL(i2c_clients_command);
 
-#if IS_ENABLED(CONFIG_OF_DYNAMIC)
-static int of_i2c_notify(struct notifier_block *nb, unsigned long action,
-			 void *arg)
-{
-	struct of_reconfig_data *rd = arg;
-	struct i2c_adapter *adap;
-	struct i2c_client *client;
-
-	switch (of_reconfig_get_state_change(action, rd)) {
-	case OF_RECONFIG_CHANGE_ADD:
-		adap = of_find_i2c_adapter_by_node(rd->dn->parent);
-		if (adap == NULL)
-			return NOTIFY_OK;	/* not for us */
-
-		if (of_node_test_and_set_flag(rd->dn, OF_POPULATED)) {
-			put_device(&adap->dev);
-			return NOTIFY_OK;
-		}
-
-		client = of_i2c_register_device(adap, rd->dn);
-		put_device(&adap->dev);
-
-		if (IS_ERR(client)) {
-			dev_err(&adap->dev, "failed to create client for '%s'\n",
-				 rd->dn->full_name);
-			of_node_clear_flag(rd->dn, OF_POPULATED);
-			return notifier_from_errno(PTR_ERR(client));
-		}
-		break;
-	case OF_RECONFIG_CHANGE_REMOVE:
-		/* already depopulated? */
-		if (!of_node_check_flag(rd->dn, OF_POPULATED))
-			return NOTIFY_OK;
-
-		/* find our device by node */
-		client = of_find_i2c_device_by_node(rd->dn);
-		if (client == NULL)
-			return NOTIFY_OK;	/* no? not meant for us */
-
-		/* unregister takes one ref away */
-		i2c_unregister_device(client);
-
-		/* and put the reference of the find */
-		put_device(&client->dev);
-		break;
-	}
-
-	return NOTIFY_OK;
-}
-static struct notifier_block i2c_of_notifier = {
-	.notifier_call = of_i2c_notify,
-};
-#else
-extern struct notifier_block i2c_of_notifier;
-#endif /* CONFIG_OF_DYNAMIC */
-
 static int __init i2c_init(void)
 {
 	int retval;
@@ -3156,676 +2240,6 @@ void i2c_put_adapter(struct i2c_adapter *adap)
 }
 EXPORT_SYMBOL(i2c_put_adapter);
 
-/* The SMBus parts */
-
-#define POLY    (0x1070U << 3)
-static u8 crc8(u16 data)
-{
-	int i;
-
-	for (i = 0; i < 8; i++) {
-		if (data & 0x8000)
-			data = data ^ POLY;
-		data = data << 1;
-	}
-	return (u8)(data >> 8);
-}
-
-/* Incremental CRC8 over count bytes in the array pointed to by p */
-static u8 i2c_smbus_pec(u8 crc, u8 *p, size_t count)
-{
-	int i;
-
-	for (i = 0; i < count; i++)
-		crc = crc8((crc ^ p[i]) << 8);
-	return crc;
-}
-
-/* Assume a 7-bit address, which is reasonable for SMBus */
-static u8 i2c_smbus_msg_pec(u8 pec, struct i2c_msg *msg)
-{
-	/* The address will be sent first */
-	u8 addr = i2c_8bit_addr_from_msg(msg);
-	pec = i2c_smbus_pec(pec, &addr, 1);
-
-	/* The data buffer follows */
-	return i2c_smbus_pec(pec, msg->buf, msg->len);
-}
-
-/* Used for write only transactions */
-static inline void i2c_smbus_add_pec(struct i2c_msg *msg)
-{
-	msg->buf[msg->len] = i2c_smbus_msg_pec(0, msg);
-	msg->len++;
-}
-
-/* Return <0 on CRC error
-   If there was a write before this read (most cases) we need to take the
-   partial CRC from the write part into account.
-   Note that this function does modify the message (we need to decrease the
-   message length to hide the CRC byte from the caller). */
-static int i2c_smbus_check_pec(u8 cpec, struct i2c_msg *msg)
-{
-	u8 rpec = msg->buf[--msg->len];
-	cpec = i2c_smbus_msg_pec(cpec, msg);
-
-	if (rpec != cpec) {
-		pr_debug("Bad PEC 0x%02x vs. 0x%02x\n",
-			rpec, cpec);
-		return -EBADMSG;
-	}
-	return 0;
-}
-
-/**
- * i2c_smbus_read_byte - SMBus "receive byte" protocol
- * @client: Handle to slave device
- *
- * This executes the SMBus "receive byte" protocol, returning negative errno
- * else the byte received from the device.
- */
-s32 i2c_smbus_read_byte(const struct i2c_client *client)
-{
-	union i2c_smbus_data data;
-	int status;
-
-	status = i2c_smbus_xfer(client->adapter, client->addr, client->flags,
-				I2C_SMBUS_READ, 0,
-				I2C_SMBUS_BYTE, &data);
-	return (status < 0) ? status : data.byte;
-}
-EXPORT_SYMBOL(i2c_smbus_read_byte);
-
-/**
- * i2c_smbus_write_byte - SMBus "send byte" protocol
- * @client: Handle to slave device
- * @value: Byte to be sent
- *
- * This executes the SMBus "send byte" protocol, returning negative errno
- * else zero on success.
- */
-s32 i2c_smbus_write_byte(const struct i2c_client *client, u8 value)
-{
-	return i2c_smbus_xfer(client->adapter, client->addr, client->flags,
-	                      I2C_SMBUS_WRITE, value, I2C_SMBUS_BYTE, NULL);
-}
-EXPORT_SYMBOL(i2c_smbus_write_byte);
-
-/**
- * i2c_smbus_read_byte_data - SMBus "read byte" protocol
- * @client: Handle to slave device
- * @command: Byte interpreted by slave
- *
- * This executes the SMBus "read byte" protocol, returning negative errno
- * else a data byte received from the device.
- */
-s32 i2c_smbus_read_byte_data(const struct i2c_client *client, u8 command)
-{
-	union i2c_smbus_data data;
-	int status;
-
-	status = i2c_smbus_xfer(client->adapter, client->addr, client->flags,
-				I2C_SMBUS_READ, command,
-				I2C_SMBUS_BYTE_DATA, &data);
-	return (status < 0) ? status : data.byte;
-}
-EXPORT_SYMBOL(i2c_smbus_read_byte_data);
-
-/**
- * i2c_smbus_write_byte_data - SMBus "write byte" protocol
- * @client: Handle to slave device
- * @command: Byte interpreted by slave
- * @value: Byte being written
- *
- * This executes the SMBus "write byte" protocol, returning negative errno
- * else zero on success.
- */
-s32 i2c_smbus_write_byte_data(const struct i2c_client *client, u8 command,
-			      u8 value)
-{
-	union i2c_smbus_data data;
-	data.byte = value;
-	return i2c_smbus_xfer(client->adapter, client->addr, client->flags,
-			      I2C_SMBUS_WRITE, command,
-			      I2C_SMBUS_BYTE_DATA, &data);
-}
-EXPORT_SYMBOL(i2c_smbus_write_byte_data);
-
-/**
- * i2c_smbus_read_word_data - SMBus "read word" protocol
- * @client: Handle to slave device
- * @command: Byte interpreted by slave
- *
- * This executes the SMBus "read word" protocol, returning negative errno
- * else a 16-bit unsigned "word" received from the device.
- */
-s32 i2c_smbus_read_word_data(const struct i2c_client *client, u8 command)
-{
-	union i2c_smbus_data data;
-	int status;
-
-	status = i2c_smbus_xfer(client->adapter, client->addr, client->flags,
-				I2C_SMBUS_READ, command,
-				I2C_SMBUS_WORD_DATA, &data);
-	return (status < 0) ? status : data.word;
-}
-EXPORT_SYMBOL(i2c_smbus_read_word_data);
-
-/**
- * i2c_smbus_write_word_data - SMBus "write word" protocol
- * @client: Handle to slave device
- * @command: Byte interpreted by slave
- * @value: 16-bit "word" being written
- *
- * This executes the SMBus "write word" protocol, returning negative errno
- * else zero on success.
- */
-s32 i2c_smbus_write_word_data(const struct i2c_client *client, u8 command,
-			      u16 value)
-{
-	union i2c_smbus_data data;
-	data.word = value;
-	return i2c_smbus_xfer(client->adapter, client->addr, client->flags,
-			      I2C_SMBUS_WRITE, command,
-			      I2C_SMBUS_WORD_DATA, &data);
-}
-EXPORT_SYMBOL(i2c_smbus_write_word_data);
-
-/**
- * i2c_smbus_read_block_data - SMBus "block read" protocol
- * @client: Handle to slave device
- * @command: Byte interpreted by slave
- * @values: Byte array into which data will be read; big enough to hold
- *	the data returned by the slave.  SMBus allows at most 32 bytes.
- *
- * This executes the SMBus "block read" protocol, returning negative errno
- * else the number of data bytes in the slave's response.
- *
- * Note that using this function requires that the client's adapter support
- * the I2C_FUNC_SMBUS_READ_BLOCK_DATA functionality.  Not all adapter drivers
- * support this; its emulation through I2C messaging relies on a specific
- * mechanism (I2C_M_RECV_LEN) which may not be implemented.
- */
-s32 i2c_smbus_read_block_data(const struct i2c_client *client, u8 command,
-			      u8 *values)
-{
-	union i2c_smbus_data data;
-	int status;
-
-	status = i2c_smbus_xfer(client->adapter, client->addr, client->flags,
-				I2C_SMBUS_READ, command,
-				I2C_SMBUS_BLOCK_DATA, &data);
-	if (status)
-		return status;
-
-	memcpy(values, &data.block[1], data.block[0]);
-	return data.block[0];
-}
-EXPORT_SYMBOL(i2c_smbus_read_block_data);
-
-/**
- * i2c_smbus_write_block_data - SMBus "block write" protocol
- * @client: Handle to slave device
- * @command: Byte interpreted by slave
- * @length: Size of data block; SMBus allows at most 32 bytes
- * @values: Byte array which will be written.
- *
- * This executes the SMBus "block write" protocol, returning negative errno
- * else zero on success.
- */
-s32 i2c_smbus_write_block_data(const struct i2c_client *client, u8 command,
-			       u8 length, const u8 *values)
-{
-	union i2c_smbus_data data;
-
-	if (length > I2C_SMBUS_BLOCK_MAX)
-		length = I2C_SMBUS_BLOCK_MAX;
-	data.block[0] = length;
-	memcpy(&data.block[1], values, length);
-	return i2c_smbus_xfer(client->adapter, client->addr, client->flags,
-			      I2C_SMBUS_WRITE, command,
-			      I2C_SMBUS_BLOCK_DATA, &data);
-}
-EXPORT_SYMBOL(i2c_smbus_write_block_data);
-
-/* Returns the number of read bytes */
-s32 i2c_smbus_read_i2c_block_data(const struct i2c_client *client, u8 command,
-				  u8 length, u8 *values)
-{
-	union i2c_smbus_data data;
-	int status;
-
-	if (length > I2C_SMBUS_BLOCK_MAX)
-		length = I2C_SMBUS_BLOCK_MAX;
-	data.block[0] = length;
-	status = i2c_smbus_xfer(client->adapter, client->addr, client->flags,
-				I2C_SMBUS_READ, command,
-				I2C_SMBUS_I2C_BLOCK_DATA, &data);
-	if (status < 0)
-		return status;
-
-	memcpy(values, &data.block[1], data.block[0]);
-	return data.block[0];
-}
-EXPORT_SYMBOL(i2c_smbus_read_i2c_block_data);
-
-s32 i2c_smbus_write_i2c_block_data(const struct i2c_client *client, u8 command,
-				   u8 length, const u8 *values)
-{
-	union i2c_smbus_data data;
-
-	if (length > I2C_SMBUS_BLOCK_MAX)
-		length = I2C_SMBUS_BLOCK_MAX;
-	data.block[0] = length;
-	memcpy(data.block + 1, values, length);
-	return i2c_smbus_xfer(client->adapter, client->addr, client->flags,
-			      I2C_SMBUS_WRITE, command,
-			      I2C_SMBUS_I2C_BLOCK_DATA, &data);
-}
-EXPORT_SYMBOL(i2c_smbus_write_i2c_block_data);
-
-/* Simulate a SMBus command using the i2c protocol
-   No checking of parameters is done!  */
-static s32 i2c_smbus_xfer_emulated(struct i2c_adapter *adapter, u16 addr,
-				   unsigned short flags,
-				   char read_write, u8 command, int size,
-				   union i2c_smbus_data *data)
-{
-	/* So we need to generate a series of msgs. In the case of writing, we
-	  need to use only one message; when reading, we need two. We initialize
-	  most things with sane defaults, to keep the code below somewhat
-	  simpler. */
-	unsigned char msgbuf0[I2C_SMBUS_BLOCK_MAX+3];
-	unsigned char msgbuf1[I2C_SMBUS_BLOCK_MAX+2];
-	int num = read_write == I2C_SMBUS_READ ? 2 : 1;
-	int i;
-	u8 partial_pec = 0;
-	int status;
-	struct i2c_msg msg[2] = {
-		{
-			.addr = addr,
-			.flags = flags,
-			.len = 1,
-			.buf = msgbuf0,
-		}, {
-			.addr = addr,
-			.flags = flags | I2C_M_RD,
-			.len = 0,
-			.buf = msgbuf1,
-		},
-	};
-
-	msgbuf0[0] = command;
-	switch (size) {
-	case I2C_SMBUS_QUICK:
-		msg[0].len = 0;
-		/* Special case: The read/write field is used as data */
-		msg[0].flags = flags | (read_write == I2C_SMBUS_READ ?
-					I2C_M_RD : 0);
-		num = 1;
-		break;
-	case I2C_SMBUS_BYTE:
-		if (read_write == I2C_SMBUS_READ) {
-			/* Special case: only a read! */
-			msg[0].flags = I2C_M_RD | flags;
-			num = 1;
-		}
-		break;
-	case I2C_SMBUS_BYTE_DATA:
-		if (read_write == I2C_SMBUS_READ)
-			msg[1].len = 1;
-		else {
-			msg[0].len = 2;
-			msgbuf0[1] = data->byte;
-		}
-		break;
-	case I2C_SMBUS_WORD_DATA:
-		if (read_write == I2C_SMBUS_READ)
-			msg[1].len = 2;
-		else {
-			msg[0].len = 3;
-			msgbuf0[1] = data->word & 0xff;
-			msgbuf0[2] = data->word >> 8;
-		}
-		break;
-	case I2C_SMBUS_PROC_CALL:
-		num = 2; /* Special case */
-		read_write = I2C_SMBUS_READ;
-		msg[0].len = 3;
-		msg[1].len = 2;
-		msgbuf0[1] = data->word & 0xff;
-		msgbuf0[2] = data->word >> 8;
-		break;
-	case I2C_SMBUS_BLOCK_DATA:
-		if (read_write == I2C_SMBUS_READ) {
-			msg[1].flags |= I2C_M_RECV_LEN;
-			msg[1].len = 1; /* block length will be added by
-					   the underlying bus driver */
-		} else {
-			msg[0].len = data->block[0] + 2;
-			if (msg[0].len > I2C_SMBUS_BLOCK_MAX + 2) {
-				dev_err(&adapter->dev,
-					"Invalid block write size %d\n",
-					data->block[0]);
-				return -EINVAL;
-			}
-			for (i = 1; i < msg[0].len; i++)
-				msgbuf0[i] = data->block[i-1];
-		}
-		break;
-	case I2C_SMBUS_BLOCK_PROC_CALL:
-		num = 2; /* Another special case */
-		read_write = I2C_SMBUS_READ;
-		if (data->block[0] > I2C_SMBUS_BLOCK_MAX) {
-			dev_err(&adapter->dev,
-				"Invalid block write size %d\n",
-				data->block[0]);
-			return -EINVAL;
-		}
-		msg[0].len = data->block[0] + 2;
-		for (i = 1; i < msg[0].len; i++)
-			msgbuf0[i] = data->block[i-1];
-		msg[1].flags |= I2C_M_RECV_LEN;
-		msg[1].len = 1; /* block length will be added by
-				   the underlying bus driver */
-		break;
-	case I2C_SMBUS_I2C_BLOCK_DATA:
-		if (read_write == I2C_SMBUS_READ) {
-			msg[1].len = data->block[0];
-		} else {
-			msg[0].len = data->block[0] + 1;
-			if (msg[0].len > I2C_SMBUS_BLOCK_MAX + 1) {
-				dev_err(&adapter->dev,
-					"Invalid block write size %d\n",
-					data->block[0]);
-				return -EINVAL;
-			}
-			for (i = 1; i <= data->block[0]; i++)
-				msgbuf0[i] = data->block[i];
-		}
-		break;
-	default:
-		dev_err(&adapter->dev, "Unsupported transaction %d\n", size);
-		return -EOPNOTSUPP;
-	}
-
-	i = ((flags & I2C_CLIENT_PEC) && size != I2C_SMBUS_QUICK
-				      && size != I2C_SMBUS_I2C_BLOCK_DATA);
-	if (i) {
-		/* Compute PEC if first message is a write */
-		if (!(msg[0].flags & I2C_M_RD)) {
-			if (num == 1) /* Write only */
-				i2c_smbus_add_pec(&msg[0]);
-			else /* Write followed by read */
-				partial_pec = i2c_smbus_msg_pec(0, &msg[0]);
-		}
-		/* Ask for PEC if last message is a read */
-		if (msg[num-1].flags & I2C_M_RD)
-			msg[num-1].len++;
-	}
-
-	status = i2c_transfer(adapter, msg, num);
-	if (status < 0)
-		return status;
-
-	/* Check PEC if last message is a read */
-	if (i && (msg[num-1].flags & I2C_M_RD)) {
-		status = i2c_smbus_check_pec(partial_pec, &msg[num-1]);
-		if (status < 0)
-			return status;
-	}
-
-	if (read_write == I2C_SMBUS_READ)
-		switch (size) {
-		case I2C_SMBUS_BYTE:
-			data->byte = msgbuf0[0];
-			break;
-		case I2C_SMBUS_BYTE_DATA:
-			data->byte = msgbuf1[0];
-			break;
-		case I2C_SMBUS_WORD_DATA:
-		case I2C_SMBUS_PROC_CALL:
-			data->word = msgbuf1[0] | (msgbuf1[1] << 8);
-			break;
-		case I2C_SMBUS_I2C_BLOCK_DATA:
-			for (i = 0; i < data->block[0]; i++)
-				data->block[i+1] = msgbuf1[i];
-			break;
-		case I2C_SMBUS_BLOCK_DATA:
-		case I2C_SMBUS_BLOCK_PROC_CALL:
-			for (i = 0; i < msgbuf1[0] + 1; i++)
-				data->block[i] = msgbuf1[i];
-			break;
-		}
-	return 0;
-}
-
-/**
- * i2c_smbus_xfer - execute SMBus protocol operations
- * @adapter: Handle to I2C bus
- * @addr: Address of SMBus slave on that bus
- * @flags: I2C_CLIENT_* flags (usually zero or I2C_CLIENT_PEC)
- * @read_write: I2C_SMBUS_READ or I2C_SMBUS_WRITE
- * @command: Byte interpreted by slave, for protocols which use such bytes
- * @protocol: SMBus protocol operation to execute, such as I2C_SMBUS_PROC_CALL
- * @data: Data to be read or written
- *
- * This executes an SMBus protocol operation, and returns a negative
- * errno code else zero on success.
- */
-s32 i2c_smbus_xfer(struct i2c_adapter *adapter, u16 addr, unsigned short flags,
-		   char read_write, u8 command, int protocol,
-		   union i2c_smbus_data *data)
-{
-	unsigned long orig_jiffies;
-	int try;
-	s32 res;
-
-	/* If enabled, the following two tracepoints are conditional on
-	 * read_write and protocol.
-	 */
-	trace_smbus_write(adapter, addr, flags, read_write,
-			  command, protocol, data);
-	trace_smbus_read(adapter, addr, flags, read_write,
-			 command, protocol);
-
-	flags &= I2C_M_TEN | I2C_CLIENT_PEC | I2C_CLIENT_SCCB;
-
-	if (adapter->algo->smbus_xfer) {
-		i2c_lock_bus(adapter, I2C_LOCK_SEGMENT);
-
-		/* Retry automatically on arbitration loss */
-		orig_jiffies = jiffies;
-		for (res = 0, try = 0; try <= adapter->retries; try++) {
-			res = adapter->algo->smbus_xfer(adapter, addr, flags,
-							read_write, command,
-							protocol, data);
-			if (res != -EAGAIN)
-				break;
-			if (time_after(jiffies,
-				       orig_jiffies + adapter->timeout))
-				break;
-		}
-		i2c_unlock_bus(adapter, I2C_LOCK_SEGMENT);
-
-		if (res != -EOPNOTSUPP || !adapter->algo->master_xfer)
-			goto trace;
-		/*
-		 * Fall back to i2c_smbus_xfer_emulated if the adapter doesn't
-		 * implement native support for the SMBus operation.
-		 */
-	}
-
-	res = i2c_smbus_xfer_emulated(adapter, addr, flags, read_write,
-				      command, protocol, data);
-
-trace:
-	/* If enabled, the reply tracepoint is conditional on read_write. */
-	trace_smbus_reply(adapter, addr, flags, read_write,
-			  command, protocol, data);
-	trace_smbus_result(adapter, addr, flags, read_write,
-			   command, protocol, res);
-
-	return res;
-}
-EXPORT_SYMBOL(i2c_smbus_xfer);
-
-/**
- * i2c_smbus_read_i2c_block_data_or_emulated - read block or emulate
- * @client: Handle to slave device
- * @command: Byte interpreted by slave
- * @length: Size of data block; SMBus allows at most I2C_SMBUS_BLOCK_MAX bytes
- * @values: Byte array into which data will be read; big enough to hold
- *	the data returned by the slave.  SMBus allows at most
- *	I2C_SMBUS_BLOCK_MAX bytes.
- *
- * This executes the SMBus "block read" protocol if supported by the adapter.
- * If block read is not supported, it emulates it using either word or byte
- * read protocols depending on availability.
- *
- * The addresses of the I2C slave device that are accessed with this function
- * must be mapped to a linear region, so that a block read will have the same
- * effect as a byte read. Before using this function you must double-check
- * if the I2C slave does support exchanging a block transfer with a byte
- * transfer.
- */
-s32 i2c_smbus_read_i2c_block_data_or_emulated(const struct i2c_client *client,
-					      u8 command, u8 length, u8 *values)
-{
-	u8 i = 0;
-	int status;
-
-	if (length > I2C_SMBUS_BLOCK_MAX)
-		length = I2C_SMBUS_BLOCK_MAX;
-
-	if (i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_READ_I2C_BLOCK))
-		return i2c_smbus_read_i2c_block_data(client, command, length, values);
-
-	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_READ_BYTE_DATA))
-		return -EOPNOTSUPP;
-
-	if (i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_READ_WORD_DATA)) {
-		while ((i + 2) <= length) {
-			status = i2c_smbus_read_word_data(client, command + i);
-			if (status < 0)
-				return status;
-			values[i] = status & 0xff;
-			values[i + 1] = status >> 8;
-			i += 2;
-		}
-	}
-
-	while (i < length) {
-		status = i2c_smbus_read_byte_data(client, command + i);
-		if (status < 0)
-			return status;
-		values[i] = status;
-		i++;
-	}
-
-	return i;
-}
-EXPORT_SYMBOL(i2c_smbus_read_i2c_block_data_or_emulated);
-
-#if IS_ENABLED(CONFIG_I2C_SLAVE)
-int i2c_slave_register(struct i2c_client *client, i2c_slave_cb_t slave_cb)
-{
-	int ret;
-
-	if (!client || !slave_cb) {
-		WARN(1, "insufficient data\n");
-		return -EINVAL;
-	}
-
-	if (!(client->flags & I2C_CLIENT_SLAVE))
-		dev_warn(&client->dev, "%s: client slave flag not set. You might see address collisions\n",
-			 __func__);
-
-	if (!(client->flags & I2C_CLIENT_TEN)) {
-		/* Enforce stricter address checking */
-		ret = i2c_check_7bit_addr_validity_strict(client->addr);
-		if (ret) {
-			dev_err(&client->dev, "%s: invalid address\n", __func__);
-			return ret;
-		}
-	}
-
-	if (!client->adapter->algo->reg_slave) {
-		dev_err(&client->dev, "%s: not supported by adapter\n", __func__);
-		return -EOPNOTSUPP;
-	}
-
-	client->slave_cb = slave_cb;
-
-	i2c_lock_adapter(client->adapter);
-	ret = client->adapter->algo->reg_slave(client);
-	i2c_unlock_adapter(client->adapter);
-
-	if (ret) {
-		client->slave_cb = NULL;
-		dev_err(&client->dev, "%s: adapter returned error %d\n", __func__, ret);
-	}
-
-	return ret;
-}
-EXPORT_SYMBOL_GPL(i2c_slave_register);
-
-int i2c_slave_unregister(struct i2c_client *client)
-{
-	int ret;
-
-	if (!client->adapter->algo->unreg_slave) {
-		dev_err(&client->dev, "%s: not supported by adapter\n", __func__);
-		return -EOPNOTSUPP;
-	}
-
-	i2c_lock_adapter(client->adapter);
-	ret = client->adapter->algo->unreg_slave(client);
-	i2c_unlock_adapter(client->adapter);
-
-	if (ret == 0)
-		client->slave_cb = NULL;
-	else
-		dev_err(&client->dev, "%s: adapter returned error %d\n", __func__, ret);
-
-	return ret;
-}
-EXPORT_SYMBOL_GPL(i2c_slave_unregister);
-
-/**
- * i2c_detect_slave_mode - detect operation mode
- * @dev: The device owning the bus
- *
- * This checks the device nodes for an I2C slave by checking the address
- * used in the reg property. If the address match the I2C_OWN_SLAVE_ADDRESS
- * flag this means the device is configured to act as a I2C slave and it will
- * be listening at that address.
- *
- * Returns true if an I2C own slave address is detected, otherwise returns
- * false.
- */
-bool i2c_detect_slave_mode(struct device *dev)
-{
-	if (IS_BUILTIN(CONFIG_OF) && dev->of_node) {
-		struct device_node *child;
-		u32 reg;
-
-		for_each_child_of_node(dev->of_node, child) {
-			of_property_read_u32(child, "reg", &reg);
-			if (reg & I2C_OWN_SLAVE_ADDRESS) {
-				of_node_put(child);
-				return true;
-			}
-		}
-	} else if (IS_BUILTIN(CONFIG_ACPI) && ACPI_HANDLE(dev)) {
-		dev_dbg(dev, "ACPI slave is not supported yet\n");
-	}
-	return false;
-}
-EXPORT_SYMBOL_GPL(i2c_detect_slave_mode);
-
-#endif
-
 MODULE_AUTHOR("Simon G. Vogl <simon@tk.uni-linz.ac.at>");
 MODULE_DESCRIPTION("I2C-Bus main module");
 MODULE_LICENSE("GPL");
diff --git a/drivers/i2c/i2c-core-of.c b/drivers/i2c/i2c-core-of.c
new file mode 100644
index 000000000000..ccf82fdbcd8e
--- /dev/null
+++ b/drivers/i2c/i2c-core-of.c
@@ -0,0 +1,276 @@
+/*
+ * Linux I2C core OF support code
+ *
+ * Copyright (C) 2008 Jochen Friedrich <jochen@scram.de>
+ * based on a previous patch from Jon Smirl <jonsmirl@gmail.com>
+ *
+ * Copyright (C) 2013 Wolfram Sang <wsa@the-dreams.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 <dt-bindings/i2c/i2c.h>
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+
+#include "i2c-core.h"
+
+static struct i2c_client *of_i2c_register_device(struct i2c_adapter *adap,
+						 struct device_node *node)
+{
+	struct i2c_client *result;
+	struct i2c_board_info info = {};
+	struct dev_archdata dev_ad = {};
+	const __be32 *addr_be;
+	u32 addr;
+	int len;
+
+	dev_dbg(&adap->dev, "of_i2c: register %s\n", node->full_name);
+
+	if (of_modalias_node(node, info.type, sizeof(info.type)) < 0) {
+		dev_err(&adap->dev, "of_i2c: modalias failure on %s\n",
+			node->full_name);
+		return ERR_PTR(-EINVAL);
+	}
+
+	addr_be = of_get_property(node, "reg", &len);
+	if (!addr_be || (len < sizeof(*addr_be))) {
+		dev_err(&adap->dev, "of_i2c: invalid reg on %s\n",
+			node->full_name);
+		return ERR_PTR(-EINVAL);
+	}
+
+	addr = be32_to_cpup(addr_be);
+	if (addr & I2C_TEN_BIT_ADDRESS) {
+		addr &= ~I2C_TEN_BIT_ADDRESS;
+		info.flags |= I2C_CLIENT_TEN;
+	}
+
+	if (addr & I2C_OWN_SLAVE_ADDRESS) {
+		addr &= ~I2C_OWN_SLAVE_ADDRESS;
+		info.flags |= I2C_CLIENT_SLAVE;
+	}
+
+	if (i2c_check_addr_validity(addr, info.flags)) {
+		dev_err(&adap->dev, "of_i2c: invalid addr=%x on %s\n",
+			addr, node->full_name);
+		return ERR_PTR(-EINVAL);
+	}
+
+	info.addr = addr;
+	info.of_node = of_node_get(node);
+	info.archdata = &dev_ad;
+
+	if (of_property_read_bool(node, "host-notify"))
+		info.flags |= I2C_CLIENT_HOST_NOTIFY;
+
+	if (of_get_property(node, "wakeup-source", NULL))
+		info.flags |= I2C_CLIENT_WAKE;
+
+	result = i2c_new_device(adap, &info);
+	if (result == NULL) {
+		dev_err(&adap->dev, "of_i2c: Failure registering %s\n",
+			node->full_name);
+		of_node_put(node);
+		return ERR_PTR(-EINVAL);
+	}
+	return result;
+}
+
+void of_i2c_register_devices(struct i2c_adapter *adap)
+{
+	struct device_node *bus, *node;
+	struct i2c_client *client;
+
+	/* Only register child devices if the adapter has a node pointer set */
+	if (!adap->dev.of_node)
+		return;
+
+	dev_dbg(&adap->dev, "of_i2c: walking child nodes\n");
+
+	bus = of_get_child_by_name(adap->dev.of_node, "i2c-bus");
+	if (!bus)
+		bus = of_node_get(adap->dev.of_node);
+
+	for_each_available_child_of_node(bus, node) {
+		if (of_node_test_and_set_flag(node, OF_POPULATED))
+			continue;
+
+		client = of_i2c_register_device(adap, node);
+		if (IS_ERR(client)) {
+			dev_warn(&adap->dev,
+				 "Failed to create I2C device for %s\n",
+				 node->full_name);
+			of_node_clear_flag(node, OF_POPULATED);
+		}
+	}
+
+	of_node_put(bus);
+}
+
+static int of_dev_node_match(struct device *dev, void *data)
+{
+	return dev->of_node == data;
+}
+
+/* must call put_device() when done with returned i2c_client device */
+struct i2c_client *of_find_i2c_device_by_node(struct device_node *node)
+{
+	struct device *dev;
+	struct i2c_client *client;
+
+	dev = bus_find_device(&i2c_bus_type, NULL, node, of_dev_node_match);
+	if (!dev)
+		return NULL;
+
+	client = i2c_verify_client(dev);
+	if (!client)
+		put_device(dev);
+
+	return client;
+}
+EXPORT_SYMBOL(of_find_i2c_device_by_node);
+
+/* must call put_device() when done with returned i2c_adapter device */
+struct i2c_adapter *of_find_i2c_adapter_by_node(struct device_node *node)
+{
+	struct device *dev;
+	struct i2c_adapter *adapter;
+
+	dev = bus_find_device(&i2c_bus_type, NULL, node, of_dev_node_match);
+	if (!dev)
+		return NULL;
+
+	adapter = i2c_verify_adapter(dev);
+	if (!adapter)
+		put_device(dev);
+
+	return adapter;
+}
+EXPORT_SYMBOL(of_find_i2c_adapter_by_node);
+
+/* must call i2c_put_adapter() when done with returned i2c_adapter device */
+struct i2c_adapter *of_get_i2c_adapter_by_node(struct device_node *node)
+{
+	struct i2c_adapter *adapter;
+
+	adapter = of_find_i2c_adapter_by_node(node);
+	if (!adapter)
+		return NULL;
+
+	if (!try_module_get(adapter->owner)) {
+		put_device(&adapter->dev);
+		adapter = NULL;
+	}
+
+	return adapter;
+}
+EXPORT_SYMBOL(of_get_i2c_adapter_by_node);
+
+static const struct of_device_id*
+i2c_of_match_device_sysfs(const struct of_device_id *matches,
+				  struct i2c_client *client)
+{
+	const char *name;
+
+	for (; matches->compatible[0]; matches++) {
+		/*
+		 * Adding devices through the i2c sysfs interface provides us
+		 * a string to match which may be compatible with the device
+		 * tree compatible strings, however with no actual of_node the
+		 * of_match_device() will not match
+		 */
+		if (sysfs_streq(client->name, matches->compatible))
+			return matches;
+
+		name = strchr(matches->compatible, ',');
+		if (!name)
+			name = matches->compatible;
+		else
+			name++;
+
+		if (sysfs_streq(client->name, name))
+			return matches;
+	}
+
+	return NULL;
+}
+
+const struct of_device_id
+*i2c_of_match_device(const struct of_device_id *matches,
+		     struct i2c_client *client)
+{
+	const struct of_device_id *match;
+
+	if (!(client && matches))
+		return NULL;
+
+	match = of_match_device(matches, &client->dev);
+	if (match)
+		return match;
+
+	return i2c_of_match_device_sysfs(matches, client);
+}
+EXPORT_SYMBOL_GPL(i2c_of_match_device);
+
+#if IS_ENABLED(CONFIG_OF_DYNAMIC)
+static int of_i2c_notify(struct notifier_block *nb, unsigned long action,
+			 void *arg)
+{
+	struct of_reconfig_data *rd = arg;
+	struct i2c_adapter *adap;
+	struct i2c_client *client;
+
+	switch (of_reconfig_get_state_change(action, rd)) {
+	case OF_RECONFIG_CHANGE_ADD:
+		adap = of_find_i2c_adapter_by_node(rd->dn->parent);
+		if (adap == NULL)
+			return NOTIFY_OK;	/* not for us */
+
+		if (of_node_test_and_set_flag(rd->dn, OF_POPULATED)) {
+			put_device(&adap->dev);
+			return NOTIFY_OK;
+		}
+
+		client = of_i2c_register_device(adap, rd->dn);
+		put_device(&adap->dev);
+
+		if (IS_ERR(client)) {
+			dev_err(&adap->dev, "failed to create client for '%s'\n",
+				 rd->dn->full_name);
+			of_node_clear_flag(rd->dn, OF_POPULATED);
+			return notifier_from_errno(PTR_ERR(client));
+		}
+		break;
+	case OF_RECONFIG_CHANGE_REMOVE:
+		/* already depopulated? */
+		if (!of_node_check_flag(rd->dn, OF_POPULATED))
+			return NOTIFY_OK;
+
+		/* find our device by node */
+		client = of_find_i2c_device_by_node(rd->dn);
+		if (client == NULL)
+			return NOTIFY_OK;	/* no? not meant for us */
+
+		/* unregister takes one ref away */
+		i2c_unregister_device(client);
+
+		/* and put the reference of the find */
+		put_device(&client->dev);
+		break;
+	}
+
+	return NOTIFY_OK;
+}
+
+struct notifier_block i2c_of_notifier = {
+	.notifier_call = of_i2c_notify,
+};
+#endif /* CONFIG_OF_DYNAMIC */
diff --git a/drivers/i2c/i2c-core-slave.c b/drivers/i2c/i2c-core-slave.c
new file mode 100644
index 000000000000..4a78c65e9971
--- /dev/null
+++ b/drivers/i2c/i2c-core-slave.c
@@ -0,0 +1,115 @@
+/*
+ * Linux I2C core slave support code
+ *
+ * Copyright (C) 2014 by Wolfram Sang <wsa@sang-engineering.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 <dt-bindings/i2c/i2c.h>
+#include <linux/acpi.h>
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/i2c.h>
+#include <linux/of.h>
+
+#include "i2c-core.h"
+
+int i2c_slave_register(struct i2c_client *client, i2c_slave_cb_t slave_cb)
+{
+	int ret;
+
+	if (!client || !slave_cb) {
+		WARN(1, "insufficient data\n");
+		return -EINVAL;
+	}
+
+	if (!(client->flags & I2C_CLIENT_SLAVE))
+		dev_warn(&client->dev, "%s: client slave flag not set. You might see address collisions\n",
+			 __func__);
+
+	if (!(client->flags & I2C_CLIENT_TEN)) {
+		/* Enforce stricter address checking */
+		ret = i2c_check_7bit_addr_validity_strict(client->addr);
+		if (ret) {
+			dev_err(&client->dev, "%s: invalid address\n", __func__);
+			return ret;
+		}
+	}
+
+	if (!client->adapter->algo->reg_slave) {
+		dev_err(&client->dev, "%s: not supported by adapter\n", __func__);
+		return -EOPNOTSUPP;
+	}
+
+	client->slave_cb = slave_cb;
+
+	i2c_lock_adapter(client->adapter);
+	ret = client->adapter->algo->reg_slave(client);
+	i2c_unlock_adapter(client->adapter);
+
+	if (ret) {
+		client->slave_cb = NULL;
+		dev_err(&client->dev, "%s: adapter returned error %d\n", __func__, ret);
+	}
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(i2c_slave_register);
+
+int i2c_slave_unregister(struct i2c_client *client)
+{
+	int ret;
+
+	if (!client->adapter->algo->unreg_slave) {
+		dev_err(&client->dev, "%s: not supported by adapter\n", __func__);
+		return -EOPNOTSUPP;
+	}
+
+	i2c_lock_adapter(client->adapter);
+	ret = client->adapter->algo->unreg_slave(client);
+	i2c_unlock_adapter(client->adapter);
+
+	if (ret == 0)
+		client->slave_cb = NULL;
+	else
+		dev_err(&client->dev, "%s: adapter returned error %d\n", __func__, ret);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(i2c_slave_unregister);
+
+/**
+ * i2c_detect_slave_mode - detect operation mode
+ * @dev: The device owning the bus
+ *
+ * This checks the device nodes for an I2C slave by checking the address
+ * used in the reg property. If the address match the I2C_OWN_SLAVE_ADDRESS
+ * flag this means the device is configured to act as a I2C slave and it will
+ * be listening at that address.
+ *
+ * Returns true if an I2C own slave address is detected, otherwise returns
+ * false.
+ */
+bool i2c_detect_slave_mode(struct device *dev)
+{
+	if (IS_BUILTIN(CONFIG_OF) && dev->of_node) {
+		struct device_node *child;
+		u32 reg;
+
+		for_each_child_of_node(dev->of_node, child) {
+			of_property_read_u32(child, "reg", &reg);
+			if (reg & I2C_OWN_SLAVE_ADDRESS) {
+				of_node_put(child);
+				return true;
+			}
+		}
+	} else if (IS_BUILTIN(CONFIG_ACPI) && ACPI_HANDLE(dev)) {
+		dev_dbg(dev, "ACPI slave is not supported yet\n");
+	}
+	return false;
+}
+EXPORT_SYMBOL_GPL(i2c_detect_slave_mode);
diff --git a/drivers/i2c/i2c-core-smbus.c b/drivers/i2c/i2c-core-smbus.c
new file mode 100644
index 000000000000..10f00a82ec9d
--- /dev/null
+++ b/drivers/i2c/i2c-core-smbus.c
@@ -0,0 +1,594 @@
+/*
+ * Linux I2C core SMBus and SMBus emulation code
+ *
+ * This file contains the SMBus functions which are always included in the I2C
+ * core because they can be emulated via I2C. SMBus specific extensions
+ * (e.g. smbalert) are handled in a seperate i2c-smbus module.
+ *
+ * All SMBus-related things are written by Frodo Looijaard <frodol@dds.nl>
+ * SMBus 2.0 support by Mark Studebaker <mdsxyz123@yahoo.com> and
+ * Jean Delvare <jdelvare@suse.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/device.h>
+#include <linux/err.h>
+#include <linux/i2c.h>
+
+#define CREATE_TRACE_POINTS
+#include <trace/events/smbus.h>
+
+
+/* The SMBus parts */
+
+#define POLY    (0x1070U << 3)
+static u8 crc8(u16 data)
+{
+	int i;
+
+	for (i = 0; i < 8; i++) {
+		if (data & 0x8000)
+			data = data ^ POLY;
+		data = data << 1;
+	}
+	return (u8)(data >> 8);
+}
+
+/* Incremental CRC8 over count bytes in the array pointed to by p */
+static u8 i2c_smbus_pec(u8 crc, u8 *p, size_t count)
+{
+	int i;
+
+	for (i = 0; i < count; i++)
+		crc = crc8((crc ^ p[i]) << 8);
+	return crc;
+}
+
+/* Assume a 7-bit address, which is reasonable for SMBus */
+static u8 i2c_smbus_msg_pec(u8 pec, struct i2c_msg *msg)
+{
+	/* The address will be sent first */
+	u8 addr = i2c_8bit_addr_from_msg(msg);
+	pec = i2c_smbus_pec(pec, &addr, 1);
+
+	/* The data buffer follows */
+	return i2c_smbus_pec(pec, msg->buf, msg->len);
+}
+
+/* Used for write only transactions */
+static inline void i2c_smbus_add_pec(struct i2c_msg *msg)
+{
+	msg->buf[msg->len] = i2c_smbus_msg_pec(0, msg);
+	msg->len++;
+}
+
+/* Return <0 on CRC error
+   If there was a write before this read (most cases) we need to take the
+   partial CRC from the write part into account.
+   Note that this function does modify the message (we need to decrease the
+   message length to hide the CRC byte from the caller). */
+static int i2c_smbus_check_pec(u8 cpec, struct i2c_msg *msg)
+{
+	u8 rpec = msg->buf[--msg->len];
+	cpec = i2c_smbus_msg_pec(cpec, msg);
+
+	if (rpec != cpec) {
+		pr_debug("Bad PEC 0x%02x vs. 0x%02x\n",
+			rpec, cpec);
+		return -EBADMSG;
+	}
+	return 0;
+}
+
+/**
+ * i2c_smbus_read_byte - SMBus "receive byte" protocol
+ * @client: Handle to slave device
+ *
+ * This executes the SMBus "receive byte" protocol, returning negative errno
+ * else the byte received from the device.
+ */
+s32 i2c_smbus_read_byte(const struct i2c_client *client)
+{
+	union i2c_smbus_data data;
+	int status;
+
+	status = i2c_smbus_xfer(client->adapter, client->addr, client->flags,
+				I2C_SMBUS_READ, 0,
+				I2C_SMBUS_BYTE, &data);
+	return (status < 0) ? status : data.byte;
+}
+EXPORT_SYMBOL(i2c_smbus_read_byte);
+
+/**
+ * i2c_smbus_write_byte - SMBus "send byte" protocol
+ * @client: Handle to slave device
+ * @value: Byte to be sent
+ *
+ * This executes the SMBus "send byte" protocol, returning negative errno
+ * else zero on success.
+ */
+s32 i2c_smbus_write_byte(const struct i2c_client *client, u8 value)
+{
+	return i2c_smbus_xfer(client->adapter, client->addr, client->flags,
+	                      I2C_SMBUS_WRITE, value, I2C_SMBUS_BYTE, NULL);
+}
+EXPORT_SYMBOL(i2c_smbus_write_byte);
+
+/**
+ * i2c_smbus_read_byte_data - SMBus "read byte" protocol
+ * @client: Handle to slave device
+ * @command: Byte interpreted by slave
+ *
+ * This executes the SMBus "read byte" protocol, returning negative errno
+ * else a data byte received from the device.
+ */
+s32 i2c_smbus_read_byte_data(const struct i2c_client *client, u8 command)
+{
+	union i2c_smbus_data data;
+	int status;
+
+	status = i2c_smbus_xfer(client->adapter, client->addr, client->flags,
+				I2C_SMBUS_READ, command,
+				I2C_SMBUS_BYTE_DATA, &data);
+	return (status < 0) ? status : data.byte;
+}
+EXPORT_SYMBOL(i2c_smbus_read_byte_data);
+
+/**
+ * i2c_smbus_write_byte_data - SMBus "write byte" protocol
+ * @client: Handle to slave device
+ * @command: Byte interpreted by slave
+ * @value: Byte being written
+ *
+ * This executes the SMBus "write byte" protocol, returning negative errno
+ * else zero on success.
+ */
+s32 i2c_smbus_write_byte_data(const struct i2c_client *client, u8 command,
+			      u8 value)
+{
+	union i2c_smbus_data data;
+	data.byte = value;
+	return i2c_smbus_xfer(client->adapter, client->addr, client->flags,
+			      I2C_SMBUS_WRITE, command,
+			      I2C_SMBUS_BYTE_DATA, &data);
+}
+EXPORT_SYMBOL(i2c_smbus_write_byte_data);
+
+/**
+ * i2c_smbus_read_word_data - SMBus "read word" protocol
+ * @client: Handle to slave device
+ * @command: Byte interpreted by slave
+ *
+ * This executes the SMBus "read word" protocol, returning negative errno
+ * else a 16-bit unsigned "word" received from the device.
+ */
+s32 i2c_smbus_read_word_data(const struct i2c_client *client, u8 command)
+{
+	union i2c_smbus_data data;
+	int status;
+
+	status = i2c_smbus_xfer(client->adapter, client->addr, client->flags,
+				I2C_SMBUS_READ, command,
+				I2C_SMBUS_WORD_DATA, &data);
+	return (status < 0) ? status : data.word;
+}
+EXPORT_SYMBOL(i2c_smbus_read_word_data);
+
+/**
+ * i2c_smbus_write_word_data - SMBus "write word" protocol
+ * @client: Handle to slave device
+ * @command: Byte interpreted by slave
+ * @value: 16-bit "word" being written
+ *
+ * This executes the SMBus "write word" protocol, returning negative errno
+ * else zero on success.
+ */
+s32 i2c_smbus_write_word_data(const struct i2c_client *client, u8 command,
+			      u16 value)
+{
+	union i2c_smbus_data data;
+	data.word = value;
+	return i2c_smbus_xfer(client->adapter, client->addr, client->flags,
+			      I2C_SMBUS_WRITE, command,
+			      I2C_SMBUS_WORD_DATA, &data);
+}
+EXPORT_SYMBOL(i2c_smbus_write_word_data);
+
+/**
+ * i2c_smbus_read_block_data - SMBus "block read" protocol
+ * @client: Handle to slave device
+ * @command: Byte interpreted by slave
+ * @values: Byte array into which data will be read; big enough to hold
+ *	the data returned by the slave.  SMBus allows at most 32 bytes.
+ *
+ * This executes the SMBus "block read" protocol, returning negative errno
+ * else the number of data bytes in the slave's response.
+ *
+ * Note that using this function requires that the client's adapter support
+ * the I2C_FUNC_SMBUS_READ_BLOCK_DATA functionality.  Not all adapter drivers
+ * support this; its emulation through I2C messaging relies on a specific
+ * mechanism (I2C_M_RECV_LEN) which may not be implemented.
+ */
+s32 i2c_smbus_read_block_data(const struct i2c_client *client, u8 command,
+			      u8 *values)
+{
+	union i2c_smbus_data data;
+	int status;
+
+	status = i2c_smbus_xfer(client->adapter, client->addr, client->flags,
+				I2C_SMBUS_READ, command,
+				I2C_SMBUS_BLOCK_DATA, &data);
+	if (status)
+		return status;
+
+	memcpy(values, &data.block[1], data.block[0]);
+	return data.block[0];
+}
+EXPORT_SYMBOL(i2c_smbus_read_block_data);
+
+/**
+ * i2c_smbus_write_block_data - SMBus "block write" protocol
+ * @client: Handle to slave device
+ * @command: Byte interpreted by slave
+ * @length: Size of data block; SMBus allows at most 32 bytes
+ * @values: Byte array which will be written.
+ *
+ * This executes the SMBus "block write" protocol, returning negative errno
+ * else zero on success.
+ */
+s32 i2c_smbus_write_block_data(const struct i2c_client *client, u8 command,
+			       u8 length, const u8 *values)
+{
+	union i2c_smbus_data data;
+
+	if (length > I2C_SMBUS_BLOCK_MAX)
+		length = I2C_SMBUS_BLOCK_MAX;
+	data.block[0] = length;
+	memcpy(&data.block[1], values, length);
+	return i2c_smbus_xfer(client->adapter, client->addr, client->flags,
+			      I2C_SMBUS_WRITE, command,
+			      I2C_SMBUS_BLOCK_DATA, &data);
+}
+EXPORT_SYMBOL(i2c_smbus_write_block_data);
+
+/* Returns the number of read bytes */
+s32 i2c_smbus_read_i2c_block_data(const struct i2c_client *client, u8 command,
+				  u8 length, u8 *values)
+{
+	union i2c_smbus_data data;
+	int status;
+
+	if (length > I2C_SMBUS_BLOCK_MAX)
+		length = I2C_SMBUS_BLOCK_MAX;
+	data.block[0] = length;
+	status = i2c_smbus_xfer(client->adapter, client->addr, client->flags,
+				I2C_SMBUS_READ, command,
+				I2C_SMBUS_I2C_BLOCK_DATA, &data);
+	if (status < 0)
+		return status;
+
+	memcpy(values, &data.block[1], data.block[0]);
+	return data.block[0];
+}
+EXPORT_SYMBOL(i2c_smbus_read_i2c_block_data);
+
+s32 i2c_smbus_write_i2c_block_data(const struct i2c_client *client, u8 command,
+				   u8 length, const u8 *values)
+{
+	union i2c_smbus_data data;
+
+	if (length > I2C_SMBUS_BLOCK_MAX)
+		length = I2C_SMBUS_BLOCK_MAX;
+	data.block[0] = length;
+	memcpy(data.block + 1, values, length);
+	return i2c_smbus_xfer(client->adapter, client->addr, client->flags,
+			      I2C_SMBUS_WRITE, command,
+			      I2C_SMBUS_I2C_BLOCK_DATA, &data);
+}
+EXPORT_SYMBOL(i2c_smbus_write_i2c_block_data);
+
+/* Simulate a SMBus command using the i2c protocol
+   No checking of parameters is done!  */
+static s32 i2c_smbus_xfer_emulated(struct i2c_adapter *adapter, u16 addr,
+				   unsigned short flags,
+				   char read_write, u8 command, int size,
+				   union i2c_smbus_data *data)
+{
+	/* So we need to generate a series of msgs. In the case of writing, we
+	  need to use only one message; when reading, we need two. We initialize
+	  most things with sane defaults, to keep the code below somewhat
+	  simpler. */
+	unsigned char msgbuf0[I2C_SMBUS_BLOCK_MAX+3];
+	unsigned char msgbuf1[I2C_SMBUS_BLOCK_MAX+2];
+	int num = read_write == I2C_SMBUS_READ ? 2 : 1;
+	int i;
+	u8 partial_pec = 0;
+	int status;
+	struct i2c_msg msg[2] = {
+		{
+			.addr = addr,
+			.flags = flags,
+			.len = 1,
+			.buf = msgbuf0,
+		}, {
+			.addr = addr,
+			.flags = flags | I2C_M_RD,
+			.len = 0,
+			.buf = msgbuf1,
+		},
+	};
+
+	msgbuf0[0] = command;
+	switch (size) {
+	case I2C_SMBUS_QUICK:
+		msg[0].len = 0;
+		/* Special case: The read/write field is used as data */
+		msg[0].flags = flags | (read_write == I2C_SMBUS_READ ?
+					I2C_M_RD : 0);
+		num = 1;
+		break;
+	case I2C_SMBUS_BYTE:
+		if (read_write == I2C_SMBUS_READ) {
+			/* Special case: only a read! */
+			msg[0].flags = I2C_M_RD | flags;
+			num = 1;
+		}
+		break;
+	case I2C_SMBUS_BYTE_DATA:
+		if (read_write == I2C_SMBUS_READ)
+			msg[1].len = 1;
+		else {
+			msg[0].len = 2;
+			msgbuf0[1] = data->byte;
+		}
+		break;
+	case I2C_SMBUS_WORD_DATA:
+		if (read_write == I2C_SMBUS_READ)
+			msg[1].len = 2;
+		else {
+			msg[0].len = 3;
+			msgbuf0[1] = data->word & 0xff;
+			msgbuf0[2] = data->word >> 8;
+		}
+		break;
+	case I2C_SMBUS_PROC_CALL:
+		num = 2; /* Special case */
+		read_write = I2C_SMBUS_READ;
+		msg[0].len = 3;
+		msg[1].len = 2;
+		msgbuf0[1] = data->word & 0xff;
+		msgbuf0[2] = data->word >> 8;
+		break;
+	case I2C_SMBUS_BLOCK_DATA:
+		if (read_write == I2C_SMBUS_READ) {
+			msg[1].flags |= I2C_M_RECV_LEN;
+			msg[1].len = 1; /* block length will be added by
+					   the underlying bus driver */
+		} else {
+			msg[0].len = data->block[0] + 2;
+			if (msg[0].len > I2C_SMBUS_BLOCK_MAX + 2) {
+				dev_err(&adapter->dev,
+					"Invalid block write size %d\n",
+					data->block[0]);
+				return -EINVAL;
+			}
+			for (i = 1; i < msg[0].len; i++)
+				msgbuf0[i] = data->block[i-1];
+		}
+		break;
+	case I2C_SMBUS_BLOCK_PROC_CALL:
+		num = 2; /* Another special case */
+		read_write = I2C_SMBUS_READ;
+		if (data->block[0] > I2C_SMBUS_BLOCK_MAX) {
+			dev_err(&adapter->dev,
+				"Invalid block write size %d\n",
+				data->block[0]);
+			return -EINVAL;
+		}
+		msg[0].len = data->block[0] + 2;
+		for (i = 1; i < msg[0].len; i++)
+			msgbuf0[i] = data->block[i-1];
+		msg[1].flags |= I2C_M_RECV_LEN;
+		msg[1].len = 1; /* block length will be added by
+				   the underlying bus driver */
+		break;
+	case I2C_SMBUS_I2C_BLOCK_DATA:
+		if (read_write == I2C_SMBUS_READ) {
+			msg[1].len = data->block[0];
+		} else {
+			msg[0].len = data->block[0] + 1;
+			if (msg[0].len > I2C_SMBUS_BLOCK_MAX + 1) {
+				dev_err(&adapter->dev,
+					"Invalid block write size %d\n",
+					data->block[0]);
+				return -EINVAL;
+			}
+			for (i = 1; i <= data->block[0]; i++)
+				msgbuf0[i] = data->block[i];
+		}
+		break;
+	default:
+		dev_err(&adapter->dev, "Unsupported transaction %d\n", size);
+		return -EOPNOTSUPP;
+	}
+
+	i = ((flags & I2C_CLIENT_PEC) && size != I2C_SMBUS_QUICK
+				      && size != I2C_SMBUS_I2C_BLOCK_DATA);
+	if (i) {
+		/* Compute PEC if first message is a write */
+		if (!(msg[0].flags & I2C_M_RD)) {
+			if (num == 1) /* Write only */
+				i2c_smbus_add_pec(&msg[0]);
+			else /* Write followed by read */
+				partial_pec = i2c_smbus_msg_pec(0, &msg[0]);
+		}
+		/* Ask for PEC if last message is a read */
+		if (msg[num-1].flags & I2C_M_RD)
+			msg[num-1].len++;
+	}
+
+	status = i2c_transfer(adapter, msg, num);
+	if (status < 0)
+		return status;
+
+	/* Check PEC if last message is a read */
+	if (i && (msg[num-1].flags & I2C_M_RD)) {
+		status = i2c_smbus_check_pec(partial_pec, &msg[num-1]);
+		if (status < 0)
+			return status;
+	}
+
+	if (read_write == I2C_SMBUS_READ)
+		switch (size) {
+		case I2C_SMBUS_BYTE:
+			data->byte = msgbuf0[0];
+			break;
+		case I2C_SMBUS_BYTE_DATA:
+			data->byte = msgbuf1[0];
+			break;
+		case I2C_SMBUS_WORD_DATA:
+		case I2C_SMBUS_PROC_CALL:
+			data->word = msgbuf1[0] | (msgbuf1[1] << 8);
+			break;
+		case I2C_SMBUS_I2C_BLOCK_DATA:
+			for (i = 0; i < data->block[0]; i++)
+				data->block[i+1] = msgbuf1[i];
+			break;
+		case I2C_SMBUS_BLOCK_DATA:
+		case I2C_SMBUS_BLOCK_PROC_CALL:
+			for (i = 0; i < msgbuf1[0] + 1; i++)
+				data->block[i] = msgbuf1[i];
+			break;
+		}
+	return 0;
+}
+
+/**
+ * i2c_smbus_xfer - execute SMBus protocol operations
+ * @adapter: Handle to I2C bus
+ * @addr: Address of SMBus slave on that bus
+ * @flags: I2C_CLIENT_* flags (usually zero or I2C_CLIENT_PEC)
+ * @read_write: I2C_SMBUS_READ or I2C_SMBUS_WRITE
+ * @command: Byte interpreted by slave, for protocols which use such bytes
+ * @protocol: SMBus protocol operation to execute, such as I2C_SMBUS_PROC_CALL
+ * @data: Data to be read or written
+ *
+ * This executes an SMBus protocol operation, and returns a negative
+ * errno code else zero on success.
+ */
+s32 i2c_smbus_xfer(struct i2c_adapter *adapter, u16 addr, unsigned short flags,
+		   char read_write, u8 command, int protocol,
+		   union i2c_smbus_data *data)
+{
+	unsigned long orig_jiffies;
+	int try;
+	s32 res;
+
+	/* If enabled, the following two tracepoints are conditional on
+	 * read_write and protocol.
+	 */
+	trace_smbus_write(adapter, addr, flags, read_write,
+			  command, protocol, data);
+	trace_smbus_read(adapter, addr, flags, read_write,
+			 command, protocol);
+
+	flags &= I2C_M_TEN | I2C_CLIENT_PEC | I2C_CLIENT_SCCB;
+
+	if (adapter->algo->smbus_xfer) {
+		i2c_lock_bus(adapter, I2C_LOCK_SEGMENT);
+
+		/* Retry automatically on arbitration loss */
+		orig_jiffies = jiffies;
+		for (res = 0, try = 0; try <= adapter->retries; try++) {
+			res = adapter->algo->smbus_xfer(adapter, addr, flags,
+							read_write, command,
+							protocol, data);
+			if (res != -EAGAIN)
+				break;
+			if (time_after(jiffies,
+				       orig_jiffies + adapter->timeout))
+				break;
+		}
+		i2c_unlock_bus(adapter, I2C_LOCK_SEGMENT);
+
+		if (res != -EOPNOTSUPP || !adapter->algo->master_xfer)
+			goto trace;
+		/*
+		 * Fall back to i2c_smbus_xfer_emulated if the adapter doesn't
+		 * implement native support for the SMBus operation.
+		 */
+	}
+
+	res = i2c_smbus_xfer_emulated(adapter, addr, flags, read_write,
+				      command, protocol, data);
+
+trace:
+	/* If enabled, the reply tracepoint is conditional on read_write. */
+	trace_smbus_reply(adapter, addr, flags, read_write,
+			  command, protocol, data);
+	trace_smbus_result(adapter, addr, flags, read_write,
+			   command, protocol, res);
+
+	return res;
+}
+EXPORT_SYMBOL(i2c_smbus_xfer);
+
+/**
+ * i2c_smbus_read_i2c_block_data_or_emulated - read block or emulate
+ * @client: Handle to slave device
+ * @command: Byte interpreted by slave
+ * @length: Size of data block; SMBus allows at most I2C_SMBUS_BLOCK_MAX bytes
+ * @values: Byte array into which data will be read; big enough to hold
+ *	the data returned by the slave.  SMBus allows at most
+ *	I2C_SMBUS_BLOCK_MAX bytes.
+ *
+ * This executes the SMBus "block read" protocol if supported by the adapter.
+ * If block read is not supported, it emulates it using either word or byte
+ * read protocols depending on availability.
+ *
+ * The addresses of the I2C slave device that are accessed with this function
+ * must be mapped to a linear region, so that a block read will have the same
+ * effect as a byte read. Before using this function you must double-check
+ * if the I2C slave does support exchanging a block transfer with a byte
+ * transfer.
+ */
+s32 i2c_smbus_read_i2c_block_data_or_emulated(const struct i2c_client *client,
+					      u8 command, u8 length, u8 *values)
+{
+	u8 i = 0;
+	int status;
+
+	if (length > I2C_SMBUS_BLOCK_MAX)
+		length = I2C_SMBUS_BLOCK_MAX;
+
+	if (i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_READ_I2C_BLOCK))
+		return i2c_smbus_read_i2c_block_data(client, command, length, values);
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_READ_BYTE_DATA))
+		return -EOPNOTSUPP;
+
+	if (i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_READ_WORD_DATA)) {
+		while ((i + 2) <= length) {
+			status = i2c_smbus_read_word_data(client, command + i);
+			if (status < 0)
+				return status;
+			values[i] = status & 0xff;
+			values[i + 1] = status >> 8;
+			i += 2;
+		}
+	}
+
+	while (i < length) {
+		status = i2c_smbus_read_byte_data(client, command + i);
+		if (status < 0)
+			return status;
+		values[i] = status;
+		i++;
+	}
+
+	return i;
+}
+EXPORT_SYMBOL(i2c_smbus_read_i2c_block_data_or_emulated);
diff --git a/drivers/i2c/i2c-core.h b/drivers/i2c/i2c-core.h
index 17700bfddcf5..3b63f5e5b89c 100644
--- a/drivers/i2c/i2c-core.h
+++ b/drivers/i2c/i2c-core.h
@@ -27,3 +27,27 @@ extern struct rw_semaphore	__i2c_board_lock;
 extern struct list_head	__i2c_board_list;
 extern int		__i2c_first_dynamic_bus_num;
 
+int i2c_check_addr_validity(unsigned addr, unsigned short flags);
+int i2c_check_7bit_addr_validity_strict(unsigned short addr);
+
+#ifdef CONFIG_ACPI
+void i2c_acpi_register_devices(struct i2c_adapter *adap);
+#else /* CONFIG_ACPI */
+static inline void i2c_acpi_register_devices(struct i2c_adapter *adap) { }
+#endif /* CONFIG_ACPI */
+extern struct notifier_block i2c_acpi_notifier;
+
+#ifdef CONFIG_ACPI_I2C_OPREGION
+int i2c_acpi_install_space_handler(struct i2c_adapter *adapter);
+void i2c_acpi_remove_space_handler(struct i2c_adapter *adapter);
+#else /* CONFIG_ACPI_I2C_OPREGION */
+static inline int i2c_acpi_install_space_handler(struct i2c_adapter *adapter) { return 0; }
+static inline void i2c_acpi_remove_space_handler(struct i2c_adapter *adapter) { }
+#endif /* CONFIG_ACPI_I2C_OPREGION */
+
+#ifdef CONFIG_OF
+void of_i2c_register_devices(struct i2c_adapter *adap);
+#else
+static inline void of_i2c_register_devices(struct i2c_adapter *adap) { }
+#endif
+extern struct notifier_block i2c_of_notifier;
diff --git a/drivers/i2c/i2c-mux.c b/drivers/i2c/i2c-mux.c
index 26f7237558ba..9669ca4937b8 100644
--- a/drivers/i2c/i2c-mux.c
+++ b/drivers/i2c/i2c-mux.c
@@ -395,18 +395,20 @@ int i2c_mux_add_adapter(struct i2c_mux_core *muxc,
 	if (force_nr) {
 		priv->adap.nr = force_nr;
 		ret = i2c_add_numbered_adapter(&priv->adap);
-		dev_err(&parent->dev,
-			"failed to add mux-adapter %u as bus %u (error=%d)\n",
-			chan_id, force_nr, ret);
+		if (ret < 0) {
+			dev_err(&parent->dev,
+				"failed to add mux-adapter %u as bus %u (error=%d)\n",
+				chan_id, force_nr, ret);
+			goto err_free_priv;
+		}
 	} else {
 		ret = i2c_add_adapter(&priv->adap);
-		dev_err(&parent->dev,
-			"failed to add mux-adapter %u (error=%d)\n",
-			chan_id, ret);
-	}
-	if (ret < 0) {
-		kfree(priv);
-		return ret;
+		if (ret < 0) {
+			dev_err(&parent->dev,
+				"failed to add mux-adapter %u (error=%d)\n",
+				chan_id, ret);
+			goto err_free_priv;
+		}
 	}
 
 	WARN(sysfs_create_link(&priv->adap.dev.kobj, &muxc->dev->kobj,
@@ -422,6 +424,10 @@ int i2c_mux_add_adapter(struct i2c_mux_core *muxc,
 
 	muxc->adapter[muxc->num_adapters++] = &priv->adap;
 	return 0;
+
+err_free_priv:
+	kfree(priv);
+	return ret;
 }
 EXPORT_SYMBOL_GPL(i2c_mux_add_adapter);
 
diff --git a/drivers/i2c/muxes/i2c-mux-reg.c b/drivers/i2c/muxes/i2c-mux-reg.c
index 406d5059072c..d97031804de8 100644
--- a/drivers/i2c/muxes/i2c-mux-reg.c
+++ b/drivers/i2c/muxes/i2c-mux-reg.c
@@ -196,20 +196,25 @@ static int i2c_mux_reg_probe(struct platform_device *pdev)
 		res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 		mux->data.reg_size = resource_size(res);
 		mux->data.reg = devm_ioremap_resource(&pdev->dev, res);
-		if (IS_ERR(mux->data.reg))
-			return PTR_ERR(mux->data.reg);
+		if (IS_ERR(mux->data.reg)) {
+			ret = PTR_ERR(mux->data.reg);
+			goto err_put_parent;
+		}
 	}
 
 	if (mux->data.reg_size != 4 && mux->data.reg_size != 2 &&
 	    mux->data.reg_size != 1) {
 		dev_err(&pdev->dev, "Invalid register size\n");
-		return -EINVAL;
+		ret = -EINVAL;
+		goto err_put_parent;
 	}
 
 	muxc = i2c_mux_alloc(parent, &pdev->dev, mux->data.n_values, 0, 0,
 			     i2c_mux_reg_select, NULL);
-	if (!muxc)
-		return -ENOMEM;
+	if (!muxc) {
+		ret = -ENOMEM;
+		goto err_put_parent;
+	}
 	muxc->priv = mux;
 
 	platform_set_drvdata(pdev, muxc);
@@ -223,7 +228,7 @@ static int i2c_mux_reg_probe(struct platform_device *pdev)
 
 		ret = i2c_mux_add_adapter(muxc, nr, mux->data.values[i], class);
 		if (ret)
-			goto add_adapter_failed;
+			goto err_del_mux_adapters;
 	}
 
 	dev_dbg(&pdev->dev, "%d port mux on %s adapter\n",
@@ -231,8 +236,10 @@ static int i2c_mux_reg_probe(struct platform_device *pdev)
 
 	return 0;
 
-add_adapter_failed:
+err_del_mux_adapters:
 	i2c_mux_del_adapters(muxc);
+err_put_parent:
+	i2c_put_adapter(parent);
 
 	return ret;
 }
diff --git a/drivers/iio/adc/bcm_iproc_adc.c b/drivers/iio/adc/bcm_iproc_adc.c
index 21d38c8af21e..7f4f9c4150e3 100644
--- a/drivers/iio/adc/bcm_iproc_adc.c
+++ b/drivers/iio/adc/bcm_iproc_adc.c
@@ -143,7 +143,7 @@ static void iproc_adc_reg_dump(struct iio_dev *indio_dev)
 	iproc_adc_dbg_reg(dev, adc_priv, IPROC_SOFT_BYPASS_DATA);
 }
 
-static irqreturn_t iproc_adc_interrupt_handler(int irq, void *data)
+static irqreturn_t iproc_adc_interrupt_thread(int irq, void *data)
 {
 	u32 channel_intr_status;
 	u32 intr_status;
@@ -167,7 +167,7 @@ static irqreturn_t iproc_adc_interrupt_handler(int irq, void *data)
 	return IRQ_NONE;
 }
 
-static irqreturn_t iproc_adc_interrupt_thread(int irq, void *data)
+static irqreturn_t iproc_adc_interrupt_handler(int irq, void *data)
 {
 	irqreturn_t retval = IRQ_NONE;
 	struct iproc_adc_priv *adc_priv;
@@ -181,7 +181,7 @@ static irqreturn_t iproc_adc_interrupt_thread(int irq, void *data)
 	adc_priv = iio_priv(indio_dev);
 
 	regmap_read(adc_priv->regmap, IPROC_INTERRUPT_STATUS, &intr_status);
-	dev_dbg(&indio_dev->dev, "iproc_adc_interrupt_thread(),INTRPT_STS:%x\n",
+	dev_dbg(&indio_dev->dev, "iproc_adc_interrupt_handler(),INTRPT_STS:%x\n",
 			intr_status);
 
 	intr_channels = (intr_status & IPROC_ADC_INTR_MASK) >> IPROC_ADC_INTR;
@@ -566,8 +566,8 @@ static int iproc_adc_probe(struct platform_device *pdev)
 	}
 
 	ret = devm_request_threaded_irq(&pdev->dev, adc_priv->irqno,
-				iproc_adc_interrupt_thread,
 				iproc_adc_interrupt_handler,
+				iproc_adc_interrupt_thread,
 				IRQF_SHARED, "iproc-adc", indio_dev);
 	if (ret) {
 		dev_err(&pdev->dev, "request_irq error %d\n", ret);
diff --git a/drivers/iio/adc/max9611.c b/drivers/iio/adc/max9611.c
index ec82106480e1..b0526e4b9530 100644
--- a/drivers/iio/adc/max9611.c
+++ b/drivers/iio/adc/max9611.c
@@ -438,10 +438,10 @@ static ssize_t max9611_shunt_resistor_show(struct device *dev,
 	struct max9611_dev *max9611 = iio_priv(dev_to_iio_dev(dev));
 	unsigned int i, r;
 
-	i = max9611->shunt_resistor_uohm / 1000;
-	r = max9611->shunt_resistor_uohm % 1000;
+	i = max9611->shunt_resistor_uohm / 1000000;
+	r = max9611->shunt_resistor_uohm % 1000000;
 
-	return sprintf(buf, "%u.%03u\n", i, r);
+	return sprintf(buf, "%u.%06u\n", i, r);
 }
 
 static IIO_DEVICE_ATTR(in_power_shunt_resistor, 0444,
@@ -536,8 +536,8 @@ static int max9611_probe(struct i2c_client *client,
 	int ret;
 
 	indio_dev = devm_iio_device_alloc(&client->dev, sizeof(*max9611));
-	if (IS_ERR(indio_dev))
-		return PTR_ERR(indio_dev);
+	if (!indio_dev)
+		return -ENOMEM;
 
 	i2c_set_clientdata(client, indio_dev);
 
diff --git a/drivers/iio/adc/sun4i-gpadc-iio.c b/drivers/iio/adc/sun4i-gpadc-iio.c
index b23527309088..81d4c39e414a 100644
--- a/drivers/iio/adc/sun4i-gpadc-iio.c
+++ b/drivers/iio/adc/sun4i-gpadc-iio.c
@@ -105,6 +105,8 @@ struct sun4i_gpadc_iio {
 	bool				no_irq;
 	/* prevents concurrent reads of temperature and ADC */
 	struct mutex			mutex;
+	struct thermal_zone_device	*tzd;
+	struct device			*sensor_device;
 };
 
 #define SUN4I_GPADC_ADC_CHANNEL(_channel, _name) {		\
@@ -502,7 +504,6 @@ static int sun4i_gpadc_probe_dt(struct platform_device *pdev,
 {
 	struct sun4i_gpadc_iio *info = iio_priv(indio_dev);
 	const struct of_device_id *of_dev;
-	struct thermal_zone_device *tzd;
 	struct resource *mem;
 	void __iomem *base;
 	int ret;
@@ -532,13 +533,14 @@ static int sun4i_gpadc_probe_dt(struct platform_device *pdev,
 	if (!IS_ENABLED(CONFIG_THERMAL_OF))
 		return 0;
 
-	tzd = devm_thermal_zone_of_sensor_register(&pdev->dev, 0, info,
-						   &sun4i_ts_tz_ops);
-	if (IS_ERR(tzd))
+	info->sensor_device = &pdev->dev;
+	info->tzd = thermal_zone_of_sensor_register(info->sensor_device, 0,
+						    info, &sun4i_ts_tz_ops);
+	if (IS_ERR(info->tzd))
 		dev_err(&pdev->dev, "could not register thermal sensor: %ld\n",
-			PTR_ERR(tzd));
+			PTR_ERR(info->tzd));
 
-	return PTR_ERR_OR_ZERO(tzd);
+	return PTR_ERR_OR_ZERO(info->tzd);
 }
 
 static int sun4i_gpadc_probe_mfd(struct platform_device *pdev,
@@ -584,15 +586,15 @@ static int sun4i_gpadc_probe_mfd(struct platform_device *pdev,
 		 * of_node, and the device from this driver as third argument to
 		 * return the temperature.
 		 */
-		struct thermal_zone_device *tzd;
-		tzd = devm_thermal_zone_of_sensor_register(pdev->dev.parent, 0,
-							   info,
-							   &sun4i_ts_tz_ops);
-		if (IS_ERR(tzd)) {
+		info->sensor_device = pdev->dev.parent;
+		info->tzd = thermal_zone_of_sensor_register(info->sensor_device,
+							    0, info,
+							    &sun4i_ts_tz_ops);
+		if (IS_ERR(info->tzd)) {
 			dev_err(&pdev->dev,
 				"could not register thermal sensor: %ld\n",
-				PTR_ERR(tzd));
-			return PTR_ERR(tzd);
+				PTR_ERR(info->tzd));
+			return PTR_ERR(info->tzd);
 		}
 	} else {
 		indio_dev->num_channels =
@@ -688,7 +690,13 @@ static int sun4i_gpadc_remove(struct platform_device *pdev)
 
 	pm_runtime_put(&pdev->dev);
 	pm_runtime_disable(&pdev->dev);
-	if (!info->no_irq && IS_ENABLED(CONFIG_THERMAL_OF))
+
+	if (!IS_ENABLED(CONFIG_THERMAL_OF))
+		return 0;
+
+	thermal_zone_of_sensor_unregister(info->sensor_device, info->tzd);
+
+	if (!info->no_irq)
 		iio_map_array_unregister(indio_dev);
 
 	return 0;
@@ -700,6 +708,7 @@ static const struct platform_device_id sun4i_gpadc_id[] = {
 	{ "sun6i-a31-gpadc-iio", (kernel_ulong_t)&sun6i_gpadc_data },
 	{ /* sentinel */ },
 };
+MODULE_DEVICE_TABLE(platform, sun4i_gpadc_id);
 
 static struct platform_driver sun4i_gpadc_driver = {
 	.driver = {
@@ -711,6 +720,7 @@ static struct platform_driver sun4i_gpadc_driver = {
 	.probe = sun4i_gpadc_probe,
 	.remove = sun4i_gpadc_remove,
 };
+MODULE_DEVICE_TABLE(of, sun4i_gpadc_of_id);
 
 module_platform_driver(sun4i_gpadc_driver);
 
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/ti_am335x_adc.c b/drivers/iio/adc/ti_am335x_adc.c
index 4282ceca3d8f..6cbed7eb118a 100644
--- a/drivers/iio/adc/ti_am335x_adc.c
+++ b/drivers/iio/adc/ti_am335x_adc.c
@@ -614,7 +614,7 @@ static int tiadc_probe(struct platform_device *pdev)
 		return -EINVAL;
 	}
 
-	indio_dev = devm_iio_device_alloc(&pdev->dev, sizeof(*indio_dev));
+	indio_dev = devm_iio_device_alloc(&pdev->dev, sizeof(*adc_dev));
 	if (indio_dev == NULL) {
 		dev_err(&pdev->dev, "failed to allocate iio device\n");
 		return -ENOMEM;
diff --git a/drivers/iio/industrialio-trigger.c b/drivers/iio/industrialio-trigger.c
index 978e1592c2a3..4061fed93f1f 100644
--- a/drivers/iio/industrialio-trigger.c
+++ b/drivers/iio/industrialio-trigger.c
@@ -451,7 +451,8 @@ static ssize_t iio_trigger_write_current(struct device *dev,
 	return len;
 
 out_trigger_put:
-	iio_trigger_put(trig);
+	if (trig)
+		iio_trigger_put(trig);
 	return ret;
 }
 
diff --git a/drivers/iio/light/ltr501.c b/drivers/iio/light/ltr501.c
index b30e0c1c6cc4..67838edd8b37 100644
--- a/drivers/iio/light/ltr501.c
+++ b/drivers/iio/light/ltr501.c
@@ -74,9 +74,9 @@ static const int int_time_mapping[] = {100000, 50000, 200000, 400000};
 static const struct reg_field reg_field_it =
 				REG_FIELD(LTR501_ALS_MEAS_RATE, 3, 4);
 static const struct reg_field reg_field_als_intr =
-				REG_FIELD(LTR501_INTR, 0, 0);
-static const struct reg_field reg_field_ps_intr =
 				REG_FIELD(LTR501_INTR, 1, 1);
+static const struct reg_field reg_field_ps_intr =
+				REG_FIELD(LTR501_INTR, 0, 0);
 static const struct reg_field reg_field_als_rate =
 				REG_FIELD(LTR501_ALS_MEAS_RATE, 0, 2);
 static const struct reg_field reg_field_ps_rate =
diff --git a/drivers/iio/proximity/as3935.c b/drivers/iio/proximity/as3935.c
index ddf9bee89f77..aa4df0dcc8c9 100644
--- a/drivers/iio/proximity/as3935.c
+++ b/drivers/iio/proximity/as3935.c
@@ -40,9 +40,9 @@
 #define AS3935_AFE_PWR_BIT	BIT(0)
 
 #define AS3935_INT		0x03
-#define AS3935_INT_MASK		0x07
+#define AS3935_INT_MASK		0x0f
 #define AS3935_EVENT_INT	BIT(3)
-#define AS3935_NOISE_INT	BIT(1)
+#define AS3935_NOISE_INT	BIT(0)
 
 #define AS3935_DATA		0x07
 #define AS3935_DATA_MASK	0x3F
@@ -215,7 +215,7 @@ static irqreturn_t as3935_trigger_handler(int irq, void *private)
 
 	st->buffer[0] = val & AS3935_DATA_MASK;
 	iio_push_to_buffers_with_timestamp(indio_dev, &st->buffer,
-					   pf->timestamp);
+					   iio_get_time_ns(indio_dev));
 err_read:
 	iio_trigger_notify_done(indio_dev->trig);
 
@@ -244,7 +244,7 @@ static void as3935_event_work(struct work_struct *work)
 
 	switch (val) {
 	case AS3935_EVENT_INT:
-		iio_trigger_poll(st->trig);
+		iio_trigger_poll_chained(st->trig);
 		break;
 	case AS3935_NOISE_INT:
 		dev_warn(&st->spi->dev, "noise level is too high\n");
@@ -269,8 +269,6 @@ static irqreturn_t as3935_interrupt_handler(int irq, void *private)
 
 static void calibrate_as3935(struct as3935_state *st)
 {
-	mutex_lock(&st->lock);
-
 	/* mask disturber interrupt bit */
 	as3935_write(st, AS3935_INT, BIT(5));
 
@@ -280,8 +278,6 @@ static void calibrate_as3935(struct as3935_state *st)
 
 	mdelay(2);
 	as3935_write(st, AS3935_TUNE_CAP, (st->tune_cap / TUNE_CAP_DIV));
-
-	mutex_unlock(&st->lock);
 }
 
 #ifdef CONFIG_PM_SLEEP
@@ -318,6 +314,8 @@ static int as3935_resume(struct device *dev)
 	val &= ~AS3935_AFE_PWR_BIT;
 	ret = as3935_write(st, AS3935_AFE_GAIN, val);
 
+	calibrate_as3935(st);
+
 err_resume:
 	mutex_unlock(&st->lock);
 
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 d45772da0963..42defaa0d6c6 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,
@@ -2263,7 +2262,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;
@@ -3456,7 +3455,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;
 
@@ -3485,7 +3484,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;
 
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 93959e1e43a3..d17aad0f54c0 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;
 
@@ -4294,7 +4294,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;
 
@@ -4321,7 +4321,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;
 
@@ -4625,7 +4625,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;
 
@@ -4855,7 +4855,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;
@@ -4934,7 +4934,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/input/mouse/elan_i2c_i2c.c b/drivers/input/mouse/elan_i2c_i2c.c
index a679e56c44cd..f431da07f861 100644
--- a/drivers/input/mouse/elan_i2c_i2c.c
+++ b/drivers/input/mouse/elan_i2c_i2c.c
@@ -554,32 +554,34 @@ static int elan_i2c_finish_fw_update(struct i2c_client *client,
 				     struct completion *completion)
 {
 	struct device *dev = &client->dev;
-	long ret;
 	int error;
 	int len;
-	u8 buffer[ETP_I2C_INF_LENGTH];
+	u8 buffer[ETP_I2C_REPORT_LEN];
+
+	len = i2c_master_recv(client, buffer, ETP_I2C_REPORT_LEN);
+	if (len != ETP_I2C_REPORT_LEN) {
+		error = len < 0 ? len : -EIO;
+		dev_warn(dev, "failed to read I2C data after FW WDT reset: %d (%d)\n",
+			error, len);
+	}
 
 	reinit_completion(completion);
 	enable_irq(client->irq);
 
 	error = elan_i2c_write_cmd(client, ETP_I2C_STAND_CMD, ETP_I2C_RESET);
-	if (!error)
-		ret = wait_for_completion_interruptible_timeout(completion,
-							msecs_to_jiffies(300));
-	disable_irq(client->irq);
-
 	if (error) {
 		dev_err(dev, "device reset failed: %d\n", error);
-		return error;
-	} else if (ret == 0) {
+	} else if (!wait_for_completion_timeout(completion,
+						msecs_to_jiffies(300))) {
 		dev_err(dev, "timeout waiting for device reset\n");
-		return -ETIMEDOUT;
-	} else if (ret < 0) {
-		error = ret;
-		dev_err(dev, "error waiting for device reset: %d\n", error);
-		return error;
+		error = -ETIMEDOUT;
 	}
 
+	disable_irq(client->irq);
+
+	if (error)
+		return error;
+
 	len = i2c_master_recv(client, buffer, ETP_I2C_INF_LENGTH);
 	if (len != ETP_I2C_INF_LENGTH) {
 		error = len < 0 ? len : -EIO;
diff --git a/drivers/input/mouse/synaptics.c b/drivers/input/mouse/synaptics.c
index 131df9d3660f..16c30460ef04 100644
--- a/drivers/input/mouse/synaptics.c
+++ b/drivers/input/mouse/synaptics.c
@@ -176,6 +176,12 @@ static const char * const smbus_pnp_ids[] = {
 	NULL
 };
 
+static const char * const forcepad_pnp_ids[] = {
+	"SYN300D",
+	"SYN3014",
+	NULL
+};
+
 /*
  * Send a command to the synpatics touchpad by special commands
  */
@@ -397,6 +403,8 @@ static int synaptics_query_hardware(struct psmouse *psmouse,
 {
 	int error;
 
+	memset(info, 0, sizeof(*info));
+
 	error = synaptics_identify(psmouse, info);
 	if (error)
 		return error;
@@ -480,13 +488,6 @@ static const struct min_max_quirk min_max_pnpid_table[] = {
 	{ }
 };
 
-/* This list has been kindly provided by Synaptics. */
-static const char * const forcepad_pnp_ids[] = {
-	"SYN300D",
-	"SYN3014",
-	NULL
-};
-
 /*****************************************************************************
  *	Synaptics communications functions
  ****************************************************************************/
@@ -1687,7 +1688,8 @@ enum {
 	SYNAPTICS_INTERTOUCH_ON,
 };
 
-static int synaptics_intertouch = SYNAPTICS_INTERTOUCH_NOT_SET;
+static int synaptics_intertouch = IS_ENABLED(CONFIG_RMI4_SMB) ?
+		SYNAPTICS_INTERTOUCH_NOT_SET : SYNAPTICS_INTERTOUCH_OFF;
 module_param_named(synaptics_intertouch, synaptics_intertouch, int, 0644);
 MODULE_PARM_DESC(synaptics_intertouch, "Use a secondary bus for the Synaptics device.");
 
@@ -1737,8 +1739,16 @@ static int synaptics_setup_intertouch(struct psmouse *psmouse,
 
 	if (synaptics_intertouch == SYNAPTICS_INTERTOUCH_NOT_SET) {
 		if (!psmouse_matches_pnp_id(psmouse, topbuttonpad_pnp_ids) &&
-		    !psmouse_matches_pnp_id(psmouse, smbus_pnp_ids))
+		    !psmouse_matches_pnp_id(psmouse, smbus_pnp_ids)) {
+
+			if (!psmouse_matches_pnp_id(psmouse, forcepad_pnp_ids))
+				psmouse_info(psmouse,
+					     "Your touchpad (%s) says it can support a different bus. "
+					     "If i2c-hid and hid-rmi are not used, you might want to try setting psmouse.synaptics_intertouch to 1 and report this to linux-input@vger.kernel.org.\n",
+					     psmouse->ps2dev.serio->firmware_id);
+
 			return -ENXIO;
+		}
 	}
 
 	psmouse_info(psmouse, "Trying to set up SMBus access\n");
@@ -1810,6 +1820,15 @@ int synaptics_init(struct psmouse *psmouse)
 	}
 
 	if (SYN_CAP_INTERTOUCH(info.ext_cap_0c)) {
+		if ((!IS_ENABLED(CONFIG_RMI4_SMB) ||
+		     !IS_ENABLED(CONFIG_MOUSE_PS2_SYNAPTICS_SMBUS)) &&
+		    /* Forcepads need F21, which is not ready */
+		    !psmouse_matches_pnp_id(psmouse, forcepad_pnp_ids)) {
+			psmouse_warn(psmouse,
+				     "The touchpad can support a better bus than the too old PS/2 protocol. "
+				     "Make sure MOUSE_PS2_SYNAPTICS_SMBUS and RMI4_SMB are enabled to get a better touchpad experience.\n");
+		}
+
 		error = synaptics_setup_intertouch(psmouse, &info, true);
 		if (!error)
 			return PSMOUSE_SYNAPTICS_SMBUS;
diff --git a/drivers/input/touchscreen/atmel_mxt_ts.c b/drivers/input/touchscreen/atmel_mxt_ts.c
index 2302aef2b2d4..dd042a9b0aaa 100644
--- a/drivers/input/touchscreen/atmel_mxt_ts.c
+++ b/drivers/input/touchscreen/atmel_mxt_ts.c
@@ -350,6 +350,7 @@ static bool mxt_object_readable(unsigned int type)
 	case MXT_TOUCH_KEYARRAY_T15:
 	case MXT_TOUCH_PROXIMITY_T23:
 	case MXT_TOUCH_PROXKEY_T52:
+	case MXT_TOUCH_MULTITOUCHSCREEN_T100:
 	case MXT_PROCI_GRIPFACE_T20:
 	case MXT_PROCG_NOISE_T22:
 	case MXT_PROCI_ONETOUCH_T24:
diff --git a/drivers/input/touchscreen/edt-ft5x06.c b/drivers/input/touchscreen/edt-ft5x06.c
index 8cf8d8d5d4ef..f872817e81e4 100644
--- a/drivers/input/touchscreen/edt-ft5x06.c
+++ b/drivers/input/touchscreen/edt-ft5x06.c
@@ -471,7 +471,7 @@ static EDT_ATTR(gain, S_IWUSR | S_IRUGO, WORK_REGISTER_GAIN,
 static EDT_ATTR(offset, S_IWUSR | S_IRUGO, WORK_REGISTER_OFFSET,
 		M09_REGISTER_OFFSET, 0, 31);
 static EDT_ATTR(threshold, S_IWUSR | S_IRUGO, WORK_REGISTER_THRESHOLD,
-		M09_REGISTER_THRESHOLD, 20, 80);
+		M09_REGISTER_THRESHOLD, 0, 80);
 static EDT_ATTR(report_rate, S_IWUSR | S_IRUGO, WORK_REGISTER_REPORT_RATE,
 		NO_REGISTER, 3, 14);
 
diff --git a/drivers/input/touchscreen/silead.c b/drivers/input/touchscreen/silead.c
index 813dd68a5c82..0dbcf105f7db 100644
--- a/drivers/input/touchscreen/silead.c
+++ b/drivers/input/touchscreen/silead.c
@@ -526,6 +526,7 @@ static int __maybe_unused silead_ts_suspend(struct device *dev)
 {
 	struct i2c_client *client = to_i2c_client(dev);
 
+	disable_irq(client->irq);
 	silead_ts_set_power(client, SILEAD_POWER_OFF);
 	return 0;
 }
@@ -551,6 +552,8 @@ static int __maybe_unused silead_ts_resume(struct device *dev)
 		return -ENODEV;
 	}
 
+	enable_irq(client->irq);
+
 	return 0;
 }
 
diff --git a/drivers/iommu/dma-iommu.c b/drivers/iommu/dma-iommu.c
index 8348f366ddd1..62618e77bedc 100644
--- a/drivers/iommu/dma-iommu.c
+++ b/drivers/iommu/dma-iommu.c
@@ -396,13 +396,13 @@ static void iommu_dma_free_iova(struct iommu_dma_cookie *cookie,
 		dma_addr_t iova, size_t size)
 {
 	struct iova_domain *iovad = &cookie->iovad;
-	unsigned long shift = iova_shift(iovad);
 
 	/* The MSI case is only ever cleaning up its most recent allocation */
 	if (cookie->type == IOMMU_DMA_MSI_COOKIE)
 		cookie->msi_iova -= size;
 	else
-		free_iova_fast(iovad, iova >> shift, size >> shift);
+		free_iova_fast(iovad, iova_pfn(iovad, iova),
+				size >> iova_shift(iovad));
 }
 
 static void __iommu_dma_unmap(struct iommu_domain *domain, dma_addr_t dma_addr,
@@ -617,11 +617,14 @@ static dma_addr_t __iommu_dma_map(struct device *dev, phys_addr_t phys,
 {
 	struct iommu_domain *domain = iommu_get_domain_for_dev(dev);
 	struct iommu_dma_cookie *cookie = domain->iova_cookie;
-	struct iova_domain *iovad = &cookie->iovad;
-	size_t iova_off = iova_offset(iovad, phys);
+	size_t iova_off = 0;
 	dma_addr_t iova;
 
-	size = iova_align(iovad, size + iova_off);
+	if (cookie->type == IOMMU_DMA_IOVA_COOKIE) {
+		iova_off = iova_offset(&cookie->iovad, phys);
+		size = iova_align(&cookie->iovad, size + iova_off);
+	}
+
 	iova = iommu_dma_alloc_iova(domain, size, dma_get_mask(dev), dev);
 	if (!iova)
 		return DMA_ERROR_CODE;
diff --git a/drivers/iommu/intel-iommu.c b/drivers/iommu/intel-iommu.c
index 90ab0115d78e..fc2765ccdb57 100644
--- a/drivers/iommu/intel-iommu.c
+++ b/drivers/iommu/intel-iommu.c
@@ -2055,11 +2055,14 @@ static int domain_context_mapping_one(struct dmar_domain *domain,
 	if (context_copied(context)) {
 		u16 did_old = context_domain_id(context);
 
-		if (did_old >= 0 && did_old < cap_ndoms(iommu->cap))
+		if (did_old >= 0 && did_old < cap_ndoms(iommu->cap)) {
 			iommu->flush.flush_context(iommu, did_old,
 						   (((u16)bus) << 8) | devfn,
 						   DMA_CCMD_MASK_NOBIT,
 						   DMA_CCMD_DEVICE_INVL);
+			iommu->flush.flush_iotlb(iommu, did_old, 0, 0,
+						 DMA_TLB_DSI_FLUSH);
+		}
 	}
 
 	pgd = domain->pgd;
diff --git a/drivers/iommu/mtk_iommu_v1.c b/drivers/iommu/mtk_iommu_v1.c
index a27ef570c328..bc1efbfb9ddf 100644
--- a/drivers/iommu/mtk_iommu_v1.c
+++ b/drivers/iommu/mtk_iommu_v1.c
@@ -18,6 +18,7 @@
 #include <linux/clk.h>
 #include <linux/component.h>
 #include <linux/device.h>
+#include <linux/dma-mapping.h>
 #include <linux/dma-iommu.h>
 #include <linux/err.h>
 #include <linux/interrupt.h>
diff --git a/drivers/irqchip/irq-mbigen.c b/drivers/irqchip/irq-mbigen.c
index d2306c821ebb..31d6b5a582d2 100644
--- a/drivers/irqchip/irq-mbigen.c
+++ b/drivers/irqchip/irq-mbigen.c
@@ -106,10 +106,7 @@ static inline void get_mbigen_type_reg(irq_hw_number_t hwirq,
 static inline void get_mbigen_clear_reg(irq_hw_number_t hwirq,
 					u32 *mask, u32 *addr)
 {
-	unsigned int ofst;
-
-	hwirq -= RESERVED_IRQ_PER_MBIGEN_CHIP;
-	ofst = hwirq / 32 * 4;
+	unsigned int ofst = (hwirq / 32) * 4;
 
 	*mask = 1 << (hwirq % 32);
 	*addr = ofst + REG_MBIGEN_CLEAR_OFFSET;
@@ -337,9 +334,15 @@ static int mbigen_device_probe(struct platform_device *pdev)
 	mgn_chip->pdev = pdev;
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	mgn_chip->base = devm_ioremap_resource(&pdev->dev, res);
-	if (IS_ERR(mgn_chip->base))
-		return PTR_ERR(mgn_chip->base);
+	if (!res)
+		return -EINVAL;
+
+	mgn_chip->base = devm_ioremap(&pdev->dev, res->start,
+				      resource_size(res));
+	if (!mgn_chip->base) {
+		dev_err(&pdev->dev, "failed to ioremap %pR\n", res);
+		return -ENOMEM;
+	}
 
 	if (IS_ENABLED(CONFIG_OF) && pdev->dev.of_node)
 		err = mbigen_of_create_domain(pdev, mgn_chip);
diff --git a/drivers/isdn/i4l/isdn_ppp.c b/drivers/isdn/i4l/isdn_ppp.c
index d07dd5196ffc..8aa158a09180 100644
--- a/drivers/isdn/i4l/isdn_ppp.c
+++ b/drivers/isdn/i4l/isdn_ppp.c
@@ -2364,7 +2364,7 @@ static struct ippp_ccp_reset_state *isdn_ppp_ccp_reset_alloc_state(struct ippp_s
 		       id);
 		return NULL;
 	} else {
-		rs = kzalloc(sizeof(struct ippp_ccp_reset_state), GFP_KERNEL);
+		rs = kzalloc(sizeof(struct ippp_ccp_reset_state), GFP_ATOMIC);
 		if (!rs)
 			return NULL;
 		rs->state = CCPResetIdle;
diff --git a/drivers/leds/leds-pca955x.c b/drivers/leds/leds-pca955x.c
index 78a7ce816a47..9a873118ea5f 100644
--- a/drivers/leds/leds-pca955x.c
+++ b/drivers/leds/leds-pca955x.c
@@ -285,7 +285,7 @@ static int pca955x_probe(struct i2c_client *client,
 			"slave address 0x%02x\n",
 			client->name, chip->bits, client->addr);
 
-	if (!i2c_check_functionality(adapter, I2C_FUNC_I2C))
+	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE_DATA))
 		return -EIO;
 
 	if (pdata) {
diff --git a/drivers/md/dm-bufio.c b/drivers/md/dm-bufio.c
index 5db11a405129..cd8139593ccd 100644
--- a/drivers/md/dm-bufio.c
+++ b/drivers/md/dm-bufio.c
@@ -218,7 +218,7 @@ static DEFINE_SPINLOCK(param_spinlock);
  * Buffers are freed after this timeout
  */
 static unsigned dm_bufio_max_age = DM_BUFIO_DEFAULT_AGE_SECS;
-static unsigned dm_bufio_retain_bytes = DM_BUFIO_DEFAULT_RETAIN_BYTES;
+static unsigned long dm_bufio_retain_bytes = DM_BUFIO_DEFAULT_RETAIN_BYTES;
 
 static unsigned long dm_bufio_peak_allocated;
 static unsigned long dm_bufio_allocated_kmem_cache;
@@ -1558,10 +1558,10 @@ static bool __try_evict_buffer(struct dm_buffer *b, gfp_t gfp)
 	return true;
 }
 
-static unsigned get_retain_buffers(struct dm_bufio_client *c)
+static unsigned long get_retain_buffers(struct dm_bufio_client *c)
 {
-        unsigned retain_bytes = ACCESS_ONCE(dm_bufio_retain_bytes);
-        return retain_bytes / c->block_size;
+        unsigned long retain_bytes = ACCESS_ONCE(dm_bufio_retain_bytes);
+        return retain_bytes >> (c->sectors_per_block_bits + SECTOR_SHIFT);
 }
 
 static unsigned long __scan(struct dm_bufio_client *c, unsigned long nr_to_scan,
@@ -1571,7 +1571,7 @@ static unsigned long __scan(struct dm_bufio_client *c, unsigned long nr_to_scan,
 	struct dm_buffer *b, *tmp;
 	unsigned long freed = 0;
 	unsigned long count = nr_to_scan;
-	unsigned retain_target = get_retain_buffers(c);
+	unsigned long retain_target = get_retain_buffers(c);
 
 	for (l = 0; l < LIST_SIZE; l++) {
 		list_for_each_entry_safe_reverse(b, tmp, &c->lru[l], lru_list) {
@@ -1794,8 +1794,8 @@ static bool older_than(struct dm_buffer *b, unsigned long age_hz)
 static void __evict_old_buffers(struct dm_bufio_client *c, unsigned long age_hz)
 {
 	struct dm_buffer *b, *tmp;
-	unsigned retain_target = get_retain_buffers(c);
-	unsigned count;
+	unsigned long retain_target = get_retain_buffers(c);
+	unsigned long count;
 	LIST_HEAD(write_list);
 
 	dm_bufio_lock(c);
@@ -1955,7 +1955,7 @@ MODULE_PARM_DESC(max_cache_size_bytes, "Size of metadata cache");
 module_param_named(max_age_seconds, dm_bufio_max_age, uint, S_IRUGO | S_IWUSR);
 MODULE_PARM_DESC(max_age_seconds, "Max age of a buffer in seconds");
 
-module_param_named(retain_bytes, dm_bufio_retain_bytes, uint, S_IRUGO | S_IWUSR);
+module_param_named(retain_bytes, dm_bufio_retain_bytes, ulong, S_IRUGO | S_IWUSR);
 MODULE_PARM_DESC(retain_bytes, "Try to keep at least this many bytes cached in memory");
 
 module_param_named(peak_allocated_bytes, dm_bufio_peak_allocated, ulong, S_IRUGO | S_IWUSR);
diff --git a/drivers/md/dm-cache-background-tracker.c b/drivers/md/dm-cache-background-tracker.c
index 9b1afdfb13f0..707233891291 100644
--- a/drivers/md/dm-cache-background-tracker.c
+++ b/drivers/md/dm-cache-background-tracker.c
@@ -33,6 +33,11 @@ struct background_tracker *btracker_create(unsigned max_work)
 {
 	struct background_tracker *b = kmalloc(sizeof(*b), GFP_KERNEL);
 
+	if (!b) {
+		DMERR("couldn't create background_tracker");
+		return NULL;
+	}
+
 	b->max_work = max_work;
 	atomic_set(&b->pending_promotes, 0);
 	atomic_set(&b->pending_writebacks, 0);
diff --git a/drivers/md/dm-cache-policy-smq.c b/drivers/md/dm-cache-policy-smq.c
index 72479bd61e11..e5eb9c9b4bc8 100644
--- a/drivers/md/dm-cache-policy-smq.c
+++ b/drivers/md/dm-cache-policy-smq.c
@@ -1120,8 +1120,6 @@ static bool clean_target_met(struct smq_policy *mq, bool idle)
 	 * Cache entries may not be populated.  So we cannot rely on the
 	 * size of the clean queue.
 	 */
-	unsigned nr_clean;
-
 	if (idle) {
 		/*
 		 * We'd like to clean everything.
@@ -1129,18 +1127,16 @@ static bool clean_target_met(struct smq_policy *mq, bool idle)
 		return q_size(&mq->dirty) == 0u;
 	}
 
-	nr_clean = from_cblock(mq->cache_size) - q_size(&mq->dirty);
-	return (nr_clean + btracker_nr_writebacks_queued(mq->bg_work)) >=
-		percent_to_target(mq, CLEAN_TARGET);
+	/*
+	 * If we're busy we don't worry about cleaning at all.
+	 */
+	return true;
 }
 
-static bool free_target_met(struct smq_policy *mq, bool idle)
+static bool free_target_met(struct smq_policy *mq)
 {
 	unsigned nr_free;
 
-	if (!idle)
-		return true;
-
 	nr_free = from_cblock(mq->cache_size) - mq->cache_alloc.nr_allocated;
 	return (nr_free + btracker_nr_demotions_queued(mq->bg_work)) >=
 		percent_to_target(mq, FREE_TARGET);
@@ -1190,9 +1186,9 @@ static void queue_demotion(struct smq_policy *mq)
 	if (unlikely(WARN_ON_ONCE(!mq->migrations_allowed)))
 		return;
 
-	e = q_peek(&mq->clean, mq->clean.nr_levels, true);
+	e = q_peek(&mq->clean, mq->clean.nr_levels / 2, true);
 	if (!e) {
-		if (!clean_target_met(mq, false))
+		if (!clean_target_met(mq, true))
 			queue_writeback(mq);
 		return;
 	}
@@ -1220,7 +1216,7 @@ static void queue_promotion(struct smq_policy *mq, dm_oblock_t oblock,
 		 * We always claim to be 'idle' to ensure some demotions happen
 		 * with continuous loads.
 		 */
-		if (!free_target_met(mq, true))
+		if (!free_target_met(mq))
 			queue_demotion(mq);
 		return;
 	}
@@ -1421,14 +1417,10 @@ static int smq_get_background_work(struct dm_cache_policy *p, bool idle,
 	spin_lock_irqsave(&mq->lock, flags);
 	r = btracker_issue(mq->bg_work, result);
 	if (r == -ENODATA) {
-		/* find some writeback work to do */
-		if (mq->migrations_allowed && !free_target_met(mq, idle))
-			queue_demotion(mq);
-
-		else if (!clean_target_met(mq, idle))
+		if (!clean_target_met(mq, idle)) {
 			queue_writeback(mq);
-
-		r = btracker_issue(mq->bg_work, result);
+			r = btracker_issue(mq->bg_work, result);
+		}
 	}
 	spin_unlock_irqrestore(&mq->lock, flags);
 
@@ -1452,6 +1444,7 @@ static void __complete_background_work(struct smq_policy *mq,
 		clear_pending(mq, e);
 		if (success) {
 			e->oblock = work->oblock;
+			e->level = NR_CACHE_LEVELS - 1;
 			push(mq, e);
 			// h, q, a
 		} else {
diff --git a/drivers/md/dm-cache-target.c b/drivers/md/dm-cache-target.c
index 1db375f50a13..d682a0511381 100644
--- a/drivers/md/dm-cache-target.c
+++ b/drivers/md/dm-cache-target.c
@@ -94,6 +94,9 @@ static void iot_io_begin(struct io_tracker *iot, sector_t len)
 
 static void __iot_io_end(struct io_tracker *iot, sector_t len)
 {
+	if (!len)
+		return;
+
 	iot->in_flight -= len;
 	if (!iot->in_flight)
 		iot->idle_time = jiffies;
@@ -474,7 +477,7 @@ struct cache {
 	spinlock_t invalidation_lock;
 	struct list_head invalidation_requests;
 
-	struct io_tracker origin_tracker;
+	struct io_tracker tracker;
 
 	struct work_struct commit_ws;
 	struct batcher committer;
@@ -901,8 +904,7 @@ static dm_oblock_t get_bio_block(struct cache *cache, struct bio *bio)
 
 static bool accountable_bio(struct cache *cache, struct bio *bio)
 {
-	return ((bio->bi_bdev == cache->origin_dev->bdev) &&
-		bio_op(bio) != REQ_OP_DISCARD);
+	return bio_op(bio) != REQ_OP_DISCARD;
 }
 
 static void accounted_begin(struct cache *cache, struct bio *bio)
@@ -912,7 +914,7 @@ static void accounted_begin(struct cache *cache, struct bio *bio)
 
 	if (accountable_bio(cache, bio)) {
 		pb->len = bio_sectors(bio);
-		iot_io_begin(&cache->origin_tracker, pb->len);
+		iot_io_begin(&cache->tracker, pb->len);
 	}
 }
 
@@ -921,7 +923,7 @@ static void accounted_complete(struct cache *cache, struct bio *bio)
 	size_t pb_data_size = get_per_bio_data_size(cache);
 	struct per_bio_data *pb = get_per_bio_data(bio, pb_data_size);
 
-	iot_io_end(&cache->origin_tracker, pb->len);
+	iot_io_end(&cache->tracker, pb->len);
 }
 
 static void accounted_request(struct cache *cache, struct bio *bio)
@@ -1716,20 +1718,19 @@ static int invalidate_start(struct cache *cache, dm_cblock_t cblock,
 
 enum busy {
 	IDLE,
-	MODERATE,
 	BUSY
 };
 
 static enum busy spare_migration_bandwidth(struct cache *cache)
 {
-	bool idle = iot_idle_for(&cache->origin_tracker, HZ);
+	bool idle = iot_idle_for(&cache->tracker, HZ);
 	sector_t current_volume = (atomic_read(&cache->nr_io_migrations) + 1) *
 		cache->sectors_per_block;
 
-	if (current_volume <= cache->migration_threshold)
-		return idle ? IDLE : MODERATE;
+	if (idle && current_volume <= cache->migration_threshold)
+		return IDLE;
 	else
-		return idle ? MODERATE : BUSY;
+		return BUSY;
 }
 
 static void inc_hit_counter(struct cache *cache, struct bio *bio)
@@ -2045,8 +2046,6 @@ static void check_migrations(struct work_struct *ws)
 
 	for (;;) {
 		b = spare_migration_bandwidth(cache);
-		if (b == BUSY)
-			break;
 
 		r = policy_get_background_work(cache->policy, b == IDLE, &op);
 		if (r == -ENODATA)
@@ -2717,7 +2716,7 @@ static int cache_create(struct cache_args *ca, struct cache **result)
 
 	batcher_init(&cache->committer, commit_op, cache,
 		     issue_op, cache, cache->wq);
-	iot_init(&cache->origin_tracker);
+	iot_init(&cache->tracker);
 
 	init_rwsem(&cache->background_work_lock);
 	prevent_background_work(cache);
@@ -2941,7 +2940,7 @@ static void cache_postsuspend(struct dm_target *ti)
 
 	cancel_delayed_work(&cache->waker);
 	flush_workqueue(cache->wq);
-	WARN_ON(cache->origin_tracker.in_flight);
+	WARN_ON(cache->tracker.in_flight);
 
 	/*
 	 * If it's a flush suspend there won't be any deferred bios, so this
diff --git a/drivers/md/dm-mpath.c b/drivers/md/dm-mpath.c
index 926a6bcb32c8..3df056b73b66 100644
--- a/drivers/md/dm-mpath.c
+++ b/drivers/md/dm-mpath.c
@@ -447,7 +447,7 @@ failed:
  * it has been invoked.
  */
 #define dm_report_EIO(m)						\
-({									\
+do {									\
 	struct mapped_device *md = dm_table_get_md((m)->ti->table);	\
 									\
 	pr_debug("%s: returning EIO; QIFNP = %d; SQIFNP = %d; DNFS = %d\n", \
@@ -455,8 +455,7 @@ failed:
 		 test_bit(MPATHF_QUEUE_IF_NO_PATH, &(m)->flags),	\
 		 test_bit(MPATHF_SAVED_QUEUE_IF_NO_PATH, &(m)->flags),	\
 		 dm_noflush_suspending((m)->ti));			\
-	-EIO;								\
-})
+} while (0)
 
 /*
  * Map cloned requests (request-based multipath)
@@ -481,7 +480,8 @@ static int multipath_clone_and_map(struct dm_target *ti, struct request *rq,
 	if (!pgpath) {
 		if (test_bit(MPATHF_QUEUE_IF_NO_PATH, &m->flags))
 			return DM_MAPIO_DELAY_REQUEUE;
-		return dm_report_EIO(m);	/* Failed */
+		dm_report_EIO(m);	/* Failed */
+		return DM_MAPIO_KILL;
 	} else if (test_bit(MPATHF_QUEUE_IO, &m->flags) ||
 		   test_bit(MPATHF_PG_INIT_REQUIRED, &m->flags)) {
 		if (pg_init_all_paths(m))
@@ -558,7 +558,8 @@ static int __multipath_map_bio(struct multipath *m, struct bio *bio, struct dm_m
 	if (!pgpath) {
 		if (test_bit(MPATHF_QUEUE_IF_NO_PATH, &m->flags))
 			return DM_MAPIO_REQUEUE;
-		return dm_report_EIO(m);
+		dm_report_EIO(m);
+		return -EIO;
 	}
 
 	mpio->pgpath = 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)
-				error = dm_report_EIO(m);
+				dm_report_EIO(m);
 			/* complete with the original error */
 			r = DM_ENDIO_DONE;
 		}
@@ -1524,8 +1525,10 @@ static int do_end_io_bio(struct multipath *m, struct bio *clone,
 		fail_path(mpio->pgpath);
 
 	if (atomic_read(&m->nr_valid_paths) == 0 &&
-	    !test_bit(MPATHF_QUEUE_IF_NO_PATH, &m->flags))
-		return dm_report_EIO(m);
+	    !test_bit(MPATHF_QUEUE_IF_NO_PATH, &m->flags)) {
+		dm_report_EIO(m);
+		return -EIO;
+	}
 
 	/* Queue for the daemon to resubmit */
 	dm_bio_restore(get_bio_details_from_bio(clone), clone);
diff --git a/drivers/md/dm-rq.c b/drivers/md/dm-rq.c
index 2af27026aa2e..b639fa7246ee 100644
--- a/drivers/md/dm-rq.c
+++ b/drivers/md/dm-rq.c
@@ -507,6 +507,7 @@ static int map_request(struct dm_rq_target_io *tio)
 	case DM_MAPIO_KILL:
 		/* The target wants to complete the I/O */
 		dm_kill_unmapped_request(rq, -EIO);
+		break;
 	default:
 		DMWARN("unimplemented target map return value: %d", r);
 		BUG();
diff --git a/drivers/md/dm-thin-metadata.c b/drivers/md/dm-thin-metadata.c
index 0f0251d0d337..d31d18d9727c 100644
--- a/drivers/md/dm-thin-metadata.c
+++ b/drivers/md/dm-thin-metadata.c
@@ -484,11 +484,11 @@ static int __write_initial_superblock(struct dm_pool_metadata *pmd)
 	if (r < 0)
 		return r;
 
-	r = save_sm_roots(pmd);
+	r = dm_tm_pre_commit(pmd->tm);
 	if (r < 0)
 		return r;
 
-	r = dm_tm_pre_commit(pmd->tm);
+	r = save_sm_roots(pmd);
 	if (r < 0)
 		return r;
 
diff --git a/drivers/md/md.c b/drivers/md/md.c
index 82f798be964f..10367ffe92e3 100644
--- a/drivers/md/md.c
+++ b/drivers/md/md.c
@@ -8022,18 +8022,15 @@ EXPORT_SYMBOL(md_write_end);
  * may proceed without blocking.  It is important to call this before
  * attempting a GFP_KERNEL allocation while holding the mddev lock.
  * Must be called with mddev_lock held.
- *
- * In the ->external case MD_SB_CHANGE_PENDING can not be cleared until mddev->lock
- * is dropped, so return -EAGAIN after notifying userspace.
  */
-int md_allow_write(struct mddev *mddev)
+void md_allow_write(struct mddev *mddev)
 {
 	if (!mddev->pers)
-		return 0;
+		return;
 	if (mddev->ro)
-		return 0;
+		return;
 	if (!mddev->pers->sync_request)
-		return 0;
+		return;
 
 	spin_lock(&mddev->lock);
 	if (mddev->in_sync) {
@@ -8046,13 +8043,12 @@ int md_allow_write(struct mddev *mddev)
 		spin_unlock(&mddev->lock);
 		md_update_sb(mddev, 0);
 		sysfs_notify_dirent_safe(mddev->sysfs_state);
+		/* wait for the dirty state to be recorded in the metadata */
+		wait_event(mddev->sb_wait,
+			   !test_bit(MD_SB_CHANGE_CLEAN, &mddev->sb_flags) &&
+			   !test_bit(MD_SB_CHANGE_PENDING, &mddev->sb_flags));
 	} else
 		spin_unlock(&mddev->lock);
-
-	if (test_bit(MD_SB_CHANGE_PENDING, &mddev->sb_flags))
-		return -EAGAIN;
-	else
-		return 0;
 }
 EXPORT_SYMBOL_GPL(md_allow_write);
 
diff --git a/drivers/md/md.h b/drivers/md/md.h
index 4e75d121bfcc..11f15146ce51 100644
--- a/drivers/md/md.h
+++ b/drivers/md/md.h
@@ -665,7 +665,7 @@ extern int sync_page_io(struct md_rdev *rdev, sector_t sector, int size,
 			bool metadata_op);
 extern void md_do_sync(struct md_thread *thread);
 extern void md_new_event(struct mddev *mddev);
-extern int md_allow_write(struct mddev *mddev);
+extern void md_allow_write(struct mddev *mddev);
 extern void md_wait_for_blocked_rdev(struct md_rdev *rdev, struct mddev *mddev);
 extern void md_set_array_sectors(struct mddev *mddev, sector_t array_sectors);
 extern int md_check_no_bitmap(struct mddev *mddev);
diff --git a/drivers/md/persistent-data/dm-space-map-disk.c b/drivers/md/persistent-data/dm-space-map-disk.c
index ebb280a14325..32adf6b4a9c7 100644
--- a/drivers/md/persistent-data/dm-space-map-disk.c
+++ b/drivers/md/persistent-data/dm-space-map-disk.c
@@ -142,10 +142,23 @@ static int sm_disk_inc_block(struct dm_space_map *sm, dm_block_t b)
 
 static int sm_disk_dec_block(struct dm_space_map *sm, dm_block_t b)
 {
+	int r;
+	uint32_t old_count;
 	enum allocation_event ev;
 	struct sm_disk *smd = container_of(sm, struct sm_disk, sm);
 
-	return sm_ll_dec(&smd->ll, b, &ev);
+	r = sm_ll_dec(&smd->ll, b, &ev);
+	if (!r && (ev == SM_FREE)) {
+		/*
+		 * It's only free if it's also free in the last
+		 * transaction.
+		 */
+		r = sm_ll_lookup(&smd->old_ll, b, &old_count);
+		if (!r && !old_count)
+			smd->nr_allocated_this_transaction--;
+	}
+
+	return r;
 }
 
 static int sm_disk_new_block(struct dm_space_map *sm, dm_block_t *b)
diff --git a/drivers/md/raid0.c b/drivers/md/raid0.c
index 84e58596594d..d6c0bc76e837 100644
--- a/drivers/md/raid0.c
+++ b/drivers/md/raid0.c
@@ -385,7 +385,7 @@ static int raid0_run(struct mddev *mddev)
 		blk_queue_max_hw_sectors(mddev->queue, mddev->chunk_sectors);
 		blk_queue_max_write_same_sectors(mddev->queue, mddev->chunk_sectors);
 		blk_queue_max_write_zeroes_sectors(mddev->queue, mddev->chunk_sectors);
-		blk_queue_max_discard_sectors(mddev->queue, mddev->chunk_sectors);
+		blk_queue_max_discard_sectors(mddev->queue, UINT_MAX);
 
 		blk_queue_io_min(mddev->queue, mddev->chunk_sectors << 9);
 		blk_queue_io_opt(mddev->queue,
@@ -459,6 +459,95 @@ static inline int is_io_in_chunk_boundary(struct mddev *mddev,
 	}
 }
 
+static void raid0_handle_discard(struct mddev *mddev, struct bio *bio)
+{
+	struct r0conf *conf = mddev->private;
+	struct strip_zone *zone;
+	sector_t start = bio->bi_iter.bi_sector;
+	sector_t end;
+	unsigned int stripe_size;
+	sector_t first_stripe_index, last_stripe_index;
+	sector_t start_disk_offset;
+	unsigned int start_disk_index;
+	sector_t end_disk_offset;
+	unsigned int end_disk_index;
+	unsigned int disk;
+
+	zone = find_zone(conf, &start);
+
+	if (bio_end_sector(bio) > zone->zone_end) {
+		struct bio *split = bio_split(bio,
+			zone->zone_end - bio->bi_iter.bi_sector, GFP_NOIO,
+			mddev->bio_set);
+		bio_chain(split, bio);
+		generic_make_request(bio);
+		bio = split;
+		end = zone->zone_end;
+	} else
+		end = bio_end_sector(bio);
+
+	if (zone != conf->strip_zone)
+		end = end - zone[-1].zone_end;
+
+	/* Now start and end is the offset in zone */
+	stripe_size = zone->nb_dev * mddev->chunk_sectors;
+
+	first_stripe_index = start;
+	sector_div(first_stripe_index, stripe_size);
+	last_stripe_index = end;
+	sector_div(last_stripe_index, stripe_size);
+
+	start_disk_index = (int)(start - first_stripe_index * stripe_size) /
+		mddev->chunk_sectors;
+	start_disk_offset = ((int)(start - first_stripe_index * stripe_size) %
+		mddev->chunk_sectors) +
+		first_stripe_index * mddev->chunk_sectors;
+	end_disk_index = (int)(end - last_stripe_index * stripe_size) /
+		mddev->chunk_sectors;
+	end_disk_offset = ((int)(end - last_stripe_index * stripe_size) %
+		mddev->chunk_sectors) +
+		last_stripe_index * mddev->chunk_sectors;
+
+	for (disk = 0; disk < zone->nb_dev; disk++) {
+		sector_t dev_start, dev_end;
+		struct bio *discard_bio = NULL;
+		struct md_rdev *rdev;
+
+		if (disk < start_disk_index)
+			dev_start = (first_stripe_index + 1) *
+				mddev->chunk_sectors;
+		else if (disk > start_disk_index)
+			dev_start = first_stripe_index * mddev->chunk_sectors;
+		else
+			dev_start = start_disk_offset;
+
+		if (disk < end_disk_index)
+			dev_end = (last_stripe_index + 1) * mddev->chunk_sectors;
+		else if (disk > end_disk_index)
+			dev_end = last_stripe_index * mddev->chunk_sectors;
+		else
+			dev_end = end_disk_offset;
+
+		if (dev_end <= dev_start)
+			continue;
+
+		rdev = conf->devlist[(zone - conf->strip_zone) *
+			conf->strip_zone[0].nb_dev + disk];
+		if (__blkdev_issue_discard(rdev->bdev,
+			dev_start + zone->dev_start + rdev->data_offset,
+			dev_end - dev_start, GFP_NOIO, 0, &discard_bio) ||
+		    !discard_bio)
+			continue;
+		bio_chain(discard_bio, bio);
+		if (mddev->gendisk)
+			trace_block_bio_remap(bdev_get_queue(rdev->bdev),
+				discard_bio, disk_devt(mddev->gendisk),
+				bio->bi_iter.bi_sector);
+		generic_make_request(discard_bio);
+	}
+	bio_endio(bio);
+}
+
 static void raid0_make_request(struct mddev *mddev, struct bio *bio)
 {
 	struct strip_zone *zone;
@@ -473,6 +562,11 @@ static void raid0_make_request(struct mddev *mddev, struct bio *bio)
 		return;
 	}
 
+	if (unlikely((bio_op(bio) == REQ_OP_DISCARD))) {
+		raid0_handle_discard(mddev, bio);
+		return;
+	}
+
 	bio_sector = bio->bi_iter.bi_sector;
 	sector = bio_sector;
 	chunk_sects = mddev->chunk_sectors;
@@ -498,19 +592,13 @@ static void raid0_make_request(struct mddev *mddev, struct bio *bio)
 	bio->bi_iter.bi_sector = sector + zone->dev_start +
 		tmp_dev->data_offset;
 
-	if (unlikely((bio_op(bio) == REQ_OP_DISCARD) &&
-		     !blk_queue_discard(bdev_get_queue(bio->bi_bdev)))) {
-		/* Just ignore it */
-		bio_endio(bio);
-	} else {
-		if (mddev->gendisk)
-			trace_block_bio_remap(bdev_get_queue(bio->bi_bdev),
-					      bio, disk_devt(mddev->gendisk),
-					      bio_sector);
-		mddev_check_writesame(mddev, bio);
-		mddev_check_write_zeroes(mddev, bio);
-		generic_make_request(bio);
-	}
+	if (mddev->gendisk)
+		trace_block_bio_remap(bdev_get_queue(bio->bi_bdev),
+				      bio, disk_devt(mddev->gendisk),
+				      bio_sector);
+	mddev_check_writesame(mddev, bio);
+	mddev_check_write_zeroes(mddev, bio);
+	generic_make_request(bio);
 }
 
 static void raid0_status(struct seq_file *seq, struct mddev *mddev)
diff --git a/drivers/md/raid1.c b/drivers/md/raid1.c
index 7ed59351fe97..af5056d56878 100644
--- a/drivers/md/raid1.c
+++ b/drivers/md/raid1.c
@@ -666,8 +666,11 @@ static int read_balance(struct r1conf *conf, struct r1bio *r1_bio, int *max_sect
 					break;
 			}
 			continue;
-		} else
+		} else {
+			if ((sectors > best_good_sectors) && (best_disk >= 0))
+				best_disk = -1;
 			best_good_sectors = sectors;
+		}
 
 		if (best_disk >= 0)
 			/* At least two disks to choose from so failfast is OK */
@@ -1529,17 +1532,16 @@ static void raid1_write_request(struct mddev *mddev, struct bio *bio,
 			plug = container_of(cb, struct raid1_plug_cb, cb);
 		else
 			plug = NULL;
-		spin_lock_irqsave(&conf->device_lock, flags);
 		if (plug) {
 			bio_list_add(&plug->pending, mbio);
 			plug->pending_cnt++;
 		} else {
+			spin_lock_irqsave(&conf->device_lock, flags);
 			bio_list_add(&conf->pending_bio_list, mbio);
 			conf->pending_count++;
-		}
-		spin_unlock_irqrestore(&conf->device_lock, flags);
-		if (!plug)
+			spin_unlock_irqrestore(&conf->device_lock, flags);
 			md_wakeup_thread(mddev->thread);
+		}
 	}
 
 	r1_bio_write_done(r1_bio);
@@ -3197,7 +3199,7 @@ static int raid1_reshape(struct mddev *mddev)
 	struct r1conf *conf = mddev->private;
 	int cnt, raid_disks;
 	unsigned long flags;
-	int d, d2, err;
+	int d, d2;
 
 	/* Cannot change chunk_size, layout, or level */
 	if (mddev->chunk_sectors != mddev->new_chunk_sectors ||
@@ -3209,11 +3211,8 @@ static int raid1_reshape(struct mddev *mddev)
 		return -EINVAL;
 	}
 
-	if (!mddev_is_clustered(mddev)) {
-		err = md_allow_write(mddev);
-		if (err)
-			return err;
-	}
+	if (!mddev_is_clustered(mddev))
+		md_allow_write(mddev);
 
 	raid_disks = mddev->raid_disks + mddev->delta_disks;
 
diff --git a/drivers/md/raid10.c b/drivers/md/raid10.c
index 6b86a0032cf8..4343d7ff9916 100644
--- a/drivers/md/raid10.c
+++ b/drivers/md/raid10.c
@@ -1282,17 +1282,16 @@ static void raid10_write_one_disk(struct mddev *mddev, struct r10bio *r10_bio,
 		plug = container_of(cb, struct raid10_plug_cb, cb);
 	else
 		plug = NULL;
-	spin_lock_irqsave(&conf->device_lock, flags);
 	if (plug) {
 		bio_list_add(&plug->pending, mbio);
 		plug->pending_cnt++;
 	} else {
+		spin_lock_irqsave(&conf->device_lock, flags);
 		bio_list_add(&conf->pending_bio_list, mbio);
 		conf->pending_count++;
-	}
-	spin_unlock_irqrestore(&conf->device_lock, flags);
-	if (!plug)
+		spin_unlock_irqrestore(&conf->device_lock, flags);
 		md_wakeup_thread(mddev->thread);
+	}
 }
 
 static void raid10_write_request(struct mddev *mddev, struct bio *bio,
diff --git a/drivers/md/raid5-cache.c b/drivers/md/raid5-cache.c
index 26ba09282e7c..4c00bc248287 100644
--- a/drivers/md/raid5-cache.c
+++ b/drivers/md/raid5-cache.c
@@ -24,6 +24,7 @@
 #include "md.h"
 #include "raid5.h"
 #include "bitmap.h"
+#include "raid5-log.h"
 
 /*
  * metadata/data stored in disk with 4k size unit (a block) regardless
@@ -622,20 +623,30 @@ static void r5l_do_submit_io(struct r5l_log *log, struct r5l_io_unit *io)
 	__r5l_set_io_unit_state(io, IO_UNIT_IO_START);
 	spin_unlock_irqrestore(&log->io_list_lock, flags);
 
+	/*
+	 * In case of journal device failures, submit_bio will get error
+	 * and calls endio, then active stripes will continue write
+	 * process. Therefore, it is not necessary to check Faulty bit
+	 * of journal device here.
+	 *
+	 * We can't check split_bio after current_bio is submitted. If
+	 * io->split_bio is null, after current_bio is submitted, current_bio
+	 * might already be completed and the io_unit is freed. We submit
+	 * split_bio first to avoid the issue.
+	 */
+	if (io->split_bio) {
+		if (io->has_flush)
+			io->split_bio->bi_opf |= REQ_PREFLUSH;
+		if (io->has_fua)
+			io->split_bio->bi_opf |= REQ_FUA;
+		submit_bio(io->split_bio);
+	}
+
 	if (io->has_flush)
 		io->current_bio->bi_opf |= REQ_PREFLUSH;
 	if (io->has_fua)
 		io->current_bio->bi_opf |= REQ_FUA;
 	submit_bio(io->current_bio);
-
-	if (!io->split_bio)
-		return;
-
-	if (io->has_flush)
-		io->split_bio->bi_opf |= REQ_PREFLUSH;
-	if (io->has_fua)
-		io->split_bio->bi_opf |= REQ_FUA;
-	submit_bio(io->split_bio);
 }
 
 /* deferred io_unit will be dispatched here */
@@ -670,6 +681,11 @@ static void r5c_disable_writeback_async(struct work_struct *work)
 		return;
 	pr_info("md/raid:%s: Disabling writeback cache for degraded array.\n",
 		mdname(mddev));
+
+	/* wait superblock change before suspend */
+	wait_event(mddev->sb_wait,
+		   !test_bit(MD_SB_CHANGE_PENDING, &mddev->sb_flags));
+
 	mddev_suspend(mddev);
 	log->r5c_journal_mode = R5C_JOURNAL_MODE_WRITE_THROUGH;
 	mddev_resume(mddev);
@@ -2621,8 +2637,11 @@ int r5c_try_caching_write(struct r5conf *conf,
 	 * When run in degraded mode, array is set to write-through mode.
 	 * This check helps drain pending write safely in the transition to
 	 * write-through mode.
+	 *
+	 * When a stripe is syncing, the write is also handled in write
+	 * through mode.
 	 */
-	if (s->failed) {
+	if (s->failed || test_bit(STRIPE_SYNCING, &sh->state)) {
 		r5c_make_stripe_write_out(sh);
 		return -EAGAIN;
 	}
@@ -2825,6 +2844,9 @@ void r5c_finish_stripe_write_out(struct r5conf *conf,
 	}
 
 	r5l_append_flush_payload(log, sh->sector);
+	/* stripe is flused to raid disks, we can do resync now */
+	if (test_bit(STRIPE_SYNC_REQUESTED, &sh->state))
+		set_bit(STRIPE_HANDLE, &sh->state);
 }
 
 int r5c_cache_data(struct r5l_log *log, struct stripe_head *sh)
@@ -2973,7 +2995,7 @@ ioerr:
 	return ret;
 }
 
-void r5c_update_on_rdev_error(struct mddev *mddev)
+void r5c_update_on_rdev_error(struct mddev *mddev, struct md_rdev *rdev)
 {
 	struct r5conf *conf = mddev->private;
 	struct r5l_log *log = conf->log;
@@ -2981,7 +3003,8 @@ void r5c_update_on_rdev_error(struct mddev *mddev)
 	if (!log)
 		return;
 
-	if (raid5_calc_degraded(conf) > 0 &&
+	if ((raid5_calc_degraded(conf) > 0 ||
+	     test_bit(Journal, &rdev->flags)) &&
 	    conf->log->r5c_journal_mode == R5C_JOURNAL_MODE_WRITE_BACK)
 		schedule_work(&log->disable_writeback_work);
 }
diff --git a/drivers/md/raid5-log.h b/drivers/md/raid5-log.h
index 27097101ccca..328d67aedda4 100644
--- a/drivers/md/raid5-log.h
+++ b/drivers/md/raid5-log.h
@@ -28,7 +28,8 @@ extern void r5c_flush_cache(struct r5conf *conf, int num);
 extern void r5c_check_stripe_cache_usage(struct r5conf *conf);
 extern void r5c_check_cached_full_stripe(struct r5conf *conf);
 extern struct md_sysfs_entry r5c_journal_mode;
-extern void r5c_update_on_rdev_error(struct mddev *mddev);
+extern void r5c_update_on_rdev_error(struct mddev *mddev,
+				     struct md_rdev *rdev);
 extern bool r5c_big_stripe_cached(struct r5conf *conf, sector_t sect);
 
 extern struct dma_async_tx_descriptor *
diff --git a/drivers/md/raid5.c b/drivers/md/raid5.c
index 2e38cfac5b1d..9c4f7659f8b1 100644
--- a/drivers/md/raid5.c
+++ b/drivers/md/raid5.c
@@ -103,8 +103,7 @@ static inline void unlock_device_hash_lock(struct r5conf *conf, int hash)
 static inline void lock_all_device_hash_locks_irq(struct r5conf *conf)
 {
 	int i;
-	local_irq_disable();
-	spin_lock(conf->hash_locks);
+	spin_lock_irq(conf->hash_locks);
 	for (i = 1; i < NR_STRIPE_HASH_LOCKS; i++)
 		spin_lock_nest_lock(conf->hash_locks + i, conf->hash_locks);
 	spin_lock(&conf->device_lock);
@@ -114,9 +113,9 @@ static inline void unlock_all_device_hash_locks_irq(struct r5conf *conf)
 {
 	int i;
 	spin_unlock(&conf->device_lock);
-	for (i = NR_STRIPE_HASH_LOCKS; i; i--)
-		spin_unlock(conf->hash_locks + i - 1);
-	local_irq_enable();
+	for (i = NR_STRIPE_HASH_LOCKS - 1; i; i--)
+		spin_unlock(conf->hash_locks + i);
+	spin_unlock_irq(conf->hash_locks);
 }
 
 /* Find first data disk in a raid6 stripe */
@@ -234,11 +233,15 @@ static void do_release_stripe(struct r5conf *conf, struct stripe_head *sh,
 			if (test_bit(R5_InJournal, &sh->dev[i].flags))
 				injournal++;
 	/*
-	 * When quiesce in r5c write back, set STRIPE_HANDLE for stripes with
-	 * data in journal, so they are not released to cached lists
+	 * In the following cases, the stripe cannot be released to cached
+	 * lists. Therefore, we make the stripe write out and set
+	 * STRIPE_HANDLE:
+	 *   1. when quiesce in r5c write back;
+	 *   2. when resync is requested fot the stripe.
 	 */
-	if (conf->quiesce && r5c_is_writeback(conf->log) &&
-	    !test_bit(STRIPE_HANDLE, &sh->state) && injournal != 0) {
+	if (test_bit(STRIPE_SYNC_REQUESTED, &sh->state) ||
+	    (conf->quiesce && r5c_is_writeback(conf->log) &&
+	     !test_bit(STRIPE_HANDLE, &sh->state) && injournal != 0)) {
 		if (test_bit(STRIPE_R5C_CACHING, &sh->state))
 			r5c_make_stripe_write_out(sh);
 		set_bit(STRIPE_HANDLE, &sh->state);
@@ -714,12 +717,11 @@ static bool is_full_stripe_write(struct stripe_head *sh)
 
 static void lock_two_stripes(struct stripe_head *sh1, struct stripe_head *sh2)
 {
-	local_irq_disable();
 	if (sh1 > sh2) {
-		spin_lock(&sh2->stripe_lock);
+		spin_lock_irq(&sh2->stripe_lock);
 		spin_lock_nested(&sh1->stripe_lock, 1);
 	} else {
-		spin_lock(&sh1->stripe_lock);
+		spin_lock_irq(&sh1->stripe_lock);
 		spin_lock_nested(&sh2->stripe_lock, 1);
 	}
 }
@@ -727,8 +729,7 @@ static void lock_two_stripes(struct stripe_head *sh1, struct stripe_head *sh2)
 static void unlock_two_stripes(struct stripe_head *sh1, struct stripe_head *sh2)
 {
 	spin_unlock(&sh1->stripe_lock);
-	spin_unlock(&sh2->stripe_lock);
-	local_irq_enable();
+	spin_unlock_irq(&sh2->stripe_lock);
 }
 
 /* Only freshly new full stripe normal write stripe can be added to a batch list */
@@ -2312,14 +2313,12 @@ static int resize_stripes(struct r5conf *conf, int newsize)
 	struct stripe_head *osh, *nsh;
 	LIST_HEAD(newstripes);
 	struct disk_info *ndisks;
-	int err;
+	int err = 0;
 	struct kmem_cache *sc;
 	int i;
 	int hash, cnt;
 
-	err = md_allow_write(conf->mddev);
-	if (err)
-		return err;
+	md_allow_write(conf->mddev);
 
 	/* Step 1 */
 	sc = kmem_cache_create(conf->cache_name[1-conf->active_name],
@@ -2694,7 +2693,7 @@ static void raid5_error(struct mddev *mddev, struct md_rdev *rdev)
 		bdevname(rdev->bdev, b),
 		mdname(mddev),
 		conf->raid_disks - mddev->degraded);
-	r5c_update_on_rdev_error(mddev);
+	r5c_update_on_rdev_error(mddev, rdev);
 }
 
 /*
@@ -3055,6 +3054,11 @@ sector_t raid5_compute_blocknr(struct stripe_head *sh, int i, int previous)
  *      When LOG_CRITICAL, stripes with injournal == 0 will be sent to
  *      no_space_stripes list.
  *
+ *   3. during journal failure
+ *      In journal failure, we try to flush all cached data to raid disks
+ *      based on data in stripe cache. The array is read-only to upper
+ *      layers, so we would skip all pending writes.
+ *
  */
 static inline bool delay_towrite(struct r5conf *conf,
 				 struct r5dev *dev,
@@ -3068,6 +3072,9 @@ static inline bool delay_towrite(struct r5conf *conf,
 	if (test_bit(R5C_LOG_CRITICAL, &conf->cache_state) &&
 	    s->injournal > 0)
 		return true;
+	/* case 3 above */
+	if (s->log_failed && s->injournal)
+		return true;
 	return false;
 }
 
@@ -4653,8 +4660,13 @@ static void handle_stripe(struct stripe_head *sh)
 
 	if (test_bit(STRIPE_SYNC_REQUESTED, &sh->state) && !sh->batch_head) {
 		spin_lock(&sh->stripe_lock);
-		/* Cannot process 'sync' concurrently with 'discard' */
-		if (!test_bit(STRIPE_DISCARD, &sh->state) &&
+		/*
+		 * Cannot process 'sync' concurrently with 'discard'.
+		 * Flush data in r5cache before 'sync'.
+		 */
+		if (!test_bit(STRIPE_R5C_PARTIAL_STRIPE, &sh->state) &&
+		    !test_bit(STRIPE_R5C_FULL_STRIPE, &sh->state) &&
+		    !test_bit(STRIPE_DISCARD, &sh->state) &&
 		    test_and_clear_bit(STRIPE_SYNC_REQUESTED, &sh->state)) {
 			set_bit(STRIPE_SYNCING, &sh->state);
 			clear_bit(STRIPE_INSYNC, &sh->state);
@@ -4701,10 +4713,15 @@ static void handle_stripe(struct stripe_head *sh)
 	       " to_write=%d failed=%d failed_num=%d,%d\n",
 	       s.locked, s.uptodate, s.to_read, s.to_write, s.failed,
 	       s.failed_num[0], s.failed_num[1]);
-	/* check if the array has lost more than max_degraded devices and,
+	/*
+	 * check if the array has lost more than max_degraded devices and,
 	 * if so, some requests might need to be failed.
+	 *
+	 * When journal device failed (log_failed), we will only process
+	 * the stripe if there is data need write to raid disks
 	 */
-	if (s.failed > conf->max_degraded || s.log_failed) {
+	if (s.failed > conf->max_degraded ||
+	    (s.log_failed && s.injournal == 0)) {
 		sh->check_state = 0;
 		sh->reconstruct_state = 0;
 		break_stripe_batch_list(sh, 0);
@@ -5277,8 +5294,10 @@ static struct stripe_head *__get_priority_stripe(struct r5conf *conf, int group)
 	struct stripe_head *sh, *tmp;
 	struct list_head *handle_list = NULL;
 	struct r5worker_group *wg;
-	bool second_try = !r5c_is_writeback(conf->log);
-	bool try_loprio = test_bit(R5C_LOG_TIGHT, &conf->cache_state);
+	bool second_try = !r5c_is_writeback(conf->log) &&
+		!r5l_log_disk_error(conf);
+	bool try_loprio = test_bit(R5C_LOG_TIGHT, &conf->cache_state) ||
+		r5l_log_disk_error(conf);
 
 again:
 	wg = NULL;
@@ -6313,7 +6332,6 @@ int
 raid5_set_cache_size(struct mddev *mddev, int size)
 {
 	struct r5conf *conf = mddev->private;
-	int err;
 
 	if (size <= 16 || size > 32768)
 		return -EINVAL;
@@ -6325,10 +6343,7 @@ raid5_set_cache_size(struct mddev *mddev, int size)
 		;
 	mutex_unlock(&conf->cache_size_mutex);
 
-
-	err = md_allow_write(mddev);
-	if (err)
-		return err;
+	md_allow_write(mddev);
 
 	mutex_lock(&conf->cache_size_mutex);
 	while (size > conf->max_nr_stripes)
@@ -7530,7 +7545,9 @@ static int raid5_remove_disk(struct mddev *mddev, struct md_rdev *rdev)
 		 * neilb: there is no locking about new writes here,
 		 * so this cannot be safe.
 		 */
-		if (atomic_read(&conf->active_stripes)) {
+		if (atomic_read(&conf->active_stripes) ||
+		    atomic_read(&conf->r5c_cached_full_stripes) ||
+		    atomic_read(&conf->r5c_cached_partial_stripes)) {
 			return -EBUSY;
 		}
 		log_exit(conf);
diff --git a/drivers/media/cec/Kconfig b/drivers/media/cec/Kconfig
index f944d93e3167..488fb908244d 100644
--- a/drivers/media/cec/Kconfig
+++ b/drivers/media/cec/Kconfig
@@ -9,6 +9,7 @@ config MEDIA_CEC_NOTIFIER
 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/dvb-frontends/af9013.c b/drivers/media/dvb-frontends/af9013.c
index b978002af4d8..51f942fc0669 100644
--- a/drivers/media/dvb-frontends/af9013.c
+++ b/drivers/media/dvb-frontends/af9013.c
@@ -535,6 +535,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++;
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/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/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/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/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/tc358743.c b/drivers/media/i2c/tc358743.c
index acef4eca269f..3251cba89e8f 100644
--- a/drivers/media/i2c/tc358743.c
+++ b/drivers/media/i2c/tc358743.c
@@ -223,7 +223,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)
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/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-video.c b/drivers/media/pci/cx88/cx88-video.c
index c7d4e87ccb64..36b4fb908e31 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");
 	}
diff --git a/drivers/media/pci/ddbridge/ddbridge-core.c b/drivers/media/pci/ddbridge/ddbridge-core.c
index 340cff02dee2..09b5338f4f8b 100644
--- a/drivers/media/pci/ddbridge/ddbridge-core.c
+++ b/drivers/media/pci/ddbridge/ddbridge-core.c
@@ -789,9 +789,10 @@ static void dvb_input_detach(struct ddb_input *input)
 			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 +800,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);
 	}
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-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-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/platform/exynos4-is/fimc-capture.c b/drivers/media/platform/exynos4-is/fimc-capture.c
index 8a7cd07dbe28..db60a63c0768 100644
--- a/drivers/media/platform/exynos4-is/fimc-capture.c
+++ b/drivers/media/platform/exynos4-is/fimc-capture.c
@@ -1277,6 +1277,7 @@ static int fimc_cap_g_selection(struct file *file, void *fh,
 	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;
@@ -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/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-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/pxa_camera.c b/drivers/media/platform/pxa_camera.c
index 929006f65cc7..5d3f04210a0b 100644
--- a/drivers/media/platform/pxa_camera.c
+++ b/drivers/media/platform/pxa_camera.c
@@ -1291,6 +1291,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:
@@ -2177,6 +2178,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;
 
@@ -2501,7 +2508,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/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..1da2c94e1dca 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;
 
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/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/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..8f0669c9894c 100644
--- a/drivers/media/rc/ir-lirc-codec.c
+++ b/drivers/media/rc/ir-lirc-codec.c
@@ -354,7 +354,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 +361,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 +395,12 @@ static int ir_lirc_register(struct rc_dev *dev)
 	drv->minor = -1;
 	drv->features = features;
 	drv->data = &dev->raw->lirc;
-	drv->rbuf = rbuf;
+	drv->rbuf = NULL;
 	drv->set_use_inc = &ir_lirc_open;
 	drv->set_use_dec = &ir_lirc_close;
 	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 +409,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 +426,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/lirc_dev.c b/drivers/media/rc/lirc_dev.c
index 8d60c9f00df9..42704552b005 100644
--- a/drivers/media/rc/lirc_dev.c
+++ b/drivers/media/rc/lirc_dev.c
@@ -52,6 +52,7 @@ struct irctl {
 
 	struct mutex irctl_lock;
 	struct lirc_buffer *buf;
+	bool buf_internal;
 	unsigned int chunk_size;
 
 	struct device dev;
@@ -83,7 +84,7 @@ static void lirc_release(struct device *ld)
 
 	put_device(ir->dev.parent);
 
-	if (ir->buf != ir->d.rbuf) {
+	if (ir->buf_internal) {
 		lirc_buffer_free(ir->buf);
 		kfree(ir->buf);
 	}
@@ -198,6 +199,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,8 +210,11 @@ 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;
 	}
 	ir->chunk_size = ir->buf->chunk_size;
 
@@ -362,6 +367,12 @@ int lirc_register_driver(struct lirc_driver *d)
 		err = lirc_allocate_buffer(irctls[minor]);
 		if (err)
 			lirc_unregister_driver(minor);
+		else
+			/*
+			 * This is kind of a hack but ir-lirc-codec needs
+			 * access to the buffer that lirc_dev allocated.
+			 */
+			d->rbuf = irctls[minor]->buf;
 	}
 
 	return err ? err : minor;
diff --git a/drivers/media/rc/rc-main.c b/drivers/media/rc/rc-main.c
index 6ec73357fa47..802e559cc30e 100644
--- a/drivers/media/rc/rc-main.c
+++ b/drivers/media/rc/rc-main.c
@@ -1703,6 +1703,16 @@ 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;
+
+	/* rc_open will be called here */
+	rc = input_register_device(dev->input_dev);
+	if (rc)
+		goto out_table;
+
 	/*
 	 * Default delay of 250ms is too short for some protocols, especially
 	 * since the timeout is currently set to 250ms. Increase it to 500ms,
@@ -1718,16 +1728,6 @@ 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:
diff --git a/drivers/media/rc/sir_ir.c b/drivers/media/rc/sir_ir.c
index e12ec50bf0bf..90a5f8fd5eea 100644
--- a/drivers/media/rc/sir_ir.c
+++ b/drivers/media/rc/sir_ir.c
@@ -183,9 +183,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);
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..afdf0c9903d2 100644
--- a/drivers/media/tuners/xc5000.c
+++ b/drivers/media/tuners/xc5000.c
@@ -788,6 +788,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/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/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/dvb-usb-v2/af9015.c b/drivers/media/usb/dvb-usb-v2/af9015.c
index caa1e6101f58..612431ab0fb3 100644
--- a/drivers/media/usb/dvb-usb-v2/af9015.c
+++ b/drivers/media/usb/dvb-usb-v2/af9015.c
@@ -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;
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/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/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/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 541ca543f71f..8d3ca2c8b20f 100644
--- a/drivers/media/usb/rainshadow-cec/rainshadow-cec.c
+++ b/drivers/media/usb/rainshadow-cec/rainshadow-cec.c
@@ -123,11 +123,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 +297,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 +337,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/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/uvc/uvc_video.c b/drivers/media/usb/uvc/uvc_video.c
index 47d93a938dde..a29f39d4e05b 100644
--- a/drivers/media/usb/uvc/uvc_video.c
+++ b/drivers/media/usb/uvc/uvc_video.c
@@ -1327,7 +1327,7 @@ static void uvc_video_complete(struct urb *urb)
 	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/videobuf2-core.c b/drivers/media/v4l2-core/videobuf2-core.c
index 94afbbf92807..c0175ea7e7ad 100644
--- a/drivers/media/v4l2-core/videobuf2-core.c
+++ b/drivers/media/v4l2-core/videobuf2-core.c
@@ -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);
diff --git a/drivers/memory/atmel-ebi.c b/drivers/memory/atmel-ebi.c
index 35910f945bfa..99e644cda4d1 100644
--- a/drivers/memory/atmel-ebi.c
+++ b/drivers/memory/atmel-ebi.c
@@ -581,7 +581,7 @@ static int atmel_ebi_probe(struct platform_device *pdev)
 	return of_platform_populate(np, NULL, NULL, dev);
 }
 
-static int atmel_ebi_resume(struct device *dev)
+static __maybe_unused int atmel_ebi_resume(struct device *dev)
 {
 	struct atmel_ebi *ebi = dev_get_drvdata(dev);
 	struct atmel_ebi_dev *ebid;
diff --git a/drivers/memory/omap-gpmc.c b/drivers/memory/omap-gpmc.c
index bf0fe0137dfe..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 */" : "");
 	}
 
@@ -512,7 +512,7 @@ static void gpmc_cs_show_timings(int cs, const char *desc)
 	pr_info("gpmc cs%i access configuration:\n", cs);
 	GPMC_GET_RAW_BOOL(GPMC_CS_CONFIG1,  4,  4, "time-para-granularity");
 	GPMC_GET_RAW(GPMC_CS_CONFIG1,  8,  9, "mux-add-data");
-	GPMC_GET_RAW_MAX(GPMC_CS_CONFIG1, 12, 13,
+	GPMC_GET_RAW_SHIFT_MAX(GPMC_CS_CONFIG1, 12, 13, 1,
 			 GPMC_CONFIG1_DEVICESIZE_MAX, "device-width");
 	GPMC_GET_RAW(GPMC_CS_CONFIG1, 16, 17, "wait-pin");
 	GPMC_GET_RAW_BOOL(GPMC_CS_CONFIG1, 21, 21, "wait-on-write");
@@ -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/misc/Kconfig b/drivers/misc/Kconfig
index 2cba76e6fa3c..07bbd4cc1852 100644
--- a/drivers/misc/Kconfig
+++ b/drivers/misc/Kconfig
@@ -492,6 +492,7 @@ config ASPEED_LPC_CTRL
 
 config PCI_ENDPOINT_TEST
 	depends on PCI
+	select CRC32
 	tristate "PCI Endpoint Test driver"
 	---help---
            Enable this configuration option to enable the host side test driver
diff --git a/drivers/mmc/core/pwrseq_simple.c b/drivers/mmc/core/pwrseq_simple.c
index 1304160de168..13ef162cf066 100644
--- a/drivers/mmc/core/pwrseq_simple.c
+++ b/drivers/mmc/core/pwrseq_simple.c
@@ -27,6 +27,7 @@ struct mmc_pwrseq_simple {
 	struct mmc_pwrseq pwrseq;
 	bool clk_enabled;
 	u32 post_power_on_delay_ms;
+	u32 power_off_delay_us;
 	struct clk *ext_clk;
 	struct gpio_descs *reset_gpios;
 };
@@ -78,6 +79,10 @@ static void mmc_pwrseq_simple_power_off(struct mmc_host *host)
 
 	mmc_pwrseq_simple_set_gpios_value(pwrseq, 1);
 
+	if (pwrseq->power_off_delay_us)
+		usleep_range(pwrseq->power_off_delay_us,
+			2 * pwrseq->power_off_delay_us);
+
 	if (!IS_ERR(pwrseq->ext_clk) && pwrseq->clk_enabled) {
 		clk_disable_unprepare(pwrseq->ext_clk);
 		pwrseq->clk_enabled = false;
@@ -119,6 +124,8 @@ static int mmc_pwrseq_simple_probe(struct platform_device *pdev)
 
 	device_property_read_u32(dev, "post-power-on-delay-ms",
 				 &pwrseq->post_power_on_delay_ms);
+	device_property_read_u32(dev, "power-off-delay-us",
+				 &pwrseq->power_off_delay_us);
 
 	pwrseq->pwrseq.dev = dev;
 	pwrseq->pwrseq.ops = &mmc_pwrseq_simple_ops;
diff --git a/drivers/mmc/host/cavium-octeon.c b/drivers/mmc/host/cavium-octeon.c
index 772d0900026d..951d2cdd7888 100644
--- a/drivers/mmc/host/cavium-octeon.c
+++ b/drivers/mmc/host/cavium-octeon.c
@@ -108,7 +108,7 @@ static void octeon_mmc_release_bus(struct cvm_mmc_host *host)
 static void octeon_mmc_int_enable(struct cvm_mmc_host *host, u64 val)
 {
 	writeq(val, host->base + MIO_EMM_INT(host));
-	if (!host->dma_active || (host->dma_active && !host->has_ciu3))
+	if (!host->has_ciu3)
 		writeq(val, host->base + MIO_EMM_INT_EN(host));
 }
 
@@ -267,7 +267,7 @@ static int octeon_mmc_probe(struct platform_device *pdev)
 	}
 
 	host->global_pwr_gpiod = devm_gpiod_get_optional(&pdev->dev,
-							 "power-gpios",
+							 "power",
 							 GPIOD_OUT_HIGH);
 	if (IS_ERR(host->global_pwr_gpiod)) {
 		dev_err(&pdev->dev, "Invalid power GPIO\n");
@@ -288,11 +288,20 @@ static int octeon_mmc_probe(struct platform_device *pdev)
 		if (ret) {
 			dev_err(&pdev->dev, "Error populating slots\n");
 			octeon_mmc_set_shared_power(host, 0);
-			return ret;
+			goto error;
 		}
 		i++;
 	}
 	return 0;
+
+error:
+	for (i = 0; i < CAVIUM_MAX_MMC; i++) {
+		if (host->slot[i])
+			cvm_mmc_of_slot_remove(host->slot[i]);
+		if (host->slot_pdev[i])
+			of_platform_device_destroy(&host->slot_pdev[i]->dev, NULL);
+	}
+	return ret;
 }
 
 static int octeon_mmc_remove(struct platform_device *pdev)
diff --git a/drivers/mmc/host/cavium-thunderx.c b/drivers/mmc/host/cavium-thunderx.c
index fe3d77267cd6..b9cc95998799 100644
--- a/drivers/mmc/host/cavium-thunderx.c
+++ b/drivers/mmc/host/cavium-thunderx.c
@@ -146,6 +146,12 @@ static int thunder_mmc_probe(struct pci_dev *pdev,
 	return 0;
 
 error:
+	for (i = 0; i < CAVIUM_MAX_MMC; i++) {
+		if (host->slot[i])
+			cvm_mmc_of_slot_remove(host->slot[i]);
+		if (host->slot_pdev[i])
+			of_platform_device_destroy(&host->slot_pdev[i]->dev, NULL);
+	}
 	clk_disable_unprepare(host->clk);
 	return ret;
 }
diff --git a/drivers/mmc/host/cavium.c b/drivers/mmc/host/cavium.c
index 58b51ba6aabd..b8aaf0fdb77c 100644
--- a/drivers/mmc/host/cavium.c
+++ b/drivers/mmc/host/cavium.c
@@ -839,14 +839,14 @@ static void cvm_mmc_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 		cvm_mmc_reset_bus(slot);
 		if (host->global_pwr_gpiod)
 			host->set_shared_power(host, 0);
-		else
+		else if (!IS_ERR(mmc->supply.vmmc))
 			mmc_regulator_set_ocr(mmc, mmc->supply.vmmc, 0);
 		break;
 
 	case MMC_POWER_UP:
 		if (host->global_pwr_gpiod)
 			host->set_shared_power(host, 1);
-		else
+		else if (!IS_ERR(mmc->supply.vmmc))
 			mmc_regulator_set_ocr(mmc, mmc->supply.vmmc, ios->vdd);
 		break;
 	}
@@ -968,20 +968,15 @@ static int cvm_mmc_of_parse(struct device *dev, struct cvm_mmc_slot *slot)
 		return -EINVAL;
 	}
 
-	mmc->supply.vmmc = devm_regulator_get_optional(dev, "vmmc");
-	if (IS_ERR(mmc->supply.vmmc)) {
-		if (PTR_ERR(mmc->supply.vmmc) == -EPROBE_DEFER)
-			return -EPROBE_DEFER;
-		/*
-		 * Legacy Octeon firmware has no regulator entry, fall-back to
-		 * a hard-coded voltage to get a sane OCR.
-		 */
+	ret = mmc_regulator_get_supply(mmc);
+	if (ret == -EPROBE_DEFER)
+		return ret;
+	/*
+	 * Legacy Octeon firmware has no regulator entry, fall-back to
+	 * a hard-coded voltage to get a sane OCR.
+	 */
+	if (IS_ERR(mmc->supply.vmmc))
 		mmc->ocr_avail = MMC_VDD_32_33 | MMC_VDD_33_34;
-	} else {
-		ret = mmc_regulator_get_ocrmask(mmc->supply.vmmc);
-		if (ret > 0)
-			mmc->ocr_avail = ret;
-	}
 
 	/* Common MMC bindings */
 	ret = mmc_of_parse(mmc);
diff --git a/drivers/mmc/host/sdhci-iproc.c b/drivers/mmc/host/sdhci-iproc.c
index 3275d4995812..61666d269771 100644
--- a/drivers/mmc/host/sdhci-iproc.c
+++ b/drivers/mmc/host/sdhci-iproc.c
@@ -187,7 +187,8 @@ static const struct sdhci_iproc_data iproc_cygnus_data = {
 };
 
 static const struct sdhci_pltfm_data sdhci_iproc_pltfm_data = {
-	.quirks = SDHCI_QUIRK_DATA_TIMEOUT_USES_SDCLK,
+	.quirks = SDHCI_QUIRK_DATA_TIMEOUT_USES_SDCLK |
+		  SDHCI_QUIRK_MULTIBLOCK_READ_ACMD12,
 	.quirks2 = SDHCI_QUIRK2_ACMD23_BROKEN,
 	.ops = &sdhci_iproc_ops,
 };
diff --git a/drivers/mmc/host/sdhci-xenon-phy.c b/drivers/mmc/host/sdhci-xenon-phy.c
index 6356781f1cca..f7e26b031e76 100644
--- a/drivers/mmc/host/sdhci-xenon-phy.c
+++ b/drivers/mmc/host/sdhci-xenon-phy.c
@@ -787,14 +787,6 @@ int xenon_phy_adj(struct sdhci_host *host, struct mmc_ios *ios)
 	return ret;
 }
 
-void xenon_clean_phy(struct sdhci_host *host)
-{
-	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
-	struct xenon_priv *priv = sdhci_pltfm_priv(pltfm_host);
-
-	kfree(priv->phy_params);
-}
-
 static int xenon_add_phy(struct device_node *np, struct sdhci_host *host,
 			 const char *phy_name)
 {
@@ -819,11 +811,7 @@ static int xenon_add_phy(struct device_node *np, struct sdhci_host *host,
 	if (ret)
 		return ret;
 
-	ret = xenon_emmc_phy_parse_param_dt(host, np, priv->phy_params);
-	if (ret)
-		xenon_clean_phy(host);
-
-	return ret;
+	return xenon_emmc_phy_parse_param_dt(host, np, priv->phy_params);
 }
 
 int xenon_phy_parse_dt(struct device_node *np, struct sdhci_host *host)
diff --git a/drivers/mmc/host/sdhci-xenon.c b/drivers/mmc/host/sdhci-xenon.c
index 67246655315b..bc1781bb070b 100644
--- a/drivers/mmc/host/sdhci-xenon.c
+++ b/drivers/mmc/host/sdhci-xenon.c
@@ -486,7 +486,7 @@ static int xenon_probe(struct platform_device *pdev)
 
 	err = xenon_sdhc_prepare(host);
 	if (err)
-		goto clean_phy_param;
+		goto err_clk;
 
 	err = sdhci_add_host(host);
 	if (err)
@@ -496,8 +496,6 @@ static int xenon_probe(struct platform_device *pdev)
 
 remove_sdhc:
 	xenon_sdhc_unprepare(host);
-clean_phy_param:
-	xenon_clean_phy(host);
 err_clk:
 	clk_disable_unprepare(pltfm_host->clk);
 free_pltfm:
@@ -510,8 +508,6 @@ static int xenon_remove(struct platform_device *pdev)
 	struct sdhci_host *host = platform_get_drvdata(pdev);
 	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
 
-	xenon_clean_phy(host);
-
 	sdhci_remove_host(host, 0);
 
 	xenon_sdhc_unprepare(host);
diff --git a/drivers/mmc/host/sdhci-xenon.h b/drivers/mmc/host/sdhci-xenon.h
index 6e6523ea01ce..73debb42dc2f 100644
--- a/drivers/mmc/host/sdhci-xenon.h
+++ b/drivers/mmc/host/sdhci-xenon.h
@@ -93,7 +93,6 @@ struct xenon_priv {
 };
 
 int xenon_phy_adj(struct sdhci_host *host, struct mmc_ios *ios);
-void xenon_clean_phy(struct sdhci_host *host);
 int xenon_phy_parse_dt(struct device_node *np,
 		       struct sdhci_host *host);
 void xenon_soc_pad_ctrl(struct sdhci_host *host,
diff --git a/drivers/mtd/chips/cfi_cmdset_0020.c b/drivers/mtd/chips/cfi_cmdset_0020.c
index 94d3eb42c4d5..7d342965f392 100644
--- a/drivers/mtd/chips/cfi_cmdset_0020.c
+++ b/drivers/mtd/chips/cfi_cmdset_0020.c
@@ -666,7 +666,7 @@ cfi_staa_writev(struct mtd_info *mtd, const struct kvec *vecs,
 	size_t	 totlen = 0, thislen;
 	int	 ret = 0;
 	size_t	 buflen = 0;
-	static char *buffer;
+	char *buffer;
 
 	if (!ECCBUF_SIZE) {
 		/* We should fall back to a general writev implementation.
diff --git a/drivers/mtd/devices/Kconfig b/drivers/mtd/devices/Kconfig
index 58329d2dacd1..6def5445e03e 100644
--- a/drivers/mtd/devices/Kconfig
+++ b/drivers/mtd/devices/Kconfig
@@ -95,6 +95,16 @@ config MTD_M25P80
 	  if you want to specify device partitioning or to use a device which
 	  doesn't support the JEDEC ID instruction.
 
+config MTD_MCHP23K256
+	tristate "Microchip 23K256 SRAM"
+	depends on SPI_MASTER
+	help
+	  This enables access to Microchip 23K256 SRAM chips, using SPI.
+
+	  Set up your spi devices with the right board-specific
+	  platform data, or a device tree description if you want to
+	  specify device partitioning
+
 config MTD_SPEAR_SMI
 	tristate "SPEAR MTD NOR Support through SMI controller"
 	depends on PLAT_SPEAR
diff --git a/drivers/mtd/devices/Makefile b/drivers/mtd/devices/Makefile
index 7912d3a0ee34..f0f767624cc6 100644
--- a/drivers/mtd/devices/Makefile
+++ b/drivers/mtd/devices/Makefile
@@ -12,6 +12,7 @@ obj-$(CONFIG_MTD_LART)		+= lart.o
 obj-$(CONFIG_MTD_BLOCK2MTD)	+= block2mtd.o
 obj-$(CONFIG_MTD_DATAFLASH)	+= mtd_dataflash.o
 obj-$(CONFIG_MTD_M25P80)	+= m25p80.o
+obj-$(CONFIG_MTD_MCHP23K256)	+= mchp23k256.o
 obj-$(CONFIG_MTD_SPEAR_SMI)	+= spear_smi.o
 obj-$(CONFIG_MTD_SST25L)	+= sst25l.o
 obj-$(CONFIG_MTD_BCM47XXSFLASH)	+= bcm47xxsflash.o
diff --git a/drivers/mtd/devices/m25p80.c b/drivers/mtd/devices/m25p80.c
index c4df3b1bded0..00eea6fd379c 100644
--- a/drivers/mtd/devices/m25p80.c
+++ b/drivers/mtd/devices/m25p80.c
@@ -78,11 +78,17 @@ static ssize_t m25p80_write(struct spi_nor *nor, loff_t to, size_t len,
 {
 	struct m25p *flash = nor->priv;
 	struct spi_device *spi = flash->spi;
-	struct spi_transfer t[2] = {};
+	unsigned int inst_nbits, addr_nbits, data_nbits, data_idx;
+	struct spi_transfer t[3] = {};
 	struct spi_message m;
 	int cmd_sz = m25p_cmdsz(nor);
 	ssize_t ret;
 
+	/* get transfer protocols. */
+	inst_nbits = spi_nor_get_protocol_inst_nbits(nor->write_proto);
+	addr_nbits = spi_nor_get_protocol_addr_nbits(nor->write_proto);
+	data_nbits = spi_nor_get_protocol_data_nbits(nor->write_proto);
+
 	spi_message_init(&m);
 
 	if (nor->program_opcode == SPINOR_OP_AAI_WP && nor->sst_write_second)
@@ -92,12 +98,27 @@ static ssize_t m25p80_write(struct spi_nor *nor, loff_t to, size_t len,
 	m25p_addr2cmd(nor, to, flash->command);
 
 	t[0].tx_buf = flash->command;
+	t[0].tx_nbits = inst_nbits;
 	t[0].len = cmd_sz;
 	spi_message_add_tail(&t[0], &m);
 
-	t[1].tx_buf = buf;
-	t[1].len = len;
-	spi_message_add_tail(&t[1], &m);
+	/* split the op code and address bytes into two transfers if needed. */
+	data_idx = 1;
+	if (addr_nbits != inst_nbits) {
+		t[0].len = 1;
+
+		t[1].tx_buf = &flash->command[1];
+		t[1].tx_nbits = addr_nbits;
+		t[1].len = cmd_sz - 1;
+		spi_message_add_tail(&t[1], &m);
+
+		data_idx = 2;
+	}
+
+	t[data_idx].tx_buf = buf;
+	t[data_idx].tx_nbits = data_nbits;
+	t[data_idx].len = len;
+	spi_message_add_tail(&t[data_idx], &m);
 
 	ret = spi_sync(spi, &m);
 	if (ret)
@@ -109,18 +130,6 @@ static ssize_t m25p80_write(struct spi_nor *nor, loff_t to, size_t len,
 	return ret;
 }
 
-static inline unsigned int m25p80_rx_nbits(struct spi_nor *nor)
-{
-	switch (nor->flash_read) {
-	case SPI_NOR_DUAL:
-		return 2;
-	case SPI_NOR_QUAD:
-		return 4;
-	default:
-		return 0;
-	}
-}
-
 /*
  * Read an address range from the nor chip.  The address range
  * may be any size provided it is within the physical boundaries.
@@ -130,13 +139,20 @@ static ssize_t m25p80_read(struct spi_nor *nor, loff_t from, size_t len,
 {
 	struct m25p *flash = nor->priv;
 	struct spi_device *spi = flash->spi;
-	struct spi_transfer t[2];
+	unsigned int inst_nbits, addr_nbits, data_nbits, data_idx;
+	struct spi_transfer t[3];
 	struct spi_message m;
 	unsigned int dummy = nor->read_dummy;
 	ssize_t ret;
+	int cmd_sz;
+
+	/* get transfer protocols. */
+	inst_nbits = spi_nor_get_protocol_inst_nbits(nor->read_proto);
+	addr_nbits = spi_nor_get_protocol_addr_nbits(nor->read_proto);
+	data_nbits = spi_nor_get_protocol_data_nbits(nor->read_proto);
 
 	/* convert the dummy cycles to the number of bytes */
-	dummy /= 8;
+	dummy = (dummy * addr_nbits) / 8;
 
 	if (spi_flash_read_supported(spi)) {
 		struct spi_flash_read_message msg;
@@ -149,10 +165,9 @@ static ssize_t m25p80_read(struct spi_nor *nor, loff_t from, size_t len,
 		msg.read_opcode = nor->read_opcode;
 		msg.addr_width = nor->addr_width;
 		msg.dummy_bytes = dummy;
-		/* TODO: Support other combinations */
-		msg.opcode_nbits = SPI_NBITS_SINGLE;
-		msg.addr_nbits = SPI_NBITS_SINGLE;
-		msg.data_nbits = m25p80_rx_nbits(nor);
+		msg.opcode_nbits = inst_nbits;
+		msg.addr_nbits = addr_nbits;
+		msg.data_nbits = data_nbits;
 
 		ret = spi_flash_read(spi, &msg);
 		if (ret < 0)
@@ -167,20 +182,45 @@ static ssize_t m25p80_read(struct spi_nor *nor, loff_t from, size_t len,
 	m25p_addr2cmd(nor, from, flash->command);
 
 	t[0].tx_buf = flash->command;
+	t[0].tx_nbits = inst_nbits;
 	t[0].len = m25p_cmdsz(nor) + dummy;
 	spi_message_add_tail(&t[0], &m);
 
-	t[1].rx_buf = buf;
-	t[1].rx_nbits = m25p80_rx_nbits(nor);
-	t[1].len = min3(len, spi_max_transfer_size(spi),
-			spi_max_message_size(spi) - t[0].len);
-	spi_message_add_tail(&t[1], &m);
+	/*
+	 * Set all dummy/mode cycle bits to avoid sending some manufacturer
+	 * specific pattern, which might make the memory enter its Continuous
+	 * Read mode by mistake.
+	 * Based on the different mode cycle bit patterns listed and described
+	 * in the JESD216B specification, the 0xff value works for all memories
+	 * and all manufacturers.
+	 */
+	cmd_sz = t[0].len;
+	memset(flash->command + cmd_sz - dummy, 0xff, dummy);
+
+	/* split the op code and address bytes into two transfers if needed. */
+	data_idx = 1;
+	if (addr_nbits != inst_nbits) {
+		t[0].len = 1;
+
+		t[1].tx_buf = &flash->command[1];
+		t[1].tx_nbits = addr_nbits;
+		t[1].len = cmd_sz - 1;
+		spi_message_add_tail(&t[1], &m);
+
+		data_idx = 2;
+	}
+
+	t[data_idx].rx_buf = buf;
+	t[data_idx].rx_nbits = data_nbits;
+	t[data_idx].len = min3(len, spi_max_transfer_size(spi),
+			       spi_max_message_size(spi) - cmd_sz);
+	spi_message_add_tail(&t[data_idx], &m);
 
 	ret = spi_sync(spi, &m);
 	if (ret)
 		return ret;
 
-	ret = m.actual_length - m25p_cmdsz(nor) - dummy;
+	ret = m.actual_length - cmd_sz;
 	if (ret < 0)
 		return -EIO;
 	return ret;
@@ -196,7 +236,11 @@ static int m25p_probe(struct spi_device *spi)
 	struct flash_platform_data	*data;
 	struct m25p *flash;
 	struct spi_nor *nor;
-	enum read_mode mode = SPI_NOR_NORMAL;
+	struct spi_nor_hwcaps hwcaps = {
+		.mask = SNOR_HWCAPS_READ |
+			SNOR_HWCAPS_READ_FAST |
+			SNOR_HWCAPS_PP,
+	};
 	char *flash_name;
 	int ret;
 
@@ -221,10 +265,19 @@ static int m25p_probe(struct spi_device *spi)
 	spi_set_drvdata(spi, flash);
 	flash->spi = spi;
 
-	if (spi->mode & SPI_RX_QUAD)
-		mode = SPI_NOR_QUAD;
-	else if (spi->mode & SPI_RX_DUAL)
-		mode = SPI_NOR_DUAL;
+	if (spi->mode & SPI_RX_QUAD) {
+		hwcaps.mask |= SNOR_HWCAPS_READ_1_1_4;
+
+		if (spi->mode & SPI_TX_QUAD)
+			hwcaps.mask |= (SNOR_HWCAPS_READ_1_4_4 |
+					SNOR_HWCAPS_PP_1_1_4 |
+					SNOR_HWCAPS_PP_1_4_4);
+	} else if (spi->mode & SPI_RX_DUAL) {
+		hwcaps.mask |= SNOR_HWCAPS_READ_1_1_2;
+
+		if (spi->mode & SPI_TX_DUAL)
+			hwcaps.mask |= SNOR_HWCAPS_READ_1_2_2;
+	}
 
 	if (data && data->name)
 		nor->mtd.name = data->name;
@@ -241,7 +294,7 @@ static int m25p_probe(struct spi_device *spi)
 	else
 		flash_name = spi->modalias;
 
-	ret = spi_nor_scan(nor, flash_name, mode);
+	ret = spi_nor_scan(nor, flash_name, &hwcaps);
 	if (ret)
 		return ret;
 
diff --git a/drivers/mtd/devices/mchp23k256.c b/drivers/mtd/devices/mchp23k256.c
new file mode 100644
index 000000000000..e237db9f1bdb
--- /dev/null
+++ b/drivers/mtd/devices/mchp23k256.c
@@ -0,0 +1,182 @@
+/*
+ * mchp23k256.c
+ *
+ * Driver for Microchip 23k256 SPI RAM chips
+ *
+ * Copyright © 2016 Andrew Lunn <andrew@lunn.ch>
+ *
+ * This code is free software; you can 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/module.h>
+#include <linux/mtd/mtd.h>
+#include <linux/mtd/partitions.h>
+#include <linux/mutex.h>
+#include <linux/sched.h>
+#include <linux/sizes.h>
+#include <linux/spi/flash.h>
+#include <linux/spi/spi.h>
+
+struct mchp23k256_flash {
+	struct spi_device	*spi;
+	struct mutex		lock;
+	struct mtd_info		mtd;
+};
+
+#define MCHP23K256_CMD_WRITE_STATUS	0x01
+#define MCHP23K256_CMD_WRITE		0x02
+#define MCHP23K256_CMD_READ		0x03
+#define MCHP23K256_MODE_SEQ		BIT(6)
+
+#define to_mchp23k256_flash(x) container_of(x, struct mchp23k256_flash, mtd)
+
+static int mchp23k256_write(struct mtd_info *mtd, loff_t to, size_t len,
+			    size_t *retlen, const unsigned char *buf)
+{
+	struct mchp23k256_flash *flash = to_mchp23k256_flash(mtd);
+	struct spi_transfer transfer[2] = {};
+	struct spi_message message;
+	unsigned char command[3];
+
+	spi_message_init(&message);
+
+	command[0] = MCHP23K256_CMD_WRITE;
+	command[1] = to >> 8;
+	command[2] = to;
+
+	transfer[0].tx_buf = command;
+	transfer[0].len = sizeof(command);
+	spi_message_add_tail(&transfer[0], &message);
+
+	transfer[1].tx_buf = buf;
+	transfer[1].len = len;
+	spi_message_add_tail(&transfer[1], &message);
+
+	mutex_lock(&flash->lock);
+
+	spi_sync(flash->spi, &message);
+
+	if (retlen && message.actual_length > sizeof(command))
+		*retlen += message.actual_length - sizeof(command);
+
+	mutex_unlock(&flash->lock);
+	return 0;
+}
+
+static int mchp23k256_read(struct mtd_info *mtd, loff_t from, size_t len,
+			   size_t *retlen, unsigned char *buf)
+{
+	struct mchp23k256_flash *flash = to_mchp23k256_flash(mtd);
+	struct spi_transfer transfer[2] = {};
+	struct spi_message message;
+	unsigned char command[3];
+
+	spi_message_init(&message);
+
+	memset(&transfer, 0, sizeof(transfer));
+	command[0] = MCHP23K256_CMD_READ;
+	command[1] = from >> 8;
+	command[2] = from;
+
+	transfer[0].tx_buf = command;
+	transfer[0].len = sizeof(command);
+	spi_message_add_tail(&transfer[0], &message);
+
+	transfer[1].rx_buf = buf;
+	transfer[1].len = len;
+	spi_message_add_tail(&transfer[1], &message);
+
+	mutex_lock(&flash->lock);
+
+	spi_sync(flash->spi, &message);
+
+	if (retlen && message.actual_length > sizeof(command))
+		*retlen += message.actual_length - sizeof(command);
+
+	mutex_unlock(&flash->lock);
+	return 0;
+}
+
+/*
+ * Set the device into sequential mode. This allows read/writes to the
+ * entire SRAM in a single operation
+ */
+static int mchp23k256_set_mode(struct spi_device *spi)
+{
+	struct spi_transfer transfer = {};
+	struct spi_message message;
+	unsigned char command[2];
+
+	spi_message_init(&message);
+
+	command[0] = MCHP23K256_CMD_WRITE_STATUS;
+	command[1] = MCHP23K256_MODE_SEQ;
+
+	transfer.tx_buf = command;
+	transfer.len = sizeof(command);
+	spi_message_add_tail(&transfer, &message);
+
+	return spi_sync(spi, &message);
+}
+
+static int mchp23k256_probe(struct spi_device *spi)
+{
+	struct mchp23k256_flash *flash;
+	struct flash_platform_data *data;
+	int err;
+
+	flash = devm_kzalloc(&spi->dev, sizeof(*flash), GFP_KERNEL);
+	if (!flash)
+		return -ENOMEM;
+
+	flash->spi = spi;
+	mutex_init(&flash->lock);
+	spi_set_drvdata(spi, flash);
+
+	err = mchp23k256_set_mode(spi);
+	if (err)
+		return err;
+
+	data = dev_get_platdata(&spi->dev);
+
+	flash->mtd.dev.parent	= &spi->dev;
+	flash->mtd.type		= MTD_RAM;
+	flash->mtd.flags	= MTD_CAP_RAM;
+	flash->mtd.writesize	= 1;
+	flash->mtd.size		= SZ_32K;
+	flash->mtd._read	= mchp23k256_read;
+	flash->mtd._write	= mchp23k256_write;
+
+	err = mtd_device_parse_register(&flash->mtd, NULL, NULL,
+					data ? data->parts : NULL,
+					data ? data->nr_parts : 0);
+	if (err)
+		return err;
+
+	return 0;
+}
+
+static int mchp23k256_remove(struct spi_device *spi)
+{
+	struct mchp23k256_flash *flash = spi_get_drvdata(spi);
+
+	return mtd_device_unregister(&flash->mtd);
+}
+
+static struct spi_driver mchp23k256_driver = {
+	.driver = {
+		.name	= "mchp23k256",
+	},
+	.probe		= mchp23k256_probe,
+	.remove		= mchp23k256_remove,
+};
+
+module_spi_driver(mchp23k256_driver);
+
+MODULE_DESCRIPTION("MTD SPI driver for MCHP23K256 RAM chips");
+MODULE_AUTHOR("Andrew Lunn <andre@lunn.ch>");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("spi:mchp23k256");
diff --git a/drivers/mtd/devices/mtd_dataflash.c b/drivers/mtd/devices/mtd_dataflash.c
index f9e9bd1cfaa0..5dc8bd042cc5 100644
--- a/drivers/mtd/devices/mtd_dataflash.c
+++ b/drivers/mtd/devices/mtd_dataflash.c
@@ -82,9 +82,13 @@
 #define OP_WRITE_SECURITY_REVC	0x9A
 #define OP_WRITE_SECURITY	0x9B	/* revision D */
 
+#define CFI_MFR_ATMEL		0x1F
+
+#define DATAFLASH_SHIFT_EXTID	24
+#define DATAFLASH_SHIFT_ID	40
 
 struct dataflash {
-	uint8_t			command[4];
+	u8			command[4];
 	char			name[24];
 
 	unsigned short		page_offset;	/* offset in flash address */
@@ -129,8 +133,7 @@ static int dataflash_waitready(struct spi_device *spi)
 	for (;;) {
 		status = dataflash_status(spi);
 		if (status < 0) {
-			pr_debug("%s: status %d?\n",
-					dev_name(&spi->dev), status);
+			dev_dbg(&spi->dev, "status %d?\n", status);
 			status = 0;
 		}
 
@@ -153,12 +156,11 @@ static int dataflash_erase(struct mtd_info *mtd, struct erase_info *instr)
 	struct spi_transfer	x = { };
 	struct spi_message	msg;
 	unsigned		blocksize = priv->page_size << 3;
-	uint8_t			*command;
-	uint32_t		rem;
+	u8			*command;
+	u32			rem;
 
-	pr_debug("%s: erase addr=0x%llx len 0x%llx\n",
-	      dev_name(&spi->dev), (long long)instr->addr,
-	      (long long)instr->len);
+	dev_dbg(&spi->dev, "erase addr=0x%llx len 0x%llx\n",
+		(long long)instr->addr, (long long)instr->len);
 
 	div_u64_rem(instr->len, priv->page_size, &rem);
 	if (rem)
@@ -187,11 +189,11 @@ static int dataflash_erase(struct mtd_info *mtd, struct erase_info *instr)
 		pageaddr = pageaddr << priv->page_offset;
 
 		command[0] = do_block ? OP_ERASE_BLOCK : OP_ERASE_PAGE;
-		command[1] = (uint8_t)(pageaddr >> 16);
-		command[2] = (uint8_t)(pageaddr >> 8);
+		command[1] = (u8)(pageaddr >> 16);
+		command[2] = (u8)(pageaddr >> 8);
 		command[3] = 0;
 
-		pr_debug("ERASE %s: (%x) %x %x %x [%i]\n",
+		dev_dbg(&spi->dev, "ERASE %s: (%x) %x %x %x [%i]\n",
 			do_block ? "block" : "page",
 			command[0], command[1], command[2], command[3],
 			pageaddr);
@@ -200,8 +202,8 @@ static int dataflash_erase(struct mtd_info *mtd, struct erase_info *instr)
 		(void) dataflash_waitready(spi);
 
 		if (status < 0) {
-			printk(KERN_ERR "%s: erase %x, err %d\n",
-				dev_name(&spi->dev), pageaddr, status);
+			dev_err(&spi->dev, "erase %x, err %d\n",
+				pageaddr, status);
 			/* REVISIT:  can retry instr->retries times; or
 			 * giveup and instr->fail_addr = instr->addr;
 			 */
@@ -239,11 +241,11 @@ static int dataflash_read(struct mtd_info *mtd, loff_t from, size_t len,
 	struct spi_transfer	x[2] = { };
 	struct spi_message	msg;
 	unsigned int		addr;
-	uint8_t			*command;
+	u8			*command;
 	int			status;
 
-	pr_debug("%s: read 0x%x..0x%x\n", dev_name(&priv->spi->dev),
-			(unsigned)from, (unsigned)(from + len));
+	dev_dbg(&priv->spi->dev, "read 0x%x..0x%x\n",
+		  (unsigned int)from, (unsigned int)(from + len));
 
 	/* Calculate flash page/byte address */
 	addr = (((unsigned)from / priv->page_size) << priv->page_offset)
@@ -251,7 +253,7 @@ static int dataflash_read(struct mtd_info *mtd, loff_t from, size_t len,
 
 	command = priv->command;
 
-	pr_debug("READ: (%x) %x %x %x\n",
+	dev_dbg(&priv->spi->dev, "READ: (%x) %x %x %x\n",
 		command[0], command[1], command[2], command[3]);
 
 	spi_message_init(&msg);
@@ -271,9 +273,9 @@ static int dataflash_read(struct mtd_info *mtd, loff_t from, size_t len,
 	 * fewer "don't care" bytes.  Both buffers stay unchanged.
 	 */
 	command[0] = OP_READ_CONTINUOUS;
-	command[1] = (uint8_t)(addr >> 16);
-	command[2] = (uint8_t)(addr >> 8);
-	command[3] = (uint8_t)(addr >> 0);
+	command[1] = (u8)(addr >> 16);
+	command[2] = (u8)(addr >> 8);
+	command[3] = (u8)(addr >> 0);
 	/* plus 4 "don't care" bytes */
 
 	status = spi_sync(priv->spi, &msg);
@@ -283,8 +285,7 @@ static int dataflash_read(struct mtd_info *mtd, loff_t from, size_t len,
 		*retlen = msg.actual_length - 8;
 		status = 0;
 	} else
-		pr_debug("%s: read %x..%x --> %d\n",
-			dev_name(&priv->spi->dev),
+		dev_dbg(&priv->spi->dev, "read %x..%x --> %d\n",
 			(unsigned)from, (unsigned)(from + len),
 			status);
 	return status;
@@ -308,10 +309,10 @@ static int dataflash_write(struct mtd_info *mtd, loff_t to, size_t len,
 	size_t			remaining = len;
 	u_char			*writebuf = (u_char *) buf;
 	int			status = -EINVAL;
-	uint8_t			*command;
+	u8			*command;
 
-	pr_debug("%s: write 0x%x..0x%x\n",
-		dev_name(&spi->dev), (unsigned)to, (unsigned)(to + len));
+	dev_dbg(&spi->dev, "write 0x%x..0x%x\n",
+		(unsigned int)to, (unsigned int)(to + len));
 
 	spi_message_init(&msg);
 
@@ -328,7 +329,7 @@ static int dataflash_write(struct mtd_info *mtd, loff_t to, size_t len,
 
 	mutex_lock(&priv->lock);
 	while (remaining > 0) {
-		pr_debug("write @ %i:%i len=%i\n",
+		dev_dbg(&spi->dev, "write @ %i:%i len=%i\n",
 			pageaddr, offset, writelen);
 
 		/* REVISIT:
@@ -356,13 +357,13 @@ static int dataflash_write(struct mtd_info *mtd, loff_t to, size_t len,
 			command[2] = (addr & 0x0000FF00) >> 8;
 			command[3] = 0;
 
-			pr_debug("TRANSFER: (%x) %x %x %x\n",
+			dev_dbg(&spi->dev, "TRANSFER: (%x) %x %x %x\n",
 				command[0], command[1], command[2], command[3]);
 
 			status = spi_sync(spi, &msg);
 			if (status < 0)
-				pr_debug("%s: xfer %u -> %d\n",
-					dev_name(&spi->dev), addr, status);
+				dev_dbg(&spi->dev, "xfer %u -> %d\n",
+					addr, status);
 
 			(void) dataflash_waitready(priv->spi);
 		}
@@ -374,7 +375,7 @@ static int dataflash_write(struct mtd_info *mtd, loff_t to, size_t len,
 		command[2] = (addr & 0x0000FF00) >> 8;
 		command[3] = (addr & 0x000000FF);
 
-		pr_debug("PROGRAM: (%x) %x %x %x\n",
+		dev_dbg(&spi->dev, "PROGRAM: (%x) %x %x %x\n",
 			command[0], command[1], command[2], command[3]);
 
 		x[1].tx_buf = writebuf;
@@ -383,8 +384,8 @@ static int dataflash_write(struct mtd_info *mtd, loff_t to, size_t len,
 		status = spi_sync(spi, &msg);
 		spi_transfer_del(x + 1);
 		if (status < 0)
-			pr_debug("%s: pgm %u/%u -> %d\n",
-				dev_name(&spi->dev), addr, writelen, status);
+			dev_dbg(&spi->dev, "pgm %u/%u -> %d\n",
+				addr, writelen, status);
 
 		(void) dataflash_waitready(priv->spi);
 
@@ -398,20 +399,20 @@ static int dataflash_write(struct mtd_info *mtd, loff_t to, size_t len,
 		command[2] = (addr & 0x0000FF00) >> 8;
 		command[3] = 0;
 
-		pr_debug("COMPARE: (%x) %x %x %x\n",
+		dev_dbg(&spi->dev, "COMPARE: (%x) %x %x %x\n",
 			command[0], command[1], command[2], command[3]);
 
 		status = spi_sync(spi, &msg);
 		if (status < 0)
-			pr_debug("%s: compare %u -> %d\n",
-				dev_name(&spi->dev), addr, status);
+			dev_dbg(&spi->dev, "compare %u -> %d\n",
+				addr, status);
 
 		status = dataflash_waitready(priv->spi);
 
 		/* Check result of the compare operation */
 		if (status & (1 << 6)) {
-			printk(KERN_ERR "%s: compare page %u, err %d\n",
-				dev_name(&spi->dev), pageaddr, status);
+			dev_err(&spi->dev, "compare page %u, err %d\n",
+				pageaddr, status);
 			remaining = 0;
 			status = -EIO;
 			break;
@@ -455,11 +456,11 @@ static int dataflash_get_otp_info(struct mtd_info *mtd, size_t len,
 }
 
 static ssize_t otp_read(struct spi_device *spi, unsigned base,
-		uint8_t *buf, loff_t off, size_t len)
+		u8 *buf, loff_t off, size_t len)
 {
 	struct spi_message	m;
 	size_t			l;
-	uint8_t			*scratch;
+	u8			*scratch;
 	struct spi_transfer	t;
 	int			status;
 
@@ -538,7 +539,7 @@ static int dataflash_write_user_otp(struct mtd_info *mtd,
 {
 	struct spi_message	m;
 	const size_t		l = 4 + 64;
-	uint8_t			*scratch;
+	u8			*scratch;
 	struct spi_transfer	t;
 	struct dataflash	*priv = mtd->priv;
 	int			status;
@@ -689,14 +690,15 @@ struct flash_info {
 	/* JEDEC id has a high byte of zero plus three data bytes:
 	 * the manufacturer id, then a two byte device id.
 	 */
-	uint32_t	jedec_id;
+	u64		jedec_id;
 
 	/* The size listed here is what works with OP_ERASE_PAGE. */
 	unsigned	nr_pages;
-	uint16_t	pagesize;
-	uint16_t	pageoffset;
+	u16		pagesize;
+	u16		pageoffset;
 
-	uint16_t	flags;
+	u16		flags;
+#define SUP_EXTID	0x0004		/* supports extended ID data */
 #define SUP_POW2PS	0x0002		/* supports 2^N byte pages */
 #define IS_POW2PS	0x0001		/* uses 2^N byte pages */
 };
@@ -734,54 +736,32 @@ static struct flash_info dataflash_data[] = {
 
 	{ "AT45DB642x",  0x1f2800, 8192, 1056, 11, SUP_POW2PS},
 	{ "at45db642d",  0x1f2800, 8192, 1024, 10, SUP_POW2PS | IS_POW2PS},
+
+	{ "AT45DB641E",  0x1f28000100, 32768, 264, 9, SUP_EXTID | SUP_POW2PS},
+	{ "at45db641e",  0x1f28000100, 32768, 256, 8, SUP_EXTID | SUP_POW2PS | IS_POW2PS},
 };
 
-static struct flash_info *jedec_probe(struct spi_device *spi)
+static struct flash_info *jedec_lookup(struct spi_device *spi,
+				       u64 jedec, bool use_extid)
 {
-	int			tmp;
-	uint8_t			code = OP_READ_ID;
-	uint8_t			id[3];
-	uint32_t		jedec;
-	struct flash_info	*info;
+	struct flash_info *info;
 	int status;
 
-	/* JEDEC also defines an optional "extended device information"
-	 * string for after vendor-specific data, after the three bytes
-	 * we use here.  Supporting some chips might require using it.
-	 *
-	 * If the vendor ID isn't Atmel's (0x1f), assume this call failed.
-	 * That's not an error; only rev C and newer chips handle it, and
-	 * only Atmel sells these chips.
-	 */
-	tmp = spi_write_then_read(spi, &code, 1, id, 3);
-	if (tmp < 0) {
-		pr_debug("%s: error %d reading JEDEC ID\n",
-			dev_name(&spi->dev), tmp);
-		return ERR_PTR(tmp);
-	}
-	if (id[0] != 0x1f)
-		return NULL;
-
-	jedec = id[0];
-	jedec = jedec << 8;
-	jedec |= id[1];
-	jedec = jedec << 8;
-	jedec |= id[2];
+	for (info = dataflash_data;
+	     info < dataflash_data + ARRAY_SIZE(dataflash_data);
+	     info++) {
+		if (use_extid && !(info->flags & SUP_EXTID))
+			continue;
 
-	for (tmp = 0, info = dataflash_data;
-			tmp < ARRAY_SIZE(dataflash_data);
-			tmp++, info++) {
 		if (info->jedec_id == jedec) {
-			pr_debug("%s: OTP, sector protect%s\n",
-				dev_name(&spi->dev),
-				(info->flags & SUP_POW2PS)
-					? ", binary pagesize" : ""
-				);
+			dev_dbg(&spi->dev, "OTP, sector protect%s\n",
+				(info->flags & SUP_POW2PS) ?
+				", binary pagesize" : "");
 			if (info->flags & SUP_POW2PS) {
 				status = dataflash_status(spi);
 				if (status < 0) {
-					pr_debug("%s: status error %d\n",
-						dev_name(&spi->dev), status);
+					dev_dbg(&spi->dev, "status error %d\n",
+						status);
 					return ERR_PTR(status);
 				}
 				if (status & 0x1) {
@@ -796,12 +776,58 @@ static struct flash_info *jedec_probe(struct spi_device *spi)
 		}
 	}
 
+	return ERR_PTR(-ENODEV);
+}
+
+static struct flash_info *jedec_probe(struct spi_device *spi)
+{
+	int ret;
+	u8 code = OP_READ_ID;
+	u64 jedec;
+	u8 id[sizeof(jedec)] = {0};
+	const unsigned int id_size = 5;
+	struct flash_info *info;
+
+	/*
+	 * JEDEC also defines an optional "extended device information"
+	 * string for after vendor-specific data, after the three bytes
+	 * we use here.  Supporting some chips might require using it.
+	 *
+	 * If the vendor ID isn't Atmel's (0x1f), assume this call failed.
+	 * That's not an error; only rev C and newer chips handle it, and
+	 * only Atmel sells these chips.
+	 */
+	ret = spi_write_then_read(spi, &code, 1, id, id_size);
+	if (ret < 0) {
+		dev_dbg(&spi->dev, "error %d reading JEDEC ID\n", ret);
+		return ERR_PTR(ret);
+	}
+
+	if (id[0] != CFI_MFR_ATMEL)
+		return NULL;
+
+	jedec = be64_to_cpup((__be64 *)id);
+
+	/*
+	 * First, try to match device using extended device
+	 * information
+	 */
+	info = jedec_lookup(spi, jedec >> DATAFLASH_SHIFT_EXTID, true);
+	if (!IS_ERR(info))
+		return info;
+	/*
+	 * If that fails, make another pass using regular ID
+	 * information
+	 */
+	info = jedec_lookup(spi, jedec >> DATAFLASH_SHIFT_ID, false);
+	if (!IS_ERR(info))
+		return info;
 	/*
 	 * Treat other chips as errors ... we won't know the right page
 	 * size (it might be binary) even when we can tell which density
 	 * class is involved (legacy chip id scheme).
 	 */
-	dev_warn(&spi->dev, "JEDEC id %06x not handled\n", jedec);
+	dev_warn(&spi->dev, "JEDEC id %016llx not handled\n", jedec);
 	return ERR_PTR(-ENODEV);
 }
 
@@ -845,8 +871,7 @@ static int dataflash_probe(struct spi_device *spi)
 	 */
 	status = dataflash_status(spi);
 	if (status <= 0 || status == 0xff) {
-		pr_debug("%s: status error %d\n",
-				dev_name(&spi->dev), status);
+		dev_dbg(&spi->dev, "status error %d\n", status);
 		if (status == 0 || status == 0xff)
 			status = -ENODEV;
 		return status;
@@ -887,8 +912,7 @@ static int dataflash_probe(struct spi_device *spi)
 	}
 
 	if (status < 0)
-		pr_debug("%s: add_dataflash --> %d\n", dev_name(&spi->dev),
-				status);
+		dev_dbg(&spi->dev, "add_dataflash --> %d\n", status);
 
 	return status;
 }
@@ -898,7 +922,7 @@ static int dataflash_remove(struct spi_device *spi)
 	struct dataflash	*flash = spi_get_drvdata(spi);
 	int			status;
 
-	pr_debug("%s: remove\n", dev_name(&spi->dev));
+	dev_dbg(&spi->dev, "remove\n");
 
 	status = mtd_device_unregister(&flash->mtd);
 	if (status == 0)
diff --git a/drivers/mtd/maps/physmap_of_gemini.c b/drivers/mtd/maps/physmap_of_gemini.c
index 9d371cd728ea..05b286b5289f 100644
--- a/drivers/mtd/maps/physmap_of_gemini.c
+++ b/drivers/mtd/maps/physmap_of_gemini.c
@@ -59,7 +59,7 @@ int of_flash_probe_gemini(struct platform_device *pdev,
 			  struct device_node *np,
 			  struct map_info *map)
 {
-	static struct regmap *rmap;
+	struct regmap *rmap;
 	struct device *dev = &pdev->dev;
 	u32 val;
 	int ret;
diff --git a/drivers/mtd/nand/Kconfig b/drivers/mtd/nand/Kconfig
index c3029528063b..dbfa72d61d5a 100644
--- a/drivers/mtd/nand/Kconfig
+++ b/drivers/mtd/nand/Kconfig
@@ -308,6 +308,7 @@ config MTD_NAND_CS553X
 config MTD_NAND_ATMEL
 	tristate "Support for NAND Flash / SmartMedia on AT91"
 	depends on ARCH_AT91
+	select MFD_ATMEL_SMC
 	help
 	  Enables support for NAND Flash / Smart Media Card interface
 	  on Atmel AT91 processors.
@@ -542,6 +543,7 @@ config MTD_NAND_SUNXI
 
 config MTD_NAND_HISI504
 	tristate "Support for NAND controller on Hisilicon SoC Hip04"
+	depends on ARCH_HISI || COMPILE_TEST
 	depends on HAS_DMA
 	help
 	  Enables support for NAND controller on Hisilicon SoC Hip04.
@@ -555,6 +557,7 @@ config MTD_NAND_QCOM
 
 config MTD_NAND_MTK
 	tristate "Support for NAND controller on MTK SoCs"
+	depends on ARCH_MEDIATEK || COMPILE_TEST
 	depends on HAS_DMA
 	help
 	  Enables support for NAND controller on MTK SoCs.
diff --git a/drivers/mtd/nand/atmel/nand-controller.c b/drivers/mtd/nand/atmel/nand-controller.c
index 3b2446896147..d24e67b95167 100644
--- a/drivers/mtd/nand/atmel/nand-controller.c
+++ b/drivers/mtd/nand/atmel/nand-controller.c
@@ -57,6 +57,7 @@
 #include <linux/interrupt.h>
 #include <linux/mfd/syscon.h>
 #include <linux/mfd/syscon/atmel-matrix.h>
+#include <linux/mfd/syscon/atmel-smc.h>
 #include <linux/module.h>
 #include <linux/mtd/nand.h>
 #include <linux/of_address.h>
@@ -151,6 +152,8 @@ struct atmel_nand_cs {
 		void __iomem *virt;
 		dma_addr_t dma;
 	} io;
+
+	struct atmel_smc_cs_conf smcconf;
 };
 
 struct atmel_nand {
@@ -196,6 +199,8 @@ struct atmel_nand_controller_ops {
 	void (*nand_init)(struct atmel_nand_controller *nc,
 			  struct atmel_nand *nand);
 	int (*ecc_init)(struct atmel_nand *nand);
+	int (*setup_data_interface)(struct atmel_nand *nand, int csline,
+				    const struct nand_data_interface *conf);
 };
 
 struct atmel_nand_controller_caps {
@@ -912,7 +917,7 @@ static int atmel_hsmc_nand_pmecc_write_pg(struct nand_chip *chip,
 	struct mtd_info *mtd = nand_to_mtd(chip);
 	struct atmel_nand *nand = to_atmel_nand(chip);
 	struct atmel_hsmc_nand_controller *nc;
-	int ret;
+	int ret, status;
 
 	nc = to_hsmc_nand_controller(chip->controller);
 
@@ -954,6 +959,10 @@ static int atmel_hsmc_nand_pmecc_write_pg(struct nand_chip *chip,
 		dev_err(nc->base.dev, "Failed to program NAND page (err = %d)\n",
 			ret);
 
+	status = chip->waitfunc(mtd, chip);
+	if (status & NAND_STATUS_FAIL)
+		return -EIO;
+
 	return ret;
 }
 
@@ -1175,6 +1184,295 @@ static int atmel_hsmc_nand_ecc_init(struct atmel_nand *nand)
 	return 0;
 }
 
+static int atmel_smc_nand_prepare_smcconf(struct atmel_nand *nand,
+					const struct nand_data_interface *conf,
+					struct atmel_smc_cs_conf *smcconf)
+{
+	u32 ncycles, totalcycles, timeps, mckperiodps;
+	struct atmel_nand_controller *nc;
+	int ret;
+
+	nc = to_nand_controller(nand->base.controller);
+
+	/* DDR interface not supported. */
+	if (conf->type != NAND_SDR_IFACE)
+		return -ENOTSUPP;
+
+	/*
+	 * tRC < 30ns implies EDO mode. This controller does not support this
+	 * mode.
+	 */
+	if (conf->timings.sdr.tRC_min < 30)
+		return -ENOTSUPP;
+
+	atmel_smc_cs_conf_init(smcconf);
+
+	mckperiodps = NSEC_PER_SEC / clk_get_rate(nc->mck);
+	mckperiodps *= 1000;
+
+	/*
+	 * Set write pulse timing. This one is easy to extract:
+	 *
+	 * NWE_PULSE = tWP
+	 */
+	ncycles = DIV_ROUND_UP(conf->timings.sdr.tWP_min, mckperiodps);
+	totalcycles = ncycles;
+	ret = atmel_smc_cs_conf_set_pulse(smcconf, ATMEL_SMC_NWE_SHIFT,
+					  ncycles);
+	if (ret)
+		return ret;
+
+	/*
+	 * The write setup timing depends on the operation done on the NAND.
+	 * All operations goes through the same data bus, but the operation
+	 * type depends on the address we are writing to (ALE/CLE address
+	 * lines).
+	 * Since we have no way to differentiate the different operations at
+	 * the SMC level, we must consider the worst case (the biggest setup
+	 * time among all operation types):
+	 *
+	 * NWE_SETUP = max(tCLS, tCS, tALS, tDS) - NWE_PULSE
+	 */
+	timeps = max3(conf->timings.sdr.tCLS_min, conf->timings.sdr.tCS_min,
+		      conf->timings.sdr.tALS_min);
+	timeps = max(timeps, conf->timings.sdr.tDS_min);
+	ncycles = DIV_ROUND_UP(timeps, mckperiodps);
+	ncycles = ncycles > totalcycles ? ncycles - totalcycles : 0;
+	totalcycles += ncycles;
+	ret = atmel_smc_cs_conf_set_setup(smcconf, ATMEL_SMC_NWE_SHIFT,
+					  ncycles);
+	if (ret)
+		return ret;
+
+	/*
+	 * As for the write setup timing, the write hold timing depends on the
+	 * operation done on the NAND:
+	 *
+	 * NWE_HOLD = max(tCLH, tCH, tALH, tDH, tWH)
+	 */
+	timeps = max3(conf->timings.sdr.tCLH_min, conf->timings.sdr.tCH_min,
+		      conf->timings.sdr.tALH_min);
+	timeps = max3(timeps, conf->timings.sdr.tDH_min,
+		      conf->timings.sdr.tWH_min);
+	ncycles = DIV_ROUND_UP(timeps, mckperiodps);
+	totalcycles += ncycles;
+
+	/*
+	 * The write cycle timing is directly matching tWC, but is also
+	 * dependent on the other timings on the setup and hold timings we
+	 * calculated earlier, which gives:
+	 *
+	 * NWE_CYCLE = max(tWC, NWE_SETUP + NWE_PULSE + NWE_HOLD)
+	 */
+	ncycles = DIV_ROUND_UP(conf->timings.sdr.tWC_min, mckperiodps);
+	ncycles = max(totalcycles, ncycles);
+	ret = atmel_smc_cs_conf_set_cycle(smcconf, ATMEL_SMC_NWE_SHIFT,
+					  ncycles);
+	if (ret)
+		return ret;
+
+	/*
+	 * We don't want the CS line to be toggled between each byte/word
+	 * transfer to the NAND. The only way to guarantee that is to have the
+	 * NCS_{WR,RD}_{SETUP,HOLD} timings set to 0, which in turn means:
+	 *
+	 * NCS_WR_PULSE = NWE_CYCLE
+	 */
+	ret = atmel_smc_cs_conf_set_pulse(smcconf, ATMEL_SMC_NCS_WR_SHIFT,
+					  ncycles);
+	if (ret)
+		return ret;
+
+	/*
+	 * As for the write setup timing, the read hold timing depends on the
+	 * operation done on the NAND:
+	 *
+	 * NRD_HOLD = max(tREH, tRHOH)
+	 */
+	timeps = max(conf->timings.sdr.tREH_min, conf->timings.sdr.tRHOH_min);
+	ncycles = DIV_ROUND_UP(timeps, mckperiodps);
+	totalcycles = ncycles;
+
+	/*
+	 * TDF = tRHZ - NRD_HOLD
+	 */
+	ncycles = DIV_ROUND_UP(conf->timings.sdr.tRHZ_max, mckperiodps);
+	ncycles -= totalcycles;
+
+	/*
+	 * In ONFI 4.0 specs, tRHZ has been increased to support EDO NANDs and
+	 * we might end up with a config that does not fit in the TDF field.
+	 * Just take the max value in this case and hope that the NAND is more
+	 * tolerant than advertised.
+	 */
+	if (ncycles > ATMEL_SMC_MODE_TDF_MAX)
+		ncycles = ATMEL_SMC_MODE_TDF_MAX;
+	else if (ncycles < ATMEL_SMC_MODE_TDF_MIN)
+		ncycles = ATMEL_SMC_MODE_TDF_MIN;
+
+	smcconf->mode |= ATMEL_SMC_MODE_TDF(ncycles) |
+			 ATMEL_SMC_MODE_TDFMODE_OPTIMIZED;
+
+	/*
+	 * Read pulse timing directly matches tRP:
+	 *
+	 * NRD_PULSE = tRP
+	 */
+	ncycles = DIV_ROUND_UP(conf->timings.sdr.tRP_min, mckperiodps);
+	totalcycles += ncycles;
+	ret = atmel_smc_cs_conf_set_pulse(smcconf, ATMEL_SMC_NRD_SHIFT,
+					  ncycles);
+	if (ret)
+		return ret;
+
+	/*
+	 * The write cycle timing is directly matching tWC, but is also
+	 * dependent on the setup and hold timings we calculated earlier,
+	 * which gives:
+	 *
+	 * NRD_CYCLE = max(tRC, NRD_PULSE + NRD_HOLD)
+	 *
+	 * NRD_SETUP is always 0.
+	 */
+	ncycles = DIV_ROUND_UP(conf->timings.sdr.tRC_min, mckperiodps);
+	ncycles = max(totalcycles, ncycles);
+	ret = atmel_smc_cs_conf_set_cycle(smcconf, ATMEL_SMC_NRD_SHIFT,
+					  ncycles);
+	if (ret)
+		return ret;
+
+	/*
+	 * We don't want the CS line to be toggled between each byte/word
+	 * transfer from the NAND. The only way to guarantee that is to have
+	 * the NCS_{WR,RD}_{SETUP,HOLD} timings set to 0, which in turn means:
+	 *
+	 * NCS_RD_PULSE = NRD_CYCLE
+	 */
+	ret = atmel_smc_cs_conf_set_pulse(smcconf, ATMEL_SMC_NCS_RD_SHIFT,
+					  ncycles);
+	if (ret)
+		return ret;
+
+	/* Txxx timings are directly matching tXXX ones. */
+	ncycles = DIV_ROUND_UP(conf->timings.sdr.tCLR_min, mckperiodps);
+	ret = atmel_smc_cs_conf_set_timing(smcconf,
+					   ATMEL_HSMC_TIMINGS_TCLR_SHIFT,
+					   ncycles);
+	if (ret)
+		return ret;
+
+	ncycles = DIV_ROUND_UP(conf->timings.sdr.tADL_min, mckperiodps);
+	ret = atmel_smc_cs_conf_set_timing(smcconf,
+					   ATMEL_HSMC_TIMINGS_TADL_SHIFT,
+					   ncycles);
+	if (ret)
+		return ret;
+
+	ncycles = DIV_ROUND_UP(conf->timings.sdr.tAR_min, mckperiodps);
+	ret = atmel_smc_cs_conf_set_timing(smcconf,
+					   ATMEL_HSMC_TIMINGS_TAR_SHIFT,
+					   ncycles);
+	if (ret)
+		return ret;
+
+	ncycles = DIV_ROUND_UP(conf->timings.sdr.tRR_min, mckperiodps);
+	ret = atmel_smc_cs_conf_set_timing(smcconf,
+					   ATMEL_HSMC_TIMINGS_TRR_SHIFT,
+					   ncycles);
+	if (ret)
+		return ret;
+
+	ncycles = DIV_ROUND_UP(conf->timings.sdr.tWB_max, mckperiodps);
+	ret = atmel_smc_cs_conf_set_timing(smcconf,
+					   ATMEL_HSMC_TIMINGS_TWB_SHIFT,
+					   ncycles);
+	if (ret)
+		return ret;
+
+	/* Attach the CS line to the NFC logic. */
+	smcconf->timings |= ATMEL_HSMC_TIMINGS_NFSEL;
+
+	/* Set the appropriate data bus width. */
+	if (nand->base.options & NAND_BUSWIDTH_16)
+		smcconf->mode |= ATMEL_SMC_MODE_DBW_16;
+
+	/* Operate in NRD/NWE READ/WRITEMODE. */
+	smcconf->mode |= ATMEL_SMC_MODE_READMODE_NRD |
+			 ATMEL_SMC_MODE_WRITEMODE_NWE;
+
+	return 0;
+}
+
+static int atmel_smc_nand_setup_data_interface(struct atmel_nand *nand,
+					int csline,
+					const struct nand_data_interface *conf)
+{
+	struct atmel_nand_controller *nc;
+	struct atmel_smc_cs_conf smcconf;
+	struct atmel_nand_cs *cs;
+	int ret;
+
+	nc = to_nand_controller(nand->base.controller);
+
+	ret = atmel_smc_nand_prepare_smcconf(nand, conf, &smcconf);
+	if (ret)
+		return ret;
+
+	if (csline == NAND_DATA_IFACE_CHECK_ONLY)
+		return 0;
+
+	cs = &nand->cs[csline];
+	cs->smcconf = smcconf;
+	atmel_smc_cs_conf_apply(nc->smc, cs->id, &cs->smcconf);
+
+	return 0;
+}
+
+static int atmel_hsmc_nand_setup_data_interface(struct atmel_nand *nand,
+					int csline,
+					const struct nand_data_interface *conf)
+{
+	struct atmel_nand_controller *nc;
+	struct atmel_smc_cs_conf smcconf;
+	struct atmel_nand_cs *cs;
+	int ret;
+
+	nc = to_nand_controller(nand->base.controller);
+
+	ret = atmel_smc_nand_prepare_smcconf(nand, conf, &smcconf);
+	if (ret)
+		return ret;
+
+	if (csline == NAND_DATA_IFACE_CHECK_ONLY)
+		return 0;
+
+	cs = &nand->cs[csline];
+	cs->smcconf = smcconf;
+
+	if (cs->rb.type == ATMEL_NAND_NATIVE_RB)
+		cs->smcconf.timings |= ATMEL_HSMC_TIMINGS_RBNSEL(cs->rb.id);
+
+	atmel_hsmc_cs_conf_apply(nc->smc, cs->id, &cs->smcconf);
+
+	return 0;
+}
+
+static int atmel_nand_setup_data_interface(struct mtd_info *mtd, int csline,
+					const struct nand_data_interface *conf)
+{
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct atmel_nand *nand = to_atmel_nand(chip);
+	struct atmel_nand_controller *nc;
+
+	nc = to_nand_controller(nand->base.controller);
+
+	if (csline >= nand->numcs ||
+	    (csline < 0 && csline != NAND_DATA_IFACE_CHECK_ONLY))
+		return -EINVAL;
+
+	return nc->caps->ops->setup_data_interface(nand, csline, conf);
+}
+
 static void atmel_nand_init(struct atmel_nand_controller *nc,
 			    struct atmel_nand *nand)
 {
@@ -1192,6 +1490,9 @@ static void atmel_nand_init(struct atmel_nand_controller *nc,
 	chip->write_buf = atmel_nand_write_buf;
 	chip->select_chip = atmel_nand_select_chip;
 
+	if (nc->mck && nc->caps->ops->setup_data_interface)
+		chip->setup_data_interface = atmel_nand_setup_data_interface;
+
 	/* Some NANDs require a longer delay than the default one (20us). */
 	chip->chip_delay = 40;
 
@@ -1677,6 +1978,12 @@ static int atmel_nand_controller_init(struct atmel_nand_controller *nc,
 	if (nc->caps->legacy_of_bindings)
 		return 0;
 
+	nc->mck = of_clk_get(dev->parent->of_node, 0);
+	if (IS_ERR(nc->mck)) {
+		dev_err(dev, "Failed to retrieve MCK clk\n");
+		return PTR_ERR(nc->mck);
+	}
+
 	np = of_parse_phandle(dev->parent->of_node, "atmel,smc", 0);
 	if (!np) {
 		dev_err(dev, "Missing or invalid atmel,smc property\n");
@@ -1983,6 +2290,7 @@ static const struct atmel_nand_controller_ops atmel_hsmc_nc_ops = {
 	.remove = atmel_hsmc_nand_controller_remove,
 	.ecc_init = atmel_hsmc_nand_ecc_init,
 	.nand_init = atmel_hsmc_nand_init,
+	.setup_data_interface = atmel_hsmc_nand_setup_data_interface,
 };
 
 static const struct atmel_nand_controller_caps atmel_sama5_nc_caps = {
@@ -2037,7 +2345,14 @@ atmel_smc_nand_controller_remove(struct atmel_nand_controller *nc)
 	return 0;
 }
 
-static const struct atmel_nand_controller_ops atmel_smc_nc_ops = {
+/*
+ * The SMC reg layout of at91rm9200 is completely different which prevents us
+ * from re-using atmel_smc_nand_setup_data_interface() for the
+ * ->setup_data_interface() hook.
+ * At this point, there's no support for the at91rm9200 SMC IP, so we leave
+ * ->setup_data_interface() unassigned.
+ */
+static const struct atmel_nand_controller_ops at91rm9200_nc_ops = {
 	.probe = atmel_smc_nand_controller_probe,
 	.remove = atmel_smc_nand_controller_remove,
 	.ecc_init = atmel_nand_ecc_init,
@@ -2047,6 +2362,20 @@ static const struct atmel_nand_controller_ops atmel_smc_nc_ops = {
 static const struct atmel_nand_controller_caps atmel_rm9200_nc_caps = {
 	.ale_offs = BIT(21),
 	.cle_offs = BIT(22),
+	.ops = &at91rm9200_nc_ops,
+};
+
+static const struct atmel_nand_controller_ops atmel_smc_nc_ops = {
+	.probe = atmel_smc_nand_controller_probe,
+	.remove = atmel_smc_nand_controller_remove,
+	.ecc_init = atmel_nand_ecc_init,
+	.nand_init = atmel_smc_nand_init,
+	.setup_data_interface = atmel_smc_nand_setup_data_interface,
+};
+
+static const struct atmel_nand_controller_caps atmel_sam9260_nc_caps = {
+	.ale_offs = BIT(21),
+	.cle_offs = BIT(22),
 	.ops = &atmel_smc_nc_ops,
 };
 
@@ -2093,7 +2422,7 @@ static const struct of_device_id atmel_nand_controller_of_ids[] = {
 	},
 	{
 		.compatible = "atmel,at91sam9260-nand-controller",
-		.data = &atmel_rm9200_nc_caps,
+		.data = &atmel_sam9260_nc_caps,
 	},
 	{
 		.compatible = "atmel,at91sam9261-nand-controller",
@@ -2181,6 +2510,24 @@ static int atmel_nand_controller_remove(struct platform_device *pdev)
 	return nc->caps->ops->remove(nc);
 }
 
+static __maybe_unused int atmel_nand_controller_resume(struct device *dev)
+{
+	struct atmel_nand_controller *nc = dev_get_drvdata(dev);
+	struct atmel_nand *nand;
+
+	list_for_each_entry(nand, &nc->chips, node) {
+		int i;
+
+		for (i = 0; i < nand->numcs; i++)
+			nand_reset(&nand->base, i);
+	}
+
+	return 0;
+}
+
+static SIMPLE_DEV_PM_OPS(atmel_nand_controller_pm_ops, NULL,
+			 atmel_nand_controller_resume);
+
 static struct platform_driver atmel_nand_controller_driver = {
 	.driver = {
 		.name = "atmel-nand-controller",
diff --git a/drivers/mtd/nand/bcm47xxnflash/ops_bcm4706.c b/drivers/mtd/nand/bcm47xxnflash/ops_bcm4706.c
index f1da4ea88f2c..54bac5b73f0a 100644
--- a/drivers/mtd/nand/bcm47xxnflash/ops_bcm4706.c
+++ b/drivers/mtd/nand/bcm47xxnflash/ops_bcm4706.c
@@ -392,6 +392,8 @@ int bcm47xxnflash_ops_bcm4706_init(struct bcm47xxnflash *b47n)
 	b47n->nand_chip.read_byte = bcm47xxnflash_ops_bcm4706_read_byte;
 	b47n->nand_chip.read_buf = bcm47xxnflash_ops_bcm4706_read_buf;
 	b47n->nand_chip.write_buf = bcm47xxnflash_ops_bcm4706_write_buf;
+	b47n->nand_chip.onfi_set_features = nand_onfi_get_set_features_notsupp;
+	b47n->nand_chip.onfi_get_features = nand_onfi_get_set_features_notsupp;
 
 	nand_chip->chip_delay = 50;
 	b47n->nand_chip.bbt_options = NAND_BBT_USE_FLASH;
diff --git a/drivers/mtd/nand/cafe_nand.c b/drivers/mtd/nand/cafe_nand.c
index d40c32d311d8..2fd733eba0a3 100644
--- a/drivers/mtd/nand/cafe_nand.c
+++ b/drivers/mtd/nand/cafe_nand.c
@@ -654,6 +654,8 @@ static int cafe_nand_probe(struct pci_dev *pdev,
 	cafe->nand.read_buf = cafe_read_buf;
 	cafe->nand.write_buf = cafe_write_buf;
 	cafe->nand.select_chip = cafe_select_chip;
+	cafe->nand.onfi_set_features = nand_onfi_get_set_features_notsupp;
+	cafe->nand.onfi_get_features = nand_onfi_get_set_features_notsupp;
 
 	cafe->nand.chip_delay = 0;
 
diff --git a/drivers/mtd/nand/davinci_nand.c b/drivers/mtd/nand/davinci_nand.c
index 531c51991e57..7b26e53b95b1 100644
--- a/drivers/mtd/nand/davinci_nand.c
+++ b/drivers/mtd/nand/davinci_nand.c
@@ -771,11 +771,14 @@ static int nand_davinci_probe(struct platform_device *pdev)
 			info->chip.ecc.hwctl = nand_davinci_hwctl_4bit;
 			info->chip.ecc.bytes = 10;
 			info->chip.ecc.options = NAND_ECC_GENERIC_ERASED_CHECK;
+			info->chip.ecc.algo = NAND_ECC_BCH;
 		} else {
+			/* 1bit ecc hamming */
 			info->chip.ecc.calculate = nand_davinci_calculate_1bit;
 			info->chip.ecc.correct = nand_davinci_correct_1bit;
 			info->chip.ecc.hwctl = nand_davinci_hwctl_1bit;
 			info->chip.ecc.bytes = 3;
+			info->chip.ecc.algo = NAND_ECC_HAMMING;
 		}
 		info->chip.ecc.size = 512;
 		info->chip.ecc.strength = pdata->ecc_bits;
diff --git a/drivers/mtd/nand/denali.c b/drivers/mtd/nand/denali.c
index 16634df2e39a..b3c99d98fdee 100644
--- a/drivers/mtd/nand/denali.c
+++ b/drivers/mtd/nand/denali.c
@@ -1531,6 +1531,8 @@ int denali_init(struct denali_nand_info *denali)
 	chip->cmdfunc = denali_cmdfunc;
 	chip->read_byte = denali_read_byte;
 	chip->waitfunc = denali_waitfunc;
+	chip->onfi_set_features = nand_onfi_get_set_features_notsupp;
+	chip->onfi_get_features = nand_onfi_get_set_features_notsupp;
 
 	/*
 	 * scan for NAND devices attached to the controller
diff --git a/drivers/mtd/nand/docg4.c b/drivers/mtd/nand/docg4.c
index 7af2a3cd949e..a27a84fbfb84 100644
--- a/drivers/mtd/nand/docg4.c
+++ b/drivers/mtd/nand/docg4.c
@@ -1260,6 +1260,8 @@ static void __init init_mtd_structs(struct mtd_info *mtd)
 	nand->read_buf = docg4_read_buf;
 	nand->write_buf = docg4_write_buf16;
 	nand->erase = docg4_erase_block;
+	nand->onfi_set_features = nand_onfi_get_set_features_notsupp;
+	nand->onfi_get_features = nand_onfi_get_set_features_notsupp;
 	nand->ecc.read_page = docg4_read_page;
 	nand->ecc.write_page = docg4_write_page;
 	nand->ecc.read_page_raw = docg4_read_page_raw;
diff --git a/drivers/mtd/nand/fsl_elbc_nand.c b/drivers/mtd/nand/fsl_elbc_nand.c
index 113f76e59937..b9ac16f05057 100644
--- a/drivers/mtd/nand/fsl_elbc_nand.c
+++ b/drivers/mtd/nand/fsl_elbc_nand.c
@@ -775,6 +775,8 @@ static int fsl_elbc_chip_init(struct fsl_elbc_mtd *priv)
 	chip->select_chip = fsl_elbc_select_chip;
 	chip->cmdfunc = fsl_elbc_cmdfunc;
 	chip->waitfunc = fsl_elbc_wait;
+	chip->onfi_set_features = nand_onfi_get_set_features_notsupp;
+	chip->onfi_get_features = nand_onfi_get_set_features_notsupp;
 
 	chip->bbt_td = &bbt_main_descr;
 	chip->bbt_md = &bbt_mirror_descr;
diff --git a/drivers/mtd/nand/fsl_ifc_nand.c b/drivers/mtd/nand/fsl_ifc_nand.c
index d1570f512f0b..89e14daeaba6 100644
--- a/drivers/mtd/nand/fsl_ifc_nand.c
+++ b/drivers/mtd/nand/fsl_ifc_nand.c
@@ -831,6 +831,8 @@ static int fsl_ifc_chip_init(struct fsl_ifc_mtd *priv)
 	chip->select_chip = fsl_ifc_select_chip;
 	chip->cmdfunc = fsl_ifc_cmdfunc;
 	chip->waitfunc = fsl_ifc_wait;
+	chip->onfi_set_features = nand_onfi_get_set_features_notsupp;
+	chip->onfi_get_features = nand_onfi_get_set_features_notsupp;
 
 	chip->bbt_td = &bbt_main_descr;
 	chip->bbt_md = &bbt_mirror_descr;
diff --git a/drivers/mtd/nand/fsmc_nand.c b/drivers/mtd/nand/fsmc_nand.c
index cea50d2f218c..9d8b051d3187 100644
--- a/drivers/mtd/nand/fsmc_nand.c
+++ b/drivers/mtd/nand/fsmc_nand.c
@@ -302,25 +302,13 @@ static void fsmc_cmd_ctrl(struct mtd_info *mtd, int cmd, unsigned int ctrl)
  * This routine initializes timing parameters related to NAND memory access in
  * FSMC registers
  */
-static void fsmc_nand_setup(void __iomem *regs, uint32_t bank,
-			   uint32_t busw, struct fsmc_nand_timings *timings)
+static void fsmc_nand_setup(struct fsmc_nand_data *host,
+			    struct fsmc_nand_timings *tims)
 {
 	uint32_t value = FSMC_DEVTYPE_NAND | FSMC_ENABLE | FSMC_WAITON;
 	uint32_t tclr, tar, thiz, thold, twait, tset;
-	struct fsmc_nand_timings *tims;
-	struct fsmc_nand_timings default_timings = {
-		.tclr	= FSMC_TCLR_1,
-		.tar	= FSMC_TAR_1,
-		.thiz	= FSMC_THIZ_1,
-		.thold	= FSMC_THOLD_4,
-		.twait	= FSMC_TWAIT_6,
-		.tset	= FSMC_TSET_0,
-	};
-
-	if (timings)
-		tims = timings;
-	else
-		tims = &default_timings;
+	unsigned int bank = host->bank;
+	void __iomem *regs = host->regs_va;
 
 	tclr = (tims->tclr & FSMC_TCLR_MASK) << FSMC_TCLR_SHIFT;
 	tar = (tims->tar & FSMC_TAR_MASK) << FSMC_TAR_SHIFT;
@@ -329,7 +317,7 @@ static void fsmc_nand_setup(void __iomem *regs, uint32_t bank,
 	twait = (tims->twait & FSMC_TWAIT_MASK) << FSMC_TWAIT_SHIFT;
 	tset = (tims->tset & FSMC_TSET_MASK) << FSMC_TSET_SHIFT;
 
-	if (busw)
+	if (host->nand.options & NAND_BUSWIDTH_16)
 		writel_relaxed(value | FSMC_DEVWID_16,
 				FSMC_NAND_REG(regs, bank, PC));
 	else
@@ -344,6 +332,87 @@ static void fsmc_nand_setup(void __iomem *regs, uint32_t bank,
 			FSMC_NAND_REG(regs, bank, ATTRIB));
 }
 
+static int fsmc_calc_timings(struct fsmc_nand_data *host,
+			     const struct nand_sdr_timings *sdrt,
+			     struct fsmc_nand_timings *tims)
+{
+	unsigned long hclk = clk_get_rate(host->clk);
+	unsigned long hclkn = NSEC_PER_SEC / hclk;
+	uint32_t thiz, thold, twait, tset;
+
+	if (sdrt->tRC_min < 30000)
+		return -EOPNOTSUPP;
+
+	tims->tar = DIV_ROUND_UP(sdrt->tAR_min / 1000, hclkn) - 1;
+	if (tims->tar > FSMC_TAR_MASK)
+		tims->tar = FSMC_TAR_MASK;
+	tims->tclr = DIV_ROUND_UP(sdrt->tCLR_min / 1000, hclkn) - 1;
+	if (tims->tclr > FSMC_TCLR_MASK)
+		tims->tclr = FSMC_TCLR_MASK;
+
+	thiz = sdrt->tCS_min - sdrt->tWP_min;
+	tims->thiz = DIV_ROUND_UP(thiz / 1000, hclkn);
+
+	thold = sdrt->tDH_min;
+	if (thold < sdrt->tCH_min)
+		thold = sdrt->tCH_min;
+	if (thold < sdrt->tCLH_min)
+		thold = sdrt->tCLH_min;
+	if (thold < sdrt->tWH_min)
+		thold = sdrt->tWH_min;
+	if (thold < sdrt->tALH_min)
+		thold = sdrt->tALH_min;
+	if (thold < sdrt->tREH_min)
+		thold = sdrt->tREH_min;
+	tims->thold = DIV_ROUND_UP(thold / 1000, hclkn);
+	if (tims->thold == 0)
+		tims->thold = 1;
+	else if (tims->thold > FSMC_THOLD_MASK)
+		tims->thold = FSMC_THOLD_MASK;
+
+	twait = max(sdrt->tRP_min, sdrt->tWP_min);
+	tims->twait = DIV_ROUND_UP(twait / 1000, hclkn) - 1;
+	if (tims->twait == 0)
+		tims->twait = 1;
+	else if (tims->twait > FSMC_TWAIT_MASK)
+		tims->twait = FSMC_TWAIT_MASK;
+
+	tset = max(sdrt->tCS_min - sdrt->tWP_min,
+		   sdrt->tCEA_max - sdrt->tREA_max);
+	tims->tset = DIV_ROUND_UP(tset / 1000, hclkn) - 1;
+	if (tims->tset == 0)
+		tims->tset = 1;
+	else if (tims->tset > FSMC_TSET_MASK)
+		tims->tset = FSMC_TSET_MASK;
+
+	return 0;
+}
+
+static int fsmc_setup_data_interface(struct mtd_info *mtd, int csline,
+				     const struct nand_data_interface *conf)
+{
+	struct nand_chip *nand = mtd_to_nand(mtd);
+	struct fsmc_nand_data *host = nand_get_controller_data(nand);
+	struct fsmc_nand_timings tims;
+	const struct nand_sdr_timings *sdrt;
+	int ret;
+
+	sdrt = nand_get_sdr_timings(conf);
+	if (IS_ERR(sdrt))
+		return PTR_ERR(sdrt);
+
+	ret = fsmc_calc_timings(host, sdrt, &tims);
+	if (ret)
+		return ret;
+
+	if (csline == NAND_DATA_IFACE_CHECK_ONLY)
+		return 0;
+
+	fsmc_nand_setup(host, &tims);
+
+	return 0;
+}
+
 /*
  * fsmc_enable_hwecc - Enables Hardware ECC through FSMC registers
  */
@@ -796,10 +865,8 @@ static int fsmc_nand_probe_config_dt(struct platform_device *pdev,
 		return -ENOMEM;
 	ret = of_property_read_u8_array(np, "timings", (u8 *)host->dev_timings,
 						sizeof(*host->dev_timings));
-	if (ret) {
-		dev_info(&pdev->dev, "No timings in dts specified, using default timings!\n");
+	if (ret)
 		host->dev_timings = NULL;
-	}
 
 	/* Set default NAND bank to 0 */
 	host->bank = 0;
@@ -933,9 +1000,10 @@ static int __init fsmc_nand_probe(struct platform_device *pdev)
 		break;
 	}
 
-	fsmc_nand_setup(host->regs_va, host->bank,
-			nand->options & NAND_BUSWIDTH_16,
-			host->dev_timings);
+	if (host->dev_timings)
+		fsmc_nand_setup(host, host->dev_timings);
+	else
+		nand->setup_data_interface = fsmc_setup_data_interface;
 
 	if (AMBA_REV_BITS(host->pid) >= 8) {
 		nand->ecc.read_page = fsmc_read_page_hwecc;
@@ -986,6 +1054,9 @@ static int __init fsmc_nand_probe(struct platform_device *pdev)
 				break;
 			}
 
+		case NAND_ECC_ON_DIE:
+			break;
+
 		default:
 			dev_err(&pdev->dev, "Unsupported ECC mode!\n");
 			goto err_probe;
@@ -1073,9 +1144,8 @@ static int fsmc_nand_resume(struct device *dev)
 	struct fsmc_nand_data *host = dev_get_drvdata(dev);
 	if (host) {
 		clk_prepare_enable(host->clk);
-		fsmc_nand_setup(host->regs_va, host->bank,
-				host->nand.options & NAND_BUSWIDTH_16,
-				host->dev_timings);
+		if (host->dev_timings)
+			fsmc_nand_setup(host, host->dev_timings);
 	}
 	return 0;
 }
diff --git a/drivers/mtd/nand/gpmi-nand/gpmi-nand.c b/drivers/mtd/nand/gpmi-nand/gpmi-nand.c
index d52139635b67..50f8d4a1b983 100644
--- a/drivers/mtd/nand/gpmi-nand/gpmi-nand.c
+++ b/drivers/mtd/nand/gpmi-nand/gpmi-nand.c
@@ -82,6 +82,10 @@ static int gpmi_ooblayout_free(struct mtd_info *mtd, int section,
 	return 0;
 }
 
+static const char * const gpmi_clks_for_mx2x[] = {
+	"gpmi_io",
+};
+
 static const struct mtd_ooblayout_ops gpmi_ooblayout_ops = {
 	.ecc = gpmi_ooblayout_ecc,
 	.free = gpmi_ooblayout_free,
@@ -91,24 +95,48 @@ static const struct gpmi_devdata gpmi_devdata_imx23 = {
 	.type = IS_MX23,
 	.bch_max_ecc_strength = 20,
 	.max_chain_delay = 16,
+	.clks = gpmi_clks_for_mx2x,
+	.clks_count = ARRAY_SIZE(gpmi_clks_for_mx2x),
 };
 
 static const struct gpmi_devdata gpmi_devdata_imx28 = {
 	.type = IS_MX28,
 	.bch_max_ecc_strength = 20,
 	.max_chain_delay = 16,
+	.clks = gpmi_clks_for_mx2x,
+	.clks_count = ARRAY_SIZE(gpmi_clks_for_mx2x),
+};
+
+static const char * const gpmi_clks_for_mx6[] = {
+	"gpmi_io", "gpmi_apb", "gpmi_bch", "gpmi_bch_apb", "per1_bch",
 };
 
 static const struct gpmi_devdata gpmi_devdata_imx6q = {
 	.type = IS_MX6Q,
 	.bch_max_ecc_strength = 40,
 	.max_chain_delay = 12,
+	.clks = gpmi_clks_for_mx6,
+	.clks_count = ARRAY_SIZE(gpmi_clks_for_mx6),
 };
 
 static const struct gpmi_devdata gpmi_devdata_imx6sx = {
 	.type = IS_MX6SX,
 	.bch_max_ecc_strength = 62,
 	.max_chain_delay = 12,
+	.clks = gpmi_clks_for_mx6,
+	.clks_count = ARRAY_SIZE(gpmi_clks_for_mx6),
+};
+
+static const char * const gpmi_clks_for_mx7d[] = {
+	"gpmi_io", "gpmi_bch_apb",
+};
+
+static const struct gpmi_devdata gpmi_devdata_imx7d = {
+	.type = IS_MX7D,
+	.bch_max_ecc_strength = 62,
+	.max_chain_delay = 12,
+	.clks = gpmi_clks_for_mx7d,
+	.clks_count = ARRAY_SIZE(gpmi_clks_for_mx7d),
 };
 
 static irqreturn_t bch_irq(int irq, void *cookie)
@@ -599,35 +627,14 @@ acquire_err:
 	return -EINVAL;
 }
 
-static char *extra_clks_for_mx6q[GPMI_CLK_MAX] = {
-	"gpmi_apb", "gpmi_bch", "gpmi_bch_apb", "per1_bch",
-};
-
 static int gpmi_get_clks(struct gpmi_nand_data *this)
 {
 	struct resources *r = &this->resources;
-	char **extra_clks = NULL;
 	struct clk *clk;
 	int err, i;
 
-	/* The main clock is stored in the first. */
-	r->clock[0] = devm_clk_get(this->dev, "gpmi_io");
-	if (IS_ERR(r->clock[0])) {
-		err = PTR_ERR(r->clock[0]);
-		goto err_clock;
-	}
-
-	/* Get extra clocks */
-	if (GPMI_IS_MX6(this))
-		extra_clks = extra_clks_for_mx6q;
-	if (!extra_clks)
-		return 0;
-
-	for (i = 1; i < GPMI_CLK_MAX; i++) {
-		if (extra_clks[i - 1] == NULL)
-			break;
-
-		clk = devm_clk_get(this->dev, extra_clks[i - 1]);
+	for (i = 0; i < this->devdata->clks_count; i++) {
+		clk = devm_clk_get(this->dev, this->devdata->clks[i]);
 		if (IS_ERR(clk)) {
 			err = PTR_ERR(clk);
 			goto err_clock;
@@ -1929,12 +1936,6 @@ static int gpmi_set_geometry(struct gpmi_nand_data *this)
 	return gpmi_alloc_dma_buffer(this);
 }
 
-static void gpmi_nand_exit(struct gpmi_nand_data *this)
-{
-	nand_release(nand_to_mtd(&this->nand));
-	gpmi_free_dma_buffer(this);
-}
-
 static int gpmi_init_last(struct gpmi_nand_data *this)
 {
 	struct nand_chip *chip = &this->nand;
@@ -2048,18 +2049,20 @@ static int gpmi_nand_init(struct gpmi_nand_data *this)
 
 	ret = nand_boot_init(this);
 	if (ret)
-		goto err_out;
+		goto err_nand_cleanup;
 	ret = chip->scan_bbt(mtd);
 	if (ret)
-		goto err_out;
+		goto err_nand_cleanup;
 
 	ret = mtd_device_register(mtd, NULL, 0);
 	if (ret)
-		goto err_out;
+		goto err_nand_cleanup;
 	return 0;
 
+err_nand_cleanup:
+	nand_cleanup(chip);
 err_out:
-	gpmi_nand_exit(this);
+	gpmi_free_dma_buffer(this);
 	return ret;
 }
 
@@ -2076,6 +2079,9 @@ static const struct of_device_id gpmi_nand_id_table[] = {
 	}, {
 		.compatible = "fsl,imx6sx-gpmi-nand",
 		.data = &gpmi_devdata_imx6sx,
+	}, {
+		.compatible = "fsl,imx7d-gpmi-nand",
+		.data = &gpmi_devdata_imx7d,
 	}, {}
 };
 MODULE_DEVICE_TABLE(of, gpmi_nand_id_table);
@@ -2129,7 +2135,8 @@ static int gpmi_nand_remove(struct platform_device *pdev)
 {
 	struct gpmi_nand_data *this = platform_get_drvdata(pdev);
 
-	gpmi_nand_exit(this);
+	nand_release(nand_to_mtd(&this->nand));
+	gpmi_free_dma_buffer(this);
 	release_resources(this);
 	return 0;
 }
diff --git a/drivers/mtd/nand/gpmi-nand/gpmi-nand.h b/drivers/mtd/nand/gpmi-nand/gpmi-nand.h
index 4e49a1f5fa27..e88a45a62ab6 100644
--- a/drivers/mtd/nand/gpmi-nand/gpmi-nand.h
+++ b/drivers/mtd/nand/gpmi-nand/gpmi-nand.h
@@ -123,13 +123,16 @@ enum gpmi_type {
 	IS_MX23,
 	IS_MX28,
 	IS_MX6Q,
-	IS_MX6SX
+	IS_MX6SX,
+	IS_MX7D,
 };
 
 struct gpmi_devdata {
 	enum gpmi_type type;
 	int bch_max_ecc_strength;
 	int max_chain_delay; /* See the async EDO mode */
+	const char * const *clks;
+	const int clks_count;
 };
 
 struct gpmi_nand_data {
@@ -305,6 +308,8 @@ void gpmi_copy_bits(u8 *dst, size_t dst_bit_off,
 #define GPMI_IS_MX28(x)		((x)->devdata->type == IS_MX28)
 #define GPMI_IS_MX6Q(x)		((x)->devdata->type == IS_MX6Q)
 #define GPMI_IS_MX6SX(x)	((x)->devdata->type == IS_MX6SX)
+#define GPMI_IS_MX7D(x)		((x)->devdata->type == IS_MX7D)
 
-#define GPMI_IS_MX6(x)		(GPMI_IS_MX6Q(x) || GPMI_IS_MX6SX(x))
+#define GPMI_IS_MX6(x)		(GPMI_IS_MX6Q(x) || GPMI_IS_MX6SX(x) || \
+				 GPMI_IS_MX7D(x))
 #endif
diff --git a/drivers/mtd/nand/hisi504_nand.c b/drivers/mtd/nand/hisi504_nand.c
index e40364eeb556..530caa80b1b6 100644
--- a/drivers/mtd/nand/hisi504_nand.c
+++ b/drivers/mtd/nand/hisi504_nand.c
@@ -764,6 +764,8 @@ static int hisi_nfc_probe(struct platform_device *pdev)
 	chip->write_buf		= hisi_nfc_write_buf;
 	chip->read_buf		= hisi_nfc_read_buf;
 	chip->chip_delay	= HINFC504_CHIP_DELAY;
+	chip->onfi_set_features	= nand_onfi_get_set_features_notsupp;
+	chip->onfi_get_features	= nand_onfi_get_set_features_notsupp;
 
 	hisi_nfc_host_init(host);
 
diff --git a/drivers/mtd/nand/jz4780_nand.c b/drivers/mtd/nand/jz4780_nand.c
index a39bb70175ee..8bc835f71b26 100644
--- a/drivers/mtd/nand/jz4780_nand.c
+++ b/drivers/mtd/nand/jz4780_nand.c
@@ -205,7 +205,7 @@ static int jz4780_nand_init_ecc(struct jz4780_nand_chip *nand, struct device *de
 		return -EINVAL;
 	}
 
-	mtd->ooblayout = &nand_ooblayout_lp_ops;
+	mtd_set_ooblayout(mtd, &nand_ooblayout_lp_ops);
 
 	return 0;
 }
diff --git a/drivers/mtd/nand/mpc5121_nfc.c b/drivers/mtd/nand/mpc5121_nfc.c
index 6d6eaed2d20c..0e86fb6277c3 100644
--- a/drivers/mtd/nand/mpc5121_nfc.c
+++ b/drivers/mtd/nand/mpc5121_nfc.c
@@ -708,6 +708,8 @@ static int mpc5121_nfc_probe(struct platform_device *op)
 	chip->read_buf = mpc5121_nfc_read_buf;
 	chip->write_buf = mpc5121_nfc_write_buf;
 	chip->select_chip = mpc5121_nfc_select_chip;
+	chip->onfi_set_features	= nand_onfi_get_set_features_notsupp;
+	chip->onfi_get_features	= nand_onfi_get_set_features_notsupp;
 	chip->bbt_options = NAND_BBT_USE_FLASH;
 	chip->ecc.mode = NAND_ECC_SOFT;
 	chip->ecc.algo = NAND_ECC_HAMMING;
diff --git a/drivers/mtd/nand/mtk_ecc.c b/drivers/mtd/nand/mtk_ecc.c
index dbf256217b3e..4958121cb827 100644
--- a/drivers/mtd/nand/mtk_ecc.c
+++ b/drivers/mtd/nand/mtk_ecc.c
@@ -28,36 +28,16 @@
 
 #define ECC_IDLE_MASK		BIT(0)
 #define ECC_IRQ_EN		BIT(0)
+#define ECC_PG_IRQ_SEL		BIT(1)
 #define ECC_OP_ENABLE		(1)
 #define ECC_OP_DISABLE		(0)
 
 #define ECC_ENCCON		(0x00)
 #define ECC_ENCCNFG		(0x04)
-#define		ECC_CNFG_4BIT		(0)
-#define		ECC_CNFG_6BIT		(1)
-#define		ECC_CNFG_8BIT		(2)
-#define		ECC_CNFG_10BIT		(3)
-#define		ECC_CNFG_12BIT		(4)
-#define		ECC_CNFG_14BIT		(5)
-#define		ECC_CNFG_16BIT		(6)
-#define		ECC_CNFG_18BIT		(7)
-#define		ECC_CNFG_20BIT		(8)
-#define		ECC_CNFG_22BIT		(9)
-#define		ECC_CNFG_24BIT		(0xa)
-#define		ECC_CNFG_28BIT		(0xb)
-#define		ECC_CNFG_32BIT		(0xc)
-#define		ECC_CNFG_36BIT		(0xd)
-#define		ECC_CNFG_40BIT		(0xe)
-#define		ECC_CNFG_44BIT		(0xf)
-#define		ECC_CNFG_48BIT		(0x10)
-#define		ECC_CNFG_52BIT		(0x11)
-#define		ECC_CNFG_56BIT		(0x12)
-#define		ECC_CNFG_60BIT		(0x13)
 #define		ECC_MODE_SHIFT		(5)
 #define		ECC_MS_SHIFT		(16)
 #define ECC_ENCDIADDR		(0x08)
 #define ECC_ENCIDLE		(0x0C)
-#define ECC_ENCPAR(x)		(0x10 + (x) * sizeof(u32))
 #define ECC_ENCIRQ_EN		(0x80)
 #define ECC_ENCIRQ_STA		(0x84)
 #define ECC_DECCON		(0x100)
@@ -66,7 +46,6 @@
 #define		DEC_CNFG_CORRECT	(0x3 << 12)
 #define ECC_DECIDLE		(0x10C)
 #define ECC_DECENUM0		(0x114)
-#define		ERR_MASK		(0x3f)
 #define ECC_DECDONE		(0x124)
 #define ECC_DECIRQ_EN		(0x200)
 #define ECC_DECIRQ_STA		(0x204)
@@ -78,8 +57,17 @@
 #define ECC_IRQ_REG(op)		((op) == ECC_ENCODE ? \
 					ECC_ENCIRQ_EN : ECC_DECIRQ_EN)
 
+struct mtk_ecc_caps {
+	u32 err_mask;
+	const u8 *ecc_strength;
+	u8 num_ecc_strength;
+	u32 encode_parity_reg0;
+	int pg_irq_sel;
+};
+
 struct mtk_ecc {
 	struct device *dev;
+	const struct mtk_ecc_caps *caps;
 	void __iomem *regs;
 	struct clk *clk;
 
@@ -87,7 +75,18 @@ struct mtk_ecc {
 	struct mutex lock;
 	u32 sectors;
 
-	u8 eccdata[112];
+	u8 *eccdata;
+};
+
+/* ecc strength that each IP supports */
+static const u8 ecc_strength_mt2701[] = {
+	4, 6, 8, 10, 12, 14, 16, 18, 20, 22, 24, 28, 32, 36,
+	40, 44, 48, 52, 56, 60
+};
+
+static const u8 ecc_strength_mt2712[] = {
+	4, 6, 8, 10, 12, 14, 16, 18, 20, 22, 24, 28, 32, 36,
+	40, 44, 48, 52, 56, 60, 68, 72, 80
 };
 
 static inline void mtk_ecc_wait_idle(struct mtk_ecc *ecc,
@@ -136,77 +135,24 @@ static irqreturn_t mtk_ecc_irq(int irq, void *id)
 	return IRQ_HANDLED;
 }
 
-static void mtk_ecc_config(struct mtk_ecc *ecc, struct mtk_ecc_config *config)
+static int mtk_ecc_config(struct mtk_ecc *ecc, struct mtk_ecc_config *config)
 {
-	u32 ecc_bit = ECC_CNFG_4BIT, dec_sz, enc_sz;
-	u32 reg;
-
-	switch (config->strength) {
-	case 4:
-		ecc_bit = ECC_CNFG_4BIT;
-		break;
-	case 6:
-		ecc_bit = ECC_CNFG_6BIT;
-		break;
-	case 8:
-		ecc_bit = ECC_CNFG_8BIT;
-		break;
-	case 10:
-		ecc_bit = ECC_CNFG_10BIT;
-		break;
-	case 12:
-		ecc_bit = ECC_CNFG_12BIT;
-		break;
-	case 14:
-		ecc_bit = ECC_CNFG_14BIT;
-		break;
-	case 16:
-		ecc_bit = ECC_CNFG_16BIT;
-		break;
-	case 18:
-		ecc_bit = ECC_CNFG_18BIT;
-		break;
-	case 20:
-		ecc_bit = ECC_CNFG_20BIT;
-		break;
-	case 22:
-		ecc_bit = ECC_CNFG_22BIT;
-		break;
-	case 24:
-		ecc_bit = ECC_CNFG_24BIT;
-		break;
-	case 28:
-		ecc_bit = ECC_CNFG_28BIT;
-		break;
-	case 32:
-		ecc_bit = ECC_CNFG_32BIT;
-		break;
-	case 36:
-		ecc_bit = ECC_CNFG_36BIT;
-		break;
-	case 40:
-		ecc_bit = ECC_CNFG_40BIT;
-		break;
-	case 44:
-		ecc_bit = ECC_CNFG_44BIT;
-		break;
-	case 48:
-		ecc_bit = ECC_CNFG_48BIT;
-		break;
-	case 52:
-		ecc_bit = ECC_CNFG_52BIT;
-		break;
-	case 56:
-		ecc_bit = ECC_CNFG_56BIT;
-		break;
-	case 60:
-		ecc_bit = ECC_CNFG_60BIT;
-		break;
-	default:
-		dev_err(ecc->dev, "invalid strength %d, default to 4 bits\n",
+	u32 ecc_bit, dec_sz, enc_sz;
+	u32 reg, i;
+
+	for (i = 0; i < ecc->caps->num_ecc_strength; i++) {
+		if (ecc->caps->ecc_strength[i] == config->strength)
+			break;
+	}
+
+	if (i == ecc->caps->num_ecc_strength) {
+		dev_err(ecc->dev, "invalid ecc strength %d\n",
 			config->strength);
+		return -EINVAL;
 	}
 
+	ecc_bit = i;
+
 	if (config->op == ECC_ENCODE) {
 		/* configure ECC encoder (in bits) */
 		enc_sz = config->len << 3;
@@ -232,6 +178,8 @@ static void mtk_ecc_config(struct mtk_ecc *ecc, struct mtk_ecc_config *config)
 		if (config->sectors)
 			ecc->sectors = 1 << (config->sectors - 1);
 	}
+
+	return 0;
 }
 
 void mtk_ecc_get_stats(struct mtk_ecc *ecc, struct mtk_ecc_stats *stats,
@@ -247,8 +195,8 @@ void mtk_ecc_get_stats(struct mtk_ecc *ecc, struct mtk_ecc_stats *stats,
 		offset = (i >> 2) << 2;
 		err = readl(ecc->regs + ECC_DECENUM0 + offset);
 		err = err >> ((i % 4) * 8);
-		err &= ERR_MASK;
-		if (err == ERR_MASK) {
+		err &= ecc->caps->err_mask;
+		if (err == ecc->caps->err_mask) {
 			/* uncorrectable errors */
 			stats->failed++;
 			continue;
@@ -313,6 +261,7 @@ EXPORT_SYMBOL(of_mtk_ecc_get);
 int mtk_ecc_enable(struct mtk_ecc *ecc, struct mtk_ecc_config *config)
 {
 	enum mtk_ecc_operation op = config->op;
+	u16 reg_val;
 	int ret;
 
 	ret = mutex_lock_interruptible(&ecc->lock);
@@ -322,11 +271,23 @@ int mtk_ecc_enable(struct mtk_ecc *ecc, struct mtk_ecc_config *config)
 	}
 
 	mtk_ecc_wait_idle(ecc, op);
-	mtk_ecc_config(ecc, config);
+
+	ret = mtk_ecc_config(ecc, config);
+	if (ret)
+		return ret;
+
 	writew(ECC_OP_ENABLE, ecc->regs + ECC_CTL_REG(op));
 
 	init_completion(&ecc->done);
-	writew(ECC_IRQ_EN, ecc->regs + ECC_IRQ_REG(op));
+	reg_val = ECC_IRQ_EN;
+	/*
+	 * For ECC_NFI_MODE, if ecc->caps->pg_irq_sel is 1, then it
+	 * means this chip can only generate one ecc irq during page
+	 * read / write. If is 0, generate one ecc irq each ecc step.
+	 */
+	if ((ecc->caps->pg_irq_sel) && (config->mode == ECC_NFI_MODE))
+		reg_val |= ECC_PG_IRQ_SEL;
+	writew(reg_val, ecc->regs + ECC_IRQ_REG(op));
 
 	return 0;
 }
@@ -396,7 +357,9 @@ int mtk_ecc_encode(struct mtk_ecc *ecc, struct mtk_ecc_config *config,
 	len = (config->strength * ECC_PARITY_BITS + 7) >> 3;
 
 	/* write the parity bytes generated by the ECC back to temp buffer */
-	__ioread32_copy(ecc->eccdata, ecc->regs + ECC_ENCPAR(0), round_up(len, 4));
+	__ioread32_copy(ecc->eccdata,
+			ecc->regs + ecc->caps->encode_parity_reg0,
+			round_up(len, 4));
 
 	/* copy into possibly unaligned OOB region with actual length */
 	memcpy(data + bytes, ecc->eccdata, len);
@@ -409,37 +372,79 @@ timeout:
 }
 EXPORT_SYMBOL(mtk_ecc_encode);
 
-void mtk_ecc_adjust_strength(u32 *p)
+void mtk_ecc_adjust_strength(struct mtk_ecc *ecc, u32 *p)
 {
-	u32 ecc[] = {4, 6, 8, 10, 12, 14, 16, 18, 20, 22, 24, 28, 32, 36,
-			40, 44, 48, 52, 56, 60};
+	const u8 *ecc_strength = ecc->caps->ecc_strength;
 	int i;
 
-	for (i = 0; i < ARRAY_SIZE(ecc); i++) {
-		if (*p <= ecc[i]) {
+	for (i = 0; i < ecc->caps->num_ecc_strength; i++) {
+		if (*p <= ecc_strength[i]) {
 			if (!i)
-				*p = ecc[i];
-			else if (*p != ecc[i])
-				*p = ecc[i - 1];
+				*p = ecc_strength[i];
+			else if (*p != ecc_strength[i])
+				*p = ecc_strength[i - 1];
 			return;
 		}
 	}
 
-	*p = ecc[ARRAY_SIZE(ecc) - 1];
+	*p = ecc_strength[ecc->caps->num_ecc_strength - 1];
 }
 EXPORT_SYMBOL(mtk_ecc_adjust_strength);
 
+static const struct mtk_ecc_caps mtk_ecc_caps_mt2701 = {
+	.err_mask = 0x3f,
+	.ecc_strength = ecc_strength_mt2701,
+	.num_ecc_strength = 20,
+	.encode_parity_reg0 = 0x10,
+	.pg_irq_sel = 0,
+};
+
+static const struct mtk_ecc_caps mtk_ecc_caps_mt2712 = {
+	.err_mask = 0x7f,
+	.ecc_strength = ecc_strength_mt2712,
+	.num_ecc_strength = 23,
+	.encode_parity_reg0 = 0x300,
+	.pg_irq_sel = 1,
+};
+
+static const struct of_device_id mtk_ecc_dt_match[] = {
+	{
+		.compatible = "mediatek,mt2701-ecc",
+		.data = &mtk_ecc_caps_mt2701,
+	}, {
+		.compatible = "mediatek,mt2712-ecc",
+		.data = &mtk_ecc_caps_mt2712,
+	},
+	{},
+};
+
 static int mtk_ecc_probe(struct platform_device *pdev)
 {
 	struct device *dev = &pdev->dev;
 	struct mtk_ecc *ecc;
 	struct resource *res;
+	const struct of_device_id *of_ecc_id = NULL;
+	u32 max_eccdata_size;
 	int irq, ret;
 
 	ecc = devm_kzalloc(dev, sizeof(*ecc), GFP_KERNEL);
 	if (!ecc)
 		return -ENOMEM;
 
+	of_ecc_id = of_match_device(mtk_ecc_dt_match, &pdev->dev);
+	if (!of_ecc_id)
+		return -ENODEV;
+
+	ecc->caps = of_ecc_id->data;
+
+	max_eccdata_size = ecc->caps->num_ecc_strength - 1;
+	max_eccdata_size = ecc->caps->ecc_strength[max_eccdata_size];
+	max_eccdata_size = (max_eccdata_size * ECC_PARITY_BITS + 7) >> 3;
+	max_eccdata_size = round_up(max_eccdata_size, 4);
+	ecc->eccdata = devm_kzalloc(dev, max_eccdata_size, GFP_KERNEL);
+	if (!ecc->eccdata)
+		return -ENOMEM;
+
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	ecc->regs = devm_ioremap_resource(dev, res);
 	if (IS_ERR(ecc->regs)) {
@@ -508,11 +513,6 @@ static int mtk_ecc_resume(struct device *dev)
 static SIMPLE_DEV_PM_OPS(mtk_ecc_pm_ops, mtk_ecc_suspend, mtk_ecc_resume);
 #endif
 
-static const struct of_device_id mtk_ecc_dt_match[] = {
-	{ .compatible = "mediatek,mt2701-ecc" },
-	{},
-};
-
 MODULE_DEVICE_TABLE(of, mtk_ecc_dt_match);
 
 static struct platform_driver mtk_ecc_driver = {
diff --git a/drivers/mtd/nand/mtk_ecc.h b/drivers/mtd/nand/mtk_ecc.h
index cbeba5cd1c13..d245c14f1b80 100644
--- a/drivers/mtd/nand/mtk_ecc.h
+++ b/drivers/mtd/nand/mtk_ecc.h
@@ -42,7 +42,7 @@ void mtk_ecc_get_stats(struct mtk_ecc *, struct mtk_ecc_stats *, int);
 int mtk_ecc_wait_done(struct mtk_ecc *, enum mtk_ecc_operation);
 int mtk_ecc_enable(struct mtk_ecc *, struct mtk_ecc_config *);
 void mtk_ecc_disable(struct mtk_ecc *);
-void mtk_ecc_adjust_strength(u32 *);
+void mtk_ecc_adjust_strength(struct mtk_ecc *ecc, u32 *p);
 
 struct mtk_ecc *of_mtk_ecc_get(struct device_node *);
 void mtk_ecc_release(struct mtk_ecc *);
diff --git a/drivers/mtd/nand/mtk_nand.c b/drivers/mtd/nand/mtk_nand.c
index 6c517c682939..4d9ee278b9a0 100644
--- a/drivers/mtd/nand/mtk_nand.c
+++ b/drivers/mtd/nand/mtk_nand.c
@@ -24,6 +24,7 @@
 #include <linux/module.h>
 #include <linux/iopoll.h>
 #include <linux/of.h>
+#include <linux/of_device.h>
 #include "mtk_ecc.h"
 
 /* NAND controller register definition */
@@ -38,23 +39,6 @@
 #define NFI_PAGEFMT		(0x04)
 #define		PAGEFMT_FDM_ECC_SHIFT	(12)
 #define		PAGEFMT_FDM_SHIFT	(8)
-#define		PAGEFMT_SPARE_16	(0)
-#define		PAGEFMT_SPARE_26	(1)
-#define		PAGEFMT_SPARE_27	(2)
-#define		PAGEFMT_SPARE_28	(3)
-#define		PAGEFMT_SPARE_32	(4)
-#define		PAGEFMT_SPARE_36	(5)
-#define		PAGEFMT_SPARE_40	(6)
-#define		PAGEFMT_SPARE_44	(7)
-#define		PAGEFMT_SPARE_48	(8)
-#define		PAGEFMT_SPARE_49	(9)
-#define		PAGEFMT_SPARE_50	(0xa)
-#define		PAGEFMT_SPARE_51	(0xb)
-#define		PAGEFMT_SPARE_52	(0xc)
-#define		PAGEFMT_SPARE_62	(0xd)
-#define		PAGEFMT_SPARE_63	(0xe)
-#define		PAGEFMT_SPARE_64	(0xf)
-#define		PAGEFMT_SPARE_SHIFT	(4)
 #define		PAGEFMT_SEC_SEL_512	BIT(2)
 #define		PAGEFMT_512_2K		(0)
 #define		PAGEFMT_2K_4K		(1)
@@ -115,6 +99,13 @@
 #define MTK_RESET_TIMEOUT	(1000000)
 #define MTK_MAX_SECTOR		(16)
 #define MTK_NAND_MAX_NSELS	(2)
+#define MTK_NFC_MIN_SPARE	(16)
+
+struct mtk_nfc_caps {
+	const u8 *spare_size;
+	u8 num_spare_size;
+	u8 pageformat_spare_shift;
+};
 
 struct mtk_nfc_bad_mark_ctl {
 	void (*bm_swap)(struct mtd_info *, u8 *buf, int raw);
@@ -155,6 +146,7 @@ struct mtk_nfc {
 	struct mtk_ecc *ecc;
 
 	struct device *dev;
+	const struct mtk_nfc_caps *caps;
 	void __iomem *regs;
 
 	struct completion done;
@@ -163,6 +155,20 @@ struct mtk_nfc {
 	u8 *buffer;
 };
 
+/*
+ * supported spare size of each IP.
+ * order should be the same with the spare size bitfiled defination of
+ * register NFI_PAGEFMT.
+ */
+static const u8 spare_size_mt2701[] = {
+	16, 26, 27, 28, 32, 36, 40, 44,	48, 49, 50, 51, 52, 62, 63, 64
+};
+
+static const u8 spare_size_mt2712[] = {
+	16, 26, 27, 28, 32, 36, 40, 44, 48, 49, 50, 51, 52, 62, 61, 63, 64, 67,
+	74
+};
+
 static inline struct mtk_nfc_nand_chip *to_mtk_nand(struct nand_chip *nand)
 {
 	return container_of(nand, struct mtk_nfc_nand_chip, nand);
@@ -308,7 +314,7 @@ static int mtk_nfc_hw_runtime_config(struct mtd_info *mtd)
 	struct nand_chip *chip = mtd_to_nand(mtd);
 	struct mtk_nfc_nand_chip *mtk_nand = to_mtk_nand(chip);
 	struct mtk_nfc *nfc = nand_get_controller_data(chip);
-	u32 fmt, spare;
+	u32 fmt, spare, i;
 
 	if (!mtd->writesize)
 		return 0;
@@ -352,63 +358,21 @@ static int mtk_nfc_hw_runtime_config(struct mtd_info *mtd)
 	if (chip->ecc.size == 1024)
 		spare >>= 1;
 
-	switch (spare) {
-	case 16:
-		fmt |= (PAGEFMT_SPARE_16 << PAGEFMT_SPARE_SHIFT);
-		break;
-	case 26:
-		fmt |= (PAGEFMT_SPARE_26 << PAGEFMT_SPARE_SHIFT);
-		break;
-	case 27:
-		fmt |= (PAGEFMT_SPARE_27 << PAGEFMT_SPARE_SHIFT);
-		break;
-	case 28:
-		fmt |= (PAGEFMT_SPARE_28 << PAGEFMT_SPARE_SHIFT);
-		break;
-	case 32:
-		fmt |= (PAGEFMT_SPARE_32 << PAGEFMT_SPARE_SHIFT);
-		break;
-	case 36:
-		fmt |= (PAGEFMT_SPARE_36 << PAGEFMT_SPARE_SHIFT);
-		break;
-	case 40:
-		fmt |= (PAGEFMT_SPARE_40 << PAGEFMT_SPARE_SHIFT);
-		break;
-	case 44:
-		fmt |= (PAGEFMT_SPARE_44 << PAGEFMT_SPARE_SHIFT);
-		break;
-	case 48:
-		fmt |= (PAGEFMT_SPARE_48 << PAGEFMT_SPARE_SHIFT);
-		break;
-	case 49:
-		fmt |= (PAGEFMT_SPARE_49 << PAGEFMT_SPARE_SHIFT);
-		break;
-	case 50:
-		fmt |= (PAGEFMT_SPARE_50 << PAGEFMT_SPARE_SHIFT);
-		break;
-	case 51:
-		fmt |= (PAGEFMT_SPARE_51 << PAGEFMT_SPARE_SHIFT);
-		break;
-	case 52:
-		fmt |= (PAGEFMT_SPARE_52 << PAGEFMT_SPARE_SHIFT);
-		break;
-	case 62:
-		fmt |= (PAGEFMT_SPARE_62 << PAGEFMT_SPARE_SHIFT);
-		break;
-	case 63:
-		fmt |= (PAGEFMT_SPARE_63 << PAGEFMT_SPARE_SHIFT);
-		break;
-	case 64:
-		fmt |= (PAGEFMT_SPARE_64 << PAGEFMT_SPARE_SHIFT);
-		break;
-	default:
-		dev_err(nfc->dev, "invalid spare per sector %d\n", spare);
+	for (i = 0; i < nfc->caps->num_spare_size; i++) {
+		if (nfc->caps->spare_size[i] == spare)
+			break;
+	}
+
+	if (i == nfc->caps->num_spare_size) {
+		dev_err(nfc->dev, "invalid spare size %d\n", spare);
 		return -EINVAL;
 	}
 
+	fmt |= i << nfc->caps->pageformat_spare_shift;
+
 	fmt |= mtk_nand->fdm.reg_size << PAGEFMT_FDM_SHIFT;
 	fmt |= mtk_nand->fdm.ecc_size << PAGEFMT_FDM_ECC_SHIFT;
-	nfi_writew(nfc, fmt, NFI_PAGEFMT);
+	nfi_writel(nfc, fmt, NFI_PAGEFMT);
 
 	nfc->ecc_cfg.strength = chip->ecc.strength;
 	nfc->ecc_cfg.len = chip->ecc.size + mtk_nand->fdm.ecc_size;
@@ -1009,7 +973,7 @@ static inline void mtk_nfc_hw_init(struct mtk_nfc *nfc)
 	 * 0  : poll the status of the busy/ready signal after [7:4]*16 cycles.
 	 */
 	nfi_writew(nfc, 0xf1, NFI_CNRNB);
-	nfi_writew(nfc, PAGEFMT_8K_16K, NFI_PAGEFMT);
+	nfi_writel(nfc, PAGEFMT_8K_16K, NFI_PAGEFMT);
 
 	mtk_nfc_hw_reset(nfc);
 
@@ -1131,12 +1095,12 @@ static void mtk_nfc_set_bad_mark_ctl(struct mtk_nfc_bad_mark_ctl *bm_ctl,
 	}
 }
 
-static void mtk_nfc_set_spare_per_sector(u32 *sps, struct mtd_info *mtd)
+static int mtk_nfc_set_spare_per_sector(u32 *sps, struct mtd_info *mtd)
 {
 	struct nand_chip *nand = mtd_to_nand(mtd);
-	u32 spare[] = {16, 26, 27, 28, 32, 36, 40, 44,
-			48, 49, 50, 51, 52, 62, 63, 64};
-	u32 eccsteps, i;
+	struct mtk_nfc *nfc = nand_get_controller_data(nand);
+	const u8 *spare = nfc->caps->spare_size;
+	u32 eccsteps, i, closest_spare = 0;
 
 	eccsteps = mtd->writesize / nand->ecc.size;
 	*sps = mtd->oobsize / eccsteps;
@@ -1144,28 +1108,31 @@ static void mtk_nfc_set_spare_per_sector(u32 *sps, struct mtd_info *mtd)
 	if (nand->ecc.size == 1024)
 		*sps >>= 1;
 
-	for (i = 0; i < ARRAY_SIZE(spare); i++) {
-		if (*sps <= spare[i]) {
-			if (!i)
-				*sps = spare[i];
-			else if (*sps != spare[i])
-				*sps = spare[i - 1];
-			break;
+	if (*sps < MTK_NFC_MIN_SPARE)
+		return -EINVAL;
+
+	for (i = 0; i < nfc->caps->num_spare_size; i++) {
+		if (*sps >= spare[i] && spare[i] >= spare[closest_spare]) {
+			closest_spare = i;
+			if (*sps == spare[i])
+				break;
 		}
 	}
 
-	if (i >= ARRAY_SIZE(spare))
-		*sps = spare[ARRAY_SIZE(spare) - 1];
+	*sps = spare[closest_spare];
 
 	if (nand->ecc.size == 1024)
 		*sps <<= 1;
+
+	return 0;
 }
 
 static int mtk_nfc_ecc_init(struct device *dev, struct mtd_info *mtd)
 {
 	struct nand_chip *nand = mtd_to_nand(mtd);
+	struct mtk_nfc *nfc = nand_get_controller_data(nand);
 	u32 spare;
-	int free;
+	int free, ret;
 
 	/* support only ecc hw mode */
 	if (nand->ecc.mode != NAND_ECC_HW) {
@@ -1194,7 +1161,9 @@ static int mtk_nfc_ecc_init(struct device *dev, struct mtd_info *mtd)
 			nand->ecc.size = 1024;
 		}
 
-		mtk_nfc_set_spare_per_sector(&spare, mtd);
+		ret = mtk_nfc_set_spare_per_sector(&spare, mtd);
+		if (ret)
+			return ret;
 
 		/* calculate oob bytes except ecc parity data */
 		free = ((nand->ecc.strength * ECC_PARITY_BITS) + 7) >> 3;
@@ -1214,7 +1183,7 @@ static int mtk_nfc_ecc_init(struct device *dev, struct mtd_info *mtd)
 		}
 	}
 
-	mtk_ecc_adjust_strength(&nand->ecc.strength);
+	mtk_ecc_adjust_strength(nfc->ecc, &nand->ecc.strength);
 
 	dev_info(dev, "eccsize %d eccstrength %d\n",
 		 nand->ecc.size, nand->ecc.strength);
@@ -1312,7 +1281,10 @@ static int mtk_nfc_nand_chip_init(struct device *dev, struct mtk_nfc *nfc,
 		return -EINVAL;
 	}
 
-	mtk_nfc_set_spare_per_sector(&chip->spare_per_sector, mtd);
+	ret = mtk_nfc_set_spare_per_sector(&chip->spare_per_sector, mtd);
+	if (ret)
+		return ret;
+
 	mtk_nfc_set_fdm(&chip->fdm, mtd);
 	mtk_nfc_set_bad_mark_ctl(&chip->bad_mark, mtd);
 
@@ -1354,12 +1326,37 @@ static int mtk_nfc_nand_chips_init(struct device *dev, struct mtk_nfc *nfc)
 	return 0;
 }
 
+static const struct mtk_nfc_caps mtk_nfc_caps_mt2701 = {
+	.spare_size = spare_size_mt2701,
+	.num_spare_size = 16,
+	.pageformat_spare_shift = 4,
+};
+
+static const struct mtk_nfc_caps mtk_nfc_caps_mt2712 = {
+	.spare_size = spare_size_mt2712,
+	.num_spare_size = 19,
+	.pageformat_spare_shift = 16,
+};
+
+static const struct of_device_id mtk_nfc_id_table[] = {
+	{
+		.compatible = "mediatek,mt2701-nfc",
+		.data = &mtk_nfc_caps_mt2701,
+	}, {
+		.compatible = "mediatek,mt2712-nfc",
+		.data = &mtk_nfc_caps_mt2712,
+	},
+	{}
+};
+MODULE_DEVICE_TABLE(of, mtk_nfc_id_table);
+
 static int mtk_nfc_probe(struct platform_device *pdev)
 {
 	struct device *dev = &pdev->dev;
 	struct device_node *np = dev->of_node;
 	struct mtk_nfc *nfc;
 	struct resource *res;
+	const struct of_device_id *of_nfc_id = NULL;
 	int ret, irq;
 
 	nfc = devm_kzalloc(dev, sizeof(*nfc), GFP_KERNEL);
@@ -1423,6 +1420,14 @@ static int mtk_nfc_probe(struct platform_device *pdev)
 		goto clk_disable;
 	}
 
+	of_nfc_id = of_match_device(mtk_nfc_id_table, &pdev->dev);
+	if (!of_nfc_id) {
+		ret = -ENODEV;
+		goto clk_disable;
+	}
+
+	nfc->caps = of_nfc_id->data;
+
 	platform_set_drvdata(pdev, nfc);
 
 	ret = mtk_nfc_nand_chips_init(dev, nfc);
@@ -1503,12 +1508,6 @@ static int mtk_nfc_resume(struct device *dev)
 static SIMPLE_DEV_PM_OPS(mtk_nfc_pm_ops, mtk_nfc_suspend, mtk_nfc_resume);
 #endif
 
-static const struct of_device_id mtk_nfc_id_table[] = {
-	{ .compatible = "mediatek,mt2701-nfc" },
-	{}
-};
-MODULE_DEVICE_TABLE(of, mtk_nfc_id_table);
-
 static struct platform_driver mtk_nfc_driver = {
 	.probe  = mtk_nfc_probe,
 	.remove = mtk_nfc_remove,
diff --git a/drivers/mtd/nand/mxc_nand.c b/drivers/mtd/nand/mxc_nand.c
index 61ca020c5272..a764d5ca7536 100644
--- a/drivers/mtd/nand/mxc_nand.c
+++ b/drivers/mtd/nand/mxc_nand.c
@@ -152,9 +152,8 @@ struct mxc_nand_devtype_data {
 	void (*select_chip)(struct mtd_info *mtd, int chip);
 	int (*correct_data)(struct mtd_info *mtd, u_char *dat,
 			u_char *read_ecc, u_char *calc_ecc);
-	int (*setup_data_interface)(struct mtd_info *mtd,
-				    const struct nand_data_interface *conf,
-				    bool check_only);
+	int (*setup_data_interface)(struct mtd_info *mtd, int csline,
+				    const struct nand_data_interface *conf);
 
 	/*
 	 * On i.MX21 the CONFIG2:INT bit cannot be read if interrupts are masked
@@ -1015,9 +1014,8 @@ static void preset_v1(struct mtd_info *mtd)
 	writew(0x4, NFC_V1_V2_WRPROT);
 }
 
-static int mxc_nand_v2_setup_data_interface(struct mtd_info *mtd,
-					const struct nand_data_interface *conf,
-					bool check_only)
+static int mxc_nand_v2_setup_data_interface(struct mtd_info *mtd, int csline,
+					const struct nand_data_interface *conf)
 {
 	struct nand_chip *nand_chip = mtd_to_nand(mtd);
 	struct mxc_nand_host *host = nand_get_controller_data(nand_chip);
@@ -1075,7 +1073,7 @@ static int mxc_nand_v2_setup_data_interface(struct mtd_info *mtd,
 		return -EINVAL;
 	}
 
-	if (check_only)
+	if (csline == NAND_DATA_IFACE_CHECK_ONLY)
 		return 0;
 
 	ret = clk_set_rate(host->clk, rate);
diff --git a/drivers/mtd/nand/nand_base.c b/drivers/mtd/nand/nand_base.c
index d474378ed810..836b88d21156 100644
--- a/drivers/mtd/nand/nand_base.c
+++ b/drivers/mtd/nand/nand_base.c
@@ -202,7 +202,7 @@ static int nand_ooblayout_free_lp_hamming(struct mtd_info *mtd, int section,
 	return 0;
 }
 
-const struct mtd_ooblayout_ops nand_ooblayout_lp_hamming_ops = {
+static const struct mtd_ooblayout_ops nand_ooblayout_lp_hamming_ops = {
 	.ecc = nand_ooblayout_ecc_lp_hamming,
 	.free = nand_ooblayout_free_lp_hamming,
 };
@@ -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.
 */
@@ -753,6 +755,16 @@ static void nand_command(struct mtd_info *mtd, unsigned int command,
 		return;
 
 		/* This applies to read commands */
+	case NAND_CMD_READ0:
+		/*
+		 * READ0 is sometimes used to exit GET STATUS mode. When this
+		 * is the case no address cycles are requested, and we can use
+		 * this information to detect that we should not wait for the
+		 * device to be ready.
+		 */
+		if (column == -1 && page_addr == -1)
+			return;
+
 	default:
 		/*
 		 * If we don't have access to the busy pin, we apply the given
@@ -887,6 +899,15 @@ static void nand_command_lp(struct mtd_info *mtd, unsigned int command,
 		return;
 
 	case NAND_CMD_READ0:
+		/*
+		 * READ0 is sometimes used to exit GET STATUS mode. When this
+		 * is the case no address cycles are requested, and we can use
+		 * this information to detect that READSTART should not be
+		 * issued.
+		 */
+		if (column == -1 && page_addr == -1)
+			return;
+
 		chip->cmd_ctrl(mtd, NAND_CMD_READSTART,
 			       NAND_NCE | NAND_CLE | NAND_CTRL_CHANGE);
 		chip->cmd_ctrl(mtd, NAND_CMD_NONE,
@@ -1042,12 +1063,13 @@ static int nand_wait(struct mtd_info *mtd, struct nand_chip *chip)
 /**
  * nand_reset_data_interface - Reset data interface and timings
  * @chip: The NAND chip
+ * @chipnr: Internal die id
  *
  * Reset the Data interface and timings to ONFI mode 0.
  *
  * Returns 0 for success or negative error code otherwise.
  */
-static int nand_reset_data_interface(struct nand_chip *chip)
+static int nand_reset_data_interface(struct nand_chip *chip, int chipnr)
 {
 	struct mtd_info *mtd = nand_to_mtd(chip);
 	const struct nand_data_interface *conf;
@@ -1071,7 +1093,7 @@ static int nand_reset_data_interface(struct nand_chip *chip)
 	 */
 
 	conf = nand_get_default_data_interface();
-	ret = chip->setup_data_interface(mtd, conf, false);
+	ret = chip->setup_data_interface(mtd, chipnr, conf);
 	if (ret)
 		pr_err("Failed to configure data interface to SDR timing mode 0\n");
 
@@ -1081,6 +1103,7 @@ static int nand_reset_data_interface(struct nand_chip *chip)
 /**
  * nand_setup_data_interface - Setup the best data interface and timings
  * @chip: The NAND chip
+ * @chipnr: Internal die id
  *
  * Find and configure the best data interface and NAND timings supported by
  * the chip and the driver.
@@ -1090,7 +1113,7 @@ static int nand_reset_data_interface(struct nand_chip *chip)
  *
  * Returns 0 for success or negative error code otherwise.
  */
-static int nand_setup_data_interface(struct nand_chip *chip)
+static int nand_setup_data_interface(struct nand_chip *chip, int chipnr)
 {
 	struct mtd_info *mtd = nand_to_mtd(chip);
 	int ret;
@@ -1114,7 +1137,7 @@ static int nand_setup_data_interface(struct nand_chip *chip)
 			goto err;
 	}
 
-	ret = chip->setup_data_interface(mtd, chip->data_interface, false);
+	ret = chip->setup_data_interface(mtd, chipnr, chip->data_interface);
 err:
 	return ret;
 }
@@ -1165,8 +1188,10 @@ static int nand_init_data_interface(struct nand_chip *chip)
 		if (ret)
 			continue;
 
-		ret = chip->setup_data_interface(mtd, chip->data_interface,
-						 true);
+		/* Pass -1 to only */
+		ret = chip->setup_data_interface(mtd,
+						 NAND_DATA_IFACE_CHECK_ONLY,
+						 chip->data_interface);
 		if (!ret) {
 			chip->onfi_timing_mode_default = mode;
 			break;
@@ -1193,7 +1218,7 @@ int nand_reset(struct nand_chip *chip, int chipnr)
 	struct mtd_info *mtd = nand_to_mtd(chip);
 	int ret;
 
-	ret = nand_reset_data_interface(chip);
+	ret = nand_reset_data_interface(chip, chipnr);
 	if (ret)
 		return ret;
 
@@ -1206,7 +1231,7 @@ int nand_reset(struct nand_chip *chip, int chipnr)
 	chip->select_chip(mtd, -1);
 
 	chip->select_chip(mtd, chipnr);
-	ret = nand_setup_data_interface(chip);
+	ret = nand_setup_data_interface(chip, chipnr);
 	chip->select_chip(mtd, -1);
 	if (ret)
 		return ret;
@@ -1219,9 +1244,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.
@@ -1421,7 +1447,10 @@ static int nand_check_erased_buf(void *buf, int len, int bitflips_threshold)
 
 	for (; len >= sizeof(long);
 	     len -= sizeof(long), bitmap += sizeof(long)) {
-		weight = hweight_long(*((unsigned long *)bitmap));
+		unsigned long d = *((unsigned long *)bitmap);
+		if (d == ~0UL)
+			continue;
+		weight = hweight_long(d);
 		bitflips += BITS_PER_LONG - weight;
 		if (unlikely(bitflips > bitflips_threshold))
 			return -EBADMSG;
@@ -1524,14 +1553,15 @@ EXPORT_SYMBOL(nand_check_erased_ecc_chunk);
  *
  * Not for syndrome calculating ECC controllers, which use a special oob layout.
  */
-static int nand_read_page_raw(struct mtd_info *mtd, struct nand_chip *chip,
-			      uint8_t *buf, int oob_required, int page)
+int nand_read_page_raw(struct mtd_info *mtd, struct nand_chip *chip,
+		       uint8_t *buf, int oob_required, int page)
 {
 	chip->read_buf(mtd, buf, mtd->writesize);
 	if (oob_required)
 		chip->read_buf(mtd, chip->oob_poi, mtd->oobsize);
 	return 0;
 }
+EXPORT_SYMBOL(nand_read_page_raw);
 
 /**
  * nand_read_page_raw_syndrome - [INTERN] read raw page data without ecc
@@ -2469,8 +2499,8 @@ static int nand_read_oob(struct mtd_info *mtd, loff_t from,
  *
  * Not for syndrome calculating ECC controllers, which use a special oob layout.
  */
-static int nand_write_page_raw(struct mtd_info *mtd, struct nand_chip *chip,
-			       const uint8_t *buf, int oob_required, int page)
+int nand_write_page_raw(struct mtd_info *mtd, struct nand_chip *chip,
+			const uint8_t *buf, int oob_required, int page)
 {
 	chip->write_buf(mtd, buf, mtd->writesize);
 	if (oob_required)
@@ -2478,6 +2508,7 @@ static int nand_write_page_raw(struct mtd_info *mtd, struct nand_chip *chip,
 
 	return 0;
 }
+EXPORT_SYMBOL(nand_write_page_raw);
 
 /**
  * nand_write_page_raw_syndrome - [INTERN] raw page write function
@@ -2715,7 +2746,7 @@ static int nand_write_page_syndrome(struct mtd_info *mtd,
  */
 static int nand_write_page(struct mtd_info *mtd, struct nand_chip *chip,
 		uint32_t offset, int data_len, const uint8_t *buf,
-		int oob_required, int page, int cached, int raw)
+		int oob_required, int page, int raw)
 {
 	int status, subpage;
 
@@ -2741,30 +2772,12 @@ static int nand_write_page(struct mtd_info *mtd, struct nand_chip *chip,
 	if (status < 0)
 		return status;
 
-	/*
-	 * Cached progamming disabled for now. Not sure if it's worth the
-	 * trouble. The speed gain is not very impressive. (2.3->2.6Mib/s).
-	 */
-	cached = 0;
+	if (nand_standard_page_accessors(&chip->ecc)) {
+		chip->cmdfunc(mtd, NAND_CMD_PAGEPROG, -1, -1);
 
-	if (!cached || !NAND_HAS_CACHEPROG(chip)) {
-
-		if (nand_standard_page_accessors(&chip->ecc))
-			chip->cmdfunc(mtd, NAND_CMD_PAGEPROG, -1, -1);
 		status = chip->waitfunc(mtd, chip);
-		/*
-		 * See if operation failed and additional status checks are
-		 * available.
-		 */
-		if ((status & NAND_STATUS_FAIL) && (chip->errstat))
-			status = chip->errstat(mtd, chip, FL_WRITING, status,
-					       page);
-
 		if (status & NAND_STATUS_FAIL)
 			return -EIO;
-	} else {
-		chip->cmdfunc(mtd, NAND_CMD_CACHEDPROG, -1, -1);
-		status = chip->waitfunc(mtd, chip);
 	}
 
 	return 0;
@@ -2872,7 +2885,6 @@ static int nand_do_write_ops(struct mtd_info *mtd, loff_t to,
 
 	while (1) {
 		int bytes = mtd->writesize;
-		int cached = writelen > bytes && page != blockmask;
 		uint8_t *wbuf = buf;
 		int use_bufpoi;
 		int part_pagewr = (column || writelen < mtd->writesize);
@@ -2890,7 +2902,6 @@ static int nand_do_write_ops(struct mtd_info *mtd, loff_t to,
 		if (use_bufpoi) {
 			pr_debug("%s: using write bounce buffer for buf@%p\n",
 					 __func__, buf);
-			cached = 0;
 			if (part_pagewr)
 				bytes = min_t(int, bytes - column, writelen);
 			chip->pagebuf = -1;
@@ -2909,7 +2920,7 @@ static int nand_do_write_ops(struct mtd_info *mtd, loff_t to,
 		}
 
 		ret = nand_write_page(mtd, chip, column, bytes, wbuf,
-				      oob_required, page, cached,
+				      oob_required, page,
 				      (ops->mode == MTD_OPS_RAW));
 		if (ret)
 			break;
@@ -3225,14 +3236,6 @@ int nand_erase_nand(struct mtd_info *mtd, struct erase_info *instr,
 
 		status = chip->erase(mtd, page & chip->pagemask);
 
-		/*
-		 * See if operation failed and additional status checks are
-		 * available
-		 */
-		if ((status & NAND_STATUS_FAIL) && (chip->errstat))
-			status = chip->errstat(mtd, chip, FL_ERASING,
-					       status, page);
-
 		/* See if block erase succeeded */
 		if (status & NAND_STATUS_FAIL) {
 			pr_debug("%s: failed erase, page 0x%08x\n",
@@ -3419,6 +3422,25 @@ static int nand_onfi_get_features(struct mtd_info *mtd, struct nand_chip *chip,
 }
 
 /**
+ * nand_onfi_get_set_features_notsupp - set/get features stub returning
+ *					-ENOTSUPP
+ * @mtd: MTD device structure
+ * @chip: nand chip info structure
+ * @addr: feature address.
+ * @subfeature_param: the subfeature parameters, a four bytes array.
+ *
+ * Should be used by NAND controller drivers that do not support the SET/GET
+ * FEATURES operations.
+ */
+int nand_onfi_get_set_features_notsupp(struct mtd_info *mtd,
+				       struct nand_chip *chip, int addr,
+				       u8 *subfeature_param)
+{
+	return -ENOTSUPP;
+}
+EXPORT_SYMBOL(nand_onfi_get_set_features_notsupp);
+
+/**
  * nand_suspend - [MTD Interface] Suspend the NAND flash
  * @mtd: MTD device structure
  */
@@ -4177,6 +4199,7 @@ static const char * const nand_ecc_modes[] = {
 	[NAND_ECC_HW]		= "hw",
 	[NAND_ECC_HW_SYNDROME]	= "hw_syndrome",
 	[NAND_ECC_HW_OOB_FIRST]	= "hw_oob_first",
+	[NAND_ECC_ON_DIE]	= "on-die",
 };
 
 static int of_get_nand_ecc_mode(struct device_node *np)
@@ -4361,7 +4384,7 @@ int nand_scan_ident(struct mtd_info *mtd, int maxchips,
 	/* Initialize the ->data_interface field. */
 	ret = nand_init_data_interface(chip);
 	if (ret)
-		return ret;
+		goto err_nand_init;
 
 	/*
 	 * Setup the data interface correctly on the chip and controller side.
@@ -4371,9 +4394,9 @@ int nand_scan_ident(struct mtd_info *mtd, int maxchips,
 	 * For the other dies, nand_reset() will automatically switch to the
 	 * best mode for us.
 	 */
-	ret = nand_setup_data_interface(chip);
+	ret = nand_setup_data_interface(chip, 0);
 	if (ret)
-		return ret;
+		goto err_nand_init;
 
 	nand_maf_id = chip->id.data[0];
 	nand_dev_id = chip->id.data[1];
@@ -4404,6 +4427,12 @@ int nand_scan_ident(struct mtd_info *mtd, int maxchips,
 	mtd->size = i * chip->chipsize;
 
 	return 0;
+
+err_nand_init:
+	/* Free manufacturer priv data. */
+	nand_manufacturer_cleanup(chip);
+
+	return ret;
 }
 EXPORT_SYMBOL(nand_scan_ident);
 
@@ -4574,18 +4603,23 @@ int nand_scan_tail(struct mtd_info *mtd)
 
 	/* New bad blocks should be marked in OOB, flash-based BBT, or both */
 	if (WARN_ON((chip->bbt_options & NAND_BBT_NO_OOB_BBM) &&
-		   !(chip->bbt_options & NAND_BBT_USE_FLASH)))
-		return -EINVAL;
+		   !(chip->bbt_options & NAND_BBT_USE_FLASH))) {
+		ret = -EINVAL;
+		goto err_ident;
+	}
 
 	if (invalid_ecc_page_accessors(chip)) {
 		pr_err("Invalid ECC page accessors setup\n");
-		return -EINVAL;
+		ret = -EINVAL;
+		goto err_ident;
 	}
 
 	if (!(chip->options & NAND_OWN_BUFFERS)) {
 		nbuf = kzalloc(sizeof(*nbuf), GFP_KERNEL);
-		if (!nbuf)
-			return -ENOMEM;
+		if (!nbuf) {
+			ret = -ENOMEM;
+			goto err_ident;
+		}
 
 		nbuf->ecccalc = kmalloc(mtd->oobsize, GFP_KERNEL);
 		if (!nbuf->ecccalc) {
@@ -4608,8 +4642,10 @@ int nand_scan_tail(struct mtd_info *mtd)
 
 		chip->buffers = nbuf;
 	} else {
-		if (!chip->buffers)
-			return -ENOMEM;
+		if (!chip->buffers) {
+			ret = -ENOMEM;
+			goto err_ident;
+		}
 	}
 
 	/* Set the internal oob buffer location, just after the page data */
@@ -4717,6 +4753,18 @@ int nand_scan_tail(struct mtd_info *mtd)
 		}
 		break;
 
+	case NAND_ECC_ON_DIE:
+		if (!ecc->read_page || !ecc->write_page) {
+			WARN(1, "No ECC functions supplied; on-die ECC not possible\n");
+			ret = -EINVAL;
+			goto err_free;
+		}
+		if (!ecc->read_oob)
+			ecc->read_oob = nand_read_oob_std;
+		if (!ecc->write_oob)
+			ecc->write_oob = nand_write_oob_std;
+		break;
+
 	case NAND_ECC_NONE:
 		pr_warn("NAND_ECC_NONE selected by board driver. This is not recommended!\n");
 		ecc->read_page = nand_read_page_raw;
@@ -4757,6 +4805,11 @@ int nand_scan_tail(struct mtd_info *mtd)
 		goto err_free;
 	}
 	ecc->total = ecc->steps * ecc->bytes;
+	if (ecc->total > mtd->oobsize) {
+		WARN(1, "Total number of ECC bytes exceeded oobsize\n");
+		ret = -EINVAL;
+		goto err_free;
+	}
 
 	/*
 	 * The number of bytes available for a client to place data into
@@ -4842,7 +4895,11 @@ int nand_scan_tail(struct mtd_info *mtd)
 		return 0;
 
 	/* Build bad block table */
-	return chip->scan_bbt(mtd);
+	ret = chip->scan_bbt(mtd);
+	if (ret)
+		goto err_free;
+	return 0;
+
 err_free:
 	if (nbuf) {
 		kfree(nbuf->databuf);
@@ -4850,6 +4907,13 @@ err_free:
 		kfree(nbuf->ecccalc);
 		kfree(nbuf);
 	}
+
+err_ident:
+	/* Clean up nand_scan_ident(). */
+
+	/* Free manufacturer priv data. */
+	nand_manufacturer_cleanup(chip);
+
 	return ret;
 }
 EXPORT_SYMBOL(nand_scan_tail);
diff --git a/drivers/mtd/nand/nand_ids.c b/drivers/mtd/nand/nand_ids.c
index 9d5ca0e540b5..92e2cf8e9ff9 100644
--- a/drivers/mtd/nand/nand_ids.c
+++ b/drivers/mtd/nand/nand_ids.c
@@ -6,7 +6,6 @@
  * published by the Free Software Foundation.
  *
  */
-#include <linux/module.h>
 #include <linux/mtd/nand.h>
 #include <linux/sizes.h>
 
diff --git a/drivers/mtd/nand/nand_micron.c b/drivers/mtd/nand/nand_micron.c
index 877011069251..f3c35fafd51a 100644
--- a/drivers/mtd/nand/nand_micron.c
+++ b/drivers/mtd/nand/nand_micron.c
@@ -17,6 +17,12 @@
 
 #include <linux/mtd/nand.h>
 
+/*
+ * Special Micron status bit that indicates when the block has been
+ * corrected by on-die ECC and should be rewritten
+ */
+#define NAND_STATUS_WRITE_RECOMMENDED	BIT(3)
+
 struct nand_onfi_vendor_micron {
 	u8 two_plane_read;
 	u8 read_cache;
@@ -66,9 +72,197 @@ static int micron_nand_onfi_init(struct nand_chip *chip)
 	return 0;
 }
 
+static int micron_nand_on_die_ooblayout_ecc(struct mtd_info *mtd, int section,
+					    struct mtd_oob_region *oobregion)
+{
+	if (section >= 4)
+		return -ERANGE;
+
+	oobregion->offset = (section * 16) + 8;
+	oobregion->length = 8;
+
+	return 0;
+}
+
+static int micron_nand_on_die_ooblayout_free(struct mtd_info *mtd, int section,
+					     struct mtd_oob_region *oobregion)
+{
+	if (section >= 4)
+		return -ERANGE;
+
+	oobregion->offset = (section * 16) + 2;
+	oobregion->length = 6;
+
+	return 0;
+}
+
+static const struct mtd_ooblayout_ops micron_nand_on_die_ooblayout_ops = {
+	.ecc = micron_nand_on_die_ooblayout_ecc,
+	.free = micron_nand_on_die_ooblayout_free,
+};
+
+static int micron_nand_on_die_ecc_setup(struct nand_chip *chip, bool enable)
+{
+	u8 feature[ONFI_SUBFEATURE_PARAM_LEN] = { 0, };
+
+	if (enable)
+		feature[0] |= ONFI_FEATURE_ON_DIE_ECC_EN;
+
+	return chip->onfi_set_features(nand_to_mtd(chip), chip,
+				       ONFI_FEATURE_ON_DIE_ECC, feature);
+}
+
+static int
+micron_nand_read_page_on_die_ecc(struct mtd_info *mtd, struct nand_chip *chip,
+				 uint8_t *buf, int oob_required,
+				 int page)
+{
+	int status;
+	int max_bitflips = 0;
+
+	micron_nand_on_die_ecc_setup(chip, true);
+
+	chip->cmdfunc(mtd, NAND_CMD_READ0, 0x00, page);
+	chip->cmdfunc(mtd, NAND_CMD_STATUS, -1, -1);
+	status = chip->read_byte(mtd);
+	if (status & NAND_STATUS_FAIL)
+		mtd->ecc_stats.failed++;
+	/*
+	 * The internal ECC doesn't tell us the number of bitflips
+	 * that have been corrected, but tells us if it recommends to
+	 * rewrite the block. If it's the case, then we pretend we had
+	 * a number of bitflips equal to the ECC strength, which will
+	 * hint the NAND core to rewrite the block.
+	 */
+	else if (status & NAND_STATUS_WRITE_RECOMMENDED)
+		max_bitflips = chip->ecc.strength;
+
+	chip->cmdfunc(mtd, NAND_CMD_READ0, -1, -1);
+
+	nand_read_page_raw(mtd, chip, buf, oob_required, page);
+
+	micron_nand_on_die_ecc_setup(chip, false);
+
+	return max_bitflips;
+}
+
+static int
+micron_nand_write_page_on_die_ecc(struct mtd_info *mtd, struct nand_chip *chip,
+				  const uint8_t *buf, int oob_required,
+				  int page)
+{
+	int status;
+
+	micron_nand_on_die_ecc_setup(chip, true);
+
+	chip->cmdfunc(mtd, NAND_CMD_SEQIN, 0x00, page);
+	nand_write_page_raw(mtd, chip, buf, oob_required, page);
+	chip->cmdfunc(mtd, NAND_CMD_PAGEPROG, -1, -1);
+	status = chip->waitfunc(mtd, chip);
+
+	micron_nand_on_die_ecc_setup(chip, false);
+
+	return status & NAND_STATUS_FAIL ? -EIO : 0;
+}
+
+static int
+micron_nand_read_page_raw_on_die_ecc(struct mtd_info *mtd,
+				     struct nand_chip *chip,
+				     uint8_t *buf, int oob_required,
+				     int page)
+{
+	chip->cmdfunc(mtd, NAND_CMD_READ0, 0x00, page);
+	nand_read_page_raw(mtd, chip, buf, oob_required, page);
+
+	return 0;
+}
+
+static int
+micron_nand_write_page_raw_on_die_ecc(struct mtd_info *mtd,
+				      struct nand_chip *chip,
+				      const uint8_t *buf, int oob_required,
+				      int page)
+{
+	int status;
+
+	chip->cmdfunc(mtd, NAND_CMD_SEQIN, 0x00, page);
+	nand_write_page_raw(mtd, chip, buf, oob_required, page);
+	chip->cmdfunc(mtd, NAND_CMD_PAGEPROG, -1, -1);
+	status = chip->waitfunc(mtd, chip);
+
+	return status & NAND_STATUS_FAIL ? -EIO : 0;
+}
+
+enum {
+	/* The NAND flash doesn't support on-die ECC */
+	MICRON_ON_DIE_UNSUPPORTED,
+
+	/*
+	 * The NAND flash supports on-die ECC and it can be
+	 * enabled/disabled by a set features command.
+	 */
+	MICRON_ON_DIE_SUPPORTED,
+
+	/*
+	 * The NAND flash supports on-die ECC, and it cannot be
+	 * disabled.
+	 */
+	MICRON_ON_DIE_MANDATORY,
+};
+
+/*
+ * Try to detect if the NAND support on-die ECC. To do this, we enable
+ * the feature, and read back if it has been enabled as expected. We
+ * also check if it can be disabled, because some Micron NANDs do not
+ * allow disabling the on-die ECC and we don't support such NANDs for
+ * now.
+ *
+ * This function also has the side effect of disabling on-die ECC if
+ * it had been left enabled by the firmware/bootloader.
+ */
+static int micron_supports_on_die_ecc(struct nand_chip *chip)
+{
+	u8 feature[ONFI_SUBFEATURE_PARAM_LEN] = { 0, };
+	int ret;
+
+	if (chip->onfi_version == 0)
+		return MICRON_ON_DIE_UNSUPPORTED;
+
+	if (chip->bits_per_cell != 1)
+		return MICRON_ON_DIE_UNSUPPORTED;
+
+	ret = micron_nand_on_die_ecc_setup(chip, true);
+	if (ret)
+		return MICRON_ON_DIE_UNSUPPORTED;
+
+	chip->onfi_get_features(nand_to_mtd(chip), chip,
+				ONFI_FEATURE_ON_DIE_ECC, feature);
+	if ((feature[0] & ONFI_FEATURE_ON_DIE_ECC_EN) == 0)
+		return MICRON_ON_DIE_UNSUPPORTED;
+
+	ret = micron_nand_on_die_ecc_setup(chip, false);
+	if (ret)
+		return MICRON_ON_DIE_UNSUPPORTED;
+
+	chip->onfi_get_features(nand_to_mtd(chip), chip,
+				ONFI_FEATURE_ON_DIE_ECC, feature);
+	if (feature[0] & ONFI_FEATURE_ON_DIE_ECC_EN)
+		return MICRON_ON_DIE_MANDATORY;
+
+	/*
+	 * Some Micron NANDs have an on-die ECC of 4/512, some other
+	 * 8/512. We only support the former.
+	 */
+	if (chip->onfi_params.ecc_bits != 4)
+		return MICRON_ON_DIE_UNSUPPORTED;
+
+	return MICRON_ON_DIE_SUPPORTED;
+}
+
 static int micron_nand_init(struct nand_chip *chip)
 {
 	struct mtd_info *mtd = nand_to_mtd(chip);
+	int ondie;
 	int ret;
 
 	ret = micron_nand_onfi_init(chip);
@@ -78,6 +272,33 @@ static int micron_nand_init(struct nand_chip *chip)
 	if (mtd->writesize == 2048)
 		chip->bbt_options |= NAND_BBT_SCAN2NDPAGE;
 
+	ondie = micron_supports_on_die_ecc(chip);
+
+	if (ondie == MICRON_ON_DIE_MANDATORY) {
+		pr_err("On-die ECC forcefully enabled, not supported\n");
+		return -EINVAL;
+	}
+
+	if (chip->ecc.mode == NAND_ECC_ON_DIE) {
+		if (ondie == MICRON_ON_DIE_UNSUPPORTED) {
+			pr_err("On-die ECC selected but not supported\n");
+			return -EINVAL;
+		}
+
+		chip->ecc.options = NAND_ECC_CUSTOM_PAGE_ACCESS;
+		chip->ecc.bytes = 32;
+		chip->ecc.strength = 4;
+		chip->ecc.algo = NAND_ECC_BCH;
+		chip->ecc.read_page = micron_nand_read_page_on_die_ecc;
+		chip->ecc.write_page = micron_nand_write_page_on_die_ecc;
+		chip->ecc.read_page_raw =
+			micron_nand_read_page_raw_on_die_ecc;
+		chip->ecc.write_page_raw =
+			micron_nand_write_page_raw_on_die_ecc;
+
+		mtd_set_ooblayout(mtd, &micron_nand_on_die_ooblayout_ops);
+	}
+
 	return 0;
 }
 
diff --git a/drivers/mtd/nand/nand_samsung.c b/drivers/mtd/nand/nand_samsung.c
index 9cfc4035a420..1e0755997762 100644
--- a/drivers/mtd/nand/nand_samsung.c
+++ b/drivers/mtd/nand/nand_samsung.c
@@ -84,6 +84,9 @@ static void samsung_nand_decode_id(struct nand_chip *chip)
 			case 7:
 				chip->ecc_strength_ds = 60;
 				break;
+			default:
+				WARN(1, "Could not decode ECC info");
+				chip->ecc_step_ds = 0;
 			}
 		}
 	} else {
diff --git a/drivers/mtd/nand/pxa3xx_nand.c b/drivers/mtd/nand/pxa3xx_nand.c
index 649ba8200832..74dae4bbdac8 100644
--- a/drivers/mtd/nand/pxa3xx_nand.c
+++ b/drivers/mtd/nand/pxa3xx_nand.c
@@ -1812,6 +1812,8 @@ static int alloc_nand_resource(struct platform_device *pdev)
 		chip->write_buf		= pxa3xx_nand_write_buf;
 		chip->options		|= NAND_NO_SUBPAGE_WRITE;
 		chip->cmdfunc		= nand_cmdfunc;
+		chip->onfi_set_features	= nand_onfi_get_set_features_notsupp;
+		chip->onfi_get_features	= nand_onfi_get_set_features_notsupp;
 	}
 
 	nand_hw_control_init(chip->controller);
diff --git a/drivers/mtd/nand/qcom_nandc.c b/drivers/mtd/nand/qcom_nandc.c
index 57d483ac5765..88af7145a51a 100644
--- a/drivers/mtd/nand/qcom_nandc.c
+++ b/drivers/mtd/nand/qcom_nandc.c
@@ -2008,6 +2008,8 @@ static int qcom_nand_host_init(struct qcom_nand_controller *nandc,
 	chip->read_byte		= qcom_nandc_read_byte;
 	chip->read_buf		= qcom_nandc_read_buf;
 	chip->write_buf		= qcom_nandc_write_buf;
+	chip->onfi_set_features	= nand_onfi_get_set_features_notsupp;
+	chip->onfi_get_features	= nand_onfi_get_set_features_notsupp;
 
 	/*
 	 * the bad block marker is readable only when we read the last codeword
diff --git a/drivers/mtd/nand/s3c2410.c b/drivers/mtd/nand/s3c2410.c
index f0b030d44f71..9e0c849607b9 100644
--- a/drivers/mtd/nand/s3c2410.c
+++ b/drivers/mtd/nand/s3c2410.c
@@ -812,9 +812,8 @@ static int s3c2410_nand_add_partition(struct s3c2410_nand_info *info,
 	return -ENODEV;
 }
 
-static int s3c2410_nand_setup_data_interface(struct mtd_info *mtd,
-					const struct nand_data_interface *conf,
-					bool check_only)
+static int s3c2410_nand_setup_data_interface(struct mtd_info *mtd, int csline,
+					const struct nand_data_interface *conf)
 {
 	struct s3c2410_nand_info *info = s3c2410_nand_mtd_toinfo(mtd);
 	struct s3c2410_platform_nand *pdata = info->platform;
diff --git a/drivers/mtd/nand/sh_flctl.c b/drivers/mtd/nand/sh_flctl.c
index 442ce619b3b6..891ac7b99305 100644
--- a/drivers/mtd/nand/sh_flctl.c
+++ b/drivers/mtd/nand/sh_flctl.c
@@ -1183,6 +1183,8 @@ static int flctl_probe(struct platform_device *pdev)
 	nand->read_buf = flctl_read_buf;
 	nand->select_chip = flctl_select_chip;
 	nand->cmdfunc = flctl_cmdfunc;
+	nand->onfi_set_features = nand_onfi_get_set_features_notsupp;
+	nand->onfi_get_features = nand_onfi_get_set_features_notsupp;
 
 	if (pdata->flcmncr_val & SEL_16BIT)
 		nand->options |= NAND_BUSWIDTH_16;
diff --git a/drivers/mtd/nand/sunxi_nand.c b/drivers/mtd/nand/sunxi_nand.c
index 118a26fff368..d0b6f8f9f297 100644
--- a/drivers/mtd/nand/sunxi_nand.c
+++ b/drivers/mtd/nand/sunxi_nand.c
@@ -1301,7 +1301,6 @@ static int sunxi_nfc_hw_ecc_read_subpage(struct mtd_info *mtd,
 
 	sunxi_nfc_hw_ecc_enable(mtd);
 
-	chip->cmdfunc(mtd, NAND_CMD_READ0, 0, page);
 	for (i = data_offs / ecc->size;
 	     i < DIV_ROUND_UP(data_offs + readlen, ecc->size); i++) {
 		int data_off = i * ecc->size;
@@ -1592,9 +1591,8 @@ static int _sunxi_nand_lookup_timing(const s32 *lut, int lut_size, u32 duration,
 #define sunxi_nand_lookup_timing(l, p, c) \
 			_sunxi_nand_lookup_timing(l, ARRAY_SIZE(l), p, c)
 
-static int sunxi_nfc_setup_data_interface(struct mtd_info *mtd,
-					const struct nand_data_interface *conf,
-					bool check_only)
+static int sunxi_nfc_setup_data_interface(struct mtd_info *mtd, int csline,
+					const struct nand_data_interface *conf)
 {
 	struct nand_chip *nand = mtd_to_nand(mtd);
 	struct sunxi_nand_chip *chip = to_sunxi_nand(nand);
@@ -1707,7 +1705,7 @@ static int sunxi_nfc_setup_data_interface(struct mtd_info *mtd,
 		return tRHW;
 	}
 
-	if (check_only)
+	if (csline == NAND_DATA_IFACE_CHECK_ONLY)
 		return 0;
 
 	/*
@@ -1922,7 +1920,6 @@ static int sunxi_nand_hw_ecc_ctrl_init(struct mtd_info *mtd,
 	ecc->write_subpage = sunxi_nfc_hw_ecc_write_subpage;
 	ecc->read_oob_raw = nand_read_oob_std;
 	ecc->write_oob_raw = nand_write_oob_std;
-	ecc->read_subpage = sunxi_nfc_hw_ecc_read_subpage;
 
 	return 0;
 }
diff --git a/drivers/mtd/nand/tango_nand.c b/drivers/mtd/nand/tango_nand.c
index 05b6e1065203..9d40b793b1c4 100644
--- a/drivers/mtd/nand/tango_nand.c
+++ b/drivers/mtd/nand/tango_nand.c
@@ -55,10 +55,10 @@
  * byte 1 for other packets in the page (PKT_N, for N > 0)
  * ERR_COUNT_PKT_N is the max error count over all but the first packet.
  */
-#define DECODE_OK_PKT_0(v)	((v) & BIT(7))
-#define DECODE_OK_PKT_N(v)	((v) & BIT(15))
 #define ERR_COUNT_PKT_0(v)	(((v) >> 0) & 0x3f)
 #define ERR_COUNT_PKT_N(v)	(((v) >> 8) & 0x3f)
+#define DECODE_FAIL_PKT_0(v)	(((v) & BIT(7)) == 0)
+#define DECODE_FAIL_PKT_N(v)	(((v) & BIT(15)) == 0)
 
 /* Offsets relative to pbus_base */
 #define PBUS_CS_CTRL	0x83c
@@ -193,6 +193,8 @@ static int check_erased_page(struct nand_chip *chip, u8 *buf)
 						  chip->ecc.strength);
 		if (res < 0)
 			mtd->ecc_stats.failed++;
+		else
+			mtd->ecc_stats.corrected += res;
 
 		bitflips = max(res, bitflips);
 		buf += pkt_size;
@@ -202,9 +204,11 @@ static int check_erased_page(struct nand_chip *chip, u8 *buf)
 	return bitflips;
 }
 
-static int decode_error_report(struct tango_nfc *nfc)
+static int decode_error_report(struct nand_chip *chip)
 {
 	u32 status, res;
+	struct mtd_info *mtd = nand_to_mtd(chip);
+	struct tango_nfc *nfc = to_tango_nfc(chip->controller);
 
 	status = readl_relaxed(nfc->reg_base + NFC_XFER_STATUS);
 	if (status & PAGE_IS_EMPTY)
@@ -212,10 +216,14 @@ static int decode_error_report(struct tango_nfc *nfc)
 
 	res = readl_relaxed(nfc->mem_base + ERROR_REPORT);
 
-	if (DECODE_OK_PKT_0(res) && DECODE_OK_PKT_N(res))
-		return max(ERR_COUNT_PKT_0(res), ERR_COUNT_PKT_N(res));
+	if (DECODE_FAIL_PKT_0(res) || DECODE_FAIL_PKT_N(res))
+		return -EBADMSG;
+
+	/* ERR_COUNT_PKT_N is max, not sum, but that's all we have */
+	mtd->ecc_stats.corrected +=
+		ERR_COUNT_PKT_0(res) + ERR_COUNT_PKT_N(res);
 
-	return -EBADMSG;
+	return max(ERR_COUNT_PKT_0(res), ERR_COUNT_PKT_N(res));
 }
 
 static void tango_dma_callback(void *arg)
@@ -282,7 +290,7 @@ static int tango_read_page(struct mtd_info *mtd, struct nand_chip *chip,
 	if (err)
 		return err;
 
-	res = decode_error_report(nfc);
+	res = decode_error_report(chip);
 	if (res < 0) {
 		chip->ecc.read_oob_raw(mtd, chip, page);
 		res = check_erased_page(chip, buf);
@@ -295,7 +303,7 @@ static int tango_write_page(struct mtd_info *mtd, struct nand_chip *chip,
 			    const u8 *buf, int oob_required, int page)
 {
 	struct tango_nfc *nfc = to_tango_nfc(chip->controller);
-	int err, len = mtd->writesize;
+	int err, status, len = mtd->writesize;
 
 	/* Calling tango_write_oob() would send PAGEPROG twice */
 	if (oob_required)
@@ -306,6 +314,10 @@ static int tango_write_page(struct mtd_info *mtd, struct nand_chip *chip,
 	if (err)
 		return err;
 
+	status = chip->waitfunc(mtd, chip);
+	if (status & NAND_STATUS_FAIL)
+		return -EIO;
+
 	return 0;
 }
 
@@ -332,7 +344,7 @@ static void aux_write(struct nand_chip *chip, const u8 **buf, int len, int *pos)
 
 	if (!*buf) {
 		/* skip over "len" bytes */
-		chip->cmdfunc(mtd, NAND_CMD_SEQIN, *pos, -1);
+		chip->cmdfunc(mtd, NAND_CMD_RNDIN, *pos, -1);
 	} else {
 		tango_write_buf(mtd, *buf, len);
 		*buf += len;
@@ -423,9 +435,16 @@ static int tango_read_page_raw(struct mtd_info *mtd, struct nand_chip *chip,
 static int tango_write_page_raw(struct mtd_info *mtd, struct nand_chip *chip,
 				const u8 *buf, int oob_required, int page)
 {
+	int status;
+
 	chip->cmdfunc(mtd, NAND_CMD_SEQIN, 0, page);
 	raw_write(chip, buf, chip->oob_poi);
 	chip->cmdfunc(mtd, NAND_CMD_PAGEPROG, -1, -1);
+
+	status = chip->waitfunc(mtd, chip);
+	if (status & NAND_STATUS_FAIL)
+		return -EIO;
+
 	return 0;
 }
 
@@ -476,9 +495,8 @@ static u32 to_ticks(int kHz, int ps)
 	return DIV_ROUND_UP_ULL((u64)kHz * ps, NSEC_PER_SEC);
 }
 
-static int tango_set_timings(struct mtd_info *mtd,
-			     const struct nand_data_interface *conf,
-			     bool check_only)
+static int tango_set_timings(struct mtd_info *mtd, int csline,
+			     const struct nand_data_interface *conf)
 {
 	const struct nand_sdr_timings *sdr = nand_get_sdr_timings(conf);
 	struct nand_chip *chip = mtd_to_nand(mtd);
@@ -490,7 +508,7 @@ static int tango_set_timings(struct mtd_info *mtd,
 	if (IS_ERR(sdr))
 		return PTR_ERR(sdr);
 
-	if (check_only)
+	if (csline == NAND_DATA_IFACE_CHECK_ONLY)
 		return 0;
 
 	Trdy = to_ticks(kHz, sdr->tCEA_max - sdr->tREA_max);
@@ -663,6 +681,7 @@ static const struct of_device_id tango_nand_ids[] = {
 	{ .compatible = "sigma,smp8758-nand" },
 	{ /* sentinel */ }
 };
+MODULE_DEVICE_TABLE(of, tango_nand_ids);
 
 static struct platform_driver tango_nand_driver = {
 	.probe	= tango_nand_probe,
diff --git a/drivers/mtd/nand/vf610_nfc.c b/drivers/mtd/nand/vf610_nfc.c
index 3ea4bb19e12d..744ab10e8962 100644
--- a/drivers/mtd/nand/vf610_nfc.c
+++ b/drivers/mtd/nand/vf610_nfc.c
@@ -703,6 +703,8 @@ static int vf610_nfc_probe(struct platform_device *pdev)
 	chip->read_buf = vf610_nfc_read_buf;
 	chip->write_buf = vf610_nfc_write_buf;
 	chip->select_chip = vf610_nfc_select_chip;
+	chip->onfi_set_features = nand_onfi_get_set_features_notsupp;
+	chip->onfi_get_features = nand_onfi_get_set_features_notsupp;
 
 	chip->options |= NAND_NO_SUBPAGE_WRITE;
 
diff --git a/drivers/mtd/spi-nor/Kconfig b/drivers/mtd/spi-nor/Kconfig
index bfdfb1e72b38..293c8a4d1e49 100644
--- a/drivers/mtd/spi-nor/Kconfig
+++ b/drivers/mtd/spi-nor/Kconfig
@@ -108,7 +108,7 @@ config SPI_INTEL_SPI_PLATFORM
 
 config SPI_STM32_QUADSPI
 	tristate "STM32 Quad SPI controller"
-	depends on ARCH_STM32
+	depends on ARCH_STM32 || COMPILE_TEST
 	help
 	  This enables support for the STM32 Quad SPI controller.
 	  We only connect the NOR to this controller.
diff --git a/drivers/mtd/spi-nor/aspeed-smc.c b/drivers/mtd/spi-nor/aspeed-smc.c
index 56051d30f000..3f875c8d6339 100644
--- a/drivers/mtd/spi-nor/aspeed-smc.c
+++ b/drivers/mtd/spi-nor/aspeed-smc.c
@@ -585,14 +585,12 @@ static int aspeed_smc_chip_setup_finish(struct aspeed_smc_chip *chip)
 	 * TODO: Adjust clocks if fast read is supported and interpret
 	 * SPI-NOR flags to adjust controller settings.
 	 */
-	switch (chip->nor.flash_read) {
-	case SPI_NOR_NORMAL:
-		cmd = CONTROL_COMMAND_MODE_NORMAL;
-		break;
-	case SPI_NOR_FAST:
-		cmd = CONTROL_COMMAND_MODE_FREAD;
-		break;
-	default:
+	if (chip->nor.read_proto == SNOR_PROTO_1_1_1) {
+		if (chip->nor.read_dummy == 0)
+			cmd = CONTROL_COMMAND_MODE_NORMAL;
+		else
+			cmd = CONTROL_COMMAND_MODE_FREAD;
+	} else {
 		dev_err(chip->nor.dev, "unsupported SPI read mode\n");
 		return -EINVAL;
 	}
@@ -608,6 +606,11 @@ static int aspeed_smc_chip_setup_finish(struct aspeed_smc_chip *chip)
 static int aspeed_smc_setup_flash(struct aspeed_smc_controller *controller,
 				  struct device_node *np, struct resource *r)
 {
+	const struct spi_nor_hwcaps hwcaps = {
+		.mask = SNOR_HWCAPS_READ |
+			SNOR_HWCAPS_READ_FAST |
+			SNOR_HWCAPS_PP,
+	};
 	const struct aspeed_smc_info *info = controller->info;
 	struct device *dev = controller->dev;
 	struct device_node *child;
@@ -671,11 +674,11 @@ static int aspeed_smc_setup_flash(struct aspeed_smc_controller *controller,
 			break;
 
 		/*
-		 * TODO: Add support for SPI_NOR_QUAD and SPI_NOR_DUAL
+		 * TODO: Add support for Dual and Quad SPI protocols
 		 * attach when board support is present as determined
 		 * by of property.
 		 */
-		ret = spi_nor_scan(nor, NULL, SPI_NOR_NORMAL);
+		ret = spi_nor_scan(nor, NULL, &hwcaps);
 		if (ret)
 			break;
 
diff --git a/drivers/mtd/spi-nor/atmel-quadspi.c b/drivers/mtd/spi-nor/atmel-quadspi.c
index 47937d9beec6..ba76fa8f2031 100644
--- a/drivers/mtd/spi-nor/atmel-quadspi.c
+++ b/drivers/mtd/spi-nor/atmel-quadspi.c
@@ -275,14 +275,48 @@ static void atmel_qspi_debug_command(struct atmel_qspi *aq,
 
 static int atmel_qspi_run_command(struct atmel_qspi *aq,
 				  const struct atmel_qspi_command *cmd,
-				  u32 ifr_tfrtyp, u32 ifr_width)
+				  u32 ifr_tfrtyp, enum spi_nor_protocol proto)
 {
 	u32 iar, icr, ifr, sr;
 	int err = 0;
 
 	iar = 0;
 	icr = 0;
-	ifr = ifr_tfrtyp | ifr_width;
+	ifr = ifr_tfrtyp;
+
+	/* Set the SPI protocol */
+	switch (proto) {
+	case SNOR_PROTO_1_1_1:
+		ifr |= QSPI_IFR_WIDTH_SINGLE_BIT_SPI;
+		break;
+
+	case SNOR_PROTO_1_1_2:
+		ifr |= QSPI_IFR_WIDTH_DUAL_OUTPUT;
+		break;
+
+	case SNOR_PROTO_1_1_4:
+		ifr |= QSPI_IFR_WIDTH_QUAD_OUTPUT;
+		break;
+
+	case SNOR_PROTO_1_2_2:
+		ifr |= QSPI_IFR_WIDTH_DUAL_IO;
+		break;
+
+	case SNOR_PROTO_1_4_4:
+		ifr |= QSPI_IFR_WIDTH_QUAD_IO;
+		break;
+
+	case SNOR_PROTO_2_2_2:
+		ifr |= QSPI_IFR_WIDTH_DUAL_CMD;
+		break;
+
+	case SNOR_PROTO_4_4_4:
+		ifr |= QSPI_IFR_WIDTH_QUAD_CMD;
+		break;
+
+	default:
+		return -EINVAL;
+	}
 
 	/* Compute instruction parameters */
 	if (cmd->enable.bits.instruction) {
@@ -434,7 +468,7 @@ static int atmel_qspi_read_reg(struct spi_nor *nor, u8 opcode,
 	cmd.rx_buf = buf;
 	cmd.buf_len = len;
 	return atmel_qspi_run_command(aq, &cmd, QSPI_IFR_TFRTYP_TRSFR_READ,
-				      QSPI_IFR_WIDTH_SINGLE_BIT_SPI);
+				      nor->reg_proto);
 }
 
 static int atmel_qspi_write_reg(struct spi_nor *nor, u8 opcode,
@@ -450,7 +484,7 @@ static int atmel_qspi_write_reg(struct spi_nor *nor, u8 opcode,
 	cmd.tx_buf = buf;
 	cmd.buf_len = len;
 	return atmel_qspi_run_command(aq, &cmd, QSPI_IFR_TFRTYP_TRSFR_WRITE,
-				      QSPI_IFR_WIDTH_SINGLE_BIT_SPI);
+				      nor->reg_proto);
 }
 
 static ssize_t atmel_qspi_write(struct spi_nor *nor, loff_t to, size_t len,
@@ -469,7 +503,7 @@ static ssize_t atmel_qspi_write(struct spi_nor *nor, loff_t to, size_t len,
 	cmd.tx_buf = write_buf;
 	cmd.buf_len = len;
 	ret = atmel_qspi_run_command(aq, &cmd, QSPI_IFR_TFRTYP_TRSFR_WRITE_MEM,
-				     QSPI_IFR_WIDTH_SINGLE_BIT_SPI);
+				     nor->write_proto);
 	return (ret < 0) ? ret : len;
 }
 
@@ -484,7 +518,7 @@ static int atmel_qspi_erase(struct spi_nor *nor, loff_t offs)
 	cmd.instruction = nor->erase_opcode;
 	cmd.address = (u32)offs;
 	return atmel_qspi_run_command(aq, &cmd, QSPI_IFR_TFRTYP_TRSFR_WRITE,
-				      QSPI_IFR_WIDTH_SINGLE_BIT_SPI);
+				      nor->reg_proto);
 }
 
 static ssize_t atmel_qspi_read(struct spi_nor *nor, loff_t from, size_t len,
@@ -493,27 +527,8 @@ static ssize_t atmel_qspi_read(struct spi_nor *nor, loff_t from, size_t len,
 	struct atmel_qspi *aq = nor->priv;
 	struct atmel_qspi_command cmd;
 	u8 num_mode_cycles, num_dummy_cycles;
-	u32 ifr_width;
 	ssize_t ret;
 
-	switch (nor->flash_read) {
-	case SPI_NOR_NORMAL:
-	case SPI_NOR_FAST:
-		ifr_width = QSPI_IFR_WIDTH_SINGLE_BIT_SPI;
-		break;
-
-	case SPI_NOR_DUAL:
-		ifr_width = QSPI_IFR_WIDTH_DUAL_OUTPUT;
-		break;
-
-	case SPI_NOR_QUAD:
-		ifr_width = QSPI_IFR_WIDTH_QUAD_OUTPUT;
-		break;
-
-	default:
-		return -EINVAL;
-	}
-
 	if (nor->read_dummy >= 2) {
 		num_mode_cycles = 2;
 		num_dummy_cycles = nor->read_dummy - 2;
@@ -536,7 +551,7 @@ static ssize_t atmel_qspi_read(struct spi_nor *nor, loff_t from, size_t len,
 	cmd.rx_buf = read_buf;
 	cmd.buf_len = len;
 	ret = atmel_qspi_run_command(aq, &cmd, QSPI_IFR_TFRTYP_TRSFR_READ_MEM,
-				     ifr_width);
+				     nor->read_proto);
 	return (ret < 0) ? ret : len;
 }
 
@@ -590,6 +605,20 @@ static irqreturn_t atmel_qspi_interrupt(int irq, void *dev_id)
 
 static int atmel_qspi_probe(struct platform_device *pdev)
 {
+	const struct spi_nor_hwcaps hwcaps = {
+		.mask = SNOR_HWCAPS_READ |
+			SNOR_HWCAPS_READ_FAST |
+			SNOR_HWCAPS_READ_1_1_2 |
+			SNOR_HWCAPS_READ_1_2_2 |
+			SNOR_HWCAPS_READ_2_2_2 |
+			SNOR_HWCAPS_READ_1_1_4 |
+			SNOR_HWCAPS_READ_1_4_4 |
+			SNOR_HWCAPS_READ_4_4_4 |
+			SNOR_HWCAPS_PP |
+			SNOR_HWCAPS_PP_1_1_4 |
+			SNOR_HWCAPS_PP_1_4_4 |
+			SNOR_HWCAPS_PP_4_4_4,
+	};
 	struct device_node *child, *np = pdev->dev.of_node;
 	struct atmel_qspi *aq;
 	struct resource *res;
@@ -679,7 +708,7 @@ static int atmel_qspi_probe(struct platform_device *pdev)
 	if (err)
 		goto disable_clk;
 
-	err = spi_nor_scan(nor, NULL, SPI_NOR_QUAD);
+	err = spi_nor_scan(nor, NULL, &hwcaps);
 	if (err)
 		goto disable_clk;
 
diff --git a/drivers/mtd/spi-nor/cadence-quadspi.c b/drivers/mtd/spi-nor/cadence-quadspi.c
index 9f8102de1b16..40096d73536c 100644
--- a/drivers/mtd/spi-nor/cadence-quadspi.c
+++ b/drivers/mtd/spi-nor/cadence-quadspi.c
@@ -855,15 +855,14 @@ static int cqspi_set_protocol(struct spi_nor *nor, const int read)
 	f_pdata->data_width = CQSPI_INST_TYPE_SINGLE;
 
 	if (read) {
-		switch (nor->flash_read) {
-		case SPI_NOR_NORMAL:
-		case SPI_NOR_FAST:
+		switch (nor->read_proto) {
+		case SNOR_PROTO_1_1_1:
 			f_pdata->data_width = CQSPI_INST_TYPE_SINGLE;
 			break;
-		case SPI_NOR_DUAL:
+		case SNOR_PROTO_1_1_2:
 			f_pdata->data_width = CQSPI_INST_TYPE_DUAL;
 			break;
-		case SPI_NOR_QUAD:
+		case SNOR_PROTO_1_1_4:
 			f_pdata->data_width = CQSPI_INST_TYPE_QUAD;
 			break;
 		default:
@@ -1069,6 +1068,13 @@ static void cqspi_controller_init(struct cqspi_st *cqspi)
 
 static int cqspi_setup_flash(struct cqspi_st *cqspi, struct device_node *np)
 {
+	const struct spi_nor_hwcaps hwcaps = {
+		.mask = SNOR_HWCAPS_READ |
+			SNOR_HWCAPS_READ_FAST |
+			SNOR_HWCAPS_READ_1_1_2 |
+			SNOR_HWCAPS_READ_1_1_4 |
+			SNOR_HWCAPS_PP,
+	};
 	struct platform_device *pdev = cqspi->pdev;
 	struct device *dev = &pdev->dev;
 	struct cqspi_flash_pdata *f_pdata;
@@ -1123,7 +1129,7 @@ static int cqspi_setup_flash(struct cqspi_st *cqspi, struct device_node *np)
 			goto err;
 		}
 
-		ret = spi_nor_scan(nor, NULL, SPI_NOR_QUAD);
+		ret = spi_nor_scan(nor, NULL, &hwcaps);
 		if (ret)
 			goto err;
 
diff --git a/drivers/mtd/spi-nor/fsl-quadspi.c b/drivers/mtd/spi-nor/fsl-quadspi.c
index 1476135e0d50..f17d22435bfc 100644
--- a/drivers/mtd/spi-nor/fsl-quadspi.c
+++ b/drivers/mtd/spi-nor/fsl-quadspi.c
@@ -957,6 +957,10 @@ static void fsl_qspi_unprep(struct spi_nor *nor, enum spi_nor_ops ops)
 
 static int fsl_qspi_probe(struct platform_device *pdev)
 {
+	const struct spi_nor_hwcaps hwcaps = {
+		.mask = SNOR_HWCAPS_READ_1_1_4 |
+			SNOR_HWCAPS_PP,
+	};
 	struct device_node *np = pdev->dev.of_node;
 	struct device *dev = &pdev->dev;
 	struct fsl_qspi *q;
@@ -1065,7 +1069,7 @@ static int fsl_qspi_probe(struct platform_device *pdev)
 		/* set the chip address for READID */
 		fsl_qspi_set_base_addr(q, nor);
 
-		ret = spi_nor_scan(nor, NULL, SPI_NOR_QUAD);
+		ret = spi_nor_scan(nor, NULL, &hwcaps);
 		if (ret)
 			goto mutex_failed;
 
diff --git a/drivers/mtd/spi-nor/hisi-sfc.c b/drivers/mtd/spi-nor/hisi-sfc.c
index a286350627a6..d1106832b9d5 100644
--- a/drivers/mtd/spi-nor/hisi-sfc.c
+++ b/drivers/mtd/spi-nor/hisi-sfc.c
@@ -120,19 +120,24 @@ static inline int wait_op_finish(struct hifmc_host *host)
 		(reg & FMC_INT_OP_DONE), 0, FMC_WAIT_TIMEOUT);
 }
 
-static int get_if_type(enum read_mode flash_read)
+static int get_if_type(enum spi_nor_protocol proto)
 {
 	enum hifmc_iftype if_type;
 
-	switch (flash_read) {
-	case SPI_NOR_DUAL:
+	switch (proto) {
+	case SNOR_PROTO_1_1_2:
 		if_type = IF_TYPE_DUAL;
 		break;
-	case SPI_NOR_QUAD:
+	case SNOR_PROTO_1_2_2:
+		if_type = IF_TYPE_DIO;
+		break;
+	case SNOR_PROTO_1_1_4:
 		if_type = IF_TYPE_QUAD;
 		break;
-	case SPI_NOR_NORMAL:
-	case SPI_NOR_FAST:
+	case SNOR_PROTO_1_4_4:
+		if_type = IF_TYPE_QIO;
+		break;
+	case SNOR_PROTO_1_1_1:
 	default:
 		if_type = IF_TYPE_STD;
 		break;
@@ -253,7 +258,10 @@ static int hisi_spi_nor_dma_transfer(struct spi_nor *nor, loff_t start_off,
 	writel(FMC_DMA_LEN_SET(len), host->regbase + FMC_DMA_LEN);
 
 	reg = OP_CFG_FM_CS(priv->chipselect);
-	if_type = get_if_type(nor->flash_read);
+	if (op_type == FMC_OP_READ)
+		if_type = get_if_type(nor->read_proto);
+	else
+		if_type = get_if_type(nor->write_proto);
 	reg |= OP_CFG_MEM_IF_TYPE(if_type);
 	if (op_type == FMC_OP_READ)
 		reg |= OP_CFG_DUMMY_NUM(nor->read_dummy >> 3);
@@ -321,6 +329,13 @@ static ssize_t hisi_spi_nor_write(struct spi_nor *nor, loff_t to,
 static int hisi_spi_nor_register(struct device_node *np,
 				struct hifmc_host *host)
 {
+	const struct spi_nor_hwcaps hwcaps = {
+		.mask = SNOR_HWCAPS_READ |
+			SNOR_HWCAPS_READ_FAST |
+			SNOR_HWCAPS_READ_1_1_2 |
+			SNOR_HWCAPS_READ_1_1_4 |
+			SNOR_HWCAPS_PP,
+	};
 	struct device *dev = host->dev;
 	struct spi_nor *nor;
 	struct hifmc_priv *priv;
@@ -362,7 +377,7 @@ static int hisi_spi_nor_register(struct device_node *np,
 	nor->read = hisi_spi_nor_read;
 	nor->write = hisi_spi_nor_write;
 	nor->erase = NULL;
-	ret = spi_nor_scan(nor, NULL, SPI_NOR_QUAD);
+	ret = spi_nor_scan(nor, NULL, &hwcaps);
 	if (ret)
 		return ret;
 
diff --git a/drivers/mtd/spi-nor/intel-spi.c b/drivers/mtd/spi-nor/intel-spi.c
index 986a3d020a3a..8a596bfeddff 100644
--- a/drivers/mtd/spi-nor/intel-spi.c
+++ b/drivers/mtd/spi-nor/intel-spi.c
@@ -715,6 +715,11 @@ static void intel_spi_fill_partition(struct intel_spi *ispi,
 struct intel_spi *intel_spi_probe(struct device *dev,
 	struct resource *mem, const struct intel_spi_boardinfo *info)
 {
+	const struct spi_nor_hwcaps hwcaps = {
+		.mask = SNOR_HWCAPS_READ |
+			SNOR_HWCAPS_READ_FAST |
+			SNOR_HWCAPS_PP,
+	};
 	struct mtd_partition part;
 	struct intel_spi *ispi;
 	int ret;
@@ -746,7 +751,7 @@ struct intel_spi *intel_spi_probe(struct device *dev,
 	ispi->nor.write = intel_spi_write;
 	ispi->nor.erase = intel_spi_erase;
 
-	ret = spi_nor_scan(&ispi->nor, NULL, SPI_NOR_NORMAL);
+	ret = spi_nor_scan(&ispi->nor, NULL, &hwcaps);
 	if (ret) {
 		dev_info(dev, "failed to locate the chip\n");
 		return ERR_PTR(ret);
diff --git a/drivers/mtd/spi-nor/mtk-quadspi.c b/drivers/mtd/spi-nor/mtk-quadspi.c
index b6377707ce32..8a20ec4991c8 100644
--- a/drivers/mtd/spi-nor/mtk-quadspi.c
+++ b/drivers/mtd/spi-nor/mtk-quadspi.c
@@ -123,20 +123,20 @@ static void mt8173_nor_set_read_mode(struct mt8173_nor *mt8173_nor)
 {
 	struct spi_nor *nor = &mt8173_nor->nor;
 
-	switch (nor->flash_read) {
-	case SPI_NOR_FAST:
+	switch (nor->read_proto) {
+	case SNOR_PROTO_1_1_1:
 		writeb(nor->read_opcode, mt8173_nor->base +
 		       MTK_NOR_PRGDATA3_REG);
 		writeb(MTK_NOR_FAST_READ, mt8173_nor->base +
 		       MTK_NOR_CFG1_REG);
 		break;
-	case SPI_NOR_DUAL:
+	case SNOR_PROTO_1_1_2:
 		writeb(nor->read_opcode, mt8173_nor->base +
 		       MTK_NOR_PRGDATA3_REG);
 		writeb(MTK_NOR_DUAL_READ_EN, mt8173_nor->base +
 		       MTK_NOR_DUAL_REG);
 		break;
-	case SPI_NOR_QUAD:
+	case SNOR_PROTO_1_1_4:
 		writeb(nor->read_opcode, mt8173_nor->base +
 		       MTK_NOR_PRGDATA4_REG);
 		writeb(MTK_NOR_QUAD_READ_EN, mt8173_nor->base +
@@ -408,6 +408,11 @@ static int mt8173_nor_write_reg(struct spi_nor *nor, u8 opcode, u8 *buf,
 static int mtk_nor_init(struct mt8173_nor *mt8173_nor,
 			struct device_node *flash_node)
 {
+	const struct spi_nor_hwcaps hwcaps = {
+		.mask = SNOR_HWCAPS_READ_FAST |
+			SNOR_HWCAPS_READ_1_1_2 |
+			SNOR_HWCAPS_PP,
+	};
 	int ret;
 	struct spi_nor *nor;
 
@@ -426,7 +431,7 @@ static int mtk_nor_init(struct mt8173_nor *mt8173_nor,
 	nor->write_reg = mt8173_nor_write_reg;
 	nor->mtd.name = "mtk_nor";
 	/* initialized with NULL */
-	ret = spi_nor_scan(nor, NULL, SPI_NOR_DUAL);
+	ret = spi_nor_scan(nor, NULL, &hwcaps);
 	if (ret)
 		return ret;
 
diff --git a/drivers/mtd/spi-nor/nxp-spifi.c b/drivers/mtd/spi-nor/nxp-spifi.c
index 73a14f40928b..15374216d4d9 100644
--- a/drivers/mtd/spi-nor/nxp-spifi.c
+++ b/drivers/mtd/spi-nor/nxp-spifi.c
@@ -240,13 +240,12 @@ static int nxp_spifi_erase(struct spi_nor *nor, loff_t offs)
 
 static int nxp_spifi_setup_memory_cmd(struct nxp_spifi *spifi)
 {
-	switch (spifi->nor.flash_read) {
-	case SPI_NOR_NORMAL:
-	case SPI_NOR_FAST:
+	switch (spifi->nor.read_proto) {
+	case SNOR_PROTO_1_1_1:
 		spifi->mcmd = SPIFI_CMD_FIELDFORM_ALL_SERIAL;
 		break;
-	case SPI_NOR_DUAL:
-	case SPI_NOR_QUAD:
+	case SNOR_PROTO_1_1_2:
+	case SNOR_PROTO_1_1_4:
 		spifi->mcmd = SPIFI_CMD_FIELDFORM_QUAD_DUAL_DATA;
 		break;
 	default:
@@ -274,7 +273,11 @@ static void nxp_spifi_dummy_id_read(struct spi_nor *nor)
 static int nxp_spifi_setup_flash(struct nxp_spifi *spifi,
 				 struct device_node *np)
 {
-	enum read_mode flash_read;
+	struct spi_nor_hwcaps hwcaps = {
+		.mask = SNOR_HWCAPS_READ |
+			SNOR_HWCAPS_READ_FAST |
+			SNOR_HWCAPS_PP,
+	};
 	u32 ctrl, property;
 	u16 mode = 0;
 	int ret;
@@ -308,13 +311,12 @@ static int nxp_spifi_setup_flash(struct nxp_spifi *spifi,
 
 	if (mode & SPI_RX_DUAL) {
 		ctrl |= SPIFI_CTRL_DUAL;
-		flash_read = SPI_NOR_DUAL;
+		hwcaps.mask |= SNOR_HWCAPS_READ_1_1_2;
 	} else if (mode & SPI_RX_QUAD) {
 		ctrl &= ~SPIFI_CTRL_DUAL;
-		flash_read = SPI_NOR_QUAD;
+		hwcaps.mask |= SNOR_HWCAPS_READ_1_1_4;
 	} else {
 		ctrl |= SPIFI_CTRL_DUAL;
-		flash_read = SPI_NOR_NORMAL;
 	}
 
 	switch (mode & (SPI_CPHA | SPI_CPOL)) {
@@ -351,7 +353,7 @@ static int nxp_spifi_setup_flash(struct nxp_spifi *spifi,
 	 */
 	nxp_spifi_dummy_id_read(&spifi->nor);
 
-	ret = spi_nor_scan(&spifi->nor, NULL, flash_read);
+	ret = spi_nor_scan(&spifi->nor, NULL, &hwcaps);
 	if (ret) {
 		dev_err(spifi->dev, "device scan failed\n");
 		return ret;
diff --git a/drivers/mtd/spi-nor/spi-nor.c b/drivers/mtd/spi-nor/spi-nor.c
index dea8c9cbadf0..eef55b597ec7 100644
--- a/drivers/mtd/spi-nor/spi-nor.c
+++ b/drivers/mtd/spi-nor/spi-nor.c
@@ -150,24 +150,6 @@ static int read_cr(struct spi_nor *nor)
 }
 
 /*
- * Dummy Cycle calculation for different type of read.
- * It can be used to support more commands with
- * different dummy cycle requirements.
- */
-static inline int spi_nor_read_dummy_cycles(struct spi_nor *nor)
-{
-	switch (nor->flash_read) {
-	case SPI_NOR_FAST:
-	case SPI_NOR_DUAL:
-	case SPI_NOR_QUAD:
-		return 8;
-	case SPI_NOR_NORMAL:
-		return 0;
-	}
-	return 0;
-}
-
-/*
  * Write status register 1 byte
  * Returns negative if error occurred.
  */
@@ -221,6 +203,10 @@ static inline u8 spi_nor_convert_3to4_read(u8 opcode)
 		{ SPINOR_OP_READ_1_2_2,	SPINOR_OP_READ_1_2_2_4B },
 		{ SPINOR_OP_READ_1_1_4,	SPINOR_OP_READ_1_1_4_4B },
 		{ SPINOR_OP_READ_1_4_4,	SPINOR_OP_READ_1_4_4_4B },
+
+		{ SPINOR_OP_READ_1_1_1_DTR,	SPINOR_OP_READ_1_1_1_DTR_4B },
+		{ SPINOR_OP_READ_1_2_2_DTR,	SPINOR_OP_READ_1_2_2_DTR_4B },
+		{ SPINOR_OP_READ_1_4_4_DTR,	SPINOR_OP_READ_1_4_4_DTR_4B },
 	};
 
 	return spi_nor_convert_opcode(opcode, spi_nor_3to4_read,
@@ -1403,8 +1389,9 @@ static int macronix_quad_enable(struct spi_nor *nor)
 
 	write_sr(nor, val | SR_QUAD_EN_MX);
 
-	if (spi_nor_wait_till_ready(nor))
-		return 1;
+	ret = spi_nor_wait_till_ready(nor);
+	if (ret)
+		return ret;
 
 	ret = read_sr(nor);
 	if (!(ret > 0 && (ret & SR_QUAD_EN_MX))) {
@@ -1460,30 +1447,6 @@ static int spansion_quad_enable(struct spi_nor *nor)
 	return 0;
 }
 
-static int set_quad_mode(struct spi_nor *nor, const struct flash_info *info)
-{
-	int status;
-
-	switch (JEDEC_MFR(info)) {
-	case SNOR_MFR_MACRONIX:
-		status = macronix_quad_enable(nor);
-		if (status) {
-			dev_err(nor->dev, "Macronix quad-read not enabled\n");
-			return -EINVAL;
-		}
-		return status;
-	case SNOR_MFR_MICRON:
-		return 0;
-	default:
-		status = spansion_quad_enable(nor);
-		if (status) {
-			dev_err(nor->dev, "Spansion quad-read not enabled\n");
-			return -EINVAL;
-		}
-		return status;
-	}
-}
-
 static int spi_nor_check(struct spi_nor *nor)
 {
 	if (!nor->dev || !nor->read || !nor->write ||
@@ -1536,8 +1499,349 @@ static int s3an_nor_scan(const struct flash_info *info, struct spi_nor *nor)
 	return 0;
 }
 
-int spi_nor_scan(struct spi_nor *nor, const char *name, enum read_mode mode)
+struct spi_nor_read_command {
+	u8			num_mode_clocks;
+	u8			num_wait_states;
+	u8			opcode;
+	enum spi_nor_protocol	proto;
+};
+
+struct spi_nor_pp_command {
+	u8			opcode;
+	enum spi_nor_protocol	proto;
+};
+
+enum spi_nor_read_command_index {
+	SNOR_CMD_READ,
+	SNOR_CMD_READ_FAST,
+	SNOR_CMD_READ_1_1_1_DTR,
+
+	/* Dual SPI */
+	SNOR_CMD_READ_1_1_2,
+	SNOR_CMD_READ_1_2_2,
+	SNOR_CMD_READ_2_2_2,
+	SNOR_CMD_READ_1_2_2_DTR,
+
+	/* Quad SPI */
+	SNOR_CMD_READ_1_1_4,
+	SNOR_CMD_READ_1_4_4,
+	SNOR_CMD_READ_4_4_4,
+	SNOR_CMD_READ_1_4_4_DTR,
+
+	/* Octo SPI */
+	SNOR_CMD_READ_1_1_8,
+	SNOR_CMD_READ_1_8_8,
+	SNOR_CMD_READ_8_8_8,
+	SNOR_CMD_READ_1_8_8_DTR,
+
+	SNOR_CMD_READ_MAX
+};
+
+enum spi_nor_pp_command_index {
+	SNOR_CMD_PP,
+
+	/* Quad SPI */
+	SNOR_CMD_PP_1_1_4,
+	SNOR_CMD_PP_1_4_4,
+	SNOR_CMD_PP_4_4_4,
+
+	/* Octo SPI */
+	SNOR_CMD_PP_1_1_8,
+	SNOR_CMD_PP_1_8_8,
+	SNOR_CMD_PP_8_8_8,
+
+	SNOR_CMD_PP_MAX
+};
+
+struct spi_nor_flash_parameter {
+	u64				size;
+	u32				page_size;
+
+	struct spi_nor_hwcaps		hwcaps;
+	struct spi_nor_read_command	reads[SNOR_CMD_READ_MAX];
+	struct spi_nor_pp_command	page_programs[SNOR_CMD_PP_MAX];
+
+	int (*quad_enable)(struct spi_nor *nor);
+};
+
+static void
+spi_nor_set_read_settings(struct spi_nor_read_command *read,
+			  u8 num_mode_clocks,
+			  u8 num_wait_states,
+			  u8 opcode,
+			  enum spi_nor_protocol proto)
 {
+	read->num_mode_clocks = num_mode_clocks;
+	read->num_wait_states = num_wait_states;
+	read->opcode = opcode;
+	read->proto = proto;
+}
+
+static void
+spi_nor_set_pp_settings(struct spi_nor_pp_command *pp,
+			u8 opcode,
+			enum spi_nor_protocol proto)
+{
+	pp->opcode = opcode;
+	pp->proto = proto;
+}
+
+static int spi_nor_init_params(struct spi_nor *nor,
+			       const struct flash_info *info,
+			       struct spi_nor_flash_parameter *params)
+{
+	/* Set legacy flash parameters as default. */
+	memset(params, 0, sizeof(*params));
+
+	/* Set SPI NOR sizes. */
+	params->size = info->sector_size * info->n_sectors;
+	params->page_size = info->page_size;
+
+	/* (Fast) Read settings. */
+	params->hwcaps.mask |= SNOR_HWCAPS_READ;
+	spi_nor_set_read_settings(&params->reads[SNOR_CMD_READ],
+				  0, 0, SPINOR_OP_READ,
+				  SNOR_PROTO_1_1_1);
+
+	if (!(info->flags & SPI_NOR_NO_FR)) {
+		params->hwcaps.mask |= SNOR_HWCAPS_READ_FAST;
+		spi_nor_set_read_settings(&params->reads[SNOR_CMD_READ_FAST],
+					  0, 8, SPINOR_OP_READ_FAST,
+					  SNOR_PROTO_1_1_1);
+	}
+
+	if (info->flags & SPI_NOR_DUAL_READ) {
+		params->hwcaps.mask |= SNOR_HWCAPS_READ_1_1_2;
+		spi_nor_set_read_settings(&params->reads[SNOR_CMD_READ_1_1_2],
+					  0, 8, SPINOR_OP_READ_1_1_2,
+					  SNOR_PROTO_1_1_2);
+	}
+
+	if (info->flags & SPI_NOR_QUAD_READ) {
+		params->hwcaps.mask |= SNOR_HWCAPS_READ_1_1_4;
+		spi_nor_set_read_settings(&params->reads[SNOR_CMD_READ_1_1_4],
+					  0, 8, SPINOR_OP_READ_1_1_4,
+					  SNOR_PROTO_1_1_4);
+	}
+
+	/* Page Program settings. */
+	params->hwcaps.mask |= SNOR_HWCAPS_PP;
+	spi_nor_set_pp_settings(&params->page_programs[SNOR_CMD_PP],
+				SPINOR_OP_PP, SNOR_PROTO_1_1_1);
+
+	/* Select the procedure to set the Quad Enable bit. */
+	if (params->hwcaps.mask & (SNOR_HWCAPS_READ_QUAD |
+				   SNOR_HWCAPS_PP_QUAD)) {
+		switch (JEDEC_MFR(info)) {
+		case SNOR_MFR_MACRONIX:
+			params->quad_enable = macronix_quad_enable;
+			break;
+
+		case SNOR_MFR_MICRON:
+			break;
+
+		default:
+			params->quad_enable = spansion_quad_enable;
+			break;
+		}
+	}
+
+	return 0;
+}
+
+static int spi_nor_hwcaps2cmd(u32 hwcaps, const int table[][2], size_t size)
+{
+	size_t i;
+
+	for (i = 0; i < size; i++)
+		if (table[i][0] == (int)hwcaps)
+			return table[i][1];
+
+	return -EINVAL;
+}
+
+static int spi_nor_hwcaps_read2cmd(u32 hwcaps)
+{
+	static const int hwcaps_read2cmd[][2] = {
+		{ SNOR_HWCAPS_READ,		SNOR_CMD_READ },
+		{ SNOR_HWCAPS_READ_FAST,	SNOR_CMD_READ_FAST },
+		{ SNOR_HWCAPS_READ_1_1_1_DTR,	SNOR_CMD_READ_1_1_1_DTR },
+		{ SNOR_HWCAPS_READ_1_1_2,	SNOR_CMD_READ_1_1_2 },
+		{ SNOR_HWCAPS_READ_1_2_2,	SNOR_CMD_READ_1_2_2 },
+		{ SNOR_HWCAPS_READ_2_2_2,	SNOR_CMD_READ_2_2_2 },
+		{ SNOR_HWCAPS_READ_1_2_2_DTR,	SNOR_CMD_READ_1_2_2_DTR },
+		{ SNOR_HWCAPS_READ_1_1_4,	SNOR_CMD_READ_1_1_4 },
+		{ SNOR_HWCAPS_READ_1_4_4,	SNOR_CMD_READ_1_4_4 },
+		{ SNOR_HWCAPS_READ_4_4_4,	SNOR_CMD_READ_4_4_4 },
+		{ SNOR_HWCAPS_READ_1_4_4_DTR,	SNOR_CMD_READ_1_4_4_DTR },
+		{ SNOR_HWCAPS_READ_1_1_8,	SNOR_CMD_READ_1_1_8 },
+		{ SNOR_HWCAPS_READ_1_8_8,	SNOR_CMD_READ_1_8_8 },
+		{ SNOR_HWCAPS_READ_8_8_8,	SNOR_CMD_READ_8_8_8 },
+		{ SNOR_HWCAPS_READ_1_8_8_DTR,	SNOR_CMD_READ_1_8_8_DTR },
+	};
+
+	return spi_nor_hwcaps2cmd(hwcaps, hwcaps_read2cmd,
+				  ARRAY_SIZE(hwcaps_read2cmd));
+}
+
+static int spi_nor_hwcaps_pp2cmd(u32 hwcaps)
+{
+	static const int hwcaps_pp2cmd[][2] = {
+		{ SNOR_HWCAPS_PP,		SNOR_CMD_PP },
+		{ SNOR_HWCAPS_PP_1_1_4,		SNOR_CMD_PP_1_1_4 },
+		{ SNOR_HWCAPS_PP_1_4_4,		SNOR_CMD_PP_1_4_4 },
+		{ SNOR_HWCAPS_PP_4_4_4,		SNOR_CMD_PP_4_4_4 },
+		{ SNOR_HWCAPS_PP_1_1_8,		SNOR_CMD_PP_1_1_8 },
+		{ SNOR_HWCAPS_PP_1_8_8,		SNOR_CMD_PP_1_8_8 },
+		{ SNOR_HWCAPS_PP_8_8_8,		SNOR_CMD_PP_8_8_8 },
+	};
+
+	return spi_nor_hwcaps2cmd(hwcaps, hwcaps_pp2cmd,
+				  ARRAY_SIZE(hwcaps_pp2cmd));
+}
+
+static int spi_nor_select_read(struct spi_nor *nor,
+			       const struct spi_nor_flash_parameter *params,
+			       u32 shared_hwcaps)
+{
+	int cmd, best_match = fls(shared_hwcaps & SNOR_HWCAPS_READ_MASK) - 1;
+	const struct spi_nor_read_command *read;
+
+	if (best_match < 0)
+		return -EINVAL;
+
+	cmd = spi_nor_hwcaps_read2cmd(BIT(best_match));
+	if (cmd < 0)
+		return -EINVAL;
+
+	read = &params->reads[cmd];
+	nor->read_opcode = read->opcode;
+	nor->read_proto = read->proto;
+
+	/*
+	 * In the spi-nor framework, we don't need to make the difference
+	 * between mode clock cycles and wait state clock cycles.
+	 * Indeed, the value of the mode clock cycles is used by a QSPI
+	 * flash memory to know whether it should enter or leave its 0-4-4
+	 * (Continuous Read / XIP) mode.
+	 * eXecution In Place is out of the scope of the mtd sub-system.
+	 * Hence we choose to merge both mode and wait state clock cycles
+	 * into the so called dummy clock cycles.
+	 */
+	nor->read_dummy = read->num_mode_clocks + read->num_wait_states;
+	return 0;
+}
+
+static int spi_nor_select_pp(struct spi_nor *nor,
+			     const struct spi_nor_flash_parameter *params,
+			     u32 shared_hwcaps)
+{
+	int cmd, best_match = fls(shared_hwcaps & SNOR_HWCAPS_PP_MASK) - 1;
+	const struct spi_nor_pp_command *pp;
+
+	if (best_match < 0)
+		return -EINVAL;
+
+	cmd = spi_nor_hwcaps_pp2cmd(BIT(best_match));
+	if (cmd < 0)
+		return -EINVAL;
+
+	pp = &params->page_programs[cmd];
+	nor->program_opcode = pp->opcode;
+	nor->write_proto = pp->proto;
+	return 0;
+}
+
+static int spi_nor_select_erase(struct spi_nor *nor,
+				const struct flash_info *info)
+{
+	struct mtd_info *mtd = &nor->mtd;
+
+#ifdef CONFIG_MTD_SPI_NOR_USE_4K_SECTORS
+	/* prefer "small sector" erase if possible */
+	if (info->flags & SECT_4K) {
+		nor->erase_opcode = SPINOR_OP_BE_4K;
+		mtd->erasesize = 4096;
+	} else if (info->flags & SECT_4K_PMC) {
+		nor->erase_opcode = SPINOR_OP_BE_4K_PMC;
+		mtd->erasesize = 4096;
+	} else
+#endif
+	{
+		nor->erase_opcode = SPINOR_OP_SE;
+		mtd->erasesize = info->sector_size;
+	}
+	return 0;
+}
+
+static int spi_nor_setup(struct spi_nor *nor, const struct flash_info *info,
+			 const struct spi_nor_flash_parameter *params,
+			 const struct spi_nor_hwcaps *hwcaps)
+{
+	u32 ignored_mask, shared_mask;
+	bool enable_quad_io;
+	int err;
+
+	/*
+	 * Keep only the hardware capabilities supported by both the SPI
+	 * controller and the SPI flash memory.
+	 */
+	shared_mask = hwcaps->mask & params->hwcaps.mask;
+
+	/* SPI n-n-n protocols are not supported yet. */
+	ignored_mask = (SNOR_HWCAPS_READ_2_2_2 |
+			SNOR_HWCAPS_READ_4_4_4 |
+			SNOR_HWCAPS_READ_8_8_8 |
+			SNOR_HWCAPS_PP_4_4_4 |
+			SNOR_HWCAPS_PP_8_8_8);
+	if (shared_mask & ignored_mask) {
+		dev_dbg(nor->dev,
+			"SPI n-n-n protocols are not supported yet.\n");
+		shared_mask &= ~ignored_mask;
+	}
+
+	/* Select the (Fast) Read command. */
+	err = spi_nor_select_read(nor, params, shared_mask);
+	if (err) {
+		dev_err(nor->dev,
+			"can't select read settings supported by both the SPI controller and memory.\n");
+		return err;
+	}
+
+	/* Select the Page Program command. */
+	err = spi_nor_select_pp(nor, params, shared_mask);
+	if (err) {
+		dev_err(nor->dev,
+			"can't select write settings supported by both the SPI controller and memory.\n");
+		return err;
+	}
+
+	/* Select the Sector Erase command. */
+	err = spi_nor_select_erase(nor, info);
+	if (err) {
+		dev_err(nor->dev,
+			"can't select erase settings supported by both the SPI controller and memory.\n");
+		return err;
+	}
+
+	/* Enable Quad I/O if needed. */
+	enable_quad_io = (spi_nor_get_protocol_width(nor->read_proto) == 4 ||
+			  spi_nor_get_protocol_width(nor->write_proto) == 4);
+	if (enable_quad_io && params->quad_enable) {
+		err = params->quad_enable(nor);
+		if (err) {
+			dev_err(nor->dev, "quad mode not supported\n");
+			return err;
+		}
+	}
+
+	return 0;
+}
+
+int spi_nor_scan(struct spi_nor *nor, const char *name,
+		 const struct spi_nor_hwcaps *hwcaps)
+{
+	struct spi_nor_flash_parameter params;
 	const struct flash_info *info = NULL;
 	struct device *dev = nor->dev;
 	struct mtd_info *mtd = &nor->mtd;
@@ -1549,6 +1853,11 @@ int spi_nor_scan(struct spi_nor *nor, const char *name, enum read_mode mode)
 	if (ret)
 		return ret;
 
+	/* Reset SPI protocol for all commands. */
+	nor->reg_proto = SNOR_PROTO_1_1_1;
+	nor->read_proto = SNOR_PROTO_1_1_1;
+	nor->write_proto = SNOR_PROTO_1_1_1;
+
 	if (name)
 		info = spi_nor_match_id(name);
 	/* Try to auto-detect if chip name wasn't specified or not found */
@@ -1591,6 +1900,11 @@ int spi_nor_scan(struct spi_nor *nor, const char *name, enum read_mode mode)
 	if (info->flags & SPI_S3AN)
 		nor->flags |=  SNOR_F_READY_XSR_RDY;
 
+	/* Parse the Serial Flash Discoverable Parameters table. */
+	ret = spi_nor_init_params(nor, info, &params);
+	if (ret)
+		return ret;
+
 	/*
 	 * Atmel, SST, Intel/Numonyx, and others serial NOR tend to power up
 	 * with the software protection bits set
@@ -1611,7 +1925,7 @@ int spi_nor_scan(struct spi_nor *nor, const char *name, enum read_mode mode)
 	mtd->type = MTD_NORFLASH;
 	mtd->writesize = 1;
 	mtd->flags = MTD_CAP_NORFLASH;
-	mtd->size = info->sector_size * info->n_sectors;
+	mtd->size = params.size;
 	mtd->_erase = spi_nor_erase;
 	mtd->_read = spi_nor_read;
 
@@ -1642,75 +1956,38 @@ int spi_nor_scan(struct spi_nor *nor, const char *name, enum read_mode mode)
 	if (info->flags & NO_CHIP_ERASE)
 		nor->flags |= SNOR_F_NO_OP_CHIP_ERASE;
 
-#ifdef CONFIG_MTD_SPI_NOR_USE_4K_SECTORS
-	/* prefer "small sector" erase if possible */
-	if (info->flags & SECT_4K) {
-		nor->erase_opcode = SPINOR_OP_BE_4K;
-		mtd->erasesize = 4096;
-	} else if (info->flags & SECT_4K_PMC) {
-		nor->erase_opcode = SPINOR_OP_BE_4K_PMC;
-		mtd->erasesize = 4096;
-	} else
-#endif
-	{
-		nor->erase_opcode = SPINOR_OP_SE;
-		mtd->erasesize = info->sector_size;
-	}
-
 	if (info->flags & SPI_NOR_NO_ERASE)
 		mtd->flags |= MTD_NO_ERASE;
 
 	mtd->dev.parent = dev;
-	nor->page_size = info->page_size;
+	nor->page_size = params.page_size;
 	mtd->writebufsize = nor->page_size;
 
 	if (np) {
 		/* If we were instantiated by DT, use it */
 		if (of_property_read_bool(np, "m25p,fast-read"))
-			nor->flash_read = SPI_NOR_FAST;
+			params.hwcaps.mask |= SNOR_HWCAPS_READ_FAST;
 		else
-			nor->flash_read = SPI_NOR_NORMAL;
+			params.hwcaps.mask &= ~SNOR_HWCAPS_READ_FAST;
 	} else {
 		/* If we weren't instantiated by DT, default to fast-read */
-		nor->flash_read = SPI_NOR_FAST;
+		params.hwcaps.mask |= SNOR_HWCAPS_READ_FAST;
 	}
 
 	/* Some devices cannot do fast-read, no matter what DT tells us */
 	if (info->flags & SPI_NOR_NO_FR)
-		nor->flash_read = SPI_NOR_NORMAL;
-
-	/* Quad/Dual-read mode takes precedence over fast/normal */
-	if (mode == SPI_NOR_QUAD && info->flags & SPI_NOR_QUAD_READ) {
-		ret = set_quad_mode(nor, info);
-		if (ret) {
-			dev_err(dev, "quad mode not supported\n");
-			return ret;
-		}
-		nor->flash_read = SPI_NOR_QUAD;
-	} else if (mode == SPI_NOR_DUAL && info->flags & SPI_NOR_DUAL_READ) {
-		nor->flash_read = SPI_NOR_DUAL;
-	}
-
-	/* Default commands */
-	switch (nor->flash_read) {
-	case SPI_NOR_QUAD:
-		nor->read_opcode = SPINOR_OP_READ_1_1_4;
-		break;
-	case SPI_NOR_DUAL:
-		nor->read_opcode = SPINOR_OP_READ_1_1_2;
-		break;
-	case SPI_NOR_FAST:
-		nor->read_opcode = SPINOR_OP_READ_FAST;
-		break;
-	case SPI_NOR_NORMAL:
-		nor->read_opcode = SPINOR_OP_READ;
-		break;
-	default:
-		dev_err(dev, "No Read opcode defined\n");
-		return -EINVAL;
-	}
+		params.hwcaps.mask &= ~SNOR_HWCAPS_READ_FAST;
 
-	nor->program_opcode = SPINOR_OP_PP;
+	/*
+	 * Configure the SPI memory:
+	 * - select op codes for (Fast) Read, Page Program and Sector Erase.
+	 * - set the number of dummy cycles (mode cycles + wait states).
+	 * - set the SPI protocols for register and memory accesses.
+	 * - set the Quad Enable bit if needed (required by SPI x-y-4 protos).
+	 */
+	ret = spi_nor_setup(nor, info, &params, hwcaps);
+	if (ret)
+		return ret;
 
 	if (info->addr_width)
 		nor->addr_width = info->addr_width;
@@ -1732,8 +2009,6 @@ int spi_nor_scan(struct spi_nor *nor, const char *name, enum read_mode mode)
 		return -EINVAL;
 	}
 
-	nor->read_dummy = spi_nor_read_dummy_cycles(nor);
-
 	if (info->flags & SPI_S3AN) {
 		ret = s3an_nor_scan(info, nor);
 		if (ret)
diff --git a/drivers/mtd/spi-nor/stm32-quadspi.c b/drivers/mtd/spi-nor/stm32-quadspi.c
index ae45f81b8cd3..86c0931543c5 100644
--- a/drivers/mtd/spi-nor/stm32-quadspi.c
+++ b/drivers/mtd/spi-nor/stm32-quadspi.c
@@ -19,6 +19,7 @@
 #include <linux/of_device.h>
 #include <linux/platform_device.h>
 #include <linux/reset.h>
+#include <linux/sizes.h>
 
 #define QUADSPI_CR		0x00
 #define CR_EN			BIT(0)
@@ -192,15 +193,15 @@ static void stm32_qspi_set_framemode(struct spi_nor *nor,
 	cmd->framemode = CCR_IMODE_1;
 
 	if (read) {
-		switch (nor->flash_read) {
-		case SPI_NOR_NORMAL:
-		case SPI_NOR_FAST:
+		switch (nor->read_proto) {
+		default:
+		case SNOR_PROTO_1_1_1:
 			dmode = CCR_DMODE_1;
 			break;
-		case SPI_NOR_DUAL:
+		case SNOR_PROTO_1_1_2:
 			dmode = CCR_DMODE_2;
 			break;
-		case SPI_NOR_QUAD:
+		case SNOR_PROTO_1_1_4:
 			dmode = CCR_DMODE_4;
 			break;
 		}
@@ -375,7 +376,7 @@ static ssize_t stm32_qspi_read(struct spi_nor *nor, loff_t from, size_t len,
 	struct stm32_qspi_cmd cmd;
 	int err;
 
-	dev_dbg(qspi->dev, "read(%#.2x): buf:%p from:%#.8x len:%#x\n",
+	dev_dbg(qspi->dev, "read(%#.2x): buf:%p from:%#.8x len:%#zx\n",
 		nor->read_opcode, buf, (u32)from, len);
 
 	memset(&cmd, 0, sizeof(cmd));
@@ -402,7 +403,7 @@ static ssize_t stm32_qspi_write(struct spi_nor *nor, loff_t to, size_t len,
 	struct stm32_qspi_cmd cmd;
 	int err;
 
-	dev_dbg(dev, "write(%#.2x): buf:%p to:%#.8x len:%#x\n",
+	dev_dbg(dev, "write(%#.2x): buf:%p to:%#.8x len:%#zx\n",
 		nor->program_opcode, buf, (u32)to, len);
 
 	memset(&cmd, 0, sizeof(cmd));
@@ -480,7 +481,12 @@ static void stm32_qspi_unprep(struct spi_nor *nor, enum spi_nor_ops ops)
 static int stm32_qspi_flash_setup(struct stm32_qspi *qspi,
 				  struct device_node *np)
 {
-	u32 width, flash_read, presc, cs_num, max_rate = 0;
+	struct spi_nor_hwcaps hwcaps = {
+		.mask = SNOR_HWCAPS_READ |
+			SNOR_HWCAPS_READ_FAST |
+			SNOR_HWCAPS_PP,
+	};
+	u32 width, presc, cs_num, max_rate = 0;
 	struct stm32_qspi_flash *flash;
 	struct mtd_info *mtd;
 	int ret;
@@ -499,12 +505,10 @@ static int stm32_qspi_flash_setup(struct stm32_qspi *qspi,
 		width = 1;
 
 	if (width == 4)
-		flash_read = SPI_NOR_QUAD;
+		hwcaps.mask |= SNOR_HWCAPS_READ_1_1_4;
 	else if (width == 2)
-		flash_read = SPI_NOR_DUAL;
-	else if (width == 1)
-		flash_read = SPI_NOR_NORMAL;
-	else
+		hwcaps.mask |= SNOR_HWCAPS_READ_1_1_2;
+	else if (width != 1)
 		return -EINVAL;
 
 	flash = &qspi->flash[cs_num];
@@ -539,7 +543,7 @@ static int stm32_qspi_flash_setup(struct stm32_qspi *qspi,
 	 */
 	flash->fsize = FSIZE_VAL(SZ_1K);
 
-	ret = spi_nor_scan(&flash->nor, NULL, flash_read);
+	ret = spi_nor_scan(&flash->nor, NULL, &hwcaps);
 	if (ret) {
 		dev_err(qspi->dev, "device scan failed\n");
 		return ret;
diff --git a/drivers/net/bonding/bond_3ad.c b/drivers/net/bonding/bond_3ad.c
index c5fd4259da33..b44a6aeb346d 100644
--- a/drivers/net/bonding/bond_3ad.c
+++ b/drivers/net/bonding/bond_3ad.c
@@ -2577,7 +2577,7 @@ int __bond_3ad_get_active_agg_info(struct bonding *bond,
 		return -1;
 
 	ad_info->aggregator_id = aggregator->aggregator_identifier;
-	ad_info->ports = aggregator->num_of_ports;
+	ad_info->ports = __agg_active_ports(aggregator);
 	ad_info->actor_key = aggregator->actor_oper_aggregator_key;
 	ad_info->partner_key = aggregator->partner_oper_aggregator_key;
 	ether_addr_copy(ad_info->partner_system,
diff --git a/drivers/net/bonding/bond_main.c b/drivers/net/bonding/bond_main.c
index 2be78807fd6e..d4484d1a8164 100644
--- a/drivers/net/bonding/bond_main.c
+++ b/drivers/net/bonding/bond_main.c
@@ -2612,11 +2612,13 @@ static void bond_loadbalance_arp_mon(struct bonding *bond)
 	bond_for_each_slave_rcu(bond, slave, iter) {
 		unsigned long trans_start = dev_trans_start(slave->dev);
 
+		slave->new_link = BOND_LINK_NOCHANGE;
+
 		if (slave->link != BOND_LINK_UP) {
 			if (bond_time_in_interval(bond, trans_start, 1) &&
 			    bond_time_in_interval(bond, slave->last_rx, 1)) {
 
-				slave->link  = BOND_LINK_UP;
+				slave->new_link = BOND_LINK_UP;
 				slave_state_changed = 1;
 
 				/* primary_slave has no meaning in round-robin
@@ -2643,7 +2645,7 @@ static void bond_loadbalance_arp_mon(struct bonding *bond)
 			if (!bond_time_in_interval(bond, trans_start, 2) ||
 			    !bond_time_in_interval(bond, slave->last_rx, 2)) {
 
-				slave->link  = BOND_LINK_DOWN;
+				slave->new_link = BOND_LINK_DOWN;
 				slave_state_changed = 1;
 
 				if (slave->link_failure_count < UINT_MAX)
@@ -2674,6 +2676,11 @@ static void bond_loadbalance_arp_mon(struct bonding *bond)
 		if (!rtnl_trylock())
 			goto re_arm;
 
+		bond_for_each_slave(bond, slave, iter) {
+			if (slave->new_link != BOND_LINK_NOCHANGE)
+				slave->link = slave->new_link;
+		}
+
 		if (slave_state_changed) {
 			bond_slave_state_change(bond);
 			if (BOND_MODE(bond) == BOND_MODE_XOR)
@@ -3481,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;
@@ -4271,10 +4279,10 @@ static int bond_check_params(struct bond_params *params)
 	int arp_validate_value, fail_over_mac_value, primary_reselect_value, i;
 	struct bond_opt_value newval;
 	const struct bond_opt_value *valptr;
-	int arp_all_targets_value;
+	int arp_all_targets_value = 0;
 	u16 ad_actor_sys_prio = 0;
 	u16 ad_user_port_key = 0;
-	__be32 arp_target[BOND_MAX_ARP_TARGETS];
+	__be32 arp_target[BOND_MAX_ARP_TARGETS] = { 0 };
 	int arp_ip_count;
 	int bond_mode	= BOND_MODE_ROUNDROBIN;
 	int xmit_hashtype = BOND_XMIT_POLICY_LAYER2;
@@ -4501,7 +4509,6 @@ static int bond_check_params(struct bond_params *params)
 		arp_validate_value = 0;
 	}
 
-	arp_all_targets_value = 0;
 	if (arp_all_targets) {
 		bond_opt_initstr(&newval, arp_all_targets);
 		valptr = bond_opt_parse(bond_opt_get(BOND_OPT_ARP_ALL_TARGETS),
diff --git a/drivers/net/bonding/bond_options.c b/drivers/net/bonding/bond_options.c
index 1bcbb8913e17..8ca683396fcc 100644
--- a/drivers/net/bonding/bond_options.c
+++ b/drivers/net/bonding/bond_options.c
@@ -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;
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/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..687a8bae5d73 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;
 
@@ -832,7 +831,7 @@ static int bcm_sf2_sw_set_wol(struct dsa_switch *ds, int port,
 {
 	struct net_device *p = ds->dst[ds->index].master_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..79e62593ff4e 100644
--- a/drivers/net/dsa/dsa_loop.c
+++ b/drivers/net/dsa/dsa_loop.c
@@ -17,7 +17,6 @@
 #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"
@@ -188,7 +187,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;
@@ -272,7 +271,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)
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..25e00d5e0eec 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 };
@@ -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 19581d783d8e..7cf470c3e662 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 "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);
@@ -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)
@@ -849,6 +620,9 @@ static uint64_t _mv88e6xxx_get_ethtool_stat(struct mv88e6xxx_chip *chip,
 		mv88e6xxx_g1_stats_read(chip, reg, &low);
 		if (s->sizeof_stat == 8)
 			mv88e6xxx_g1_stats_read(chip, reg + 1, &high);
+		break;
+	default:
+		return UINT64_MAX;
 	}
 	value = (((u64)high) << 16) | low;
 	return value;
@@ -1266,7 +1040,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 = {
@@ -1697,7 +1471,7 @@ 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;
@@ -1752,7 +1526,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,
@@ -1789,7 +1563,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;
@@ -1948,24 +1722,6 @@ 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)
@@ -2047,6 +1803,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;
@@ -2097,21 +1862,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
@@ -2214,6 +1972,30 @@ static int mv88e6xxx_setup_port(struct mv88e6xxx_chip *chip, int port)
 	return mv88e6xxx_port_write(chip, port, PORT_DEFAULT_VLAN, 0x0000);
 }
 
+static int mv88e6xxx_port_enable(struct dsa_switch *ds, int port,
+				 struct phy_device *phydev)
+{
+	struct mv88e6xxx_chip *chip = ds->priv;
+	int err;
+
+	mutex_lock(&chip->reg_lock);
+	err = mv88e6xxx_serdes_power(chip, port, true);
+	mutex_unlock(&chip->reg_lock);
+
+	return err;
+}
+
+static void mv88e6xxx_port_disable(struct dsa_switch *ds, int port,
+				   struct phy_device *phydev)
+{
+	struct mv88e6xxx_chip *chip = ds->priv;
+
+	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_g1_set_switch_mac(struct mv88e6xxx_chip *chip, u8 *addr)
 {
 	int err;
@@ -2252,13 +2034,6 @@ 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 (err)
@@ -2358,6 +2133,10 @@ static int mv88e6xxx_setup(struct dsa_switch *ds)
 			goto unlock;
 	}
 
+	err = mv88e6xxx_phy_setup(chip);
+	if (err)
+		goto unlock;
+
 	err = mv88e6xxx_vtu_setup(chip);
 	if (err)
 		goto unlock;
@@ -2592,8 +2371,8 @@ static int mv88e6xxx_set_eeprom(struct dsa_switch *ds,
 static const struct mv88e6xxx_ops mv88e6085_ops = {
 	/* MV88E6XXX_FAMILY_6097 */
 	.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,
@@ -2623,8 +2402,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,
@@ -2701,8 +2480,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,
@@ -2877,6 +2656,7 @@ static const struct mv88e6xxx_ops mv88e6172_ops = {
 	.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 = {
@@ -2941,13 +2721,14 @@ static const struct mv88e6xxx_ops mv88e6176_ops = {
 	.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,
@@ -3000,6 +2781,7 @@ static const struct mv88e6xxx_ops mv88e6190_ops = {
 	.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 = {
@@ -3032,6 +2814,7 @@ static const struct mv88e6xxx_ops mv88e6190x_ops = {
 	.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 = {
@@ -3064,6 +2847,7 @@ static const struct mv88e6xxx_ops mv88e6191_ops = {
 	.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 = {
@@ -3097,6 +2881,7 @@ static const struct mv88e6xxx_ops mv88e6240_ops = {
 	.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 = {
@@ -3130,6 +2915,7 @@ static const struct mv88e6xxx_ops mv88e6290_ops = {
 	.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 = {
@@ -3319,6 +3105,7 @@ static const struct mv88e6xxx_ops mv88e6352_ops = {
 	.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 = {
@@ -3354,6 +3141,7 @@ static const struct mv88e6xxx_ops mv88e6390_ops = {
 	.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 = {
@@ -3388,6 +3176,7 @@ static const struct mv88e6xxx_ops mv88e6390x_ops = {
 	.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[] = {
@@ -3912,18 +3701,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)
 {
@@ -4035,7 +3812,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;
@@ -4056,6 +3833,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,
@@ -4105,7 +3884,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/global2.h b/drivers/net/dsa/mv88e6xxx/global2.h
index 96046bb12ca1..14c0be98e0a4 100644
--- a/drivers/net/dsa/mv88e6xxx/global2.h
+++ b/drivers/net/dsa/mv88e6xxx/global2.h
@@ -114,13 +114,13 @@ static inline int mv88e6xxx_g2_set_eeprom16(struct mv88e6xxx_chip *chip,
 	return -EOPNOTSUPP;
 }
 
-int mv88e6xxx_g2_pvt_write(struct mv88e6xxx_chip *chip, int src_dev,
-			   int src_port, u16 data)
+static inline int mv88e6xxx_g2_pvt_write(struct mv88e6xxx_chip *chip,
+					 int src_dev, int src_port, u16 data)
 {
 	return -EOPNOTSUPP;
 }
 
-int mv88e6xxx_g2_misc_4_bit_port(struct mv88e6xxx_chip *chip)
+static inline int mv88e6xxx_g2_misc_4_bit_port(struct mv88e6xxx_chip *chip)
 {
 	return -EOPNOTSUPP;
 }
diff --git a/drivers/net/dsa/mv88e6xxx/mv88e6xxx.h b/drivers/net/dsa/mv88e6xxx/mv88e6xxx.h
index 77236cd72df2..9087cb009cc3 100644
--- a/drivers/net/dsa/mv88e6xxx/mv88e6xxx.h
+++ b/drivers/net/dsa/mv88e6xxx/mv88e6xxx.h
@@ -37,9 +37,6 @@
 #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)
@@ -511,14 +508,6 @@ enum mv88e6xxx_cap {
 	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 */
@@ -553,10 +542,6 @@ enum mv88e6xxx_cap {
 #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)
@@ -577,11 +562,6 @@ enum mv88e6xxx_cap {
 	(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 |	\
@@ -629,8 +609,7 @@ enum mv88e6xxx_cap {
 	 MV88E6XXX_FLAG_G2_INT |	\
 	 MV88E6XXX_FLAG_G2_POT |	\
 	 MV88E6XXX_FLAGS_IRL |		\
-	 MV88E6XXX_FLAGS_MULTI_CHIP |	\
-	 MV88E6XXX_FLAGS_SERDES)
+	 MV88E6XXX_FLAGS_MULTI_CHIP)
 
 #define MV88E6XXX_FLAGS_FAMILY_6351	\
 	(MV88E6XXX_FLAG_G1_VTU_FID |	\
@@ -651,8 +630,7 @@ enum mv88e6xxx_cap {
 	 MV88E6XXX_FLAG_G2_MGMT_EN_0X |	\
 	 MV88E6XXX_FLAG_G2_POT |	\
 	 MV88E6XXX_FLAGS_IRL |		\
-	 MV88E6XXX_FLAGS_MULTI_CHIP |	\
-	 MV88E6XXX_FLAGS_SERDES)
+	 MV88E6XXX_FLAGS_MULTI_CHIP)
 
 #define MV88E6XXX_FLAGS_FAMILY_6390	\
 	(MV88E6XXX_FLAG_EEE |		\
@@ -884,6 +862,9 @@ struct mv88e6xxx_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);
@@ -942,5 +923,5 @@ 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
diff --git a/drivers/net/dsa/mv88e6xxx/phy.c b/drivers/net/dsa/mv88e6xxx/phy.c
new file mode 100644
index 000000000000..d47a6e08d88c
--- /dev/null
+++ b/drivers/net/dsa/mv88e6xxx/phy.c
@@ -0,0 +1,248 @@
+/*
+ * 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 "mv88e6xxx.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, 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);
+	}
+}
+
+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;
+}
+
+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_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..91fe3c3e9aea
--- /dev/null
+++ b/drivers/net/dsa/mv88e6xxx/phy.h
@@ -0,0 +1,40 @@
+/*
+ * 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
+
+/* 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/serdes.c b/drivers/net/dsa/mv88e6xxx/serdes.c
new file mode 100644
index 000000000000..53795676bd70
--- /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 "global2.h"
+#include "mv88e6xxx.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 == PORT_STATUS_CMODE_100BASE_X) ||
+	    (cmode == PORT_STATUS_CMODE_1000BASE_X) ||
+	    (cmode == PORT_STATUS_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 PORT_STATUS_CMODE_RXAUI:
+		if (!rxaui)
+			break;
+		/* Fall through */
+	case PORT_STATUS_CMODE_1000BASE_X:
+	case PORT_STATUS_CMODE_SGMII:
+	case PORT_STATUS_CMODE_2500BASEX:
+		if (cmode == PORT_STATUS_CMODE_1000BASE_X ||
+		    cmode == PORT_STATUS_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 PORT_STATUS_CMODE_1000BASE_X:
+	case PORT_STATUS_CMODE_SGMII:
+		return mv88e6390_serdes_sgmii(chip, MV88E6390_PORT9_LANE0, on);
+	case PORT_STATUS_CMODE_XAUI:
+	case PORT_STATUS_CMODE_RXAUI:
+	case PORT_STATUS_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 PORT_STATUS_CMODE_SGMII:
+		return mv88e6390_serdes_sgmii(chip, MV88E6390_PORT10_LANE0, on);
+	case PORT_STATUS_CMODE_XAUI:
+	case PORT_STATUS_CMODE_RXAUI:
+	case PORT_STATUS_CMODE_1000BASE_X:
+	case PORT_STATUS_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 cmode;
+
+	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..eb3ceaef790f
--- /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 "mv88e6xxx.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/ethernet/8390/ax88796.c b/drivers/net/ethernet/8390/ax88796.c
index b0a3b85fc6f8..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";
@@ -748,13 +754,13 @@ static int ax_init_dev(struct net_device *dev)
 
 	ret = ax_mii_init(dev);
 	if (ret)
-		goto out_irq;
+		goto err_out;
 
 	ax_NS8390_init(dev, 0);
 
 	ret = register_netdev(dev);
 	if (ret)
-		goto out_irq;
+		goto err_out;
 
 	netdev_info(dev, "%dbit, irq %d, %lx, MAC: %pM\n",
 		    ei_local->word16 ? 16 : 8, dev->irq, dev->base_addr,
@@ -762,9 +768,6 @@ static int ax_init_dev(struct net_device *dev)
 
 	return 0;
 
- out_irq:
-	/* cleanup irq */
-	free_irq(dev->irq, dev);
  err_out:
 	return ret;
 }
diff --git a/drivers/net/ethernet/amd/xgbe/xgbe-drv.c b/drivers/net/ethernet/amd/xgbe/xgbe-drv.c
index c772420fa41c..5a2ad9c5faab 100644
--- a/drivers/net/ethernet/amd/xgbe/xgbe-drv.c
+++ b/drivers/net/ethernet/amd/xgbe/xgbe-drv.c
@@ -1268,6 +1268,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 +1391,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)
diff --git a/drivers/net/ethernet/apm/xgene/xgene_enet_ethtool.c b/drivers/net/ethernet/apm/xgene/xgene_enet_ethtool.c
index 28fdedc30b74..559963b1aa32 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,7 +127,7 @@ 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;
 
@@ -111,7 +177,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 +208,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 +220,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 +304,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/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_a0.c b/drivers/net/ethernet/aquantia/atlantic/hw_atl/hw_atl_a0.c
index 4ee15ff06a44..faeb4935ef3e 100644
--- a/drivers/net/ethernet/aquantia/atlantic/hw_atl/hw_atl_a0.c
+++ b/drivers/net/ethernet/aquantia/atlantic/hw_atl/hw_atl_a0.c
@@ -200,29 +200,18 @@ err_exit:
 static int hw_atl_a0_hw_offload_set(struct aq_hw_s *self,
 				    struct aq_nic_cfg_s *aq_nic_cfg)
 {
-	int err = 0;
-
 	/* TX checksums offloads*/
 	tpo_ipv4header_crc_offload_en_set(self, 1);
 	tpo_tcp_udp_crc_offload_en_set(self, 1);
-	if (err < 0)
-		goto err_exit;
 
 	/* RX checksums offloads*/
 	rpo_ipv4header_crc_offload_en_set(self, 1);
 	rpo_tcp_udp_crc_offload_en_set(self, 1);
-	if (err < 0)
-		goto err_exit;
 
 	/* LSO offloads*/
 	tdm_large_send_offload_en_set(self, 0xFFFFFFFFU);
-	if (err < 0)
-		goto err_exit;
-
-	err = aq_hw_err_from_flags(self);
 
-err_exit:
-	return err;
+	return aq_hw_err_from_flags(self);
 }
 
 static int hw_atl_a0_hw_init_tx_path(struct aq_hw_s *self)
diff --git a/drivers/net/ethernet/aquantia/atlantic/hw_atl/hw_atl_b0.c b/drivers/net/ethernet/aquantia/atlantic/hw_atl/hw_atl_b0.c
index 42150708191d..1bceb7358e5c 100644
--- a/drivers/net/ethernet/aquantia/atlantic/hw_atl/hw_atl_b0.c
+++ b/drivers/net/ethernet/aquantia/atlantic/hw_atl/hw_atl_b0.c
@@ -200,25 +200,18 @@ err_exit:
 static int hw_atl_b0_hw_offload_set(struct aq_hw_s *self,
 				    struct aq_nic_cfg_s *aq_nic_cfg)
 {
-	int err = 0;
 	unsigned int i;
 
 	/* TX checksums offloads*/
 	tpo_ipv4header_crc_offload_en_set(self, 1);
 	tpo_tcp_udp_crc_offload_en_set(self, 1);
-	if (err < 0)
-		goto err_exit;
 
 	/* RX checksums offloads*/
 	rpo_ipv4header_crc_offload_en_set(self, 1);
 	rpo_tcp_udp_crc_offload_en_set(self, 1);
-	if (err < 0)
-		goto err_exit;
 
 	/* LSO offloads*/
 	tdm_large_send_offload_en_set(self, 0xFFFFFFFFU);
-	if (err < 0)
-		goto err_exit;
 
 /* LRO offloads */
 	{
@@ -245,10 +238,7 @@ static int hw_atl_b0_hw_offload_set(struct aq_hw_s *self,
 
 		rpo_lro_en_set(self, aq_nic_cfg->is_lro ? 0xFFFFFFFFU : 0U);
 	}
-	err = aq_hw_err_from_flags(self);
-
-err_exit:
-	return err;
+	return aq_hw_err_from_flags(self);
 }
 
 static int hw_atl_b0_hw_init_tx_path(struct aq_hw_s *self)
diff --git a/drivers/net/ethernet/atheros/atlx/atl2.c b/drivers/net/ethernet/atheros/atlx/atl2.c
index 63f2deec2a52..77a1c03255de 100644
--- a/drivers/net/ethernet/atheros/atlx/atl2.c
+++ b/drivers/net/ethernet/atheros/atlx/atl2.c
@@ -1353,6 +1353,7 @@ static int atl2_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	if (pci_set_dma_mask(pdev, DMA_BIT_MASK(32)) &&
 		pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(32))) {
 		printk(KERN_ERR "atl2: No usable DMA configuration, aborting\n");
+		err = -EIO;
 		goto err_dma;
 	}
 
@@ -1366,10 +1367,11 @@ static int atl2_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	 * pcibios_set_master to do the needed arch specific settings */
 	pci_set_master(pdev);
 
-	err = -ENOMEM;
 	netdev = alloc_etherdev(sizeof(struct atl2_adapter));
-	if (!netdev)
+	if (!netdev) {
+		err = -ENOMEM;
 		goto err_alloc_etherdev;
+	}
 
 	SET_NETDEV_DEV(netdev, &pdev->dev);
 
@@ -1408,8 +1410,6 @@ static int atl2_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	if (err)
 		goto err_sw_init;
 
-	err = -EIO;
-
 	netdev->hw_features = NETIF_F_HW_VLAN_CTAG_RX;
 	netdev->features |= (NETIF_F_HW_VLAN_CTAG_TX | NETIF_F_HW_VLAN_CTAG_RX);
 
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c
index a851f95c307a..14c236e5bdb1 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");
 		}
@@ -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/bnxt/bnxt.c b/drivers/net/ethernet/broadcom/bnxt/bnxt.c
index b56c54d68d5e..c1cd72a5eccf 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;
@@ -1329,8 +1339,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);
@@ -1771,8 +1782,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 +1798,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 +1861,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 +1975,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 +1999,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 +2020,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 +2866,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 +3239,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 +4711,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 +4749,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;
@@ -7345,6 +7412,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 +7567,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 +7662,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;
@@ -7630,14 +7703,14 @@ static int bnxt_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	dev->min_mtu = ETH_ZLEN;
 	dev->max_mtu = BNXT_MAX_MTU;
 
-	bnxt_dcb_init(bp);
-
 #ifdef CONFIG_BNXT_SRIOV
 	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)
@@ -7669,12 +7742,13 @@ static int bnxt_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	bnxt_hwrm_func_qcfg(bp);
 	bnxt_hwrm_port_led_qcaps(bp);
 	bnxt_ethtool_init(bp);
+	bnxt_dcb_init(bp);
 
 	bnxt_set_rx_skb_mode(bp, false);
 	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;
@@ -7686,9 +7760,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;
@@ -7760,6 +7832,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..5984423499e6 100644
--- a/drivers/net/ethernet/broadcom/bnxt/bnxt.h
+++ b/drivers/net/ethernet/broadcom/bnxt/bnxt.h
@@ -500,6 +500,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 +938,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 +1021,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 +1037,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 +1130,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 +1255,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_dcb.c b/drivers/net/ethernet/broadcom/bnxt/bnxt_dcb.c
index 46de2f8ff024..5c6dd0ce209f 100644
--- a/drivers/net/ethernet/broadcom/bnxt/bnxt_dcb.c
+++ b/drivers/net/ethernet/broadcom/bnxt/bnxt_dcb.c
@@ -553,8 +553,10 @@ static u8 bnxt_dcbnl_setdcbx(struct net_device *dev, u8 mode)
 	if ((mode & DCB_CAP_DCBX_VER_CEE) || !(mode & DCB_CAP_DCBX_VER_IEEE))
 		return 1;
 
-	if ((mode & DCB_CAP_DCBX_HOST) && BNXT_VF(bp))
-		return 1;
+	if (mode & DCB_CAP_DCBX_HOST) {
+		if (BNXT_VF(bp) || (bp->flags & BNXT_FLAG_FW_LLDP_AGENT))
+			return 1;
+	}
 
 	if (mode == bp->dcbx_cap)
 		return 0;
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..8ce793a0d030 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:
diff --git a/drivers/net/ethernet/cavium/liquidio/lio_main.c b/drivers/net/ethernet/cavium/liquidio/lio_main.c
index 927617cbf6a9..ba012427edd6 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:
@@ -3694,6 +3699,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);
@@ -4511,6 +4519,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..31d737c22648 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:
diff --git a/drivers/net/ethernet/cavium/liquidio/octeon_device.c b/drivers/net/ethernet/cavium/liquidio/octeon_device.c
index e21b477d0159..3b7cc9320deb 100644
--- a/drivers/net/ethernet/cavium/liquidio/octeon_device.c
+++ b/drivers/net/ethernet/cavium/liquidio/octeon_device.c
@@ -543,7 +543,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 +565,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 +725,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 +753,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)
 {
@@ -1354,13 +1429,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_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/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/chelsio/cxgb4/cxgb4.h b/drivers/net/ethernet/chelsio/cxgb4/cxgb4.h
index e88c1808e46f..1cf3e2f89fc1 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/cxgb4.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4.h
@@ -1551,6 +1551,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_ethtool.c b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_ethtool.c
index 0ba7866c8259..e9bab72253bb 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) {
diff --git a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c
index 38a5c6764bb5..ac9ba8bc6c37 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c
@@ -891,7 +891,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;
 
@@ -2245,6 +2245,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);
@@ -2720,6 +2727,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,
@@ -2741,6 +2758,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
@@ -2771,6 +2789,9 @@ void t4_fatal_err(struct adapter *adap)
 {
 	int port;
 
+	if (pci_channel_offline(adap->pdev))
+		return;
+
 	/* Disable the SGE since ULDs are going to free resources that
 	 * could be exposed to the adapter.  RDMA MWs for example...
 	 */
@@ -3882,9 +3903,10 @@ static pci_ers_result_t eeh_err_detected(struct pci_dev *pdev,
 	spin_lock(&adap->stats_lock);
 	for_each_port(adap, i) {
 		struct net_device *dev = adap->port[i];
-
-		netif_device_detach(dev);
-		netif_carrier_off(dev);
+		if (dev) {
+			netif_device_detach(dev);
+			netif_carrier_off(dev);
+		}
 	}
 	spin_unlock(&adap->stats_lock);
 	disable_interrupts(adap);
@@ -3963,12 +3985,13 @@ static void eeh_resume(struct pci_dev *pdev)
 	rtnl_lock();
 	for_each_port(adap, i) {
 		struct net_device *dev = adap->port[i];
-
-		if (netif_running(dev)) {
-			link_start(dev);
-			cxgb_set_rxmode(dev);
+		if (dev) {
+			if (netif_running(dev)) {
+				link_start(dev);
+				cxgb_set_rxmode(dev);
+			}
+			netif_device_attach(dev);
 		}
-		netif_device_attach(dev);
 	}
 	rtnl_unlock();
 }
@@ -4007,10 +4030,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;
 	}
@@ -4031,7 +4051,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 */
@@ -4044,6 +4064,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);
 
@@ -4949,6 +4972,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;
diff --git a/drivers/net/ethernet/chelsio/cxgb4/t4_hw.c b/drivers/net/ethernet/chelsio/cxgb4/t4_hw.c
index aded42b96f6d..410702920bd0 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/t4_hw.c
+++ b/drivers/net/ethernet/chelsio/cxgb4/t4_hw.c
@@ -4557,8 +4557,13 @@ void t4_intr_enable(struct adapter *adapter)
  */
 void t4_intr_disable(struct adapter *adapter)
 {
-	u32 whoami = t4_read_reg(adapter, PL_WHOAMI_A);
-	u32 pf = CHELSIO_CHIP_VERSION(adapter->params.chip) <= CHELSIO_T5 ?
+	u32 whoami, pf;
+
+	if (pci_channel_offline(adapter->pdev))
+		return;
+
+	whoami = t4_read_reg(adapter, PL_WHOAMI_A);
+	pf = CHELSIO_CHIP_VERSION(adapter->params.chip) <= CHELSIO_T5 ?
 			SOURCEPF_G(whoami) : T6_SOURCEPF_G(whoami);
 
 	t4_write_reg(adapter, MYPF_REG(PL_PF_INT_ENABLE_A), 0);
@@ -6288,13 +6293,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
@@ -6305,7 +6315,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;
 }
 
 /**
@@ -7355,11 +7375,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
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..be7041f6cf71 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:
 	 */
diff --git a/drivers/net/ethernet/chelsio/cxgb4/t4fw_api.h b/drivers/net/ethernet/chelsio/cxgb4/t4fw_api.h
index 251a35e9795c..c65c33c03bcb 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/t4fw_api.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/t4fw_api.h
@@ -2572,6 +2572,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
 };
diff --git a/drivers/net/ethernet/chelsio/cxgb4/t4fw_version.h b/drivers/net/ethernet/chelsio/cxgb4/t4fw_version.h
index fa376444e57c..3549d3876278 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/t4fw_version.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/t4fw_version.h
@@ -37,7 +37,7 @@
 
 #define T4FW_VERSION_MAJOR 0x01
 #define T4FW_VERSION_MINOR 0x10
-#define T4FW_VERSION_MICRO 0x21
+#define T4FW_VERSION_MICRO 0x2B
 #define T4FW_VERSION_BUILD 0x00
 
 #define T4FW_MIN_VERSION_MAJOR 0x01
@@ -46,7 +46,7 @@
 
 #define T5FW_VERSION_MAJOR 0x01
 #define T5FW_VERSION_MINOR 0x10
-#define T5FW_VERSION_MICRO 0x21
+#define T5FW_VERSION_MICRO 0x2B
 #define T5FW_VERSION_BUILD 0x00
 
 #define T5FW_MIN_VERSION_MAJOR 0x00
@@ -55,7 +55,7 @@
 
 #define T6FW_VERSION_MAJOR 0x01
 #define T6FW_VERSION_MINOR 0x10
-#define T6FW_VERSION_MICRO 0x21
+#define T6FW_VERSION_MICRO 0x2B
 #define T6FW_VERSION_BUILD 0x00
 
 #define T6FW_MIN_VERSION_MAJOR 0x00
diff --git a/drivers/net/ethernet/cisco/enic/enic_main.c b/drivers/net/ethernet/cisco/enic/enic_main.c
index 4b87beeabce1..6a9c8878aca0 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]);
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 f3a09ab55900..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)
@@ -5078,9 +5089,11 @@ static netdev_features_t be_features_check(struct sk_buff *skb,
 	struct be_adapter *adapter = netdev_priv(dev);
 	u8 l4_hdr = 0;
 
-	/* The code below restricts offload features for some tunneled packets.
+	/* The code below restricts offload features for some tunneled and
+	 * Q-in-Q packets.
 	 * Offload features for normal (non tunnel) packets are unchanged.
 	 */
+	features = vlan_features_check(skb, features);
 	if (!skb->encapsulation ||
 	    !(adapter->flags & BE_FLAGS_VXLAN_OFFLOADS))
 		return features;
diff --git a/drivers/net/ethernet/faraday/ftmac100.c b/drivers/net/ethernet/faraday/ftmac100.c
index 6ac336b546e6..1536356e2ea8 100644
--- a/drivers/net/ethernet/faraday/ftmac100.c
+++ b/drivers/net/ethernet/faraday/ftmac100.c
@@ -1174,11 +1174,17 @@ static int ftmac100_remove(struct platform_device *pdev)
 	return 0;
 }
 
+static const struct of_device_id ftmac100_of_ids[] = {
+	{ .compatible = "andestech,atmac100" },
+	{ }
+};
+
 static struct platform_driver ftmac100_driver = {
 	.probe		= ftmac100_probe,
 	.remove		= ftmac100_remove,
 	.driver		= {
 		.name	= DRV_NAME,
+		.of_match_table = ftmac100_of_ids
 	},
 };
 
@@ -1202,3 +1208,4 @@ module_exit(ftmac100_exit);
 MODULE_AUTHOR("Po-Yu Chuang <ratbert@faraday-tech.com>");
 MODULE_DESCRIPTION("FTMAC100 driver");
 MODULE_LICENSE("GPL");
+MODULE_DEVICE_TABLE(of, ftmac100_of_ids);
diff --git a/drivers/net/ethernet/freescale/fec_main.c b/drivers/net/ethernet/freescale/fec_main.c
index 56a563f90b0b..f7c8649fd28f 100644
--- a/drivers/net/ethernet/freescale/fec_main.c
+++ b/drivers/net/ethernet/freescale/fec_main.c
@@ -3192,7 +3192,7 @@ static int fec_reset_phy(struct platform_device *pdev)
 {
 	int err, phy_reset;
 	bool active_high = false;
-	int msec = 1;
+	int msec = 1, phy_post_delay = 0;
 	struct device_node *np = pdev->dev.of_node;
 
 	if (!np)
@@ -3209,6 +3209,11 @@ static int fec_reset_phy(struct platform_device *pdev)
 	else if (!gpio_is_valid(phy_reset))
 		return 0;
 
+	err = of_property_read_u32(np, "phy-reset-post-delay", &phy_post_delay);
+	/* valid reset duration should be less than 1s */
+	if (!err && phy_post_delay > 1000)
+		return -EINVAL;
+
 	active_high = of_property_read_bool(np, "phy-reset-active-high");
 
 	err = devm_gpio_request_one(&pdev->dev, phy_reset,
@@ -3226,6 +3231,15 @@ static int fec_reset_phy(struct platform_device *pdev)
 
 	gpio_set_value_cansleep(phy_reset, !active_high);
 
+	if (!phy_post_delay)
+		return 0;
+
+	if (phy_post_delay > 20)
+		msleep(phy_post_delay);
+	else
+		usleep_range(phy_post_delay * 1000,
+			     phy_post_delay * 1000 + 1000);
+
 	return 0;
 }
 #else /* CONFIG_OF */
diff --git a/drivers/net/ethernet/freescale/fsl_pq_mdio.c b/drivers/net/ethernet/freescale/fsl_pq_mdio.c
index 446c7b374ff5..a10de1e9c157 100644
--- a/drivers/net/ethernet/freescale/fsl_pq_mdio.c
+++ b/drivers/net/ethernet/freescale/fsl_pq_mdio.c
@@ -381,7 +381,7 @@ static int fsl_pq_mdio_probe(struct platform_device *pdev)
 {
 	const struct of_device_id *id =
 		of_match_device(fsl_pq_mdio_match, &pdev->dev);
-	const struct fsl_pq_mdio_data *data = id->data;
+	const struct fsl_pq_mdio_data *data;
 	struct device_node *np = pdev->dev.of_node;
 	struct resource res;
 	struct device_node *tbi;
@@ -389,6 +389,13 @@ static int fsl_pq_mdio_probe(struct platform_device *pdev)
 	struct mii_bus *new_bus;
 	int err;
 
+	if (!id) {
+		dev_err(&pdev->dev, "Failed to match device\n");
+		return -ENODEV;
+	}
+
+	data = id->data;
+
 	dev_dbg(&pdev->dev, "found %s compatible node\n", id->compatible);
 
 	new_bus = mdiobus_alloc_size(sizeof(*priv));
diff --git a/drivers/net/ethernet/ibm/ibmveth.c b/drivers/net/ethernet/ibm/ibmveth.c
index 72ab7b6bf20b..9a74c4e2e193 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;
 
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 4f2d329dba99..8dcf58088178 100644
--- a/drivers/net/ethernet/ibm/ibmvnic.c
+++ b/drivers/net/ethernet/ibm/ibmvnic.c
@@ -163,6 +163,16 @@ static long h_reg_sub_crq(unsigned long unit_address, unsigned long token,
 	return rc;
 }
 
+static void 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);
+}
+
 static int alloc_long_term_buff(struct ibmvnic_adapter *adapter,
 				struct ibmvnic_long_term_buff *ltb, int size)
 {
@@ -200,6 +210,15 @@ static void free_long_term_buff(struct ibmvnic_adapter *adapter,
 	dma_free_coherent(dev, ltb->size, ltb->buff, ltb->addr);
 }
 
+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 +236,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 +309,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 +362,32 @@ 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;
+
+	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];
+
+		reset_long_term_buff(adapter, &rx_pool->long_term_buff);
+
+		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;
+	}
+
+	return 0;
+}
+
 static void release_rx_pools(struct ibmvnic_adapter *adapter)
 {
 	struct ibmvnic_rx_pool *rx_pool;
@@ -432,6 +489,32 @@ 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;
+
+	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];
+
+		reset_long_term_buff(adapter, &tx_pool->long_term_buff);
+
+		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 +601,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);
@@ -674,9 +783,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
@@ -779,13 +886,7 @@ static int __ibmvnic_close(struct net_device *netdev)
 
 	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]);
-	}
-
-	clean_tx_pools(adapter);
+	ibmvnic_napi_disable(adapter);
 
 	if (adapter->tx_scrq) {
 		for (i = 0; i < adapter->req_tx_queues; i++)
@@ -814,6 +915,7 @@ static int __ibmvnic_close(struct net_device *netdev)
 		}
 	}
 
+	clean_tx_pools(adapter);
 	adapter->state = VNIC_CLOSED;
 	return rc;
 }
@@ -1092,8 +1194,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 +1314,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 +1364,7 @@ static int do_reset(struct ibmvnic_adapter *adapter,
 	for (i = 0; i < adapter->req_rx_queues; i++)
 		napi_schedule(&adapter->napi[i]);
 
+	netdev_notify_peers(netdev);
 	return 0;
 }
 
@@ -1313,6 +1424,7 @@ static void __ibmvnic_reset(struct work_struct *work)
 
 	if (rc) {
 		free_all_rwi(adapter);
+		mutex_unlock(&adapter->reset_lock);
 		return;
 	}
 
@@ -1383,6 +1495,10 @@ 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;
+
+	if (adapter->resetting)
+		return 0;
+
 restart_poll:
 	while (frames_processed < budget) {
 		struct sk_buff *skb;
@@ -1441,7 +1557,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]);
@@ -1608,6 +1726,45 @@ 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, 2 * 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;
+	}
+
+	rc = init_sub_crq_irqs(adapter);
+	return rc;
+}
+
 static void release_sub_crq_queue(struct ibmvnic_adapter *adapter,
 				  struct ibmvnic_sub_crq_queue *scrq)
 {
@@ -2742,6 +2899,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,
@@ -3147,6 +3306,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");
@@ -3455,21 +3616,38 @@ 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);
 	ibmvnic_send_crq_init(adapter);
 	if (!wait_for_completion_timeout(&adapter->init_done, timeout)) {
 		dev_err(dev, "Initialization sequence timed out\n");
-		release_crq_queue(adapter);
 		return -1;
 	}
 
-	rc = init_sub_crqs(adapter);
+	if (adapter->from_passive_init) {
+		adapter->state = VNIC_OPEN;
+		adapter->from_passive_init = false;
+		return -1;
+	}
+
+	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);
diff --git a/drivers/net/ethernet/ibm/ibmvnic.h b/drivers/net/ethernet/ibm/ibmvnic.h
index 4702b48cfa44..7e2300e64a47 100644
--- a/drivers/net/ethernet/ibm/ibmvnic.h
+++ b/drivers/net/ethernet/ibm/ibmvnic.h
@@ -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 {
@@ -1031,4 +1032,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/e1000e/netdev.c b/drivers/net/ethernet/intel/e1000e/netdev.c
index b3679728caac..6ed3bc419b96 100644
--- a/drivers/net/ethernet/intel/e1000e/netdev.c
+++ b/drivers/net/ethernet/intel/e1000e/netdev.c
@@ -3680,6 +3680,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;
@@ -5867,10 +5868,10 @@ static netdev_tx_t e1000_xmit_frame(struct sk_buff *skb,
 			adapter->tx_hwtstamp_skb = skb_get(skb);
 			adapter->tx_hwtstamp_start = jiffies;
 			schedule_work(&adapter->tx_hwtstamp_work);
-		} else {
-			skb_tx_timestamp(skb);
 		}
 
+		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. */
diff --git a/drivers/net/ethernet/intel/i40e/i40e.h b/drivers/net/ethernet/intel/i40e/i40e.h
index cdde3cc28fb5..25bf336c5f38 100644
--- a/drivers/net/ethernet/intel/i40e/i40e.h
+++ b/drivers/net/ethernet/intel/i40e/i40e.h
@@ -502,10 +502,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. */
@@ -955,7 +957,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);
diff --git a/drivers/net/ethernet/intel/i40e/i40e_client.c b/drivers/net/ethernet/intel/i40e/i40e_client.c
index c3b81a97558e..088b4a43bd2a 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_client.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_client.c
@@ -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_ethtool.c b/drivers/net/ethernet/intel/i40e/i40e_ethtool.c
index 7a8eb486b9ea..35a246f05520 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),
diff --git a/drivers/net/ethernet/intel/i40e/i40e_main.c b/drivers/net/ethernet/intel/i40e/i40e_main.c
index d5c9c9e06ff5..e4eb97832413 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_main.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_main.c
@@ -6372,7 +6372,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);
 }
 
 /**
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 29321a6167a6..c2e9013d05eb 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_txrx.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_txrx.c
@@ -2628,8 +2628,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;
 	}
 
@@ -2932,10 +2934,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);
@@ -3093,7 +3097,7 @@ do_rs:
 		mmiowb();
 	}
 
-	return;
+	return 0;
 
 dma_error:
 	dev_info(tx_ring->dev, "TX DMA map failed\n");
@@ -3110,6 +3114,8 @@ dma_error:
 	}
 
 	tx_ring->next_to_use = i;
+
+	return -1;
 }
 
 /**
@@ -3210,8 +3216,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;
 
@@ -3219,6 +3226,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/i40evf/i40e_common.c b/drivers/net/ethernet/intel/i40evf/i40e_common.c
index 43f10761f4ba..6729624fda5b 100644
--- a/drivers/net/ethernet/intel/i40evf/i40e_common.c
+++ b/drivers/net/ethernet/intel/i40evf/i40e_common.c
@@ -1105,8 +1105,7 @@ void i40e_vf_parse_hw_config(struct i40e_hw *hw,
 	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;
+	hw->dev_caps.fcoe = 0;
 	for (i = 0; i < msg->num_vsis; i++) {
 		if (vsi_res->vsi_type == I40E_VSI_SRIOV) {
 			ether_addr_copy(hw->mac.perm_addr,
diff --git a/drivers/net/ethernet/intel/i40evf/i40e_virtchnl.h b/drivers/net/ethernet/intel/i40evf/i40e_virtchnl.h
index c5ad0388c3d5..b0b8de5d6f57 100644
--- a/drivers/net/ethernet/intel/i40evf/i40e_virtchnl.h
+++ b/drivers/net/ethernet/intel/i40evf/i40e_virtchnl.h
@@ -79,7 +79,7 @@ enum i40e_virtchnl_ops {
 	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_RSVD = 16,
 	I40E_VIRTCHNL_OP_EVENT = 17, /* must ALWAYS be 17 */
 	I40E_VIRTCHNL_OP_IWARP = 20,
 	I40E_VIRTCHNL_OP_CONFIG_IWARP_IRQ_MAP = 21,
@@ -155,7 +155,6 @@ struct i40e_virtchnl_vsi_resource {
 /* 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
diff --git a/drivers/net/ethernet/intel/i40evf/i40evf_virtchnl.c b/drivers/net/ethernet/intel/i40evf/i40evf_virtchnl.c
index deb2cb8dac6b..91b21f26f8d4 100644
--- a/drivers/net/ethernet/intel/i40evf/i40evf_virtchnl.c
+++ b/drivers/net/ethernet/intel/i40evf/i40evf_virtchnl.c
@@ -152,9 +152,8 @@ 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_PF |
 	       I40E_VIRTCHNL_VF_OFFLOAD_RSS_AQ |
 	       I40E_VIRTCHNL_VF_OFFLOAD_RSS_REG |
 	       I40E_VIRTCHNL_VF_OFFLOAD_VLAN |
diff --git a/drivers/net/ethernet/intel/igb/igb_ptp.c b/drivers/net/ethernet/intel/igb/igb_ptp.c
index 7a3fd4d74592..d333d6d80194 100644
--- a/drivers/net/ethernet/intel/igb/igb_ptp.c
+++ b/drivers/net/ethernet/intel/igb/igb_ptp.c
@@ -941,6 +941,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_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..9113e8099b03 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_ethtool.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_ethtool.c
@@ -2254,6 +2254,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 +2668,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_main.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_main.c
index d39cba214320..54463f03b3db 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"
@@ -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 +=
@@ -7667,7 +7668,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 */
@@ -8205,6 +8209,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);
 	}
@@ -9929,6 +9934,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 +10197,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 +10285,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);
 
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..d44c728fdc0b 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_ptp.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_ptp.c
@@ -883,6 +883,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..e2766da5fe02 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)
@@ -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..aced91c9c034 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"
 
@@ -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..0e5083a48937 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)
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/en_netdev.c b/drivers/net/ethernet/mellanox/mlx4/en_netdev.c
index 94fab20ef146..82436742ad75 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_netdev.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_netdev.c
@@ -2375,6 +2375,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:
diff --git a/drivers/net/ethernet/mellanox/mlx4/main.c b/drivers/net/ethernet/mellanox/mlx4/main.c
index 703205475524..83aab1e4c8c8 100644
--- a/drivers/net/ethernet/mellanox/mlx4/main.c
+++ b/drivers/net/ethernet/mellanox/mlx4/main.c
@@ -2862,12 +2862,10 @@ static void mlx4_enable_msi_x(struct mlx4_dev *dev)
 	int port = 0;
 
 	if (msi_x) {
-		int nreq = dev->caps.num_ports * num_online_cpus() + 1;
-
-		nreq = min_t(int, dev->caps.num_eqs - dev->caps.reserved_eqs,
-			     nreq);
-		if (nreq > MAX_MSIX)
-			nreq = MAX_MSIX;
+		int nreq = min3(dev->caps.num_ports *
+				(int)num_online_cpus() + 1,
+				dev->caps.num_eqs - dev->caps.reserved_eqs,
+				MAX_MSIX);
 
 		entries = kcalloc(nreq, sizeof *entries, GFP_KERNEL);
 		if (!entries)
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/Kconfig b/drivers/net/ethernet/mellanox/mlx5/core/Kconfig
index fc52d742b7f7..cf1ef48bfd8d 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/Kconfig
+++ b/drivers/net/ethernet/mellanox/mlx5/core/Kconfig
@@ -11,9 +11,19 @@ config MLX5_CORE
 	  Core driver for low level functionality of the ConnectX-4 and
 	  Connect-IB cards by Mellanox Technologies.
 
+config MLX5_FPGA
+        bool "Mellanox Technologies Innova support"
+        depends on MLX5_CORE
+        ---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 && PCI && MLX5_CORE
+	depends on NETDEVICES && ETHERNET && INET && PCI && MLX5_CORE
 	depends on IPV6=y || IPV6=n || MLX5_CORE=m
 	imply PTP_1588_CLOCK
 	default n
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/Makefile b/drivers/net/ethernet/mellanox/mlx5/core/Makefile
index 9e644615f07a..12556c03eec4 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/Makefile
+++ b/drivers/net/ethernet/mellanox/mlx5/core/Makefile
@@ -1,10 +1,13 @@
 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
 
+mlx5_core-$(CONFIG_MLX5_FPGA) += fpga/cmd.o fpga/core.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 \
 		en_rx.o en_rx_am.o en_txrx.o en_clock.o vxlan.o \
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/cmd.c b/drivers/net/ethernet/mellanox/mlx5/core/cmd.c
index 5bdaf3d545b2..10d282841f5b 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/cmd.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/cmd.c
@@ -774,7 +774,7 @@ static void cb_timeout_handler(struct work_struct *work)
 	mlx5_core_warn(dev, "%s(0x%x) timeout. Will cause a leak of a command resource\n",
 		       mlx5_command_str(msg_to_opcode(ent->in)),
 		       msg_to_opcode(ent->in));
-	mlx5_cmd_comp_handler(dev, 1UL << ent->idx);
+	mlx5_cmd_comp_handler(dev, 1UL << ent->idx, true);
 }
 
 static void cmd_work_handler(struct work_struct *work)
@@ -804,6 +804,7 @@ static void cmd_work_handler(struct work_struct *work)
 	}
 
 	cmd->ent_arr[ent->idx] = ent;
+	set_bit(MLX5_CMD_ENT_STATE_PENDING_COMP, &ent->state);
 	lay = get_inst(cmd, ent->idx);
 	ent->lay = lay;
 	memset(lay, 0, sizeof(*lay));
@@ -825,6 +826,20 @@ static void cmd_work_handler(struct work_struct *work)
 	if (ent->callback)
 		schedule_delayed_work(&ent->cb_timeout_work, cb_timeout);
 
+	/* Skip sending command to fw if internal error */
+	if (pci_channel_offline(dev->pdev) ||
+	    dev->state == MLX5_DEVICE_STATE_INTERNAL_ERROR) {
+		u8 status = 0;
+		u32 drv_synd;
+
+		ent->ret = mlx5_internal_err_ret_value(dev, msg_to_opcode(ent->in), &drv_synd, &status);
+		MLX5_SET(mbox_out, ent->out, status, status);
+		MLX5_SET(mbox_out, ent->out, syndrome, drv_synd);
+
+		mlx5_cmd_comp_handler(dev, 1UL << ent->idx, true);
+		return;
+	}
+
 	/* ring doorbell after the descriptor is valid */
 	mlx5_core_dbg(dev, "writing 0x%x to command doorbell\n", 1 << ent->idx);
 	wmb();
@@ -835,7 +850,7 @@ static void cmd_work_handler(struct work_struct *work)
 		poll_timeout(ent);
 		/* make sure we read the descriptor after ownership is SW */
 		rmb();
-		mlx5_cmd_comp_handler(dev, 1UL << ent->idx);
+		mlx5_cmd_comp_handler(dev, 1UL << ent->idx, (ent->ret == -ETIMEDOUT));
 	}
 }
 
@@ -879,7 +894,7 @@ static int wait_func(struct mlx5_core_dev *dev, struct mlx5_cmd_work_ent *ent)
 		wait_for_completion(&ent->done);
 	} else if (!wait_for_completion_timeout(&ent->done, timeout)) {
 		ent->ret = -ETIMEDOUT;
-		mlx5_cmd_comp_handler(dev, 1UL << ent->idx);
+		mlx5_cmd_comp_handler(dev, 1UL << ent->idx, true);
 	}
 
 	err = ent->ret;
@@ -1375,7 +1390,7 @@ static void free_msg(struct mlx5_core_dev *dev, struct mlx5_cmd_msg *msg)
 	}
 }
 
-void mlx5_cmd_comp_handler(struct mlx5_core_dev *dev, u64 vec)
+void mlx5_cmd_comp_handler(struct mlx5_core_dev *dev, u64 vec, bool forced)
 {
 	struct mlx5_cmd *cmd = &dev->cmd;
 	struct mlx5_cmd_work_ent *ent;
@@ -1395,6 +1410,19 @@ void mlx5_cmd_comp_handler(struct mlx5_core_dev *dev, u64 vec)
 			struct semaphore *sem;
 
 			ent = cmd->ent_arr[i];
+
+			/* if we already completed the command, ignore it */
+			if (!test_and_clear_bit(MLX5_CMD_ENT_STATE_PENDING_COMP,
+						&ent->state)) {
+				/* only real completion can free the cmd slot */
+				if (!forced) {
+					mlx5_core_err(dev, "Command completion arrived after timeout (entry idx = %d).\n",
+						      ent->idx);
+					free_ent(cmd, ent->idx);
+				}
+				continue;
+			}
+
 			if (ent->callback)
 				cancel_delayed_work(&ent->cb_timeout_work);
 			if (ent->page_queue)
@@ -1417,7 +1445,10 @@ void mlx5_cmd_comp_handler(struct mlx5_core_dev *dev, u64 vec)
 				mlx5_core_dbg(dev, "command completed. ret 0x%x, delivery status %s(0x%x)\n",
 					      ent->ret, deliv_status_to_str(ent->status), ent->status);
 			}
-			free_ent(cmd, ent->idx);
+
+			/* only real completion will free the entry slot */
+			if (!forced)
+				free_ent(cmd, ent->idx);
 
 			if (ent->callback) {
 				ds = ent->ts2 - ent->ts1;
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/debugfs.c b/drivers/net/ethernet/mellanox/mlx5/core/debugfs.c
index e94a9532e218..de40b6cfee95 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/debugfs.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/debugfs.c
@@ -405,7 +405,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;
 
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en.h b/drivers/net/ethernet/mellanox/mlx5/core/en.h
index 0099a3e397bc..2fd044b23875 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en.h
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en.h
@@ -1003,7 +1003,7 @@ int mlx5e_create_direct_tirs(struct mlx5e_priv *priv);
 void mlx5e_destroy_direct_tirs(struct mlx5e_priv *priv);
 void mlx5e_destroy_rqt(struct mlx5e_priv *priv, struct mlx5e_rqt *rqt);
 
-int mlx5e_create_ttc_table(struct mlx5e_priv *priv, u32 underlay_qpn);
+int mlx5e_create_ttc_table(struct mlx5e_priv *priv);
 void mlx5e_destroy_ttc_table(struct mlx5e_priv *priv);
 
 int mlx5e_create_tis(struct mlx5_core_dev *mdev, int tc,
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_arfs.c b/drivers/net/ethernet/mellanox/mlx5/core/en_arfs.c
index c8a005326e30..f4017c06ddd2 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en_arfs.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_arfs.c
@@ -180,9 +180,8 @@ static int arfs_add_default_rule(struct mlx5e_priv *priv,
 	struct mlx5_flow_spec *spec;
 	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;
 	}
@@ -237,7 +236,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 +480,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..e29494464cae 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en_clock.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_clock.c
@@ -128,6 +128,7 @@ 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");
 		err = mlx5e_modify_rx_cqe_compression_locked(priv, false);
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_common.c b/drivers/net/ethernet/mellanox/mlx5/core/en_common.c
index f1f17f7a3cd0..46e56ec4c26f 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;
 
@@ -147,7 +147,7 @@ int mlx5e_refresh_tirs(struct mlx5e_priv *priv, bool enable_uc_lb)
 
 
 	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 ce7b09d72ff6..e9e33fd68279 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en_ethtool.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_ethtool.c
@@ -39,7 +39,7 @@ static void mlx5e_get_drvinfo(struct net_device *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)",
@@ -794,7 +794,6 @@ static void get_supported(u32 eth_proto_cap,
 	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);
-	ethtool_link_ksettings_add_link_mode(link_ksettings, supported, Asym_Pause);
 }
 
 static void get_advertising(u32 eth_proto_cap, u8 tx_pause,
@@ -804,7 +803,7 @@ static void get_advertising(u32 eth_proto_cap, u8 tx_pause,
 	unsigned long *advertising = link_ksettings->link_modes.advertising;
 
 	ptys2ethtool_adver_link(advertising, eth_proto_cap);
-	if (tx_pause)
+	if (rx_pause)
 		ethtool_link_ksettings_add_link_mode(link_ksettings, advertising, Pause);
 	if (tx_pause ^ rx_pause)
 		ethtool_link_ksettings_add_link_mode(link_ksettings, advertising, Asym_Pause);
@@ -849,6 +848,8 @@ static int mlx5e_get_link_ksettings(struct net_device *netdev,
 	struct mlx5e_priv *priv    = netdev_priv(netdev);
 	struct mlx5_core_dev *mdev = priv->mdev;
 	u32 out[MLX5_ST_SZ_DW(ptys_reg)] = {0};
+	u32 rx_pause = 0;
+	u32 tx_pause = 0;
 	u32 eth_proto_cap;
 	u32 eth_proto_admin;
 	u32 eth_proto_lp;
@@ -871,11 +872,13 @@ static int mlx5e_get_link_ksettings(struct net_device *netdev,
 	an_disable_admin = MLX5_GET(ptys_reg, out, an_disable_admin);
 	an_status        = MLX5_GET(ptys_reg, out, an_status);
 
+	mlx5_query_port_pause(mdev, &rx_pause, &tx_pause);
+
 	ethtool_link_ksettings_zero_link_mode(link_ksettings, supported);
 	ethtool_link_ksettings_zero_link_mode(link_ksettings, advertising);
 
 	get_supported(eth_proto_cap, link_ksettings);
-	get_advertising(eth_proto_admin, 0, 0, link_ksettings);
+	get_advertising(eth_proto_admin, tx_pause, rx_pause, link_ksettings);
 	get_speed_duplex(netdev, eth_proto_oper, link_ksettings);
 
 	eth_proto_oper = eth_proto_oper ? eth_proto_oper : eth_proto_cap;
@@ -1045,7 +1048,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;
 
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_fs.c b/drivers/net/ethernet/mellanox/mlx5/core/en_fs.c
index 576d6787b484..7acc4fba7ece 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en_fs.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_fs.c
@@ -218,11 +218,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 +658,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 +738,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;
@@ -800,7 +796,7 @@ void mlx5e_destroy_ttc_table(struct mlx5e_priv *priv)
 	mlx5e_destroy_flow_table(&ttc->ft);
 }
 
-int mlx5e_create_ttc_table(struct mlx5e_priv *priv, u32 underlay_qpn)
+int mlx5e_create_ttc_table(struct mlx5e_priv *priv)
 {
 	struct mlx5e_ttc_table *ttc = &priv->fs.ttc;
 	struct mlx5_flow_table_attr ft_attr = {};
@@ -810,7 +806,6 @@ int mlx5e_create_ttc_table(struct mlx5e_priv *priv, u32 underlay_qpn)
 	ft_attr.max_fte = MLX5E_TTC_TABLE_SIZE;
 	ft_attr.level = MLX5E_TTC_FT_LEVEL;
 	ft_attr.prio = MLX5E_NIC_PRIO;
-	ft_attr.underlay_qpn = underlay_qpn;
 
 	ft->t = mlx5_create_flow_table(priv->fs.ns, &ft_attr);
 	if (IS_ERR(ft->t)) {
@@ -853,11 +848,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);
@@ -917,7 +910,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;
@@ -1072,7 +1065,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;
 
@@ -1147,7 +1140,7 @@ int mlx5e_create_flow_steering(struct mlx5e_priv *priv)
 		priv->netdev->hw_features &= ~NETIF_F_NTUPLE;
 	}
 
-	err = mlx5e_create_ttc_table(priv, 0);
+	err = mlx5e_create_ttc_table(priv);
 	if (err) {
 		netdev_err(priv->netdev, "Failed to create ttc table, err=%d\n",
 			   err);
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 a61b71b6fff3..cdff04b2aea1 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en_main.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_main.c
@@ -252,9 +252,9 @@ static void mlx5e_update_pport_counters(struct mlx5e_priv *priv)
 	void *out;
 	u32 *in;
 
-	in = mlx5_vzalloc(sz);
+	in = kvzalloc(sz, GFP_KERNEL);
 	if (!in)
-		goto free_out;
+		return;
 
 	MLX5_SET(ppcnt_reg, in, local_port, 1);
 
@@ -288,7 +288,6 @@ static void mlx5e_update_pport_counters(struct mlx5e_priv *priv)
 				     MLX5_REG_PPCNT, 0, 0);
 	}
 
-free_out:
 	kvfree(in);
 }
 
@@ -314,7 +313,7 @@ static void mlx5e_update_pcie_counters(struct mlx5e_priv *priv)
 	if (!MLX5_CAP_MCAM_FEATURE(mdev, pcie_performance_group))
 		return;
 
-	in = mlx5_vzalloc(sz);
+	in = kvzalloc(sz, GFP_KERNEL);
 	if (!in)
 		return;
 
@@ -503,7 +502,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;
 
@@ -711,7 +710,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 +747,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 +775,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 +804,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;
 
@@ -1134,7 +1133,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 +1181,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 +1495,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;
 
@@ -2091,7 +2090,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 +2209,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 +2432,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;
 
@@ -2850,7 +2849,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 +2888,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;
 
@@ -2976,7 +2975,7 @@ static int mlx5e_setup_tc(struct net_device *netdev, u8 tc)
 	new_channels.params = priv->channels.params;
 	new_channels.params.num_tc = tc ? tc : 1;
 
-	if (test_bit(MLX5E_STATE_OPENED, &priv->state)) {
+	if (!test_bit(MLX5E_STATE_OPENED, &priv->state)) {
 		priv->channels.params = new_channels.params;
 		goto out;
 	}
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_rx.c b/drivers/net/ethernet/mellanox/mlx5/core/en_rx.c
index 7b1566f0ae58..66b5fec15313 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en_rx.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_rx.c
@@ -1041,6 +1041,8 @@ void mlx5e_free_xdpsq_descs(struct mlx5e_xdpsq *sq)
 #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,
@@ -1048,6 +1050,7 @@ static inline void mlx5i_complete_rx_cqe(struct mlx5e_rq *rq,
 					 struct sk_buff *skb)
 {
 	struct net_device *netdev = rq->netdev;
+	char *pseudo_header;
 	u8 *dgid;
 	u8 g;
 
@@ -1076,8 +1079,11 @@ static inline void mlx5i_complete_rx_cqe(struct mlx5e_rq *rq,
 	if (likely(netdev->features & NETIF_F_RXHASH))
 		mlx5e_skb_set_hash(cqe, skb);
 
+	/* 20 bytes of ipoib header and 4 for encap existing */
+	pseudo_header = skb_push(skb, MLX5_IPOIB_PSEUDO_LEN);
+	memset(pseudo_header, 0, MLX5_IPOIB_PSEUDO_LEN);
 	skb_reset_mac_header(skb);
-	skb_pull(skb, MLX5_IPOIB_ENCAP_LEN);
+	skb_pull(skb, MLX5_IPOIB_HARD_LEN);
 
 	skb->dev = netdev;
 
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_tc.c b/drivers/net/ethernet/mellanox/mlx5/core/en_tc.c
index 11c27e4fadf6..7914a32a3036 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en_tc.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_tc.c
@@ -43,6 +43,7 @@
 #include <net/tc_act/tc_vlan.h>
 #include <net/tc_act/tc_tunnel_key.h>
 #include <net/tc_act/tc_pedit.h>
+#include <net/tc_act/tc_csum.h>
 #include <net/vxlan.h>
 #include <net/arp.h>
 #include "en.h"
@@ -384,7 +385,7 @@ static void mlx5e_detach_encap(struct mlx5e_priv *priv,
 		if (e->flags & MLX5_ENCAP_ENTRY_VALID)
 			mlx5_encap_dealloc(priv->mdev, e->encap_id);
 
-		hlist_del_rcu(&e->encap_hlist);
+		hash_del_rcu(&e->encap_hlist);
 		kfree(e->encap_header);
 		kfree(e);
 	}
@@ -925,11 +926,11 @@ 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;
+	int i, action_size, nactions, max_actions, first, last, first_z;
 	void *s_masks_p, *a_masks_p, *vals_p;
-	u32 s_mask, a_mask, val;
 	struct mlx5_fields *f;
 	u8 cmd, field_bsize;
+	u32 s_mask, a_mask;
 	unsigned long mask;
 	void *action;
 
@@ -946,7 +947,8 @@ static int offload_pedit_fields(struct pedit_headers *masks,
 	for (i = 0; i < ARRAY_SIZE(fields); i++) {
 		f = &fields[i];
 		/* avoid seeing bits set from previous iterations */
-		s_mask = a_mask = mask = val = 0;
+		s_mask = 0;
+		a_mask = 0;
 
 		s_masks_p = (void *)set_masks + f->offset;
 		a_masks_p = (void *)add_masks + f->offset;
@@ -981,12 +983,12 @@ static int offload_pedit_fields(struct pedit_headers *masks,
 			memset(a_masks_p, 0, f->size);
 		}
 
-		memcpy(&val, vals_p, f->size);
-
 		field_bsize = f->size * BITS_PER_BYTE;
+
+		first_z = find_first_zero_bit(&mask, field_bsize);
 		first = find_first_bit(&mask, field_bsize);
 		last  = find_last_bit(&mask, field_bsize);
-		if (first > 0 || last != (field_bsize - 1)) {
+		if (first > 0 || last != (field_bsize - 1) || first_z < last) {
 			printk(KERN_WARNING "mlx5: partial rewrite (mask %lx) is currently not offloaded\n",
 			       mask);
 			return -EOPNOTSUPP;
@@ -1002,11 +1004,11 @@ static int offload_pedit_fields(struct pedit_headers *masks,
 		}
 
 		if (field_bsize == 32)
-			MLX5_SET(set_action_in, action, data, ntohl(val));
+			MLX5_SET(set_action_in, action, data, ntohl(*(__be32 *)vals_p));
 		else if (field_bsize == 16)
-			MLX5_SET(set_action_in, action, data, ntohs(val));
+			MLX5_SET(set_action_in, action, data, ntohs(*(__be16 *)vals_p));
 		else if (field_bsize == 8)
-			MLX5_SET(set_action_in, action, data, val);
+			MLX5_SET(set_action_in, action, data, *(u8 *)vals_p);
 
 		action += action_size;
 		nactions++;
@@ -1109,6 +1111,28 @@ out_err:
 	return err;
 }
 
+static bool csum_offload_supported(struct mlx5e_priv *priv, u32 action, u32 update_flags)
+{
+	u32 prot_flags = TCA_CSUM_UPDATE_FLAG_IPV4HDR | TCA_CSUM_UPDATE_FLAG_TCP |
+			 TCA_CSUM_UPDATE_FLAG_UDP;
+
+	/*  The HW recalcs checksums only if re-writing headers */
+	if (!(action & MLX5_FLOW_CONTEXT_ACTION_MOD_HDR)) {
+		netdev_warn(priv->netdev,
+			    "TC csum action is only offloaded with pedit\n");
+		return false;
+	}
+
+	if (update_flags & ~prot_flags) {
+		netdev_warn(priv->netdev,
+			    "can't offload TC csum action for some header/s - flags %#x\n",
+			    update_flags);
+		return false;
+	}
+
+	return true;
+}
+
 static int parse_tc_nic_actions(struct mlx5e_priv *priv, struct tcf_exts *exts,
 				struct mlx5e_tc_flow_parse_attr *parse_attr,
 				struct mlx5e_tc_flow *flow)
@@ -1149,6 +1173,14 @@ static int parse_tc_nic_actions(struct mlx5e_priv *priv, struct tcf_exts *exts,
 			continue;
 		}
 
+		if (is_tcf_csum(a)) {
+			if (csum_offload_supported(priv, attr->action,
+						   tcf_csum_update_flags(a)))
+				continue;
+
+			return -EOPNOTSUPP;
+		}
+
 		if (is_tcf_skbedit_mark(a)) {
 			u32 mark = tcf_skbedit_mark(a);
 
@@ -1404,8 +1436,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,
@@ -1510,8 +1542,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,
@@ -1651,6 +1683,14 @@ static int parse_tc_fdb_actions(struct mlx5e_priv *priv, struct tcf_exts *exts,
 			continue;
 		}
 
+		if (is_tcf_csum(a)) {
+			if (csum_offload_supported(priv, attr->action,
+						   tcf_csum_update_flags(a)))
+				continue;
+
+			return -EOPNOTSUPP;
+		}
+
 		if (is_tcf_mirred_egress_redirect(a)) {
 			int ifindex = tcf_mirred_ifindex(a);
 			struct net_device *out_dev, *encap_dev = NULL;
@@ -1738,7 +1778,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;
@@ -1823,9 +1863,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;
@@ -1844,13 +1882,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;
 }
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/eq.c b/drivers/net/ethernet/mellanox/mlx5/core/eq.c
index ea5d8d37a75c..0ed8e90ba54f 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,8 @@ 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_FPGA_ERROR:
+		return "MLX5_EVENT_TYPE_FPGA_ERROR";
 	default:
 		return "Unrecognized event";
 	}
@@ -422,7 +425,7 @@ static irqreturn_t mlx5_eq_int(int irq, void *eq_ptr)
 			break;
 
 		case MLX5_EVENT_TYPE_CMD:
-			mlx5_cmd_comp_handler(dev, be32_to_cpu(eqe->data.cmd.vector));
+			mlx5_cmd_comp_handler(dev, be32_to_cpu(eqe->data.cmd.vector), false);
 			break;
 
 		case MLX5_EVENT_TYPE_PORT_CHANGE:
@@ -476,6 +479,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 +556,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;
@@ -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..81dfcd90b1f5 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;
 
@@ -1241,11 +1239,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 +1318,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;
 	}
 
@@ -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_offloads.c b/drivers/net/ethernet/mellanox/mlx5/core/eswitch_offloads.c
index f991f669047e..3795943ef2d1 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;
 	}
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..99cba644b4fc
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx5/core/fpga/cmd.c
@@ -0,0 +1,64 @@
+/*
+ * 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 "mlx5_core.h"
+#include "fpga/cmd.h"
+
+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_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;
+}
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..a74396a61bc3
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx5/core/fpga/cmd.h
@@ -0,0 +1,59 @@
+/*
+ * 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;
+};
+
+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);
+
+#endif /* __MLX5_FPGA_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..d88b332e9669
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx5/core/fpga/core.c
@@ -0,0 +1,202 @@
+/*
+ * 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 "fpga/core.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_start(struct mlx5_core_dev *mdev)
+{
+	struct mlx5_fpga_device *fdev = mdev->fpga;
+	unsigned long flags;
+	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));
+
+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_device_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_cleanup(struct mlx5_core_dev *mdev)
+{
+	kfree(mdev->fpga);
+	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..c55044d66778
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx5/core/fpga/core.h
@@ -0,0 +1,99 @@
+/*
+ * 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;
+};
+
+#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_device_init(struct mlx5_core_dev *mdev);
+void mlx5_fpga_device_cleanup(struct mlx5_core_dev *mdev);
+int mlx5_fpga_device_start(struct mlx5_core_dev *mdev);
+void mlx5_fpga_event(struct mlx5_core_dev *mdev, u8 event, void *data);
+
+#else
+
+static inline int mlx5_fpga_device_init(struct mlx5_core_dev *mdev)
+{
+	return 0;
+}
+
+static inline void mlx5_fpga_device_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_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/fs_cmd.c b/drivers/net/ethernet/mellanox/mlx5/core/fs_cmd.c
index 19e3d2fc2099..abb44a268563 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/fs_cmd.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/fs_cmd.c
@@ -40,28 +40,25 @@
 #include "eswitch.h"
 
 int mlx5_cmd_update_root_ft(struct mlx5_core_dev *dev,
-			    struct mlx5_flow_table *ft)
+			    struct mlx5_flow_table *ft, u32 underlay_qpn)
 {
 	u32 in[MLX5_ST_SZ_DW(set_flow_table_root_in)]   = {0};
 	u32 out[MLX5_ST_SZ_DW(set_flow_table_root_out)] = {0};
 
 	if ((MLX5_CAP_GEN(dev, port_type) == MLX5_CAP_PORT_TYPE_IB) &&
-	    ft->underlay_qpn == 0)
+	    underlay_qpn == 0)
 		return 0;
 
 	MLX5_SET(set_flow_table_root_in, in, opcode,
 		 MLX5_CMD_OP_SET_FLOW_TABLE_ROOT);
 	MLX5_SET(set_flow_table_root_in, in, table_type, ft->type);
 	MLX5_SET(set_flow_table_root_in, in, table_id, ft->id);
+	MLX5_SET(set_flow_table_root_in, in, underlay_qpn, underlay_qpn);
 	if (ft->vport) {
 		MLX5_SET(set_flow_table_root_in, in, vport_number, ft->vport);
 		MLX5_SET(set_flow_table_root_in, in, other_vport, 1);
 	}
 
-	if ((MLX5_CAP_GEN(dev, port_type) == MLX5_CAP_PORT_TYPE_IB) &&
-	    ft->underlay_qpn != 0)
-		MLX5_SET(set_flow_table_root_in, in, underlay_qpn, ft->underlay_qpn);
-
 	return mlx5_cmd_exec(dev, in, sizeof(in), out, sizeof(out));
 }
 
@@ -235,11 +232,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_cmd.h b/drivers/net/ethernet/mellanox/mlx5/core/fs_cmd.h
index 8fad80688536..0f98a7cf4877 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/fs_cmd.h
+++ b/drivers/net/ethernet/mellanox/mlx5/core/fs_cmd.h
@@ -71,7 +71,8 @@ int mlx5_cmd_delete_fte(struct mlx5_core_dev *dev,
 			unsigned int index);
 
 int mlx5_cmd_update_root_ft(struct mlx5_core_dev *dev,
-			    struct mlx5_flow_table *ft);
+			    struct mlx5_flow_table *ft,
+			    u32 underlay_qpn);
 
 int mlx5_cmd_fc_alloc(struct mlx5_core_dev *dev, u16 *id);
 int mlx5_cmd_fc_free(struct mlx5_core_dev *dev, u16 id);
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/fs_core.c b/drivers/net/ethernet/mellanox/mlx5/core/fs_core.c
index b8a176503d38..6c636c21d24f 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/fs_core.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/fs_core.c
@@ -376,11 +376,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);
@@ -650,7 +648,7 @@ static int update_root_ft_create(struct mlx5_flow_table *ft, struct fs_prio
 	if (ft->level >= min_level)
 		return 0;
 
-	err = mlx5_cmd_update_root_ft(root->dev, ft);
+	err = mlx5_cmd_update_root_ft(root->dev, ft, root->underlay_qpn);
 	if (err)
 		mlx5_core_warn(root->dev, "Update root flow table of id=%u failed\n",
 			       ft->id);
@@ -818,8 +816,6 @@ static struct mlx5_flow_table *__mlx5_create_flow_table(struct mlx5_flow_namespa
 		goto unlock_root;
 	}
 
-	ft->underlay_qpn = ft_attr->underlay_qpn;
-
 	tree_init_node(&ft->node, 1, del_flow_table);
 	log_table_sz = ft->max_fte ? ilog2(ft->max_fte) : 0;
 	next_ft = find_next_chained_ft(fs_prio);
@@ -1159,7 +1155,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);
 
@@ -1489,7 +1485,8 @@ static int update_root_ft_destroy(struct mlx5_flow_table *ft)
 
 	new_root_ft = find_next_ft(ft);
 	if (new_root_ft) {
-		int err = mlx5_cmd_update_root_ft(root->dev, new_root_ft);
+		int err = mlx5_cmd_update_root_ft(root->dev, new_root_ft,
+						  root->underlay_qpn);
 
 		if (err) {
 			mlx5_core_warn(root->dev, "Update root flow table of id=%u failed\n",
@@ -1778,7 +1775,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;
 
@@ -2062,3 +2059,21 @@ err:
 	mlx5_cleanup_fs(dev);
 	return err;
 }
+
+int mlx5_fs_add_rx_underlay_qpn(struct mlx5_core_dev *dev, u32 underlay_qpn)
+{
+	struct mlx5_flow_root_namespace *root = dev->priv.steering->root_ns;
+
+	root->underlay_qpn = underlay_qpn;
+	return 0;
+}
+EXPORT_SYMBOL(mlx5_fs_add_rx_underlay_qpn);
+
+int mlx5_fs_remove_rx_underlay_qpn(struct mlx5_core_dev *dev, u32 underlay_qpn)
+{
+	struct mlx5_flow_root_namespace *root = dev->priv.steering->root_ns;
+
+	root->underlay_qpn = 0;
+	return 0;
+}
+EXPORT_SYMBOL(mlx5_fs_remove_rx_underlay_qpn);
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/fs_core.h b/drivers/net/ethernet/mellanox/mlx5/core/fs_core.h
index 81eafc7b9dd9..990acee6fb09 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/fs_core.h
+++ b/drivers/net/ethernet/mellanox/mlx5/core/fs_core.h
@@ -118,7 +118,6 @@ struct mlx5_flow_table {
 	/* FWD rules that point on this flow table */
 	struct list_head		fwd_rules;
 	u32				flags;
-	u32				underlay_qpn;
 };
 
 struct mlx5_fc_cache {
@@ -195,6 +194,7 @@ struct mlx5_flow_root_namespace {
 	struct mlx5_flow_table		*root_ft;
 	/* Should be held when chaining flow tables */
 	struct mutex			chain_lock;
+	u32				underlay_qpn;
 };
 
 int mlx5_init_fc_stats(struct mlx5_core_dev *dev);
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/health.c b/drivers/net/ethernet/mellanox/mlx5/core/health.c
index d0515391d33b..80b23333de7a 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/health.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/health.c
@@ -90,7 +90,7 @@ static void trigger_cmd_completions(struct mlx5_core_dev *dev)
 	spin_unlock_irqrestore(&dev->cmd.alloc_lock, flags);
 
 	mlx5_core_dbg(dev, "vector 0x%llx\n", vector);
-	mlx5_cmd_comp_handler(dev, vector);
+	mlx5_cmd_comp_handler(dev, vector, true);
 	return;
 
 no_trig:
@@ -185,6 +185,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 +193,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);
+	spin_lock_irqsave(&health->wq_lock, flags);
 	if (!test_bit(MLX5_DROP_NEW_HEALTH_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,6 +270,20 @@ 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;
@@ -297,13 +312,7 @@ static void poll_health(unsigned long data)
 	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);
 	}
 }
 
@@ -333,10 +342,11 @@ 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);
+	spin_unlock_irqrestore(&health->wq_lock, flags);
 	cancel_delayed_work_sync(&health->recover_work);
 	cancel_work_sync(&health->work);
 }
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/ipoib.c b/drivers/net/ethernet/mellanox/mlx5/core/ipoib.c
index 019c230da498..22ca59145e6c 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/ipoib.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/ipoib.c
@@ -66,6 +66,10 @@ static void mlx5i_init(struct mlx5_core_dev *mdev,
 
 	mlx5e_build_nic_params(mdev, &priv->channels.params, profile->max_nch(mdev));
 
+	/* 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->hw_features    |= NETIF_F_SG;
@@ -98,7 +102,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;
 
@@ -156,6 +160,8 @@ out:
 
 static void mlx5i_destroy_underlay_qp(struct mlx5_core_dev *mdev, struct mlx5_core_qp *qp)
 {
+	mlx5_fs_remove_rx_underlay_qpn(mdev, qp->qpn);
+
 	mlx5_core_destroy_qp(mdev, qp);
 }
 
@@ -170,6 +176,8 @@ static int mlx5i_init_tx(struct mlx5e_priv *priv)
 		return err;
 	}
 
+	mlx5_fs_add_rx_underlay_qpn(priv->mdev, ipriv->qp.qpn);
+
 	err = mlx5e_create_tis(priv->mdev, 0 /* tc */, ipriv->qp.qpn, &priv->tisn[0]);
 	if (err) {
 		mlx5_core_warn(priv->mdev, "create tis failed, %d\n", err);
@@ -189,7 +197,6 @@ static void mlx5i_cleanup_tx(struct mlx5e_priv *priv)
 
 static int mlx5i_create_flow_steering(struct mlx5e_priv *priv)
 {
-	struct mlx5i_priv *ipriv = priv->ppriv;
 	int err;
 
 	priv->fs.ns = mlx5_get_flow_namespace(priv->mdev,
@@ -205,7 +212,7 @@ static int mlx5i_create_flow_steering(struct mlx5e_priv *priv)
 		priv->netdev->hw_features &= ~NETIF_F_NTUPLE;
 	}
 
-	err = mlx5e_create_ttc_table(priv, ipriv->qp.qpn);
+	err = mlx5e_create_ttc_table(priv);
 	if (err) {
 		netdev_err(priv->netdev, "Failed to create ttc table, err=%d\n",
 			   err);
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/main.c b/drivers/net/ethernet/mellanox/mlx5/core/main.c
index 0c123d571b4c..9274d93d3183 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/main.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/main.c
@@ -56,6 +56,7 @@
 #ifdef CONFIG_MLX5_CORE_EN
 #include "eswitch.h"
 #endif
+#include "fpga/core.h"
 
 MODULE_AUTHOR("Eli Cohen <eli@mellanox.com>");
 MODULE_DESCRIPTION("Mellanox Connect-IB, ConnectX-4 core driver");
@@ -612,7 +613,6 @@ static int mlx5_irq_set_affinity_hint(struct mlx5_core_dev *mdev, int i)
 	struct mlx5_priv *priv  = &mdev->priv;
 	struct msix_entry *msix = priv->msix_arr;
 	int irq                 = msix[i + MLX5_EQ_VEC_COMP_BASE].vector;
-	int err;
 
 	if (!zalloc_cpumask_var(&priv->irq_info[i].mask, GFP_KERNEL)) {
 		mlx5_core_warn(mdev, "zalloc_cpumask_var failed");
@@ -622,18 +622,11 @@ static int mlx5_irq_set_affinity_hint(struct mlx5_core_dev *mdev, int i)
 	cpumask_set_cpu(cpumask_local_spread(i, priv->numa_node),
 			priv->irq_info[i].mask);
 
-	err = irq_set_affinity_hint(irq, priv->irq_info[i].mask);
-	if (err) {
-		mlx5_core_warn(mdev, "irq_set_affinity_hint failed,irq 0x%.4x",
-			       irq);
-		goto err_clear_mask;
-	}
+	if (IS_ENABLED(CONFIG_SMP) &&
+	    irq_set_affinity_hint(irq, priv->irq_info[i].mask))
+		mlx5_core_warn(mdev, "irq_set_affinity_hint failed, irq 0x%.4x", irq);
 
 	return 0;
-
-err_clear_mask:
-	free_cpumask_var(priv->irq_info[i].mask);
-	return err;
 }
 
 static void mlx5_irq_clear_affinity_hint(struct mlx5_core_dev *mdev, int i)
@@ -1113,10 +1106,16 @@ static int mlx5_load_one(struct mlx5_core_dev *dev, struct mlx5_priv *priv,
 		goto err_disable_msix;
 	}
 
+	err = mlx5_fpga_device_init(dev);
+	if (err) {
+		dev_err(&pdev->dev, "fpga device init failed %d\n", err);
+		goto err_put_uars;
+	}
+
 	err = mlx5_start_eqs(dev);
 	if (err) {
 		dev_err(&pdev->dev, "Failed to start pages and async EQs\n");
-		goto err_put_uars;
+		goto err_fpga_init;
 	}
 
 	err = alloc_comp_eqs(dev);
@@ -1147,6 +1146,12 @@ 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_reg_dev;
+	}
+
 	if (mlx5_device_registered(dev)) {
 		mlx5_attach_device(dev);
 	} else {
@@ -1182,6 +1187,9 @@ err_affinity_hints:
 err_stop_eqs:
 	mlx5_stop_eqs(dev);
 
+err_fpga_init:
+	mlx5_fpga_device_cleanup(dev);
+
 err_put_uars:
 	mlx5_put_uars_page(dev, priv->uar);
 
@@ -1246,6 +1254,7 @@ static int mlx5_unload_one(struct mlx5_core_dev *dev, struct mlx5_priv *priv,
 	mlx5_irq_clear_affinity_hints(dev);
 	free_comp_eqs(dev);
 	mlx5_stop_eqs(dev);
+	mlx5_fpga_device_cleanup(dev);
 	mlx5_put_uars_page(dev, priv->uar);
 	mlx5_disable_msix(dev);
 	if (cleanup)
@@ -1520,6 +1529,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..cf69b42278df 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/mlx5_core.h
+++ b/drivers/net/ethernet/mellanox/mlx5/core/mlx5_core.h
@@ -39,8 +39,7 @@
 #include <linux/if_link.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))
 
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/pagealloc.c b/drivers/net/ethernet/mellanox/mlx5/core/pagealloc.c
index a57d5a81eb05..efcded7ca27a 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;
 
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..573a6b27fed8 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/qp.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/qp.c
@@ -527,7 +527,7 @@ int mlx5_core_query_out_of_buffer(struct mlx5_core_dev *dev, u16 counter_id,
 	void *out;
 	int err;
 
-	out = mlx5_vzalloc(outlen);
+	out = kvzalloc(outlen, GFP_KERNEL);
 	if (!out)
 		return -ENOMEM;
 
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..06019d00ab7b 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,
@@ -952,7 +946,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/mlxfw/Kconfig b/drivers/net/ethernet/mellanox/mlxfw/Kconfig
new file mode 100644
index 000000000000..2b21af8a2b1d
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlxfw/Kconfig
@@ -0,0 +1,7 @@
+#
+# Mellanox firmware flash library configuration
+#
+
+config MLXFW
+        tristate "mlxfw" if COMPILE_TEST
+	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..beea4ba83495
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlxfw/mlxfw.h
@@ -0,0 +1,102 @@
+/*
+ * 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;
+};
+
+int mlxfw_firmware_flash(struct mlxfw_dev *mlxfw_dev,
+			 const struct firmware *firmware);
+
+#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..7e9589061d30
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlxfw/mlxfw_mfa2.c
@@ -0,0 +1,620 @@
+/*
+ * 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);
+	if (err)
+		goto out;
+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");
+		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..b9f80c2a8ae9 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/Kconfig
+++ b/drivers/net/ethernet/mellanox/mlxsw/Kconfig
@@ -75,6 +75,7 @@ config MLXSW_SPECTRUM
 	depends on MLXSW_CORE && MLXSW_PCI && NET_SWITCHDEV && VLAN_8021Q
 	depends on PSAMPLE || PSAMPLE=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_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/reg.h b/drivers/net/ethernet/mellanox/mlxsw/reg.h
index 83b277c8090e..182150afd5ad 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/reg.h
+++ b/drivers/net/ethernet/mellanox/mlxsw/reg.h
@@ -5643,6 +5643,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.
@@ -6221,6 +6437,9 @@ static const struct mlxsw_reg_info *mlxsw_reg_infos[] = {
 	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..666bcf4854e6 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 \
+	"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,216 @@ 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 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;
+	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
+	};
+	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 = mlxfw_firmware_flash(&mlxsw_sp_mlxfw_dev.mlxfw_dev, 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 +436,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};
@@ -609,8 +870,7 @@ static int mlxsw_sp_port_swid_set(struct mlxsw_sp_port *mlxsw_sp_port, u8 swid)
 					swid);
 }
 
-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 +879,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 +889,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)
 {
-	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 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)
+{
+	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
@@ -1100,95 +1385,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;
-	}
-
-	return 0;
+	struct mlxsw_sp_port_vlan *mlxsw_sp_port_vlan, *tmp;
 
-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 +1468,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 +1483,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;
 }
@@ -2398,24 +2624,12 @@ 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)
 {
+	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;
 
 	dev = alloc_etherdev(sizeof(struct mlxsw_sp_port));
@@ -2426,23 +2640,13 @@ static int __mlxsw_sp_port_create(struct mlxsw_sp *mlxsw_sp, u8 local_port,
 	mlxsw_sp_port->dev = dev;
 	mlxsw_sp_port->mlxsw_sp = mlxsw_sp;
 	mlxsw_sp_port->local_port = local_port;
+	mlxsw_sp_port->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 =
@@ -2547,11 +2751,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_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_pvid_vport_create;
+		goto err_port_vlan_get;
 	}
 
 	mlxsw_sp_port_switchdev_init(mlxsw_sp_port);
@@ -2572,8 +2783,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:
@@ -2591,10 +2804,6 @@ err_alloc_hw_stats:
 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;
 }
@@ -2630,16 +2839,15 @@ 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);
 	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);
 }
 
@@ -3192,57 +3400,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 +3447,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 +3455,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 +3468,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 +3528,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 +3537,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 +3553,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 +3564,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 +3573,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 +3589,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 +3649,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;
 
@@ -3562,176 +3701,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 +3819,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 +3856,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 +3886,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 +3897,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 +3941,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 +3970,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 +3984,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 +3994,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 +4021,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 +4037,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 +4058,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 +4089,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 +4115,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 +4125,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 +4162,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 +4173,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 +4191,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 +4221,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 +4296,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..1a834109bda1 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,10 +230,12 @@ 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_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);
 
@@ -385,100 +257,25 @@ 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)
-{
-	u16 vid = mlxsw_sp_vport_vid_get(mlxsw_sp_port);
-
-	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;
-	}
-
-	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)
+static inline struct mlxsw_sp_port_vlan *
+mlxsw_sp_port_vlan_find_by_vid(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) {
-		struct mlxsw_sp_fid *f = mlxsw_sp_vport_fid_get(mlxsw_sp_vport);
+	struct mlxsw_sp_port_vlan *mlxsw_sp_port_vlan;
 
-		if (f && f->fid == fid)
-			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_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,
 };
 
 int mlxsw_sp_buffers_init(struct mlxsw_sp *mlxsw_sp);
@@ -515,26 +312,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);
 
 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);
+
 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,9 +343,15 @@ 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);
 
 #ifdef CONFIG_MLXSW_SPECTRUM_DCB
 
@@ -574,10 +377,11 @@ 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);
 
 int mlxsw_sp_kvdl_alloc(struct mlxsw_sp *mlxsw_sp, unsigned int entry_count,
 			u32 *p_entry_index);
@@ -683,6 +487,8 @@ 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);
 
@@ -702,4 +508,31 @@ int mlxsw_sp_flow_counter_alloc(struct mlxsw_sp *mlxsw_sp,
 void mlxsw_sp_flow_counter_free(struct mlxsw_sp *mlxsw_sp,
 				unsigned int counter_index);
 
+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..1da889a044df 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)
@@ -676,6 +682,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 +713,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 +735,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 +755,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 ea56f6ade6b4..af2c65a3fd9f 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/spectrum_dpipe.c
+++ b/drivers/net/ethernet/mellanox/mlxsw/spectrum_dpipe.c
@@ -199,10 +199,11 @@ static int mlxsw_sp_erif_entry_get(struct mlxsw_sp *mlxsw_sp,
 
 	entry->counter_valid = false;
 	entry->counter = 0;
+	entry->index = mlxsw_sp_rif_index(rif);
+
 	if (!counters_enabled)
 		return 0;
 
-	entry->index = mlxsw_sp_rif_index(rif);
 	err = mlxsw_sp_rif_counter_value_get(mlxsw_sp, rif,
 					     MLXSW_SP_RIF_COUNTER_EGRESS,
 					     &cnt);
@@ -217,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};
@@ -226,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);
@@ -241,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;
@@ -281,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..c7590aea1aee
--- /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_OFFEST,
+		.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_OFFEST,
+		.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_OFFEST,
+		.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..13af8e358847 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/spectrum_flower.c
+++ b/drivers/net/ethernet/mellanox/mlxsw/spectrum_flower.c
@@ -70,9 +70,13 @@ static int mlxsw_sp_flower_parse_actions(struct mlxsw_sp *mlxsw_sp,
 		} 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 +182,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 +224,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 +316,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 +397,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 +417,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 33cec1cc1642..a4272c351e3a 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)
@@ -206,6 +268,9 @@ void mlxsw_sp_rif_counter_free(struct mlxsw_sp *mlxsw_sp,
 {
 	unsigned int *p_counter_index;
 
+	if (!mlxsw_sp_rif_counter_valid_get(rif, dir))
+		return;
+
 	p_counter_index = mlxsw_sp_rif_p_counter_get(rif, dir);
 	if (WARN_ON(!p_counter_index))
 		return;
@@ -216,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)
 
@@ -284,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;
@@ -310,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;
@@ -320,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,
@@ -358,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;
 	}
@@ -455,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,
@@ -493,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;
 	}
 
@@ -510,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)
@@ -524,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;
 	}
@@ -570,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;
 	}
@@ -677,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;
 	}
 
@@ -703,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 {
@@ -755,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);
 }
@@ -764,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);
 }
@@ -812,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);
 }
 
@@ -821,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,
@@ -836,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",
@@ -951,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.
@@ -963,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
@@ -999,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);
 }
 
@@ -1127,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;
@@ -1168,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;
@@ -1179,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,
@@ -1267,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);
 }
@@ -1275,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);
 }
@@ -1284,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);
 }
 
@@ -1297,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);
 }
 
@@ -1312,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);
 }
 
@@ -1599,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);
@@ -1697,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;
@@ -2510,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);
@@ -2550,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);
@@ -2581,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];
 
@@ -2663,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;
@@ -2675,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");
@@ -2745,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;
@@ -2757,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) {
@@ -2795,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;
 }
@@ -2846,77 +2960,44 @@ static bool mlxsw_sp_rif_should_config(struct mlxsw_sp_rif *rif,
 	return false;
 }
 
+static enum mlxsw_sp_rif_type
+mlxsw_sp_dev_rif_type(const struct mlxsw_sp *mlxsw_sp,
+		      const struct net_device *dev)
+{
+	enum mlxsw_sp_fid_type type;
+
+	/* 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;
+
+	return mlxsw_sp_fid_type_rif_type(mlxsw_sp, type);
+}
+
 #define MLXSW_SP_INVALID_INDEX_RIF 0xffff
 static int mlxsw_sp_avail_rif_get(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])
+		if (!mlxsw_sp->router->rifs[i])
 			return i;
 
 	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);
-
-static u16 mlxsw_sp_rif_sp_to_fid(u16 rif_index)
-{
-	return MLXSW_SP_RFID_BASE + rif_index;
-}
-
-static struct mlxsw_sp_fid *
-mlxsw_sp_rfid_alloc(u16 fid, struct net_device *l3_dev)
-{
-	struct mlxsw_sp_fid *f;
-
-	f = kzalloc(sizeof(*f), GFP_KERNEL);
-	if (!f)
-		return NULL;
-
-	f->leave = mlxsw_sp_vport_rif_sp_leave;
-	f->ref_count = 0;
-	f->dev = l3_dev;
-	f->fid = fid;
-
-	return f;
-}
-
-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;
 
@@ -2927,11 +3008,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;
@@ -2943,152 +3029,201 @@ 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);
-	if (err)
-		goto err_vport_rif_sp_op;
-
-	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_index = mlxsw_sp_avail_rif_get(mlxsw_sp);
+	if (rif_index == MLXSW_SP_INVALID_INDEX_RIF) {
+		err = -ERANGE;
+		goto err_avail_rif_get;
 	}
 
-	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);
 
-	f->rif = rif;
-	mlxsw_sp->rifs[rif_index] = rif;
+	err = ops->configure(rif);
+	if (err)
+		goto err_configure;
+
+	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_avail_rif_get:
 	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;
 
-	netdev_dbg(mlxsw_sp_vport->dev, "Joined FID=%d\n", rif->f->fid);
+	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;
+
+	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;
 	}
 
@@ -3103,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,
@@ -3116,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;
 		}
@@ -3135,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;
 	}
 
@@ -3328,19 +3299,16 @@ 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 (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;
 }
@@ -3353,7 +3321,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
@@ -3403,6 +3371,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);
@@ -3412,8 +3381,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;
 
@@ -3422,7 +3392,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;
 
@@ -3436,7 +3406,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;
 }
 
@@ -3489,16 +3459,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)
@@ -3509,55 +3688,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;
@@ -3574,8 +3748,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;
@@ -3589,21 +3763,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 0d8411f1f954..edcc273d7597 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/spectrum_switchdev.c
+++ b/drivers/net/ethernet/mellanox/mlxsw/spectrum_switchdev.c
@@ -52,340 +52,596 @@
 #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;
+
+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;
+};
+
+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;
+	};
+};
 
-	fid = f ? f->fid : fid;
+struct mlxsw_sp_bridge_vlan {
+	struct list_head list;
+	struct list_head port_vlan_list;
+	u16 vid;
+	u8 egress_untagged:1,
+	   pvid:1;
+};
+
+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;
 
-	if (!fid)
-		fid = mlxsw_sp_port->pvid;
+	list_for_each_entry(bridge_device, &bridge->bridges_list, list)
+		if (bridge_device->dev == br_dev)
+			return bridge_device;
 
-	return fid;
+	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;
+	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 __mlxsw_sp_port_vid_learning_set(mlxsw_sp_port, vid, vid,
-							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;
+}
+
+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 +653,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 +682,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;
 
-	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);
+	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;
+
+	/* 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 +762,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 +786,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 +802,245 @@ 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;
-
-	list_del(&f->list);
-
-	if (f->rif)
-		mlxsw_sp_rif_bridge_destroy(mlxsw_sp, f->rif);
-
-	kfree(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;
+	u8 local_port = mlxsw_sp_port->local_port;
+	u16 vid = mlxsw_sp_port_vlan->vid;
 
-	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;
-
-	return mlxsw_sp_port_vid_to_fid_set(mlxsw_sp_port, mt, valid, fid, fid);
-}
+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;
 
-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;
+	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);
 
-	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);
+	bridge_vlan->egress_untagged = is_untagged;
+	bridge_vlan->pvid = is_pvid;
 
 	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 +1048,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)
@@ -939,24 +1146,39 @@ mlxsw_sp_port_fdb_static_add(struct mlxsw_sp_port *mlxsw_sp_port,
 			     const struct switchdev_obj_port_fdb *fdb,
 			     struct switchdev_trans *trans)
 {
-	u16 fid = mlxsw_sp_port_vid_to_fid_get(mlxsw_sp_port, fdb->vid);
-	u16 lag_vid = 0;
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
+	struct net_device *orig_dev = fdb->obj.orig_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;
 
 	if (switchdev_trans_ph_prepare(trans))
 		return 0;
 
-	if (mlxsw_sp_port_is_vport(mlxsw_sp_port)) {
-		lag_vid = mlxsw_sp_vport_vid_get(mlxsw_sp_port);
-	}
+	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,
+							       fdb->vid);
+	if (!mlxsw_sp_port_vlan)
+		return 0;
+
+	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,
+		return mlxsw_sp_port_fdb_uc_op(mlxsw_sp,
 					       mlxsw_sp_port->local_port,
-					       fdb->addr, fid, true, false);
+					       fdb->addr, fid_index, true,
+					       false);
 	else
-		return mlxsw_sp_port_fdb_uc_lag_op(mlxsw_sp_port->mlxsw_sp,
+		return mlxsw_sp_port_fdb_uc_lag_op(mlxsw_sp,
 						   mlxsw_sp_port->lag_id,
-						   fdb->addr, fid, lag_vid,
+						   fdb->addr, fid_index, vid,
 						   true, false);
 }
 
@@ -1006,7 +1228,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 +1242,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 +1251,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 +1266,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 +1278,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 +1321,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,15 +1343,8 @@ 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);
@@ -1135,68 +1367,78 @@ 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);
+	u16 pvid = mlxsw_sp_port->pvid == vid ? 0 : vid;
+	struct mlxsw_sp_port_vlan *mlxsw_sp_port_vlan;
 
-	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);
-
-	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;
+
+	if (!bridge_port->bridge_device->vlan_enabled)
+		return 0;
+
+	for (vid = vlan->vid_begin; vid <= vlan->vid_end; vid++)
+		mlxsw_sp_bridge_port_vlan_del(mlxsw_sp_port, bridge_port, vid);
+
+	return 0;
 }
 
 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;
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
+	struct net_device *orig_dev = fdb->obj.orig_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 (WARN_ON(!bridge_port))
+		return -EINVAL;
 
-	if (mlxsw_sp_port_is_vport(mlxsw_sp_port)) {
-		lag_vid = mlxsw_sp_vport_vid_get(mlxsw_sp_port);
-	}
+	bridge_device = bridge_port->bridge_device;
+	mlxsw_sp_port_vlan = mlxsw_sp_port_vlan_find_by_bridge(mlxsw_sp_port,
+							       bridge_device,
+							       fdb->vid);
+	if (!mlxsw_sp_port_vlan)
+		return 0;
+
+	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,
+		return mlxsw_sp_port_fdb_uc_op(mlxsw_sp,
 					       mlxsw_sp_port->local_port,
-					       fdb->addr, fid,
-					       false, false);
+					       fdb->addr, fid_index, false,
+					       false);
 	else
-		return mlxsw_sp_port_fdb_uc_lag_op(mlxsw_sp_port->mlxsw_sp,
+		return mlxsw_sp_port_fdb_uc_lag_op(mlxsw_sp,
 						   mlxsw_sp_port->lag_id,
-						   fdb->addr, fid, lag_vid,
+						   fdb->addr, fid_index, vid,
 						   false, false);
 }
 
@@ -1204,13 +1446,30 @@ 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 +1481,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,15 +1496,8 @@ 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;
@@ -1284,32 +1536,32 @@ static struct mlxsw_sp_port *mlxsw_sp_lag_rep_port(struct mlxsw_sp *mlxsw_sp,
 
 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)
+				  switchdev_obj_dump_cb_t *cb)
 {
 	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;
+	struct net_device *orig_dev = fdb->obj.orig_dev;
+	struct mlxsw_sp_bridge_port *bridge_port;
+	u16 lag_id, fid_index;
 	char mac[ETH_ALEN];
-	u16 fid;
-	u8 local_port;
-	u16 lag_id;
-	u8 num_rec;
 	int stored_err = 0;
-	int i;
+	char *sfd_pl;
+	u8 num_rec;
 	int err;
 
+	bridge_port = mlxsw_sp_bridge_port_find(mlxsw_sp->bridge, orig_dev);
+	if (!bridge_port)
+		return 0;
+
 	sfd_pl = kmalloc(MLXSW_REG_SFD_LEN, GFP_KERNEL);
 	if (!sfd_pl)
 		return -ENOMEM;
 
-	f = mlxsw_sp_vport_fid_get(mlxsw_sp_port);
-	vport_fid = f ? f->fid : 0;
-
 	mlxsw_reg_sfd_pack(sfd_pl, MLXSW_REG_SFD_OP_QUERY_DUMP, 0);
 	do {
+		struct mlxsw_sp_port *tmp;
+		u8 local_port;
+		int i;
+
 		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)
@@ -1326,48 +1578,44 @@ static int mlxsw_sp_port_fdb_dump(struct mlxsw_sp_port *mlxsw_sp_port,
 		for (i = 0; i < num_rec; i++) {
 			switch (mlxsw_reg_sfd_rec_type_get(sfd_pl, i)) {
 			case MLXSW_REG_SFD_REC_TYPE_UNICAST:
-				mlxsw_reg_sfd_uc_unpack(sfd_pl, i, mac, &fid,
+				mlxsw_reg_sfd_uc_unpack(sfd_pl, i, mac,
+							&fid_index,
 							&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;
-				}
+				if (bridge_port->lagged)
+					continue;
+				if (bridge_port->system_port != local_port)
+					continue;
+				if (bridge_port->bridge_device->vlan_enabled)
+					fdb->vid = fid_index;
+				else
+					fdb->vid = 0;
+				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);
+							    mac, &fid_index,
+							    &lag_id);
+				if (!bridge_port->lagged)
+					continue;
+				if (bridge_port->lag_id != lag_id)
+					continue;
 				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;
-				}
+				if (tmp->local_port !=
+				    mlxsw_sp_port->local_port)
+					continue;
+				if (bridge_port->bridge_device->vlan_enabled)
+					fdb->vid = fid_index;
+				else
+					fdb->vid = 0;
+				ether_addr_copy(fdb->addr, mac);
+				fdb->ndm_state = NUD_REACHABLE;
+				err = cb(&fdb->obj);
+				if (err)
+					stored_err = err;
 				break;
 			}
 		}
@@ -1382,28 +1630,32 @@ 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)
 {
-	u16 vid;
+	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;
+	struct mlxsw_sp_bridge_vlan *bridge_vlan;
 	int err = 0;
 
-	if (mlxsw_sp_port_is_vport(mlxsw_sp_port)) {
-		vlan->flags = 0;
-		vlan->vid_begin = mlxsw_sp_vport_vid_get(mlxsw_sp_port);
-		vlan->vid_end = mlxsw_sp_vport_vid_get(mlxsw_sp_port);
-		return cb(&vlan->obj);
-	}
+	bridge_port = mlxsw_sp_bridge_port_find(mlxsw_sp->bridge, orig_dev);
+	if (WARN_ON(!bridge_port))
+		return -EINVAL;
+
+	if (!bridge_port->bridge_device->vlan_enabled)
+		return 0;
 
-	for_each_set_bit(vid, mlxsw_sp_port->active_vlans, VLAN_N_VID) {
+	list_for_each_entry(bridge_vlan, &bridge_port->vlans_list, list) {
 		vlan->flags = 0;
-		if (vid == mlxsw_sp_port->pvid)
+		if (bridge_vlan->pvid)
 			vlan->flags |= BRIDGE_VLAN_INFO_PVID;
-		if (test_bit(vid, mlxsw_sp_port->untagged_vlans))
+		if (bridge_vlan->egress_untagged)
 			vlan->flags |= BRIDGE_VLAN_INFO_UNTAGGED;
-		vlan->vid_begin = vid;
-		vlan->vid_end = vid;
+		vlan->vid_begin = bridge_vlan->vid;
+		vlan->vid_end = bridge_vlan->vid;
 		err = cb(&vlan->obj);
 		if (err)
 			break;
 	}
+
 	return err;
 }
 
@@ -1414,10 +1666,6 @@ static int mlxsw_sp_port_obj_dump(struct net_device *dev,
 	struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
 	int err = 0;
 
-	mlxsw_sp_port = mlxsw_sp_port_orig_get(obj->orig_dev, mlxsw_sp_port);
-	if (!mlxsw_sp_port)
-		return -EINVAL;
-
 	switch (obj->id) {
 	case SWITCHDEV_OBJ_ID_PORT_VLAN:
 		err = mlxsw_sp_port_vlan_dump(mlxsw_sp_port,
@@ -1425,8 +1673,7 @@ static int mlxsw_sp_port_obj_dump(struct net_device *dev,
 		break;
 	case SWITCHDEV_OBJ_ID_PORT_FDB:
 		err = mlxsw_sp_port_fdb_dump(mlxsw_sp_port,
-					     SWITCHDEV_OBJ_PORT_FDB(obj), cb,
-					     obj->orig_dev);
+					     SWITCHDEV_OBJ_PORT_FDB(obj), cb);
 		break;
 	default:
 		err = -EOPNOTSUPP;
@@ -1444,6 +1691,172 @@ static const struct switchdev_ops mlxsw_sp_port_switchdev_ops = {
 	.switchdev_port_obj_dump	= mlxsw_sp_port_obj_dump,
 };
 
+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_port_vlan *mlxsw_sp_port_vlan;
+
+	if (is_vlan_dev(bridge_port->dev))
+		return -EINVAL;
+
+	mlxsw_sp_port_vlan = mlxsw_sp_port_vlan_find_by_vid(mlxsw_sp_port, 1);
+	if (WARN_ON(!mlxsw_sp_port_vlan))
+		return -EINVAL;
+
+	/* Let VLAN-aware bridge take care of its own VLANs */
+	mlxsw_sp_port_vlan_put(mlxsw_sp_port_vlan);
+
+	return 0;
+}
+
+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);
+}
+
+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);
+
+	return mlxsw_sp_fid_8021q_get(mlxsw_sp, vid);
+}
+
+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)
+{
+	struct mlxsw_sp_port_vlan *mlxsw_sp_port_vlan;
+
+	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;
+	}
+
+	return false;
+}
+
+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_vlan *mlxsw_sp_port_vlan;
+	u16 vid;
+
+	if (!is_vlan_dev(bridge_port->dev))
+		return -EINVAL;
+	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 -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;
+	}
+
+	/* 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 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,
+};
+
+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(bool learning_sync, bool adding,
 					char *mac, u16 vid,
 					struct net_device *dev)
@@ -1463,6 +1876,9 @@ 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;
 	char mac[ETH_ALEN];
 	u8 local_port;
@@ -1477,35 +1893,33 @@ 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);
 	if (err) {
-		if (net_ratelimit())
-			netdev_err(mlxsw_sp_port->dev, "Failed to set FDB entry\n");
+		dev_err_ratelimited(mlxsw_sp->bus_info->dev, "Failed to set FDB entry\n");
 		return;
 	}
 
 	if (!do_notification)
 		return;
-	mlxsw_sp_fdb_call_notifiers(mlxsw_sp_port->learning_sync,
-				    adding, mac, vid, mlxsw_sp_port->dev);
+	mlxsw_sp_fdb_call_notifiers(bridge_port->flags & BR_LEARNING_SYNC,
+				    adding, mac, vid, bridge_port->dev);
 	return;
 
 just_remove:
@@ -1518,8 +1932,10 @@ 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;
 	char mac[ETH_ALEN];
 	u16 lag_vid = 0;
 	u16 lag_id;
@@ -1534,39 +1950,34 @@ 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);
 	if (err) {
-		if (net_ratelimit())
-			netdev_err(mlxsw_sp_port->dev, "Failed to set FDB entry\n");
+		dev_err_ratelimited(mlxsw_sp->bus_info->dev, "Failed to set FDB entry\n");
 		return;
 	}
 
 	if (!do_notification)
 		return;
-	mlxsw_sp_fdb_call_notifiers(mlxsw_sp_port->learning_sync, adding, mac,
-				    vid, dev);
+	mlxsw_sp_fdb_call_notifiers(bridge_port->flags & BR_LEARNING_SYNC,
+				    adding, mac, vid, bridge_port->dev);
 	return;
 
 just_remove:
@@ -1600,12 +2011,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;
@@ -1616,7 +2030,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);
@@ -1637,6 +2052,7 @@ out:
 
 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);
@@ -1644,25 +2060,42 @@ 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;
+	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);
 }
 
 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/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..0d5a7b9203a4 100644
--- a/drivers/net/ethernet/netronome/Kconfig
+++ b/drivers/net/ethernet/netronome/Kconfig
@@ -19,6 +19,7 @@ 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
diff --git a/drivers/net/ethernet/netronome/nfp/Makefile b/drivers/net/ethernet/netronome/nfp/Makefile
index 4b15f0f496aa..5ad9a557f06a 100644
--- a/drivers/net/ethernet/netronome/nfp/Makefile
+++ b/drivers/net/ethernet/netronome/nfp/Makefile
@@ -14,17 +14,24 @@ 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_netvf_main.o \
+	    nfp_port.o \
+	    bpf/main.o \
+	    bpf/offload.o \
+	    nic/main.o
 
 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/nfp_app.c b/drivers/net/ethernet/netronome/nfp/nfp_app.c
new file mode 100644
index 000000000000..cea2090cf063
--- /dev/null
+++ b/drivers/net/ethernet/netronome/nfp/nfp_app.c
@@ -0,0 +1,76 @@
+/*
+ * 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/slab.h>
+
+#include "nfpcore/nfp_cpp.h"
+#include "nfp_app.h"
+#include "nfp_main.h"
+
+static const struct nfp_app_type *apps[] = {
+	&app_nic,
+	&app_bpf,
+};
+
+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..13efdefffa1a
--- /dev/null
+++ b/drivers/net/ethernet/netronome/nfp/nfp_app.h
@@ -0,0 +1,174 @@
+/*
+ * 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
+
+struct bpf_prog;
+struct net_device;
+struct pci_dev;
+struct tc_to_netdev;
+struct nfp_app;
+struct nfp_cpp;
+struct nfp_pf;
+struct nfp_net;
+
+enum nfp_app_id {
+	NFP_APP_CORE_NIC	= 0x1,
+	NFP_APP_BPF_NIC		= 0x2,
+};
+
+extern const struct nfp_app_type app_nic;
+extern const struct nfp_app_type app_bpf;
+
+/**
+ * struct nfp_app_type - application definition
+ * @id:		application ID
+ * @name:	application name
+ *
+ * Callbacks
+ * @init:	perform basic app checks
+ * @extra_cap:	extra capabilities string
+ * @vnic_init:	init vNICs (assign port types, etc.)
+ * @vnic_clean:	clean up app's vNIC state
+ * @setup_tc:	setup TC ndo
+ * @tc_busy:	TC HW offload busy (rules loaded)
+ * @xdp_offload:    offload an XDP program
+ */
+struct nfp_app_type {
+	enum nfp_app_id id;
+	const char *name;
+
+	int (*init)(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 (*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);
+};
+
+/**
+ * struct nfp_app - NFP application container
+ * @pdev:	backpointer to PCI device
+ * @pf:		backpointer to NFP PF structure
+ * @cpp:	pointer to the CPP handle
+ * @type:	pointer to const application ops and info
+ */
+struct nfp_app {
+	struct pci_dev *pdev;
+	struct nfp_pf *pf;
+	struct nfp_cpp *cpp;
+
+	const struct nfp_app_type *type;
+};
+
+static inline int nfp_app_init(struct nfp_app *app)
+{
+	if (!app->type->init)
+		return 0;
+	return app->type->init(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 const char *nfp_app_name(struct nfp_app *app)
+{
+	if (!app)
+		return "";
+	return app->type->name;
+}
+
+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);
+}
+
+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..1a33ad9f4170
--- /dev/null
+++ b/drivers/net/ethernet/netronome/nfp/nfp_app_nic.c
@@ -0,0 +1,86 @@
+/*
+ * 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)
+{
+	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);
+
+	nn->port->eth_id = id;
+	nn->port->eth_port = nfp_net_find_port(pf->eth_tbl, id);
+
+	/* Check if vNIC has external port associated and cfg is OK */
+	if (!nn->port->eth_port) {
+		nfp_err(app->cpp,
+			"NSP port entries don't match vNICs (no entry for port #%d)\n",
+			id);
+		nfp_port_free(nn->port);
+		return -EINVAL;
+	}
+	if (nn->port->eth_port->override_changed) {
+		nfp_warn(app->cpp,
+			 "Config changed for port #%d, reboot required before port will be operational\n",
+			 id);
+		nn->port->type = NFP_PORT_INVALID;
+		return 1;
+	}
+
+	return 0;
+}
+
+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(nn, app->cpp, id);
+
+	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..2609a0f28e81
--- /dev/null
+++ b/drivers/net/ethernet/netronome/nfp/nfp_devlink.c
@@ -0,0 +1,181 @@
+/*
+ * 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;
+}
+
+const struct devlink_ops nfp_devlink_ops = {
+	.port_split		= nfp_devlink_port_split,
+	.port_unsplit		= nfp_devlink_port_unsplit,
+};
+
+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..0c2e64d217b5 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"
@@ -71,20 +73,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);
 	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)
@@ -253,7 +257,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 +273,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 +317,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,12 +338,15 @@ 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;
 
@@ -359,27 +365,47 @@ static int nfp_pci_probe(struct pci_dev *pdev,
 		 nfp_hwinfo_lookup(pf->cpp, "assembly.revision"),
 		 nfp_hwinfo_lookup(pf->cpp, "cpld.version"));
 
-	err = nfp_nsp_init(pdev, pf);
+	err = devlink_register(devlink, &pdev->dev);
 	if (err)
 		goto err_cpp_free;
 
-	nfp_pcie_sriov_read_nfd_limit(pf);
+	err = nfp_nsp_init(pdev, pf);
+	if (err)
+		goto err_devlink_unreg;
 
-	err = nfp_net_pci_probe(pf);
+	err = nfp_pcie_sriov_read_nfd_limit(pf);
 	if (err)
 		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:
 	if (pf->fw_loaded)
 		nfp_fw_unload(pf);
 	kfree(pf->eth_tbl);
+	kfree(pf->nspi);
+err_devlink_unreg:
+	devlink_unregister(devlink);
 err_cpp_free:
 	nfp_cpp_free(pf->cpp);
 err_disable_msix:
 	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,10 +417,18 @@ 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);
 
 	if (pf->fw_loaded)
 		nfp_fw_unload(pf);
@@ -403,7 +437,9 @@ static void nfp_pci_remove(struct pci_dev *pdev)
 	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..c46d00bbf19d 100644
--- a/drivers/net/ethernet/netronome/nfp/nfp_main.h
+++ b/drivers/net/ethernet/netronome/nfp/nfp_main.h
@@ -47,37 +47,47 @@
 #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_net;
+struct nfp_nsp_identify;
 
 /**
  * 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
+ * @app:		Pointer to the APP handle
+ * @data_vnic_bar:	Pointer to the CPP area for the data vNICs' BARs
  * @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
+ * @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?
  * @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)
  * @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_app *app;
+
+	struct nfp_cpp_area *data_vnic_bar;
 	struct nfp_cpp_area *tx_area;
 	struct nfp_cpp_area *rx_area;
 
@@ -89,20 +99,34 @@ struct nfp_pf {
 	bool fw_loaded;
 
 	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 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);
+
+struct nfp_eth_table_port *
+nfp_net_find_port(struct nfp_eth_table *eth_tbl, unsigned int id);
+void
+nfp_net_get_mac_addr(struct nfp_net *nn, struct nfp_cpp *cpp, unsigned int id);
+
 #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..2e526338f678 100644
--- a/drivers/net/ethernet/netronome/nfp/nfp_net.h
+++ b/drivers/net/ethernet/netronome/nfp/nfp_net.h
@@ -84,7 +84,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 +102,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 +116,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 +158,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 +297,11 @@ 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;
+	__wsum csum;
 };
 
 struct nfp_net_rx_hash {
@@ -316,8 +328,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 +341,6 @@ struct nfp_net_rx_ring {
 	u32 rd_p;
 
 	u32 idx;
-	u32 wr_ptr_add;
 
 	int fl_qcidx;
 	u8 __iomem *qcp_fl;
@@ -508,11 +517,6 @@ 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)
  * @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 +535,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 +547,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,11 +566,6 @@ 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;
-
 	unsigned int max_tx_rings;
 	unsigned int max_rx_rings;
 
@@ -589,7 +588,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 +612,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_eth_table_port *eth_port;
+	struct nfp_port *port;
+
+	void *app_priv;
 };
 
 /* Functions to read/write from/to a BAR
@@ -791,16 +791,25 @@ static inline u32 nfp_qcp_wr_ptr_read(u8 __iomem *q)
 /* 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,
+	      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);
+
 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 +830,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 +851,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 +860,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..c3235d03b8eb 100644
--- a/drivers/net/ethernet/netronome/nfp/nfp_net_common.c
+++ b/drivers/net/ethernet/netronome/nfp/nfp_net_common.c
@@ -64,12 +64,13 @@
 
 #include <linux/ktime.h>
 
-#include <net/pkt_cls.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
@@ -391,19 +392,6 @@ 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)
-{
-	unsigned long flags;
-	bool ret;
-
-	spin_lock_irqsave(&nn->link_status_lock, flags);
-	ret = nn->link_changed;
-	nn->link_changed = false;
-	spin_unlock_irqrestore(&nn->link_status_lock, flags);
-
-	return ret;
-}
-
 /**
  * nfp_net_read_link_status() - Reread link status from control BAR
  * @nn:       NFP Network structure
@@ -423,7 +411,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);
@@ -516,11 +505,10 @@ nfp_net_rx_ring_init(struct nfp_net_rx_ring *rx_ring,
 
 /**
  * nfp_net_vecs_init() - Assign IRQs and setup rvecs.
- * @netdev:   netdev structure
+ * @nn:		NFP Network structure
  */
-static void nfp_net_vecs_init(struct net_device *netdev)
+static void nfp_net_vecs_init(struct nfp_net *nn)
 {
-	struct nfp_net *nn = netdev_priv(netdev);
 	struct nfp_net_r_vector *r_vec;
 	int r;
 
@@ -667,17 +655,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;
 
@@ -804,7 +797,7 @@ static int nfp_net_tx(struct sk_buff *skb, struct net_device *netdev)
 	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];
@@ -823,12 +816,11 @@ static int nfp_net_tx(struct sk_buff *skb, struct net_device *netdev)
 
 	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 +840,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;
@@ -936,14 +928,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)
@@ -997,45 +985,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 +1044,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;
@@ -1209,7 +1197,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 +1212,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 +1233,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 +1243,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 +1335,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 +1441,12 @@ 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_CSUM:
+			meta->csum_type = CHECKSUM_COMPLETE;
+			meta->csum =
+				(__force __wsum)__get_unaligned_cpu32(data);
+			data += 4;
+			break;
 		default:
 			return NULL;
 		}
@@ -1479,18 +1481,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 +1525,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 +1569,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;
@@ -1577,7 +1588,7 @@ static int nfp_net_rx(struct nfp_net_rx_ring *rx_ring, int budget)
 		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))
@@ -1669,14 +1680,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);
@@ -1708,7 +1722,7 @@ static int nfp_net_rx(struct nfp_net_rx_ring *rx_ring, int budget)
 		skb_record_rx_queue(skb, rx_ring->idx);
 		skb->protocol = eth_type_trans(skb, dp->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 +1731,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 +1759,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))
@@ -2105,17 +2122,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 +2213,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,7 +2236,7 @@ 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);
+	nfp_net_write_mac_addr(nn, nn->dp.netdev->dev_addr);
 
 	nn_writel(nn, NFP_NET_CFG_MTU, nn->dp.netdev->mtu);
 
@@ -2667,33 +2681,13 @@ 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;
+	return nfp_app_setup_tc(nn->app, netdev, handle, proto, tc);
 }
 
 static int nfp_net_set_features(struct net_device *netdev,
@@ -2710,9 +2704,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 +2718,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) {
@@ -2750,7 +2745,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 +2813,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,34 +2894,6 @@ 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)
 {
 	struct bpf_prog *old_prog = nn->dp.xdp_prog;
@@ -2959,7 +2906,7 @@ static int nfp_net_xdp_setup(struct nfp_net *nn, struct netdev_xdp *xdp)
 	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);
+		nfp_app_xdp_offload(nn->app, nn, nn->dp.xdp_prog);
 		return 0;
 	}
 
@@ -2980,7 +2927,7 @@ static int nfp_net_xdp_setup(struct nfp_net *nn, struct netdev_xdp *xdp)
 	if (old_prog)
 		bpf_prog_put(old_prog);
 
-	nfp_net_xdp_offload(nn, nn->dp.xdp_prog);
+	nfp_app_xdp_offload(nn->app, nn, nn->dp.xdp_prog);
 
 	return 0;
 }
@@ -3000,7 +2947,28 @@ static int nfp_net_xdp(struct net_device *netdev, struct netdev_xdp *xdp)
 	}
 }
 
-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,
@@ -3009,10 +2977,10 @@ static const struct net_device_ops nfp_net_netdev_ops = {
 	.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 +3000,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\n",
 		nn->cap,
 		nn->cap & NFP_NET_CFG_CTRL_PROMISC  ? "PROMISC "  : "",
 		nn->cap & NFP_NET_CFG_CTRL_L2BC     ? "L2BCFILT " : "",
@@ -3043,18 +3011,23 @@ 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_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
  * @max_tx_rings: Maximum number of TX rings supported by device
  * @max_rx_rings: Maximum number of RX rings supported by device
@@ -3064,9 +3037,9 @@ void nfp_net_info(struct nfp_net *nn)
  *
  * 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,
+			      unsigned int max_tx_rings,
+			      unsigned int max_rx_rings)
 {
 	struct net_device *netdev;
 	struct nfp_net *nn;
@@ -3099,22 +3072,19 @@ 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);
 }
@@ -3188,25 +3158,29 @@ static void nfp_net_irqmod_init(struct nfp_net *nn)
 }
 
 /**
- * nfp_net_netdev_init() - Initialise/finalise the netdev structure
- * @netdev:      netdev structure
+ * 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_netdev_init(struct net_device *netdev)
+int nfp_net_init(struct nfp_net *nn)
 {
-	struct nfp_net *nn = netdev_priv(netdev);
+	struct net_device *netdev = nn->dp.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);
 
-	nfp_net_write_mac_addr(nn);
+	/* Chained metadata is signalled by capabilities except in version 4 */
+	nn->dp.chained_metadata_format = nn->fw_ver.major == 4 ||
+					 nn->cap & NFP_NET_CFG_CTRL_CHAIN_META;
+	if (nn->dp.chained_metadata_format && nn->fw_ver.major != 4)
+		nn->cap &= ~NFP_NET_CFG_CTRL_RSS;
+
+	nfp_net_write_mac_addr(nn, nn->dp.netdev->dev_addr);
 
 	/* Determine RX packet/metadata boundary offset */
 	if (nn->fw_ver.major >= 2) {
@@ -3236,10 +3210,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,14 +3226,17 @@ 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;
+		nn->dp.ctrl |= nn->cap & NFP_NET_CFG_CTRL_RSS2 ?:
+					 NFP_NET_CFG_CTRL_RSS;
 	}
 	if (nn->cap & NFP_NET_CFG_CTRL_VXLAN &&
 	    nn->cap & NFP_NET_CFG_CTRL_NVGRE) {
@@ -3275,17 +3255,22 @@ 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;
+		}
 	}
 
 	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;
 
 	/* Allow L2 Broadcast and Multicast through by default, if supported */
 	if (nn->cap & NFP_NET_CFG_CTRL_L2BC)
@@ -3322,23 +3307,19 @@ int nfp_net_netdev_init(struct net_device *netdev)
 	netif_carrier_off(netdev);
 
 	nfp_net_set_ethtool_ops(netdev);
-	nfp_net_vecs_init(netdev);
+	nfp_net_vecs_init(nn);
 
 	return register_netdev(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);
-
 	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..c8208bf370e0 100644
--- a/drivers/net/ethernet/netronome/nfp/nfp_net_ctrl.h
+++ b/drivers/net/ethernet/netronome/nfp/nfp_net_ctrl.h
@@ -71,6 +71,7 @@
 #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_CSUM		6 /* checksum complete type */
 
 /**
  * Hash type pre-pended when a RSS hash was computed
@@ -119,9 +120,9 @@
 #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_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 +132,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 +158,7 @@
 #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_ERR          (0x1 << 31) /* A error occurred */
 #define NFP_NET_CFG_TXRS_ENABLE         0x0008
 #define NFP_NET_CFG_RXRS_ENABLE         0x0010
diff --git a/drivers/net/ethernet/netronome/nfp/nfp_net_debugfs.c b/drivers/net/ethernet/netronome/nfp/nfp_net_debugfs.c
index 4077c59bf782..8c52c0e8379c 100644
--- a/drivers/net/ethernet/netronome/nfp/nfp_net_debugfs.c
+++ b/drivers/net/ethernet/netronome/nfp/nfp_net_debugfs.c
@@ -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);
@@ -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,7 @@ 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);
+	sprintf(name, "vnic%d", id);
 	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..83664ca25213 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",
 		 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_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..7dd310911d9f 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,10 +55,11 @@
 #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)
 
@@ -139,17 +141,19 @@ err_area:
  * First try to get the MAC address from NSP ETH table. If that
  * fails try HWInfo.  As a last resort generate a random address.
  */
-static void
+void
 nfp_net_get_mac_addr(struct nfp_net *nn, struct nfp_cpp *cpp, unsigned int id)
 {
+	struct nfp_eth_table_port *eth_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);
+	eth_port = __nfp_port_get_eth_port(nn->port);
+	if (eth_port) {
+		ether_addr_copy(dp->netdev->dev_addr, eth_port->mac_addr);
+		ether_addr_copy(dp->netdev->perm_addr, eth_port->mac_addr);
 		return;
 	}
 
@@ -175,7 +179,7 @@ nfp_net_get_mac_addr(struct nfp_net *nn, struct nfp_cpp *cpp, unsigned int id)
 	ether_addr_copy(dp->netdev->perm_addr, mac_addr);
 }
 
-static struct nfp_eth_table_port *
+struct nfp_eth_table_port *
 nfp_net_find_port(struct nfp_eth_table *eth_tbl, unsigned int id)
 {
 	int i;
@@ -187,30 +191,38 @@ nfp_net_find_port(struct nfp_eth_table *eth_tbl, unsigned int id)
 	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 */
 	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 int nfp_net_pf_get_num_ports(struct nfp_pf *pf)
+{
+	return nfp_net_pf_rtsym_read_optional(pf, "nfd_cfg_pf%u_num_ports", 1);
+}
+
+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 unsigned int
 nfp_net_pf_total_qcs(struct nfp_pf *pf, void __iomem *ctrl_bar,
 		     unsigned int stride, u32 start_off, u32 num_off)
@@ -220,7 +232,7 @@ nfp_net_pf_total_qcs(struct nfp_pf *pf, void __iomem *ctrl_bar,
 	min_qc = readl(ctrl_bar + start_off);
 	max_qc = min_qc;
 
-	for (i = 0; i < pf->num_ports; i++) {
+	for (i = 0; i < pf->max_data_vnics; 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).
@@ -241,13 +253,9 @@ static u8 __iomem *nfp_net_pf_map_ctrl_bar(struct nfp_pf *pf)
 	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);
+	snprintf(pf_symbol, sizeof(pf_symbol), "_pf%u_net_bar0",
+		 nfp_cppcore_pcie_unit(pf->cpp));
 
 	ctrl_sym = nfp_rtsym_lookup(pf->cpp, pf_symbol);
 	if (!ctrl_sym) {
@@ -256,17 +264,17 @@ static u8 __iomem *nfp_net_pf_map_ctrl_bar(struct nfp_pf *pf)
 		return NULL;
 	}
 
-	if (ctrl_sym->size < pf->num_ports * NFP_PF_CSR_SLICE_SIZE) {
+	if (ctrl_sym->size < pf->max_data_vnics * NFP_PF_CSR_SLICE_SIZE) {
 		dev_err(&pf->pdev->dev,
-			"PF BAR0 too small to contain %d ports\n",
-			pf->num_ports);
+			"PF BAR0 too small to contain %d vNICs\n",
+			pf->max_data_vnics);
 		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);
+				    &pf->data_vnic_bar);
 	if (IS_ERR(ctrl_bar)) {
 		dev_err(&pf->pdev->dev, "Failed to map PF BAR0: %ld\n",
 			PTR_ERR(ctrl_bar));
@@ -276,37 +284,43 @@ static u8 __iomem *nfp_net_pf_map_ctrl_bar(struct nfp_pf *pf)
 	return ctrl_bar;
 }
 
-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;
 
-		nfp_net_netdev_free(nn);
+	while (!list_empty(&pf->vnics)) {
+		nn = list_first_entry(&pf->vnics, struct nfp_net, vnic_list);
+		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, void __iomem *ctrl_bar,
+		      void __iomem *tx_bar, void __iomem *rx_bar,
+		      int stride, struct nfp_net_fw_version *fw_ver,
+		      unsigned int eth_id)
 {
 	u32 n_tx_rings, n_rx_rings;
 	struct nfp_net *nn;
+	int err;
 
 	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, n_tx_rings, n_rx_rings);
 	if (IS_ERR(nn))
 		return nn;
 
-	nn->cpp = pf->cpp;
+	nn->app = pf->app;
 	nn->fw_ver = *fw_ver;
 	nn->dp.ctrl_bar = ctrl_bar;
 	nn->tx_bar = tx_bar;
@@ -314,44 +328,58 @@ nfp_net_pf_alloc_port_netdev(struct nfp_pf *pf, void __iomem *ctrl_bar,
 	nn->dp.is_vf = 0;
 	nn->stride_rx = stride;
 	nn->stride_tx = stride;
-	nn->eth_port = eth_port;
+
+	err = nfp_app_vnic_init(pf->app, nn, eth_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 *tx_bar, void __iomem *rx_bar,
+		       int stride, struct nfp_net_fw_version *fw_ver)
 {
 	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;
@@ -359,7 +387,7 @@ nfp_net_pf_alloc_netdevs(struct nfp_pf *pf, void __iomem *ctrl_bar,
 	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++) {
+	for (i = 0; i < pf->max_data_vnics; 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;
@@ -367,53 +395,60 @@ nfp_net_pf_alloc_netdevs(struct nfp_pf *pf, void __iomem *ctrl_bar,
 		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++;
+		nn = nfp_net_pf_alloc_vnic(pf, ctrl_bar, tx_bar, rx_bar,
+					   stride, fw_ver, 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 void nfp_net_pf_clean_vnic(struct nfp_pf *pf, struct nfp_net *nn)
+{
+	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);
+}
+
 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)
+nfp_net_pf_spawn_vnics(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)
 {
-	unsigned int id, wanted_irqs, num_irqs, ports_left, irqs_left;
+	unsigned int id, wanted_irqs, num_irqs, vnics_left, irqs_left;
 	struct nfp_net *nn;
 	int err;
 
-	/* Allocate the netdevs and do basic init */
-	err = nfp_net_pf_alloc_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, tx_bar, rx_bar,
+				     stride, fw_ver);
 	if (err)
 		return err;
 
 	/* 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);
@@ -423,7 +458,7 @@ nfp_net_pf_spawn_netdevs(struct nfp_pf *pf,
 	}
 
 	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");
@@ -431,23 +466,23 @@ nfp_net_pf_spawn_netdevs(struct nfp_pf *pf,
 		goto err_vec_free;
 	}
 
-	/* 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 = 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 */
+	/* 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) {
+		err = nfp_net_pf_init_vnic(pf, nn, id);
 		if (err)
 			goto err_prev_deinit;
 
@@ -457,18 +492,41 @@ 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)
+		nfp_net_pf_clean_vnic(pf, nn);
 	nfp_net_irqs_disable(pf->pdev);
 err_vec_free:
 	kfree(pf->irq_entries);
 err_nn_free:
-	nfp_net_pf_free_netdevs(pf);
+	nfp_net_pf_free_vnics(pf);
 	return err;
 }
 
+static int nfp_net_pf_app_init(struct nfp_pf *pf)
+{
+	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;
+
+	return 0;
+
+err_free:
+	nfp_app_free(pf->app);
+	return err;
+}
+
+static void nfp_net_pf_app_clean(struct nfp_pf *pf)
+{
+	nfp_app_free(pf->app);
+	pf->app = NULL;
+}
+
 static void nfp_net_pci_remove_finish(struct nfp_pf *pf)
 {
 	nfp_net_debugfs_dir_clean(&pf->ddir);
@@ -476,99 +534,126 @@ static void nfp_net_pci_remove_finish(struct nfp_pf *pf)
 	nfp_net_irqs_disable(pf->pdev);
 	kfree(pf->irq_entries);
 
+	nfp_net_pf_app_clean(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);
+	nfp_cpp_area_release_free(pf->data_vnic_bar);
+}
+
+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);
+
+	mutex_lock(&pf->lock);
+	nfp_net_refresh_port_table_sync(pf);
+	mutex_unlock(&pf->lock);
+}
+
+void nfp_net_refresh_port_table(struct nfp_port *port)
+{
+	struct nfp_pf *pf = port->app->pf;
+
+	set_bit(NFP_PORT_CHANGED, &port->flags);
 
 	schedule_work(&pf->port_refresh_work);
 }
 
-int nfp_net_refresh_eth_port(struct nfp_net *nn)
+int nfp_net_refresh_eth_port(struct nfp_port *port)
 {
-	struct nfp_eth_table_port *eth_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;
 }
 
 /*
@@ -584,8 +669,7 @@ int nfp_net_pci_probe(struct nfp_pf *pf)
 	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)) {
@@ -593,8 +677,12 @@ int nfp_net_pci_probe(struct nfp_pf *pf)
 		return -EINVAL;
 	}
 
-	mutex_lock(&pf->port_lock);
-	pf->num_ports = nfp_net_pf_get_num_ports(pf);
+	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;
+	}
 
 	ctrl_bar = nfp_net_pf_map_ctrl_bar(pf);
 	if (!ctrl_bar) {
@@ -667,26 +755,32 @@ int nfp_net_pci_probe(struct nfp_pf *pf)
 		goto err_unmap_tx;
 	}
 
+	err = nfp_net_pf_app_init(pf);
+	if (err)
+		goto err_unmap_rx;
+
 	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);
+	err = nfp_net_pf_spawn_vnics(pf, ctrl_bar, tx_bar, rx_bar,
+				     stride, &fw_ver);
 	if (err)
 		goto err_clean_ddir;
 
-	mutex_unlock(&pf->port_lock);
+	mutex_unlock(&pf->lock);
 
 	return 0;
 
 err_clean_ddir:
 	nfp_net_debugfs_dir_clean(&pf->ddir);
+	nfp_net_pf_app_clean(pf);
+err_unmap_rx:
 	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_cpp_area_release_free(pf->data_vnic_bar);
 err_unlock:
-	mutex_unlock(&pf->port_lock);
+	mutex_unlock(&pf->lock);
 	return err;
 }
 
@@ -694,21 +788,18 @@ 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)
+		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_netvf_main.c b/drivers/net/ethernet/netronome/nfp/nfp_netvf_main.c
index 86e61be6f35c..3f1c7f0f392e 100644
--- a/drivers/net/ethernet/netronome/nfp/nfp_netvf_main.c
+++ b/drivers/net/ethernet/netronome/nfp/nfp_netvf_main.c
@@ -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, 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..a17410ac01ab
--- /dev/null
+++ b/drivers/net/ethernet/netronome/nfp/nfp_port.c
@@ -0,0 +1,140 @@
+/*
+ * 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 "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)
+{
+	struct nfp_net *nn;
+
+	if (WARN_ON(!nfp_netdev_is_nfp_net(netdev)))
+		return NULL;
+	nn = netdev_priv(netdev);
+
+	return nn->port;
+}
+
+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);
+	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);
+	if (n >= len)
+		return -EINVAL;
+
+	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..4d1a9b3fed41
--- /dev/null
+++ b/drivers/net/ethernet/netronome/nfp/nfp_port.h
@@ -0,0 +1,114 @@
+/*
+ * 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 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
+ */
+enum nfp_port_type {
+	NFP_PORT_INVALID,
+	NFP_PORT_PHYS_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
+ * @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;
+
+	unsigned int eth_id;
+	struct nfp_eth_table_port *eth_port;
+
+	struct list_head port_list;
+};
+
+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_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);
+
+#endif
diff --git a/drivers/net/ethernet/netronome/nfp/nfpcore/nfp.h b/drivers/net/ethernet/netronome/nfp/nfpcore/nfp.h
index 4df2ce261b3f..94641b4c2c55 100644
--- a/drivers/net/ethernet/netronome/nfp/nfpcore/nfp.h
+++ b/drivers/net/ethernet/netronome/nfp/nfpcore/nfp.h
@@ -64,6 +64,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..0a46c0984e68 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;
 
@@ -289,6 +297,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..5672d309d07d 100644
--- a/drivers/net/ethernet/netronome/nfp/nfpcore/nfp_cppcore.c
+++ b/drivers/net/ethernet/netronome/nfp/nfpcore/nfp_cppcore.c
@@ -924,18 +924,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 +959,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 +1027,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_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_nsp.c b/drivers/net/ethernet/netronome/nfp/nfpcore/nfp_nsp.c
index 2fa9247bb23d..eefdb756d74e 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)
@@ -498,3 +507,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..26d7dcea4fd9 100644
--- a/drivers/net/ethernet/netronome/nfp/nfpcore/nfp_nsp.h
+++ b/drivers/net/ethernet/netronome/nfp/nfpcore/nfp_nsp.h
@@ -96,6 +96,7 @@ 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 {
@@ -127,6 +128,8 @@ struct nfp_eth_table {
 		/* Computed fields */
 		u8 port_type;
 
+		unsigned int port_lanes;
+
 		bool is_split;
 	} ports[0];
 };
@@ -157,6 +160,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 +171,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..b0f8785c064f 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,19 @@ 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 (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,7 +207,6 @@ 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;
 		}
 }
 
@@ -289,7 +290,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/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/qlogic/netxen/netxen_nic_ctx.c b/drivers/net/ethernet/qlogic/netxen/netxen_nic_ctx.c
index b8d5270359cd..e30676515529 100644
--- a/drivers/net/ethernet/qlogic/netxen/netxen_nic_ctx.c
+++ b/drivers/net/ethernet/qlogic/netxen/netxen_nic_ctx.c
@@ -247,7 +247,7 @@ nx_fw_cmd_set_mtu(struct netxen_adapter *adapter, int mtu)
 	cmd.req.arg3 = 0;
 
 	if (recv_ctx->state == NX_HOST_CTX_STATE_ACTIVE)
-		netxen_issue_cmd(adapter, &cmd);
+		rcode = netxen_issue_cmd(adapter, &cmd);
 
 	if (rcode != NX_RCODE_SUCCESS)
 		return -EIO;
diff --git a/drivers/net/ethernet/qlogic/qed/qed.h b/drivers/net/ethernet/qlogic/qed/qed.h
index 2ab1aab7c3fe..e0becec17b09 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)
 
@@ -598,16 +598,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 +616,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 +627,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 +638,6 @@ struct qed_dev {
 
 	int				pcie_width;
 	int				pcie_speed;
-	u8				ver_str[VER_SIZE];
 
 	/* Add MF related configuration */
 	u8				mcp_rev;
diff --git a/drivers/net/ethernet/qlogic/qed/qed_dcbx.c b/drivers/net/ethernet/qlogic/qed/qed_dcbx.c
index d883ad5bec6d..e2a62c091b80 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_dcbx.c
+++ b/drivers/net/ethernet/qlogic/qed/qed_dcbx.c
@@ -191,17 +191,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 +215,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 +232,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 +305,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 +322,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);
 	}
 
@@ -923,6 +913,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 +935,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 +1450,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 +1524,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..d73e3c265466 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_dev.c
+++ b/drivers/net/ethernet/qlogic/qed/qed_dev.c
@@ -161,6 +161,7 @@ void qed_resc_free(struct qed_dev *cdev)
 	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,18 +169,18 @@ 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_dmae_info_free(p_hwfn);
@@ -299,7 +300,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 +329,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 +693,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 +823,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,15 +844,7 @@ 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;
 
@@ -956,45 +949,38 @@ 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;
 
 #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 */
@@ -1033,8 +1019,8 @@ void qed_resc_setup(struct qed_dev *cdev)
 
 		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);
@@ -1047,14 +1033,14 @@ void qed_resc_setup(struct qed_dev *cdev)
 		qed_iov_setup(p_hwfn, p_hwfn->p_main_ptt);
 #ifdef CONFIG_QED_LL2
 		if (p_hwfn->using_ll2)
-			qed_ll2_setup(p_hwfn, p_hwfn->p_ll2_info);
+			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 +1108,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 +1120,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;
 	}
 
@@ -1241,6 +1227,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 +1257,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 +1437,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 +1524,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 +1709,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 +1959,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 +1978,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 */
@@ -2252,7 +2263,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;
@@ -2669,15 +2680,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 +2698,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 +2830,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;
 }
 
@@ -4074,7 +4079,7 @@ 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)
diff --git a/drivers/net/ethernet/qlogic/qed/qed_fcoe.c b/drivers/net/ethernet/qlogic/qed/qed_fcoe.c
index 21a58fffd02b..cb342f16c137 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>
@@ -539,7 +538,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 +546,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 +578,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 +594,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
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..3bf3614b3084 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_hsi.h
+++ b/drivers/net/ethernet/qlogic/qed/qed_hsi.h
@@ -346,7 +346,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 +368,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 +681,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 +694,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 +776,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 +806,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 +919,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 +970,7 @@ enum personality_type {
 	PERSONALITY_ISCSI,
 	PERSONALITY_FCOE,
 	PERSONALITY_RDMA_AND_ETH,
-	PERSONALITY_RESERVED3,
+	PERSONALITY_RDMA,
 	PERSONALITY_CORE,
 	PERSONALITY_ETH,
 	PERSONALITY_RESERVED4,
@@ -971,16 +981,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 +996,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 +1014,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 +1029,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 +1038,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 +1127,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 +1326,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 +1470,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 +1584,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 +1642,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 +1657,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 +1674,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 +1733,8 @@ enum block_id {
 	BLOCK_MULD,
 	BLOCK_YULD,
 	BLOCK_XYLD,
+	BLOCK_PTLD,
+	BLOCK_YPLD,
 	BLOCK_PRM,
 	BLOCK_PBF_PB1,
 	BLOCK_PBF_PB2,
@@ -1745,6 +1748,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 +1765,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 +1783,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 +1869,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 +1909,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 +1944,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 +2034,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 +2085,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 +2126,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 +2180,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 +2203,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 +2234,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 +2250,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 +2359,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 +2379,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 +2476,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 +2493,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 +2740,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 +2799,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 +2823,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 +2843,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 +2860,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 +2881,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 +2900,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 +2925,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 +2941,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 +2963,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 +2980,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 +3002,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 +3076,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 +3125,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 +3134,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 +3169,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 +3186,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 +3201,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 +3218,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 +3233,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 +3250,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 +3265,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 +3283,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 +3299,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 +3316,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 +3331,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 +3929,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 +3967,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 +4066,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 +4647,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 +4680,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 +4967,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 +5220,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 +5330,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 +5476,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 +5688,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 +5705,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 +5954,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 +5962,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 +6235,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 +6261,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 +6555,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 +7071,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 +7102,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 +7499,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 +7647,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 +7726,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 +7836,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 +8517,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 +8620,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 +10448,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 +11511,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 +11672,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 +11680,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 +11695,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 +11720,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 +11822,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 +11995,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 67200c5498ab..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(union gft_cam_line_union));
-
-	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..6ac6d80311bb 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",
@@ -2679,6 +1514,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)
@@ -3157,6 +1993,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)
diff --git a/drivers/net/ethernet/qlogic/qed/qed_iscsi.c b/drivers/net/ethernet/qlogic/qed/qed_iscsi.c
index 339c91dfa658..43a20a6fd1b6 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>
@@ -186,7 +185,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;
 	}
 
@@ -375,7 +374,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 +398,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;
@@ -822,29 +818,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 +853,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,
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_l2.c b/drivers/net/ethernet/qlogic/qed/qed_l2.c
index 746fed4099c8..93dd781cf61d 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>
@@ -2301,14 +2300,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 +2335,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_ll2.c b/drivers/net/ethernet/qlogic/qed/qed_ll2.c
index 09c86411918c..f67ed6d39dfd 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>
@@ -1921,7 +1920,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 +1930,31 @@ 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_tstats(struct qed_hwfn *p_hwfn,
diff --git a/drivers/net/ethernet/qlogic/qed/qed_ll2.h b/drivers/net/ethernet/qlogic/qed/qed_ll2.h
index 31a409033c41..2c07d0ed971a 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_ll2.h
+++ b/drivers/net/ethernet/qlogic/qed/qed_ll2.h
@@ -306,27 +306,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 537d1236a4fe..97f5052bbdb6 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>
@@ -282,6 +281,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);
 
@@ -336,6 +338,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 +610,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 +633,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)
@@ -1112,17 +1129,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,
@@ -1676,7 +1689,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,
@@ -1730,7 +1743,8 @@ void qed_get_protocol_stats(struct qed_dev *cdev,
 		qed_get_protocol_stats_iscsi(cdev, &stats->iscsi_stats);
 		break;
 	default:
-		DP_ERR(cdev, "Invalid protocol type = %d\n", type);
+		DP_VERBOSE(cdev, QED_MSG_SP,
+			   "Invalid protocol type = %d\n", type);
 		return;
 	}
 }
diff --git a/drivers/net/ethernet/qlogic/qed/qed_mcp.c b/drivers/net/ethernet/qlogic/qed/qed_mcp.c
index 7266b36a2655..31c88e192cd0 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];
@@ -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,
diff --git a/drivers/net/ethernet/qlogic/qed/qed_mcp.h b/drivers/net/ethernet/qlogic/qed/qed_mcp.h
index 2b09b8545236..40247593e772 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
@@ -482,7 +494,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_reg_addr.h b/drivers/net/ethernet/qlogic/qed/qed_reg_addr.h
index 1ae73b2d6d1e..67172d7a7868 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 \
@@ -592,15 +594,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 +614,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 +646,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 +926,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 +996,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 +1246,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 +1330,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 +1432,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 +1482,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,6 +1559,7 @@
 #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
 
diff --git a/drivers/net/ethernet/qlogic/qed/qed_roce.c b/drivers/net/ethernet/qlogic/qed/qed_roce.c
index 56289d7cd306..eb1a5cfc49c0 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_roce.c
+++ b/drivers/net/ethernet/qlogic/qed/qed_roce.c
@@ -2432,10 +2432,6 @@ qed_rdma_register_tid(void *rdma_cxt,
 			  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);
 
diff --git a/drivers/net/ethernet/qlogic/qed/qed_sp.h b/drivers/net/ethernet/qlogic/qed/qed_sp.h
index 3357bbefa445..b9464f3ab0e2 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_sp.h
+++ b/drivers/net/ethernet/qlogic/qed/qed_sp.h
@@ -270,28 +270,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 +337,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 +391,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 +400,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 +418,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 +443,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..dede73f41e61 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_spq.c
+++ b/drivers/net/ethernet/qlogic/qed/qed_spq.c
@@ -403,14 +403,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,
@@ -426,24 +426,28 @@ struct qed_eq *qed_eq_alloc(struct qed_hwfn *p_hwfn, u16 num_elem)
 	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;
 }
 
 /***************************************************************************
@@ -583,8 +587,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 +938,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,
@@ -952,22 +956,26 @@ struct qed_consq *qed_consq_alloc(struct qed_hwfn *p_hwfn)
 			    0x80, &p_consq->chain))
 		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..b6bda45d0489 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_sriov.c
+++ b/drivers/net/ethernet/qlogic/qed/qed_sriov.c
@@ -747,6 +747,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 +800,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;
 
@@ -2209,7 +2239,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;
diff --git a/drivers/net/ethernet/qlogic/qede/qede.h b/drivers/net/ethernet/qlogic/qede/qede.h
index 9b4f08b6f9b9..694c09b8997e 100644
--- a/drivers/net/ethernet/qlogic/qede/qede.h
+++ b/drivers/net/ethernet/qlogic/qede/qede.h
@@ -197,7 +197,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..13955a3bd3b3 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,
@@ -1061,41 +1065,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 +1214,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..892eb98290f6 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++;
 	}
 
@@ -1424,7 +1427,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 +1458,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 +1516,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 +1525,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 +1589,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 +1642,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..00c70625f8a4 100644
--- a/drivers/net/ethernet/qlogic/qede/qede_main.c
+++ b/drivers/net/ethernet/qlogic/qede/qede_main.c
@@ -259,7 +259,7 @@ 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);
@@ -618,6 +618,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;
@@ -846,6 +852,43 @@ static void qede_update_pf_params(struct qed_dev *cdev)
 	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,
 };
@@ -924,7 +967,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,8 +982,7 @@ 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:
@@ -1066,12 +1108,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 */
@@ -1298,12 +1343,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,7 +1358,7 @@ 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,
+					    txq->num_tx_buffers,
 					    sizeof(*p_virt), &txq->tx_pbl);
 	if (rc)
 		goto err;
@@ -1328,7 +1373,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);
@@ -1890,9 +1935,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_roce_dev_event_close(edev);
+
 	/* Close OS Tx */
 	netif_tx_disable(edev->ndev);
 	netif_carrier_off(edev->ndev);
@@ -1988,9 +2034,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);
 
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_roce.c
index f00657ce7c8f..c0030fb8d842 100644
--- a/drivers/net/ethernet/qlogic/qede/qede_roce.c
+++ b/drivers/net/ethernet/qlogic/qede/qede_roce.c
@@ -221,8 +221,8 @@ static void qede_roce_changeaddr(struct qede_dev *edev)
 		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_roce_event_work *
+qede_roce_get_free_event_node(struct qede_dev *edev)
 {
 	struct qede_roce_event_work *event_node = NULL;
 	struct list_head *list_node = NULL;
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic.h b/drivers/net/ethernet/qlogic/qlcnic/qlcnic.h
index 49bad00a0f8f..7245b1072518 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic.h
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic.h
@@ -37,8 +37,8 @@
 
 #define _QLCNIC_LINUX_MAJOR 5
 #define _QLCNIC_LINUX_MINOR 3
-#define _QLCNIC_LINUX_SUBVERSION 65
-#define QLCNIC_LINUX_VERSIONID  "5.3.65"
+#define _QLCNIC_LINUX_SUBVERSION 66
+#define QLCNIC_LINUX_VERSIONID  "5.3.66"
 #define QLCNIC_DRV_IDC_VER  0x01
 #define QLCNIC_DRIVER_VERSION  ((_QLCNIC_LINUX_MAJOR << 16) |\
 		 (_QLCNIC_LINUX_MINOR << 8) | (_QLCNIC_LINUX_SUBVERSION))
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_83xx_hw.c b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_83xx_hw.c
index 718bf58a7da6..4fb68797630e 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_83xx_hw.c
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_83xx_hw.c
@@ -3168,6 +3168,40 @@ int qlcnic_83xx_flash_read32(struct qlcnic_adapter *adapter, u32 flash_addr,
 	return 0;
 }
 
+void qlcnic_83xx_get_port_type(struct qlcnic_adapter *adapter)
+{
+	struct qlcnic_hardware_context *ahw = adapter->ahw;
+	struct qlcnic_cmd_args cmd;
+	u32 config;
+	int err;
+
+	err = qlcnic_alloc_mbx_args(&cmd, adapter, QLCNIC_CMD_GET_LINK_STATUS);
+	if (err)
+		return;
+
+	err = qlcnic_issue_cmd(adapter, &cmd);
+	if (err) {
+		dev_info(&adapter->pdev->dev,
+			 "Get Link Status Command failed: 0x%x\n", err);
+		goto out;
+	} else {
+		config = cmd.rsp.arg[3];
+
+		switch (QLC_83XX_SFP_MODULE_TYPE(config)) {
+		case QLC_83XX_MODULE_FIBRE_1000BASE_SX:
+		case QLC_83XX_MODULE_FIBRE_1000BASE_LX:
+		case QLC_83XX_MODULE_FIBRE_1000BASE_CX:
+		case QLC_83XX_MODULE_TP_1000BASE_T:
+			ahw->port_type = QLCNIC_GBE;
+			break;
+		default:
+			ahw->port_type = QLCNIC_XGBE;
+		}
+	}
+out:
+	qlcnic_free_mbx_args(&cmd);
+}
+
 int qlcnic_83xx_test_link(struct qlcnic_adapter *adapter)
 {
 	u8 pci_func;
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_83xx_hw.h b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_83xx_hw.h
index 3dfe8e27b51c..b75a81246856 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_83xx_hw.h
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_83xx_hw.h
@@ -637,6 +637,7 @@ void qlcnic_83xx_get_pauseparam(struct qlcnic_adapter *,
 int qlcnic_83xx_set_pauseparam(struct qlcnic_adapter *,
 			       struct ethtool_pauseparam *);
 int qlcnic_83xx_test_link(struct qlcnic_adapter *);
+void qlcnic_83xx_get_port_type(struct qlcnic_adapter *adapter);
 int qlcnic_83xx_reg_test(struct qlcnic_adapter *);
 int qlcnic_83xx_get_regs_len(struct qlcnic_adapter *);
 int qlcnic_83xx_get_registers(struct qlcnic_adapter *, u32 *);
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_ethtool.c b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_ethtool.c
index 9a869c15d8bf..7f7deeaf1cf0 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_ethtool.c
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_ethtool.c
@@ -486,6 +486,9 @@ static int qlcnic_set_link_ksettings(struct net_device *dev,
 	u32 ret = 0;
 	struct qlcnic_adapter *adapter = netdev_priv(dev);
 
+	if (qlcnic_83xx_check(adapter))
+		qlcnic_83xx_get_port_type(adapter);
+
 	if (adapter->ahw->port_type != QLCNIC_GBE)
 		return -EOPNOTSUPP;
 
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/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 513e6c74e199..de78f60309a0 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;
@@ -296,8 +319,9 @@ qcaspi_receive(struct qcaspi *qca)
 
 	/* Allocate rx SKB if we don't have one available. */
 	if (!qca->rx_skb) {
-		qca->rx_skb = netdev_alloc_skb(net_dev,
-					       net_dev->mtu + VLAN_ETH_HLEN);
+		qca->rx_skb = netdev_alloc_skb_ip_align(net_dev,
+							net_dev->mtu +
+							VLAN_ETH_HLEN);
 		if (!qca->rx_skb) {
 			netdev_dbg(net_dev, "out of RX resources\n");
 			qca->stats.out_of_mem++;
@@ -377,7 +401,7 @@ qcaspi_receive(struct qcaspi *qca)
 					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(net_dev,
+				qca->rx_skb = netdev_alloc_skb_ip_align(net_dev,
 					net_dev->mtu + VLAN_ETH_HLEN);
 				if (!qca->rx_skb) {
 					netdev_dbg(net_dev, "out of RX resources\n");
@@ -402,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
@@ -602,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);
@@ -614,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);
@@ -639,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);
@@ -666,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");
@@ -745,7 +769,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;
@@ -759,7 +783,8 @@ qcaspi_netdev_init(struct net_device *dev)
 	if (!qca->rx_buffer)
 		return -ENOBUFS;
 
-	qca->rx_skb = netdev_alloc_skb(dev, qca->net_dev->mtu + VLAN_ETH_HLEN);
+	qca->rx_skb = netdev_alloc_skb_ip_align(dev, qca->net_dev->mtu +
+						VLAN_ETH_HLEN);
 	if (!qca->rx_skb) {
 		kfree(qca->rx_buffer);
 		netdev_info(qca->net_dev, "Failed to allocate RX sk_buff.\n");
@@ -803,8 +828,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));
@@ -892,6 +917,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) {
@@ -973,7 +999,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/renesas/sh_eth.c b/drivers/net/ethernet/renesas/sh_eth.c
index f68c4db656ed..2d686ccf971b 100644
--- a/drivers/net/ethernet/renesas/sh_eth.c
+++ b/drivers/net/ethernet/renesas/sh_eth.c
@@ -3220,7 +3220,8 @@ static int sh_eth_drv_probe(struct platform_device *pdev)
 	/* MDIO bus init */
 	ret = sh_mdio_init(mdp, pd);
 	if (ret) {
-		dev_err(&ndev->dev, "failed to initialise MDIO\n");
+		if (ret != -EPROBE_DEFER)
+			dev_err(&pdev->dev, "MDIO init failed: %d\n", ret);
 		goto out_release;
 	}
 
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..ad9c4ded2b90 100644
--- a/drivers/net/ethernet/sfc/ef10.c
+++ b/drivers/net/ethernet/sfc/ef10.c
@@ -6068,6 +6068,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/nic.h b/drivers/net/ethernet/sfc/nic.h
index 7b916aa21bde..4d7fb8af880d 100644
--- a/drivers/net/ethernet/sfc/nic.h
+++ b/drivers/net/ethernet/sfc/nic.h
@@ -18,8 +18,12 @@
 #include "mcdi.h"
 
 enum {
-	EFX_REV_SIENA_A0 = 0,
-	EFX_REV_HUNT_A0 = 1,
+	/* Revisions 0-2 were Falcon A0, A1 and B0 respectively.
+	 * They are not supported by this driver but these revision numbers
+	 * form part of the ethtool API for register dumping.
+	 */
+	EFX_REV_SIENA_A0 = 3,
+	EFX_REV_HUNT_A0 = 4,
 };
 
 static inline int efx_nic_rev(struct efx_nic *efx)
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/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/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/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_main.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
index cd8c60132390..f158273eab9b 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
@@ -644,6 +644,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 +775,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 +788,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 +800,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 +826,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 +894,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;
 
@@ -2879,8 +2867,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)) {
@@ -3083,8 +3070,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
@@ -3725,7 +3711,7 @@ static void sysfs_display_ring(void *head, int size, int extend_desc,
 			ep++;
 		} else {
 			seq_printf(seq, "%d [0x%x]: 0x%x 0x%x 0x%x 0x%x\n",
-				   i, (unsigned int)virt_to_phys(ep),
+				   i, (unsigned int)virt_to_phys(p),
 				   le32_to_cpu(p->des0), le32_to_cpu(p->des1),
 				   le32_to_cpu(p->des2), le32_to_cpu(p->des3));
 			p++;
@@ -4292,7 +4278,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/sun/ldmvsw.c b/drivers/net/ethernet/sun/ldmvsw.c
index 5a90fed06260..5b56c24b6ed2 100644
--- a/drivers/net/ethernet/sun/ldmvsw.c
+++ b/drivers/net/ethernet/sun/ldmvsw.c
@@ -411,13 +411,14 @@ static int vsw_port_remove(struct vio_dev *vdev)
 
 	if (port) {
 		del_timer_sync(&port->vio.timer);
+		del_timer_sync(&port->clean_timer);
 
 		napi_disable(&port->napi);
+		unregister_netdev(port->dev);
 
 		list_del_rcu(&port->list);
 
 		synchronize_rcu();
-		del_timer_sync(&port->clean_timer);
 		spin_lock_irqsave(&port->vp->lock, flags);
 		sunvnet_port_rm_txq_common(port);
 		spin_unlock_irqrestore(&port->vp->lock, flags);
@@ -427,7 +428,6 @@ static int vsw_port_remove(struct vio_dev *vdev)
 
 		dev_set_drvdata(&vdev->dev, NULL);
 
-		unregister_netdev(port->dev);
 		free_netdev(port->dev);
 	}
 
diff --git a/drivers/net/ethernet/ti/cpsw.c b/drivers/net/ethernet/ti/cpsw.c
index f4d7aec50479..37fc16521143 100644
--- a/drivers/net/ethernet/ti/cpsw.c
+++ b/drivers/net/ethernet/ti/cpsw.c
@@ -1734,6 +1734,7 @@ static int cpsw_hwtstamp_set(struct net_device *dev, struct ifreq *ifr)
 	case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
 	case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
 	case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
+	case HWTSTAMP_FILTER_NTP_ALL:
 		return -ERANGE;
 	case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
 	case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
diff --git a/drivers/net/ethernet/ti/netcp_core.c b/drivers/net/ethernet/ti/netcp_core.c
index 729a7da90b5b..e6222e535019 100644
--- a/drivers/net/ethernet/ti/netcp_core.c
+++ b/drivers/net/ethernet/ti/netcp_core.c
@@ -1353,9 +1353,10 @@ int netcp_txpipe_open(struct netcp_tx_pipe *tx_pipe)
 
 	tx_pipe->dma_channel = knav_dma_open_channel(dev,
 				tx_pipe->dma_chan_name, &config);
-	if (IS_ERR_OR_NULL(tx_pipe->dma_channel)) {
+	if (IS_ERR(tx_pipe->dma_channel)) {
 		dev_err(dev, "failed opening tx chan(%s)\n",
 			tx_pipe->dma_chan_name);
+		ret = PTR_ERR(tx_pipe->dma_channel);
 		goto err;
 	}
 
@@ -1673,9 +1674,10 @@ static int netcp_setup_navigator_resources(struct net_device *ndev)
 
 	netcp->rx_channel = knav_dma_open_channel(netcp->netcp_device->device,
 					netcp->dma_chan_name, &config);
-	if (IS_ERR_OR_NULL(netcp->rx_channel)) {
+	if (IS_ERR(netcp->rx_channel)) {
 		dev_err(netcp->ndev_dev, "failed opening rx chan(%s\n",
 			netcp->dma_chan_name);
+		ret = PTR_ERR(netcp->rx_channel);
 		goto fail;
 	}
 
diff --git a/drivers/net/ethernet/ti/netcp_ethss.c b/drivers/net/ethernet/ti/netcp_ethss.c
index 897176fc5043..dd92950a4615 100644
--- a/drivers/net/ethernet/ti/netcp_ethss.c
+++ b/drivers/net/ethernet/ti/netcp_ethss.c
@@ -2651,7 +2651,6 @@ static int gbe_hwtstamp_set(struct gbe_intf *gbe_intf, struct ifreq *ifr)
 	case HWTSTAMP_FILTER_NONE:
 		cpts_rx_enable(cpts, 0);
 		break;
-	case HWTSTAMP_FILTER_ALL:
 	case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
 	case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
 	case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
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/geneve.c b/drivers/net/geneve.c
index dec5d563ab19..959fd12d2e67 100644
--- a/drivers/net/geneve.c
+++ b/drivers/net/geneve.c
@@ -1293,7 +1293,7 @@ static int geneve_fill_info(struct sk_buff *skb, const struct net_device *dev)
 	if (nla_put_u32(skb, IFLA_GENEVE_ID, vni))
 		goto nla_put_failure;
 
-	if (ip_tunnel_info_af(info) == AF_INET) {
+	if (rtnl_dereference(geneve->sock4)) {
 		if (nla_put_in_addr(skb, IFLA_GENEVE_REMOTE,
 				    info->key.u.ipv4.dst))
 			goto nla_put_failure;
@@ -1302,8 +1302,10 @@ static int geneve_fill_info(struct sk_buff *skb, const struct net_device *dev)
 			       !!(info->key.tun_flags & TUNNEL_CSUM)))
 			goto nla_put_failure;
 
+	}
+
 #if IS_ENABLED(CONFIG_IPV6)
-	} else {
+	if (rtnl_dereference(geneve->sock6)) {
 		if (nla_put_in6_addr(skb, IFLA_GENEVE_REMOTE6,
 				     &info->key.u.ipv6.dst))
 			goto nla_put_failure;
@@ -1315,8 +1317,8 @@ static int geneve_fill_info(struct sk_buff *skb, const struct net_device *dev)
 		if (nla_put_u8(skb, IFLA_GENEVE_UDP_ZERO_CSUM6_RX,
 			       !geneve->use_udp6_rx_checksums))
 			goto nla_put_failure;
-#endif
 	}
+#endif
 
 	if (nla_put_u8(skb, IFLA_GENEVE_TTL, info->key.ttl) ||
 	    nla_put_u8(skb, IFLA_GENEVE_TOS, info->key.tos) ||
diff --git a/drivers/net/gtp.c b/drivers/net/gtp.c
index 4fea1b3dfbb4..7b652bb7ebe4 100644
--- a/drivers/net/gtp.c
+++ b/drivers/net/gtp.c
@@ -873,7 +873,7 @@ static struct gtp_dev *gtp_find_dev(struct net *src_net, struct nlattr *nla[])
 
 	/* Check if there's an existing gtpX device to configure */
 	dev = dev_get_by_index_rcu(net, nla_get_u32(nla[GTPA_LINK]));
-	if (dev->netdev_ops == &gtp_netdev_ops)
+	if (dev && dev->netdev_ops == &gtp_netdev_ops)
 		gtp = netdev_priv(dev);
 
 	put_net(net);
diff --git a/drivers/net/hamradio/hdlcdrv.c b/drivers/net/hamradio/hdlcdrv.c
index 8c3633c1d078..97e3bc60c3e7 100644
--- a/drivers/net/hamradio/hdlcdrv.c
+++ b/drivers/net/hamradio/hdlcdrv.c
@@ -576,6 +576,8 @@ static int hdlcdrv_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
 	case HDLCDRVCTL_CALIBRATE:
 		if(!capable(CAP_SYS_RAWIO))
 			return -EPERM;
+		if (s->par.bitrate <= 0)
+			return -EINVAL;
 		if (bi.data.calibrate > INT_MAX / s->par.bitrate)
 			return -EINVAL;
 		s->hdlctx.calibrate = bi.data.calibrate * s->par.bitrate / 16;
diff --git a/drivers/net/hyperv/netvsc_drv.c b/drivers/net/hyperv/netvsc_drv.c
index 4421a6d00375..2564ac83eb64 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"
 
diff --git a/drivers/net/ieee802154/ca8210.c b/drivers/net/ieee802154/ca8210.c
index 25fd3b04b3c0..7a218549c80a 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 */
@@ -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/irda/irda-usb.c b/drivers/net/irda/irda-usb.c
index 8716b8c07feb..6f3c805f7211 100644
--- a/drivers/net/irda/irda-usb.c
+++ b/drivers/net/irda/irda-usb.c
@@ -1077,7 +1077,7 @@ static int stir421x_patch_device(struct irda_usb_cb *self)
          * are "42101001.sb" or "42101002.sb"
          */
         sprintf(stir421x_fw_name, "4210%4X.sb",
-                self->usbdev->descriptor.bcdDevice);
+		le16_to_cpu(self->usbdev->descriptor.bcdDevice));
         ret = request_firmware(&fw, stir421x_fw_name, &self->usbdev->dev);
         if (ret < 0)
                 return ret;
diff --git a/drivers/net/macsec.c b/drivers/net/macsec.c
index cdc347be68f2..91642fd87cd1 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++;
 	}
 }
 
@@ -883,7 +881,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 +902,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();
 
@@ -1037,7 +1032,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 +1048,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++;
 		}
 	}
 
diff --git a/drivers/net/macvlan.c b/drivers/net/macvlan.c
index b34eaaae03fd..346ad2ff3998 100644
--- a/drivers/net/macvlan.c
+++ b/drivers/net/macvlan.c
@@ -789,10 +789,12 @@ static int macvlan_change_mtu(struct net_device *dev, int new_mtu)
  */
 static struct lock_class_key macvlan_netdev_addr_lock_key;
 
-#define ALWAYS_ON_FEATURES \
-	(NETIF_F_SG | NETIF_F_HW_CSUM | NETIF_F_GSO_SOFTWARE | NETIF_F_LLTX | \
+#define ALWAYS_ON_OFFLOADS \
+	(NETIF_F_SG | NETIF_F_HW_CSUM | NETIF_F_GSO_SOFTWARE | \
 	 NETIF_F_GSO_ROBUST)
 
+#define ALWAYS_ON_FEATURES (ALWAYS_ON_OFFLOADS | NETIF_F_LLTX)
+
 #define MACVLAN_FEATURES \
 	(NETIF_F_SG | NETIF_F_HW_CSUM | NETIF_F_HIGHDMA | NETIF_F_FRAGLIST | \
 	 NETIF_F_GSO | NETIF_F_TSO | NETIF_F_UFO | NETIF_F_LRO | \
@@ -827,6 +829,7 @@ static int macvlan_init(struct net_device *dev)
 	dev->features		|= ALWAYS_ON_FEATURES;
 	dev->hw_features	|= NETIF_F_LRO;
 	dev->vlan_features	= lowerdev->vlan_features & MACVLAN_FEATURES;
+	dev->vlan_features	|= ALWAYS_ON_OFFLOADS;
 	dev->gso_max_size	= lowerdev->gso_max_size;
 	dev->gso_max_segs	= lowerdev->gso_max_segs;
 	dev->hard_header_len	= lowerdev->hard_header_len;
diff --git a/drivers/net/phy/Kconfig b/drivers/net/phy/Kconfig
index 60ffc9da6a28..0c516d3229d0 100644
--- a/drivers/net/phy/Kconfig
+++ b/drivers/net/phy/Kconfig
@@ -108,7 +108,7 @@ config MDIO_MOXART
 config MDIO_OCTEON
 	tristate "Octeon and some ThunderX SOCs MDIO buses"
 	depends on 64BIT
-	depends on HAS_IOMEM
+	depends on HAS_IOMEM && OF_MDIO
 	select MDIO_CAVIUM
 	help
 	  This module provides a driver for the Octeon and ThunderX MDIO
@@ -234,6 +234,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---
diff --git a/drivers/net/phy/Makefile b/drivers/net/phy/Makefile
index e36db9a2ba38..e2fde094f63d 100644
--- a/drivers/net/phy/Makefile
+++ b/drivers/net/phy/Makefile
@@ -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
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/marvell.c b/drivers/net/phy/marvell.c
index 272b051a0199..7c2cde45c262 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;
 }
@@ -255,34 +278,6 @@ static int marvell_config_aneg(struct phy_device *phydev)
 {
 	int err;
 
-	/* The Marvell PHY has an errata which requires
-	 * that certain registers get written in order
-	 * to restart autonegotiation */
-	err = phy_write(phydev, MII_BMCR, BMCR_RESET);
-
-	if (err < 0)
-		return err;
-
-	err = phy_write(phydev, 0x1d, 0x1f);
-	if (err < 0)
-		return err;
-
-	err = phy_write(phydev, 0x1e, 0x200c);
-	if (err < 0)
-		return err;
-
-	err = phy_write(phydev, 0x1d, 0x5);
-	if (err < 0)
-		return err;
-
-	err = phy_write(phydev, 0x1e, 0);
-	if (err < 0)
-		return err;
-
-	err = phy_write(phydev, 0x1e, 0x100);
-	if (err < 0)
-		return err;
-
 	err = marvell_set_polarity(phydev, phydev->mdix_ctrl);
 	if (err < 0)
 		return err;
@@ -299,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.
 		 */
@@ -316,6 +310,42 @@ static int marvell_config_aneg(struct phy_device *phydev)
 	return 0;
 }
 
+static int m88e1101_config_aneg(struct phy_device *phydev)
+{
+	int err;
+
+	/* This Marvell PHY has an errata which requires
+	 * that certain registers get written in order
+	 * to restart autonegotiation
+	 */
+	err = phy_write(phydev, MII_BMCR, BMCR_RESET);
+
+	if (err < 0)
+		return err;
+
+	err = phy_write(phydev, 0x1d, 0x1f);
+	if (err < 0)
+		return err;
+
+	err = phy_write(phydev, 0x1e, 0x200c);
+	if (err < 0)
+		return err;
+
+	err = phy_write(phydev, 0x1d, 0x5);
+	if (err < 0)
+		return err;
+
+	err = phy_write(phydev, 0x1e, 0);
+	if (err < 0)
+		return err;
+
+	err = phy_write(phydev, 0x1e, 0x100);
+	if (err < 0)
+		return err;
+
+	return marvell_config_aneg(phydev);
+}
+
 static int m88e1111_config_aneg(struct phy_device *phydev)
 {
 	int err;
@@ -359,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>,...;
@@ -386,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;
@@ -394,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;
 		}
@@ -421,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;
 	}
@@ -442,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;
 
@@ -467,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)
@@ -485,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;
@@ -499,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;
 
@@ -599,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;
 
@@ -609,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;
 
@@ -617,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;
 }
 
@@ -643,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;
 
@@ -655,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);
@@ -664,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;
 
@@ -698,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;
 	}
@@ -810,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,
@@ -822,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);
@@ -836,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;
 
@@ -855,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;
 	}
@@ -885,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;
 
@@ -895,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;
 
@@ -912,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;
 
@@ -924,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;
 
@@ -938,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);
@@ -968,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;
 	}
@@ -1057,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)
@@ -1074,125 +1133,141 @@ 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;
+	int adv;
 
-	/* Detect and update the link, but return if there
-	 * was an error */
-	if (page == MII_M1111_FIBER)
-		fiber = 1;
-	else
-		fiber = 0;
+	status = phy_read(phydev, MII_M1011_PHY_STATUS);
+	if (status < 0)
+		return status;
 
-	err = marvell_update_link(phydev, fiber);
-	if (err)
-		return err;
+	lpa = phy_read(phydev, MII_LPA);
+	if (lpa < 0)
+		return lpa;
 
-	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;
+	lpagb = phy_read(phydev, MII_STAT1000);
+	if (lpagb < 0)
+		return lpagb;
 
-		lpagb = phy_read(phydev, MII_STAT1000);
-		if (lpagb < 0)
-			return lpagb;
+	adv = phy_read(phydev, MII_ADVERTISE);
+	if (adv < 0)
+		return adv;
 
-		adv = phy_read(phydev, MII_ADVERTISE);
-		if (adv < 0)
-			return adv;
+	if (status & MII_M1011_PHY_STATUS_FULLDUPLEX)
+		phydev->duplex = DUPLEX_FULL;
+	else
+		phydev->duplex = DUPLEX_HALF;
 
-		lpa &= 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.
@@ -1209,33 +1284,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;
 }
 
@@ -1250,7 +1326,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;
 
@@ -1260,7 +1336,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;
 	}
@@ -1269,7 +1345,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;
 }
 
@@ -1284,7 +1360,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;
 
@@ -1294,7 +1370,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;
 	}
@@ -1303,13 +1379,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);
 }
 
@@ -1325,32 +1402,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;
 
@@ -1361,8 +1439,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;
 
@@ -1375,8 +1452,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;
 
@@ -1405,8 +1481,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;
 
@@ -1419,7 +1494,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;
 
@@ -1451,13 +1526,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);
@@ -1469,7 +1542,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;
 }
@@ -1486,6 +1559,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;
 
@@ -1493,9 +1567,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);
@@ -1525,7 +1601,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;
@@ -1602,15 +1678,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)
@@ -1619,7 +1698,7 @@ 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;
@@ -1627,15 +1706,18 @@ error:
 
 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)
@@ -1647,7 +1729,7 @@ 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;
@@ -1655,13 +1737,16 @@ error:
 
 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)
@@ -1674,7 +1759,7 @@ 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;
@@ -1682,15 +1767,18 @@ error:
 
 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)
@@ -1698,7 +1786,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;
@@ -1892,7 +1980,7 @@ static struct phy_driver marvell_drivers[] = {
 		.flags = PHY_HAS_INTERRUPT,
 		.probe = marvell_probe,
 		.config_init = &marvell_config_init,
-		.config_aneg = &marvell_config_aneg,
+		.config_aneg = &m88e1101_config_aneg,
 		.read_status = &genphy_read_status,
 		.ack_interrupt = &marvell_ack_interrupt,
 		.config_intr = &marvell_config_intr,
diff --git a/drivers/net/phy/mdio-mux.c b/drivers/net/phy/mdio-mux.c
index 963838d4fac1..599ce24c514f 100644
--- a/drivers/net/phy/mdio-mux.c
+++ b/drivers/net/phy/mdio-mux.c
@@ -122,10 +122,9 @@ int mdio_mux_init(struct device *dev,
 	pb = devm_kzalloc(dev, sizeof(*pb), GFP_KERNEL);
 	if (pb == NULL) {
 		ret_val = -ENOMEM;
-		goto err_parent_bus;
+		goto err_pb_kz;
 	}
 
-
 	pb->switch_data = data;
 	pb->switch_fn = switch_fn;
 	pb->current_child = -1;
@@ -154,6 +153,7 @@ int mdio_mux_init(struct device *dev,
 		cb->mii_bus = mdiobus_alloc();
 		if (!cb->mii_bus) {
 			ret_val = -ENOMEM;
+			devm_kfree(dev, cb);
 			of_node_put(child_bus_node);
 			break;
 		}
@@ -170,7 +170,6 @@ int mdio_mux_init(struct device *dev,
 			mdiobus_free(cb->mii_bus);
 			devm_kfree(dev, cb);
 		} else {
-			of_node_get(child_bus_node);
 			cb->next = pb->children;
 			pb->children = cb;
 		}
@@ -181,9 +180,11 @@ int mdio_mux_init(struct device *dev,
 		return 0;
 	}
 
+	devm_kfree(dev, pb);
+err_pb_kz:
 	/* balance the reference of_mdio_find_bus() took */
-	put_device(&pb->mii_bus->dev);
-
+	if (!mux_bus)
+		put_device(&parent_bus->dev);
 err_parent_bus:
 	of_node_put(parent_bus_node);
 	return ret_val;
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 a898e5c4ef1b..f99c21f78b63 100644
--- a/drivers/net/phy/mdio_bus.c
+++ b/drivers/net/phy/mdio_bus.c
@@ -364,9 +364,6 @@ int __mdiobus_register(struct mii_bus *bus, struct module *owner)
 
 	mutex_init(&bus->mdio_lock);
 
-	if (bus->reset)
-		bus->reset(bus);
-
 	/* de-assert bus level PHY GPIO resets */
 	if (bus->num_reset_gpios > 0) {
 		bus->reset_gpiod = devm_kcalloc(&bus->dev,
@@ -396,6 +393,9 @@ int __mdiobus_register(struct mii_bus *bus, struct module *owner)
 		}
 	}
 
+	if (bus->reset)
+		bus->reset(bus);
+
 	for (i = 0; i < PHY_MAX_ADDR; i++) {
 		if ((bus->phy_mask & (1 << i)) == 0) {
 			struct phy_device *phydev;
@@ -658,6 +658,18 @@ static int mdio_bus_match(struct device *dev, struct device_driver *drv)
 	return 0;
 }
 
+static int mdio_uevent(struct device *dev, struct kobj_uevent_env *env)
+{
+	int rc;
+
+	/* Some devices have extra OF data and an OF-style MODALIAS */
+	rc = of_device_uevent_modalias(dev, env);
+	if (rc != -ENODEV)
+		return rc;
+
+	return 0;
+}
+
 #ifdef CONFIG_PM
 static int mdio_bus_suspend(struct device *dev)
 {
@@ -708,6 +720,7 @@ static const struct dev_pm_ops mdio_bus_pm_ops = {
 struct bus_type mdio_bus_type = {
 	.name		= "mdio_bus",
 	.match		= mdio_bus_match,
+	.uevent		= mdio_uevent,
 	.pm		= MDIO_BUS_PM_OPS,
 };
 EXPORT_SYMBOL(mdio_bus_type);
diff --git a/drivers/net/phy/micrel.c b/drivers/net/phy/micrel.c
index 6a5fd18f062c..46e80bcc7a8a 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>
@@ -779,7 +780,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,
@@ -793,7 +794,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,
@@ -811,7 +812,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,
@@ -829,7 +830,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,
@@ -847,7 +848,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,
@@ -865,7 +866,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,
@@ -883,7 +884,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,
@@ -901,7 +902,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,
@@ -919,7 +920,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,
@@ -932,7 +933,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,
@@ -952,7 +953,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,
@@ -969,7 +970,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,
@@ -980,7 +980,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,
@@ -991,12 +991,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.c b/drivers/net/phy/phy.c
index 82ab8fb82587..709b8be53443 100644
--- a/drivers/net/phy/phy.c
+++ b/drivers/net/phy/phy.c
@@ -377,6 +377,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
diff --git a/drivers/net/phy/phy_device.c b/drivers/net/phy/phy_device.c
index 1219eeab69d1..37a1e98908e3 100644
--- a/drivers/net/phy/phy_device.c
+++ b/drivers/net/phy/phy_device.c
@@ -961,6 +961,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;
@@ -1050,6 +1071,10 @@ void phy_detach(struct phy_device *phydev)
 	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);
@@ -1571,13 +1596,13 @@ int genphy_config_init(struct phy_device *phydev)
 
 	return 0;
 }
+EXPORT_SYMBOL(genphy_config_init);
 
 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)
 {
diff --git a/drivers/net/phy/smsc.c b/drivers/net/phy/smsc.c
index cef6967b0396..67c9f2b26c8e 100644
--- a/drivers/net/phy/smsc.c
+++ b/drivers/net/phy/smsc.c
@@ -170,7 +170,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 +192,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,
 
@@ -214,7 +214,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,
 
@@ -236,7 +236,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 +257,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,
 
@@ -279,7 +279,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,
 
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/tun.c b/drivers/net/tun.c
index bbd707b9ef7a..f8041f9c7e65 100644
--- a/drivers/net/tun.c
+++ b/drivers/net/tun.c
@@ -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;
@@ -1646,7 +1647,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 +2628,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/cdc_ether.c b/drivers/net/usb/cdc_ether.c
index f3ae88fdf332..8ab281b478f2 100644
--- a/drivers/net/usb/cdc_ether.c
+++ b/drivers/net/usb/cdc_ether.c
@@ -310,6 +310,26 @@ skip:
 		return -ENODEV;
 	}
 
+	return 0;
+
+bad_desc:
+	dev_info(&dev->udev->dev, "bad CDC descriptors\n");
+	return -ENODEV;
+}
+EXPORT_SYMBOL_GPL(usbnet_generic_cdc_bind);
+
+
+/* like usbnet_generic_cdc_bind() but handles filter initialization
+ * correctly
+ */
+int usbnet_ether_cdc_bind(struct usbnet *dev, struct usb_interface *intf)
+{
+	int rv;
+
+	rv = usbnet_generic_cdc_bind(dev, intf);
+	if (rv < 0)
+		goto bail_out;
+
 	/* Some devices don't initialise properly. In particular
 	 * the packet filter is not reset. There are devices that
 	 * don't do reset all the way. So the packet filter should
@@ -317,13 +337,10 @@ skip:
 	 */
 	usbnet_cdc_update_filter(dev);
 
-	return 0;
-
-bad_desc:
-	dev_info(&dev->udev->dev, "bad CDC descriptors\n");
-	return -ENODEV;
+bail_out:
+	return rv;
 }
-EXPORT_SYMBOL_GPL(usbnet_generic_cdc_bind);
+EXPORT_SYMBOL_GPL(usbnet_ether_cdc_bind);
 
 void usbnet_cdc_unbind(struct usbnet *dev, struct usb_interface *intf)
 {
@@ -417,7 +434,7 @@ int usbnet_cdc_bind(struct usbnet *dev, struct usb_interface *intf)
 	BUILD_BUG_ON((sizeof(((struct usbnet *)0)->data)
 			< sizeof(struct cdc_state)));
 
-	status = usbnet_generic_cdc_bind(dev, intf);
+	status = usbnet_ether_cdc_bind(dev, intf);
 	if (status < 0)
 		return status;
 
diff --git a/drivers/net/usb/ch9200.c b/drivers/net/usb/ch9200.c
index c4f1c363e24b..9df3c1ffff35 100644
--- a/drivers/net/usb/ch9200.c
+++ b/drivers/net/usb/ch9200.c
@@ -310,8 +310,8 @@ static int get_mac_address(struct usbnet *dev, unsigned char *data)
 	int rd_mac_len = 0;
 
 	netdev_dbg(dev->net, "get_mac_address:\n\tusbnet VID:%0x PID:%0x\n",
-		   dev->udev->descriptor.idVendor,
-		   dev->udev->descriptor.idProduct);
+		   le16_to_cpu(dev->udev->descriptor.idVendor),
+		   le16_to_cpu(dev->udev->descriptor.idProduct));
 
 	memset(mac_addr, 0, sizeof(mac_addr));
 	rd_mac_len = control_read(dev, REQUEST_READ, 0,
diff --git a/drivers/net/usb/net1080.c b/drivers/net/usb/net1080.c
index 4cbdb1307f3e..3202c19df83d 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)) );
diff --git a/drivers/net/usb/qmi_wwan.c b/drivers/net/usb/qmi_wwan.c
index d7165767ca9d..8f923a147fa9 100644
--- a/drivers/net/usb/qmi_wwan.c
+++ b/drivers/net/usb/qmi_wwan.c
@@ -1196,6 +1196,8 @@ static const struct usb_device_id products[] = {
 	{QMI_FIXED_INTF(0x1199, 0x9071, 10)},	/* Sierra Wireless MC74xx */
 	{QMI_FIXED_INTF(0x1199, 0x9079, 8)},	/* Sierra Wireless EM74xx */
 	{QMI_FIXED_INTF(0x1199, 0x9079, 10)},	/* Sierra Wireless EM74xx */
+	{QMI_FIXED_INTF(0x1199, 0x907b, 8)},	/* Sierra Wireless EM74xx */
+	{QMI_FIXED_INTF(0x1199, 0x907b, 10)},	/* Sierra Wireless EM74xx */
 	{QMI_FIXED_INTF(0x1bbb, 0x011e, 4)},	/* Telekom Speedstick LTE II (Alcatel One Touch L100V LTE) */
 	{QMI_FIXED_INTF(0x1bbb, 0x0203, 2)},	/* Alcatel L800MA */
 	{QMI_FIXED_INTF(0x2357, 0x0201, 4)},	/* TP-LINK HSUPA Modem MA180 */
diff --git a/drivers/net/usb/r8152.c b/drivers/net/usb/r8152.c
index ddc62cb69be8..e902df9595b9 100644
--- a/drivers/net/usb/r8152.c
+++ b/drivers/net/usb/r8152.c
@@ -841,12 +841,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);
diff --git a/drivers/net/usb/smsc95xx.c b/drivers/net/usb/smsc95xx.c
index 765400b62168..2dfca96a63b6 100644
--- a/drivers/net/usb/smsc95xx.c
+++ b/drivers/net/usb/smsc95xx.c
@@ -681,7 +681,7 @@ static int smsc95xx_set_features(struct net_device *netdev,
 	if (ret < 0)
 		return ret;
 
-	if (features & NETIF_F_HW_CSUM)
+	if (features & NETIF_F_IP_CSUM)
 		read_buf |= Tx_COE_EN_;
 	else
 		read_buf &= ~Tx_COE_EN_;
@@ -1279,12 +1279,19 @@ static int smsc95xx_bind(struct usbnet *dev, struct usb_interface *intf)
 
 	spin_lock_init(&pdata->mac_cr_lock);
 
+	/* LAN95xx devices do not alter the computed checksum of 0 to 0xffff.
+	 * RFC 2460, ipv6 UDP calculated checksum yields a result of zero must
+	 * be changed to 0xffff. RFC 768, ipv4 UDP computed checksum is zero,
+	 * it is transmitted as all ones. The zero transmitted checksum means
+	 * transmitter generated no checksum. Hence, enable csum offload only
+	 * for ipv4 packets.
+	 */
 	if (DEFAULT_TX_CSUM_ENABLE)
-		dev->net->features |= NETIF_F_HW_CSUM;
+		dev->net->features |= NETIF_F_IP_CSUM;
 	if (DEFAULT_RX_CSUM_ENABLE)
 		dev->net->features |= NETIF_F_RXCSUM;
 
-	dev->net->hw_features = NETIF_F_HW_CSUM | NETIF_F_RXCSUM;
+	dev->net->hw_features = NETIF_F_IP_CSUM | NETIF_F_RXCSUM;
 
 	smsc95xx_init_mac_address(dev);
 
diff --git a/drivers/net/virtio_net.c b/drivers/net/virtio_net.c
index 9320d96a1632..3e9246cc49c3 100644
--- a/drivers/net/virtio_net.c
+++ b/drivers/net/virtio_net.c
@@ -1989,6 +1989,7 @@ static const struct net_device_ops virtnet_netdev = {
 	.ndo_poll_controller = virtnet_netpoll,
 #endif
 	.ndo_xdp		= virtnet_xdp,
+	.ndo_features_check	= passthru_features_check,
 };
 
 static void virtnet_config_changed_work(struct work_struct *work)
diff --git a/drivers/net/vmxnet3/vmxnet3_drv.c b/drivers/net/vmxnet3/vmxnet3_drv.c
index 25bc764ae7dc..d1c7029ded7c 100644
--- a/drivers/net/vmxnet3/vmxnet3_drv.c
+++ b/drivers/net/vmxnet3/vmxnet3_drv.c
@@ -2962,6 +2962,11 @@ vmxnet3_force_close(struct vmxnet3_adapter *adapter)
 	/* we need to enable NAPI, otherwise dev_close will deadlock */
 	for (i = 0; i < adapter->num_rx_queues; i++)
 		napi_enable(&adapter->rx_queue[i].napi);
+	/*
+	 * Need to clear the quiesce bit to ensure that vmxnet3_close
+	 * can quiesce the device properly
+	 */
+	clear_bit(VMXNET3_STATE_BIT_QUIESCED, &adapter->state);
 	dev_close(adapter->netdev);
 }
 
diff --git a/drivers/net/vrf.c b/drivers/net/vrf.c
index ceda5861da78..db882493875c 100644
--- a/drivers/net/vrf.c
+++ b/drivers/net/vrf.c
@@ -989,6 +989,7 @@ static u32 vrf_fib_table(const struct net_device *dev)
 
 static int vrf_rcv_finish(struct net *net, struct sock *sk, struct sk_buff *skb)
 {
+	kfree_skb(skb);
 	return 0;
 }
 
@@ -998,7 +999,7 @@ static struct sk_buff *vrf_rcv_nfhook(u8 pf, unsigned int hook,
 {
 	struct net *net = dev_net(dev);
 
-	if (NF_HOOK(pf, hook, net, NULL, skb, dev, NULL, vrf_rcv_finish) < 0)
+	if (nf_hook(pf, hook, net, NULL, skb, dev, NULL, vrf_rcv_finish) != 1)
 		skb = NULL;    /* kfree_skb(skb) handled by nf code */
 
 	return skb;
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_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/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/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..0342073ed397 100644
--- a/drivers/net/wireless/ath/ath10k/bmi.h
+++ b/drivers/net/wireless/ath/ath10k/bmi.h
@@ -198,6 +198,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/core.c b/drivers/net/wireless/ath/ath10k/core.c
index 5a0638915874..eea111d704c5 100644
--- a/drivers/net/wireless/ath/ath10k/core.c
+++ b/drivers/net/wireless/ath/ath10k/core.c
@@ -389,6 +389,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;
@@ -1395,7 +1410,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 +1979,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 +2229,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;
diff --git a/drivers/net/wireless/ath/ath10k/core.h b/drivers/net/wireless/ath/ath10k/core.h
index bf091514ecc6..8fc08a5043db 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";
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/hw.h b/drivers/net/wireless/ath/ath10k/hw.h
index 5b1e90bb2a4d..d34272803fd7 100644
--- a/drivers/net/wireless/ath/ath10k/hw.h
+++ b/drivers/net/wireless/ath/ath10k/hw.h
@@ -863,6 +863,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/sdio.c b/drivers/net/wireless/ath/ath10k/sdio.c
new file mode 100644
index 000000000000..9e78fbae8413
--- /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 addrress: %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 addrress: %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/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/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/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/tx99.c b/drivers/net/wireless/ath/ath9k/tx99.c
index 16aca9e28b77..a866cbda0799 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;
 }
diff --git a/drivers/net/wireless/ath/wil6210/debugfs.c b/drivers/net/wireless/ath/wil6210/debugfs.c
index 5648ebbd0e16..5b0f9fc66bb6 100644
--- a/drivers/net/wireless/ath/wil6210/debugfs.c
+++ b/drivers/net/wireless/ath/wil6210/debugfs.c
@@ -795,15 +795,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;
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/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/cfg80211.c b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/cfg80211.c
index cd1d6730eab7..a2bf11fc8ecc 100644
--- a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/cfg80211.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/cfg80211.c
@@ -4674,9 +4674,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,
@@ -6378,16 +6375,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 +6421,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;
@@ -6483,9 +6471,10 @@ static int brcmf_setup_wiphy(struct wiphy *wiphy, struct brcmf_if *ifp)
 		wiphy->flags |= WIPHY_FLAG_SUPPORTS_FW_ROAM;
 	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;
diff --git a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/cfg80211.h b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/cfg80211.h
index 8f19d95d4175..a1c2e0af1774 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
diff --git a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/feature.c b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/feature.c
index 62985f2c0853..8c7ef59944f0 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");
diff --git a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/feature.h b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/feature.h
index db4733a95e28..c1dbd17506aa 100644
--- a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/feature.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/feature.h
@@ -31,6 +31,7 @@
  * 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.
  */
 #define BRCMF_FEAT_LIST \
 	BRCMF_FEAT_DEF(MBSS) \
@@ -44,7 +45,8 @@
 	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)
 
 /*
  * Quirks:
diff --git a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/fwil_types.h b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/fwil_types.h
index 9a1eb5ab6c4b..8c18fad3edc9 100644
--- a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/fwil_types.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/fwil_types.h
@@ -835,4 +835,63 @@ struct brcmf_gtk_keyinfo_le {
 	u8 replay_counter[BRCMF_RSN_REPLAY_LEN];
 };
 
+/**
+ * struct brcmf_gscan_bucket_config - configuration data for channel bucket.
+ *
+ * @bucket_end_index: !unknown!
+ * @bucket_freq_multiple: !unknown!
+ * @flag: !unknown!
+ * @reserved: !unknown!
+ * @repeat: !unknown!
+ * @max_freq_multiple: !unknown!
+ */
+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                     1
+
+/**
+ * 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_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_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/pno.c b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/pno.c
index 6c3bde83d070..a473445ed7c4 100644
--- a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/pno.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/pno.c
@@ -239,3 +239,13 @@ int brcmf_pno_start_sched_scan(struct brcmf_if *ifp,
 	return ret;
 }
 
+void brcmf_pno_wiphy_params(struct wiphy *wiphy, bool gscan)
+{
+	/* scheduled scan settings */
+	wiphy->max_sched_scan_reqs = gscan ? 2 : 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;
+}
+
diff --git a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/pno.h b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/pno.h
index bae55b2af78c..07ec51f6bec1 100644
--- a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/pno.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/pno.h
@@ -37,4 +37,12 @@ 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_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);
+
 #endif /* _BRCMF_PNO_H */
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/intersil/hostap/hostap_hw.c b/drivers/net/wireless/intersil/hostap/hostap_hw.c
index 04dfd040a650..d4f0b730796e 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);
 
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/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..3076f646c829 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;
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..e0196208ab0d 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;
 }
 
@@ -244,8 +241,6 @@ static int if_sdio_handle_data(struct if_sdio_card *card,
 	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",
 			(int)size);
@@ -270,8 +265,6 @@ static int if_sdio_handle_data(struct if_sdio_card *card,
 	ret = 0;
 
 out:
-	lbs_deb_leave_args(LBS_DEB_SDIO, "ret %d", ret);
-
 	return ret;
 }
 
@@ -281,8 +274,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 +298,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 +326,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 +397,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 +407,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 +434,6 @@ static void if_sdio_host_to_card_worker(struct work_struct *work)
 
 		kfree(packet);
 	}
-
-	lbs_deb_leave(LBS_DEB_SDIO);
 }
 
 /********************************************************************/
@@ -471,8 +452,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 +535,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 +548,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 +667,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 +700,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 +742,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 +920,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 +983,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 +1008,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 +1015,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 +1024,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 +1031,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 +1109,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 +1133,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 +1146,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 +1230,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 +1253,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 +1288,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 +1367,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 +1375,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..55f8c997e5cb 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)
@@ -455,8 +437,6 @@ static int lbs_thread(void *data)
 	struct lbs_private *priv = dev->ml_priv;
 	wait_queue_t wait;
 
-	lbs_deb_enter(LBS_DEB_THREAD);
-
 	init_waitqueue_entry(&wait, current);
 
 	for (;;) {
@@ -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..eeeb892219aa 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;
 }
 
@@ -823,8 +817,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 +862,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 +878,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 +897,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 +911,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 +925,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 +950,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 +973,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 +1030,6 @@ err_free_wdev:
 	kfree(mesh_wdev);
 
 done:
-	lbs_deb_leave_args(LBS_DEB_MESH, "ret %d", ret);
 	return ret;
 }
 
@@ -1060,7 +1041,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 +1049,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 +1087,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 +1105,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 +1130,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 +1145,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..a18bb7a9889c 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;
 
@@ -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/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/cfg80211.c b/drivers/net/wireless/marvell/mwifiex/cfg80211.c
index 7ec06bf13413..025bc06a19d6 100644
--- a/drivers/net/wireless/marvell/mwifiex/cfg80211.c
+++ b/drivers/net/wireless/marvell/mwifiex/cfg80211.c
@@ -2964,10 +2964,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 +2974,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 +3009,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,16 +3025,9 @@ 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,
@@ -3061,6 +3035,13 @@ struct wireless_dev *mwifiex_add_virtual_intf(struct wiphy *wiphy,
 
 	sema_init(&priv->async_sem, 1);
 
+	/* 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 +3062,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/cmdevt.c b/drivers/net/wireless/marvell/mwifiex/cmdevt.c
index 0c3b217247b1..95221306a4e5 100644
--- a/drivers/net/wireless/marvell/mwifiex/cmdevt.c
+++ b/drivers/net/wireless/marvell/mwifiex/cmdevt.c
@@ -761,8 +761,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 +769,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 +1055,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/init.c b/drivers/net/wireless/marvell/mwifiex/init.c
index 756948385b60..80bdf1c5f77f 100644
--- a/drivers/net/wireless/marvell/mwifiex/init.c
+++ b/drivers/net/wireless/marvell/mwifiex/init.c
@@ -409,11 +409,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 +434,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 +664,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 +687,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.h b/drivers/net/wireless/marvell/mwifiex/main.h
index bb2a467d8b13..c1d96c64af74 100644
--- a/drivers/net/wireless/marvell/mwifiex/main.h
+++ b/drivers/net/wireless/marvell/mwifiex/main.h
@@ -870,8 +870,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;
@@ -1235,7 +1233,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..394224d6c219 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);
@@ -2380,11 +2378,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 +2815,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 +2830,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 +2840,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 +2925,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 +2979,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 +3199,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 +3257,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/sdio.c b/drivers/net/wireless/marvell/mwifiex/sdio.c
index 0af1c6733c92..d38d31bb9b79 100644
--- a/drivers/net/wireless/marvell/mwifiex/sdio.c
+++ b/drivers/net/wireless/marvell/mwifiex/sdio.c
@@ -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/usb.c b/drivers/net/wireless/marvell/mwifiex/usb.c
index 2f7705c50161..debd6216366a 100644
--- a/drivers/net/wireless/marvell/mwifiex/usb.c
+++ b/drivers/net/wireless/marvell/mwifiex/usb.c
@@ -424,7 +424,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);
 
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..fc0ce2c09097
--- /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 = 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..f0a0cfa7d8a1
--- /dev/null
+++ b/drivers/net/wireless/quantenna/qtnfmac/commands.c
@@ -0,0 +1,1982 @@
+/*
+ * 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;
+	}
+
+	memset(skb_put(cmd_skb, cmd_size), 0, 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 = (struct qlink_tlv_channel *)skb_put(cmd_skb, sizeof(*qchan));
+
+	memset(qchan, 0, 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;
+
+	cmd_skb = qtnf_cmd_alloc_new_cmdskb(mac->macid, 0,
+					    QLINK_CMD_CHANS_INFO_GET,
+					    sizeof(*cmd));
+	if (!cmd_skb)
+		return -ENOMEM;
+
+	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 = (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 = (struct qlink_tlv_channel *)
+					skb_put(cmd_skb, sizeof(*qchan));
+			memset(qchan, 0, 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..c5ac252464f4
--- /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->destructor = free_netdev;
+	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..4814d90c8040
--- /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;
+	}
+
+	memcpy(skb_put(skb, len), 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;
+
+	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..d8de484b5995
--- /dev/null
+++ b/drivers/net/wireless/quantenna/qtnfmac/qlink_util.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_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 = (__le16 *)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)
+{
+	u8 *buf_dst;
+
+	buf_dst = skb_put(skb, len);
+	memcpy(buf_dst, 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 =
+			(void *)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 =
+			(void *)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 =
+			(void *)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..4a1bca1b1e26 100644
--- a/drivers/net/wireless/ralink/rt2x00/rt2x00debug.c
+++ b/drivers/net/wireless/ralink/rt2x00/rt2x00debug.c
@@ -460,7 +460,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/rtlwifi/btcoexist/halbtc8192e2ant.c b/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8192e2ant.c
index 57e633dbf9a9..9015512ed647 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 */
@@ -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..a57d6947eaf7 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
diff --git a/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8723b1ant.c b/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8723b1ant.c
index 2003c8c51dcc..a0f3a18add25 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,88 +2335,138 @@ 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_
  **************************************************************/
+void ex_halbtc8723b1ant_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_halbtc8723b1ant_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)
@@ -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,9 +2753,8 @@ 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);
 }
 
@@ -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,7 +2781,7 @@ 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);
 	}
@@ -1983,13 +2809,41 @@ void ex_halbtc8723b1ant_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);
@@ -2043,13 +2897,32 @@ void ex_halbtc8723b1ant_connect_notify(struct btc_coexist *btcoexist, u8 type)
 	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;
@@ -2094,27 +2967,62 @@ void ex_halbtc8723b1ant_media_status_notify(struct btc_coexist *btcoexist,
 	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;
@@ -2141,13 +3049,46 @@ void ex_halbtc8723b1ant_special_packet_notify(struct btc_coexist *btcoexist,
 	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;
@@ -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.
 		 */
@@ -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,6 +3364,43 @@ void ex_halbtc8723b1ant_bt_info_notify(struct btc_coexist *btcoexist,
 	halbtc8723b1ant_run_coexist_mechanism(btcoexist);
 }
 
+void ex_halbtc8723b1ant_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_halbtc8723b1ant_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);
 
@@ -2349,6 +3419,8 @@ void ex_halbtc8723b1ant_halt_notify(struct btc_coexist *btcoexist)
 	halbtc8723b1ant_ps_tdma(btcoexist, FORCE_EXEC, false, 0);
 
 	ex_halbtc8723b1ant_media_status_notify(btcoexist, BTC_MEDIA_DISCONNECT);
+
+	btcoexist->stop_coex_dm = true;
 }
 
 void ex_halbtc8723b1ant_pnp_notify(struct btc_coexist *btcoexist, u8 pnp_state)
@@ -2360,18 +3432,27 @@ 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);
 	}
@@ -2384,57 +3465,33 @@ void ex_halbtc8723b1ant_coex_dm_reset(struct btc_coexist *btcoexist)
 	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)
 {
 	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..506961a1ca56 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,18 +162,47 @@ 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_power_on_setting(struct btc_coexist *btcoexist);
+void ex_halbtc8723b1ant_init_hwconfig(struct btc_coexist *btcoexist,
+				      bool wifi_only);
 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);
@@ -177,6 +214,8 @@ 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_rf_status_notify(struct btc_coexist *btcoexist,
+					 u8 type);
 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);
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..50726beaeead 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
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..cb32e7a64ae6 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;
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..a1603e2d44e3 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;
 };
 
 /*===========================================
diff --git a/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtcoutsrc.h b/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtcoutsrc.h
index c8271135aaaa..c5c360e011a9 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
@@ -196,6 +199,24 @@ 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,
+};
+
 enum btc_get_type {
 	/* type bool */
 	BTC_GET_BL_HS_OPERATION,
@@ -235,6 +256,7 @@ enum btc_get_type {
 	BTC_GET_U1_WIFI_HS_CHNL,
 	BTC_GET_U1_MAC_PHY_MODE,
 	BTC_GET_U1_AP_NUM,
+	BTC_GET_U1_IOT_PEER,
 
 	/* for 1Ant */
 	BTC_GET_U1_LPS_MODE,
@@ -468,6 +490,7 @@ struct btc_statistics {
 
 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 +519,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;
diff --git a/drivers/net/wireless/realtek/rtlwifi/rtl8192ee/hw.c b/drivers/net/wireless/realtek/rtlwifi/rtl8192ee/hw.c
index 6f5098a18655..11d97fa0e921 100644
--- a/drivers/net/wireless/realtek/rtlwifi/rtl8192ee/hw.c
+++ b/drivers/net/wireless/realtek/rtlwifi/rtl8192ee/hw.c
@@ -2506,7 +2506,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/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/rtl8723be/hw.c b/drivers/net/wireless/realtek/rtlwifi/rtl8723be/hw.c
index 1acbfb86472c..a79f936bb394 100644
--- a/drivers/net/wireless/realtek/rtlwifi/rtl8723be/hw.c
+++ b/drivers/net/wireless/realtek/rtlwifi/rtl8723be/hw.c
@@ -2637,7 +2637,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/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_hal.c b/drivers/net/wireless/rsi/rsi_91x_hal.c
new file mode 100644
index 000000000000..d49dbaa14079
--- /dev/null
+++ b/drivers/net/wireless/rsi/rsi_91x_hal.c
@@ -0,0 +1,740 @@
+/**
+ * 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},
+	{"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 = 1;
+	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;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(rsi_hal_device_init);
+
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..2ef844adacf3 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.
@@ -365,6 +366,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 +489,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 +520,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 +554,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 +792,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 +854,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 +867,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,6 +900,8 @@ 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",
@@ -720,13 +909,19 @@ static int rsi_probe(struct sdio_func *pfunction,
 		goto fail;
 	}
 
-	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_disable_func(pfunction);
 		sdio_release_host(pfunction);
 		goto fail;
 	}
+	rsi_dbg(INFO_ZONE, "===> RSI Device Init Done <===\n");
+
+	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_claim_host(pfunction);
 	if (sdio_claim_irq(pfunction, rsi_handle_interrupt)) {
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..f5de6934f7ec 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);
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_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..b95200df5b0f
--- /dev/null
+++ b/drivers/net/wireless/rsi/rsi_hal.h
@@ -0,0 +1,81 @@
+/**
+ * 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
+
+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..ea4fc223cea7 100644
--- a/drivers/net/wireless/rsi/rsi_main.h
+++ b/drivers/net/wireless/rsi/rsi_main.h
@@ -82,6 +82,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 +206,20 @@ struct rsi_common {
 	struct cqm_info cqm_info;
 
 	bool hw_data_qs_blocked;
+	u8 coex_mode;
 	
 	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 +228,39 @@ struct rsi_hw {
 	struct device *device;
 	u8 sc_nvifs;
 
+	enum host_intf rsi_host_intf;
+	u16 block_size;
 #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_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/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/spi.c b/drivers/net/wireless/ti/wlcore/spi.c
index f949ad2bd898..fa3547e06424 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 \
diff --git a/drivers/net/xen-netfront.c b/drivers/net/xen-netfront.c
index 6ffc482550c1..7b61adb6270c 100644
--- a/drivers/net/xen-netfront.c
+++ b/drivers/net/xen-netfront.c
@@ -1934,8 +1934,7 @@ abort_transaction_no_dev_fatal:
 	xennet_disconnect_backend(info);
 	xennet_destroy_queues(info);
  out:
-	unregister_netdev(info->netdev);
-	xennet_free_netdev(info->netdev);
+	device_unregister(&dev->dev);
 	return err;
 }
 
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/nvme/host/core.c b/drivers/nvme/host/core.c
index d5e0906262ea..a60926410438 100644
--- a/drivers/nvme/host/core.c
+++ b/drivers/nvme/host/core.c
@@ -925,6 +925,29 @@ static int nvme_getgeo(struct block_device *bdev, struct hd_geometry *geo)
 }
 
 #ifdef CONFIG_BLK_DEV_INTEGRITY
+static void nvme_prep_integrity(struct gendisk *disk, struct nvme_id_ns *id,
+		u16 bs)
+{
+	struct nvme_ns *ns = disk->private_data;
+	u16 old_ms = ns->ms;
+	u8 pi_type = 0;
+
+	ns->ms = le16_to_cpu(id->lbaf[id->flbas & NVME_NS_FLBAS_LBA_MASK].ms);
+	ns->ext = ns->ms && (id->flbas & NVME_NS_FLBAS_META_EXT);
+
+	/* PI implementation requires metadata equal t10 pi tuple size */
+	if (ns->ms == sizeof(struct t10_pi_tuple))
+		pi_type = id->dps & NVME_NS_DPS_PI_MASK;
+
+	if (blk_get_integrity(disk) &&
+	    (ns->pi_type != pi_type || ns->ms != old_ms ||
+	     bs != queue_logical_block_size(disk->queue) ||
+	     (ns->ms && ns->ext)))
+		blk_integrity_unregister(disk);
+
+	ns->pi_type = pi_type;
+}
+
 static void nvme_init_integrity(struct nvme_ns *ns)
 {
 	struct blk_integrity integrity;
@@ -951,6 +974,10 @@ static void nvme_init_integrity(struct nvme_ns *ns)
 	blk_queue_max_integrity_segments(ns->queue, 1);
 }
 #else
+static void nvme_prep_integrity(struct gendisk *disk, struct nvme_id_ns *id,
+		u16 bs)
+{
+}
 static void nvme_init_integrity(struct nvme_ns *ns)
 {
 }
@@ -997,37 +1024,22 @@ 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;
-	u8 lbaf, pi_type;
-	u16 old_ms;
-	unsigned short bs;
-
-	old_ms = ns->ms;
-	lbaf = id->flbas & NVME_NS_FLBAS_LBA_MASK;
-	ns->lba_shift = id->lbaf[lbaf].ds;
-	ns->ms = le16_to_cpu(id->lbaf[lbaf].ms);
-	ns->ext = ns->ms && (id->flbas & NVME_NS_FLBAS_META_EXT);
+	u16 bs;
 
 	/*
 	 * If identify namespace failed, use default 512 byte block size so
 	 * block layer can use before failing read/write for 0 capacity.
 	 */
+	ns->lba_shift = id->lbaf[id->flbas & NVME_NS_FLBAS_LBA_MASK].ds;
 	if (ns->lba_shift == 0)
 		ns->lba_shift = 9;
 	bs = 1 << ns->lba_shift;
-	/* XXX: PI implementation requires metadata equal t10 pi tuple size */
-	pi_type = ns->ms == sizeof(struct t10_pi_tuple) ?
-					id->dps & NVME_NS_DPS_PI_MASK : 0;
 
 	blk_mq_freeze_queue(disk->queue);
-	if (blk_get_integrity(disk) && (ns->pi_type != pi_type ||
-				ns->ms != old_ms ||
-				bs != queue_logical_block_size(disk->queue) ||
-				(ns->ms && ns->ext)))
-		blk_integrity_unregister(disk);
 
-	ns->pi_type = pi_type;
+	if (ns->ctrl->ops->flags & NVME_F_METADATA_SUPPORTED)
+		nvme_prep_integrity(disk, id, bs);
 	blk_queue_logical_block_size(ns->queue, bs);
-
 	if (ns->ms && !blk_get_integrity(disk) && !ns->ext)
 		nvme_init_integrity(ns);
 	if (ns->ms && !(ns->ms == 8 && ns->pi_type) && !blk_get_integrity(disk))
@@ -1605,7 +1617,7 @@ int nvme_init_identify(struct nvme_ctrl *ctrl)
 	}
 	memcpy(ctrl->psd, id->psd, sizeof(ctrl->psd));
 
-	if (ctrl->ops->is_fabrics) {
+	if (ctrl->ops->flags & NVME_F_FABRICS) {
 		ctrl->icdoff = le16_to_cpu(id->icdoff);
 		ctrl->ioccsz = le32_to_cpu(id->ioccsz);
 		ctrl->iorcsz = le32_to_cpu(id->iorcsz);
@@ -2098,7 +2110,6 @@ static void nvme_ns_remove(struct nvme_ns *ns)
 		if (ns->ndev)
 			nvme_nvm_unregister_sysfs(ns);
 		del_gendisk(ns->disk);
-		blk_mq_abort_requeue_list(ns->queue);
 		blk_cleanup_queue(ns->queue);
 	}
 
@@ -2436,8 +2447,16 @@ void nvme_kill_queues(struct nvme_ctrl *ctrl)
 			continue;
 		revalidate_disk(ns->disk);
 		blk_set_queue_dying(ns->queue);
-		blk_mq_abort_requeue_list(ns->queue);
-		blk_mq_start_stopped_hw_queues(ns->queue, true);
+
+		/*
+		 * Forcibly start all queues to avoid having stuck requests.
+		 * Note that we must ensure the queues are not stopped
+		 * when the final removal happens.
+		 */
+		blk_mq_start_hw_queues(ns->queue);
+
+		/* draining requests in requeue list */
+		blk_mq_kick_requeue_list(ns->queue);
 	}
 	mutex_unlock(&ctrl->namespaces_mutex);
 }
diff --git a/drivers/nvme/host/fc.c b/drivers/nvme/host/fc.c
index 70e689bf1cad..5b14cbefb724 100644
--- a/drivers/nvme/host/fc.c
+++ b/drivers/nvme/host/fc.c
@@ -45,8 +45,6 @@ enum nvme_fc_queue_flags {
 
 #define NVMEFC_QUEUE_DELAY	3		/* ms units */
 
-#define NVME_FC_MAX_CONNECT_ATTEMPTS	1
-
 struct nvme_fc_queue {
 	struct nvme_fc_ctrl	*ctrl;
 	struct device		*dev;
@@ -165,8 +163,6 @@ struct nvme_fc_ctrl {
 	struct work_struct	delete_work;
 	struct work_struct	reset_work;
 	struct delayed_work	connect_work;
-	int			reconnect_delay;
-	int			connect_attempts;
 
 	struct kref		ref;
 	u32			flags;
@@ -1376,9 +1372,9 @@ done:
 	complete_rq = __nvme_fc_fcpop_chk_teardowns(ctrl, op);
 	if (!complete_rq) {
 		if (unlikely(op->flags & FCOP_FLAGS_TERMIO)) {
-			status = cpu_to_le16(NVME_SC_ABORT_REQ);
+			status = cpu_to_le16(NVME_SC_ABORT_REQ << 1);
 			if (blk_queue_dying(rq->q))
-				status |= cpu_to_le16(NVME_SC_DNR);
+				status |= cpu_to_le16(NVME_SC_DNR << 1);
 		}
 		nvme_end_request(rq, status, result);
 	} else
@@ -1751,9 +1747,13 @@ nvme_fc_error_recovery(struct nvme_fc_ctrl *ctrl, char *errmsg)
 	dev_warn(ctrl->ctrl.device,
 		"NVME-FC{%d}: transport association error detected: %s\n",
 		ctrl->cnum, errmsg);
-	dev_info(ctrl->ctrl.device,
+	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 "
@@ -2191,9 +2191,6 @@ nvme_fc_create_io_queues(struct nvme_fc_ctrl *ctrl)
 	if (!opts->nr_io_queues)
 		return 0;
 
-	dev_info(ctrl->ctrl.device, "creating %d I/O queues.\n",
-			opts->nr_io_queues);
-
 	nvme_fc_init_io_queues(ctrl);
 
 	memset(&ctrl->tag_set, 0, sizeof(ctrl->tag_set));
@@ -2264,9 +2261,6 @@ nvme_fc_reinit_io_queues(struct nvme_fc_ctrl *ctrl)
 	if (ctrl->queue_count == 1)
 		return 0;
 
-	dev_info(ctrl->ctrl.device, "Recreating %d I/O queues.\n",
-			opts->nr_io_queues);
-
 	nvme_fc_init_io_queues(ctrl);
 
 	ret = blk_mq_reinit_tagset(&ctrl->tag_set);
@@ -2302,7 +2296,7 @@ nvme_fc_create_association(struct nvme_fc_ctrl *ctrl)
 	int ret;
 	bool changed;
 
-	ctrl->connect_attempts++;
+	++ctrl->ctrl.opts->nr_reconnects;
 
 	/*
 	 * Create the admin queue
@@ -2399,9 +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->connect_attempts = 0;
-
-	kref_get(&ctrl->ctrl.kref);
+	ctrl->ctrl.opts->nr_reconnects = 0;
 
 	if (ctrl->queue_count > 1) {
 		nvme_start_queues(&ctrl->ctrl);
@@ -2532,26 +2524,32 @@ nvme_fc_delete_ctrl_work(struct work_struct *work)
 
 	/*
 	 * tear down the controller
-	 * This will result in the last reference on the nvme ctrl to
-	 * expire, calling the transport nvme_fc_nvme_ctrl_freed() callback.
-	 * From there, the transport will tear down it's logical queues and
-	 * association.
+	 * After the last reference on the nvme ctrl is removed,
+	 * the transport nvme_fc_nvme_ctrl_freed() callback will be
+	 * invoked. From there, the transport will tear down it's
+	 * logical queues and association.
 	 */
 	nvme_uninit_ctrl(&ctrl->ctrl);
 
 	nvme_put_ctrl(&ctrl->ctrl);
 }
 
-static int
-__nvme_fc_del_ctrl(struct nvme_fc_ctrl *ctrl)
+static bool
+__nvme_fc_schedule_delete_work(struct nvme_fc_ctrl *ctrl)
 {
 	if (!nvme_change_ctrl_state(&ctrl->ctrl, NVME_CTRL_DELETING))
-		return -EBUSY;
+		return true;
 
 	if (!queue_work(nvme_fc_wq, &ctrl->delete_work))
-		return -EBUSY;
+		return true;
 
-	return 0;
+	return false;
+}
+
+static int
+__nvme_fc_del_ctrl(struct nvme_fc_ctrl *ctrl)
+{
+	return __nvme_fc_schedule_delete_work(ctrl) ? -EBUSY : 0;
 }
 
 /*
@@ -2577,6 +2575,35 @@ nvme_fc_del_nvme_ctrl(struct nvme_ctrl *nctrl)
 }
 
 static void
+nvme_fc_reconnect_or_delete(struct nvme_fc_ctrl *ctrl, int status)
+{
+	/* If we are resetting/deleting then do nothing */
+	if (ctrl->ctrl.state != NVME_CTRL_RECONNECTING) {
+		WARN_ON_ONCE(ctrl->ctrl.state == NVME_CTRL_NEW ||
+			ctrl->ctrl.state == NVME_CTRL_LIVE);
+		return;
+	}
+
+	dev_info(ctrl->ctrl.device,
+		"NVME-FC{%d}: reset: Reconnect attempt failed (%d)\n",
+		ctrl->cnum, status);
+
+	if (nvmf_should_reconnect(&ctrl->ctrl)) {
+		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,
+				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);
+		WARN_ON(__nvme_fc_schedule_delete_work(ctrl));
+	}
+}
+
+static void
 nvme_fc_reset_ctrl_work(struct work_struct *work)
 {
 	struct nvme_fc_ctrl *ctrl =
@@ -2587,34 +2614,9 @@ nvme_fc_reset_ctrl_work(struct work_struct *work)
 	nvme_fc_delete_association(ctrl);
 
 	ret = nvme_fc_create_association(ctrl);
-	if (ret) {
-		dev_warn(ctrl->ctrl.device,
-			"NVME-FC{%d}: reset: Reconnect attempt failed (%d)\n",
-			ctrl->cnum, ret);
-		if (ctrl->connect_attempts >= NVME_FC_MAX_CONNECT_ATTEMPTS) {
-			dev_warn(ctrl->ctrl.device,
-				"NVME-FC{%d}: Max reconnect attempts (%d) "
-				"reached. Removing controller\n",
-				ctrl->cnum, ctrl->connect_attempts);
-
-			if (!nvme_change_ctrl_state(&ctrl->ctrl,
-				NVME_CTRL_DELETING)) {
-				dev_err(ctrl->ctrl.device,
-					"NVME-FC{%d}: failed to change state "
-					"to DELETING\n", ctrl->cnum);
-				return;
-			}
-
-			WARN_ON(!queue_work(nvme_fc_wq, &ctrl->delete_work));
-			return;
-		}
-
-		dev_warn(ctrl->ctrl.device,
-			"NVME-FC{%d}: Reconnect attempt in %d seconds.\n",
-			ctrl->cnum, ctrl->reconnect_delay);
-		queue_delayed_work(nvme_fc_wq, &ctrl->connect_work,
-				ctrl->reconnect_delay * HZ);
-	} else
+	if (ret)
+		nvme_fc_reconnect_or_delete(ctrl, ret);
+	else
 		dev_info(ctrl->ctrl.device,
 			"NVME-FC{%d}: controller reset complete\n", ctrl->cnum);
 }
@@ -2628,7 +2630,7 @@ nvme_fc_reset_nvme_ctrl(struct nvme_ctrl *nctrl)
 {
 	struct nvme_fc_ctrl *ctrl = to_fc_ctrl(nctrl);
 
-	dev_warn(ctrl->ctrl.device,
+	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))
@@ -2645,7 +2647,7 @@ nvme_fc_reset_nvme_ctrl(struct nvme_ctrl *nctrl)
 static const struct nvme_ctrl_ops nvme_fc_ctrl_ops = {
 	.name			= "fc",
 	.module			= THIS_MODULE,
-	.is_fabrics		= true,
+	.flags			= NVME_F_FABRICS,
 	.reg_read32		= nvmf_reg_read32,
 	.reg_read64		= nvmf_reg_read64,
 	.reg_write32		= nvmf_reg_write32,
@@ -2667,34 +2669,9 @@ nvme_fc_connect_ctrl_work(struct work_struct *work)
 				struct nvme_fc_ctrl, connect_work);
 
 	ret = nvme_fc_create_association(ctrl);
-	if (ret) {
-		dev_warn(ctrl->ctrl.device,
-			"NVME-FC{%d}: Reconnect attempt failed (%d)\n",
-			ctrl->cnum, ret);
-		if (ctrl->connect_attempts >= NVME_FC_MAX_CONNECT_ATTEMPTS) {
-			dev_warn(ctrl->ctrl.device,
-				"NVME-FC{%d}: Max reconnect attempts (%d) "
-				"reached. Removing controller\n",
-				ctrl->cnum, ctrl->connect_attempts);
-
-			if (!nvme_change_ctrl_state(&ctrl->ctrl,
-				NVME_CTRL_DELETING)) {
-				dev_err(ctrl->ctrl.device,
-					"NVME-FC{%d}: failed to change state "
-					"to DELETING\n", ctrl->cnum);
-				return;
-			}
-
-			WARN_ON(!queue_work(nvme_fc_wq, &ctrl->delete_work));
-			return;
-		}
-
-		dev_warn(ctrl->ctrl.device,
-			"NVME-FC{%d}: Reconnect attempt in %d seconds.\n",
-			ctrl->cnum, ctrl->reconnect_delay);
-		queue_delayed_work(nvme_fc_wq, &ctrl->connect_work,
-				ctrl->reconnect_delay * HZ);
-	} else
+	if (ret)
+		nvme_fc_reconnect_or_delete(ctrl, ret);
+	else
 		dev_info(ctrl->ctrl.device,
 			"NVME-FC{%d}: controller reconnect complete\n",
 			ctrl->cnum);
@@ -2720,6 +2697,12 @@ nvme_fc_init_ctrl(struct device *dev, struct nvmf_ctrl_options *opts,
 	unsigned long flags;
 	int ret, idx;
 
+	if (!(rport->remoteport.port_role &
+	    (FC_PORT_ROLE_NVME_DISCOVERY | FC_PORT_ROLE_NVME_TARGET))) {
+		ret = -EBADR;
+		goto out_fail;
+	}
+
 	ctrl = kzalloc(sizeof(*ctrl), GFP_KERNEL);
 	if (!ctrl) {
 		ret = -ENOMEM;
@@ -2745,7 +2728,6 @@ nvme_fc_init_ctrl(struct device *dev, struct nvmf_ctrl_options *opts,
 	INIT_WORK(&ctrl->delete_work, nvme_fc_delete_ctrl_work);
 	INIT_WORK(&ctrl->reset_work, nvme_fc_reset_ctrl_work);
 	INIT_DELAYED_WORK(&ctrl->connect_work, nvme_fc_connect_ctrl_work);
-	ctrl->reconnect_delay = opts->reconnect_delay;
 	spin_lock_init(&ctrl->lock);
 
 	/* io queue count */
@@ -2809,7 +2791,6 @@ nvme_fc_init_ctrl(struct device *dev, struct nvmf_ctrl_options *opts,
 		ctrl->ctrl.opts = NULL;
 		/* initiate nvme ctrl ref counting teardown */
 		nvme_uninit_ctrl(&ctrl->ctrl);
-		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,
@@ -2825,6 +2806,8 @@ nvme_fc_init_ctrl(struct device *dev, struct nvmf_ctrl_options *opts,
 		return ERR_PTR(ret);
 	}
 
+	kref_get(&ctrl->ctrl.kref);
+
 	dev_info(ctrl->ctrl.device,
 		"NVME-FC{%d}: new ctrl: NQN \"%s\"\n",
 		ctrl->cnum, ctrl->ctrl.opts->subsysnqn);
@@ -2961,7 +2944,7 @@ nvme_fc_create_ctrl(struct device *dev, struct nvmf_ctrl_options *opts)
 static struct nvmf_transport_ops nvme_fc_transport = {
 	.name		= "fc",
 	.required_opts	= NVMF_OPT_TRADDR | NVMF_OPT_HOST_TRADDR,
-	.allowed_opts	= NVMF_OPT_RECONNECT_DELAY,
+	.allowed_opts	= NVMF_OPT_RECONNECT_DELAY | NVMF_OPT_CTRL_LOSS_TMO,
 	.create_ctrl	= nvme_fc_create_ctrl,
 };
 
diff --git a/drivers/nvme/host/nvme.h b/drivers/nvme/host/nvme.h
index 29c708ca9621..9d6a070d4391 100644
--- a/drivers/nvme/host/nvme.h
+++ b/drivers/nvme/host/nvme.h
@@ -208,7 +208,9 @@ struct nvme_ns {
 struct nvme_ctrl_ops {
 	const char *name;
 	struct module *module;
-	bool is_fabrics;
+	unsigned int flags;
+#define NVME_F_FABRICS			(1 << 0)
+#define NVME_F_METADATA_SUPPORTED	(1 << 1)
 	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);
diff --git a/drivers/nvme/host/pci.c b/drivers/nvme/host/pci.c
index fed803232edc..d52701df7245 100644
--- a/drivers/nvme/host/pci.c
+++ b/drivers/nvme/host/pci.c
@@ -263,7 +263,7 @@ static void nvme_dbbuf_set(struct nvme_dev *dev)
 	c.dbbuf.prp2 = cpu_to_le64(dev->dbbuf_eis_dma_addr);
 
 	if (nvme_submit_sync_cmd(dev->ctrl.admin_q, &c, NULL, 0)) {
-		dev_warn(dev->dev, "unable to set dbbuf\n");
+		dev_warn(dev->ctrl.device, "unable to set dbbuf\n");
 		/* Free memory and continue on */
 		nvme_dbbuf_dma_free(dev);
 	}
@@ -1394,11 +1394,11 @@ static void nvme_warn_reset(struct nvme_dev *dev, u32 csts)
 	result = pci_read_config_word(to_pci_dev(dev->dev), PCI_STATUS,
 				      &pci_status);
 	if (result == PCIBIOS_SUCCESSFUL)
-		dev_warn(dev->dev,
+		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->dev,
+		dev_warn(dev->ctrl.device,
 			 "controller is down; will reset: CSTS=0x%x, PCI_STATUS read failed (%d)\n",
 			 csts, result);
 }
@@ -1506,6 +1506,11 @@ static inline void nvme_release_cmb(struct nvme_dev *dev)
 	if (dev->cmb) {
 		iounmap(dev->cmb);
 		dev->cmb = NULL;
+		if (dev->cmbsz) {
+			sysfs_remove_file_from_group(&dev->ctrl.device->kobj,
+						     &dev_attr_cmb.attr, NULL);
+			dev->cmbsz = 0;
+		}
 	}
 }
 
@@ -1735,8 +1740,8 @@ static int nvme_pci_enable(struct nvme_dev *dev)
 	 */
 	if (pdev->vendor == PCI_VENDOR_ID_APPLE && pdev->device == 0x2001) {
 		dev->q_depth = 2;
-		dev_warn(dev->dev, "detected Apple NVMe controller, set "
-			"queue depth=%u to work around controller resets\n",
+		dev_warn(dev->ctrl.device, "detected Apple NVMe controller, "
+			"set queue depth=%u to work around controller resets\n",
 			dev->q_depth);
 	}
 
@@ -1754,7 +1759,7 @@ static int nvme_pci_enable(struct nvme_dev *dev)
 		if (dev->cmbsz) {
 			if (sysfs_add_file_to_group(&dev->ctrl.device->kobj,
 						    &dev_attr_cmb.attr, NULL))
-				dev_warn(dev->dev,
+				dev_warn(dev->ctrl.device,
 					 "failed to add sysfs attribute for CMB\n");
 		}
 	}
@@ -1779,6 +1784,7 @@ static void nvme_pci_disable(struct nvme_dev *dev)
 {
 	struct pci_dev *pdev = to_pci_dev(dev->dev);
 
+	nvme_release_cmb(dev);
 	pci_free_irq_vectors(pdev);
 
 	if (pci_is_enabled(pdev)) {
@@ -2041,6 +2047,7 @@ static int nvme_pci_reset_ctrl(struct nvme_ctrl *ctrl)
 static const struct nvme_ctrl_ops nvme_pci_ctrl_ops = {
 	.name			= "pcie",
 	.module			= THIS_MODULE,
+	.flags			= NVME_F_METADATA_SUPPORTED,
 	.reg_read32		= nvme_pci_reg_read32,
 	.reg_write32		= nvme_pci_reg_write32,
 	.reg_read64		= nvme_pci_reg_read64,
@@ -2184,7 +2191,6 @@ static void nvme_remove(struct pci_dev *pdev)
 	nvme_dev_disable(dev, true);
 	nvme_dev_remove_admin(dev);
 	nvme_free_queues(dev, 0);
-	nvme_release_cmb(dev);
 	nvme_release_prp_pools(dev);
 	nvme_dev_unmap(dev);
 	nvme_put_ctrl(&dev->ctrl);
@@ -2288,6 +2294,8 @@ static const struct pci_device_id nvme_id_table[] = {
 	{ PCI_VDEVICE(INTEL, 0x0a54),
 		.driver_data = NVME_QUIRK_STRIPE_SIZE |
 				NVME_QUIRK_DEALLOCATE_ZEROES, },
+	{ PCI_VDEVICE(INTEL, 0xf1a5),	/* Intel 600P/P3100 */
+		.driver_data = NVME_QUIRK_NO_DEEPEST_PS },
 	{ PCI_VDEVICE(INTEL, 0x5845),	/* Qemu emulated controller */
 		.driver_data = NVME_QUIRK_IDENTIFY_CNS, },
 	{ PCI_DEVICE(0x1c58, 0x0003),	/* HGST adapter */
diff --git a/drivers/nvme/host/rdma.c b/drivers/nvme/host/rdma.c
index dd1c6deef82f..28bd255c144d 100644
--- a/drivers/nvme/host/rdma.c
+++ b/drivers/nvme/host/rdma.c
@@ -1038,6 +1038,19 @@ static void nvme_rdma_send_done(struct ib_cq *cq, struct ib_wc *wc)
 		nvme_rdma_wr_error(cq, wc, "SEND");
 }
 
+static inline int nvme_rdma_queue_sig_limit(struct nvme_rdma_queue *queue)
+{
+	int sig_limit;
+
+	/*
+	 * We signal completion every queue depth/2 and also handle the
+	 * degenerated case of a  device with queue_depth=1, where we
+	 * would need to signal every message.
+	 */
+	sig_limit = max(queue->queue_size / 2, 1);
+	return (++queue->sig_count % sig_limit) == 0;
+}
+
 static int nvme_rdma_post_send(struct nvme_rdma_queue *queue,
 		struct nvme_rdma_qe *qe, struct ib_sge *sge, u32 num_sge,
 		struct ib_send_wr *first, bool flush)
@@ -1065,9 +1078,6 @@ static int nvme_rdma_post_send(struct nvme_rdma_queue *queue,
 	 * Would have been way to obvious to handle this in hardware or
 	 * at least the RDMA stack..
 	 *
-	 * This messy and racy code sniplet is copy and pasted from the iSER
-	 * initiator, and the magic '32' comes from there as well.
-	 *
 	 * Always signal the flushes. The magic request used for the flush
 	 * sequencer is not allocated in our driver's tagset and it's
 	 * triggered to be freed by blk_cleanup_queue(). So we need to
@@ -1075,7 +1085,7 @@ static int nvme_rdma_post_send(struct nvme_rdma_queue *queue,
 	 * embedded in request's payload, is not freed when __ib_process_cq()
 	 * calls wr_cqe->done().
 	 */
-	if ((++queue->sig_count % 32) == 0 || flush)
+	if (nvme_rdma_queue_sig_limit(queue) || flush)
 		wr.send_flags |= IB_SEND_SIGNALED;
 
 	if (first)
@@ -1782,7 +1792,7 @@ static int nvme_rdma_reset_ctrl(struct nvme_ctrl *nctrl)
 static const struct nvme_ctrl_ops nvme_rdma_ctrl_ops = {
 	.name			= "rdma",
 	.module			= THIS_MODULE,
-	.is_fabrics		= true,
+	.flags			= NVME_F_FABRICS,
 	.reg_read32		= nvmf_reg_read32,
 	.reg_read64		= nvmf_reg_read64,
 	.reg_write32		= nvmf_reg_write32,
diff --git a/drivers/nvme/target/core.c b/drivers/nvme/target/core.c
index cf90713043da..eb9399ac97cf 100644
--- a/drivers/nvme/target/core.c
+++ b/drivers/nvme/target/core.c
@@ -529,6 +529,12 @@ fail:
 }
 EXPORT_SYMBOL_GPL(nvmet_req_init);
 
+void nvmet_req_uninit(struct nvmet_req *req)
+{
+	percpu_ref_put(&req->sq->ref);
+}
+EXPORT_SYMBOL_GPL(nvmet_req_uninit);
+
 static inline bool nvmet_cc_en(u32 cc)
 {
 	return cc & 0x1;
diff --git a/drivers/nvme/target/fc.c b/drivers/nvme/target/fc.c
index 62eba29c85fb..2006fae61980 100644
--- a/drivers/nvme/target/fc.c
+++ b/drivers/nvme/target/fc.c
@@ -517,9 +517,7 @@ nvmet_fc_queue_to_cpu(struct nvmet_fc_tgtport *tgtport, int qid)
 {
 	int cpu, idx, cnt;
 
-	if (!(tgtport->ops->target_features &
-			NVMET_FCTGTFEAT_NEEDS_CMD_CPUSCHED) ||
-	    tgtport->ops->max_hw_queues == 1)
+	if (tgtport->ops->max_hw_queues == 1)
 		return WORK_CPU_UNBOUND;
 
 	/* Simple cpu selection based on qid modulo active cpu count */
diff --git a/drivers/nvme/target/fcloop.c b/drivers/nvme/target/fcloop.c
index 15551ef79c8c..294a6611fb24 100644
--- a/drivers/nvme/target/fcloop.c
+++ b/drivers/nvme/target/fcloop.c
@@ -698,7 +698,6 @@ static struct nvmet_fc_target_template tgttemplate = {
 	.dma_boundary		= FCLOOP_DMABOUND_4G,
 	/* optional features */
 	.target_features	= NVMET_FCTGTFEAT_CMD_IN_ISR |
-				  NVMET_FCTGTFEAT_NEEDS_CMD_CPUSCHED |
 				  NVMET_FCTGTFEAT_OPDONE_IN_ISR,
 	/* sizes of additional private data for data structures */
 	.target_priv_sz		= sizeof(struct fcloop_tport),
diff --git a/drivers/nvme/target/loop.c b/drivers/nvme/target/loop.c
index feb497134aee..e503cfff0337 100644
--- a/drivers/nvme/target/loop.c
+++ b/drivers/nvme/target/loop.c
@@ -558,7 +558,7 @@ static int nvme_loop_reset_ctrl(struct nvme_ctrl *nctrl)
 static const struct nvme_ctrl_ops nvme_loop_ctrl_ops = {
 	.name			= "loop",
 	.module			= THIS_MODULE,
-	.is_fabrics		= true,
+	.flags			= NVME_F_FABRICS,
 	.reg_read32		= nvmf_reg_read32,
 	.reg_read64		= nvmf_reg_read64,
 	.reg_write32		= nvmf_reg_write32,
diff --git a/drivers/nvme/target/nvmet.h b/drivers/nvme/target/nvmet.h
index 7cb77ba5993b..cfc5c7fb0ab7 100644
--- a/drivers/nvme/target/nvmet.h
+++ b/drivers/nvme/target/nvmet.h
@@ -261,6 +261,7 @@ u16 nvmet_parse_fabrics_cmd(struct nvmet_req *req);
 
 bool nvmet_req_init(struct nvmet_req *req, struct nvmet_cq *cq,
 		struct nvmet_sq *sq, struct nvmet_fabrics_ops *ops);
+void nvmet_req_uninit(struct nvmet_req *req);
 void nvmet_req_complete(struct nvmet_req *req, u16 status);
 
 void nvmet_cq_setup(struct nvmet_ctrl *ctrl, struct nvmet_cq *cq, u16 qid,
diff --git a/drivers/nvme/target/rdma.c b/drivers/nvme/target/rdma.c
index 99c69018a35f..9e45cde63376 100644
--- a/drivers/nvme/target/rdma.c
+++ b/drivers/nvme/target/rdma.c
@@ -567,6 +567,7 @@ static void nvmet_rdma_read_data_done(struct ib_cq *cq, struct ib_wc *wc)
 	rsp->n_rdma = 0;
 
 	if (unlikely(wc->status != IB_WC_SUCCESS)) {
+		nvmet_req_uninit(&rsp->req);
 		nvmet_rdma_release_rsp(rsp);
 		if (wc->status != IB_WC_WR_FLUSH_ERR) {
 			pr_info("RDMA READ for CQE 0x%p failed with status %s (%d).\n",
diff --git a/drivers/of/fdt.c b/drivers/of/fdt.c
index 3080d9dd031d..43bd69dceabf 100644
--- a/drivers/of/fdt.c
+++ b/drivers/of/fdt.c
@@ -507,6 +507,9 @@ void *__unflatten_device_tree(const void *blob,
 
 	/* Allocate memory for the expanded device tree */
 	mem = dt_alloc(size + 4, __alignof__(struct device_node));
+	if (!mem)
+		return NULL;
+
 	memset(mem, 0, size);
 
 	*(__be32 *)(mem + size) = cpu_to_be32(0xdeadbeef);
diff --git a/drivers/of/of_reserved_mem.c b/drivers/of/of_reserved_mem.c
index 4dec07ea510f..d507c3569a88 100644
--- a/drivers/of/of_reserved_mem.c
+++ b/drivers/of/of_reserved_mem.c
@@ -197,7 +197,7 @@ static int __init __reserved_mem_init_node(struct reserved_mem *rmem)
 	const struct of_device_id *i;
 
 	for (i = __reservedmem_of_table; i < &__rmem_of_table_sentinel; i++) {
-		int const (*initfn)(struct reserved_mem *rmem) = i->data;
+		reservedmem_of_init_fn initfn = i->data;
 		const char *compat = i->compatible;
 
 		if (!of_flat_dt_is_compatible(rmem->fdt_node, compat))
diff --git a/drivers/of/platform.c b/drivers/of/platform.c
index 71fecc2debfc..703a42118ffc 100644
--- a/drivers/of/platform.c
+++ b/drivers/of/platform.c
@@ -523,7 +523,7 @@ static int __init of_platform_default_populate_init(void)
 arch_initcall_sync(of_platform_default_populate_init);
 #endif
 
-static int of_platform_device_destroy(struct device *dev, void *data)
+int of_platform_device_destroy(struct device *dev, void *data)
 {
 	/* Do not touch devices not populated from the device tree */
 	if (!dev->of_node || !of_node_check_flag(dev->of_node, OF_POPULATED))
@@ -544,6 +544,7 @@ static int of_platform_device_destroy(struct device *dev, void *data)
 	of_node_clear_flag(dev->of_node, OF_POPULATED_BUS);
 	return 0;
 }
+EXPORT_SYMBOL_GPL(of_platform_device_destroy);
 
 /**
  * of_platform_depopulate() - Remove devices populated from device tree
diff --git a/drivers/pci/access.c b/drivers/pci/access.c
index 74cf5fffb1e1..c80e37a69305 100644
--- a/drivers/pci/access.c
+++ b/drivers/pci/access.c
@@ -896,7 +896,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 +906,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 +917,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 +926,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 +934,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 +943,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/dwc/Kconfig b/drivers/pci/dwc/Kconfig
index b7e15526d676..d34b6dfbb351 100644
--- a/drivers/pci/dwc/Kconfig
+++ b/drivers/pci/dwc/Kconfig
@@ -16,6 +16,7 @@ config PCIE_DW_EP
 
 config PCI_DRA7XX
 	bool "TI DRA7xx PCIe controller"
+	depends on SOC_DRA7XX || COMPILE_TEST
 	depends on (PCI && PCI_MSI_IRQ_DOMAIN) || PCI_ENDPOINT
 	depends on OF && HAS_IOMEM && TI_PIPE3
 	help
diff --git a/drivers/pci/dwc/pci-imx6.c b/drivers/pci/dwc/pci-imx6.c
index a98cba55c7f0..19a289b8cc94 100644
--- a/drivers/pci/dwc/pci-imx6.c
+++ b/drivers/pci/dwc/pci-imx6.c
@@ -252,7 +252,34 @@ static void imx6_pcie_reset_phy(struct imx6_pcie *imx6_pcie)
 static int imx6q_pcie_abort_handler(unsigned long addr,
 		unsigned int fsr, struct pt_regs *regs)
 {
-	return 0;
+	unsigned long pc = instruction_pointer(regs);
+	unsigned long instr = *(unsigned long *)pc;
+	int reg = (instr >> 12) & 15;
+
+	/*
+	 * If the instruction being executed was a read,
+	 * make it look like it read all-ones.
+	 */
+	if ((instr & 0x0c100000) == 0x04100000) {
+		unsigned long val;
+
+		if (instr & 0x00400000)
+			val = 255;
+		else
+			val = -1;
+
+		regs->uregs[reg] = val;
+		regs->ARM_pc += 4;
+		return 0;
+	}
+
+	if ((instr & 0x0e100090) == 0x00100090) {
+		regs->uregs[reg] = -1;
+		regs->ARM_pc += 4;
+		return 0;
+	}
+
+	return 1;
 }
 
 static void imx6_pcie_assert_core_reset(struct imx6_pcie *imx6_pcie)
@@ -819,8 +846,8 @@ static int __init imx6_pcie_init(void)
 	 * we can install the handler here without risking it
 	 * accessing some uninitialized driver state.
 	 */
-	hook_fault_code(16 + 6, imx6q_pcie_abort_handler, SIGBUS, 0,
-			"imprecise external abort");
+	hook_fault_code(8, imx6q_pcie_abort_handler, SIGBUS, 0,
+			"external abort on non-linefetch");
 
 	return platform_driver_register(&imx6_pcie_driver);
 }
diff --git a/drivers/pci/dwc/pcie-qcom.c b/drivers/pci/dwc/pcie-qcom.c
index 5bf23d432fdb..289cda1b4897 100644
--- a/drivers/pci/dwc/pcie-qcom.c
+++ b/drivers/pci/dwc/pcie-qcom.c
@@ -86,10 +86,29 @@ struct qcom_pcie_resources_v2 {
 	struct clk *pipe_clk;
 };
 
+struct qcom_pcie_resources_v3 {
+	struct clk *aux_clk;
+	struct clk *master_clk;
+	struct clk *slave_clk;
+	struct reset_control *axi_m_reset;
+	struct reset_control *axi_s_reset;
+	struct reset_control *pipe_reset;
+	struct reset_control *axi_m_vmid_reset;
+	struct reset_control *axi_s_xpu_reset;
+	struct reset_control *parf_reset;
+	struct reset_control *phy_reset;
+	struct reset_control *axi_m_sticky_reset;
+	struct reset_control *pipe_sticky_reset;
+	struct reset_control *pwr_reset;
+	struct reset_control *ahb_reset;
+	struct reset_control *phy_ahb_reset;
+};
+
 union qcom_pcie_resources {
 	struct qcom_pcie_resources_v0 v0;
 	struct qcom_pcie_resources_v1 v1;
 	struct qcom_pcie_resources_v2 v2;
+	struct qcom_pcie_resources_v3 v3;
 };
 
 struct qcom_pcie;
@@ -133,26 +152,6 @@ static irqreturn_t qcom_pcie_msi_irq_handler(int irq, void *arg)
 	return dw_handle_msi_irq(pp);
 }
 
-static void qcom_pcie_v0_v1_ltssm_enable(struct qcom_pcie *pcie)
-{
-	u32 val;
-
-	/* enable link training */
-	val = readl(pcie->elbi + PCIE20_ELBI_SYS_CTRL);
-	val |= PCIE20_ELBI_SYS_CTRL_LT_ENABLE;
-	writel(val, pcie->elbi + PCIE20_ELBI_SYS_CTRL);
-}
-
-static void qcom_pcie_v2_ltssm_enable(struct qcom_pcie *pcie)
-{
-	u32 val;
-
-	/* enable link training */
-	val = readl(pcie->parf + PCIE20_PARF_LTSSM);
-	val |= BIT(8);
-	writel(val, pcie->parf + PCIE20_PARF_LTSSM);
-}
-
 static int qcom_pcie_establish_link(struct qcom_pcie *pcie)
 {
 	struct dw_pcie *pci = pcie->pci;
@@ -167,6 +166,16 @@ static int qcom_pcie_establish_link(struct qcom_pcie *pcie)
 	return dw_pcie_wait_for_link(pci);
 }
 
+static void qcom_pcie_v0_v1_ltssm_enable(struct qcom_pcie *pcie)
+{
+	u32 val;
+
+	/* enable link training */
+	val = readl(pcie->elbi + PCIE20_ELBI_SYS_CTRL);
+	val |= PCIE20_ELBI_SYS_CTRL_LT_ENABLE;
+	writel(val, pcie->elbi + PCIE20_ELBI_SYS_CTRL);
+}
+
 static int qcom_pcie_get_resources_v0(struct qcom_pcie *pcie)
 {
 	struct qcom_pcie_resources_v0 *res = &pcie->res.v0;
@@ -217,36 +226,6 @@ static int qcom_pcie_get_resources_v0(struct qcom_pcie *pcie)
 	return PTR_ERR_OR_ZERO(res->phy_reset);
 }
 
-static int qcom_pcie_get_resources_v1(struct qcom_pcie *pcie)
-{
-	struct qcom_pcie_resources_v1 *res = &pcie->res.v1;
-	struct dw_pcie *pci = pcie->pci;
-	struct device *dev = pci->dev;
-
-	res->vdda = devm_regulator_get(dev, "vdda");
-	if (IS_ERR(res->vdda))
-		return PTR_ERR(res->vdda);
-
-	res->iface = devm_clk_get(dev, "iface");
-	if (IS_ERR(res->iface))
-		return PTR_ERR(res->iface);
-
-	res->aux = devm_clk_get(dev, "aux");
-	if (IS_ERR(res->aux))
-		return PTR_ERR(res->aux);
-
-	res->master_bus = devm_clk_get(dev, "master_bus");
-	if (IS_ERR(res->master_bus))
-		return PTR_ERR(res->master_bus);
-
-	res->slave_bus = devm_clk_get(dev, "slave_bus");
-	if (IS_ERR(res->slave_bus))
-		return PTR_ERR(res->slave_bus);
-
-	res->core = devm_reset_control_get(dev, "core");
-	return PTR_ERR_OR_ZERO(res->core);
-}
-
 static void qcom_pcie_deinit_v0(struct qcom_pcie *pcie)
 {
 	struct qcom_pcie_resources_v0 *res = &pcie->res.v0;
@@ -375,6 +354,36 @@ err_refclk:
 	return ret;
 }
 
+static int qcom_pcie_get_resources_v1(struct qcom_pcie *pcie)
+{
+	struct qcom_pcie_resources_v1 *res = &pcie->res.v1;
+	struct dw_pcie *pci = pcie->pci;
+	struct device *dev = pci->dev;
+
+	res->vdda = devm_regulator_get(dev, "vdda");
+	if (IS_ERR(res->vdda))
+		return PTR_ERR(res->vdda);
+
+	res->iface = devm_clk_get(dev, "iface");
+	if (IS_ERR(res->iface))
+		return PTR_ERR(res->iface);
+
+	res->aux = devm_clk_get(dev, "aux");
+	if (IS_ERR(res->aux))
+		return PTR_ERR(res->aux);
+
+	res->master_bus = devm_clk_get(dev, "master_bus");
+	if (IS_ERR(res->master_bus))
+		return PTR_ERR(res->master_bus);
+
+	res->slave_bus = devm_clk_get(dev, "slave_bus");
+	if (IS_ERR(res->slave_bus))
+		return PTR_ERR(res->slave_bus);
+
+	res->core = devm_reset_control_get(dev, "core");
+	return PTR_ERR_OR_ZERO(res->core);
+}
+
 static void qcom_pcie_deinit_v1(struct qcom_pcie *pcie)
 {
 	struct qcom_pcie_resources_v1 *res = &pcie->res.v1;
@@ -455,6 +464,16 @@ err_res:
 	return ret;
 }
 
+static void qcom_pcie_v2_ltssm_enable(struct qcom_pcie *pcie)
+{
+	u32 val;
+
+	/* enable link training */
+	val = readl(pcie->parf + PCIE20_PARF_LTSSM);
+	val |= BIT(8);
+	writel(val, pcie->parf + PCIE20_PARF_LTSSM);
+}
+
 static int qcom_pcie_get_resources_v2(struct qcom_pcie *pcie)
 {
 	struct qcom_pcie_resources_v2 *res = &pcie->res.v2;
@@ -481,6 +500,17 @@ static int qcom_pcie_get_resources_v2(struct qcom_pcie *pcie)
 	return PTR_ERR_OR_ZERO(res->pipe_clk);
 }
 
+static void qcom_pcie_deinit_v2(struct qcom_pcie *pcie)
+{
+	struct qcom_pcie_resources_v2 *res = &pcie->res.v2;
+
+	clk_disable_unprepare(res->pipe_clk);
+	clk_disable_unprepare(res->slave_clk);
+	clk_disable_unprepare(res->master_clk);
+	clk_disable_unprepare(res->cfg_clk);
+	clk_disable_unprepare(res->aux_clk);
+}
+
 static int qcom_pcie_init_v2(struct qcom_pcie *pcie)
 {
 	struct qcom_pcie_resources_v2 *res = &pcie->res.v2;
@@ -562,22 +592,290 @@ static int qcom_pcie_post_init_v2(struct qcom_pcie *pcie)
 	return 0;
 }
 
-static int qcom_pcie_link_up(struct dw_pcie *pci)
+static int qcom_pcie_get_resources_v3(struct qcom_pcie *pcie)
 {
-	u16 val = readw(pci->dbi_base + PCIE20_CAP + PCI_EXP_LNKSTA);
+	struct qcom_pcie_resources_v3 *res = &pcie->res.v3;
+	struct dw_pcie *pci = pcie->pci;
+	struct device *dev = pci->dev;
 
-	return !!(val & PCI_EXP_LNKSTA_DLLLA);
+	res->aux_clk = devm_clk_get(dev, "aux");
+	if (IS_ERR(res->aux_clk))
+		return PTR_ERR(res->aux_clk);
+
+	res->master_clk = devm_clk_get(dev, "master_bus");
+	if (IS_ERR(res->master_clk))
+		return PTR_ERR(res->master_clk);
+
+	res->slave_clk = devm_clk_get(dev, "slave_bus");
+	if (IS_ERR(res->slave_clk))
+		return PTR_ERR(res->slave_clk);
+
+	res->axi_m_reset = devm_reset_control_get(dev, "axi_m");
+	if (IS_ERR(res->axi_m_reset))
+		return PTR_ERR(res->axi_m_reset);
+
+	res->axi_s_reset = devm_reset_control_get(dev, "axi_s");
+	if (IS_ERR(res->axi_s_reset))
+		return PTR_ERR(res->axi_s_reset);
+
+	res->pipe_reset = devm_reset_control_get(dev, "pipe");
+	if (IS_ERR(res->pipe_reset))
+		return PTR_ERR(res->pipe_reset);
+
+	res->axi_m_vmid_reset = devm_reset_control_get(dev, "axi_m_vmid");
+	if (IS_ERR(res->axi_m_vmid_reset))
+		return PTR_ERR(res->axi_m_vmid_reset);
+
+	res->axi_s_xpu_reset = devm_reset_control_get(dev, "axi_s_xpu");
+	if (IS_ERR(res->axi_s_xpu_reset))
+		return PTR_ERR(res->axi_s_xpu_reset);
+
+	res->parf_reset = devm_reset_control_get(dev, "parf");
+	if (IS_ERR(res->parf_reset))
+		return PTR_ERR(res->parf_reset);
+
+	res->phy_reset = devm_reset_control_get(dev, "phy");
+	if (IS_ERR(res->phy_reset))
+		return PTR_ERR(res->phy_reset);
+
+	res->axi_m_sticky_reset = devm_reset_control_get(dev, "axi_m_sticky");
+	if (IS_ERR(res->axi_m_sticky_reset))
+		return PTR_ERR(res->axi_m_sticky_reset);
+
+	res->pipe_sticky_reset = devm_reset_control_get(dev, "pipe_sticky");
+	if (IS_ERR(res->pipe_sticky_reset))
+		return PTR_ERR(res->pipe_sticky_reset);
+
+	res->pwr_reset = devm_reset_control_get(dev, "pwr");
+	if (IS_ERR(res->pwr_reset))
+		return PTR_ERR(res->pwr_reset);
+
+	res->ahb_reset = devm_reset_control_get(dev, "ahb");
+	if (IS_ERR(res->ahb_reset))
+		return PTR_ERR(res->ahb_reset);
+
+	res->phy_ahb_reset = devm_reset_control_get(dev, "phy_ahb");
+	if (IS_ERR(res->phy_ahb_reset))
+		return PTR_ERR(res->phy_ahb_reset);
+
+	return 0;
 }
 
-static void qcom_pcie_deinit_v2(struct qcom_pcie *pcie)
+static void qcom_pcie_deinit_v3(struct qcom_pcie *pcie)
 {
-	struct qcom_pcie_resources_v2 *res = &pcie->res.v2;
-
-	clk_disable_unprepare(res->pipe_clk);
+	struct qcom_pcie_resources_v3 *res = &pcie->res.v3;
+
+	reset_control_assert(res->axi_m_reset);
+	reset_control_assert(res->axi_s_reset);
+	reset_control_assert(res->pipe_reset);
+	reset_control_assert(res->pipe_sticky_reset);
+	reset_control_assert(res->phy_reset);
+	reset_control_assert(res->phy_ahb_reset);
+	reset_control_assert(res->axi_m_sticky_reset);
+	reset_control_assert(res->pwr_reset);
+	reset_control_assert(res->ahb_reset);
+	clk_disable_unprepare(res->aux_clk);
+	clk_disable_unprepare(res->master_clk);
 	clk_disable_unprepare(res->slave_clk);
+}
+
+static int qcom_pcie_init_v3(struct qcom_pcie *pcie)
+{
+	struct qcom_pcie_resources_v3 *res = &pcie->res.v3;
+	struct dw_pcie *pci = pcie->pci;
+	struct device *dev = pci->dev;
+	u32 val;
+	int ret;
+
+	ret = reset_control_assert(res->axi_m_reset);
+	if (ret) {
+		dev_err(dev, "cannot assert axi master reset\n");
+		return ret;
+	}
+
+	ret = reset_control_assert(res->axi_s_reset);
+	if (ret) {
+		dev_err(dev, "cannot asser axi slave reset\n");
+		return ret;
+	}
+
+	usleep_range(10000, 12000);
+
+	ret = reset_control_assert(res->pipe_reset);
+	if (ret) {
+		dev_err(dev, "cannot assert pipe reset\n");
+		return ret;
+	}
+
+	ret = reset_control_assert(res->pipe_sticky_reset);
+	if (ret) {
+		dev_err(dev, "cannot assert pipe sticky reset\n");
+		return ret;
+	}
+
+	ret = reset_control_assert(res->phy_reset);
+	if (ret) {
+		dev_err(dev, "cannot assert phy reset\n");
+		return ret;
+	}
+
+	ret = reset_control_assert(res->phy_ahb_reset);
+	if (ret) {
+		dev_err(dev, "cannot assert phy ahb reset\n");
+		return ret;
+	}
+
+	usleep_range(10000, 12000);
+
+	ret = reset_control_assert(res->axi_m_sticky_reset);
+	if (ret) {
+		dev_err(dev, "cannot assert axi master sticky reset\n");
+		return ret;
+	}
+
+	ret = reset_control_assert(res->pwr_reset);
+	if (ret) {
+		dev_err(dev, "cannot assert power reset\n");
+		return ret;
+	}
+
+	ret = reset_control_assert(res->ahb_reset);
+	if (ret) {
+		dev_err(dev, "cannot assert ahb reset\n");
+		return ret;
+	}
+
+	usleep_range(10000, 12000);
+
+	ret = reset_control_deassert(res->phy_ahb_reset);
+	if (ret) {
+		dev_err(dev, "cannot deassert phy ahb reset\n");
+		return ret;
+	}
+
+	ret = reset_control_deassert(res->phy_reset);
+	if (ret) {
+		dev_err(dev, "cannot deassert phy reset\n");
+		goto err_rst_phy;
+	}
+
+	ret = reset_control_deassert(res->pipe_reset);
+	if (ret) {
+		dev_err(dev, "cannot deassert pipe reset\n");
+		goto err_rst_pipe;
+	}
+
+	ret = reset_control_deassert(res->pipe_sticky_reset);
+	if (ret) {
+		dev_err(dev, "cannot deassert pipe sticky reset\n");
+		goto err_rst_pipe_sticky;
+	}
+
+	usleep_range(10000, 12000);
+
+	ret = reset_control_deassert(res->axi_m_reset);
+	if (ret) {
+		dev_err(dev, "cannot deassert axi master reset\n");
+		goto err_rst_axi_m;
+	}
+
+	ret = reset_control_deassert(res->axi_m_sticky_reset);
+	if (ret) {
+		dev_err(dev, "cannot deassert axi master sticky reset\n");
+		goto err_rst_axi_m_sticky;
+	}
+
+	ret = reset_control_deassert(res->axi_s_reset);
+	if (ret) {
+		dev_err(dev, "cannot deassert axi slave reset\n");
+		goto err_rst_axi_s;
+	}
+
+	ret = reset_control_deassert(res->pwr_reset);
+	if (ret) {
+		dev_err(dev, "cannot deassert power reset\n");
+		goto err_rst_pwr;
+	}
+
+	ret = reset_control_deassert(res->ahb_reset);
+	if (ret) {
+		dev_err(dev, "cannot deassert ahb reset\n");
+		goto err_rst_ahb;
+	}
+
+	usleep_range(10000, 12000);
+
+	ret = clk_prepare_enable(res->aux_clk);
+	if (ret) {
+		dev_err(dev, "cannot prepare/enable iface clock\n");
+		goto err_clk_aux;
+	}
+
+	ret = clk_prepare_enable(res->master_clk);
+	if (ret) {
+		dev_err(dev, "cannot prepare/enable core clock\n");
+		goto err_clk_axi_m;
+	}
+
+	ret = clk_prepare_enable(res->slave_clk);
+	if (ret) {
+		dev_err(dev, "cannot prepare/enable phy clock\n");
+		goto err_clk_axi_s;
+	}
+
+	/* enable PCIe clocks and resets */
+	val = readl(pcie->parf + PCIE20_PARF_PHY_CTRL);
+	val &= !BIT(0);
+	writel(val, pcie->parf + PCIE20_PARF_PHY_CTRL);
+
+	/* change DBI base address */
+	writel(0, pcie->parf + PCIE20_PARF_DBI_BASE_ADDR);
+
+	/* MAC PHY_POWERDOWN MUX DISABLE  */
+	val = readl(pcie->parf + PCIE20_PARF_SYS_CTRL);
+	val &= ~BIT(29);
+	writel(val, pcie->parf + PCIE20_PARF_SYS_CTRL);
+
+	val = readl(pcie->parf + PCIE20_PARF_MHI_CLOCK_RESET_CTRL);
+	val |= BIT(4);
+	writel(val, pcie->parf + PCIE20_PARF_MHI_CLOCK_RESET_CTRL);
+
+	val = readl(pcie->parf + PCIE20_PARF_AXI_MSTR_WR_ADDR_HALT_V2);
+	val |= BIT(31);
+	writel(val, pcie->parf + PCIE20_PARF_AXI_MSTR_WR_ADDR_HALT_V2);
+
+	return 0;
+
+err_clk_axi_s:
 	clk_disable_unprepare(res->master_clk);
-	clk_disable_unprepare(res->cfg_clk);
+err_clk_axi_m:
 	clk_disable_unprepare(res->aux_clk);
+err_clk_aux:
+	reset_control_assert(res->ahb_reset);
+err_rst_ahb:
+	reset_control_assert(res->pwr_reset);
+err_rst_pwr:
+	reset_control_assert(res->axi_s_reset);
+err_rst_axi_s:
+	reset_control_assert(res->axi_m_sticky_reset);
+err_rst_axi_m_sticky:
+	reset_control_assert(res->axi_m_reset);
+err_rst_axi_m:
+	reset_control_assert(res->pipe_sticky_reset);
+err_rst_pipe_sticky:
+	reset_control_assert(res->pipe_reset);
+err_rst_pipe:
+	reset_control_assert(res->phy_reset);
+err_rst_phy:
+	reset_control_assert(res->phy_ahb_reset);
+	return ret;
+}
+
+static int qcom_pcie_link_up(struct dw_pcie *pci)
+{
+	u16 val = readw(pci->dbi_base + PCIE20_CAP + PCI_EXP_LNKSTA);
+
+	return !!(val & PCI_EXP_LNKSTA_DLLLA);
 }
 
 static void qcom_pcie_host_init(struct pcie_port *pp)
@@ -665,6 +963,13 @@ static const struct dw_pcie_ops dw_pcie_ops = {
 	.link_up = qcom_pcie_link_up,
 };
 
+static const struct qcom_pcie_ops ops_v3 = {
+	.get_resources = qcom_pcie_get_resources_v3,
+	.init = qcom_pcie_init_v3,
+	.deinit = qcom_pcie_deinit_v3,
+	.ltssm_enable = qcom_pcie_v2_ltssm_enable,
+};
+
 static int qcom_pcie_probe(struct platform_device *pdev)
 {
 	struct device *dev = &pdev->dev;
@@ -754,6 +1059,7 @@ static const struct of_device_id qcom_pcie_match[] = {
 	{ .compatible = "qcom,pcie-apq8064", .data = &ops_v0 },
 	{ .compatible = "qcom,pcie-apq8084", .data = &ops_v1 },
 	{ .compatible = "qcom,pcie-msm8996", .data = &ops_v2 },
+	{ .compatible = "qcom,pcie-ipq4019", .data = &ops_v3 },
 	{ }
 };
 
diff --git a/drivers/pci/endpoint/Kconfig b/drivers/pci/endpoint/Kconfig
index c23f146fb5a6..c09623ca8c3b 100644
--- a/drivers/pci/endpoint/Kconfig
+++ b/drivers/pci/endpoint/Kconfig
@@ -6,6 +6,7 @@ menu "PCI Endpoint"
 
 config PCI_ENDPOINT
 	bool "PCI Endpoint Support"
+	depends on HAS_DMA
 	help
 	   Enable this configuration option to support configurable PCI
 	   endpoint. This should be enabled if the platform has a PCI
diff --git a/drivers/pci/host/Kconfig b/drivers/pci/host/Kconfig
index 7f47cd5e10a5..0cd5b30dccb1 100644
--- a/drivers/pci/host/Kconfig
+++ b/drivers/pci/host/Kconfig
@@ -180,6 +180,17 @@ config PCIE_ROCKCHIP
 	  There is 1 internal PCIe port available to support GEN2 with
 	  4 slots.
 
+config PCIE_MEDIATEK
+	bool "MediaTek PCIe controller"
+	depends on ARM && (ARCH_MEDIATEK || COMPILE_TEST)
+	depends on OF
+	depends on PCI
+	select PCIEPORTBUS
+	help
+	  Say Y here if you want to enable PCIe controller support on
+	  MT7623 series SoCs.  There is one single root complex with 3 root
+	  ports available.  Each port supports Gen2 lane x1.
+
 config VMD
 	depends on PCI_MSI && X86_64 && SRCU
 	tristate "Intel Volume Management Device Driver"
diff --git a/drivers/pci/host/Makefile b/drivers/pci/host/Makefile
index cab879578003..b10d104c85fd 100644
--- a/drivers/pci/host/Makefile
+++ b/drivers/pci/host/Makefile
@@ -18,6 +18,7 @@ obj-$(CONFIG_PCIE_IPROC_BCMA) += pcie-iproc-bcma.o
 obj-$(CONFIG_PCIE_ALTERA) += pcie-altera.o
 obj-$(CONFIG_PCIE_ALTERA_MSI) += pcie-altera-msi.o
 obj-$(CONFIG_PCIE_ROCKCHIP) += pcie-rockchip.o
+obj-$(CONFIG_PCIE_MEDIATEK) += pcie-mediatek.o
 obj-$(CONFIG_VMD) += vmd.o
 
 # The following drivers are for devices that use the generic ACPI
diff --git a/drivers/pci/host/pci-ftpci100.c b/drivers/pci/host/pci-ftpci100.c
index d26501c4145a..ebab33e99a0d 100644
--- a/drivers/pci/host/pci-ftpci100.c
+++ b/drivers/pci/host/pci-ftpci100.c
@@ -25,6 +25,7 @@
 #include <linux/irqchip/chained_irq.h>
 #include <linux/bitops.h>
 #include <linux/irq.h>
+#include <linux/clk.h>
 
 /*
  * Special configuration registers directly in the first few words
@@ -37,6 +38,7 @@
 #define PCI_CONFIG	0x28 /* PCI configuration command register */
 #define PCI_DATA	0x2C
 
+#define FARADAY_PCI_STATUS_CMD		0x04 /* Status and command */
 #define FARADAY_PCI_PMC			0x40 /* Power management control */
 #define FARADAY_PCI_PMCSR		0x44 /* Power management status */
 #define FARADAY_PCI_CTRL1		0x48 /* Control register 1 */
@@ -45,6 +47,8 @@
 #define FARADAY_PCI_MEM2_BASE_SIZE	0x54 /* Memory base and size #2 */
 #define FARADAY_PCI_MEM3_BASE_SIZE	0x58 /* Memory base and size #3 */
 
+#define PCI_STATUS_66MHZ_CAPABLE	BIT(21)
+
 /* Bits 31..28 gives INTD..INTA status */
 #define PCI_CTRL2_INTSTS_SHIFT		28
 #define PCI_CTRL2_INTMASK_CMDERR	BIT(27)
@@ -117,6 +121,7 @@ struct faraday_pci {
 	void __iomem *base;
 	struct irq_domain *irqdomain;
 	struct pci_bus *bus;
+	struct clk *bus_clk;
 };
 
 static int faraday_res_to_memcfg(resource_size_t mem_base,
@@ -428,6 +433,7 @@ static int faraday_pci_probe(struct platform_device *pdev)
 	struct resource *mem;
 	struct resource *io;
 	struct pci_host_bridge *host;
+	struct clk *clk;
 	int ret;
 	u32 val;
 	LIST_HEAD(res);
@@ -444,6 +450,28 @@ static int faraday_pci_probe(struct platform_device *pdev)
 	host->sysdata = p;
 	p->dev = dev;
 
+	/* Retrieve and enable optional clocks */
+	clk = devm_clk_get(dev, "PCLK");
+	if (IS_ERR(clk)) {
+		dev_err(dev, "no PCLK available\n");
+	} else {
+		ret = clk_prepare_enable(clk);
+		if (ret) {
+			dev_err(dev, "could not prepare PCLK\n");
+			return ret;
+		}
+	}
+	p->bus_clk = devm_clk_get(dev, "PCICLK");
+	if (IS_ERR(p->bus_clk)) {
+		dev_err(dev, "no PCICLK available\n");
+	} else {
+		ret = clk_prepare_enable(p->bus_clk);
+		if (ret) {
+			dev_err(dev, "could not prepare PCICLK\n");
+			return ret;
+		}
+	}
+
 	regs = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	p->base = devm_ioremap_resource(dev, regs);
 	if (IS_ERR(p->base))
@@ -515,6 +543,34 @@ static int faraday_pci_probe(struct platform_device *pdev)
 		}
 	}
 
+	/* Check bus clock if we can gear up to 66 MHz */
+	if (!IS_ERR(p->bus_clk)) {
+		unsigned long rate;
+		u32 val;
+
+		faraday_pci_read_config(p->bus, 0,
+					FARADAY_PCI_STATUS_CMD, 4, &val);
+		rate = clk_get_rate(p->bus_clk);
+
+		if ((rate == 33000000) && (val & PCI_STATUS_66MHZ_CAPABLE)) {
+			dev_info(dev, "33MHz bus is 66MHz capable\n");
+			p->bus->max_bus_speed = PCI_SPEED_66MHz;
+			ret = clk_set_rate(p->bus_clk, 66000000);
+			if (ret)
+				dev_err(dev, "failed to set bus clock\n");
+		} else {
+			dev_info(dev, "33MHz only bus\n");
+			p->bus->max_bus_speed = PCI_SPEED_33MHz;
+		}
+
+		/* Bumping the clock may fail so read back the rate */
+		rate = clk_get_rate(p->bus_clk);
+		if (rate == 33000000)
+			p->bus->cur_bus_speed = PCI_SPEED_33MHz;
+		if (rate == 66000000)
+			p->bus->cur_bus_speed = PCI_SPEED_66MHz;
+	}
+
 	ret = faraday_pci_parse_map_dma_ranges(p, dev->of_node);
 	if (ret)
 		return ret;
diff --git a/drivers/pci/host/pci-tegra.c b/drivers/pci/host/pci-tegra.c
index 2618f875a600..043c759d80f8 100644
--- a/drivers/pci/host/pci-tegra.c
+++ b/drivers/pci/host/pci-tegra.c
@@ -233,8 +233,8 @@ struct tegra_msi {
 	struct msi_controller chip;
 	DECLARE_BITMAP(used, INT_PCI_MSI_NR);
 	struct irq_domain *domain;
-	unsigned long pages;
 	struct mutex lock;
+	u64 phys;
 	int irq;
 };
 
@@ -1448,9 +1448,8 @@ static int tegra_msi_setup_irq(struct msi_controller *chip,
 
 	irq_set_msi_desc(irq, desc);
 
-	msg.address_lo = virt_to_phys((void *)msi->pages);
-	/* 32 bit address only */
-	msg.address_hi = 0;
+	msg.address_lo = lower_32_bits(msi->phys);
+	msg.address_hi = upper_32_bits(msi->phys);
 	msg.data = hwirq;
 
 	pci_write_msi_msg(irq, &msg);
@@ -1499,7 +1498,6 @@ static int tegra_pcie_enable_msi(struct tegra_pcie *pcie)
 	const struct tegra_pcie_soc *soc = pcie->soc;
 	struct tegra_msi *msi = &pcie->msi;
 	struct device *dev = pcie->dev;
-	unsigned long base;
 	int err;
 	u32 reg;
 
@@ -1531,12 +1529,25 @@ static int tegra_pcie_enable_msi(struct tegra_pcie *pcie)
 		goto err;
 	}
 
-	/* setup AFI/FPCI range */
-	msi->pages = __get_free_pages(GFP_KERNEL, 0);
-	base = virt_to_phys((void *)msi->pages);
+	/*
+	 * The PCI host bridge on Tegra contains some logic that intercepts
+	 * MSI writes, which means that the MSI target address doesn't have
+	 * to point to actual physical memory. Rather than allocating one 4
+	 * KiB page of system memory that's never used, we can simply pick
+	 * an arbitrary address within an area reserved for system memory
+	 * in the FPCI address map.
+	 *
+	 * However, in order to avoid confusion, we pick an address that
+	 * doesn't map to physical memory. The FPCI address map reserves a
+	 * 1012 GiB region for system memory and memory-mapped I/O. Since
+	 * none of the Tegra SoCs that contain this PCI host bridge can
+	 * address more than 16 GiB of system memory, the last 4 KiB of
+	 * these 1012 GiB is a good candidate.
+	 */
+	msi->phys = 0xfcfffff000;
 
-	afi_writel(pcie, base >> soc->msi_base_shift, AFI_MSI_FPCI_BAR_ST);
-	afi_writel(pcie, base, AFI_MSI_AXI_BAR_ST);
+	afi_writel(pcie, msi->phys >> soc->msi_base_shift, AFI_MSI_FPCI_BAR_ST);
+	afi_writel(pcie, msi->phys, AFI_MSI_AXI_BAR_ST);
 	/* this register is in 4K increments */
 	afi_writel(pcie, 1, AFI_MSI_BAR_SZ);
 
@@ -1585,8 +1596,6 @@ static int tegra_pcie_disable_msi(struct tegra_pcie *pcie)
 	afi_writel(pcie, 0, AFI_MSI_EN_VEC6);
 	afi_writel(pcie, 0, AFI_MSI_EN_VEC7);
 
-	free_pages(msi->pages, 0);
-
 	if (msi->irq > 0)
 		free_irq(msi->irq, pcie);
 
diff --git a/drivers/pci/host/pcie-mediatek.c b/drivers/pci/host/pcie-mediatek.c
new file mode 100644
index 000000000000..8ec8fa8732bf
--- /dev/null
+++ b/drivers/pci/host/pcie-mediatek.c
@@ -0,0 +1,552 @@
+/*
+ * MediaTek PCIe host controller driver.
+ *
+ * Copyright (c) 2017 MediaTek Inc.
+ * Author: Ryder Lee <ryder.lee@mediatek.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/kernel.h>
+#include <linux/of_address.h>
+#include <linux/of_pci.h>
+#include <linux/of_platform.h>
+#include <linux/pci.h>
+#include <linux/phy/phy.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/reset.h>
+
+/* PCIe shared registers */
+#define PCIE_SYS_CFG		0x00
+#define PCIE_INT_ENABLE		0x0c
+#define PCIE_CFG_ADDR		0x20
+#define PCIE_CFG_DATA		0x24
+
+/* PCIe per port registers */
+#define PCIE_BAR0_SETUP		0x10
+#define PCIE_BAR1_SETUP		0x14
+#define PCIE_BAR0_MEM_BASE	0x18
+#define PCIE_CLASS		0x34
+#define PCIE_LINK_STATUS	0x50
+
+#define PCIE_PORT_INT_EN(x)	BIT(20 + (x))
+#define PCIE_PORT_PERST(x)	BIT(1 + (x))
+#define PCIE_PORT_LINKUP	BIT(0)
+#define PCIE_BAR_MAP_MAX	GENMASK(31, 16)
+
+#define PCIE_BAR_ENABLE		BIT(0)
+#define PCIE_REVISION_ID	BIT(0)
+#define PCIE_CLASS_CODE		(0x60400 << 8)
+#define PCIE_CONF_REG(regn)	(((regn) & GENMASK(7, 2)) | \
+				((((regn) >> 8) & GENMASK(3, 0)) << 24))
+#define PCIE_CONF_FUN(fun)	(((fun) << 8) & GENMASK(10, 8))
+#define PCIE_CONF_DEV(dev)	(((dev) << 11) & GENMASK(15, 11))
+#define PCIE_CONF_BUS(bus)	(((bus) << 16) & GENMASK(23, 16))
+#define PCIE_CONF_ADDR(regn, fun, dev, bus) \
+	(PCIE_CONF_REG(regn) | PCIE_CONF_FUN(fun) | \
+	 PCIE_CONF_DEV(dev) | PCIE_CONF_BUS(bus))
+
+/* MediaTek specific configuration registers */
+#define PCIE_FTS_NUM		0x70c
+#define PCIE_FTS_NUM_MASK	GENMASK(15, 8)
+#define PCIE_FTS_NUM_L0(x)	((x) & 0xff << 8)
+
+#define PCIE_FC_CREDIT		0x73c
+#define PCIE_FC_CREDIT_MASK	(GENMASK(31, 31) | GENMASK(28, 16))
+#define PCIE_FC_CREDIT_VAL(x)	((x) << 16)
+
+/**
+ * struct mtk_pcie_port - PCIe port information
+ * @base: IO mapped register base
+ * @list: port list
+ * @pcie: pointer to PCIe host info
+ * @reset: pointer to port reset control
+ * @sys_ck: pointer to bus clock
+ * @phy: pointer to phy control block
+ * @lane: lane count
+ * @index: port index
+ */
+struct mtk_pcie_port {
+	void __iomem *base;
+	struct list_head list;
+	struct mtk_pcie *pcie;
+	struct reset_control *reset;
+	struct clk *sys_ck;
+	struct phy *phy;
+	u32 lane;
+	u32 index;
+};
+
+/**
+ * struct mtk_pcie - PCIe host information
+ * @dev: pointer to PCIe device
+ * @base: IO mapped register base
+ * @free_ck: free-run reference clock
+ * @io: IO resource
+ * @pio: PIO resource
+ * @mem: non-prefetchable memory resource
+ * @busn: bus range
+ * @offset: IO / Memory offset
+ * @ports: pointer to PCIe port information
+ */
+struct mtk_pcie {
+	struct device *dev;
+	void __iomem *base;
+	struct clk *free_ck;
+
+	struct resource io;
+	struct resource pio;
+	struct resource mem;
+	struct resource busn;
+	struct {
+		resource_size_t mem;
+		resource_size_t io;
+	} offset;
+	struct list_head ports;
+};
+
+static inline bool mtk_pcie_link_is_up(struct mtk_pcie_port *port)
+{
+	return !!(readl(port->base + PCIE_LINK_STATUS) & PCIE_PORT_LINKUP);
+}
+
+static void mtk_pcie_port_free(struct mtk_pcie_port *port)
+{
+	struct mtk_pcie *pcie = port->pcie;
+	struct device *dev = pcie->dev;
+
+	devm_iounmap(dev, port->base);
+	list_del(&port->list);
+	devm_kfree(dev, port);
+}
+
+static void mtk_pcie_put_resources(struct mtk_pcie *pcie)
+{
+	struct device *dev = pcie->dev;
+	struct mtk_pcie_port *port, *tmp;
+
+	list_for_each_entry_safe(port, tmp, &pcie->ports, list) {
+		phy_power_off(port->phy);
+		clk_disable_unprepare(port->sys_ck);
+		mtk_pcie_port_free(port);
+	}
+
+	clk_disable_unprepare(pcie->free_ck);
+	pm_runtime_put_sync(dev);
+	pm_runtime_disable(dev);
+}
+
+static void __iomem *mtk_pcie_map_bus(struct pci_bus *bus,
+				      unsigned int devfn, int where)
+{
+	struct pci_host_bridge *host = pci_find_host_bridge(bus);
+	struct mtk_pcie *pcie = pci_host_bridge_priv(host);
+
+	writel(PCIE_CONF_ADDR(where, PCI_FUNC(devfn), PCI_SLOT(devfn),
+			      bus->number), pcie->base + PCIE_CFG_ADDR);
+
+	return pcie->base + PCIE_CFG_DATA + (where & 3);
+}
+
+static struct pci_ops mtk_pcie_ops = {
+	.map_bus = mtk_pcie_map_bus,
+	.read  = pci_generic_config_read,
+	.write = pci_generic_config_write,
+};
+
+static void mtk_pcie_configure_rc(struct mtk_pcie_port *port)
+{
+	struct mtk_pcie *pcie = port->pcie;
+	u32 func = PCI_FUNC(port->index << 3);
+	u32 slot = PCI_SLOT(port->index << 3);
+	u32 val;
+
+	/* enable interrupt */
+	val = readl(pcie->base + PCIE_INT_ENABLE);
+	val |= PCIE_PORT_INT_EN(port->index);
+	writel(val, pcie->base + PCIE_INT_ENABLE);
+
+	/* map to all DDR region. We need to set it before cfg operation. */
+	writel(PCIE_BAR_MAP_MAX | PCIE_BAR_ENABLE,
+	       port->base + PCIE_BAR0_SETUP);
+
+	/* configure class Code and revision ID */
+	writel(PCIE_CLASS_CODE | PCIE_REVISION_ID,
+	       port->base + PCIE_CLASS);
+
+	/* configure FC credit */
+	writel(PCIE_CONF_ADDR(PCIE_FC_CREDIT, func, slot, 0),
+	       pcie->base + PCIE_CFG_ADDR);
+	val = readl(pcie->base + PCIE_CFG_DATA);
+	val &= ~PCIE_FC_CREDIT_MASK;
+	val |= PCIE_FC_CREDIT_VAL(0x806c);
+	writel(PCIE_CONF_ADDR(PCIE_FC_CREDIT, func, slot, 0),
+	       pcie->base + PCIE_CFG_ADDR);
+	writel(val, pcie->base + PCIE_CFG_DATA);
+
+	/* configure RC FTS number to 250 when it leaves L0s */
+	writel(PCIE_CONF_ADDR(PCIE_FTS_NUM, func, slot, 0),
+	       pcie->base + PCIE_CFG_ADDR);
+	val = readl(pcie->base + PCIE_CFG_DATA);
+	val &= ~PCIE_FTS_NUM_MASK;
+	val |= PCIE_FTS_NUM_L0(0x50);
+	writel(PCIE_CONF_ADDR(PCIE_FTS_NUM, func, slot, 0),
+	       pcie->base + PCIE_CFG_ADDR);
+	writel(val, pcie->base + PCIE_CFG_DATA);
+}
+
+static void mtk_pcie_assert_ports(struct mtk_pcie_port *port)
+{
+	struct mtk_pcie *pcie = port->pcie;
+	u32 val;
+
+	/* assert port PERST_N */
+	val = readl(pcie->base + PCIE_SYS_CFG);
+	val |= PCIE_PORT_PERST(port->index);
+	writel(val, pcie->base + PCIE_SYS_CFG);
+
+	/* de-assert port PERST_N */
+	val = readl(pcie->base + PCIE_SYS_CFG);
+	val &= ~PCIE_PORT_PERST(port->index);
+	writel(val, pcie->base + PCIE_SYS_CFG);
+
+	/* PCIe v2.0 need at least 100ms delay to train from Gen1 to Gen2 */
+	msleep(100);
+}
+
+static int mtk_pcie_enable_ports(struct mtk_pcie_port *port)
+{
+	struct device *dev = port->pcie->dev;
+	int err;
+
+	err = clk_prepare_enable(port->sys_ck);
+	if (err) {
+		dev_err(dev, "failed to enable port%d clock\n", port->index);
+		goto err_sys_clk;
+	}
+
+	reset_control_assert(port->reset);
+	reset_control_deassert(port->reset);
+
+	err = phy_power_on(port->phy);
+	if (err) {
+		dev_err(dev, "failed to power on port%d phy\n", port->index);
+		goto err_phy_on;
+	}
+
+	mtk_pcie_assert_ports(port);
+
+	/* if link up, then setup root port configuration space */
+	if (mtk_pcie_link_is_up(port)) {
+		mtk_pcie_configure_rc(port);
+		return 0;
+	}
+
+	dev_info(dev, "Port%d link down\n", port->index);
+
+	phy_power_off(port->phy);
+err_phy_on:
+	clk_disable_unprepare(port->sys_ck);
+	mtk_pcie_port_free(port);
+err_sys_clk:
+	return err;
+}
+
+static int mtk_pcie_parse_ports(struct mtk_pcie *pcie,
+				struct mtk_pcie_port **p,
+				struct device_node *node,
+				int index)
+{
+	struct mtk_pcie_port *port;
+	struct resource *regs;
+	struct device *dev = pcie->dev;
+	struct platform_device *pdev = to_platform_device(dev);
+	char name[10];
+	int err;
+
+	*p = devm_kzalloc(dev, sizeof(*port), GFP_KERNEL);
+	if (!*p)
+		return -ENOMEM;
+
+	port = *p;
+
+	err = of_property_read_u32(node, "num-lanes", &port->lane);
+	if (err) {
+		dev_err(dev, "missing num-lanes property\n");
+		return err;
+	}
+
+	regs = platform_get_resource(pdev, IORESOURCE_MEM, index + 1);
+	port->base = devm_ioremap_resource(dev, regs);
+	if (IS_ERR(port->base)) {
+		dev_err(dev, "failed to map port%d base\n", index);
+		return PTR_ERR(port->base);
+	}
+
+	snprintf(name, sizeof(name), "sys_ck%d", index);
+	port->sys_ck = devm_clk_get(dev, name);
+	if (IS_ERR(port->sys_ck)) {
+		dev_err(dev, "failed to get port%d clock\n", index);
+		return PTR_ERR(port->sys_ck);
+	}
+
+	snprintf(name, sizeof(name), "pcie-rst%d", index);
+	port->reset = devm_reset_control_get(dev, name);
+	if (IS_ERR(port->reset)) {
+		dev_err(dev, "failed to get port%d reset\n", index);
+		return PTR_ERR(port->reset);
+	}
+
+	snprintf(name, sizeof(name), "pcie-phy%d", index);
+	port->phy = devm_phy_get(dev, name);
+	if (IS_ERR(port->phy)) {
+		dev_err(dev, "failed to get port%d phy\n", index);
+		return PTR_ERR(port->phy);
+	}
+
+	port->index = index;
+	port->pcie = pcie;
+
+	INIT_LIST_HEAD(&port->list);
+	list_add_tail(&port->list, &pcie->ports);
+
+	return 0;
+}
+
+static int mtk_pcie_handle_shared_resource(struct mtk_pcie *pcie)
+{
+	struct device *dev = pcie->dev;
+	struct platform_device *pdev = to_platform_device(dev);
+	struct resource *regs;
+	int err;
+
+	/* get shared registers */
+	regs = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	pcie->base = devm_ioremap_resource(dev, regs);
+	if (IS_ERR(pcie->base)) {
+		dev_err(dev, "failed to map shared register\n");
+		return PTR_ERR(pcie->base);
+	}
+
+	pcie->free_ck = devm_clk_get(dev, "free_ck");
+	if (IS_ERR(pcie->free_ck)) {
+		dev_err(dev, "failed to get free_ck\n");
+		return PTR_ERR(pcie->free_ck);
+	}
+
+	pm_runtime_enable(dev);
+	err = pm_runtime_get_sync(dev);
+	if (err)
+		goto err_pm;
+
+	/* enable top level clock */
+	err = clk_prepare_enable(pcie->free_ck);
+	if (err) {
+		dev_err(dev, "failed to enable free_ck\n");
+		goto err_free_ck;
+	}
+
+	return 0;
+
+err_free_ck:
+	pm_runtime_put_sync(dev);
+err_pm:
+	pm_runtime_disable(dev);
+
+	return err;
+}
+
+static int mtk_pcie_parse_and_add_res(struct mtk_pcie *pcie)
+{
+	struct device *dev = pcie->dev;
+	struct device_node *node = dev->of_node, *child;
+	struct of_pci_range_parser parser;
+	struct of_pci_range range;
+	struct resource res;
+	int err, linkup = 0;
+
+	/* parse shared resources */
+	err = mtk_pcie_handle_shared_resource(pcie);
+	if (err)
+		return err;
+
+	if (of_pci_range_parser_init(&parser, node)) {
+		dev_err(dev, "missing \"ranges\" property\n");
+		return -EINVAL;
+	}
+
+	for_each_of_pci_range(&parser, &range) {
+		err = of_pci_range_to_resource(&range, node, &res);
+		if (err < 0)
+			return err;
+
+		switch (res.flags & IORESOURCE_TYPE_BITS) {
+		case IORESOURCE_IO:
+			pcie->offset.io = res.start - range.pci_addr;
+
+			memcpy(&pcie->pio, &res, sizeof(res));
+			pcie->pio.name = node->full_name;
+
+			pcie->io.start = range.cpu_addr;
+			pcie->io.end = range.cpu_addr + range.size - 1;
+			pcie->io.flags = IORESOURCE_MEM;
+			pcie->io.name = "I/O";
+
+			memcpy(&res, &pcie->io, sizeof(res));
+			break;
+
+		case IORESOURCE_MEM:
+			pcie->offset.mem = res.start - range.pci_addr;
+
+			memcpy(&pcie->mem, &res, sizeof(res));
+			pcie->mem.name = "non-prefetchable";
+			break;
+		}
+	}
+
+	err = of_pci_parse_bus_range(node, &pcie->busn);
+	if (err < 0) {
+		dev_err(dev, "failed to parse ranges property: %d\n", err);
+		pcie->busn.name = node->name;
+		pcie->busn.start = 0;
+		pcie->busn.end = 0xff;
+		pcie->busn.flags = IORESOURCE_BUS;
+	}
+
+	for_each_child_of_node(node, child) {
+		struct mtk_pcie_port *port;
+		int index;
+
+		err = of_pci_get_devfn(child);
+		if (err < 0) {
+			dev_err(dev, "failed to parse devfn: %d\n", err);
+			return err;
+		}
+
+		index = PCI_SLOT(err);
+
+		if (!of_device_is_available(child))
+			continue;
+
+		err = mtk_pcie_parse_ports(pcie, &port, child, index);
+		if (err)
+			return err;
+
+		/* enable each port, and then check link status */
+		err = mtk_pcie_enable_ports(port);
+		if (!err)
+			linkup++;
+	}
+
+	return !linkup;
+}
+
+static int mtk_pcie_request_resources(struct mtk_pcie *pcie)
+{
+	struct pci_host_bridge *host = pci_host_bridge_from_priv(pcie);
+	struct list_head *windows = &host->windows;
+	struct device *dev = pcie->dev;
+	int err;
+
+	pci_add_resource_offset(windows, &pcie->pio, pcie->offset.io);
+	pci_add_resource_offset(windows, &pcie->mem, pcie->offset.mem);
+	pci_add_resource(windows, &pcie->busn);
+
+	err = devm_request_pci_bus_resources(dev, windows);
+	if (err < 0)
+		return err;
+
+	pci_remap_iospace(&pcie->pio, pcie->io.start);
+
+	return 0;
+}
+
+static int mtk_pcie_register_host(struct pci_host_bridge *host)
+{
+	struct mtk_pcie *pcie = pci_host_bridge_priv(host);
+	struct pci_bus *child;
+	int err;
+
+	pci_add_flags(PCI_REASSIGN_ALL_RSRC | PCI_REASSIGN_ALL_BUS);
+	host->busnr = pcie->busn.start;
+	host->dev.parent = pcie->dev;
+	host->ops = &mtk_pcie_ops;
+
+	err = pci_register_host_bridge(host);
+	if (err < 0)
+		return err;
+
+	pci_scan_child_bus(host->bus);
+
+	pci_fixup_irqs(pci_common_swizzle, of_irq_parse_and_map_pci);
+	pci_bus_size_bridges(host->bus);
+	pci_bus_assign_resources(host->bus);
+
+	list_for_each_entry(child, &host->bus->children, node)
+		pcie_bus_configure_settings(child);
+
+	pci_bus_add_devices(host->bus);
+
+	return 0;
+}
+
+static int mtk_pcie_probe(struct platform_device *pdev)
+{
+	struct mtk_pcie *pcie;
+	struct pci_host_bridge *host;
+	int err;
+
+	host = pci_alloc_host_bridge(sizeof(*pcie));
+	if (!host)
+		return -ENOMEM;
+
+	pcie = pci_host_bridge_priv(host);
+
+	pcie->dev = &pdev->dev;
+	platform_set_drvdata(pdev, pcie);
+	INIT_LIST_HEAD(&pcie->ports);
+
+	err = mtk_pcie_parse_and_add_res(pcie);
+	if (err)
+		return err;
+
+	err = mtk_pcie_request_resources(pcie);
+	if (err)
+		goto put_resources;
+
+	err = mtk_pcie_register_host(host);
+	if (err)
+		goto put_resources;
+
+	return 0;
+
+put_resources:
+	mtk_pcie_put_resources(pcie);
+	return err;
+}
+
+static const struct of_device_id mtk_pcie_ids[] = {
+	{ .compatible = "mediatek,mt7623-pcie"},
+	{ .compatible = "mediatek,mt2701-pcie"},
+	{},
+};
+
+static struct platform_driver mtk_pcie_driver = {
+	.probe = mtk_pcie_probe,
+	.driver = {
+		.name = "mtk-pcie",
+		.of_match_table = mtk_pcie_ids,
+		.suppress_bind_attrs = true,
+	},
+};
+builtin_platform_driver(mtk_pcie_driver);
diff --git a/drivers/pci/host/pcie-xilinx-nwl.c b/drivers/pci/host/pcie-xilinx-nwl.c
index 4b16b26ae909..706a3ea62d1d 100644
--- a/drivers/pci/host/pcie-xilinx-nwl.c
+++ b/drivers/pci/host/pcie-xilinx-nwl.c
@@ -172,6 +172,7 @@ struct nwl_pcie {
 	u8 root_busno;
 	struct nwl_msi msi;
 	struct irq_domain *legacy_irq_domain;
+	raw_spinlock_t leg_mask_lock;
 };
 
 static inline u32 nwl_bridge_readl(struct nwl_pcie *pcie, u32 off)
@@ -383,11 +384,52 @@ static void nwl_pcie_msi_handler_low(struct irq_desc *desc)
 	chained_irq_exit(chip, desc);
 }
 
+static void nwl_mask_leg_irq(struct irq_data *data)
+{
+	struct irq_desc *desc = irq_to_desc(data->irq);
+	struct nwl_pcie *pcie;
+	unsigned long flags;
+	u32 mask;
+	u32 val;
+
+	pcie = irq_desc_get_chip_data(desc);
+	mask = 1 << (data->hwirq - 1);
+	raw_spin_lock_irqsave(&pcie->leg_mask_lock, flags);
+	val = nwl_bridge_readl(pcie, MSGF_LEG_MASK);
+	nwl_bridge_writel(pcie, (val & (~mask)), MSGF_LEG_MASK);
+	raw_spin_unlock_irqrestore(&pcie->leg_mask_lock, flags);
+}
+
+static void nwl_unmask_leg_irq(struct irq_data *data)
+{
+	struct irq_desc *desc = irq_to_desc(data->irq);
+	struct nwl_pcie *pcie;
+	unsigned long flags;
+	u32 mask;
+	u32 val;
+
+	pcie = irq_desc_get_chip_data(desc);
+	mask = 1 << (data->hwirq - 1);
+	raw_spin_lock_irqsave(&pcie->leg_mask_lock, flags);
+	val = nwl_bridge_readl(pcie, MSGF_LEG_MASK);
+	nwl_bridge_writel(pcie, (val | mask), MSGF_LEG_MASK);
+	raw_spin_unlock_irqrestore(&pcie->leg_mask_lock, flags);
+}
+
+static struct irq_chip nwl_leg_irq_chip = {
+	.name = "nwl_pcie:legacy",
+	.irq_enable = nwl_unmask_leg_irq,
+	.irq_disable = nwl_mask_leg_irq,
+	.irq_mask = nwl_mask_leg_irq,
+	.irq_unmask = nwl_unmask_leg_irq,
+};
+
 static int nwl_legacy_map(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_and_handler(irq, &nwl_leg_irq_chip, handle_level_irq);
 	irq_set_chip_data(irq, domain->host_data);
+	irq_set_status_flags(irq, IRQ_LEVEL);
 
 	return 0;
 }
@@ -526,6 +568,7 @@ static int nwl_pcie_init_irq_domain(struct nwl_pcie *pcie)
 		return -ENOMEM;
 	}
 
+	raw_spin_lock_init(&pcie->leg_mask_lock);
 	nwl_pcie_init_msi_irq_domain(pcie);
 	return 0;
 }
diff --git a/drivers/pci/pci-label.c b/drivers/pci/pci-label.c
index 51357377efbc..1d828a614ac0 100644
--- a/drivers/pci/pci-label.c
+++ b/drivers/pci/pci-label.c
@@ -43,9 +43,11 @@ static size_t find_smbios_instance_string(struct pci_dev *pdev, char *buf,
 {
 	const struct dmi_device *dmi;
 	struct dmi_dev_onboard *donboard;
+	int domain_nr;
 	int bus;
 	int devfn;
 
+	domain_nr = pci_domain_nr(pdev->bus);
 	bus = pdev->bus->number;
 	devfn = pdev->devfn;
 
@@ -53,8 +55,9 @@ static size_t find_smbios_instance_string(struct pci_dev *pdev, char *buf,
 	while ((dmi = dmi_find_device(DMI_DEV_TYPE_DEV_ONBOARD,
 				      NULL, dmi)) != NULL) {
 		donboard = dmi->device_data;
-		if (donboard && donboard->bus == bus &&
-					donboard->devfn == devfn) {
+		if (donboard && donboard->segment == domain_nr &&
+				donboard->bus == bus &&
+				donboard->devfn == devfn) {
 			if (buf) {
 				if (attribute == SMBIOS_ATTR_INSTANCE_SHOW)
 					return scnprintf(buf, PAGE_SIZE,
diff --git a/drivers/pci/pci.c b/drivers/pci/pci.c
index b01bd5bba8e6..2f6f6380fa1c 100644
--- a/drivers/pci/pci.c
+++ b/drivers/pci/pci.c
@@ -455,7 +455,7 @@ struct resource *pci_find_parent_resource(const struct pci_dev *dev,
 	pci_bus_for_each_resource(bus, r, i) {
 		if (!r)
 			continue;
-		if (res->start && resource_contains(r, res)) {
+		if (resource_contains(r, res)) {
 
 			/*
 			 * If the window is prefetchable but the BAR is
@@ -2144,7 +2144,8 @@ bool pci_dev_keep_suspended(struct pci_dev *pci_dev)
 
 	if (!pm_runtime_suspended(dev)
 	    || pci_target_state(pci_dev) != pci_dev->current_state
-	    || platform_pci_need_resume(pci_dev))
+	    || platform_pci_need_resume(pci_dev)
+	    || (pci_dev->dev_flags & PCI_DEV_FLAGS_NEEDS_RESUME))
 		return false;
 
 	/*
diff --git a/drivers/pci/pcie/pcie-dpc.c b/drivers/pci/pcie/pcie-dpc.c
index 77d2ca99d2ec..c39f32e42b4d 100644
--- a/drivers/pci/pcie/pcie-dpc.c
+++ b/drivers/pci/pcie/pcie-dpc.c
@@ -92,7 +92,7 @@ static irqreturn_t dpc_irq(int irq, void *context)
 	pci_read_config_word(pdev, dpc->cap_pos + PCI_EXP_DPC_STATUS, &status);
 	pci_read_config_word(pdev, dpc->cap_pos + PCI_EXP_DPC_SOURCE_ID,
 			     &source);
-	if (!status)
+	if (!status || status == (u16)(~0))
 		return IRQ_NONE;
 
 	dev_info(&dpc->dev->device, "DPC containment event, status:%#06x source:%#06x\n",
@@ -144,7 +144,7 @@ static int dpc_probe(struct pcie_device *dev)
 
 	dpc->rp = (cap & PCI_EXP_DPC_CAP_RP_EXT);
 
-	ctl |= PCI_EXP_DPC_CTL_EN_NONFATAL | PCI_EXP_DPC_CTL_INT_EN;
+	ctl = (ctl & 0xfff4) | PCI_EXP_DPC_CTL_EN_NONFATAL | PCI_EXP_DPC_CTL_INT_EN;
 	pci_write_config_word(pdev, dpc->cap_pos + PCI_EXP_DPC_CTL, ctl);
 
 	dev_info(&dev->device, "DPC error containment capabilities: Int Msg #%d, RPExt%c PoisonedTLP%c SwTrigger%c RP PIO Log %d, DL_ActiveErr%c\n",
diff --git a/drivers/pci/quirks.c b/drivers/pci/quirks.c
index 085fb787aa9e..16e6cd86ad71 100644
--- a/drivers/pci/quirks.c
+++ b/drivers/pci/quirks.c
@@ -304,7 +304,7 @@ static void quirk_extend_bar_to_page(struct pci_dev *dev)
 {
 	int i;
 
-	for (i = 0; i < PCI_STD_RESOURCE_END; i++) {
+	for (i = 0; i <= PCI_STD_RESOURCE_END; i++) {
 		struct resource *r = &dev->resource[i];
 
 		if (r->flags & IORESOURCE_MEM && resource_size(r) < PAGE_SIZE) {
diff --git a/drivers/pci/switch/switchtec.c b/drivers/pci/switch/switchtec.c
index cc6e085008fb..f6a63406c76e 100644
--- a/drivers/pci/switch/switchtec.c
+++ b/drivers/pci/switch/switchtec.c
@@ -1291,7 +1291,6 @@ static struct switchtec_dev *stdev_create(struct pci_dev *pdev)
 	cdev = &stdev->cdev;
 	cdev_init(cdev, &switchtec_fops);
 	cdev->owner = THIS_MODULE;
-	cdev->kobj.parent = &dev->kobj;
 
 	return stdev;
 
@@ -1442,12 +1441,15 @@ static int switchtec_init_pci(struct switchtec_dev *stdev,
 	stdev->mmio_sys_info = stdev->mmio + SWITCHTEC_GAS_SYS_INFO_OFFSET;
 	stdev->mmio_flash_info = stdev->mmio + SWITCHTEC_GAS_FLASH_INFO_OFFSET;
 	stdev->mmio_ntb = stdev->mmio + SWITCHTEC_GAS_NTB_OFFSET;
-	stdev->partition = ioread8(&stdev->mmio_ntb->partition_id);
+	stdev->partition = ioread8(&stdev->mmio_sys_info->partition_id);
 	stdev->partition_count = ioread8(&stdev->mmio_ntb->partition_count);
 	stdev->mmio_part_cfg_all = stdev->mmio + SWITCHTEC_GAS_PART_CFG_OFFSET;
 	stdev->mmio_part_cfg = &stdev->mmio_part_cfg_all[stdev->partition];
 	stdev->mmio_pff_csr = stdev->mmio + SWITCHTEC_GAS_PFF_CSR_OFFSET;
 
+	if (stdev->partition_count < 1)
+		stdev->partition_count = 1;
+
 	init_pff(stdev);
 
 	pci_set_drvdata(pdev, stdev);
@@ -1479,11 +1481,7 @@ static int switchtec_pci_probe(struct pci_dev *pdev,
 		  SWITCHTEC_EVENT_EN_IRQ,
 		  &stdev->mmio_part_cfg->mrpc_comp_hdr);
 
-	rc = cdev_add(&stdev->cdev, stdev->dev.devt, 1);
-	if (rc)
-		goto err_put;
-
-	rc = device_add(&stdev->dev);
+	rc = cdev_device_add(&stdev->cdev, &stdev->dev);
 	if (rc)
 		goto err_devadd;
 
@@ -1492,7 +1490,6 @@ static int switchtec_pci_probe(struct pci_dev *pdev,
 	return 0;
 
 err_devadd:
-	cdev_del(&stdev->cdev);
 	stdev_kill(stdev);
 err_put:
 	ida_simple_remove(&switchtec_minor_ida, MINOR(stdev->dev.devt));
@@ -1506,8 +1503,7 @@ static void switchtec_pci_remove(struct pci_dev *pdev)
 
 	pci_set_drvdata(pdev, NULL);
 
-	device_del(&stdev->dev);
-	cdev_del(&stdev->cdev);
+	cdev_device_del(&stdev->cdev, &stdev->dev);
 	ida_simple_remove(&switchtec_minor_ida, MINOR(stdev->dev.devt));
 	dev_info(&stdev->dev, "unregistered.\n");
 
diff --git a/drivers/phy/Kconfig b/drivers/phy/Kconfig
index afaf7b643eeb..01009b2a7d74 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,17 @@ 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/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..c1bd1fa3c853 100644
--- a/drivers/phy/Makefile
+++ b/drivers/phy/Makefile
@@ -3,64 +3,20 @@
 #
 
 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/	\
+					   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..edcd5b65179f
--- /dev/null
+++ b/drivers/phy/amlogic/Kconfig
@@ -0,0 +1,14 @@
+#
+# Phy drivers for Amlogic platforms
+#
+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.
diff --git a/drivers/phy/amlogic/Makefile b/drivers/phy/amlogic/Makefile
new file mode 100644
index 000000000000..47b6eecc3864
--- /dev/null
+++ b/drivers/phy/amlogic/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_PHY_MESON8B_USB2)		+= phy-meson8b-usb2.o
diff --git a/drivers/phy/phy-meson8b-usb2.c b/drivers/phy/amlogic/phy-meson8b-usb2.c
index 30f56a6a411f..30f56a6a411f 100644
--- a/drivers/phy/phy-meson8b-usb2.c
+++ b/drivers/phy/amlogic/phy-meson8b-usb2.c
diff --git a/drivers/phy/broadcom/Kconfig b/drivers/phy/broadcom/Kconfig
new file mode 100644
index 000000000000..d2d99023ec50
--- /dev/null
+++ b/drivers/phy/broadcom/Kconfig
@@ -0,0 +1,55 @@
+#
+# 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
+	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_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..357a7d16529f
--- /dev/null
+++ b/drivers/phy/broadcom/Makefile
@@ -0,0 +1,6 @@
+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_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..22b5e7047fa6 100644
--- a/drivers/phy/phy-bcm-ns-usb3.c
+++ b/drivers/phy/broadcom/phy-bcm-ns-usb3.c
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/phy-brcm-sata.c b/drivers/phy/broadcom/phy-brcm-sata.c
index ccbc3d994998..ccbc3d994998 100644
--- a/drivers/phy/phy-brcm-sata.c
+++ b/drivers/phy/broadcom/phy-brcm-sata.c
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/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 727e23be7cac..78ca62897784 100644
--- a/drivers/phy/phy-qcom-qmp.c
+++ b/drivers/phy/qualcomm/phy-qcom-qmp.c
@@ -844,7 +844,7 @@ static int qcom_qmp_phy_vreg_init(struct device *dev)
 	int num = qmp->cfg->num_vregs;
 	int i;
 
-	qmp->vregs = devm_kcalloc(dev, num, sizeof(qmp->vregs), GFP_KERNEL);
+	qmp->vregs = devm_kcalloc(dev, num, sizeof(*qmp->vregs), GFP_KERNEL);
 	if (!qmp->vregs)
 		return -ENOMEM;
 
@@ -983,16 +983,16 @@ int qcom_qmp_phy_create(struct device *dev, struct device_node *np, int id)
 	 * Resources are indexed as: tx -> 0; rx -> 1; pcs -> 2.
 	 */
 	qphy->tx = of_iomap(np, 0);
-	if (IS_ERR(qphy->tx))
-		return PTR_ERR(qphy->tx);
+	if (!qphy->tx)
+		return -ENOMEM;
 
 	qphy->rx = of_iomap(np, 1);
-	if (IS_ERR(qphy->rx))
-		return PTR_ERR(qphy->rx);
+	if (!qphy->rx)
+		return -ENOMEM;
 
 	qphy->pcs = of_iomap(np, 2);
-	if (IS_ERR(qphy->pcs))
-		return PTR_ERR(qphy->pcs);
+	if (!qphy->pcs)
+		return -ENOMEM;
 
 	/*
 	 * Get PHY's Pipe clock, if any. USB3 and PCIe are PIPE3
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..432e2715e9c4
--- /dev/null
+++ b/drivers/phy/renesas/Kconfig
@@ -0,0 +1,17 @@
+#
+# 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.
diff --git a/drivers/phy/renesas/Makefile b/drivers/phy/renesas/Makefile
new file mode 100644
index 000000000000..695241aebf69
--- /dev/null
+++ b/drivers/phy/renesas/Makefile
@@ -0,0 +1,2 @@
+obj-$(CONFIG_PHY_RCAR_GEN2)		+= phy-rcar-gen2.o
+obj-$(CONFIG_PHY_RCAR_GEN3_USB2)	+= phy-rcar-gen3-usb2.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/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..8efe78a49916 100644
--- a/drivers/phy/phy-rockchip-inno-usb2.c
+++ b/drivers/phy/rockchip/phy-rockchip-inno-usb2.c
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..bb3fb031c478 100644
--- a/drivers/phy/phy-tusb1210.c
+++ b/drivers/phy/ti/phy-tusb1210.c
@@ -12,8 +12,7 @@
 #include <linux/module.h>
 #include <linux/ulpi/driver.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
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/core.c b/drivers/pinctrl/core.c
index 1653cbda6a82..bd459a93b0e7 100644
--- a/drivers/pinctrl/core.c
+++ b/drivers/pinctrl/core.c
@@ -680,30 +680,16 @@ EXPORT_SYMBOL_GPL(pinctrl_generic_remove_group);
  * pinctrl_generic_free_groups() - removes all pin groups
  * @pctldev: pin controller device
  *
- * Note that the caller must take care of locking.
+ * Note that the caller must take care of locking. The pinctrl groups
+ * are allocated with devm_kzalloc() so no need to free them here.
  */
 static void pinctrl_generic_free_groups(struct pinctrl_dev *pctldev)
 {
 	struct radix_tree_iter iter;
-	struct group_desc *group;
-	unsigned long *indices;
 	void **slot;
-	int i = 0;
-
-	indices = devm_kzalloc(pctldev->dev, sizeof(*indices) *
-			       pctldev->num_groups, GFP_KERNEL);
-	if (!indices)
-		return;
 
 	radix_tree_for_each_slot(slot, &pctldev->pin_group_tree, &iter, 0)
-		indices[i++] = iter.index;
-
-	for (i = 0; i < pctldev->num_groups; i++) {
-		group = radix_tree_lookup(&pctldev->pin_group_tree,
-					  indices[i]);
-		radix_tree_delete(&pctldev->pin_group_tree, indices[i]);
-		devm_kfree(pctldev->dev, group);
-	}
+		radix_tree_delete(&pctldev->pin_group_tree, iter.index);
 
 	pctldev->num_groups = 0;
 }
diff --git a/drivers/pinctrl/freescale/pinctrl-mxs.c b/drivers/pinctrl/freescale/pinctrl-mxs.c
index 41b5b07d5a2b..6852010a6d70 100644
--- a/drivers/pinctrl/freescale/pinctrl-mxs.c
+++ b/drivers/pinctrl/freescale/pinctrl-mxs.c
@@ -194,6 +194,16 @@ static int mxs_pinctrl_get_func_groups(struct pinctrl_dev *pctldev,
 	return 0;
 }
 
+static void mxs_pinctrl_rmwl(u32 value, u32 mask, u8 shift, void __iomem *reg)
+{
+	u32 tmp;
+
+	tmp = readl(reg);
+	tmp &= ~(mask << shift);
+	tmp |= value << shift;
+	writel(tmp, reg);
+}
+
 static int mxs_pinctrl_set_mux(struct pinctrl_dev *pctldev, unsigned selector,
 			       unsigned group)
 {
@@ -211,8 +221,7 @@ static int mxs_pinctrl_set_mux(struct pinctrl_dev *pctldev, unsigned selector,
 		reg += bank * 0x20 + pin / 16 * 0x10;
 		shift = pin % 16 * 2;
 
-		writel(0x3 << shift, reg + CLR);
-		writel(g->muxsel[i] << shift, reg + SET);
+		mxs_pinctrl_rmwl(g->muxsel[i], 0x3, shift, reg);
 	}
 
 	return 0;
@@ -279,8 +288,7 @@ static int mxs_pinconf_group_set(struct pinctrl_dev *pctldev,
 			/* mA */
 			if (config & MA_PRESENT) {
 				shift = pin % 8 * 4;
-				writel(0x3 << shift, reg + CLR);
-				writel(ma << shift, reg + SET);
+				mxs_pinctrl_rmwl(ma, 0x3, shift, reg);
 			}
 
 			/* vol */
diff --git a/drivers/pinctrl/intel/pinctrl-cherryview.c b/drivers/pinctrl/intel/pinctrl-cherryview.c
index 2debba62fac9..20f1b4493994 100644
--- a/drivers/pinctrl/intel/pinctrl-cherryview.c
+++ b/drivers/pinctrl/intel/pinctrl-cherryview.c
@@ -1539,15 +1539,29 @@ static void chv_gpio_irq_handler(struct irq_desc *desc)
  * is not listed below.
  */
 static const struct dmi_system_id chv_no_valid_mask[] = {
+	/* See https://bugzilla.kernel.org/show_bug.cgi?id=194945 */
 	{
-		/* See https://bugzilla.kernel.org/show_bug.cgi?id=194945 */
-		.ident = "Acer Chromebook (CYAN)",
+		.ident = "Intel_Strago based Chromebooks (All models)",
 		.matches = {
 			DMI_MATCH(DMI_SYS_VENDOR, "GOOGLE"),
-			DMI_MATCH(DMI_PRODUCT_NAME, "Edgar"),
-			DMI_MATCH(DMI_BIOS_DATE, "05/21/2016"),
+			DMI_MATCH(DMI_PRODUCT_FAMILY, "Intel_Strago"),
 		},
-	}
+	},
+	{
+		.ident = "Acer Chromebook R11 (Cyan)",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "GOOGLE"),
+			DMI_MATCH(DMI_PRODUCT_NAME, "Cyan"),
+		},
+	},
+	{
+		.ident = "Samsung Chromebook 3 (Celes)",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "GOOGLE"),
+			DMI_MATCH(DMI_PRODUCT_NAME, "Celes"),
+		},
+	},
+	{}
 };
 
 static int chv_gpio_probe(struct chv_pinctrl *pctrl, int irq)
diff --git a/drivers/pinctrl/pinconf-generic.c b/drivers/pinctrl/pinconf-generic.c
index 0d6b7f4b82af..720a19fd38d2 100644
--- a/drivers/pinctrl/pinconf-generic.c
+++ b/drivers/pinctrl/pinconf-generic.c
@@ -35,7 +35,6 @@ static const struct pin_config_item conf_items[] = {
 	PCONFDUMP(PIN_CONFIG_BIAS_PULL_PIN_DEFAULT,
 				"input bias pull to pin specific state", NULL, false),
 	PCONFDUMP(PIN_CONFIG_BIAS_PULL_UP, "input bias pull up", NULL, false),
-	PCONFDUMP(PIN_CONFIG_BIDIRECTIONAL, "bi-directional pin operations", NULL, false),
 	PCONFDUMP(PIN_CONFIG_DRIVE_OPEN_DRAIN, "output drive open drain", NULL, false),
 	PCONFDUMP(PIN_CONFIG_DRIVE_OPEN_SOURCE, "output drive open source", NULL, false),
 	PCONFDUMP(PIN_CONFIG_DRIVE_PUSH_PULL, "output drive push pull", NULL, false),
@@ -161,7 +160,6 @@ static const struct pinconf_generic_params dt_params[] = {
 	{ "bias-pull-up", PIN_CONFIG_BIAS_PULL_UP, 1 },
 	{ "bias-pull-pin-default", PIN_CONFIG_BIAS_PULL_PIN_DEFAULT, 1 },
 	{ "bias-pull-down", PIN_CONFIG_BIAS_PULL_DOWN, 1 },
-	{ "bi-directional", PIN_CONFIG_BIDIRECTIONAL, 1 },
 	{ "drive-open-drain", PIN_CONFIG_DRIVE_OPEN_DRAIN, 0 },
 	{ "drive-open-source", PIN_CONFIG_DRIVE_OPEN_SOURCE, 0 },
 	{ "drive-push-pull", PIN_CONFIG_DRIVE_PUSH_PULL, 0 },
@@ -174,7 +172,6 @@ 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-enable", PIN_CONFIG_OUTPUT, 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/pinmux.c b/drivers/pinctrl/pinmux.c
index 9fd6d9087dc5..16b3ae5e4f44 100644
--- a/drivers/pinctrl/pinmux.c
+++ b/drivers/pinctrl/pinmux.c
@@ -826,30 +826,17 @@ EXPORT_SYMBOL_GPL(pinmux_generic_remove_function);
  * pinmux_generic_free_functions() - removes all functions
  * @pctldev: pin controller device
  *
- * Note that the caller must take care of locking.
+ * Note that the caller must take care of locking. The pinctrl
+ * functions are allocated with devm_kzalloc() so no need to free
+ * them here.
  */
 void pinmux_generic_free_functions(struct pinctrl_dev *pctldev)
 {
 	struct radix_tree_iter iter;
-	struct function_desc *function;
-	unsigned long *indices;
 	void **slot;
-	int i = 0;
-
-	indices = devm_kzalloc(pctldev->dev, sizeof(*indices) *
-			       pctldev->num_functions, GFP_KERNEL);
-	if (!indices)
-		return;
 
 	radix_tree_for_each_slot(slot, &pctldev->pin_function_tree, &iter, 0)
-		indices[i++] = iter.index;
-
-	for (i = 0; i < pctldev->num_functions; i++) {
-		function = radix_tree_lookup(&pctldev->pin_function_tree,
-					     indices[i]);
-		radix_tree_delete(&pctldev->pin_function_tree, indices[i]);
-		devm_kfree(pctldev->dev, function);
-	}
+		radix_tree_delete(&pctldev->pin_function_tree, iter.index);
 
 	pctldev->num_functions = 0;
 }
diff --git a/drivers/pinctrl/sunxi/pinctrl-sun8i-a83t.c b/drivers/pinctrl/sunxi/pinctrl-sun8i-a83t.c
index 9aec1d2232dd..6624499eae72 100644
--- a/drivers/pinctrl/sunxi/pinctrl-sun8i-a83t.c
+++ b/drivers/pinctrl/sunxi/pinctrl-sun8i-a83t.c
@@ -394,7 +394,7 @@ static const struct sunxi_desc_pin sun8i_a83t_pins[] = {
 	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 18),
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out"),
-		  SUNXI_FUNCTION(0x3, "owa")),		/* DOUT */
+		  SUNXI_FUNCTION(0x3, "spdif")),	/* DOUT */
 	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 19),
 		  SUNXI_FUNCTION(0x0, "gpio_in"),
 		  SUNXI_FUNCTION(0x1, "gpio_out")),
diff --git a/drivers/platform/goldfish/goldfish_pipe.c b/drivers/platform/goldfish/goldfish_pipe.c
index 2de1e603bd2b..5f3672153b12 100644
--- a/drivers/platform/goldfish/goldfish_pipe.c
+++ b/drivers/platform/goldfish/goldfish_pipe.c
@@ -704,7 +704,7 @@ static int get_free_pipe_id_locked(struct goldfish_pipe_dev *dev)
 		/* Reallocate the array */
 		u32 new_capacity = 2 * dev->pipes_capacity;
 		struct goldfish_pipe **pipes =
-			kcalloc(new_capacity, sizeof(*pipes), GFP_KERNEL);
+			kcalloc(new_capacity, sizeof(*pipes), GFP_ATOMIC);
 		if (!pipes)
 			return -ENOMEM;
 		memcpy(pipes, dev->pipes, sizeof(*pipes) * dev->pipes_capacity);
diff --git a/drivers/powercap/powercap_sys.c b/drivers/powercap/powercap_sys.c
index 14bde0db8c24..5b10b50f8686 100644
--- a/drivers/powercap/powercap_sys.c
+++ b/drivers/powercap/powercap_sys.c
@@ -538,6 +538,7 @@ struct powercap_zone *powercap_register_zone(
 
 	power_zone->id = result;
 	idr_init(&power_zone->idr);
+	result = -ENOMEM;
 	power_zone->name = kstrdup(name, GFP_KERNEL);
 	if (!power_zone->name)
 		goto err_name_alloc;
diff --git a/drivers/reset/Kconfig b/drivers/reset/Kconfig
index d21c07ccc94e..61fabf6ddaec 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,7 @@ config RESET_SUNXI
 	help
 	  This enables the reset driver for Allwinner SoCs.
 
-config TI_SYSCON_RESET
+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..e422a04f7b85 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,7 @@ 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_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/hisilicon/hi6220_reset.c b/drivers/reset/hisilicon/hi6220_reset.c
index 35ce53edabf9..d5e5229308f2 100644
--- a/drivers/reset/hisilicon/hi6220_reset.c
+++ b/drivers/reset/hisilicon/hi6220_reset.c
@@ -155,3 +155,5 @@ static int __init hi6220_reset_init(void)
 }
 
 postcore_initcall(hi6220_reset_init);
+
+MODULE_LICENSE("GPL v2");
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/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/rtc/rtc-cmos.c b/drivers/rtc/rtc-cmos.c
index b3de973a6260..9dca53df3584 100644
--- a/drivers/rtc/rtc-cmos.c
+++ b/drivers/rtc/rtc-cmos.c
@@ -1088,7 +1088,7 @@ static u32 rtc_handler(void *context)
 	}
 	spin_unlock_irqrestore(&rtc_lock, flags);
 
-	pm_wakeup_event(dev, 0);
+	pm_wakeup_hard_event(dev);
 	acpi_clear_event(ACPI_EVENT_RTC);
 	acpi_disable_event(ACPI_EVENT_RTC, 0);
 	return ACPI_INTERRUPT_HANDLED;
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_devmap.c b/drivers/s390/block/dasd_devmap.c
index 1164b51d09f3..05e5762d045e 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;
 }
 
 /*
diff --git a/drivers/s390/block/scm_blk.c b/drivers/s390/block/scm_blk.c
index 152de6817875..71c0158aa0c3 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;
 }
 
@@ -234,130 +231,123 @@ static inline void scm_request_init(struct scm_blk_dev *bdev,
 	scmrq->error = 0;
 	/* 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)
@@ -377,21 +367,6 @@ 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;
@@ -419,49 +394,41 @@ 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, int 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;
@@ -477,18 +444,22 @@ 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))
+		goto out_tag;
+
 	bdev->rq = rq;
 	nr_max_blk = min(scmdev->nr_max_block,
 			 (unsigned int) (PAGE_SIZE / sizeof(struct aidaw)));
@@ -498,7 +469,6 @@ 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)
@@ -528,6 +498,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 +507,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 +530,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..242d17a91920 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 {
@@ -36,13 +33,6 @@ struct scm_request {
 	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
 };
 
 #define to_aobrq(rq) container_of((void *) rq, struct aob_rq_header, data)
@@ -52,55 +42,11 @@ 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 *);
-
 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/cio/ccwgroup.c b/drivers/s390/cio/ccwgroup.c
index e443b0d0b236..34b9ad6b3143 100644
--- a/drivers/s390/cio/ccwgroup.c
+++ b/drivers/s390/cio/ccwgroup.c
@@ -35,7 +35,7 @@ static struct bus_type ccwgroup_bus_type;
 static void __ccwgroup_remove_symlinks(struct ccwgroup_device *gdev)
 {
 	int i;
-	char str[8];
+	char str[16];
 
 	for (i = 0; i < gdev->count; i++) {
 		sprintf(str, "cdev%d", i);
@@ -238,7 +238,7 @@ static void ccwgroup_release(struct device *dev)
 
 static int __ccwgroup_create_symlinks(struct ccwgroup_device *gdev)
 {
-	char str[8];
+	char str[16];
 	int i, rc;
 
 	for (i = 0; i < gdev->count; i++) {
diff --git a/drivers/s390/cio/qdio_debug.h b/drivers/s390/cio/qdio_debug.h
index f33ce8577619..1d595d17bf11 100644
--- a/drivers/s390/cio/qdio_debug.h
+++ b/drivers/s390/cio/qdio_debug.h
@@ -11,7 +11,7 @@
 #include "qdio.h"
 
 /* that gives us 15 characters in the text event views */
-#define QDIO_DBF_LEN	16
+#define QDIO_DBF_LEN	32
 
 extern debug_info_t *qdio_dbf_setup;
 extern debug_info_t *qdio_dbf_error;
diff --git a/drivers/s390/crypto/ap_bus.c b/drivers/s390/crypto/ap_bus.c
index 9be4596d8a08..e52706e60cf8 100644
--- a/drivers/s390/crypto/ap_bus.c
+++ b/drivers/s390/crypto/ap_bus.c
@@ -745,7 +745,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 +931,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 +968,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 +1032,8 @@ static void ap_scan_bus(struct work_struct *unused)
 				put_device(dev);
 				if (!borked) {
 					domains++;
+					if (dom == ap_domain_index)
+						defdomdevs++;
 					continue;
 				}
 			}
@@ -1088,6 +1091,8 @@ static void ap_scan_bus(struct work_struct *unused)
 				continue;
 			}
 			domains++;
+			if (dom == ap_domain_index)
+				defdomdevs++;
 		} /* end domain loop */
 		if (ac) {
 			/* remove card dev if there are no queue devices */
@@ -1096,6 +1101,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 +1174,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 +1254,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/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/qeth_core.h b/drivers/s390/net/qeth_core.h
index f6aa21176d89..30bc6105aac3 100644
--- a/drivers/s390/net/qeth_core.h
+++ b/drivers/s390/net/qeth_core.h
@@ -701,6 +701,7 @@ enum qeth_discipline_id {
 };
 
 struct qeth_discipline {
+	const struct device_type *devtype;
 	void (*start_poll)(struct ccw_device *, int, unsigned long);
 	qdio_handler_t *input_handler;
 	qdio_handler_t *output_handler;
@@ -875,6 +876,9 @@ extern struct qeth_discipline qeth_l2_discipline;
 extern struct qeth_discipline qeth_l3_discipline;
 extern const struct attribute_group *qeth_generic_attr_groups[];
 extern const struct attribute_group *qeth_osn_attr_groups[];
+extern const struct attribute_group qeth_device_attr_group;
+extern const struct attribute_group qeth_device_blkt_group;
+extern const struct device_type qeth_generic_devtype;
 extern struct workqueue_struct *qeth_wq;
 
 int qeth_card_hw_is_reachable(struct qeth_card *);
diff --git a/drivers/s390/net/qeth_core_main.c b/drivers/s390/net/qeth_core_main.c
index 38114a8d56e0..fc6d85f2b38d 100644
--- a/drivers/s390/net/qeth_core_main.c
+++ b/drivers/s390/net/qeth_core_main.c
@@ -5530,10 +5530,12 @@ void qeth_core_free_discipline(struct qeth_card *card)
 	card->discipline = NULL;
 }
 
-static const struct device_type qeth_generic_devtype = {
+const struct device_type qeth_generic_devtype = {
 	.name = "qeth_generic",
 	.groups = qeth_generic_attr_groups,
 };
+EXPORT_SYMBOL_GPL(qeth_generic_devtype);
+
 static const struct device_type qeth_osn_devtype = {
 	.name = "qeth_osn",
 	.groups = qeth_osn_attr_groups,
@@ -5659,23 +5661,22 @@ static int qeth_core_probe_device(struct ccwgroup_device *gdev)
 		goto err_card;
 	}
 
-	if (card->info.type == QETH_CARD_TYPE_OSN)
-		gdev->dev.type = &qeth_osn_devtype;
-	else
-		gdev->dev.type = &qeth_generic_devtype;
-
 	switch (card->info.type) {
 	case QETH_CARD_TYPE_OSN:
 	case QETH_CARD_TYPE_OSM:
 		rc = qeth_core_load_discipline(card, QETH_DISCIPLINE_LAYER2);
 		if (rc)
 			goto err_card;
+
+		gdev->dev.type = (card->info.type != QETH_CARD_TYPE_OSN)
+					? card->discipline->devtype
+					: &qeth_osn_devtype;
 		rc = card->discipline->setup(card->gdev);
 		if (rc)
 			goto err_disc;
-	case QETH_CARD_TYPE_OSD:
-	case QETH_CARD_TYPE_OSX:
+		break;
 	default:
+		gdev->dev.type = &qeth_generic_devtype;
 		break;
 	}
 
@@ -5731,8 +5732,10 @@ static int qeth_core_set_online(struct ccwgroup_device *gdev)
 		if (rc)
 			goto err;
 		rc = card->discipline->setup(card->gdev);
-		if (rc)
+		if (rc) {
+			qeth_core_free_discipline(card);
 			goto err;
+		}
 	}
 	rc = card->discipline->set_online(gdev);
 err:
diff --git a/drivers/s390/net/qeth_core_sys.c b/drivers/s390/net/qeth_core_sys.c
index 75b29fd2fcf4..db6a285d41e0 100644
--- a/drivers/s390/net/qeth_core_sys.c
+++ b/drivers/s390/net/qeth_core_sys.c
@@ -413,12 +413,16 @@ static ssize_t qeth_dev_layer2_store(struct device *dev,
 
 	if (card->options.layer2 == newdis)
 		goto out;
-	else {
-		card->info.mac_bits  = 0;
-		if (card->discipline) {
-			card->discipline->remove(card->gdev);
-			qeth_core_free_discipline(card);
-		}
+	if (card->info.type == QETH_CARD_TYPE_OSM) {
+		/* fixed layer, can't switch */
+		rc = -EOPNOTSUPP;
+		goto out;
+	}
+
+	card->info.mac_bits = 0;
+	if (card->discipline) {
+		card->discipline->remove(card->gdev);
+		qeth_core_free_discipline(card);
 	}
 
 	rc = qeth_core_load_discipline(card, newdis);
@@ -426,6 +430,8 @@ static ssize_t qeth_dev_layer2_store(struct device *dev,
 		goto out;
 
 	rc = card->discipline->setup(card->gdev);
+	if (rc)
+		qeth_core_free_discipline(card);
 out:
 	mutex_unlock(&card->discipline_mutex);
 	return rc ? rc : count;
@@ -703,10 +709,11 @@ static struct attribute *qeth_blkt_device_attrs[] = {
 	&dev_attr_inter_jumbo.attr,
 	NULL,
 };
-static struct attribute_group qeth_device_blkt_group = {
+const struct attribute_group qeth_device_blkt_group = {
 	.name = "blkt",
 	.attrs = qeth_blkt_device_attrs,
 };
+EXPORT_SYMBOL_GPL(qeth_device_blkt_group);
 
 static struct attribute *qeth_device_attrs[] = {
 	&dev_attr_state.attr,
@@ -726,9 +733,10 @@ static struct attribute *qeth_device_attrs[] = {
 	&dev_attr_switch_attrs.attr,
 	NULL,
 };
-static struct attribute_group qeth_device_attr_group = {
+const struct attribute_group qeth_device_attr_group = {
 	.attrs = qeth_device_attrs,
 };
+EXPORT_SYMBOL_GPL(qeth_device_attr_group);
 
 const struct attribute_group *qeth_generic_attr_groups[] = {
 	&qeth_device_attr_group,
diff --git a/drivers/s390/net/qeth_l2.h b/drivers/s390/net/qeth_l2.h
index 29d9fb3890ad..0d59f9a45ea9 100644
--- a/drivers/s390/net/qeth_l2.h
+++ b/drivers/s390/net/qeth_l2.h
@@ -8,6 +8,8 @@
 
 #include "qeth_core.h"
 
+extern const struct attribute_group *qeth_l2_attr_groups[];
+
 int qeth_l2_create_device_attributes(struct device *);
 void qeth_l2_remove_device_attributes(struct device *);
 void qeth_l2_setup_bridgeport_attrs(struct qeth_card *card);
diff --git a/drivers/s390/net/qeth_l2_main.c b/drivers/s390/net/qeth_l2_main.c
index 1b07f382d74c..bd2df62a5cdf 100644
--- a/drivers/s390/net/qeth_l2_main.c
+++ b/drivers/s390/net/qeth_l2_main.c
@@ -880,11 +880,21 @@ static int qeth_l2_stop(struct net_device *dev)
 	return 0;
 }
 
+static const struct device_type qeth_l2_devtype = {
+	.name = "qeth_layer2",
+	.groups = qeth_l2_attr_groups,
+};
+
 static int qeth_l2_probe_device(struct ccwgroup_device *gdev)
 {
 	struct qeth_card *card = dev_get_drvdata(&gdev->dev);
+	int rc;
 
-	qeth_l2_create_device_attributes(&gdev->dev);
+	if (gdev->dev.type == &qeth_generic_devtype) {
+		rc = qeth_l2_create_device_attributes(&gdev->dev);
+		if (rc)
+			return rc;
+	}
 	INIT_LIST_HEAD(&card->vid_list);
 	hash_init(card->mac_htable);
 	card->options.layer2 = 1;
@@ -896,7 +906,8 @@ static void qeth_l2_remove_device(struct ccwgroup_device *cgdev)
 {
 	struct qeth_card *card = dev_get_drvdata(&cgdev->dev);
 
-	qeth_l2_remove_device_attributes(&cgdev->dev);
+	if (cgdev->dev.type == &qeth_generic_devtype)
+		qeth_l2_remove_device_attributes(&cgdev->dev);
 	qeth_set_allowed_threads(card, 0, 1);
 	wait_event(card->wait_q, qeth_threads_running(card, 0xffffffff) == 0);
 
@@ -954,7 +965,6 @@ static int qeth_l2_setup_netdev(struct qeth_card *card)
 	case QETH_CARD_TYPE_OSN:
 		card->dev = alloc_netdev(0, "osn%d", NET_NAME_UNKNOWN,
 					 ether_setup);
-		card->dev->flags |= IFF_NOARP;
 		break;
 	default:
 		card->dev = alloc_etherdev(0);
@@ -969,9 +979,12 @@ static int qeth_l2_setup_netdev(struct qeth_card *card)
 	card->dev->min_mtu = 64;
 	card->dev->max_mtu = ETH_MAX_MTU;
 	card->dev->netdev_ops = &qeth_l2_netdev_ops;
-	card->dev->ethtool_ops =
-		(card->info.type != QETH_CARD_TYPE_OSN) ?
-		&qeth_l2_ethtool_ops : &qeth_l2_osn_ops;
+	if (card->info.type == QETH_CARD_TYPE_OSN) {
+		card->dev->ethtool_ops = &qeth_l2_osn_ops;
+		card->dev->flags |= IFF_NOARP;
+	} else {
+		card->dev->ethtool_ops = &qeth_l2_ethtool_ops;
+	}
 	card->dev->features |= NETIF_F_HW_VLAN_CTAG_FILTER;
 	if (card->info.type == QETH_CARD_TYPE_OSD && !card->info.guestlan) {
 		card->dev->hw_features = NETIF_F_SG;
@@ -1269,6 +1282,7 @@ static int qeth_l2_control_event(struct qeth_card *card,
 }
 
 struct qeth_discipline qeth_l2_discipline = {
+	.devtype = &qeth_l2_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_l2_sys.c b/drivers/s390/net/qeth_l2_sys.c
index 687972356d6b..9696baa49e2d 100644
--- a/drivers/s390/net/qeth_l2_sys.c
+++ b/drivers/s390/net/qeth_l2_sys.c
@@ -269,3 +269,11 @@ void qeth_l2_setup_bridgeport_attrs(struct qeth_card *card)
 	} else
 		qeth_bridgeport_an_set(card, 0);
 }
+
+const struct attribute_group *qeth_l2_attr_groups[] = {
+	&qeth_device_attr_group,
+	&qeth_device_blkt_group,
+	/* l2 specific, see l2_{create,remove}_device_attributes(): */
+	&qeth_l2_bridgeport_attr_group,
+	NULL,
+};
diff --git a/drivers/s390/net/qeth_l3_main.c b/drivers/s390/net/qeth_l3_main.c
index 6e0354ef4b86..d8df1e635163 100644
--- a/drivers/s390/net/qeth_l3_main.c
+++ b/drivers/s390/net/qeth_l3_main.c
@@ -3039,8 +3039,13 @@ static int qeth_l3_setup_netdev(struct qeth_card *card)
 static int qeth_l3_probe_device(struct ccwgroup_device *gdev)
 {
 	struct qeth_card *card = dev_get_drvdata(&gdev->dev);
+	int rc;
 
-	qeth_l3_create_device_attributes(&gdev->dev);
+	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;
 	card->info.hwtrap = 0;
 	return 0;
@@ -3306,6 +3311,7 @@ static int qeth_l3_control_event(struct qeth_card *card,
 }
 
 struct qeth_discipline qeth_l3_discipline = {
+	.devtype = &qeth_generic_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/virtio/virtio_ccw.c b/drivers/s390/virtio/virtio_ccw.c
index 2a76ea78a0bf..b18fe2014cf2 100644
--- a/drivers/s390/virtio/virtio_ccw.c
+++ b/drivers/s390/virtio/virtio_ccw.c
@@ -87,7 +87,7 @@ struct vq_info_block {
 } __packed;
 
 struct virtio_feature_desc {
-	__u32 features;
+	__le32 features;
 	__u8 index;
 } __packed;
 
diff --git a/drivers/scsi/csiostor/csio_hw.c b/drivers/scsi/csiostor/csio_hw.c
index 622bdabc8894..dab195f04da7 100644
--- a/drivers/scsi/csiostor/csio_hw.c
+++ b/drivers/scsi/csiostor/csio_hw.c
@@ -1769,7 +1769,6 @@ csio_hw_use_fwconfig(struct csio_hw *hw, int reset, u32 *fw_cfg_param)
 		goto bye;
 	}
 
-	mempool_free(mbp, hw->mb_mempool);
 	if (finicsum != cfcsum) {
 		csio_warn(hw,
 		      "Config File checksum mismatch: csum=%#x, computed=%#x\n",
@@ -1780,6 +1779,10 @@ csio_hw_use_fwconfig(struct csio_hw *hw, int reset, u32 *fw_cfg_param)
 	rv = csio_hw_validate_caps(hw, mbp);
 	if (rv != 0)
 		goto bye;
+
+	mempool_free(mbp, hw->mb_mempool);
+	mbp = NULL;
+
 	/*
 	 * Note that we're operating with parameters
 	 * not supplied by the driver, rather than from hard-wired
diff --git a/drivers/scsi/cxlflash/Kconfig b/drivers/scsi/cxlflash/Kconfig
index c052104e523e..a011c5dbf214 100644
--- a/drivers/scsi/cxlflash/Kconfig
+++ b/drivers/scsi/cxlflash/Kconfig
@@ -5,6 +5,7 @@
 config CXLFLASH
 	tristate "Support for IBM CAPI Flash"
 	depends on PCI && SCSI && CXL && EEH
+	select IRQ_POLL
 	default m
 	help
 	  Allows CAPI Accelerated IO to Flash
diff --git a/drivers/scsi/libfc/fc_fcp.c b/drivers/scsi/libfc/fc_fcp.c
index a808e8ef1d08..234352da5c3c 100644
--- a/drivers/scsi/libfc/fc_fcp.c
+++ b/drivers/scsi/libfc/fc_fcp.c
@@ -407,11 +407,12 @@ unlock:
  * can_queue. Eventually we will hit the point where we run
  * on all reserved structs.
  */
-static void fc_fcp_can_queue_ramp_down(struct fc_lport *lport)
+static bool fc_fcp_can_queue_ramp_down(struct fc_lport *lport)
 {
 	struct fc_fcp_internal *si = fc_get_scsi_internal(lport);
 	unsigned long flags;
 	int can_queue;
+	bool changed = false;
 
 	spin_lock_irqsave(lport->host->host_lock, flags);
 
@@ -427,9 +428,11 @@ static void fc_fcp_can_queue_ramp_down(struct fc_lport *lport)
 	if (!can_queue)
 		can_queue = 1;
 	lport->host->can_queue = can_queue;
+	changed = true;
 
 unlock:
 	spin_unlock_irqrestore(lport->host->host_lock, flags);
+	return changed;
 }
 
 /*
@@ -1896,11 +1899,11 @@ int fc_queuecommand(struct Scsi_Host *shost, struct scsi_cmnd *sc_cmd)
 
 	if (!fc_fcp_lport_queue_ready(lport)) {
 		if (lport->qfull) {
-			fc_fcp_can_queue_ramp_down(lport);
-			shost_printk(KERN_ERR, lport->host,
-				     "libfc: queue full, "
-				     "reducing can_queue to %d.\n",
-				     lport->host->can_queue);
+			if (fc_fcp_can_queue_ramp_down(lport))
+				shost_printk(KERN_ERR, lport->host,
+					     "libfc: queue full, "
+					     "reducing can_queue to %d.\n",
+					     lport->host->can_queue);
 		}
 		rc = SCSI_MLQUEUE_HOST_BUSY;
 		goto out;
diff --git a/drivers/scsi/libfc/fc_rport.c b/drivers/scsi/libfc/fc_rport.c
index b44c3136eb51..520325867e2b 100644
--- a/drivers/scsi/libfc/fc_rport.c
+++ b/drivers/scsi/libfc/fc_rport.c
@@ -1422,7 +1422,7 @@ static void fc_rport_recv_rtv_req(struct fc_rport_priv *rdata,
 	fp = fc_frame_alloc(lport, sizeof(*rtv));
 	if (!fp) {
 		rjt_data.reason = ELS_RJT_UNAB;
-		rjt_data.reason = ELS_EXPL_INSUF_RES;
+		rjt_data.explan = ELS_EXPL_INSUF_RES;
 		fc_seq_els_rsp_send(in_fp, ELS_LS_RJT, &rjt_data);
 		goto drop;
 	}
diff --git a/drivers/scsi/lpfc/lpfc.h b/drivers/scsi/lpfc/lpfc.h
index 6d7840b096e6..f2c0ba6ced78 100644
--- a/drivers/scsi/lpfc/lpfc.h
+++ b/drivers/scsi/lpfc/lpfc.h
@@ -141,6 +141,13 @@ struct lpfc_dmabuf {
 	uint32_t   buffer_tag;	/* used for tagged queue ring */
 };
 
+struct lpfc_nvmet_ctxbuf {
+	struct list_head list;
+	struct lpfc_nvmet_rcv_ctx *context;
+	struct lpfc_iocbq *iocbq;
+	struct lpfc_sglq *sglq;
+};
+
 struct lpfc_dma_pool {
 	struct lpfc_dmabuf   *elements;
 	uint32_t    max_count;
@@ -163,9 +170,7 @@ struct rqb_dmabuf {
 	struct lpfc_dmabuf dbuf;
 	uint16_t total_size;
 	uint16_t bytes_recv;
-	void *context;
-	struct lpfc_iocbq *iocbq;
-	struct lpfc_sglq *sglq;
+	uint16_t idx;
 	struct lpfc_queue *hrq;	  /* ptr to associated Header RQ */
 	struct lpfc_queue *drq;	  /* ptr to associated Data RQ */
 };
@@ -670,6 +675,8 @@ struct lpfc_hba {
 					/* INIT_LINK mailbox command */
 #define LS_NPIV_FAB_SUPPORTED 0x2	/* Fabric supports NPIV */
 #define LS_IGNORE_ERATT       0x4	/* intr handler should ignore ERATT */
+#define LS_MDS_LINK_DOWN      0x8	/* MDS Diagnostics Link Down */
+#define LS_MDS_LOOPBACK      0x16	/* MDS Diagnostics Link Up (Loopback) */
 
 	uint32_t hba_flag;	/* hba generic flags */
 #define HBA_ERATT_HANDLED	0x1 /* This flag is set when eratt handled */
@@ -777,7 +784,6 @@ struct lpfc_hba {
 	uint32_t cfg_nvme_oas;
 	uint32_t cfg_nvme_io_channel;
 	uint32_t cfg_nvmet_mrq;
-	uint32_t cfg_nvmet_mrq_post;
 	uint32_t cfg_enable_nvmet;
 	uint32_t cfg_nvme_enable_fb;
 	uint32_t cfg_nvmet_fb_size;
@@ -943,6 +949,7 @@ struct lpfc_hba {
 	struct pci_pool *lpfc_mbuf_pool;
 	struct pci_pool *lpfc_hrb_pool;	/* header receive buffer pool */
 	struct pci_pool *lpfc_drb_pool; /* data receive buffer pool */
+	struct pci_pool *lpfc_nvmet_drb_pool; /* data receive buffer pool */
 	struct pci_pool *lpfc_hbq_pool;	/* SLI3 hbq buffer pool */
 	struct pci_pool *txrdy_payload_pool;
 	struct lpfc_dma_pool lpfc_mbuf_safety_pool;
@@ -1228,7 +1235,11 @@ lpfc_sli_read_hs(struct lpfc_hba *phba)
 static inline struct lpfc_sli_ring *
 lpfc_phba_elsring(struct lpfc_hba *phba)
 {
-	if (phba->sli_rev == LPFC_SLI_REV4)
-		return phba->sli4_hba.els_wq->pring;
+	if (phba->sli_rev == LPFC_SLI_REV4) {
+		if (phba->sli4_hba.els_wq)
+			return phba->sli4_hba.els_wq->pring;
+		else
+			return NULL;
+	}
 	return &phba->sli.sli3_ring[LPFC_ELS_RING];
 }
diff --git a/drivers/scsi/lpfc/lpfc_attr.c b/drivers/scsi/lpfc/lpfc_attr.c
index 4830370bfab1..bb2d9e238225 100644
--- a/drivers/scsi/lpfc/lpfc_attr.c
+++ b/drivers/scsi/lpfc/lpfc_attr.c
@@ -60,9 +60,9 @@
 #define LPFC_MIN_DEVLOSS_TMO	1
 #define LPFC_MAX_DEVLOSS_TMO	255
 
-#define LPFC_DEF_MRQ_POST	256
-#define LPFC_MIN_MRQ_POST	32
-#define LPFC_MAX_MRQ_POST	512
+#define LPFC_DEF_MRQ_POST	512
+#define LPFC_MIN_MRQ_POST	512
+#define LPFC_MAX_MRQ_POST	2048
 
 /*
  * Write key size should be multiple of 4. If write key is changed
@@ -205,8 +205,9 @@ lpfc_nvme_info_show(struct device *dev, struct device_attribute *attr,
 				atomic_read(&tgtp->xmt_ls_rsp_error));
 
 		len += snprintf(buf+len, PAGE_SIZE-len,
-				"FCP: Rcv %08x Drop %08x\n",
+				"FCP: Rcv %08x Release %08x Drop %08x\n",
 				atomic_read(&tgtp->rcv_fcp_cmd_in),
+				atomic_read(&tgtp->xmt_fcp_release),
 				atomic_read(&tgtp->rcv_fcp_cmd_drop));
 
 		if (atomic_read(&tgtp->rcv_fcp_cmd_in) !=
@@ -218,15 +219,12 @@ lpfc_nvme_info_show(struct device *dev, struct device_attribute *attr,
 		}
 
 		len += snprintf(buf+len, PAGE_SIZE-len,
-				"FCP Rsp: RD %08x rsp %08x WR %08x rsp %08x\n",
+				"FCP Rsp: RD %08x rsp %08x WR %08x rsp %08x "
+				"drop %08x\n",
 				atomic_read(&tgtp->xmt_fcp_read),
 				atomic_read(&tgtp->xmt_fcp_read_rsp),
 				atomic_read(&tgtp->xmt_fcp_write),
-				atomic_read(&tgtp->xmt_fcp_rsp));
-
-		len += snprintf(buf+len, PAGE_SIZE-len,
-				"FCP Rsp: abort %08x drop %08x\n",
-				atomic_read(&tgtp->xmt_fcp_abort),
+				atomic_read(&tgtp->xmt_fcp_rsp),
 				atomic_read(&tgtp->xmt_fcp_drop));
 
 		len += snprintf(buf+len, PAGE_SIZE-len,
@@ -236,10 +234,22 @@ lpfc_nvme_info_show(struct device *dev, struct device_attribute *attr,
 				atomic_read(&tgtp->xmt_fcp_rsp_drop));
 
 		len += snprintf(buf+len, PAGE_SIZE-len,
-				"ABORT: Xmt %08x Err %08x Cmpl %08x",
+				"ABORT: Xmt %08x Cmpl %08x\n",
+				atomic_read(&tgtp->xmt_fcp_abort),
+				atomic_read(&tgtp->xmt_fcp_abort_cmpl));
+
+		len += snprintf(buf + len, PAGE_SIZE - len,
+				"ABORT: Sol %08x  Usol %08x Err %08x Cmpl %08x",
+				atomic_read(&tgtp->xmt_abort_sol),
+				atomic_read(&tgtp->xmt_abort_unsol),
 				atomic_read(&tgtp->xmt_abort_rsp),
-				atomic_read(&tgtp->xmt_abort_rsp_error),
-				atomic_read(&tgtp->xmt_abort_cmpl));
+				atomic_read(&tgtp->xmt_abort_rsp_error));
+
+		len += snprintf(buf + len, PAGE_SIZE - len,
+				"IO_CTX: %08x outstanding %08x total %x",
+				phba->sli4_hba.nvmet_ctx_cnt,
+				phba->sli4_hba.nvmet_io_wait_cnt,
+				phba->sli4_hba.nvmet_io_wait_total);
 
 		len +=  snprintf(buf+len, PAGE_SIZE-len, "\n");
 		return len;
@@ -3312,14 +3322,6 @@ LPFC_ATTR_R(nvmet_mrq,
 	    "Specify number of RQ pairs for processing NVMET cmds");
 
 /*
- * lpfc_nvmet_mrq_post: Specify number buffers to post on every MRQ
- *
- */
-LPFC_ATTR_R(nvmet_mrq_post, LPFC_DEF_MRQ_POST,
-	    LPFC_MIN_MRQ_POST, LPFC_MAX_MRQ_POST,
-	    "Specify number of buffers to post on every MRQ");
-
-/*
  * lpfc_enable_fc4_type: Defines what FC4 types are supported.
  * Supported Values:  1 - register just FCP
  *                    3 - register both FCP and NVME
@@ -5154,7 +5156,6 @@ struct device_attribute *lpfc_hba_attrs[] = {
 	&dev_attr_lpfc_suppress_rsp,
 	&dev_attr_lpfc_nvme_io_channel,
 	&dev_attr_lpfc_nvmet_mrq,
-	&dev_attr_lpfc_nvmet_mrq_post,
 	&dev_attr_lpfc_nvme_enable_fb,
 	&dev_attr_lpfc_nvmet_fb_size,
 	&dev_attr_lpfc_enable_bg,
@@ -6194,7 +6195,6 @@ lpfc_get_cfgparam(struct lpfc_hba *phba)
 
 	lpfc_enable_fc4_type_init(phba, lpfc_enable_fc4_type);
 	lpfc_nvmet_mrq_init(phba, lpfc_nvmet_mrq);
-	lpfc_nvmet_mrq_post_init(phba, lpfc_nvmet_mrq_post);
 
 	/* Initialize first burst. Target vs Initiator are different. */
 	lpfc_nvme_enable_fb_init(phba, lpfc_nvme_enable_fb);
@@ -6291,7 +6291,6 @@ lpfc_nvme_mod_param_dep(struct lpfc_hba *phba)
 		/* Not NVME Target mode.  Turn off Target parameters. */
 		phba->nvmet_support = 0;
 		phba->cfg_nvmet_mrq = 0;
-		phba->cfg_nvmet_mrq_post = 0;
 		phba->cfg_nvmet_fb_size = 0;
 	}
 
diff --git a/drivers/scsi/lpfc/lpfc_crtn.h b/drivers/scsi/lpfc/lpfc_crtn.h
index 944b32ca4931..8912767e7bc8 100644
--- a/drivers/scsi/lpfc/lpfc_crtn.h
+++ b/drivers/scsi/lpfc/lpfc_crtn.h
@@ -75,6 +75,10 @@ void lpfc_init_vpi_cmpl(struct lpfc_hba *, LPFC_MBOXQ_t *);
 void lpfc_cancel_all_vport_retry_delay_timer(struct lpfc_hba *);
 void lpfc_retry_pport_discovery(struct lpfc_hba *);
 void lpfc_release_rpi(struct lpfc_hba *, struct lpfc_vport *, uint16_t);
+int lpfc_init_iocb_list(struct lpfc_hba *phba, int cnt);
+void lpfc_free_iocb_list(struct lpfc_hba *phba);
+int lpfc_post_rq_buffer(struct lpfc_hba *phba, struct lpfc_queue *hrq,
+			struct lpfc_queue *drq, int count, int idx);
 
 void lpfc_mbx_cmpl_local_config_link(struct lpfc_hba *, LPFC_MBOXQ_t *);
 void lpfc_mbx_cmpl_reg_login(struct lpfc_hba *, LPFC_MBOXQ_t *);
@@ -246,16 +250,14 @@ struct hbq_dmabuf *lpfc_sli4_rb_alloc(struct lpfc_hba *);
 void lpfc_sli4_rb_free(struct lpfc_hba *, struct hbq_dmabuf *);
 struct rqb_dmabuf *lpfc_sli4_nvmet_alloc(struct lpfc_hba *phba);
 void lpfc_sli4_nvmet_free(struct lpfc_hba *phba, struct rqb_dmabuf *dmab);
-void lpfc_nvmet_rq_post(struct lpfc_hba *phba, struct lpfc_nvmet_rcv_ctx *ctxp,
-			struct lpfc_dmabuf *mp);
+void lpfc_nvmet_ctxbuf_post(struct lpfc_hba *phba,
+			    struct lpfc_nvmet_ctxbuf *ctxp);
 int lpfc_nvmet_rcv_unsol_abort(struct lpfc_vport *vport,
 			       struct fc_frame_header *fc_hdr);
 void lpfc_sli4_build_dflt_fcf_record(struct lpfc_hba *, struct fcf_record *,
 			uint16_t);
 int lpfc_sli4_rq_put(struct lpfc_queue *hq, struct lpfc_queue *dq,
 		     struct lpfc_rqe *hrqe, struct lpfc_rqe *drqe);
-int lpfc_post_rq_buffer(struct lpfc_hba *phba, struct lpfc_queue *hq,
-			struct lpfc_queue *dq, int count);
 int lpfc_free_rq_buffer(struct lpfc_hba *phba, struct lpfc_queue *hq);
 void lpfc_unregister_fcf(struct lpfc_hba *);
 void lpfc_unregister_fcf_rescan(struct lpfc_hba *);
@@ -271,6 +273,7 @@ int lpfc_sli4_fcf_rr_next_proc(struct lpfc_vport *, uint16_t);
 void lpfc_sli4_clear_fcf_rr_bmask(struct lpfc_hba *);
 
 int lpfc_mem_alloc(struct lpfc_hba *, int align);
+int lpfc_nvmet_mem_alloc(struct lpfc_hba *phba);
 int lpfc_mem_alloc_active_rrq_pool_s4(struct lpfc_hba *);
 void lpfc_mem_free(struct lpfc_hba *);
 void lpfc_mem_free_all(struct lpfc_hba *);
@@ -294,6 +297,7 @@ int lpfc_selective_reset(struct lpfc_hba *);
 void lpfc_reset_barrier(struct lpfc_hba *);
 int lpfc_sli_brdready(struct lpfc_hba *, uint32_t);
 int lpfc_sli_brdkill(struct lpfc_hba *);
+int lpfc_sli_chipset_init(struct lpfc_hba *phba);
 int lpfc_sli_brdreset(struct lpfc_hba *);
 int lpfc_sli_brdrestart(struct lpfc_hba *);
 int lpfc_sli_hba_setup(struct lpfc_hba *);
diff --git a/drivers/scsi/lpfc/lpfc_ct.c b/drivers/scsi/lpfc/lpfc_ct.c
index 1487406aea77..f2cd19c6c2df 100644
--- a/drivers/scsi/lpfc/lpfc_ct.c
+++ b/drivers/scsi/lpfc/lpfc_ct.c
@@ -630,7 +630,7 @@ lpfc_ns_rsp(struct lpfc_vport *vport, struct lpfc_dmabuf *mp, uint8_t fc4_type,
 						NLP_EVT_DEVICE_RECOVERY);
 			spin_lock_irq(shost->host_lock);
 			ndlp->nlp_flag &= ~NLP_NVMET_RECOV;
-			spin_lock_irq(shost->host_lock);
+			spin_unlock_irq(shost->host_lock);
 		}
 	}
 
@@ -2092,6 +2092,7 @@ lpfc_fdmi_port_attr_fc4type(struct lpfc_vport *vport,
 
 	ae->un.AttrTypes[3] = 0x02; /* Type 1 - ELS */
 	ae->un.AttrTypes[2] = 0x01; /* Type 8 - FCP */
+	ae->un.AttrTypes[6] = 0x01; /* Type 40 - NVME */
 	ae->un.AttrTypes[7] = 0x01; /* Type 32 - CT */
 	size = FOURBYTES + 32;
 	ad->AttrLen = cpu_to_be16(size);
diff --git a/drivers/scsi/lpfc/lpfc_debugfs.c b/drivers/scsi/lpfc/lpfc_debugfs.c
index fce549a91911..4bcb92c844ca 100644
--- a/drivers/scsi/lpfc/lpfc_debugfs.c
+++ b/drivers/scsi/lpfc/lpfc_debugfs.c
@@ -798,21 +798,22 @@ lpfc_debugfs_nvmestat_data(struct lpfc_vport *vport, char *buf, int size)
 				atomic_read(&tgtp->xmt_fcp_rsp));
 
 		len += snprintf(buf + len, size - len,
-				"FCP Rsp: abort %08x drop %08x\n",
-				atomic_read(&tgtp->xmt_fcp_abort),
-				atomic_read(&tgtp->xmt_fcp_drop));
-
-		len += snprintf(buf + len, size - len,
 				"FCP Rsp Cmpl: %08x err %08x drop %08x\n",
 				atomic_read(&tgtp->xmt_fcp_rsp_cmpl),
 				atomic_read(&tgtp->xmt_fcp_rsp_error),
 				atomic_read(&tgtp->xmt_fcp_rsp_drop));
 
 		len += snprintf(buf + len, size - len,
-				"ABORT: Xmt %08x Err %08x Cmpl %08x",
+				"ABORT: Xmt %08x Cmpl %08x\n",
+				atomic_read(&tgtp->xmt_fcp_abort),
+				atomic_read(&tgtp->xmt_fcp_abort_cmpl));
+
+		len += snprintf(buf + len, size - len,
+				"ABORT: Sol %08x  Usol %08x Err %08x Cmpl %08x",
+				atomic_read(&tgtp->xmt_abort_sol),
+				atomic_read(&tgtp->xmt_abort_unsol),
 				atomic_read(&tgtp->xmt_abort_rsp),
-				atomic_read(&tgtp->xmt_abort_rsp_error),
-				atomic_read(&tgtp->xmt_abort_cmpl));
+				atomic_read(&tgtp->xmt_abort_rsp_error));
 
 		len +=  snprintf(buf + len, size - len, "\n");
 
@@ -841,6 +842,12 @@ lpfc_debugfs_nvmestat_data(struct lpfc_vport *vport, char *buf, int size)
 			}
 			spin_unlock(&phba->sli4_hba.abts_nvme_buf_list_lock);
 		}
+
+		len += snprintf(buf + len, size - len,
+				"IO_CTX: %08x  outstanding %08x total %08x\n",
+				phba->sli4_hba.nvmet_ctx_cnt,
+				phba->sli4_hba.nvmet_io_wait_cnt,
+				phba->sli4_hba.nvmet_io_wait_total);
 	} else {
 		if (!(phba->cfg_enable_fc4_type & LPFC_ENABLE_NVME))
 			return len;
@@ -1959,6 +1966,7 @@ lpfc_debugfs_nvmestat_write(struct file *file, const char __user *buf,
 		atomic_set(&tgtp->rcv_ls_req_out, 0);
 		atomic_set(&tgtp->rcv_ls_req_drop, 0);
 		atomic_set(&tgtp->xmt_ls_abort, 0);
+		atomic_set(&tgtp->xmt_ls_abort_cmpl, 0);
 		atomic_set(&tgtp->xmt_ls_rsp, 0);
 		atomic_set(&tgtp->xmt_ls_drop, 0);
 		atomic_set(&tgtp->xmt_ls_rsp_error, 0);
@@ -1967,19 +1975,22 @@ lpfc_debugfs_nvmestat_write(struct file *file, const char __user *buf,
 		atomic_set(&tgtp->rcv_fcp_cmd_in, 0);
 		atomic_set(&tgtp->rcv_fcp_cmd_out, 0);
 		atomic_set(&tgtp->rcv_fcp_cmd_drop, 0);
-		atomic_set(&tgtp->xmt_fcp_abort, 0);
 		atomic_set(&tgtp->xmt_fcp_drop, 0);
 		atomic_set(&tgtp->xmt_fcp_read_rsp, 0);
 		atomic_set(&tgtp->xmt_fcp_read, 0);
 		atomic_set(&tgtp->xmt_fcp_write, 0);
 		atomic_set(&tgtp->xmt_fcp_rsp, 0);
+		atomic_set(&tgtp->xmt_fcp_release, 0);
 		atomic_set(&tgtp->xmt_fcp_rsp_cmpl, 0);
 		atomic_set(&tgtp->xmt_fcp_rsp_error, 0);
 		atomic_set(&tgtp->xmt_fcp_rsp_drop, 0);
 
+		atomic_set(&tgtp->xmt_fcp_abort, 0);
+		atomic_set(&tgtp->xmt_fcp_abort_cmpl, 0);
+		atomic_set(&tgtp->xmt_abort_sol, 0);
+		atomic_set(&tgtp->xmt_abort_unsol, 0);
 		atomic_set(&tgtp->xmt_abort_rsp, 0);
 		atomic_set(&tgtp->xmt_abort_rsp_error, 0);
-		atomic_set(&tgtp->xmt_abort_cmpl, 0);
 	}
 	return nbytes;
 }
@@ -3070,11 +3081,11 @@ __lpfc_idiag_print_wq(struct lpfc_queue *qp, char *wqtype,
 			qp->assoc_qid, qp->q_cnt_1,
 			(unsigned long long)qp->q_cnt_4);
 	len += snprintf(pbuffer + len, LPFC_QUE_INFO_GET_BUF_SIZE - len,
-			"\t\tWQID[%02d], QE-CNT[%04d], QE-SIZE[%04d], "
-			"HOST-IDX[%04d], PORT-IDX[%04d]",
+			"\t\tWQID[%02d], QE-CNT[%04d], QE-SZ[%04d], "
+			"HST-IDX[%04d], PRT-IDX[%04d], PST[%03d]",
 			qp->queue_id, qp->entry_count,
 			qp->entry_size, qp->host_index,
-			qp->hba_index);
+			qp->hba_index, qp->entry_repost);
 	len +=  snprintf(pbuffer + len,
 			LPFC_QUE_INFO_GET_BUF_SIZE - len, "\n");
 	return len;
@@ -3121,11 +3132,11 @@ __lpfc_idiag_print_cq(struct lpfc_queue *qp, char *cqtype,
 			qp->assoc_qid, qp->q_cnt_1, qp->q_cnt_2,
 			qp->q_cnt_3, (unsigned long long)qp->q_cnt_4);
 	len += snprintf(pbuffer + len, LPFC_QUE_INFO_GET_BUF_SIZE - len,
-			"\tCQID[%02d], QE-CNT[%04d], QE-SIZE[%04d], "
-			"HOST-IDX[%04d], PORT-IDX[%04d]",
+			"\tCQID[%02d], QE-CNT[%04d], QE-SZ[%04d], "
+			"HST-IDX[%04d], PRT-IDX[%04d], PST[%03d]",
 			qp->queue_id, qp->entry_count,
 			qp->entry_size, qp->host_index,
-			qp->hba_index);
+			qp->hba_index, qp->entry_repost);
 
 	len +=  snprintf(pbuffer + len, LPFC_QUE_INFO_GET_BUF_SIZE - len, "\n");
 
@@ -3143,20 +3154,20 @@ __lpfc_idiag_print_rqpair(struct lpfc_queue *qp, struct lpfc_queue *datqp,
 			"\t\t%s RQ info: ", rqtype);
 	len += snprintf(pbuffer + len, LPFC_QUE_INFO_GET_BUF_SIZE - len,
 			"AssocCQID[%02d]: RQ-STAT[nopost:x%x nobuf:x%x "
-			"trunc:x%x rcv:x%llx]\n",
+			"posted:x%x rcv:x%llx]\n",
 			qp->assoc_qid, qp->q_cnt_1, qp->q_cnt_2,
 			qp->q_cnt_3, (unsigned long long)qp->q_cnt_4);
 	len += snprintf(pbuffer + len, LPFC_QUE_INFO_GET_BUF_SIZE - len,
-			"\t\tHQID[%02d], QE-CNT[%04d], QE-SIZE[%04d], "
-			"HOST-IDX[%04d], PORT-IDX[%04d]\n",
+			"\t\tHQID[%02d], QE-CNT[%04d], QE-SZ[%04d], "
+			"HST-IDX[%04d], PRT-IDX[%04d], PST[%03d]\n",
 			qp->queue_id, qp->entry_count, qp->entry_size,
-			qp->host_index, qp->hba_index);
+			qp->host_index, qp->hba_index, qp->entry_repost);
 	len += snprintf(pbuffer + len, LPFC_QUE_INFO_GET_BUF_SIZE - len,
-			"\t\tDQID[%02d], QE-CNT[%04d], QE-SIZE[%04d], "
-			"HOST-IDX[%04d], PORT-IDX[%04d]\n",
+			"\t\tDQID[%02d], QE-CNT[%04d], QE-SZ[%04d], "
+			"HST-IDX[%04d], PRT-IDX[%04d], PST[%03d]\n",
 			datqp->queue_id, datqp->entry_count,
 			datqp->entry_size, datqp->host_index,
-			datqp->hba_index);
+			datqp->hba_index, datqp->entry_repost);
 	return len;
 }
 
@@ -3242,10 +3253,10 @@ __lpfc_idiag_print_eq(struct lpfc_queue *qp, char *eqtype,
 			eqtype, qp->q_cnt_1, qp->q_cnt_2, qp->q_cnt_3,
 			(unsigned long long)qp->q_cnt_4);
 	len += snprintf(pbuffer + len, LPFC_QUE_INFO_GET_BUF_SIZE - len,
-			"EQID[%02d], QE-CNT[%04d], QE-SIZE[%04d], "
-			"HOST-IDX[%04d], PORT-IDX[%04d]",
+			"EQID[%02d], QE-CNT[%04d], QE-SZ[%04d], "
+			"HST-IDX[%04d], PRT-IDX[%04d], PST[%03d]",
 			qp->queue_id, qp->entry_count, qp->entry_size,
-			qp->host_index, qp->hba_index);
+			qp->host_index, qp->hba_index, qp->entry_repost);
 	len +=  snprintf(pbuffer + len, LPFC_QUE_INFO_GET_BUF_SIZE - len, "\n");
 
 	return len;
@@ -5855,8 +5866,10 @@ lpfc_debugfs_terminate(struct lpfc_vport *vport)
 			atomic_dec(&lpfc_debugfs_hba_count);
 		}
 
-		debugfs_remove(lpfc_debugfs_root); /* lpfc */
-		lpfc_debugfs_root = NULL;
+		if (atomic_read(&lpfc_debugfs_hba_count) == 0) {
+			debugfs_remove(lpfc_debugfs_root); /* lpfc */
+			lpfc_debugfs_root = NULL;
+		}
 	}
 #endif
 	return;
diff --git a/drivers/scsi/lpfc/lpfc_disc.h b/drivers/scsi/lpfc/lpfc_disc.h
index 9d5a379f4b15..094c97b9e5f7 100644
--- a/drivers/scsi/lpfc/lpfc_disc.h
+++ b/drivers/scsi/lpfc/lpfc_disc.h
@@ -90,6 +90,7 @@ struct lpfc_nodelist {
 #define NLP_FCP_INITIATOR  0x10			/* entry is an FCP Initiator */
 #define NLP_NVME_TARGET    0x20			/* entry is a NVME Target */
 #define NLP_NVME_INITIATOR 0x40			/* entry is a NVME Initiator */
+#define NLP_NVME_DISCOVERY 0x80                 /* entry has NVME disc srvc */
 
 	uint16_t	nlp_fc4_type;		/* FC types node supports. */
 						/* Assigned from GID_FF, only
diff --git a/drivers/scsi/lpfc/lpfc_els.c b/drivers/scsi/lpfc/lpfc_els.c
index 67827e397431..8e532b39ae93 100644
--- a/drivers/scsi/lpfc/lpfc_els.c
+++ b/drivers/scsi/lpfc/lpfc_els.c
@@ -1047,6 +1047,13 @@ stop_rr_fcf_flogi:
 				 irsp->ulpStatus, irsp->un.ulpWord[4],
 				 irsp->ulpTimeout);
 
+
+		/* If this is not a loop open failure, bail out */
+		if (!(irsp->ulpStatus == IOSTAT_LOCAL_REJECT &&
+		      ((irsp->un.ulpWord[4] & IOERR_PARAM_MASK) ==
+					IOERR_LOOP_OPEN_FAILURE)))
+			goto flogifail;
+
 		/* FLOGI failed, so there is no fabric */
 		spin_lock_irq(shost->host_lock);
 		vport->fc_flag &= ~(FC_FABRIC | FC_PUBLIC_LOOP);
@@ -2077,16 +2084,19 @@ lpfc_cmpl_els_prli(struct lpfc_hba *phba, struct lpfc_iocbq *cmdiocb,
 
 	if (irsp->ulpStatus) {
 		/* Check for retry */
+		ndlp->fc4_prli_sent--;
 		if (lpfc_els_retry(phba, cmdiocb, rspiocb)) {
 			/* ELS command is being retried */
-			ndlp->fc4_prli_sent--;
 			goto out;
 		}
+
 		/* PRLI failed */
 		lpfc_printf_vlog(vport, KERN_ERR, LOG_ELS,
-				 "2754 PRLI failure DID:%06X Status:x%x/x%x\n",
+				 "2754 PRLI failure DID:%06X Status:x%x/x%x, "
+				 "data: x%x\n",
 				 ndlp->nlp_DID, irsp->ulpStatus,
-				 irsp->un.ulpWord[4]);
+				 irsp->un.ulpWord[4], ndlp->fc4_prli_sent);
+
 		/* Do not call DSM for lpfc_els_abort'ed ELS cmds */
 		if (lpfc_error_lost_link(irsp))
 			goto out;
@@ -7441,6 +7451,13 @@ lpfc_els_flush_cmd(struct lpfc_vport *vport)
 	 */
 	spin_lock_irq(&phba->hbalock);
 	pring = lpfc_phba_elsring(phba);
+
+	/* Bail out if we've no ELS wq, like in PCI error recovery case. */
+	if (unlikely(!pring)) {
+		spin_unlock_irq(&phba->hbalock);
+		return;
+	}
+
 	if (phba->sli_rev == LPFC_SLI_REV4)
 		spin_lock(&pring->ring_lock);
 
@@ -8667,7 +8684,8 @@ lpfc_cmpl_els_fdisc(struct lpfc_hba *phba, struct lpfc_iocbq *cmdiocb,
 		lpfc_do_scr_ns_plogi(phba, vport);
 	goto out;
 fdisc_failed:
-	if (vport->fc_vport->vport_state != FC_VPORT_NO_FABRIC_RSCS)
+	if (vport->fc_vport &&
+	    (vport->fc_vport->vport_state != FC_VPORT_NO_FABRIC_RSCS))
 		lpfc_vport_set_state(vport, FC_VPORT_FAILED);
 	/* Cancel discovery timer */
 	lpfc_can_disctmo(vport);
diff --git a/drivers/scsi/lpfc/lpfc_hbadisc.c b/drivers/scsi/lpfc/lpfc_hbadisc.c
index 0482c5580331..3ffcd9215ca8 100644
--- a/drivers/scsi/lpfc/lpfc_hbadisc.c
+++ b/drivers/scsi/lpfc/lpfc_hbadisc.c
@@ -693,15 +693,16 @@ lpfc_work_done(struct lpfc_hba *phba)
 	pring = lpfc_phba_elsring(phba);
 	status = (ha_copy & (HA_RXMASK  << (4*LPFC_ELS_RING)));
 	status >>= (4*LPFC_ELS_RING);
-	if ((status & HA_RXMASK) ||
-	    (pring->flag & LPFC_DEFERRED_RING_EVENT) ||
-	    (phba->hba_flag & HBA_SP_QUEUE_EVT)) {
+	if (pring && (status & HA_RXMASK ||
+		      pring->flag & LPFC_DEFERRED_RING_EVENT ||
+		      phba->hba_flag & HBA_SP_QUEUE_EVT)) {
 		if (pring->flag & LPFC_STOP_IOCB_EVENT) {
 			pring->flag |= LPFC_DEFERRED_RING_EVENT;
 			/* Set the lpfc data pending flag */
 			set_bit(LPFC_DATA_READY, &phba->data_flags);
 		} else {
-			if (phba->link_state >= LPFC_LINK_UP) {
+			if (phba->link_state >= LPFC_LINK_UP ||
+			    phba->link_flag & LS_MDS_LOOPBACK) {
 				pring->flag &= ~LPFC_DEFERRED_RING_EVENT;
 				lpfc_sli_handle_slow_ring_event(phba, pring,
 								(status &
diff --git a/drivers/scsi/lpfc/lpfc_hw4.h b/drivers/scsi/lpfc/lpfc_hw4.h
index 1d12f2be36bc..e0a5fce416ae 100644
--- a/drivers/scsi/lpfc/lpfc_hw4.h
+++ b/drivers/scsi/lpfc/lpfc_hw4.h
@@ -1356,6 +1356,7 @@ struct lpfc_mbx_wq_destroy {
 
 #define LPFC_HDR_BUF_SIZE 128
 #define LPFC_DATA_BUF_SIZE 2048
+#define LPFC_NVMET_DATA_BUF_SIZE 128
 struct rq_context {
 	uint32_t word0;
 #define lpfc_rq_context_rqe_count_SHIFT	16	/* Version 0 Only */
@@ -4420,6 +4421,19 @@ struct fcp_treceive64_wqe {
 };
 #define TXRDY_PAYLOAD_LEN      12
 
+#define CMD_SEND_FRAME	0xE1
+
+struct send_frame_wqe {
+	struct ulp_bde64 bde;          /* words 0-2 */
+	uint32_t frame_len;            /* word 3 */
+	uint32_t fc_hdr_wd0;           /* word 4 */
+	uint32_t fc_hdr_wd1;           /* word 5 */
+	struct wqe_common wqe_com;     /* words 6-11 */
+	uint32_t fc_hdr_wd2;           /* word 12 */
+	uint32_t fc_hdr_wd3;           /* word 13 */
+	uint32_t fc_hdr_wd4;           /* word 14 */
+	uint32_t fc_hdr_wd5;           /* word 15 */
+};
 
 union lpfc_wqe {
 	uint32_t words[16];
@@ -4438,7 +4452,7 @@ union lpfc_wqe {
 	struct fcp_trsp64_wqe fcp_trsp;
 	struct fcp_tsend64_wqe fcp_tsend;
 	struct fcp_treceive64_wqe fcp_treceive;
-
+	struct send_frame_wqe send_frame;
 };
 
 union lpfc_wqe128 {
diff --git a/drivers/scsi/lpfc/lpfc_init.c b/drivers/scsi/lpfc/lpfc_init.c
index 90ae354a9c45..9add9473cae5 100644
--- a/drivers/scsi/lpfc/lpfc_init.c
+++ b/drivers/scsi/lpfc/lpfc_init.c
@@ -1099,7 +1099,7 @@ lpfc_hba_down_post_s4(struct lpfc_hba *phba)
 
 		list_for_each_entry_safe(ctxp, ctxp_next, &nvmet_aborts, list) {
 			ctxp->flag &= ~(LPFC_NVMET_XBUSY | LPFC_NVMET_ABORT_OP);
-			lpfc_nvmet_rq_post(phba, ctxp, &ctxp->rqb_buffer->hbuf);
+			lpfc_nvmet_ctxbuf_post(phba, ctxp->ctxbuf);
 		}
 	}
 
@@ -3381,7 +3381,7 @@ lpfc_sli4_nvmet_sgl_update(struct lpfc_hba *phba)
 {
 	struct lpfc_sglq *sglq_entry = NULL, *sglq_entry_next = NULL;
 	uint16_t i, lxri, xri_cnt, els_xri_cnt;
-	uint16_t nvmet_xri_cnt, tot_cnt;
+	uint16_t nvmet_xri_cnt;
 	LIST_HEAD(nvmet_sgl_list);
 	int rc;
 
@@ -3389,15 +3389,9 @@ lpfc_sli4_nvmet_sgl_update(struct lpfc_hba *phba)
 	 * update on pci function's nvmet xri-sgl list
 	 */
 	els_xri_cnt = lpfc_sli4_get_els_iocb_cnt(phba);
-	nvmet_xri_cnt = phba->cfg_nvmet_mrq * phba->cfg_nvmet_mrq_post;
-	tot_cnt = phba->sli4_hba.max_cfg_param.max_xri - els_xri_cnt;
-	if (nvmet_xri_cnt > tot_cnt) {
-		phba->cfg_nvmet_mrq_post = tot_cnt / phba->cfg_nvmet_mrq;
-		nvmet_xri_cnt = phba->cfg_nvmet_mrq * phba->cfg_nvmet_mrq_post;
-		lpfc_printf_log(phba, KERN_INFO, LOG_SLI,
-				"6301 NVMET post-sgl count changed to %d\n",
-				phba->cfg_nvmet_mrq_post);
-	}
+
+	/* For NVMET, ALL remaining XRIs are dedicated for IO processing */
+	nvmet_xri_cnt = phba->sli4_hba.max_cfg_param.max_xri - els_xri_cnt;
 
 	if (nvmet_xri_cnt > phba->sli4_hba.nvmet_xri_cnt) {
 		/* els xri-sgl expanded */
@@ -3602,6 +3596,13 @@ lpfc_get_wwpn(struct lpfc_hba *phba)
 	LPFC_MBOXQ_t *mboxq;
 	MAILBOX_t *mb;
 
+	if (phba->sli_rev < LPFC_SLI_REV4) {
+		/* Reset the port first */
+		lpfc_sli_brdrestart(phba);
+		rc = lpfc_sli_chipset_init(phba);
+		if (rc)
+			return (uint64_t)-1;
+	}
 
 	mboxq = (LPFC_MBOXQ_t *) mempool_alloc(phba->mbox_mem_pool,
 						GFP_KERNEL);
@@ -4539,6 +4540,19 @@ lpfc_sli4_async_fc_evt(struct lpfc_hba *phba, struct lpfc_acqe_fc_la *acqe_fc)
 	pmb->vport = phba->pport;
 
 	if (phba->sli4_hba.link_state.status != LPFC_FC_LA_TYPE_LINK_UP) {
+		phba->link_flag &= ~(LS_MDS_LINK_DOWN | LS_MDS_LOOPBACK);
+
+		switch (phba->sli4_hba.link_state.status) {
+		case LPFC_FC_LA_TYPE_MDS_LINK_DOWN:
+			phba->link_flag |= LS_MDS_LINK_DOWN;
+			break;
+		case LPFC_FC_LA_TYPE_MDS_LOOPBACK:
+			phba->link_flag |= LS_MDS_LOOPBACK;
+			break;
+		default:
+			break;
+		}
+
 		/* Parse and translate status field */
 		mb = &pmb->u.mb;
 		mb->mbxStatus = lpfc_sli4_parse_latt_fault(phba,
@@ -5823,6 +5837,9 @@ lpfc_sli4_driver_resource_setup(struct lpfc_hba *phba)
 		spin_lock_init(&phba->sli4_hba.abts_nvme_buf_list_lock);
 		INIT_LIST_HEAD(&phba->sli4_hba.lpfc_abts_nvme_buf_list);
 		INIT_LIST_HEAD(&phba->sli4_hba.lpfc_abts_nvmet_ctx_list);
+		INIT_LIST_HEAD(&phba->sli4_hba.lpfc_nvmet_ctx_list);
+		INIT_LIST_HEAD(&phba->sli4_hba.lpfc_nvmet_io_wait_list);
+
 		/* Fast-path XRI aborted CQ Event work queue list */
 		INIT_LIST_HEAD(&phba->sli4_hba.sp_nvme_xri_aborted_work_queue);
 	}
@@ -5830,6 +5847,7 @@ lpfc_sli4_driver_resource_setup(struct lpfc_hba *phba)
 	/* This abort list used by worker thread */
 	spin_lock_init(&phba->sli4_hba.sgl_list_lock);
 	spin_lock_init(&phba->sli4_hba.nvmet_io_lock);
+	spin_lock_init(&phba->sli4_hba.nvmet_io_wait_lock);
 
 	/*
 	 * Initialize driver internal slow-path work queues
@@ -5944,16 +5962,21 @@ lpfc_sli4_driver_resource_setup(struct lpfc_hba *phba)
 		for (i = 0; i < lpfc_enable_nvmet_cnt; i++) {
 			if (wwn == lpfc_enable_nvmet[i]) {
 #if (IS_ENABLED(CONFIG_NVME_TARGET_FC))
+				if (lpfc_nvmet_mem_alloc(phba))
+					break;
+
+				phba->nvmet_support = 1; /* a match */
+
 				lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
 						"6017 NVME Target %016llx\n",
 						wwn);
-				phba->nvmet_support = 1; /* a match */
 #else
 				lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
 						"6021 Can't enable NVME Target."
 						" NVME_TARGET_FC infrastructure"
 						" is not in kernel\n");
 #endif
+				break;
 			}
 		}
 	}
@@ -6262,7 +6285,7 @@ lpfc_unset_driver_resource_phase2(struct lpfc_hba *phba)
  *
  * This routine is invoked to free the driver's IOCB list and memory.
  **/
-static void
+void
 lpfc_free_iocb_list(struct lpfc_hba *phba)
 {
 	struct lpfc_iocbq *iocbq_entry = NULL, *iocbq_next = NULL;
@@ -6290,7 +6313,7 @@ lpfc_free_iocb_list(struct lpfc_hba *phba)
  *	0 - successful
  *	other values - error
  **/
-static int
+int
 lpfc_init_iocb_list(struct lpfc_hba *phba, int iocb_count)
 {
 	struct lpfc_iocbq *iocbq_entry = NULL;
@@ -6518,7 +6541,6 @@ lpfc_sli4_create_rpi_hdr(struct lpfc_hba *phba)
 	uint16_t rpi_limit, curr_rpi_range;
 	struct lpfc_dmabuf *dmabuf;
 	struct lpfc_rpi_hdr *rpi_hdr;
-	uint32_t rpi_count;
 
 	/*
 	 * If the SLI4 port supports extents, posting the rpi header isn't
@@ -6531,8 +6553,7 @@ lpfc_sli4_create_rpi_hdr(struct lpfc_hba *phba)
 		return NULL;
 
 	/* The limit on the logical index is just the max_rpi count. */
-	rpi_limit = phba->sli4_hba.max_cfg_param.rpi_base +
-	phba->sli4_hba.max_cfg_param.max_rpi - 1;
+	rpi_limit = phba->sli4_hba.max_cfg_param.max_rpi;
 
 	spin_lock_irq(&phba->hbalock);
 	/*
@@ -6543,18 +6564,10 @@ lpfc_sli4_create_rpi_hdr(struct lpfc_hba *phba)
 	curr_rpi_range = phba->sli4_hba.next_rpi;
 	spin_unlock_irq(&phba->hbalock);
 
-	/*
-	 * The port has a limited number of rpis. The increment here
-	 * is LPFC_RPI_HDR_COUNT - 1 to account for the starting value
-	 * and to allow the full max_rpi range per port.
-	 */
-	if ((curr_rpi_range + (LPFC_RPI_HDR_COUNT - 1)) > rpi_limit)
-		rpi_count = rpi_limit - curr_rpi_range;
-	else
-		rpi_count = LPFC_RPI_HDR_COUNT;
-
-	if (!rpi_count)
+	/* Reached full RPI range */
+	if (curr_rpi_range == rpi_limit)
 		return NULL;
+
 	/*
 	 * First allocate the protocol header region for the port.  The
 	 * port expects a 4KB DMA-mapped memory region that is 4K aligned.
@@ -6588,13 +6601,9 @@ lpfc_sli4_create_rpi_hdr(struct lpfc_hba *phba)
 
 	/* The rpi_hdr stores the logical index only. */
 	rpi_hdr->start_rpi = curr_rpi_range;
+	rpi_hdr->next_rpi = phba->sli4_hba.next_rpi + LPFC_RPI_HDR_COUNT;
 	list_add_tail(&rpi_hdr->list, &phba->sli4_hba.lpfc_rpi_hdr_list);
 
-	/*
-	 * The next_rpi stores the next logical module-64 rpi value used
-	 * to post physical rpis in subsequent rpi postings.
-	 */
-	phba->sli4_hba.next_rpi += rpi_count;
 	spin_unlock_irq(&phba->hbalock);
 	return rpi_hdr;
 
@@ -8165,7 +8174,7 @@ lpfc_sli4_queue_create(struct lpfc_hba *phba)
 			/* Create NVMET Receive Queue for header */
 			qdesc = lpfc_sli4_queue_alloc(phba,
 						      phba->sli4_hba.rq_esize,
-						      phba->sli4_hba.rq_ecount);
+						      LPFC_NVMET_RQE_DEF_COUNT);
 			if (!qdesc) {
 				lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
 						"3146 Failed allocate "
@@ -8187,7 +8196,7 @@ lpfc_sli4_queue_create(struct lpfc_hba *phba)
 			/* Create NVMET Receive Queue for data */
 			qdesc = lpfc_sli4_queue_alloc(phba,
 						      phba->sli4_hba.rq_esize,
-						      phba->sli4_hba.rq_ecount);
+						      LPFC_NVMET_RQE_DEF_COUNT);
 			if (!qdesc) {
 				lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
 						"3156 Failed allocate "
@@ -8319,46 +8328,6 @@ lpfc_sli4_queue_destroy(struct lpfc_hba *phba)
 }
 
 int
-lpfc_post_rq_buffer(struct lpfc_hba *phba, struct lpfc_queue *hrq,
-		    struct lpfc_queue *drq, int count)
-{
-	int rc, i;
-	struct lpfc_rqe hrqe;
-	struct lpfc_rqe drqe;
-	struct lpfc_rqb *rqbp;
-	struct rqb_dmabuf *rqb_buffer;
-	LIST_HEAD(rqb_buf_list);
-
-	rqbp = hrq->rqbp;
-	for (i = 0; i < count; i++) {
-		rqb_buffer = (rqbp->rqb_alloc_buffer)(phba);
-		if (!rqb_buffer)
-			break;
-		rqb_buffer->hrq = hrq;
-		rqb_buffer->drq = drq;
-		list_add_tail(&rqb_buffer->hbuf.list, &rqb_buf_list);
-	}
-	while (!list_empty(&rqb_buf_list)) {
-		list_remove_head(&rqb_buf_list, rqb_buffer, struct rqb_dmabuf,
-				 hbuf.list);
-
-		hrqe.address_lo = putPaddrLow(rqb_buffer->hbuf.phys);
-		hrqe.address_hi = putPaddrHigh(rqb_buffer->hbuf.phys);
-		drqe.address_lo = putPaddrLow(rqb_buffer->dbuf.phys);
-		drqe.address_hi = putPaddrHigh(rqb_buffer->dbuf.phys);
-		rc = lpfc_sli4_rq_put(hrq, drq, &hrqe, &drqe);
-		if (rc < 0) {
-			(rqbp->rqb_free_buffer)(phba, rqb_buffer);
-		} else {
-			list_add_tail(&rqb_buffer->hbuf.list,
-				      &rqbp->rqb_buffer_list);
-			rqbp->buffer_count++;
-		}
-	}
-	return 1;
-}
-
-int
 lpfc_free_rq_buffer(struct lpfc_hba *phba, struct lpfc_queue *rq)
 {
 	struct lpfc_rqb *rqbp;
@@ -8777,9 +8746,6 @@ lpfc_sli4_queue_setup(struct lpfc_hba *phba)
 		goto out_destroy;
 	}
 
-	lpfc_rq_adjust_repost(phba, phba->sli4_hba.hdr_rq, LPFC_ELS_HBQ);
-	lpfc_rq_adjust_repost(phba, phba->sli4_hba.dat_rq, LPFC_ELS_HBQ);
-
 	rc = lpfc_rq_create(phba, phba->sli4_hba.hdr_rq, phba->sli4_hba.dat_rq,
 			    phba->sli4_hba.els_cq, LPFC_USOL);
 	if (rc) {
@@ -8847,7 +8813,7 @@ lpfc_sli4_queue_unset(struct lpfc_hba *phba)
 		lpfc_wq_destroy(phba, phba->sli4_hba.nvmels_wq);
 
 	/* Unset ELS work queue */
-	if (phba->sli4_hba.els_cq)
+	if (phba->sli4_hba.els_wq)
 		lpfc_wq_destroy(phba, phba->sli4_hba.els_wq);
 
 	/* Unset unsolicited receive queue */
@@ -11103,7 +11069,7 @@ lpfc_pci_probe_one_s4(struct pci_dev *pdev, const struct pci_device_id *pid)
 	struct lpfc_hba   *phba;
 	struct lpfc_vport *vport = NULL;
 	struct Scsi_Host  *shost = NULL;
-	int error, cnt;
+	int error;
 	uint32_t cfg_mode, intr_mode;
 
 	/* Allocate memory for HBA structure */
@@ -11137,22 +11103,6 @@ lpfc_pci_probe_one_s4(struct pci_dev *pdev, const struct pci_device_id *pid)
 		goto out_unset_pci_mem_s4;
 	}
 
-	cnt = phba->cfg_iocb_cnt * 1024;
-	if (phba->nvmet_support)
-		cnt += phba->cfg_nvmet_mrq_post * phba->cfg_nvmet_mrq;
-
-	/* Initialize and populate the iocb list per host */
-	lpfc_printf_log(phba, KERN_INFO, LOG_INIT,
-			"2821 initialize iocb list %d total %d\n",
-			phba->cfg_iocb_cnt, cnt);
-	error = lpfc_init_iocb_list(phba, cnt);
-
-	if (error) {
-		lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
-				"1413 Failed to initialize iocb list.\n");
-		goto out_unset_driver_resource_s4;
-	}
-
 	INIT_LIST_HEAD(&phba->active_rrq_list);
 	INIT_LIST_HEAD(&phba->fcf.fcf_pri_list);
 
@@ -11161,7 +11111,7 @@ lpfc_pci_probe_one_s4(struct pci_dev *pdev, const struct pci_device_id *pid)
 	if (error) {
 		lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
 				"1414 Failed to set up driver resource.\n");
-		goto out_free_iocb_list;
+		goto out_unset_driver_resource_s4;
 	}
 
 	/* Get the default values for Model Name and Description */
@@ -11261,8 +11211,6 @@ out_destroy_shost:
 	lpfc_destroy_shost(phba);
 out_unset_driver_resource:
 	lpfc_unset_driver_resource_phase2(phba);
-out_free_iocb_list:
-	lpfc_free_iocb_list(phba);
 out_unset_driver_resource_s4:
 	lpfc_sli4_driver_resource_unset(phba);
 out_unset_pci_mem_s4:
diff --git a/drivers/scsi/lpfc/lpfc_mem.c b/drivers/scsi/lpfc/lpfc_mem.c
index 5986c7957199..fcc05a1517c2 100644
--- a/drivers/scsi/lpfc/lpfc_mem.c
+++ b/drivers/scsi/lpfc/lpfc_mem.c
@@ -214,6 +214,21 @@ fail_free_drb_pool:
 	return -ENOMEM;
 }
 
+int
+lpfc_nvmet_mem_alloc(struct lpfc_hba *phba)
+{
+	phba->lpfc_nvmet_drb_pool =
+		pci_pool_create("lpfc_nvmet_drb_pool",
+				phba->pcidev, LPFC_NVMET_DATA_BUF_SIZE,
+				SGL_ALIGN_SZ, 0);
+	if (!phba->lpfc_nvmet_drb_pool) {
+		lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
+				"6024 Can't enable NVME Target - no memory\n");
+		return -ENOMEM;
+	}
+	return 0;
+}
+
 /**
  * lpfc_mem_free - Frees memory allocated by lpfc_mem_alloc
  * @phba: HBA to free memory for
@@ -232,6 +247,9 @@ lpfc_mem_free(struct lpfc_hba *phba)
 
 	/* Free HBQ pools */
 	lpfc_sli_hbqbuf_free_all(phba);
+	if (phba->lpfc_nvmet_drb_pool)
+		pci_pool_destroy(phba->lpfc_nvmet_drb_pool);
+	phba->lpfc_nvmet_drb_pool = NULL;
 	if (phba->lpfc_drb_pool)
 		pci_pool_destroy(phba->lpfc_drb_pool);
 	phba->lpfc_drb_pool = NULL;
@@ -611,8 +629,6 @@ struct rqb_dmabuf *
 lpfc_sli4_nvmet_alloc(struct lpfc_hba *phba)
 {
 	struct rqb_dmabuf *dma_buf;
-	struct lpfc_iocbq *nvmewqe;
-	union lpfc_wqe128 *wqe;
 
 	dma_buf = kzalloc(sizeof(struct rqb_dmabuf), GFP_KERNEL);
 	if (!dma_buf)
@@ -624,69 +640,15 @@ lpfc_sli4_nvmet_alloc(struct lpfc_hba *phba)
 		kfree(dma_buf);
 		return NULL;
 	}
-	dma_buf->dbuf.virt = pci_pool_alloc(phba->lpfc_drb_pool, GFP_KERNEL,
-					    &dma_buf->dbuf.phys);
+	dma_buf->dbuf.virt = pci_pool_alloc(phba->lpfc_nvmet_drb_pool,
+					    GFP_KERNEL, &dma_buf->dbuf.phys);
 	if (!dma_buf->dbuf.virt) {
 		pci_pool_free(phba->lpfc_hrb_pool, dma_buf->hbuf.virt,
 			      dma_buf->hbuf.phys);
 		kfree(dma_buf);
 		return NULL;
 	}
-	dma_buf->total_size = LPFC_DATA_BUF_SIZE;
-
-	dma_buf->context = kzalloc(sizeof(struct lpfc_nvmet_rcv_ctx),
-				   GFP_KERNEL);
-	if (!dma_buf->context) {
-		pci_pool_free(phba->lpfc_drb_pool, dma_buf->dbuf.virt,
-			      dma_buf->dbuf.phys);
-		pci_pool_free(phba->lpfc_hrb_pool, dma_buf->hbuf.virt,
-			      dma_buf->hbuf.phys);
-		kfree(dma_buf);
-		return NULL;
-	}
-
-	dma_buf->iocbq = lpfc_sli_get_iocbq(phba);
-	if (!dma_buf->iocbq) {
-		kfree(dma_buf->context);
-		pci_pool_free(phba->lpfc_drb_pool, dma_buf->dbuf.virt,
-			      dma_buf->dbuf.phys);
-		pci_pool_free(phba->lpfc_hrb_pool, dma_buf->hbuf.virt,
-			      dma_buf->hbuf.phys);
-		kfree(dma_buf);
-		lpfc_printf_log(phba, KERN_ERR, LOG_NVME,
-				"2621 Ran out of nvmet iocb/WQEs\n");
-		return NULL;
-	}
-	dma_buf->iocbq->iocb_flag = LPFC_IO_NVMET;
-	nvmewqe = dma_buf->iocbq;
-	wqe = (union lpfc_wqe128 *)&nvmewqe->wqe;
-	/* Initialize WQE */
-	memset(wqe, 0, sizeof(union lpfc_wqe));
-	/* Word 7 */
-	bf_set(wqe_ct, &wqe->generic.wqe_com, SLI4_CT_RPI);
-	bf_set(wqe_class, &wqe->generic.wqe_com, CLASS3);
-	bf_set(wqe_pu, &wqe->generic.wqe_com, 1);
-	/* Word 10 */
-	bf_set(wqe_nvme, &wqe->fcp_tsend.wqe_com, 1);
-	bf_set(wqe_ebde_cnt, &wqe->generic.wqe_com, 0);
-	bf_set(wqe_qosd, &wqe->generic.wqe_com, 0);
-
-	dma_buf->iocbq->context1 = NULL;
-	spin_lock(&phba->sli4_hba.sgl_list_lock);
-	dma_buf->sglq = __lpfc_sli_get_nvmet_sglq(phba, dma_buf->iocbq);
-	spin_unlock(&phba->sli4_hba.sgl_list_lock);
-	if (!dma_buf->sglq) {
-		lpfc_sli_release_iocbq(phba, dma_buf->iocbq);
-		kfree(dma_buf->context);
-		pci_pool_free(phba->lpfc_drb_pool, dma_buf->dbuf.virt,
-			      dma_buf->dbuf.phys);
-		pci_pool_free(phba->lpfc_hrb_pool, dma_buf->hbuf.virt,
-			      dma_buf->hbuf.phys);
-		kfree(dma_buf);
-		lpfc_printf_log(phba, KERN_ERR, LOG_NVME,
-				"6132 Ran out of nvmet XRIs\n");
-		return NULL;
-	}
+	dma_buf->total_size = LPFC_NVMET_DATA_BUF_SIZE;
 	return dma_buf;
 }
 
@@ -705,20 +667,9 @@ lpfc_sli4_nvmet_alloc(struct lpfc_hba *phba)
 void
 lpfc_sli4_nvmet_free(struct lpfc_hba *phba, struct rqb_dmabuf *dmab)
 {
-	unsigned long flags;
-
-	__lpfc_clear_active_sglq(phba, dmab->sglq->sli4_lxritag);
-	dmab->sglq->state = SGL_FREED;
-	dmab->sglq->ndlp = NULL;
-
-	spin_lock_irqsave(&phba->sli4_hba.sgl_list_lock, flags);
-	list_add_tail(&dmab->sglq->list, &phba->sli4_hba.lpfc_nvmet_sgl_list);
-	spin_unlock_irqrestore(&phba->sli4_hba.sgl_list_lock, flags);
-
-	lpfc_sli_release_iocbq(phba, dmab->iocbq);
-	kfree(dmab->context);
 	pci_pool_free(phba->lpfc_hrb_pool, dmab->hbuf.virt, dmab->hbuf.phys);
-	pci_pool_free(phba->lpfc_drb_pool, dmab->dbuf.virt, dmab->dbuf.phys);
+	pci_pool_free(phba->lpfc_nvmet_drb_pool,
+		      dmab->dbuf.virt, dmab->dbuf.phys);
 	kfree(dmab);
 }
 
@@ -803,6 +754,11 @@ lpfc_rq_buf_free(struct lpfc_hba *phba, struct lpfc_dmabuf *mp)
 	rc = lpfc_sli4_rq_put(rqb_entry->hrq, rqb_entry->drq, &hrqe, &drqe);
 	if (rc < 0) {
 		(rqbp->rqb_free_buffer)(phba, rqb_entry);
+		lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
+				"6409 Cannot post to RQ %d: %x %x\n",
+				rqb_entry->hrq->queue_id,
+				rqb_entry->hrq->host_index,
+				rqb_entry->hrq->hba_index);
 	} else {
 		list_add_tail(&rqb_entry->hbuf.list, &rqbp->rqb_buffer_list);
 		rqbp->buffer_count++;
diff --git a/drivers/scsi/lpfc/lpfc_nportdisc.c b/drivers/scsi/lpfc/lpfc_nportdisc.c
index 8777c2d5f50d..bff3de053df4 100644
--- a/drivers/scsi/lpfc/lpfc_nportdisc.c
+++ b/drivers/scsi/lpfc/lpfc_nportdisc.c
@@ -1944,7 +1944,13 @@ lpfc_cmpl_prli_prli_issue(struct lpfc_vport *vport, struct lpfc_nodelist *ndlp,
 
 		/* Target driver cannot solicit NVME FB. */
 		if (bf_get_be32(prli_tgt, nvpr)) {
+			/* Complete the nvme target roles.  The transport
+			 * needs to know if the rport is capable of
+			 * discovery in addition to its role.
+			 */
 			ndlp->nlp_type |= NLP_NVME_TARGET;
+			if (bf_get_be32(prli_disc, nvpr))
+				ndlp->nlp_type |= NLP_NVME_DISCOVERY;
 			if ((bf_get_be32(prli_fba, nvpr) == 1) &&
 			    (bf_get_be32(prli_fb_sz, nvpr) > 0) &&
 			    (phba->cfg_nvme_enable_fb) &&
diff --git a/drivers/scsi/lpfc/lpfc_nvmet.c b/drivers/scsi/lpfc/lpfc_nvmet.c
index 94434e621c33..074a6b5e7763 100644
--- a/drivers/scsi/lpfc/lpfc_nvmet.c
+++ b/drivers/scsi/lpfc/lpfc_nvmet.c
@@ -142,7 +142,7 @@ out:
 }
 
 /**
- * lpfc_nvmet_rq_post - Repost a NVMET RQ DMA buffer and clean up context
+ * lpfc_nvmet_ctxbuf_post - Repost a NVMET RQ DMA buffer and clean up context
  * @phba: HBA buffer is associated with
  * @ctxp: context to clean up
  * @mp: Buffer to free
@@ -155,24 +155,113 @@ out:
  * Returns: None
  **/
 void
-lpfc_nvmet_rq_post(struct lpfc_hba *phba, struct lpfc_nvmet_rcv_ctx *ctxp,
-		   struct lpfc_dmabuf *mp)
+lpfc_nvmet_ctxbuf_post(struct lpfc_hba *phba, struct lpfc_nvmet_ctxbuf *ctx_buf)
 {
-	if (ctxp) {
-		if (ctxp->flag)
-			lpfc_printf_log(phba, KERN_INFO, LOG_NVME_ABTS,
-				"6314 rq_post ctx xri x%x flag x%x\n",
-				ctxp->oxid, ctxp->flag);
-
-		if (ctxp->txrdy) {
-			pci_pool_free(phba->txrdy_payload_pool, ctxp->txrdy,
-				      ctxp->txrdy_phys);
-			ctxp->txrdy = NULL;
-			ctxp->txrdy_phys = 0;
+#if (IS_ENABLED(CONFIG_NVME_TARGET_FC))
+	struct lpfc_nvmet_rcv_ctx *ctxp = ctx_buf->context;
+	struct lpfc_nvmet_tgtport *tgtp;
+	struct fc_frame_header *fc_hdr;
+	struct rqb_dmabuf *nvmebuf;
+	struct lpfc_dmabuf *hbufp;
+	uint32_t *payload;
+	uint32_t size, oxid, sid, rc;
+	unsigned long iflag;
+
+	if (ctxp->txrdy) {
+		pci_pool_free(phba->txrdy_payload_pool, ctxp->txrdy,
+			      ctxp->txrdy_phys);
+		ctxp->txrdy = NULL;
+		ctxp->txrdy_phys = 0;
+	}
+	ctxp->state = LPFC_NVMET_STE_FREE;
+
+	spin_lock_irqsave(&phba->sli4_hba.nvmet_io_wait_lock, iflag);
+	if (phba->sli4_hba.nvmet_io_wait_cnt) {
+		hbufp = &nvmebuf->hbuf;
+		list_remove_head(&phba->sli4_hba.lpfc_nvmet_io_wait_list,
+				 nvmebuf, struct rqb_dmabuf,
+				 hbuf.list);
+		phba->sli4_hba.nvmet_io_wait_cnt--;
+		spin_unlock_irqrestore(&phba->sli4_hba.nvmet_io_wait_lock,
+				       iflag);
+
+		fc_hdr = (struct fc_frame_header *)(nvmebuf->hbuf.virt);
+		oxid = be16_to_cpu(fc_hdr->fh_ox_id);
+		tgtp = (struct lpfc_nvmet_tgtport *)phba->targetport->private;
+		payload = (uint32_t *)(nvmebuf->dbuf.virt);
+		size = nvmebuf->bytes_recv;
+		sid = sli4_sid_from_fc_hdr(fc_hdr);
+
+		ctxp = (struct lpfc_nvmet_rcv_ctx *)ctx_buf->context;
+		memset(ctxp, 0, sizeof(ctxp->ctx));
+		ctxp->wqeq = NULL;
+		ctxp->txrdy = NULL;
+		ctxp->offset = 0;
+		ctxp->phba = phba;
+		ctxp->size = size;
+		ctxp->oxid = oxid;
+		ctxp->sid = sid;
+		ctxp->state = LPFC_NVMET_STE_RCV;
+		ctxp->entry_cnt = 1;
+		ctxp->flag = 0;
+		ctxp->ctxbuf = ctx_buf;
+		spin_lock_init(&ctxp->ctxlock);
+
+#ifdef CONFIG_SCSI_LPFC_DEBUG_FS
+		if (phba->ktime_on) {
+			ctxp->ts_cmd_nvme = ktime_get_ns();
+			ctxp->ts_isr_cmd = ctxp->ts_cmd_nvme;
+			ctxp->ts_nvme_data = 0;
+			ctxp->ts_data_wqput = 0;
+			ctxp->ts_isr_data = 0;
+			ctxp->ts_data_nvme = 0;
+			ctxp->ts_nvme_status = 0;
+			ctxp->ts_status_wqput = 0;
+			ctxp->ts_isr_status = 0;
+			ctxp->ts_status_nvme = 0;
 		}
-		ctxp->state = LPFC_NVMET_STE_FREE;
+#endif
+		atomic_inc(&tgtp->rcv_fcp_cmd_in);
+		/*
+		 * The calling sequence should be:
+		 * nvmet_fc_rcv_fcp_req->lpfc_nvmet_xmt_fcp_op/cmp- req->done
+		 * lpfc_nvmet_xmt_fcp_op_cmp should free the allocated ctxp.
+		 * When we return from nvmet_fc_rcv_fcp_req, all relevant info
+		 * the NVME command / FC header is stored.
+		 * A buffer has already been reposted for this IO, so just free
+		 * the nvmebuf.
+		 */
+		rc = nvmet_fc_rcv_fcp_req(phba->targetport, &ctxp->ctx.fcp_req,
+					  payload, size);
+
+		/* Process FCP command */
+		if (rc == 0) {
+			atomic_inc(&tgtp->rcv_fcp_cmd_out);
+			nvmebuf->hrq->rqbp->rqb_free_buffer(phba, nvmebuf);
+			return;
+		}
+
+		atomic_inc(&tgtp->rcv_fcp_cmd_drop);
+		lpfc_printf_log(phba, KERN_ERR, LOG_NVME_IOERR,
+				"2582 FCP Drop IO x%x: err x%x: x%x x%x x%x\n",
+				ctxp->oxid, rc,
+				atomic_read(&tgtp->rcv_fcp_cmd_in),
+				atomic_read(&tgtp->rcv_fcp_cmd_out),
+				atomic_read(&tgtp->xmt_fcp_release));
+
+		lpfc_nvmet_defer_release(phba, ctxp);
+		lpfc_nvmet_unsol_fcp_issue_abort(phba, ctxp, sid, oxid);
+		nvmebuf->hrq->rqbp->rqb_free_buffer(phba, nvmebuf);
+		return;
 	}
-	lpfc_rq_buf_free(phba, mp);
+	spin_unlock_irqrestore(&phba->sli4_hba.nvmet_io_wait_lock, iflag);
+
+	spin_lock_irqsave(&phba->sli4_hba.nvmet_io_lock, iflag);
+	list_add_tail(&ctx_buf->list,
+		      &phba->sli4_hba.lpfc_nvmet_ctx_list);
+	phba->sli4_hba.nvmet_ctx_cnt++;
+	spin_unlock_irqrestore(&phba->sli4_hba.nvmet_io_lock, iflag);
+#endif
 }
 
 #ifdef CONFIG_SCSI_LPFC_DEBUG_FS
@@ -502,6 +591,7 @@ lpfc_nvmet_xmt_ls_rsp(struct nvmet_fc_target_port *tgtport,
 				"6150 LS Drop IO x%x: Prep\n",
 				ctxp->oxid);
 		lpfc_in_buf_free(phba, &nvmebuf->dbuf);
+		atomic_inc(&nvmep->xmt_ls_abort);
 		lpfc_nvmet_unsol_ls_issue_abort(phba, ctxp,
 						ctxp->sid, ctxp->oxid);
 		return -ENOMEM;
@@ -545,6 +635,7 @@ lpfc_nvmet_xmt_ls_rsp(struct nvmet_fc_target_port *tgtport,
 	lpfc_nlp_put(nvmewqeq->context1);
 
 	lpfc_in_buf_free(phba, &nvmebuf->dbuf);
+	atomic_inc(&nvmep->xmt_ls_abort);
 	lpfc_nvmet_unsol_ls_issue_abort(phba, ctxp, ctxp->sid, ctxp->oxid);
 	return -ENXIO;
 }
@@ -612,9 +703,9 @@ lpfc_nvmet_xmt_fcp_op(struct nvmet_fc_target_port *tgtport,
 	lpfc_nvmeio_data(phba, "NVMET FCP CMND: xri x%x op x%x len x%x\n",
 			 ctxp->oxid, rsp->op, rsp->rsplen);
 
+	ctxp->flag |= LPFC_NVMET_IO_INP;
 	rc = lpfc_sli4_issue_wqe(phba, LPFC_FCP_RING, nvmewqeq);
 	if (rc == WQE_SUCCESS) {
-		ctxp->flag |= LPFC_NVMET_IO_INP;
 #ifdef CONFIG_SCSI_LPFC_DEBUG_FS
 		if (!phba->ktime_on)
 			return 0;
@@ -692,6 +783,7 @@ static void
 lpfc_nvmet_xmt_fcp_release(struct nvmet_fc_target_port *tgtport,
 			   struct nvmefc_tgt_fcp_req *rsp)
 {
+	struct lpfc_nvmet_tgtport *lpfc_nvmep = tgtport->private;
 	struct lpfc_nvmet_rcv_ctx *ctxp =
 		container_of(rsp, struct lpfc_nvmet_rcv_ctx, ctx.fcp_req);
 	struct lpfc_hba *phba = ctxp->phba;
@@ -710,10 +802,12 @@ lpfc_nvmet_xmt_fcp_release(struct nvmet_fc_target_port *tgtport,
 	lpfc_nvmeio_data(phba, "NVMET FCP FREE: xri x%x ste %d\n", ctxp->oxid,
 			 ctxp->state, 0);
 
+	atomic_inc(&lpfc_nvmep->xmt_fcp_release);
+
 	if (aborting)
 		return;
 
-	lpfc_nvmet_rq_post(phba, ctxp, &ctxp->rqb_buffer->hbuf);
+	lpfc_nvmet_ctxbuf_post(phba, ctxp->ctxbuf);
 }
 
 static struct nvmet_fc_target_template lpfc_tgttemplate = {
@@ -734,17 +828,128 @@ static struct nvmet_fc_target_template lpfc_tgttemplate = {
 	.target_priv_sz = sizeof(struct lpfc_nvmet_tgtport),
 };
 
+void
+lpfc_nvmet_cleanup_io_context(struct lpfc_hba *phba)
+{
+	struct lpfc_nvmet_ctxbuf *ctx_buf, *next_ctx_buf;
+	unsigned long flags;
+
+	list_for_each_entry_safe(
+		ctx_buf, next_ctx_buf,
+		&phba->sli4_hba.lpfc_nvmet_ctx_list, list) {
+		spin_lock_irqsave(
+			&phba->sli4_hba.abts_nvme_buf_list_lock, flags);
+		list_del_init(&ctx_buf->list);
+		spin_unlock_irqrestore(
+			&phba->sli4_hba.abts_nvme_buf_list_lock, flags);
+		__lpfc_clear_active_sglq(phba,
+					 ctx_buf->sglq->sli4_lxritag);
+		ctx_buf->sglq->state = SGL_FREED;
+		ctx_buf->sglq->ndlp = NULL;
+
+		spin_lock_irqsave(&phba->sli4_hba.sgl_list_lock, flags);
+		list_add_tail(&ctx_buf->sglq->list,
+			      &phba->sli4_hba.lpfc_nvmet_sgl_list);
+		spin_unlock_irqrestore(&phba->sli4_hba.sgl_list_lock,
+				       flags);
+
+		lpfc_sli_release_iocbq(phba, ctx_buf->iocbq);
+		kfree(ctx_buf->context);
+	}
+}
+
+int
+lpfc_nvmet_setup_io_context(struct lpfc_hba *phba)
+{
+	struct lpfc_nvmet_ctxbuf *ctx_buf;
+	struct lpfc_iocbq *nvmewqe;
+	union lpfc_wqe128 *wqe;
+	int i;
+
+	lpfc_printf_log(phba, KERN_INFO, LOG_NVME,
+			"6403 Allocate NVMET resources for %d XRIs\n",
+			phba->sli4_hba.nvmet_xri_cnt);
+
+	/* For all nvmet xris, allocate resources needed to process a
+	 * received command on a per xri basis.
+	 */
+	for (i = 0; i < phba->sli4_hba.nvmet_xri_cnt; i++) {
+		ctx_buf = kzalloc(sizeof(*ctx_buf), GFP_KERNEL);
+		if (!ctx_buf) {
+			lpfc_printf_log(phba, KERN_ERR, LOG_NVME,
+					"6404 Ran out of memory for NVMET\n");
+			return -ENOMEM;
+		}
+
+		ctx_buf->context = kzalloc(sizeof(*ctx_buf->context),
+					   GFP_KERNEL);
+		if (!ctx_buf->context) {
+			kfree(ctx_buf);
+			lpfc_printf_log(phba, KERN_ERR, LOG_NVME,
+					"6405 Ran out of NVMET "
+					"context memory\n");
+			return -ENOMEM;
+		}
+		ctx_buf->context->ctxbuf = ctx_buf;
+
+		ctx_buf->iocbq = lpfc_sli_get_iocbq(phba);
+		if (!ctx_buf->iocbq) {
+			kfree(ctx_buf->context);
+			kfree(ctx_buf);
+			lpfc_printf_log(phba, KERN_ERR, LOG_NVME,
+					"6406 Ran out of NVMET iocb/WQEs\n");
+			return -ENOMEM;
+		}
+		ctx_buf->iocbq->iocb_flag = LPFC_IO_NVMET;
+		nvmewqe = ctx_buf->iocbq;
+		wqe = (union lpfc_wqe128 *)&nvmewqe->wqe;
+		/* Initialize WQE */
+		memset(wqe, 0, sizeof(union lpfc_wqe));
+		/* Word 7 */
+		bf_set(wqe_ct, &wqe->generic.wqe_com, SLI4_CT_RPI);
+		bf_set(wqe_class, &wqe->generic.wqe_com, CLASS3);
+		bf_set(wqe_pu, &wqe->generic.wqe_com, 1);
+		/* Word 10 */
+		bf_set(wqe_nvme, &wqe->fcp_tsend.wqe_com, 1);
+		bf_set(wqe_ebde_cnt, &wqe->generic.wqe_com, 0);
+		bf_set(wqe_qosd, &wqe->generic.wqe_com, 0);
+
+		ctx_buf->iocbq->context1 = NULL;
+		spin_lock(&phba->sli4_hba.sgl_list_lock);
+		ctx_buf->sglq = __lpfc_sli_get_nvmet_sglq(phba, ctx_buf->iocbq);
+		spin_unlock(&phba->sli4_hba.sgl_list_lock);
+		if (!ctx_buf->sglq) {
+			lpfc_sli_release_iocbq(phba, ctx_buf->iocbq);
+			kfree(ctx_buf->context);
+			kfree(ctx_buf);
+			lpfc_printf_log(phba, KERN_ERR, LOG_NVME,
+					"6407 Ran out of NVMET XRIs\n");
+			return -ENOMEM;
+		}
+		spin_lock(&phba->sli4_hba.nvmet_io_lock);
+		list_add_tail(&ctx_buf->list,
+			      &phba->sli4_hba.lpfc_nvmet_ctx_list);
+		spin_unlock(&phba->sli4_hba.nvmet_io_lock);
+	}
+	phba->sli4_hba.nvmet_ctx_cnt = phba->sli4_hba.nvmet_xri_cnt;
+	return 0;
+}
+
 int
 lpfc_nvmet_create_targetport(struct lpfc_hba *phba)
 {
 	struct lpfc_vport  *vport = phba->pport;
 	struct lpfc_nvmet_tgtport *tgtp;
 	struct nvmet_fc_port_info pinfo;
-	int error = 0;
+	int error;
 
 	if (phba->targetport)
 		return 0;
 
+	error = lpfc_nvmet_setup_io_context(phba);
+	if (error)
+		return error;
+
 	memset(&pinfo, 0, sizeof(struct nvmet_fc_port_info));
 	pinfo.node_name = wwn_to_u64(vport->fc_nodename.u.wwn);
 	pinfo.port_name = wwn_to_u64(vport->fc_portname.u.wwn);
@@ -764,7 +969,6 @@ lpfc_nvmet_create_targetport(struct lpfc_hba *phba)
 	lpfc_tgttemplate.max_sgl_segments = phba->cfg_nvme_seg_cnt + 1;
 	lpfc_tgttemplate.max_hw_queues = phba->cfg_nvme_io_channel;
 	lpfc_tgttemplate.target_features = NVMET_FCTGTFEAT_READDATA_RSP |
-					   NVMET_FCTGTFEAT_NEEDS_CMD_CPUSCHED |
 					   NVMET_FCTGTFEAT_CMD_IN_ISR |
 					   NVMET_FCTGTFEAT_OPDONE_IN_ISR;
 
@@ -773,13 +977,16 @@ lpfc_nvmet_create_targetport(struct lpfc_hba *phba)
 					     &phba->pcidev->dev,
 					     &phba->targetport);
 #else
-	error = -ENOMEM;
+	error = -ENOENT;
 #endif
 	if (error) {
 		lpfc_printf_log(phba, KERN_ERR, LOG_NVME_DISC,
 				"6025 Cannot register NVME targetport "
 				"x%x\n", error);
 		phba->targetport = NULL;
+
+		lpfc_nvmet_cleanup_io_context(phba);
+
 	} else {
 		tgtp = (struct lpfc_nvmet_tgtport *)
 			phba->targetport->private;
@@ -796,6 +1003,7 @@ lpfc_nvmet_create_targetport(struct lpfc_hba *phba)
 		atomic_set(&tgtp->rcv_ls_req_out, 0);
 		atomic_set(&tgtp->rcv_ls_req_drop, 0);
 		atomic_set(&tgtp->xmt_ls_abort, 0);
+		atomic_set(&tgtp->xmt_ls_abort_cmpl, 0);
 		atomic_set(&tgtp->xmt_ls_rsp, 0);
 		atomic_set(&tgtp->xmt_ls_drop, 0);
 		atomic_set(&tgtp->xmt_ls_rsp_error, 0);
@@ -803,18 +1011,21 @@ lpfc_nvmet_create_targetport(struct lpfc_hba *phba)
 		atomic_set(&tgtp->rcv_fcp_cmd_in, 0);
 		atomic_set(&tgtp->rcv_fcp_cmd_out, 0);
 		atomic_set(&tgtp->rcv_fcp_cmd_drop, 0);
-		atomic_set(&tgtp->xmt_fcp_abort, 0);
 		atomic_set(&tgtp->xmt_fcp_drop, 0);
 		atomic_set(&tgtp->xmt_fcp_read_rsp, 0);
 		atomic_set(&tgtp->xmt_fcp_read, 0);
 		atomic_set(&tgtp->xmt_fcp_write, 0);
 		atomic_set(&tgtp->xmt_fcp_rsp, 0);
+		atomic_set(&tgtp->xmt_fcp_release, 0);
 		atomic_set(&tgtp->xmt_fcp_rsp_cmpl, 0);
 		atomic_set(&tgtp->xmt_fcp_rsp_error, 0);
 		atomic_set(&tgtp->xmt_fcp_rsp_drop, 0);
+		atomic_set(&tgtp->xmt_fcp_abort, 0);
+		atomic_set(&tgtp->xmt_fcp_abort_cmpl, 0);
+		atomic_set(&tgtp->xmt_abort_unsol, 0);
+		atomic_set(&tgtp->xmt_abort_sol, 0);
 		atomic_set(&tgtp->xmt_abort_rsp, 0);
 		atomic_set(&tgtp->xmt_abort_rsp_error, 0);
-		atomic_set(&tgtp->xmt_abort_cmpl, 0);
 	}
 	return error;
 }
@@ -865,7 +1076,7 @@ lpfc_sli4_nvmet_xri_aborted(struct lpfc_hba *phba,
 	list_for_each_entry_safe(ctxp, next_ctxp,
 				 &phba->sli4_hba.lpfc_abts_nvmet_ctx_list,
 				 list) {
-		if (ctxp->rqb_buffer->sglq->sli4_xritag != xri)
+		if (ctxp->ctxbuf->sglq->sli4_xritag != xri)
 			continue;
 
 		/* Check if we already received a free context call
@@ -886,7 +1097,7 @@ lpfc_sli4_nvmet_xri_aborted(struct lpfc_hba *phba,
 		    (ndlp->nlp_state == NLP_STE_UNMAPPED_NODE ||
 		     ndlp->nlp_state == NLP_STE_MAPPED_NODE)) {
 			lpfc_set_rrq_active(phba, ndlp,
-				ctxp->rqb_buffer->sglq->sli4_lxritag,
+				ctxp->ctxbuf->sglq->sli4_lxritag,
 				rxid, 1);
 			lpfc_sli4_abts_err_handler(phba, ndlp, axri);
 		}
@@ -895,8 +1106,8 @@ lpfc_sli4_nvmet_xri_aborted(struct lpfc_hba *phba,
 				"6318 XB aborted %x flg x%x (%x)\n",
 				ctxp->oxid, ctxp->flag, released);
 		if (released)
-			lpfc_nvmet_rq_post(phba, ctxp,
-					   &ctxp->rqb_buffer->hbuf);
+			lpfc_nvmet_ctxbuf_post(phba, ctxp->ctxbuf);
+
 		if (rrq_empty)
 			lpfc_worker_wake_up(phba);
 		return;
@@ -924,7 +1135,7 @@ lpfc_nvmet_rcv_unsol_abort(struct lpfc_vport *vport,
 	list_for_each_entry_safe(ctxp, next_ctxp,
 				 &phba->sli4_hba.lpfc_abts_nvmet_ctx_list,
 				 list) {
-		if (ctxp->rqb_buffer->sglq->sli4_xritag != xri)
+		if (ctxp->ctxbuf->sglq->sli4_xritag != xri)
 			continue;
 
 		spin_unlock(&phba->sli4_hba.abts_nvme_buf_list_lock);
@@ -976,6 +1187,7 @@ lpfc_nvmet_destroy_targetport(struct lpfc_hba *phba)
 		init_completion(&tgtp->tport_unreg_done);
 		nvmet_fc_unregister_targetport(phba->targetport);
 		wait_for_completion_timeout(&tgtp->tport_unreg_done, 5);
+		lpfc_nvmet_cleanup_io_context(phba);
 	}
 	phba->targetport = NULL;
 #endif
@@ -1011,6 +1223,7 @@ lpfc_nvmet_unsol_ls_buffer(struct lpfc_hba *phba, struct lpfc_sli_ring *pring,
 		oxid = 0;
 		size = 0;
 		sid = 0;
+		ctxp = NULL;
 		goto dropit;
 	}
 
@@ -1105,39 +1318,71 @@ lpfc_nvmet_unsol_fcp_buffer(struct lpfc_hba *phba,
 	struct lpfc_nvmet_rcv_ctx *ctxp;
 	struct lpfc_nvmet_tgtport *tgtp;
 	struct fc_frame_header *fc_hdr;
+	struct lpfc_nvmet_ctxbuf *ctx_buf;
 	uint32_t *payload;
-	uint32_t size, oxid, sid, rc;
+	uint32_t size, oxid, sid, rc, qno;
+	unsigned long iflag;
 #ifdef CONFIG_SCSI_LPFC_DEBUG_FS
 	uint32_t id;
 #endif
 
+	ctx_buf = NULL;
 	if (!nvmebuf || !phba->targetport) {
 		lpfc_printf_log(phba, KERN_ERR, LOG_NVME_IOERR,
-				"6157 FCP Drop IO\n");
+				"6157 NVMET FCP Drop IO\n");
 		oxid = 0;
 		size = 0;
 		sid = 0;
+		ctxp = NULL;
 		goto dropit;
 	}
 
+	spin_lock_irqsave(&phba->sli4_hba.nvmet_io_lock, iflag);
+	if (phba->sli4_hba.nvmet_ctx_cnt) {
+		list_remove_head(&phba->sli4_hba.lpfc_nvmet_ctx_list,
+				 ctx_buf, struct lpfc_nvmet_ctxbuf, list);
+		phba->sli4_hba.nvmet_ctx_cnt--;
+	}
+	spin_unlock_irqrestore(&phba->sli4_hba.nvmet_io_lock, iflag);
 
-	tgtp = (struct lpfc_nvmet_tgtport *)phba->targetport->private;
-	payload = (uint32_t *)(nvmebuf->dbuf.virt);
 	fc_hdr = (struct fc_frame_header *)(nvmebuf->hbuf.virt);
-	size = nvmebuf->bytes_recv;
 	oxid = be16_to_cpu(fc_hdr->fh_ox_id);
-	sid = sli4_sid_from_fc_hdr(fc_hdr);
+	size = nvmebuf->bytes_recv;
 
-	ctxp = (struct lpfc_nvmet_rcv_ctx *)nvmebuf->context;
-	if (ctxp == NULL) {
-		atomic_inc(&tgtp->rcv_fcp_cmd_drop);
-		lpfc_printf_log(phba, KERN_ERR, LOG_NVME_IOERR,
-				"6158 FCP Drop IO x%x: Alloc\n",
-				oxid);
-		lpfc_nvmet_rq_post(phba, NULL, &nvmebuf->hbuf);
-		/* Cannot send ABTS without context */
+#ifdef CONFIG_SCSI_LPFC_DEBUG_FS
+	if (phba->cpucheck_on & LPFC_CHECK_NVMET_RCV) {
+		id = smp_processor_id();
+		if (id < LPFC_CHECK_CPU_CNT)
+			phba->cpucheck_rcv_io[id]++;
+	}
+#endif
+
+	lpfc_nvmeio_data(phba, "NVMET FCP  RCV: xri x%x sz %d CPU %02x\n",
+			 oxid, size, smp_processor_id());
+
+	if (!ctx_buf) {
+		/* Queue this NVME IO to process later */
+		spin_lock_irqsave(&phba->sli4_hba.nvmet_io_wait_lock, iflag);
+		list_add_tail(&nvmebuf->hbuf.list,
+			      &phba->sli4_hba.lpfc_nvmet_io_wait_list);
+		phba->sli4_hba.nvmet_io_wait_cnt++;
+		phba->sli4_hba.nvmet_io_wait_total++;
+		spin_unlock_irqrestore(&phba->sli4_hba.nvmet_io_wait_lock,
+				       iflag);
+
+		/* Post a brand new DMA buffer to RQ */
+		qno = nvmebuf->idx;
+		lpfc_post_rq_buffer(
+			phba, phba->sli4_hba.nvmet_mrq_hdr[qno],
+			phba->sli4_hba.nvmet_mrq_data[qno], 1, qno);
 		return;
 	}
+
+	tgtp = (struct lpfc_nvmet_tgtport *)phba->targetport->private;
+	payload = (uint32_t *)(nvmebuf->dbuf.virt);
+	sid = sli4_sid_from_fc_hdr(fc_hdr);
+
+	ctxp = (struct lpfc_nvmet_rcv_ctx *)ctx_buf->context;
 	memset(ctxp, 0, sizeof(ctxp->ctx));
 	ctxp->wqeq = NULL;
 	ctxp->txrdy = NULL;
@@ -1147,9 +1392,9 @@ lpfc_nvmet_unsol_fcp_buffer(struct lpfc_hba *phba,
 	ctxp->oxid = oxid;
 	ctxp->sid = sid;
 	ctxp->state = LPFC_NVMET_STE_RCV;
-	ctxp->rqb_buffer = nvmebuf;
 	ctxp->entry_cnt = 1;
 	ctxp->flag = 0;
+	ctxp->ctxbuf = ctx_buf;
 	spin_lock_init(&ctxp->ctxlock);
 
 #ifdef CONFIG_SCSI_LPFC_DEBUG_FS
@@ -1165,22 +1410,16 @@ lpfc_nvmet_unsol_fcp_buffer(struct lpfc_hba *phba,
 		ctxp->ts_isr_status = 0;
 		ctxp->ts_status_nvme = 0;
 	}
-
-	if (phba->cpucheck_on & LPFC_CHECK_NVMET_RCV) {
-		id = smp_processor_id();
-		if (id < LPFC_CHECK_CPU_CNT)
-			phba->cpucheck_rcv_io[id]++;
-	}
 #endif
 
-	lpfc_nvmeio_data(phba, "NVMET FCP  RCV: xri x%x sz %d CPU %02x\n",
-			 oxid, size, smp_processor_id());
-
 	atomic_inc(&tgtp->rcv_fcp_cmd_in);
 	/*
 	 * The calling sequence should be:
 	 * nvmet_fc_rcv_fcp_req -> lpfc_nvmet_xmt_fcp_op/cmp -> req->done
 	 * lpfc_nvmet_xmt_fcp_op_cmp should free the allocated ctxp.
+	 * When we return from nvmet_fc_rcv_fcp_req, all relevant info in
+	 * the NVME command / FC header is stored, so we are free to repost
+	 * the buffer.
 	 */
 	rc = nvmet_fc_rcv_fcp_req(phba->targetport, &ctxp->ctx.fcp_req,
 				  payload, size);
@@ -1188,26 +1427,32 @@ lpfc_nvmet_unsol_fcp_buffer(struct lpfc_hba *phba,
 	/* Process FCP command */
 	if (rc == 0) {
 		atomic_inc(&tgtp->rcv_fcp_cmd_out);
+		lpfc_rq_buf_free(phba, &nvmebuf->hbuf); /* repost */
 		return;
 	}
 
 	atomic_inc(&tgtp->rcv_fcp_cmd_drop);
 	lpfc_printf_log(phba, KERN_ERR, LOG_NVME_IOERR,
-			"6159 FCP Drop IO x%x: err x%x\n",
-			ctxp->oxid, rc);
+			"6159 FCP Drop IO x%x: err x%x: x%x x%x x%x\n",
+			ctxp->oxid, rc,
+			atomic_read(&tgtp->rcv_fcp_cmd_in),
+			atomic_read(&tgtp->rcv_fcp_cmd_out),
+			atomic_read(&tgtp->xmt_fcp_release));
 dropit:
 	lpfc_nvmeio_data(phba, "NVMET FCP DROP: xri x%x sz %d from %06x\n",
 			 oxid, size, sid);
 	if (oxid) {
+		lpfc_nvmet_defer_release(phba, ctxp);
 		lpfc_nvmet_unsol_fcp_issue_abort(phba, ctxp, sid, oxid);
+		lpfc_rq_buf_free(phba, &nvmebuf->hbuf); /* repost */
 		return;
 	}
 
-	if (nvmebuf) {
-		nvmebuf->iocbq->hba_wqidx = 0;
-		/* We assume a rcv'ed cmd ALWAYs fits into 1 buffer */
-		lpfc_nvmet_rq_post(phba, NULL, &nvmebuf->hbuf);
-	}
+	if (ctx_buf)
+		lpfc_nvmet_ctxbuf_post(phba, ctx_buf);
+
+	if (nvmebuf)
+		lpfc_rq_buf_free(phba, &nvmebuf->hbuf); /* repost */
 #endif
 }
 
@@ -1259,7 +1504,7 @@ lpfc_nvmet_unsol_fcp_event(struct lpfc_hba *phba,
 			   uint64_t isr_timestamp)
 {
 	if (phba->nvmet_support == 0) {
-		lpfc_nvmet_rq_post(phba, NULL, &nvmebuf->hbuf);
+		lpfc_rq_buf_free(phba, &nvmebuf->hbuf);
 		return;
 	}
 	lpfc_nvmet_unsol_fcp_buffer(phba, pring, nvmebuf,
@@ -1460,7 +1705,7 @@ lpfc_nvmet_prep_fcp_wqe(struct lpfc_hba *phba,
 	nvmewqe = ctxp->wqeq;
 	if (nvmewqe == NULL) {
 		/* Allocate buffer for  command wqe */
-		nvmewqe = ctxp->rqb_buffer->iocbq;
+		nvmewqe = ctxp->ctxbuf->iocbq;
 		if (nvmewqe == NULL) {
 			lpfc_printf_log(phba, KERN_ERR, LOG_NVME_IOERR,
 					"6110 lpfc_nvmet_prep_fcp_wqe: No "
@@ -1487,7 +1732,7 @@ lpfc_nvmet_prep_fcp_wqe(struct lpfc_hba *phba,
 		return NULL;
 	}
 
-	sgl  = (struct sli4_sge *)ctxp->rqb_buffer->sglq->sgl;
+	sgl  = (struct sli4_sge *)ctxp->ctxbuf->sglq->sgl;
 	switch (rsp->op) {
 	case NVMET_FCOP_READDATA:
 	case NVMET_FCOP_READDATA_RSP:
@@ -1812,7 +2057,8 @@ lpfc_nvmet_sol_fcp_abort_cmp(struct lpfc_hba *phba, struct lpfc_iocbq *cmdwqe,
 	result = wcqe->parameter;
 
 	tgtp = (struct lpfc_nvmet_tgtport *)phba->targetport->private;
-	atomic_inc(&tgtp->xmt_abort_cmpl);
+	if (ctxp->flag & LPFC_NVMET_ABORT_OP)
+		atomic_inc(&tgtp->xmt_fcp_abort_cmpl);
 
 	ctxp->state = LPFC_NVMET_STE_DONE;
 
@@ -1827,6 +2073,7 @@ lpfc_nvmet_sol_fcp_abort_cmp(struct lpfc_hba *phba, struct lpfc_iocbq *cmdwqe,
 	}
 	ctxp->flag &= ~LPFC_NVMET_ABORT_OP;
 	spin_unlock_irqrestore(&ctxp->ctxlock, flags);
+	atomic_inc(&tgtp->xmt_abort_rsp);
 
 	lpfc_printf_log(phba, KERN_ERR, LOG_NVME_ABTS,
 			"6165 ABORT cmpl: xri x%x flg x%x (%d) "
@@ -1835,15 +2082,16 @@ lpfc_nvmet_sol_fcp_abort_cmp(struct lpfc_hba *phba, struct lpfc_iocbq *cmdwqe,
 			wcqe->word0, wcqe->total_data_placed,
 			result, wcqe->word3);
 
+	cmdwqe->context2 = NULL;
+	cmdwqe->context3 = NULL;
 	/*
 	 * if transport has released ctx, then can reuse it. Otherwise,
 	 * will be recycled by transport release call.
 	 */
 	if (released)
-		lpfc_nvmet_rq_post(phba, ctxp, &ctxp->rqb_buffer->hbuf);
+		lpfc_nvmet_ctxbuf_post(phba, ctxp->ctxbuf);
 
-	cmdwqe->context2 = NULL;
-	cmdwqe->context3 = NULL;
+	/* This is the iocbq for the abort, not the command */
 	lpfc_sli_release_iocbq(phba, cmdwqe);
 
 	/* Since iaab/iaar are NOT set, there is no work left.
@@ -1877,7 +2125,8 @@ lpfc_nvmet_unsol_fcp_abort_cmp(struct lpfc_hba *phba, struct lpfc_iocbq *cmdwqe,
 	result = wcqe->parameter;
 
 	tgtp = (struct lpfc_nvmet_tgtport *)phba->targetport->private;
-	atomic_inc(&tgtp->xmt_abort_cmpl);
+	if (ctxp->flag & LPFC_NVMET_ABORT_OP)
+		atomic_inc(&tgtp->xmt_fcp_abort_cmpl);
 
 	if (!ctxp) {
 		/* if context is clear, related io alrady complete */
@@ -1907,6 +2156,7 @@ lpfc_nvmet_unsol_fcp_abort_cmp(struct lpfc_hba *phba, struct lpfc_iocbq *cmdwqe,
 	}
 	ctxp->flag &= ~LPFC_NVMET_ABORT_OP;
 	spin_unlock_irqrestore(&ctxp->ctxlock, flags);
+	atomic_inc(&tgtp->xmt_abort_rsp);
 
 	lpfc_printf_log(phba, KERN_INFO, LOG_NVME_ABTS,
 			"6316 ABTS cmpl xri x%x flg x%x (%x) "
@@ -1914,15 +2164,15 @@ lpfc_nvmet_unsol_fcp_abort_cmp(struct lpfc_hba *phba, struct lpfc_iocbq *cmdwqe,
 			ctxp->oxid, ctxp->flag, released,
 			wcqe->word0, wcqe->total_data_placed,
 			result, wcqe->word3);
+
+	cmdwqe->context2 = NULL;
+	cmdwqe->context3 = NULL;
 	/*
 	 * if transport has released ctx, then can reuse it. Otherwise,
 	 * will be recycled by transport release call.
 	 */
 	if (released)
-		lpfc_nvmet_rq_post(phba, ctxp, &ctxp->rqb_buffer->hbuf);
-
-	cmdwqe->context2 = NULL;
-	cmdwqe->context3 = NULL;
+		lpfc_nvmet_ctxbuf_post(phba, ctxp->ctxbuf);
 
 	/* Since iaab/iaar are NOT set, there is no work left.
 	 * For LPFC_NVMET_XBUSY, lpfc_sli4_nvmet_xri_aborted
@@ -1953,7 +2203,7 @@ lpfc_nvmet_xmt_ls_abort_cmp(struct lpfc_hba *phba, struct lpfc_iocbq *cmdwqe,
 	result = wcqe->parameter;
 
 	tgtp = (struct lpfc_nvmet_tgtport *)phba->targetport->private;
-	atomic_inc(&tgtp->xmt_abort_cmpl);
+	atomic_inc(&tgtp->xmt_ls_abort_cmpl);
 
 	lpfc_printf_log(phba, KERN_INFO, LOG_NVME_ABTS,
 			"6083 Abort cmpl: ctx %p WCQE: %08x %08x %08x %08x\n",
@@ -1984,10 +2234,6 @@ lpfc_nvmet_unsol_issue_abort(struct lpfc_hba *phba,
 			sid, xri, ctxp->wqeq->sli4_xritag);
 
 	tgtp = (struct lpfc_nvmet_tgtport *)phba->targetport->private;
-	if (!ctxp->wqeq) {
-		ctxp->wqeq = ctxp->rqb_buffer->iocbq;
-		ctxp->wqeq->hba_wqidx = 0;
-	}
 
 	ndlp = lpfc_findnode_did(phba->pport, sid);
 	if (!ndlp || !NLP_CHK_NODE_ACT(ndlp) ||
@@ -2083,7 +2329,7 @@ lpfc_nvmet_sol_fcp_issue_abort(struct lpfc_hba *phba,
 
 	tgtp = (struct lpfc_nvmet_tgtport *)phba->targetport->private;
 	if (!ctxp->wqeq) {
-		ctxp->wqeq = ctxp->rqb_buffer->iocbq;
+		ctxp->wqeq = ctxp->ctxbuf->iocbq;
 		ctxp->wqeq->hba_wqidx = 0;
 	}
 
@@ -2104,6 +2350,7 @@ lpfc_nvmet_sol_fcp_issue_abort(struct lpfc_hba *phba,
 	/* Issue ABTS for this WQE based on iotag */
 	ctxp->abort_wqeq = lpfc_sli_get_iocbq(phba);
 	if (!ctxp->abort_wqeq) {
+		atomic_inc(&tgtp->xmt_abort_rsp_error);
 		lpfc_printf_log(phba, KERN_WARNING, LOG_NVME_ABTS,
 				"6161 ABORT failed: No wqeqs: "
 				"xri: x%x\n", ctxp->oxid);
@@ -2128,6 +2375,7 @@ lpfc_nvmet_sol_fcp_issue_abort(struct lpfc_hba *phba,
 	/* driver queued commands are in process of being flushed */
 	if (phba->hba_flag & HBA_NVME_IOQ_FLUSH) {
 		spin_unlock_irqrestore(&phba->hbalock, flags);
+		atomic_inc(&tgtp->xmt_abort_rsp_error);
 		lpfc_printf_log(phba, KERN_ERR, LOG_NVME,
 				"6163 Driver in reset cleanup - flushing "
 				"NVME Req now. hba_flag x%x oxid x%x\n",
@@ -2140,6 +2388,7 @@ lpfc_nvmet_sol_fcp_issue_abort(struct lpfc_hba *phba,
 	/* Outstanding abort is in progress */
 	if (abts_wqeq->iocb_flag & LPFC_DRIVER_ABORTED) {
 		spin_unlock_irqrestore(&phba->hbalock, flags);
+		atomic_inc(&tgtp->xmt_abort_rsp_error);
 		lpfc_printf_log(phba, KERN_ERR, LOG_NVME,
 				"6164 Outstanding NVME I/O Abort Request "
 				"still pending on oxid x%x\n",
@@ -2190,9 +2439,12 @@ lpfc_nvmet_sol_fcp_issue_abort(struct lpfc_hba *phba,
 	abts_wqeq->context2 = ctxp;
 	rc = lpfc_sli4_issue_wqe(phba, LPFC_FCP_RING, abts_wqeq);
 	spin_unlock_irqrestore(&phba->hbalock, flags);
-	if (rc == WQE_SUCCESS)
+	if (rc == WQE_SUCCESS) {
+		atomic_inc(&tgtp->xmt_abort_sol);
 		return 0;
+	}
 
+	atomic_inc(&tgtp->xmt_abort_rsp_error);
 	ctxp->flag &= ~LPFC_NVMET_ABORT_OP;
 	lpfc_sli_release_iocbq(phba, abts_wqeq);
 	lpfc_printf_log(phba, KERN_ERR, LOG_NVME_ABTS,
@@ -2215,7 +2467,7 @@ lpfc_nvmet_unsol_fcp_issue_abort(struct lpfc_hba *phba,
 
 	tgtp = (struct lpfc_nvmet_tgtport *)phba->targetport->private;
 	if (!ctxp->wqeq) {
-		ctxp->wqeq = ctxp->rqb_buffer->iocbq;
+		ctxp->wqeq = ctxp->ctxbuf->iocbq;
 		ctxp->wqeq->hba_wqidx = 0;
 	}
 
@@ -2231,11 +2483,11 @@ lpfc_nvmet_unsol_fcp_issue_abort(struct lpfc_hba *phba,
 	rc = lpfc_sli4_issue_wqe(phba, LPFC_FCP_RING, abts_wqeq);
 	spin_unlock_irqrestore(&phba->hbalock, flags);
 	if (rc == WQE_SUCCESS) {
-		atomic_inc(&tgtp->xmt_abort_rsp);
 		return 0;
 	}
 
 aerr:
+	atomic_inc(&tgtp->xmt_abort_rsp_error);
 	ctxp->flag &= ~LPFC_NVMET_ABORT_OP;
 	atomic_inc(&tgtp->xmt_abort_rsp_error);
 	lpfc_printf_log(phba, KERN_WARNING, LOG_NVME_ABTS,
@@ -2270,6 +2522,7 @@ lpfc_nvmet_unsol_ls_issue_abort(struct lpfc_hba *phba,
 	}
 	abts_wqeq = ctxp->wqeq;
 	wqe_abts = &abts_wqeq->wqe;
+
 	lpfc_nvmet_unsol_issue_abort(phba, ctxp, sid, xri);
 
 	spin_lock_irqsave(&phba->hbalock, flags);
@@ -2279,7 +2532,7 @@ lpfc_nvmet_unsol_ls_issue_abort(struct lpfc_hba *phba,
 	rc = lpfc_sli4_issue_wqe(phba, LPFC_ELS_RING, abts_wqeq);
 	spin_unlock_irqrestore(&phba->hbalock, flags);
 	if (rc == WQE_SUCCESS) {
-		atomic_inc(&tgtp->xmt_abort_rsp);
+		atomic_inc(&tgtp->xmt_abort_unsol);
 		return 0;
 	}
 
diff --git a/drivers/scsi/lpfc/lpfc_nvmet.h b/drivers/scsi/lpfc/lpfc_nvmet.h
index 128759fe6650..6eb2f5d8d4ed 100644
--- a/drivers/scsi/lpfc/lpfc_nvmet.h
+++ b/drivers/scsi/lpfc/lpfc_nvmet.h
@@ -22,6 +22,7 @@
  ********************************************************************/
 
 #define LPFC_NVMET_DEFAULT_SEGS		(64 + 1)	/* 256K IOs */
+#define LPFC_NVMET_RQE_DEF_COUNT	512
 #define LPFC_NVMET_SUCCESS_LEN	12
 
 /* Used for NVME Target */
@@ -34,6 +35,7 @@ struct lpfc_nvmet_tgtport {
 	atomic_t rcv_ls_req_out;
 	atomic_t rcv_ls_req_drop;
 	atomic_t xmt_ls_abort;
+	atomic_t xmt_ls_abort_cmpl;
 
 	/* Stats counters - lpfc_nvmet_xmt_ls_rsp */
 	atomic_t xmt_ls_rsp;
@@ -47,9 +49,9 @@ struct lpfc_nvmet_tgtport {
 	atomic_t rcv_fcp_cmd_in;
 	atomic_t rcv_fcp_cmd_out;
 	atomic_t rcv_fcp_cmd_drop;
+	atomic_t xmt_fcp_release;
 
 	/* Stats counters - lpfc_nvmet_xmt_fcp_op */
-	atomic_t xmt_fcp_abort;
 	atomic_t xmt_fcp_drop;
 	atomic_t xmt_fcp_read_rsp;
 	atomic_t xmt_fcp_read;
@@ -62,12 +64,13 @@ struct lpfc_nvmet_tgtport {
 	atomic_t xmt_fcp_rsp_drop;
 
 
-	/* Stats counters - lpfc_nvmet_unsol_issue_abort */
+	/* Stats counters - lpfc_nvmet_xmt_fcp_abort */
+	atomic_t xmt_fcp_abort;
+	atomic_t xmt_fcp_abort_cmpl;
+	atomic_t xmt_abort_sol;
+	atomic_t xmt_abort_unsol;
 	atomic_t xmt_abort_rsp;
 	atomic_t xmt_abort_rsp_error;
-
-	/* Stats counters - lpfc_nvmet_xmt_abort_cmp */
-	atomic_t xmt_abort_cmpl;
 };
 
 struct lpfc_nvmet_rcv_ctx {
@@ -103,6 +106,7 @@ struct lpfc_nvmet_rcv_ctx {
 #define LPFC_NVMET_CTX_RLS		0x8  /* ctx free requested */
 #define LPFC_NVMET_ABTS_RCV		0x10  /* ABTS received on exchange */
 	struct rqb_dmabuf *rqb_buffer;
+	struct lpfc_nvmet_ctxbuf *ctxbuf;
 
 #ifdef CONFIG_SCSI_LPFC_DEBUG_FS
 	uint64_t ts_isr_cmd;
diff --git a/drivers/scsi/lpfc/lpfc_sli.c b/drivers/scsi/lpfc/lpfc_sli.c
index cf19f4976f5f..d6b184839bc2 100644
--- a/drivers/scsi/lpfc/lpfc_sli.c
+++ b/drivers/scsi/lpfc/lpfc_sli.c
@@ -74,6 +74,8 @@ static struct lpfc_iocbq *lpfc_sli4_els_wcqe_to_rspiocbq(struct lpfc_hba *,
 							 struct lpfc_iocbq *);
 static void lpfc_sli4_send_seq_to_ulp(struct lpfc_vport *,
 				      struct hbq_dmabuf *);
+static void lpfc_sli4_handle_mds_loopback(struct lpfc_vport *vport,
+					  struct hbq_dmabuf *dmabuf);
 static int lpfc_sli4_fp_handle_cqe(struct lpfc_hba *, struct lpfc_queue *,
 				    struct lpfc_cqe *);
 static int lpfc_sli4_post_sgl_list(struct lpfc_hba *, struct list_head *,
@@ -479,22 +481,23 @@ lpfc_sli4_rq_put(struct lpfc_queue *hq, struct lpfc_queue *dq,
 	if (unlikely(!hq) || unlikely(!dq))
 		return -ENOMEM;
 	put_index = hq->host_index;
-	temp_hrqe = hq->qe[hq->host_index].rqe;
+	temp_hrqe = hq->qe[put_index].rqe;
 	temp_drqe = dq->qe[dq->host_index].rqe;
 
 	if (hq->type != LPFC_HRQ || dq->type != LPFC_DRQ)
 		return -EINVAL;
-	if (hq->host_index != dq->host_index)
+	if (put_index != dq->host_index)
 		return -EINVAL;
 	/* If the host has not yet processed the next entry then we are done */
-	if (((hq->host_index + 1) % hq->entry_count) == hq->hba_index)
+	if (((put_index + 1) % hq->entry_count) == hq->hba_index)
 		return -EBUSY;
 	lpfc_sli_pcimem_bcopy(hrqe, temp_hrqe, hq->entry_size);
 	lpfc_sli_pcimem_bcopy(drqe, temp_drqe, dq->entry_size);
 
 	/* Update the host index to point to the next slot */
-	hq->host_index = ((hq->host_index + 1) % hq->entry_count);
+	hq->host_index = ((put_index + 1) % hq->entry_count);
 	dq->host_index = ((dq->host_index + 1) % dq->entry_count);
+	hq->RQ_buf_posted++;
 
 	/* Ring The Header Receive Queue Doorbell */
 	if (!(hq->host_index % hq->entry_repost)) {
@@ -4204,13 +4207,16 @@ lpfc_sli_brdreset(struct lpfc_hba *phba)
 	/* Reset HBA */
 	lpfc_printf_log(phba, KERN_INFO, LOG_SLI,
 			"0325 Reset HBA Data: x%x x%x\n",
-			phba->pport->port_state, psli->sli_flag);
+			(phba->pport) ? phba->pport->port_state : 0,
+			psli->sli_flag);
 
 	/* perform board reset */
 	phba->fc_eventTag = 0;
 	phba->link_events = 0;
-	phba->pport->fc_myDID = 0;
-	phba->pport->fc_prevDID = 0;
+	if (phba->pport) {
+		phba->pport->fc_myDID = 0;
+		phba->pport->fc_prevDID = 0;
+	}
 
 	/* Turn off parity checking and serr during the physical reset */
 	pci_read_config_word(phba->pcidev, PCI_COMMAND, &cfg_value);
@@ -4336,7 +4342,8 @@ lpfc_sli_brdrestart_s3(struct lpfc_hba *phba)
 	/* Restart HBA */
 	lpfc_printf_log(phba, KERN_INFO, LOG_SLI,
 			"0337 Restart HBA Data: x%x x%x\n",
-			phba->pport->port_state, psli->sli_flag);
+			(phba->pport) ? phba->pport->port_state : 0,
+			psli->sli_flag);
 
 	word0 = 0;
 	mb = (MAILBOX_t *) &word0;
@@ -4350,7 +4357,7 @@ lpfc_sli_brdrestart_s3(struct lpfc_hba *phba)
 	readl(to_slim); /* flush */
 
 	/* Only skip post after fc_ffinit is completed */
-	if (phba->pport->port_state)
+	if (phba->pport && phba->pport->port_state)
 		word0 = 1;	/* This is really setting up word1 */
 	else
 		word0 = 0;	/* This is really setting up word1 */
@@ -4359,7 +4366,8 @@ lpfc_sli_brdrestart_s3(struct lpfc_hba *phba)
 	readl(to_slim); /* flush */
 
 	lpfc_sli_brdreset(phba);
-	phba->pport->stopped = 0;
+	if (phba->pport)
+		phba->pport->stopped = 0;
 	phba->link_state = LPFC_INIT_START;
 	phba->hba_flag = 0;
 	spin_unlock_irq(&phba->hbalock);
@@ -4446,7 +4454,7 @@ lpfc_sli_brdrestart(struct lpfc_hba *phba)
  * iteration, the function will restart the HBA again. The function returns
  * zero if HBA successfully restarted else returns negative error code.
  **/
-static int
+int
 lpfc_sli_chipset_init(struct lpfc_hba *phba)
 {
 	uint32_t status, i = 0;
@@ -5901,7 +5909,7 @@ lpfc_set_features(struct lpfc_hba *phba, LPFC_MBOXQ_t *mbox,
 		bf_set(lpfc_mbx_set_feature_mds,
 		       &mbox->u.mqe.un.set_feature, 1);
 		bf_set(lpfc_mbx_set_feature_mds_deep_loopbk,
-		       &mbox->u.mqe.un.set_feature, 0);
+		       &mbox->u.mqe.un.set_feature, 1);
 		mbox->u.mqe.un.set_feature.feature = LPFC_SET_MDS_DIAGS;
 		mbox->u.mqe.un.set_feature.param_len = 8;
 		break;
@@ -6507,6 +6515,50 @@ lpfc_set_host_data(struct lpfc_hba *phba, LPFC_MBOXQ_t *mbox)
 		 (phba->hba_flag & HBA_FCOE_MODE) ? "FCoE" : "FC");
 }
 
+int
+lpfc_post_rq_buffer(struct lpfc_hba *phba, struct lpfc_queue *hrq,
+		    struct lpfc_queue *drq, int count, int idx)
+{
+	int rc, i;
+	struct lpfc_rqe hrqe;
+	struct lpfc_rqe drqe;
+	struct lpfc_rqb *rqbp;
+	struct rqb_dmabuf *rqb_buffer;
+	LIST_HEAD(rqb_buf_list);
+
+	rqbp = hrq->rqbp;
+	for (i = 0; i < count; i++) {
+		/* IF RQ is already full, don't bother */
+		if (rqbp->buffer_count + i >= rqbp->entry_count - 1)
+			break;
+		rqb_buffer = rqbp->rqb_alloc_buffer(phba);
+		if (!rqb_buffer)
+			break;
+		rqb_buffer->hrq = hrq;
+		rqb_buffer->drq = drq;
+		rqb_buffer->idx = idx;
+		list_add_tail(&rqb_buffer->hbuf.list, &rqb_buf_list);
+	}
+	while (!list_empty(&rqb_buf_list)) {
+		list_remove_head(&rqb_buf_list, rqb_buffer, struct rqb_dmabuf,
+				 hbuf.list);
+
+		hrqe.address_lo = putPaddrLow(rqb_buffer->hbuf.phys);
+		hrqe.address_hi = putPaddrHigh(rqb_buffer->hbuf.phys);
+		drqe.address_lo = putPaddrLow(rqb_buffer->dbuf.phys);
+		drqe.address_hi = putPaddrHigh(rqb_buffer->dbuf.phys);
+		rc = lpfc_sli4_rq_put(hrq, drq, &hrqe, &drqe);
+		if (rc < 0) {
+			rqbp->rqb_free_buffer(phba, rqb_buffer);
+		} else {
+			list_add_tail(&rqb_buffer->hbuf.list,
+				      &rqbp->rqb_buffer_list);
+			rqbp->buffer_count++;
+		}
+	}
+	return 1;
+}
+
 /**
  * lpfc_sli4_hba_setup - SLI4 device initialization PCI function
  * @phba: Pointer to HBA context object.
@@ -6519,7 +6571,7 @@ lpfc_set_host_data(struct lpfc_hba *phba, LPFC_MBOXQ_t *mbox)
 int
 lpfc_sli4_hba_setup(struct lpfc_hba *phba)
 {
-	int rc, i;
+	int rc, i, cnt;
 	LPFC_MBOXQ_t *mboxq;
 	struct lpfc_mqe *mqe;
 	uint8_t *vpd;
@@ -6870,6 +6922,21 @@ lpfc_sli4_hba_setup(struct lpfc_hba *phba)
 			goto out_destroy_queue;
 		}
 		phba->sli4_hba.nvmet_xri_cnt = rc;
+
+		cnt = phba->cfg_iocb_cnt * 1024;
+		/* We need 1 iocbq for every SGL, for IO processing */
+		cnt += phba->sli4_hba.nvmet_xri_cnt;
+		/* Initialize and populate the iocb list per host */
+		lpfc_printf_log(phba, KERN_INFO, LOG_INIT,
+				"2821 initialize iocb list %d total %d\n",
+				phba->cfg_iocb_cnt, cnt);
+		rc = lpfc_init_iocb_list(phba, cnt);
+		if (rc) {
+			lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
+					"1413 Failed to init iocb list.\n");
+			goto out_destroy_queue;
+		}
+
 		lpfc_nvmet_create_targetport(phba);
 	} else {
 		/* update host scsi xri-sgl sizes and mappings */
@@ -6889,28 +6956,34 @@ lpfc_sli4_hba_setup(struct lpfc_hba *phba)
 					"and mapping: %d\n", rc);
 			goto out_destroy_queue;
 		}
+
+		cnt = phba->cfg_iocb_cnt * 1024;
+		/* Initialize and populate the iocb list per host */
+		lpfc_printf_log(phba, KERN_INFO, LOG_INIT,
+				"2820 initialize iocb list %d total %d\n",
+				phba->cfg_iocb_cnt, cnt);
+		rc = lpfc_init_iocb_list(phba, cnt);
+		if (rc) {
+			lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
+					"6301 Failed to init iocb list.\n");
+			goto out_destroy_queue;
+		}
 	}
 
 	if (phba->nvmet_support && phba->cfg_nvmet_mrq) {
-
 		/* Post initial buffers to all RQs created */
 		for (i = 0; i < phba->cfg_nvmet_mrq; i++) {
 			rqbp = phba->sli4_hba.nvmet_mrq_hdr[i]->rqbp;
 			INIT_LIST_HEAD(&rqbp->rqb_buffer_list);
 			rqbp->rqb_alloc_buffer = lpfc_sli4_nvmet_alloc;
 			rqbp->rqb_free_buffer = lpfc_sli4_nvmet_free;
-			rqbp->entry_count = 256;
+			rqbp->entry_count = LPFC_NVMET_RQE_DEF_COUNT;
 			rqbp->buffer_count = 0;
 
-			/* Divide by 4 and round down to multiple of 16 */
-			rc = (phba->cfg_nvmet_mrq_post >> 2) & 0xfff8;
-			phba->sli4_hba.nvmet_mrq_hdr[i]->entry_repost = rc;
-			phba->sli4_hba.nvmet_mrq_data[i]->entry_repost = rc;
-
 			lpfc_post_rq_buffer(
 				phba, phba->sli4_hba.nvmet_mrq_hdr[i],
 				phba->sli4_hba.nvmet_mrq_data[i],
-				phba->cfg_nvmet_mrq_post);
+				LPFC_NVMET_RQE_DEF_COUNT, i);
 		}
 	}
 
@@ -7077,6 +7150,7 @@ out_unset_queue:
 	/* Unset all the queues set up in this routine when error out */
 	lpfc_sli4_queue_unset(phba);
 out_destroy_queue:
+	lpfc_free_iocb_list(phba);
 	lpfc_sli4_queue_destroy(phba);
 out_stop_timers:
 	lpfc_stop_hba_timers(phba);
@@ -8616,8 +8690,11 @@ lpfc_sli4_iocb2wqe(struct lpfc_hba *phba, struct lpfc_iocbq *iocbq,
 		memset(wqe, 0, sizeof(union lpfc_wqe128));
 	/* Some of the fields are in the right position already */
 	memcpy(wqe, &iocbq->iocb, sizeof(union lpfc_wqe));
-	wqe->generic.wqe_com.word7 = 0; /* The ct field has moved so reset */
-	wqe->generic.wqe_com.word10 = 0;
+	if (iocbq->iocb.ulpCommand != CMD_SEND_FRAME) {
+		/* The ct field has moved so reset */
+		wqe->generic.wqe_com.word7 = 0;
+		wqe->generic.wqe_com.word10 = 0;
+	}
 
 	abort_tag = (uint32_t) iocbq->iotag;
 	xritag = iocbq->sli4_xritag;
@@ -9111,6 +9188,10 @@ lpfc_sli4_iocb2wqe(struct lpfc_hba *phba, struct lpfc_iocbq *iocbq,
 		}
 
 		break;
+	case CMD_SEND_FRAME:
+		bf_set(wqe_xri_tag, &wqe->generic.wqe_com, xritag);
+		bf_set(wqe_reqtag, &wqe->generic.wqe_com, iocbq->iotag);
+		return 0;
 	case CMD_XRI_ABORTED_CX:
 	case CMD_CREATE_XRI_CR: /* Do we expect to use this? */
 	case CMD_IOCB_FCP_IBIDIR64_CR: /* bidirectional xfer */
@@ -12783,6 +12864,7 @@ lpfc_sli4_sp_handle_rcqe(struct lpfc_hba *phba, struct lpfc_rcqe *rcqe)
 	struct fc_frame_header *fc_hdr;
 	struct lpfc_queue *hrq = phba->sli4_hba.hdr_rq;
 	struct lpfc_queue *drq = phba->sli4_hba.dat_rq;
+	struct lpfc_nvmet_tgtport *tgtp;
 	struct hbq_dmabuf *dma_buf;
 	uint32_t status, rq_id;
 	unsigned long iflags;
@@ -12803,7 +12885,6 @@ lpfc_sli4_sp_handle_rcqe(struct lpfc_hba *phba, struct lpfc_rcqe *rcqe)
 	case FC_STATUS_RQ_BUF_LEN_EXCEEDED:
 		lpfc_printf_log(phba, KERN_ERR, LOG_SLI,
 				"2537 Receive Frame Truncated!!\n");
-		hrq->RQ_buf_trunc++;
 	case FC_STATUS_RQ_SUCCESS:
 		lpfc_sli4_rq_release(hrq, drq);
 		spin_lock_irqsave(&phba->hbalock, iflags);
@@ -12814,6 +12895,7 @@ lpfc_sli4_sp_handle_rcqe(struct lpfc_hba *phba, struct lpfc_rcqe *rcqe)
 			goto out;
 		}
 		hrq->RQ_rcv_buf++;
+		hrq->RQ_buf_posted--;
 		memcpy(&dma_buf->cq_event.cqe.rcqe_cmpl, rcqe, sizeof(*rcqe));
 
 		/* If a NVME LS event (type 0x28), treat it as Fast path */
@@ -12827,8 +12909,21 @@ lpfc_sli4_sp_handle_rcqe(struct lpfc_hba *phba, struct lpfc_rcqe *rcqe)
 		spin_unlock_irqrestore(&phba->hbalock, iflags);
 		workposted = true;
 		break;
-	case FC_STATUS_INSUFF_BUF_NEED_BUF:
 	case FC_STATUS_INSUFF_BUF_FRM_DISC:
+		if (phba->nvmet_support) {
+			tgtp = phba->targetport->private;
+			lpfc_printf_log(phba, KERN_ERR, LOG_SLI | LOG_NVME,
+					"6402 RQE Error x%x, posted %d err_cnt "
+					"%d: %x %x %x\n",
+					status, hrq->RQ_buf_posted,
+					hrq->RQ_no_posted_buf,
+					atomic_read(&tgtp->rcv_fcp_cmd_in),
+					atomic_read(&tgtp->rcv_fcp_cmd_out),
+					atomic_read(&tgtp->xmt_fcp_release));
+		}
+		/* fallthrough */
+
+	case FC_STATUS_INSUFF_BUF_NEED_BUF:
 		hrq->RQ_no_posted_buf++;
 		/* Post more buffers if possible */
 		spin_lock_irqsave(&phba->hbalock, iflags);
@@ -12946,7 +13041,7 @@ lpfc_sli4_sp_handle_eqe(struct lpfc_hba *phba, struct lpfc_eqe *eqe,
 		while ((cqe = lpfc_sli4_cq_get(cq))) {
 			workposted |= lpfc_sli4_sp_handle_mcqe(phba, cqe);
 			if (!(++ecount % cq->entry_repost))
-				lpfc_sli4_cq_release(cq, LPFC_QUEUE_NOARM);
+				break;
 			cq->CQ_mbox++;
 		}
 		break;
@@ -12960,7 +13055,7 @@ lpfc_sli4_sp_handle_eqe(struct lpfc_hba *phba, struct lpfc_eqe *eqe,
 				workposted |= lpfc_sli4_sp_handle_cqe(phba, cq,
 								      cqe);
 			if (!(++ecount % cq->entry_repost))
-				lpfc_sli4_cq_release(cq, LPFC_QUEUE_NOARM);
+				break;
 		}
 
 		/* Track the max number of CQEs processed in 1 EQ */
@@ -13130,6 +13225,7 @@ lpfc_sli4_nvmet_handle_rcqe(struct lpfc_hba *phba, struct lpfc_queue *cq,
 	struct lpfc_queue *drq;
 	struct rqb_dmabuf *dma_buf;
 	struct fc_frame_header *fc_hdr;
+	struct lpfc_nvmet_tgtport *tgtp;
 	uint32_t status, rq_id;
 	unsigned long iflags;
 	uint32_t fctl, idx;
@@ -13160,8 +13256,6 @@ lpfc_sli4_nvmet_handle_rcqe(struct lpfc_hba *phba, struct lpfc_queue *cq,
 	case FC_STATUS_RQ_BUF_LEN_EXCEEDED:
 		lpfc_printf_log(phba, KERN_ERR, LOG_SLI,
 				"6126 Receive Frame Truncated!!\n");
-		hrq->RQ_buf_trunc++;
-		break;
 	case FC_STATUS_RQ_SUCCESS:
 		lpfc_sli4_rq_release(hrq, drq);
 		spin_lock_irqsave(&phba->hbalock, iflags);
@@ -13173,6 +13267,7 @@ lpfc_sli4_nvmet_handle_rcqe(struct lpfc_hba *phba, struct lpfc_queue *cq,
 		}
 		spin_unlock_irqrestore(&phba->hbalock, iflags);
 		hrq->RQ_rcv_buf++;
+		hrq->RQ_buf_posted--;
 		fc_hdr = (struct fc_frame_header *)dma_buf->hbuf.virt;
 
 		/* Just some basic sanity checks on FCP Command frame */
@@ -13195,14 +13290,23 @@ lpfc_sli4_nvmet_handle_rcqe(struct lpfc_hba *phba, struct lpfc_queue *cq,
 drop:
 		lpfc_in_buf_free(phba, &dma_buf->dbuf);
 		break;
-	case FC_STATUS_INSUFF_BUF_NEED_BUF:
 	case FC_STATUS_INSUFF_BUF_FRM_DISC:
+		if (phba->nvmet_support) {
+			tgtp = phba->targetport->private;
+			lpfc_printf_log(phba, KERN_ERR, LOG_SLI | LOG_NVME,
+					"6401 RQE Error x%x, posted %d err_cnt "
+					"%d: %x %x %x\n",
+					status, hrq->RQ_buf_posted,
+					hrq->RQ_no_posted_buf,
+					atomic_read(&tgtp->rcv_fcp_cmd_in),
+					atomic_read(&tgtp->rcv_fcp_cmd_out),
+					atomic_read(&tgtp->xmt_fcp_release));
+		}
+		/* fallthrough */
+
+	case FC_STATUS_INSUFF_BUF_NEED_BUF:
 		hrq->RQ_no_posted_buf++;
 		/* Post more buffers if possible */
-		spin_lock_irqsave(&phba->hbalock, iflags);
-		phba->hba_flag |= HBA_POST_RECEIVE_BUFFER;
-		spin_unlock_irqrestore(&phba->hbalock, iflags);
-		workposted = true;
 		break;
 	}
 out:
@@ -13356,7 +13460,7 @@ process_cq:
 	while ((cqe = lpfc_sli4_cq_get(cq))) {
 		workposted |= lpfc_sli4_fp_handle_cqe(phba, cq, cqe);
 		if (!(++ecount % cq->entry_repost))
-			lpfc_sli4_cq_release(cq, LPFC_QUEUE_NOARM);
+			break;
 	}
 
 	/* Track the max number of CQEs processed in 1 EQ */
@@ -13447,7 +13551,7 @@ lpfc_sli4_fof_handle_eqe(struct lpfc_hba *phba, struct lpfc_eqe *eqe)
 	while ((cqe = lpfc_sli4_cq_get(cq))) {
 		workposted |= lpfc_sli4_fp_handle_cqe(phba, cq, cqe);
 		if (!(++ecount % cq->entry_repost))
-			lpfc_sli4_cq_release(cq, LPFC_QUEUE_NOARM);
+			break;
 	}
 
 	/* Track the max number of CQEs processed in 1 EQ */
@@ -13529,7 +13633,7 @@ lpfc_sli4_fof_intr_handler(int irq, void *dev_id)
 	while ((eqe = lpfc_sli4_eq_get(eq))) {
 		lpfc_sli4_fof_handle_eqe(phba, eqe);
 		if (!(++ecount % eq->entry_repost))
-			lpfc_sli4_eq_release(eq, LPFC_QUEUE_NOARM);
+			break;
 		eq->EQ_processed++;
 	}
 
@@ -13646,7 +13750,7 @@ lpfc_sli4_hba_intr_handler(int irq, void *dev_id)
 
 		lpfc_sli4_hba_handle_eqe(phba, eqe, hba_eqidx);
 		if (!(++ecount % fpeq->entry_repost))
-			lpfc_sli4_eq_release(fpeq, LPFC_QUEUE_NOARM);
+			break;
 		fpeq->EQ_processed++;
 	}
 
@@ -13827,17 +13931,10 @@ lpfc_sli4_queue_alloc(struct lpfc_hba *phba, uint32_t entry_size,
 	}
 	queue->entry_size = entry_size;
 	queue->entry_count = entry_count;
-
-	/*
-	 * entry_repost is calculated based on the number of entries in the
-	 * queue. This works out except for RQs. If buffers are NOT initially
-	 * posted for every RQE, entry_repost should be adjusted accordingly.
-	 */
-	queue->entry_repost = (entry_count >> 3);
-	if (queue->entry_repost < LPFC_QUEUE_MIN_REPOST)
-		queue->entry_repost = LPFC_QUEUE_MIN_REPOST;
 	queue->phba = phba;
 
+	/* entry_repost will be set during q creation */
+
 	return queue;
 out_fail:
 	lpfc_sli4_queue_free(queue);
@@ -14068,6 +14165,7 @@ lpfc_eq_create(struct lpfc_hba *phba, struct lpfc_queue *eq, uint32_t imax)
 		status = -ENXIO;
 	eq->host_index = 0;
 	eq->hba_index = 0;
+	eq->entry_repost = LPFC_EQ_REPOST;
 
 	mempool_free(mbox, phba->mbox_mem_pool);
 	return status;
@@ -14141,9 +14239,9 @@ lpfc_cq_create(struct lpfc_hba *phba, struct lpfc_queue *cq,
 	default:
 		lpfc_printf_log(phba, KERN_ERR, LOG_SLI,
 				"0361 Unsupported CQ count: "
-				"entry cnt %d sz %d pg cnt %d repost %d\n",
+				"entry cnt %d sz %d pg cnt %d\n",
 				cq->entry_count, cq->entry_size,
-				cq->page_count, cq->entry_repost);
+				cq->page_count);
 		if (cq->entry_count < 256) {
 			status = -EINVAL;
 			goto out;
@@ -14196,6 +14294,7 @@ lpfc_cq_create(struct lpfc_hba *phba, struct lpfc_queue *cq,
 	cq->assoc_qid = eq->queue_id;
 	cq->host_index = 0;
 	cq->hba_index = 0;
+	cq->entry_repost = LPFC_CQ_REPOST;
 
 out:
 	mempool_free(mbox, phba->mbox_mem_pool);
@@ -14387,6 +14486,7 @@ lpfc_cq_create_set(struct lpfc_hba *phba, struct lpfc_queue **cqp,
 		cq->assoc_qid = eq->queue_id;
 		cq->host_index = 0;
 		cq->hba_index = 0;
+		cq->entry_repost = LPFC_CQ_REPOST;
 
 		rc = 0;
 		list_for_each_entry(dmabuf, &cq->page_list, list) {
@@ -14635,6 +14735,7 @@ lpfc_mq_create(struct lpfc_hba *phba, struct lpfc_queue *mq,
 	mq->subtype = subtype;
 	mq->host_index = 0;
 	mq->hba_index = 0;
+	mq->entry_repost = LPFC_MQ_REPOST;
 
 	/* link the mq onto the parent cq child list */
 	list_add_tail(&mq->list, &cq->child_list);
@@ -14860,34 +14961,6 @@ out:
 }
 
 /**
- * lpfc_rq_adjust_repost - Adjust entry_repost for an RQ
- * @phba: HBA structure that indicates port to create a queue on.
- * @rq:   The queue structure to use for the receive queue.
- * @qno:  The associated HBQ number
- *
- *
- * For SLI4 we need to adjust the RQ repost value based on
- * the number of buffers that are initially posted to the RQ.
- */
-void
-lpfc_rq_adjust_repost(struct lpfc_hba *phba, struct lpfc_queue *rq, int qno)
-{
-	uint32_t cnt;
-
-	/* sanity check on queue memory */
-	if (!rq)
-		return;
-	cnt = lpfc_hbq_defs[qno]->entry_count;
-
-	/* Recalc repost for RQs based on buffers initially posted */
-	cnt = (cnt >> 3);
-	if (cnt < LPFC_QUEUE_MIN_REPOST)
-		cnt = LPFC_QUEUE_MIN_REPOST;
-
-	rq->entry_repost = cnt;
-}
-
-/**
  * lpfc_rq_create - Create a Receive Queue on the HBA
  * @phba: HBA structure that indicates port to create a queue on.
  * @hrq: The queue structure to use to create the header receive queue.
@@ -15072,6 +15145,7 @@ lpfc_rq_create(struct lpfc_hba *phba, struct lpfc_queue *hrq,
 	hrq->subtype = subtype;
 	hrq->host_index = 0;
 	hrq->hba_index = 0;
+	hrq->entry_repost = LPFC_RQ_REPOST;
 
 	/* now create the data queue */
 	lpfc_sli4_config(phba, mbox, LPFC_MBOX_SUBSYSTEM_FCOE,
@@ -15082,7 +15156,12 @@ lpfc_rq_create(struct lpfc_hba *phba, struct lpfc_queue *hrq,
 	if (phba->sli4_hba.pc_sli4_params.rqv == LPFC_Q_CREATE_VERSION_1) {
 		bf_set(lpfc_rq_context_rqe_count_1,
 		       &rq_create->u.request.context, hrq->entry_count);
-		rq_create->u.request.context.buffer_size = LPFC_DATA_BUF_SIZE;
+		if (subtype == LPFC_NVMET)
+			rq_create->u.request.context.buffer_size =
+				LPFC_NVMET_DATA_BUF_SIZE;
+		else
+			rq_create->u.request.context.buffer_size =
+				LPFC_DATA_BUF_SIZE;
 		bf_set(lpfc_rq_context_rqe_size, &rq_create->u.request.context,
 		       LPFC_RQE_SIZE_8);
 		bf_set(lpfc_rq_context_page_size, &rq_create->u.request.context,
@@ -15119,8 +15198,14 @@ lpfc_rq_create(struct lpfc_hba *phba, struct lpfc_queue *hrq,
 			       LPFC_RQ_RING_SIZE_4096);
 			break;
 		}
-		bf_set(lpfc_rq_context_buf_size, &rq_create->u.request.context,
-		       LPFC_DATA_BUF_SIZE);
+		if (subtype == LPFC_NVMET)
+			bf_set(lpfc_rq_context_buf_size,
+			       &rq_create->u.request.context,
+			       LPFC_NVMET_DATA_BUF_SIZE);
+		else
+			bf_set(lpfc_rq_context_buf_size,
+			       &rq_create->u.request.context,
+			       LPFC_DATA_BUF_SIZE);
 	}
 	bf_set(lpfc_rq_context_cq_id, &rq_create->u.request.context,
 	       cq->queue_id);
@@ -15153,6 +15238,7 @@ lpfc_rq_create(struct lpfc_hba *phba, struct lpfc_queue *hrq,
 	drq->subtype = subtype;
 	drq->host_index = 0;
 	drq->hba_index = 0;
+	drq->entry_repost = LPFC_RQ_REPOST;
 
 	/* link the header and data RQs onto the parent cq child list */
 	list_add_tail(&hrq->list, &cq->child_list);
@@ -15265,7 +15351,7 @@ lpfc_mrq_create(struct lpfc_hba *phba, struct lpfc_queue **hrqp,
 			       cq->queue_id);
 			bf_set(lpfc_rq_context_data_size,
 			       &rq_create->u.request.context,
-			       LPFC_DATA_BUF_SIZE);
+			       LPFC_NVMET_DATA_BUF_SIZE);
 			bf_set(lpfc_rq_context_hdr_size,
 			       &rq_create->u.request.context,
 			       LPFC_HDR_BUF_SIZE);
@@ -15310,6 +15396,7 @@ lpfc_mrq_create(struct lpfc_hba *phba, struct lpfc_queue **hrqp,
 		hrq->subtype = subtype;
 		hrq->host_index = 0;
 		hrq->hba_index = 0;
+		hrq->entry_repost = LPFC_RQ_REPOST;
 
 		drq->db_format = LPFC_DB_RING_FORMAT;
 		drq->db_regaddr = phba->sli4_hba.RQDBregaddr;
@@ -15318,6 +15405,7 @@ lpfc_mrq_create(struct lpfc_hba *phba, struct lpfc_queue **hrqp,
 		drq->subtype = subtype;
 		drq->host_index = 0;
 		drq->hba_index = 0;
+		drq->entry_repost = LPFC_RQ_REPOST;
 
 		list_add_tail(&hrq->list, &cq->child_list);
 		list_add_tail(&drq->list, &cq->child_list);
@@ -16058,6 +16146,8 @@ lpfc_fc_frame_check(struct lpfc_hba *phba, struct fc_frame_header *fc_hdr)
 	struct fc_vft_header *fc_vft_hdr;
 	uint32_t *header = (uint32_t *) fc_hdr;
 
+#define FC_RCTL_MDS_DIAGS	0xF4
+
 	switch (fc_hdr->fh_r_ctl) {
 	case FC_RCTL_DD_UNCAT:		/* uncategorized information */
 	case FC_RCTL_DD_SOL_DATA:	/* solicited data */
@@ -16085,6 +16175,7 @@ lpfc_fc_frame_check(struct lpfc_hba *phba, struct fc_frame_header *fc_hdr)
 	case FC_RCTL_F_BSY:	/* fabric busy to data frame */
 	case FC_RCTL_F_BSYL:	/* fabric busy to link control frame */
 	case FC_RCTL_LCR:	/* link credit reset */
+	case FC_RCTL_MDS_DIAGS: /* MDS Diagnostics */
 	case FC_RCTL_END:	/* end */
 		break;
 	case FC_RCTL_VFTH:	/* Virtual Fabric tagging Header */
@@ -16094,12 +16185,16 @@ lpfc_fc_frame_check(struct lpfc_hba *phba, struct fc_frame_header *fc_hdr)
 	default:
 		goto drop;
 	}
+
+#define FC_TYPE_VENDOR_UNIQUE	0xFF
+
 	switch (fc_hdr->fh_type) {
 	case FC_TYPE_BLS:
 	case FC_TYPE_ELS:
 	case FC_TYPE_FCP:
 	case FC_TYPE_CT:
 	case FC_TYPE_NVME:
+	case FC_TYPE_VENDOR_UNIQUE:
 		break;
 	case FC_TYPE_IP:
 	case FC_TYPE_ILS:
@@ -16110,12 +16205,14 @@ lpfc_fc_frame_check(struct lpfc_hba *phba, struct fc_frame_header *fc_hdr)
 	lpfc_printf_log(phba, KERN_INFO, LOG_ELS,
 			"2538 Received frame rctl:%s (x%x), type:%s (x%x), "
 			"frame Data:%08x %08x %08x %08x %08x %08x %08x\n",
+			(fc_hdr->fh_r_ctl == FC_RCTL_MDS_DIAGS) ? "MDS Diags" :
 			lpfc_rctl_names[fc_hdr->fh_r_ctl], fc_hdr->fh_r_ctl,
-			lpfc_type_names[fc_hdr->fh_type], fc_hdr->fh_type,
-			be32_to_cpu(header[0]), be32_to_cpu(header[1]),
-			be32_to_cpu(header[2]), be32_to_cpu(header[3]),
-			be32_to_cpu(header[4]), be32_to_cpu(header[5]),
-			be32_to_cpu(header[6]));
+			(fc_hdr->fh_type == FC_TYPE_VENDOR_UNIQUE) ?
+			"Vendor Unique" : lpfc_type_names[fc_hdr->fh_type],
+			fc_hdr->fh_type, be32_to_cpu(header[0]),
+			be32_to_cpu(header[1]), be32_to_cpu(header[2]),
+			be32_to_cpu(header[3]), be32_to_cpu(header[4]),
+			be32_to_cpu(header[5]), be32_to_cpu(header[6]));
 	return 0;
 drop:
 	lpfc_printf_log(phba, KERN_WARNING, LOG_ELS,
@@ -16921,6 +17018,96 @@ lpfc_sli4_send_seq_to_ulp(struct lpfc_vport *vport,
 	lpfc_sli_release_iocbq(phba, iocbq);
 }
 
+static void
+lpfc_sli4_mds_loopback_cmpl(struct lpfc_hba *phba, struct lpfc_iocbq *cmdiocb,
+			    struct lpfc_iocbq *rspiocb)
+{
+	struct lpfc_dmabuf *pcmd = cmdiocb->context2;
+
+	if (pcmd && pcmd->virt)
+		pci_pool_free(phba->lpfc_drb_pool, pcmd->virt, pcmd->phys);
+	kfree(pcmd);
+	lpfc_sli_release_iocbq(phba, cmdiocb);
+}
+
+static void
+lpfc_sli4_handle_mds_loopback(struct lpfc_vport *vport,
+			      struct hbq_dmabuf *dmabuf)
+{
+	struct fc_frame_header *fc_hdr;
+	struct lpfc_hba *phba = vport->phba;
+	struct lpfc_iocbq *iocbq = NULL;
+	union  lpfc_wqe *wqe;
+	struct lpfc_dmabuf *pcmd = NULL;
+	uint32_t frame_len;
+	int rc;
+
+	fc_hdr = (struct fc_frame_header *)dmabuf->hbuf.virt;
+	frame_len = bf_get(lpfc_rcqe_length, &dmabuf->cq_event.cqe.rcqe_cmpl);
+
+	/* Send the received frame back */
+	iocbq = lpfc_sli_get_iocbq(phba);
+	if (!iocbq)
+		goto exit;
+
+	/* Allocate buffer for command payload */
+	pcmd = kmalloc(sizeof(struct lpfc_dmabuf), GFP_KERNEL);
+	if (pcmd)
+		pcmd->virt = pci_pool_alloc(phba->lpfc_drb_pool, GFP_KERNEL,
+					    &pcmd->phys);
+	if (!pcmd || !pcmd->virt)
+		goto exit;
+
+	INIT_LIST_HEAD(&pcmd->list);
+
+	/* copyin the payload */
+	memcpy(pcmd->virt, dmabuf->dbuf.virt, frame_len);
+
+	/* fill in BDE's for command */
+	iocbq->iocb.un.xseq64.bdl.addrHigh = putPaddrHigh(pcmd->phys);
+	iocbq->iocb.un.xseq64.bdl.addrLow = putPaddrLow(pcmd->phys);
+	iocbq->iocb.un.xseq64.bdl.bdeFlags = BUFF_TYPE_BDE_64;
+	iocbq->iocb.un.xseq64.bdl.bdeSize = frame_len;
+
+	iocbq->context2 = pcmd;
+	iocbq->vport = vport;
+	iocbq->iocb_flag &= ~LPFC_FIP_ELS_ID_MASK;
+	iocbq->iocb_flag |= LPFC_USE_FCPWQIDX;
+
+	/*
+	 * Setup rest of the iocb as though it were a WQE
+	 * Build the SEND_FRAME WQE
+	 */
+	wqe = (union lpfc_wqe *)&iocbq->iocb;
+
+	wqe->send_frame.frame_len = frame_len;
+	wqe->send_frame.fc_hdr_wd0 = be32_to_cpu(*((uint32_t *)fc_hdr));
+	wqe->send_frame.fc_hdr_wd1 = be32_to_cpu(*((uint32_t *)fc_hdr + 1));
+	wqe->send_frame.fc_hdr_wd2 = be32_to_cpu(*((uint32_t *)fc_hdr + 2));
+	wqe->send_frame.fc_hdr_wd3 = be32_to_cpu(*((uint32_t *)fc_hdr + 3));
+	wqe->send_frame.fc_hdr_wd4 = be32_to_cpu(*((uint32_t *)fc_hdr + 4));
+	wqe->send_frame.fc_hdr_wd5 = be32_to_cpu(*((uint32_t *)fc_hdr + 5));
+
+	iocbq->iocb.ulpCommand = CMD_SEND_FRAME;
+	iocbq->iocb.ulpLe = 1;
+	iocbq->iocb_cmpl = lpfc_sli4_mds_loopback_cmpl;
+	rc = lpfc_sli_issue_iocb(phba, LPFC_ELS_RING, iocbq, 0);
+	if (rc == IOCB_ERROR)
+		goto exit;
+
+	lpfc_in_buf_free(phba, &dmabuf->dbuf);
+	return;
+
+exit:
+	lpfc_printf_log(phba, KERN_WARNING, LOG_SLI,
+			"2023 Unable to process MDS loopback frame\n");
+	if (pcmd && pcmd->virt)
+		pci_pool_free(phba->lpfc_drb_pool, pcmd->virt, pcmd->phys);
+	kfree(pcmd);
+	lpfc_sli_release_iocbq(phba, iocbq);
+	lpfc_in_buf_free(phba, &dmabuf->dbuf);
+}
+
 /**
  * lpfc_sli4_handle_received_buffer - Handle received buffers from firmware
  * @phba: Pointer to HBA context object.
@@ -16959,6 +17146,13 @@ lpfc_sli4_handle_received_buffer(struct lpfc_hba *phba,
 		fcfi = bf_get(lpfc_rcqe_fcf_id,
 			      &dmabuf->cq_event.cqe.rcqe_cmpl);
 
+	if (fc_hdr->fh_r_ctl == 0xF4 && fc_hdr->fh_type == 0xFF) {
+		vport = phba->pport;
+		/* Handle MDS Loopback frames */
+		lpfc_sli4_handle_mds_loopback(vport, dmabuf);
+		return;
+	}
+
 	/* d_id this frame is directed to */
 	did = sli4_did_from_fc_hdr(fc_hdr);
 
@@ -17132,6 +17326,14 @@ lpfc_sli4_post_rpi_hdr(struct lpfc_hba *phba, struct lpfc_rpi_hdr *rpi_page)
 				"status x%x add_status x%x, mbx status x%x\n",
 				shdr_status, shdr_add_status, rc);
 		rc = -ENXIO;
+	} else {
+		/*
+		 * The next_rpi stores the next logical module-64 rpi value used
+		 * to post physical rpis in subsequent rpi postings.
+		 */
+		spin_lock_irq(&phba->hbalock);
+		phba->sli4_hba.next_rpi = rpi_page->next_rpi;
+		spin_unlock_irq(&phba->hbalock);
 	}
 	return rc;
 }
@@ -18712,7 +18914,7 @@ lpfc_sli4_issue_wqe(struct lpfc_hba *phba, uint32_t ring_number,
 
 		spin_lock_irqsave(&pring->ring_lock, iflags);
 		ctxp = pwqe->context2;
-		sglq = ctxp->rqb_buffer->sglq;
+		sglq = ctxp->ctxbuf->sglq;
 		if (pwqe->sli4_xritag ==  NO_XRI) {
 			pwqe->sli4_lxritag = sglq->sli4_lxritag;
 			pwqe->sli4_xritag = sglq->sli4_xritag;
diff --git a/drivers/scsi/lpfc/lpfc_sli4.h b/drivers/scsi/lpfc/lpfc_sli4.h
index da46471337c8..cf863db27700 100644
--- a/drivers/scsi/lpfc/lpfc_sli4.h
+++ b/drivers/scsi/lpfc/lpfc_sli4.h
@@ -24,7 +24,6 @@
 #define LPFC_XRI_EXCH_BUSY_WAIT_TMO		10000
 #define LPFC_XRI_EXCH_BUSY_WAIT_T1   		10
 #define LPFC_XRI_EXCH_BUSY_WAIT_T2              30000
-#define LPFC_RELEASE_NOTIFICATION_INTERVAL	32
 #define LPFC_RPI_LOW_WATER_MARK			10
 
 #define LPFC_UNREG_FCF                          1
@@ -155,7 +154,11 @@ struct lpfc_queue {
 	uint32_t entry_count;	/* Number of entries to support on the queue */
 	uint32_t entry_size;	/* Size of each queue entry. */
 	uint32_t entry_repost;	/* Count of entries before doorbell is rung */
-#define LPFC_QUEUE_MIN_REPOST	8
+#define LPFC_EQ_REPOST		8
+#define LPFC_MQ_REPOST		8
+#define LPFC_CQ_REPOST		64
+#define LPFC_RQ_REPOST		64
+#define LPFC_RELEASE_NOTIFICATION_INTERVAL	32  /* For WQs */
 	uint32_t queue_id;	/* Queue ID assigned by the hardware */
 	uint32_t assoc_qid;     /* Queue ID associated with, for CQ/WQ/MQ */
 	uint32_t page_count;	/* Number of pages allocated for this queue */
@@ -195,7 +198,7 @@ struct lpfc_queue {
 /* defines for RQ stats */
 #define	RQ_no_posted_buf	q_cnt_1
 #define	RQ_no_buf_found		q_cnt_2
-#define	RQ_buf_trunc		q_cnt_3
+#define	RQ_buf_posted		q_cnt_3
 #define	RQ_rcv_buf		q_cnt_4
 
 	uint64_t isr_timestamp;
@@ -617,12 +620,17 @@ struct lpfc_sli4_hba {
 	uint16_t scsi_xri_start;
 	uint16_t els_xri_cnt;
 	uint16_t nvmet_xri_cnt;
+	uint16_t nvmet_ctx_cnt;
+	uint16_t nvmet_io_wait_cnt;
+	uint16_t nvmet_io_wait_total;
 	struct list_head lpfc_els_sgl_list;
 	struct list_head lpfc_abts_els_sgl_list;
 	struct list_head lpfc_nvmet_sgl_list;
 	struct list_head lpfc_abts_nvmet_ctx_list;
 	struct list_head lpfc_abts_scsi_buf_list;
 	struct list_head lpfc_abts_nvme_buf_list;
+	struct list_head lpfc_nvmet_ctx_list;
+	struct list_head lpfc_nvmet_io_wait_list;
 	struct lpfc_sglq **lpfc_sglq_active_list;
 	struct list_head lpfc_rpi_hdr_list;
 	unsigned long *rpi_bmask;
@@ -654,6 +662,7 @@ struct lpfc_sli4_hba {
 	spinlock_t abts_scsi_buf_list_lock; /* list of aborted SCSI IOs */
 	spinlock_t sgl_list_lock; /* list of aborted els IOs */
 	spinlock_t nvmet_io_lock;
+	spinlock_t nvmet_io_wait_lock; /* IOs waiting for ctx resources */
 	uint32_t physical_port;
 
 	/* CPU to vector mapping information */
@@ -661,8 +670,6 @@ struct lpfc_sli4_hba {
 	uint16_t num_online_cpu;
 	uint16_t num_present_cpu;
 	uint16_t curr_disp_cpu;
-
-	uint16_t nvmet_mrq_post_idx;
 };
 
 enum lpfc_sge_type {
@@ -698,6 +705,7 @@ struct lpfc_rpi_hdr {
 	struct lpfc_dmabuf *dmabuf;
 	uint32_t page_count;
 	uint32_t start_rpi;
+	uint16_t next_rpi;
 };
 
 struct lpfc_rsrc_blks {
@@ -762,7 +770,6 @@ int lpfc_rq_create(struct lpfc_hba *, struct lpfc_queue *,
 int lpfc_mrq_create(struct lpfc_hba *phba, struct lpfc_queue **hrqp,
 			struct lpfc_queue **drqp, struct lpfc_queue **cqp,
 			uint32_t subtype);
-void lpfc_rq_adjust_repost(struct lpfc_hba *, struct lpfc_queue *, int);
 int lpfc_eq_destroy(struct lpfc_hba *, struct lpfc_queue *);
 int lpfc_cq_destroy(struct lpfc_hba *, struct lpfc_queue *);
 int lpfc_mq_destroy(struct lpfc_hba *, struct lpfc_queue *);
diff --git a/drivers/scsi/lpfc/lpfc_version.h b/drivers/scsi/lpfc/lpfc_version.h
index 1c26dc67151b..c2653244221c 100644
--- a/drivers/scsi/lpfc/lpfc_version.h
+++ b/drivers/scsi/lpfc/lpfc_version.h
@@ -20,7 +20,7 @@
  * included with this package.                                     *
  *******************************************************************/
 
-#define LPFC_DRIVER_VERSION "11.2.0.12"
+#define LPFC_DRIVER_VERSION "11.2.0.14"
 #define LPFC_DRIVER_NAME		"lpfc"
 
 /* Used for SLI 2/3 */
diff --git a/drivers/scsi/pmcraid.c b/drivers/scsi/pmcraid.c
index a4aadf5f4dc6..1cc814f1505a 100644
--- a/drivers/scsi/pmcraid.c
+++ b/drivers/scsi/pmcraid.c
@@ -3770,9 +3770,6 @@ static long pmcraid_ioctl_passthrough(
 			pmcraid_err("couldn't build passthrough ioadls\n");
 			goto out_free_cmd;
 		}
-	} else if (request_size < 0) {
-		rc = -EINVAL;
-		goto out_free_cmd;
 	}
 
 	/* If data is being written into the device, copy the data from user
diff --git a/drivers/scsi/qedf/qedf.h b/drivers/scsi/qedf/qedf.h
index 40aeb6bb96a2..07ee88200e91 100644
--- a/drivers/scsi/qedf/qedf.h
+++ b/drivers/scsi/qedf/qedf.h
@@ -259,7 +259,7 @@ struct qedf_io_log {
 	uint16_t task_id;
 	uint32_t port_id; /* Remote port fabric ID */
 	int lun;
-	char op; /* SCSI CDB */
+	unsigned char op; /* SCSI CDB */
 	uint8_t lba[4];
 	unsigned int bufflen; /* SCSI buffer length */
 	unsigned int sg_count; /* Number of SG elements */
diff --git a/drivers/scsi/qedf/qedf_els.c b/drivers/scsi/qedf/qedf_els.c
index c505d41f6dc8..90627033bde6 100644
--- a/drivers/scsi/qedf/qedf_els.c
+++ b/drivers/scsi/qedf/qedf_els.c
@@ -109,7 +109,7 @@ retry_els:
 	did = fcport->rdata->ids.port_id;
 	sid = fcport->sid;
 
-	__fc_fill_fc_hdr(fc_hdr, FC_RCTL_ELS_REQ, sid, did,
+	__fc_fill_fc_hdr(fc_hdr, FC_RCTL_ELS_REQ, did, sid,
 			   FC_TYPE_ELS, FC_FC_FIRST_SEQ | FC_FC_END_SEQ |
 			   FC_FC_SEQ_INIT, 0);
 
diff --git a/drivers/scsi/qedf/qedf_main.c b/drivers/scsi/qedf/qedf_main.c
index cceddd995a4b..b97405ed6cae 100644
--- a/drivers/scsi/qedf/qedf_main.c
+++ b/drivers/scsi/qedf/qedf_main.c
@@ -2895,7 +2895,7 @@ static int __qedf_probe(struct pci_dev *pdev, int mode)
 	slowpath_params.drv_minor = QEDF_DRIVER_MINOR_VER;
 	slowpath_params.drv_rev = QEDF_DRIVER_REV_VER;
 	slowpath_params.drv_eng = QEDF_DRIVER_ENG_VER;
-	memcpy(slowpath_params.name, "qedf", QED_DRV_VER_STR_SIZE);
+	strncpy(slowpath_params.name, "qedf", QED_DRV_VER_STR_SIZE);
 	rc = qed_ops->common->slowpath_start(qedf->cdev, &slowpath_params);
 	if (rc) {
 		QEDF_ERR(&(qedf->dbg_ctx), "Cannot start slowpath.\n");
@@ -2954,7 +2954,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 d6978cbc56f0..7658138f2283 100644
--- a/drivers/scsi/qedi/qedi_fw.c
+++ b/drivers/scsi/qedi/qedi_fw.c
@@ -2099,14 +2099,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]);
@@ -2117,7 +2119,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 3548d46f9b27..0c8ccffa4c38 100644
--- a/drivers/scsi/qedi/qedi_iscsi.c
+++ b/drivers/scsi/qedi/qedi_iscsi.c
@@ -1461,9 +1461,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 92775a8b74b1..073b3051bb8f 100644
--- a/drivers/scsi/qedi/qedi_main.c
+++ b/drivers/scsi/qedi/qedi_main.c
@@ -1843,7 +1843,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.c b/drivers/scsi/scsi.c
index 7bfbcfa7af40..61cdd99ae41e 100644
--- a/drivers/scsi/scsi.c
+++ b/drivers/scsi/scsi.c
@@ -763,6 +763,8 @@ struct scsi_device *__scsi_device_lookup(struct Scsi_Host *shost,
 	struct scsi_device *sdev;
 
 	list_for_each_entry(sdev, &shost->__devices, siblings) {
+		if (sdev->sdev_state == SDEV_DEL)
+			continue;
 		if (sdev->channel == channel && sdev->id == id &&
 				sdev->lun ==lun)
 			return sdev;
diff --git a/drivers/scsi/scsi_lib.c b/drivers/scsi/scsi_lib.c
index 814a4bd8405d..99e16ac479e3 100644
--- a/drivers/scsi/scsi_lib.c
+++ b/drivers/scsi/scsi_lib.c
@@ -30,6 +30,7 @@
 #include <scsi/scsi_driver.h>
 #include <scsi/scsi_eh.h>
 #include <scsi/scsi_host.h>
+#include <scsi/scsi_transport.h> /* __scsi_init_queue() */
 #include <scsi/scsi_dh.h>
 
 #include <trace/events/scsi.h>
@@ -1850,7 +1851,7 @@ static int scsi_mq_prep_fn(struct request *req)
 
 	/* zero out the cmd, except for the embedded scsi_request */
 	memset((char *)cmd + sizeof(cmd->req), 0,
-		sizeof(*cmd) - sizeof(cmd->req));
+		sizeof(*cmd) - sizeof(cmd->req) + shost->hostt->cmd_size);
 
 	req->special = cmd;
 
diff --git a/drivers/scsi/scsi_scan.c b/drivers/scsi/scsi_scan.c
index 6f7128f49c30..69979574004f 100644
--- a/drivers/scsi/scsi_scan.c
+++ b/drivers/scsi/scsi_scan.c
@@ -1051,10 +1051,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 d4cf32d55546..1df77453f6b6 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 remails.
+ *    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 remails.
  *
  * 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/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 f9d1432d7cc5..b6bb4e0ce0e3 100644
--- a/drivers/scsi/sd.c
+++ b/drivers/scsi/sd.c
@@ -827,21 +827,32 @@ static int sd_setup_write_zeroes_cmnd(struct scsi_cmnd *cmd)
 	struct scsi_disk *sdkp = scsi_disk(rq->rq_disk);
 	u64 sector = blk_rq_pos(rq) >> (ilog2(sdp->sector_size) - 9);
 	u32 nr_sectors = blk_rq_sectors(rq) >> (ilog2(sdp->sector_size) - 9);
+	int ret;
 
 	if (!(rq->cmd_flags & REQ_NOUNMAP)) {
 		switch (sdkp->zeroing_mode) {
 		case SD_ZERO_WS16_UNMAP:
-			return sd_setup_write_same16_cmnd(cmd, true);
+			ret = sd_setup_write_same16_cmnd(cmd, true);
+			goto out;
 		case SD_ZERO_WS10_UNMAP:
-			return sd_setup_write_same10_cmnd(cmd, true);
+			ret = sd_setup_write_same10_cmnd(cmd, true);
+			goto out;
 		}
 	}
 
 	if (sdp->no_write_same)
 		return BLKPREP_INVALID;
+
 	if (sdkp->ws16 || sector > 0xffffffff || nr_sectors > 0xffff)
-		return sd_setup_write_same16_cmnd(cmd, false);
-	return sd_setup_write_same10_cmnd(cmd, false);
+		ret = sd_setup_write_same16_cmnd(cmd, false);
+	else
+		ret = sd_setup_write_same10_cmnd(cmd, false);
+
+out:
+	if (sd_is_zoned(sdkp) && ret == BLKPREP_OK)
+		return sd_zbc_write_lock_zone(cmd);
+
+	return ret;
 }
 
 static void sd_config_write_same(struct scsi_disk *sdkp)
@@ -948,6 +959,10 @@ static int sd_setup_write_same_cmnd(struct scsi_cmnd *cmd)
 	rq->__data_len = sdp->sector_size;
 	ret = scsi_init_io(cmd);
 	rq->__data_len = nr_bytes;
+
+	if (sd_is_zoned(sdkp) && ret != BLKPREP_OK)
+		sd_zbc_write_unlock_zone(cmd);
+
 	return ret;
 }
 
@@ -1567,17 +1582,21 @@ out:
 	return retval;
 }
 
-static int sd_sync_cache(struct scsi_disk *sdkp)
+static int sd_sync_cache(struct scsi_disk *sdkp, struct scsi_sense_hdr *sshdr)
 {
 	int retries, res;
 	struct scsi_device *sdp = sdkp->device;
 	const int timeout = sdp->request_queue->rq_timeout
 		* SD_FLUSH_TIMEOUT_MULTIPLIER;
-	struct scsi_sense_hdr sshdr;
+	struct scsi_sense_hdr my_sshdr;
 
 	if (!scsi_device_online(sdp))
 		return -ENODEV;
 
+	/* caller might not be interested in sense, but we need it */
+	if (!sshdr)
+		sshdr = &my_sshdr;
+
 	for (retries = 3; retries > 0; --retries) {
 		unsigned char cmd[10] = { 0 };
 
@@ -1586,7 +1605,7 @@ static int sd_sync_cache(struct scsi_disk *sdkp)
 		 * Leave the rest of the command zero to indicate
 		 * flush everything.
 		 */
-		res = scsi_execute(sdp, cmd, DMA_NONE, NULL, 0, NULL, &sshdr,
+		res = scsi_execute(sdp, cmd, DMA_NONE, NULL, 0, NULL, sshdr,
 				timeout, SD_MAX_RETRIES, 0, RQF_PM, NULL);
 		if (res == 0)
 			break;
@@ -1596,11 +1615,12 @@ static int sd_sync_cache(struct scsi_disk *sdkp)
 		sd_print_result(sdkp, "Synchronize Cache(10) failed", res);
 
 		if (driver_byte(res) & DRIVER_SENSE)
-			sd_print_sense_hdr(sdkp, &sshdr);
+			sd_print_sense_hdr(sdkp, sshdr);
+
 		/* we need to evaluate the error return  */
-		if (scsi_sense_valid(&sshdr) &&
-			(sshdr.asc == 0x3a ||	/* medium not present */
-			 sshdr.asc == 0x20))	/* invalid command */
+		if (scsi_sense_valid(sshdr) &&
+			(sshdr->asc == 0x3a ||	/* medium not present */
+			 sshdr->asc == 0x20))	/* invalid command */
 				/* this is no error here */
 				return 0;
 
@@ -3444,7 +3464,7 @@ static void sd_shutdown(struct device *dev)
 
 	if (sdkp->WCE && sdkp->media_present) {
 		sd_printk(KERN_NOTICE, sdkp, "Synchronizing SCSI cache\n");
-		sd_sync_cache(sdkp);
+		sd_sync_cache(sdkp, NULL);
 	}
 
 	if (system_state != SYSTEM_RESTART && sdkp->device->manage_start_stop) {
@@ -3456,6 +3476,7 @@ static void sd_shutdown(struct device *dev)
 static int sd_suspend_common(struct device *dev, bool ignore_stop_errors)
 {
 	struct scsi_disk *sdkp = dev_get_drvdata(dev);
+	struct scsi_sense_hdr sshdr;
 	int ret = 0;
 
 	if (!sdkp)	/* E.g.: runtime suspend following sd_remove() */
@@ -3463,12 +3484,23 @@ static int sd_suspend_common(struct device *dev, bool ignore_stop_errors)
 
 	if (sdkp->WCE && sdkp->media_present) {
 		sd_printk(KERN_NOTICE, sdkp, "Synchronizing SCSI cache\n");
-		ret = sd_sync_cache(sdkp);
+		ret = sd_sync_cache(sdkp, &sshdr);
+
 		if (ret) {
 			/* ignore OFFLINE device */
 			if (ret == -ENODEV)
-				ret = 0;
-			goto done;
+				return 0;
+
+			if (!scsi_sense_valid(&sshdr) ||
+			    sshdr.sense_key != ILLEGAL_REQUEST)
+				return ret;
+
+			/*
+			 * sshdr.sense_key == ILLEGAL_REQUEST means this drive
+			 * doesn't support sync. There's not much to do and
+			 * suspend shouldn't fail.
+			 */
+			 ret = 0;
 		}
 	}
 
@@ -3480,7 +3512,6 @@ static int sd_suspend_common(struct device *dev, bool ignore_stop_errors)
 			ret = 0;
 	}
 
-done:
 	return ret;
 }
 
diff --git a/drivers/scsi/sg.c b/drivers/scsi/sg.c
index 0a38ba01b7b4..82c33a6edbea 100644
--- a/drivers/scsi/sg.c
+++ b/drivers/scsi/sg.c
@@ -2074,11 +2074,12 @@ sg_get_rq_mark(Sg_fd * sfp, int pack_id)
 		if ((1 == resp->done) && (!resp->sg_io_owned) &&
 		    ((-1 == pack_id) || (resp->header.pack_id == pack_id))) {
 			resp->done = 2;	/* guard against other readers */
-			break;
+			write_unlock_irqrestore(&sfp->rq_list_lock, iflags);
+			return resp;
 		}
 	}
 	write_unlock_irqrestore(&sfp->rq_list_lock, iflags);
-	return resp;
+	return NULL;
 }
 
 /* always adds to end of list */
diff --git a/drivers/scsi/ufs/ufshcd.c b/drivers/scsi/ufs/ufshcd.c
index abc7e87937cc..ffe8d8608818 100644
--- a/drivers/scsi/ufs/ufshcd.c
+++ b/drivers/scsi/ufs/ufshcd.c
@@ -7698,6 +7698,12 @@ static inline void ufshcd_add_sysfs_nodes(struct ufs_hba *hba)
 	ufshcd_add_spm_lvl_sysfs_nodes(hba);
 }
 
+static inline void ufshcd_remove_sysfs_nodes(struct ufs_hba *hba)
+{
+	device_remove_file(hba->dev, &hba->rpm_lvl_attr);
+	device_remove_file(hba->dev, &hba->spm_lvl_attr);
+}
+
 /**
  * ufshcd_shutdown - shutdown routine
  * @hba: per adapter instance
@@ -7735,6 +7741,7 @@ EXPORT_SYMBOL(ufshcd_shutdown);
  */
 void ufshcd_remove(struct ufs_hba *hba)
 {
+	ufshcd_remove_sysfs_nodes(hba);
 	scsi_remove_host(hba->host);
 	/* disable interrupts */
 	ufshcd_disable_intr(hba, hba->intr_mask);
diff --git a/drivers/soc/bcm/brcmstb/common.c b/drivers/soc/bcm/brcmstb/common.c
index b6195fdf0d00..22e98a90468c 100644
--- a/drivers/soc/bcm/brcmstb/common.c
+++ b/drivers/soc/bcm/brcmstb/common.c
@@ -49,7 +49,7 @@ static const struct of_device_id sun_top_ctrl_match[] = {
 	{ .compatible = "brcm,bcm7420-sun-top-ctrl", },
 	{ .compatible = "brcm,bcm7425-sun-top-ctrl", },
 	{ .compatible = "brcm,bcm7429-sun-top-ctrl", },
-	{ .compatible = "brcm,bcm7425-sun-top-ctrl", },
+	{ .compatible = "brcm,bcm7435-sun-top-ctrl", },
 	{ .compatible = "brcm,brcmstb-sun-top-ctrl", },
 	{ }
 };
diff --git a/drivers/soc/imx/Kconfig b/drivers/soc/imx/Kconfig
index 357a5d8f8da0..a5b86a28f343 100644
--- a/drivers/soc/imx/Kconfig
+++ b/drivers/soc/imx/Kconfig
@@ -2,8 +2,9 @@ menu "i.MX SoC drivers"
 
 config IMX7_PM_DOMAINS
 	bool "i.MX7 PM domains"
-	select PM_GENERIC_DOMAINS
 	depends on SOC_IMX7D || (COMPILE_TEST && OF)
+	depends on PM
+	select PM_GENERIC_DOMAINS
 	default y if SOC_IMX7D
 
 endmenu
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/ti/knav_dma.c b/drivers/soc/ti/knav_dma.c
index ecebe2eecc3a..026182d3b27c 100644
--- a/drivers/soc/ti/knav_dma.c
+++ b/drivers/soc/ti/knav_dma.c
@@ -413,7 +413,7 @@ static int of_channel_match_helper(struct device_node *np, const char *name,
  * @name:	slave channel name
  * @config:	dma configuration parameters
  *
- * Returns pointer to appropriate DMA channel on success or NULL.
+ * Returns pointer to appropriate DMA channel on success or error.
  */
 void *knav_dma_open_channel(struct device *dev, const char *name,
 					struct knav_dma_cfg *config)
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/devicetree.txt b/drivers/staging/android/ion/devicetree.txt
deleted file mode 100644
index 168715271f06..000000000000
--- a/drivers/staging/android/ion/devicetree.txt
+++ /dev/null
@@ -1,51 +0,0 @@
-Ion Memory Manager
-
-Ion is a memory manager that allows for sharing of buffers via dma-buf.
-Ion allows for different types of allocation via an abstraction called
-a 'heap'. A heap represents a specific type of memory. Each heap has
-a different type. There can be multiple instances of the same heap
-type.
-
-Specific heap instances are tied to heap IDs. Heap IDs are not to be specified
-in the devicetree.
-
-Required properties for Ion
-
-- compatible: "linux,ion" PLUS a compatible property for the device
-
-All child nodes of a linux,ion node are interpreted as heaps
-
-required properties for heaps
-
-- compatible: compatible string for a heap type PLUS a compatible property
-for the specific instance of the heap. Current heap types
--- linux,ion-heap-system
--- linux,ion-heap-system-contig
--- linux,ion-heap-carveout
--- linux,ion-heap-chunk
--- linux,ion-heap-dma
--- linux,ion-heap-custom
-
-Optional properties
-- memory-region: A phandle to a memory region. Required for DMA heap type
-(see reserved-memory.txt for details on the reservation)
-
-Example:
-
-	ion {
-		compatbile = "hisilicon,ion", "linux,ion";
-
-		ion-system-heap {
-			compatbile = "hisilicon,system-heap", "linux,ion-heap-system"
-		};
-
-		ion-camera-region {
-			compatible = "hisilicon,camera-heap", "linux,ion-heap-dma"
-			memory-region = <&camera_region>;
-		};
-
-		ion-fb-region {
-			compatbile = "hisilicon,fb-heap", "linux,ion-heap-dma"
-			memory-region = <&fb_region>;
-		};
-	}
diff --git a/drivers/staging/ccree/Kconfig b/drivers/staging/ccree/Kconfig
index ae627049c499..4be87f503e3b 100644
--- a/drivers/staging/ccree/Kconfig
+++ b/drivers/staging/ccree/Kconfig
@@ -1,6 +1,6 @@
 config CRYPTO_DEV_CCREE
 	tristate "Support for ARM TrustZone CryptoCell C7XX family of Crypto accelerators"
-	depends on CRYPTO_HW && OF && HAS_DMA
+	depends on CRYPTO && CRYPTO_HW && OF && HAS_DMA
 	default n
 	select CRYPTO_HASH
 	select CRYPTO_BLKCIPHER
diff --git a/drivers/staging/ccree/ssi_request_mgr.c b/drivers/staging/ccree/ssi_request_mgr.c
index 522bd62c102e..8611adf3bb2e 100644
--- a/drivers/staging/ccree/ssi_request_mgr.c
+++ b/drivers/staging/ccree/ssi_request_mgr.c
@@ -376,7 +376,6 @@ int send_request(
 	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);
-		spin_unlock_bh(&req_mgr_h->hw_lock);
 		return rc;
 	}
 #endif
diff --git a/drivers/staging/fsl-dpaa2/Kconfig b/drivers/staging/fsl-dpaa2/Kconfig
index 2e325cb747ae..730fd6d4db33 100644
--- a/drivers/staging/fsl-dpaa2/Kconfig
+++ b/drivers/staging/fsl-dpaa2/Kconfig
@@ -12,6 +12,7 @@ config FSL_DPAA2
 config FSL_DPAA2_ETH
 	tristate "Freescale DPAA2 Ethernet"
 	depends on FSL_DPAA2 && FSL_MC_DPIO
+	depends on NETDEVICES && ETHERNET
 	---help---
 	  Ethernet driver for Freescale DPAA2 SoCs, using the
 	  Freescale MC bus driver
diff --git a/drivers/staging/media/atomisp/i2c/Makefile b/drivers/staging/media/atomisp/i2c/Makefile
index 8ea01904c0ea..466517c7c8e6 100644
--- a/drivers/staging/media/atomisp/i2c/Makefile
+++ b/drivers/staging/media/atomisp/i2c/Makefile
@@ -19,5 +19,3 @@ obj-$(CONFIG_VIDEO_AP1302)     += ap1302.o
 
 obj-$(CONFIG_VIDEO_LM3554) += lm3554.o
 
-ccflags-y += -Werror
-
diff --git a/drivers/staging/media/atomisp/i2c/imx/Makefile b/drivers/staging/media/atomisp/i2c/imx/Makefile
index 1d7f7ab94cac..6b13a3a66e49 100644
--- a/drivers/staging/media/atomisp/i2c/imx/Makefile
+++ b/drivers/staging/media/atomisp/i2c/imx/Makefile
@@ -4,5 +4,3 @@ 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
-
-ccflags-y += -Werror
diff --git a/drivers/staging/media/atomisp/i2c/ov5693/Makefile b/drivers/staging/media/atomisp/i2c/ov5693/Makefile
index fceb9e9b881b..c9c0e1245858 100644
--- a/drivers/staging/media/atomisp/i2c/ov5693/Makefile
+++ b/drivers/staging/media/atomisp/i2c/ov5693/Makefile
@@ -1,3 +1 @@
 obj-$(CONFIG_VIDEO_OV5693) += ov5693.o
-
-ccflags-y += -Werror
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/Makefile b/drivers/staging/media/atomisp/pci/atomisp2/Makefile
index 3fa7c1c1479f..f126a89a08e9 100644
--- a/drivers/staging/media/atomisp/pci/atomisp2/Makefile
+++ b/drivers/staging/media/atomisp/pci/atomisp2/Makefile
@@ -351,5 +351,5 @@ DEFINES := -DHRT_HW -DHRT_ISP_CSS_CUSTOM_HOST -DHRT_USE_VIR_ADDRS -D__HOST__
 DEFINES += -DATOMISP_POSTFIX=\"css2400b0_v21\" -DISP2400B0
 DEFINES += -DSYSTEM_hive_isp_css_2400_system -DISP2400
 
-ccflags-y += $(INCLUDES) $(DEFINES) -fno-common -Werror
+ccflags-y += $(INCLUDES) $(DEFINES) -fno-common
 
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_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..a0478077a012 100644
--- a/drivers/staging/media/atomisp/pci/atomisp2/atomisp_v4l2.c
+++ b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_v4l2.c
@@ -51,7 +51,7 @@
 /* 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");
 
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/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/runtime/binary/src/binary.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/binary/src/binary.c
index a8b93a756e41..ae0b229c9fb8 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
@@ -1658,7 +1658,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/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..bcc0d464084f 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
@@ -3148,8 +3148,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 +3179,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/sh_css.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css.c
index 73c76583610a..81a21a0c1391 100644
--- a/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css.c
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css.c
@@ -2003,7 +2003,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 +2016,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 +2059,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);
@@ -2766,7 +2767,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;
 }
@@ -4514,7 +4515,7 @@ ia_css_pipe_enqueue_buffer(struct ia_css_pipe *pipe,
 #else
 		if (hmm_buffer_record) {
 #endif
-			IA_CSS_LOG("send vbuf=0x%x", h_vbuf);
+			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 +4625,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 +4641,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;
@@ -6621,7 +6622,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;
 	}
 
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_params.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_params.c
index 561f4a7236f7..d8c22e8cd3af 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
@@ -3375,7 +3375,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 +3397,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 +3437,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;
 }
 
@@ -3859,7 +3859,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 +4590,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/platform/intel-mid/atomisp_gmin_platform.c b/drivers/staging/media/atomisp/platform/intel-mid/atomisp_gmin_platform.c
index 5b4506a71126..d68e9cf33aa7 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,8 +627,10 @@ 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];
@@ -627,20 +639,21 @@ int gmin_get_config_var(struct device *dev, const char *var, char *out, size_t *
 	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 +678,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,7 +692,8 @@ 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;
@@ -692,7 +707,7 @@ int gmin_get_config_var(struct device *dev, const char *var, char *out, size_t *
 	*out_len = efilen;
 
 	if (ret)
- 		dev_warn(dev, "Failed to find gmin variable %s\n", var8);
+		dev_warn(dev, "Failed to find gmin variable %s\n", var8);
 
 	return ret;
 }
@@ -718,38 +733,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/mt29f_spinand/mt29f_spinand.c b/drivers/staging/mt29f_spinand/mt29f_spinand.c
index e389009fca42..a4e3ae8f0c85 100644
--- a/drivers/staging/mt29f_spinand/mt29f_spinand.c
+++ b/drivers/staging/mt29f_spinand/mt29f_spinand.c
@@ -915,6 +915,8 @@ static int spinand_probe(struct spi_device *spi_nand)
 	chip->waitfunc	= spinand_wait;
 	chip->options	|= NAND_CACHEPRG;
 	chip->select_chip = spinand_select_chip;
+	chip->onfi_set_features = nand_onfi_get_set_features_notsupp;
+	chip->onfi_get_features = nand_onfi_get_set_features_notsupp;
 
 	mtd = nand_to_mtd(chip);
 
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_dev.c b/drivers/staging/rtl8192e/rtl8192e/r8192E_dev.c
index 4723a0bd5067..1c6ed5b2a6f9 100644
--- a/drivers/staging/rtl8192e/rtl8192e/r8192E_dev.c
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_dev.c
@@ -97,8 +97,9 @@ void rtl92e_set_reg(struct net_device *dev, u8 variable, u8 *val)
 
 	switch (variable) {
 	case HW_VAR_BSSID:
-		rtl92e_writel(dev, BSSIDR, ((u32 *)(val))[0]);
-		rtl92e_writew(dev, BSSIDR+2, ((u16 *)(val+2))[0]);
+		/* BSSIDR 2 byte alignment */
+		rtl92e_writew(dev, BSSIDR, *(u16 *)val);
+		rtl92e_writel(dev, BSSIDR + 2, *(u32 *)(val + 2));
 		break;
 
 	case HW_VAR_MEDIA_STATUS:
@@ -624,7 +625,7 @@ void rtl92e_get_eeprom_size(struct net_device *dev)
 	struct r8192_priv *priv = rtllib_priv(dev);
 
 	RT_TRACE(COMP_INIT, "===========>%s()\n", __func__);
-	curCR = rtl92e_readl(dev, EPROM_CMD);
+	curCR = rtl92e_readw(dev, EPROM_CMD);
 	RT_TRACE(COMP_INIT, "read from Reg Cmd9346CR(%x):%x\n", EPROM_CMD,
 		 curCR);
 	priv->epromtype = (curCR & EPROM_CMD_9356SEL) ? EEPROM_93C56 :
@@ -961,8 +962,8 @@ static void _rtl92e_net_update(struct net_device *dev)
 	rtl92e_config_rate(dev, &rate_config);
 	priv->dot11CurrentPreambleMode = PREAMBLE_AUTO;
 	 priv->basic_rate = rate_config &= 0x15f;
-	rtl92e_writel(dev, BSSIDR, ((u32 *)net->bssid)[0]);
-	rtl92e_writew(dev, BSSIDR+4, ((u16 *)net->bssid)[2]);
+	rtl92e_writew(dev, BSSIDR, *(u16 *)net->bssid);
+	rtl92e_writel(dev, BSSIDR + 2, *(u32 *)(net->bssid + 2));
 
 	if (priv->rtllib->iw_mode == IW_MODE_ADHOC) {
 		rtl92e_writew(dev, ATIMWND, 2);
@@ -1182,8 +1183,7 @@ void  rtl92e_fill_tx_desc(struct net_device *dev, struct tx_desc *pdesc,
 			  struct cb_desc *cb_desc, struct sk_buff *skb)
 {
 	struct r8192_priv *priv = rtllib_priv(dev);
-	dma_addr_t mapping = pci_map_single(priv->pdev, skb->data, skb->len,
-			 PCI_DMA_TODEVICE);
+	dma_addr_t mapping;
 	struct tx_fwinfo_8190pci *pTxFwInfo;
 
 	pTxFwInfo = (struct tx_fwinfo_8190pci *)skb->data;
@@ -1194,8 +1194,6 @@ void  rtl92e_fill_tx_desc(struct net_device *dev, struct tx_desc *pdesc,
 	pTxFwInfo->Short = _rtl92e_query_is_short(pTxFwInfo->TxHT,
 						  pTxFwInfo->TxRate, cb_desc);
 
-	if (pci_dma_mapping_error(priv->pdev, mapping))
-		netdev_err(dev, "%s(): DMA Mapping error\n", __func__);
 	if (cb_desc->bAMPDUEnable) {
 		pTxFwInfo->AllowAggregation = 1;
 		pTxFwInfo->RxMF = cb_desc->ampdu_factor;
@@ -1230,6 +1228,14 @@ void  rtl92e_fill_tx_desc(struct net_device *dev, struct tx_desc *pdesc,
 	}
 
 	memset((u8 *)pdesc, 0, 12);
+
+	mapping = pci_map_single(priv->pdev, skb->data, skb->len,
+				 PCI_DMA_TODEVICE);
+	if (pci_dma_mapping_error(priv->pdev, mapping)) {
+		netdev_err(dev, "%s(): DMA Mapping error\n", __func__);
+		return;
+	}
+
 	pdesc->LINIP = 0;
 	pdesc->CmdInit = 1;
 	pdesc->Offset = sizeof(struct tx_fwinfo_8190pci) + 8;
diff --git a/drivers/staging/rtl8192e/rtl819x_TSProc.c b/drivers/staging/rtl8192e/rtl819x_TSProc.c
index 48bbd9e8a52f..dcc4eb691889 100644
--- a/drivers/staging/rtl8192e/rtl819x_TSProc.c
+++ b/drivers/staging/rtl8192e/rtl819x_TSProc.c
@@ -306,11 +306,6 @@ static void MakeTSEntry(struct ts_common_info *pTsCommonInfo, u8 *Addr,
 	pTsCommonInfo->TClasNum = TCLAS_Num;
 }
 
-static bool IsACValid(unsigned int tid)
-{
-	return tid < 7;
-}
-
 bool GetTs(struct rtllib_device *ieee, struct ts_common_info **ppTS,
 	   u8 *Addr, u8 TID, enum tr_select TxRxSelect, bool bAddNewTs)
 {
@@ -328,12 +323,6 @@ bool GetTs(struct rtllib_device *ieee, struct ts_common_info **ppTS,
 	if (ieee->current_network.qos_data.supported == 0) {
 		UP = 0;
 	} else {
-		if (!IsACValid(TID)) {
-			netdev_warn(ieee->dev, "%s(): TID(%d) is not valid\n",
-				    __func__, TID);
-			return false;
-		}
-
 		switch (TID) {
 		case 0:
 		case 3:
@@ -351,6 +340,10 @@ bool GetTs(struct rtllib_device *ieee, struct ts_common_info **ppTS,
 		case 7:
 			UP = 7;
 			break;
+		default:
+			netdev_warn(ieee->dev, "%s(): TID(%d) is not valid\n",
+				    __func__, TID);
+			return false;
 		}
 	}
 
diff --git a/drivers/staging/rtl8723bs/os_dep/ioctl_cfg80211.c b/drivers/staging/rtl8723bs/os_dep/ioctl_cfg80211.c
index 5e7a61f24f8d..36c3189fc4b7 100644
--- a/drivers/staging/rtl8723bs/os_dep/ioctl_cfg80211.c
+++ b/drivers/staging/rtl8723bs/os_dep/ioctl_cfg80211.c
@@ -3531,7 +3531,6 @@ int rtw_wdev_alloc(struct adapter *padapter, struct device *dev)
 		pwdev_priv->power_mgmt = true;
 	else
 		pwdev_priv->power_mgmt = false;
-	kfree((u8 *)wdev);
 
 	return ret;
 
diff --git a/drivers/staging/typec/fusb302/fusb302.c b/drivers/staging/typec/fusb302/fusb302.c
index 2cee9a952c9b..4a356e509fe4 100644
--- a/drivers/staging/typec/fusb302/fusb302.c
+++ b/drivers/staging/typec/fusb302/fusb302.c
@@ -264,22 +264,36 @@ static void fusb302_debugfs_exit(const struct fusb302_chip *chip) { }
 
 #define FUSB302_RESUME_RETRY 10
 #define FUSB302_RESUME_RETRY_SLEEP 50
-static int fusb302_i2c_write(struct fusb302_chip *chip,
-			     u8 address, u8 data)
+
+static bool fusb302_is_suspended(struct fusb302_chip *chip)
 {
 	int retry_cnt;
-	int ret = 0;
 
-	atomic_set(&chip->i2c_busy, 1);
 	for (retry_cnt = 0; retry_cnt < FUSB302_RESUME_RETRY; retry_cnt++) {
 		if (atomic_read(&chip->pm_suspend)) {
-			pr_err("fusb302_i2c: pm suspend, retry %d/%d\n",
-			       retry_cnt + 1, FUSB302_RESUME_RETRY);
+			dev_err(chip->dev, "i2c: pm suspend, retry %d/%d\n",
+				retry_cnt + 1, FUSB302_RESUME_RETRY);
 			msleep(FUSB302_RESUME_RETRY_SLEEP);
 		} else {
-			break;
+			return false;
 		}
 	}
+
+	return true;
+}
+
+static int fusb302_i2c_write(struct fusb302_chip *chip,
+			     u8 address, u8 data)
+{
+	int ret = 0;
+
+	atomic_set(&chip->i2c_busy, 1);
+
+	if (fusb302_is_suspended(chip)) {
+		atomic_set(&chip->i2c_busy, 0);
+		return -ETIMEDOUT;
+	}
+
 	ret = i2c_smbus_write_byte_data(chip->i2c_client, address, data);
 	if (ret < 0)
 		fusb302_log(chip, "cannot write 0x%02x to 0x%02x, ret=%d",
@@ -292,21 +306,17 @@ static int fusb302_i2c_write(struct fusb302_chip *chip,
 static int fusb302_i2c_block_write(struct fusb302_chip *chip, u8 address,
 				   u8 length, const u8 *data)
 {
-	int retry_cnt;
 	int ret = 0;
 
 	if (length <= 0)
 		return ret;
 	atomic_set(&chip->i2c_busy, 1);
-	for (retry_cnt = 0; retry_cnt < FUSB302_RESUME_RETRY; retry_cnt++) {
-		if (atomic_read(&chip->pm_suspend)) {
-			pr_err("fusb302_i2c: pm suspend, retry %d/%d\n",
-			       retry_cnt + 1, FUSB302_RESUME_RETRY);
-			msleep(FUSB302_RESUME_RETRY_SLEEP);
-		} else {
-			break;
-		}
+
+	if (fusb302_is_suspended(chip)) {
+		atomic_set(&chip->i2c_busy, 0);
+		return -ETIMEDOUT;
 	}
+
 	ret = i2c_smbus_write_i2c_block_data(chip->i2c_client, address,
 					     length, data);
 	if (ret < 0)
@@ -320,19 +330,15 @@ static int fusb302_i2c_block_write(struct fusb302_chip *chip, u8 address,
 static int fusb302_i2c_read(struct fusb302_chip *chip,
 			    u8 address, u8 *data)
 {
-	int retry_cnt;
 	int ret = 0;
 
 	atomic_set(&chip->i2c_busy, 1);
-	for (retry_cnt = 0; retry_cnt < FUSB302_RESUME_RETRY; retry_cnt++) {
-		if (atomic_read(&chip->pm_suspend)) {
-			pr_err("fusb302_i2c: pm suspend, retry %d/%d\n",
-			       retry_cnt + 1, FUSB302_RESUME_RETRY);
-			msleep(FUSB302_RESUME_RETRY_SLEEP);
-		} else {
-			break;
-		}
+
+	if (fusb302_is_suspended(chip)) {
+		atomic_set(&chip->i2c_busy, 0);
+		return -ETIMEDOUT;
 	}
+
 	ret = i2c_smbus_read_byte_data(chip->i2c_client, address);
 	*data = (u8)ret;
 	if (ret < 0)
@@ -345,33 +351,31 @@ static int fusb302_i2c_read(struct fusb302_chip *chip,
 static int fusb302_i2c_block_read(struct fusb302_chip *chip, u8 address,
 				  u8 length, u8 *data)
 {
-	int retry_cnt;
 	int ret = 0;
 
 	if (length <= 0)
 		return ret;
 	atomic_set(&chip->i2c_busy, 1);
-	for (retry_cnt = 0; retry_cnt < FUSB302_RESUME_RETRY; retry_cnt++) {
-		if (atomic_read(&chip->pm_suspend)) {
-			pr_err("fusb302_i2c: pm suspend, retry %d/%d\n",
-			       retry_cnt + 1, FUSB302_RESUME_RETRY);
-			msleep(FUSB302_RESUME_RETRY_SLEEP);
-		} else {
-			break;
-		}
+
+	if (fusb302_is_suspended(chip)) {
+		atomic_set(&chip->i2c_busy, 0);
+		return -ETIMEDOUT;
 	}
+
 	ret = i2c_smbus_read_i2c_block_data(chip->i2c_client, address,
 					    length, data);
 	if (ret < 0) {
 		fusb302_log(chip, "cannot block read 0x%02x, len=%d, ret=%d",
 			    address, length, ret);
-		return ret;
+		goto done;
 	}
 	if (ret != length) {
 		fusb302_log(chip, "only read %d/%d bytes from 0x%02x",
 			    ret, length, address);
-		return -EIO;
+		ret = -EIO;
 	}
+
+done:
 	atomic_set(&chip->i2c_busy, 0);
 
 	return ret;
@@ -489,7 +493,7 @@ static int tcpm_init(struct tcpc_dev *dev)
 	ret = fusb302_i2c_read(chip, FUSB_REG_STATUS0, &data);
 	if (ret < 0)
 		return ret;
-	chip->vbus_present = !!(FUSB_REG_STATUS0 & FUSB_REG_STATUS0_VBUSOK);
+	chip->vbus_present = !!(data & FUSB_REG_STATUS0_VBUSOK);
 	ret = fusb302_i2c_read(chip, FUSB_REG_DEVICE_ID, &data);
 	if (ret < 0)
 		return ret;
@@ -1025,7 +1029,7 @@ static int fusb302_pd_send_message(struct fusb302_chip *chip,
 	buf[pos++] = FUSB302_TKN_SYNC1;
 	buf[pos++] = FUSB302_TKN_SYNC2;
 
-	len = pd_header_cnt(msg->header) * 4;
+	len = pd_header_cnt_le(msg->header) * 4;
 	/* plug 2 for header */
 	len += 2;
 	if (len > 0x1F) {
@@ -1481,7 +1485,7 @@ static int fusb302_pd_read_message(struct fusb302_chip *chip,
 				     (u8 *)&msg->header);
 	if (ret < 0)
 		return ret;
-	len = pd_header_cnt(msg->header) * 4;
+	len = pd_header_cnt_le(msg->header) * 4;
 	/* add 4 to length to include the CRC */
 	if (len > PD_MAX_PAYLOAD * 4) {
 		fusb302_log(chip, "PD message too long %d", len);
@@ -1663,14 +1667,12 @@ static int init_gpio(struct fusb302_chip *chip)
 	if (ret < 0) {
 		fusb302_log(chip,
 			    "cannot set GPIO Int_N to input, ret=%d", ret);
-		gpio_free(chip->gpio_int_n);
 		return ret;
 	}
 	ret = gpio_to_irq(chip->gpio_int_n);
 	if (ret < 0) {
 		fusb302_log(chip,
 			    "cannot request IRQ for GPIO Int_N, ret=%d", ret);
-		gpio_free(chip->gpio_int_n);
 		return ret;
 	}
 	chip->gpio_int_n_irq = ret;
@@ -1787,11 +1789,13 @@ static const struct of_device_id fusb302_dt_match[] = {
 	{.compatible = "fcs,fusb302"},
 	{},
 };
+MODULE_DEVICE_TABLE(of, fusb302_dt_match);
 
 static const struct i2c_device_id fusb302_i2c_device_id[] = {
 	{"typec_fusb302", 0},
 	{},
 };
+MODULE_DEVICE_TABLE(i2c, fusb302_i2c_device_id);
 
 static const struct dev_pm_ops fusb302_pm_ops = {
 	.suspend = fusb302_pm_suspend,
diff --git a/drivers/staging/typec/pd.h b/drivers/staging/typec/pd.h
index 8d97bdb95f23..510ef7279900 100644
--- a/drivers/staging/typec/pd.h
+++ b/drivers/staging/typec/pd.h
@@ -92,6 +92,16 @@ static inline unsigned int pd_header_type_le(__le16 header)
 	return pd_header_type(le16_to_cpu(header));
 }
 
+static inline unsigned int pd_header_msgid(u16 header)
+{
+	return (header >> PD_HEADER_ID_SHIFT) & PD_HEADER_ID_MASK;
+}
+
+static inline unsigned int pd_header_msgid_le(__le16 header)
+{
+	return pd_header_msgid(le16_to_cpu(header));
+}
+
 #define PD_MAX_PAYLOAD		7
 
 struct pd_message {
diff --git a/drivers/staging/typec/pd_vdo.h b/drivers/staging/typec/pd_vdo.h
index dba172e0e0d1..d92259f8de0a 100644
--- a/drivers/staging/typec/pd_vdo.h
+++ b/drivers/staging/typec/pd_vdo.h
@@ -22,6 +22,9 @@
  * VDM object is minimum of VDM header + 6 additional data objects.
  */
 
+#define VDO_MAX_OBJECTS		6
+#define VDO_MAX_SIZE		(VDO_MAX_OBJECTS + 1)
+
 /*
  * VDM header
  * ----------
@@ -34,7 +37,6 @@
  * <5>      :: reserved (SVDM), command type (UVDM)
  * <4:0>    :: command
  */
-#define VDO_MAX_SIZE 7
 #define VDO(vid, type, custom)				\
 	(((vid) << 16) |				\
 	 ((type) << 15) |				\
diff --git a/drivers/staging/typec/tcpci.c b/drivers/staging/typec/tcpci.c
index 5e5be74c7850..df72d8b01e73 100644
--- a/drivers/staging/typec/tcpci.c
+++ b/drivers/staging/typec/tcpci.c
@@ -425,7 +425,7 @@ static const struct regmap_config tcpci_regmap_config = {
 	.max_register = 0x7F, /* 0x80 .. 0xFF are vendor defined */
 };
 
-const struct tcpc_config tcpci_tcpc_config = {
+static const struct tcpc_config tcpci_tcpc_config = {
 	.type = TYPEC_PORT_DFP,
 	.default_role = TYPEC_SINK,
 };
diff --git a/drivers/staging/typec/tcpm.c b/drivers/staging/typec/tcpm.c
index abba655ba00a..20eb4ebcf8c3 100644
--- a/drivers/staging/typec/tcpm.c
+++ b/drivers/staging/typec/tcpm.c
@@ -238,6 +238,7 @@ struct tcpm_port {
 	unsigned int hard_reset_count;
 	bool pd_capable;
 	bool explicit_contract;
+	unsigned int rx_msgid;
 
 	/* Partner capabilities/requests */
 	u32 sink_request;
@@ -251,6 +252,8 @@ struct tcpm_port {
 	unsigned int nr_src_pdo;
 	u32 snk_pdo[PDO_MAX_OBJECTS];
 	unsigned int nr_snk_pdo;
+	u32 snk_vdo[VDO_MAX_OBJECTS];
+	unsigned int nr_snk_vdo;
 
 	unsigned int max_snk_mv;
 	unsigned int max_snk_ma;
@@ -997,6 +1000,7 @@ static int tcpm_pd_svdm(struct tcpm_port *port, const __le32 *payload, int cnt,
 	struct pd_mode_data *modep;
 	int rlen = 0;
 	u16 svid;
+	int i;
 
 	tcpm_log(port, "Rx VDM cmd 0x%x type %d cmd %d len %d",
 		 p0, cmd_type, cmd, cnt);
@@ -1007,6 +1011,14 @@ static int tcpm_pd_svdm(struct tcpm_port *port, const __le32 *payload, int cnt,
 	case CMDT_INIT:
 		switch (cmd) {
 		case CMD_DISCOVER_IDENT:
+			/* 6.4.4.3.1: Only respond as UFP (device) */
+			if (port->data_role == TYPEC_DEVICE &&
+			    port->nr_snk_vdo) {
+				for (i = 0; i <  port->nr_snk_vdo; i++)
+					response[i + 1]
+						= cpu_to_le32(port->snk_vdo[i]);
+				rlen = port->nr_snk_vdo + 1;
+			}
 			break;
 		case CMD_DISCOVER_SVID:
 			break;
@@ -1415,6 +1427,7 @@ static void tcpm_pd_ctrl_request(struct tcpm_port *port,
 			break;
 		case SOFT_RESET_SEND:
 			port->message_id = 0;
+			port->rx_msgid = -1;
 			if (port->pwr_role == TYPEC_SOURCE)
 				next_state = SRC_SEND_CAPABILITIES;
 			else
@@ -1503,6 +1516,22 @@ static void tcpm_pd_rx_handler(struct work_struct *work)
 		 port->attached);
 
 	if (port->attached) {
+		enum pd_ctrl_msg_type type = pd_header_type_le(msg->header);
+		unsigned int msgid = pd_header_msgid_le(msg->header);
+
+		/*
+		 * USB PD standard, 6.6.1.2:
+		 * "... if MessageID value in a received Message is the
+		 * same as the stored value, the receiver shall return a
+		 * GoodCRC Message with that MessageID value and drop
+		 * the Message (this is a retry of an already received
+		 * Message). Note: this shall not apply to the Soft_Reset
+		 * Message which always has a MessageID value of zero."
+		 */
+		if (msgid == port->rx_msgid && type != PD_CTRL_SOFT_RESET)
+			goto done;
+		port->rx_msgid = msgid;
+
 		/*
 		 * If both ends believe to be DFP/host, we have a data role
 		 * mismatch.
@@ -1520,6 +1549,7 @@ static void tcpm_pd_rx_handler(struct work_struct *work)
 		}
 	}
 
+done:
 	mutex_unlock(&port->lock);
 	kfree(event);
 }
@@ -1719,8 +1749,7 @@ static int tcpm_pd_build_request(struct tcpm_port *port, u32 *rdo)
 	}
 	ma = min(ma, port->max_snk_ma);
 
-	/* XXX: Any other flags need to be set? */
-	flags = 0;
+	flags = RDO_USB_COMM | RDO_NO_SUSPEND;
 
 	/* Set mismatch bit if offered power is less than operating power */
 	mw = ma * mv / 1000;
@@ -1957,6 +1986,12 @@ static void tcpm_reset_port(struct tcpm_port *port)
 	port->attached = false;
 	port->pd_capable = false;
 
+	/*
+	 * First Rx ID should be 0; set this to a sentinel of -1 so that
+	 * we can check tcpm_pd_rx_handler() if we had seen it before.
+	 */
+	port->rx_msgid = -1;
+
 	port->tcpc->set_pd_rx(port->tcpc, false);
 	tcpm_init_vbus(port);	/* also disables charging */
 	tcpm_init_vconn(port);
@@ -2170,6 +2205,7 @@ static void run_state_machine(struct tcpm_port *port)
 		port->pwr_opmode = TYPEC_PWR_MODE_USB;
 		port->caps_count = 0;
 		port->message_id = 0;
+		port->rx_msgid = -1;
 		port->explicit_contract = false;
 		tcpm_set_state(port, SRC_SEND_CAPABILITIES, 0);
 		break;
@@ -2329,6 +2365,7 @@ static void run_state_machine(struct tcpm_port *port)
 		typec_set_pwr_opmode(port->typec_port, TYPEC_PWR_MODE_USB);
 		port->pwr_opmode = TYPEC_PWR_MODE_USB;
 		port->message_id = 0;
+		port->rx_msgid = -1;
 		port->explicit_contract = false;
 		tcpm_set_state(port, SNK_DISCOVERY, 0);
 		break;
@@ -2496,6 +2533,7 @@ static void run_state_machine(struct tcpm_port *port)
 	/* Soft_Reset states */
 	case SOFT_RESET:
 		port->message_id = 0;
+		port->rx_msgid = -1;
 		tcpm_pd_send_control(port, PD_CTRL_ACCEPT);
 		if (port->pwr_role == TYPEC_SOURCE)
 			tcpm_set_state(port, SRC_SEND_CAPABILITIES, 0);
@@ -2504,6 +2542,7 @@ static void run_state_machine(struct tcpm_port *port)
 		break;
 	case SOFT_RESET_SEND:
 		port->message_id = 0;
+		port->rx_msgid = -1;
 		if (tcpm_pd_send_control(port, PD_CTRL_SOFT_RESET))
 			tcpm_set_state_cond(port, hard_reset_state(port), 0);
 		else
@@ -2568,6 +2607,14 @@ static void run_state_machine(struct tcpm_port *port)
 		break;
 	case PR_SWAP_SRC_SNK_SOURCE_OFF:
 		tcpm_set_cc(port, TYPEC_CC_RD);
+		/*
+		 * USB-PD standard, 6.2.1.4, Port Power Role:
+		 * "During the Power Role Swap Sequence, for the initial Source
+		 * Port, the Port Power Role field shall be set to Sink in the
+		 * PS_RDY Message indicating that the initial Source’s power
+		 * supply is turned off"
+		 */
+		tcpm_set_pwr_role(port, TYPEC_SINK);
 		if (tcpm_pd_send_control(port, PD_CTRL_PS_RDY)) {
 			tcpm_set_state(port, ERROR_RECOVERY, 0);
 			break;
@@ -2575,7 +2622,6 @@ static void run_state_machine(struct tcpm_port *port)
 		tcpm_set_state_cond(port, SNK_UNATTACHED, PD_T_PS_SOURCE_ON);
 		break;
 	case PR_SWAP_SRC_SNK_SINK_ON:
-		tcpm_set_pwr_role(port, TYPEC_SINK);
 		tcpm_swap_complete(port, 0);
 		tcpm_set_state(port, SNK_STARTUP, 0);
 		break;
@@ -2587,8 +2633,15 @@ static void run_state_machine(struct tcpm_port *port)
 	case PR_SWAP_SNK_SRC_SOURCE_ON:
 		tcpm_set_cc(port, tcpm_rp_cc(port));
 		tcpm_set_vbus(port, true);
-		tcpm_pd_send_control(port, PD_CTRL_PS_RDY);
+		/*
+		 * USB PD standard, 6.2.1.4:
+		 * "Subsequent Messages initiated by the Policy Engine,
+		 * such as the PS_RDY Message sent to indicate that Vbus
+		 * is ready, will have the Port Power Role field set to
+		 * Source."
+		 */
 		tcpm_set_pwr_role(port, TYPEC_SOURCE);
+		tcpm_pd_send_control(port, PD_CTRL_PS_RDY);
 		tcpm_swap_complete(port, 0);
 		tcpm_set_state(port, SRC_STARTUP, 0);
 		break;
@@ -3292,6 +3345,20 @@ static int tcpm_copy_pdos(u32 *dest_pdo, const u32 *src_pdo,
 	return nr_pdo;
 }
 
+static int tcpm_copy_vdos(u32 *dest_vdo, const u32 *src_vdo,
+			  unsigned int nr_vdo)
+{
+	unsigned int i;
+
+	if (nr_vdo > VDO_MAX_OBJECTS)
+		nr_vdo = VDO_MAX_OBJECTS;
+
+	for (i = 0; i < nr_vdo; i++)
+		dest_vdo[i] = src_vdo[i];
+
+	return nr_vdo;
+}
+
 void tcpm_update_source_capabilities(struct tcpm_port *port, const u32 *pdo,
 				     unsigned int nr_pdo)
 {
@@ -3382,6 +3449,8 @@ struct tcpm_port *tcpm_register_port(struct device *dev, struct tcpc_dev *tcpc)
 					  tcpc->config->nr_src_pdo);
 	port->nr_snk_pdo = tcpm_copy_pdos(port->snk_pdo, tcpc->config->snk_pdo,
 					  tcpc->config->nr_snk_pdo);
+	port->nr_snk_vdo = tcpm_copy_vdos(port->snk_vdo, tcpc->config->snk_vdo,
+					  tcpc->config->nr_snk_vdo);
 
 	port->max_snk_mv = tcpc->config->max_snk_mv;
 	port->max_snk_ma = tcpc->config->max_snk_ma;
diff --git a/drivers/staging/typec/tcpm.h b/drivers/staging/typec/tcpm.h
index 969b365e6549..19c307d31a5a 100644
--- a/drivers/staging/typec/tcpm.h
+++ b/drivers/staging/typec/tcpm.h
@@ -60,6 +60,9 @@ struct tcpc_config {
 	const u32 *snk_pdo;
 	unsigned int nr_snk_pdo;
 
+	const u32 *snk_vdo;
+	unsigned int nr_snk_vdo;
+
 	unsigned int max_snk_mv;
 	unsigned int max_snk_ma;
 	unsigned int max_snk_mw;
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 988ee61fb4a7..d04db3f55519 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
@@ -502,8 +502,15 @@ create_pagelist(char __user *buf, size_t count, unsigned short type,
 	 */
 	sg_init_table(scatterlist, num_pages);
 	/* Now set the pages for each scatterlist */
-	for (i = 0; i < num_pages; i++)
-		sg_set_page(scatterlist + i, pages[i], PAGE_SIZE, 0);
+	for (i = 0; i < num_pages; i++)	{
+		unsigned int len = PAGE_SIZE - offset;
+
+		if (len > count)
+			len = count;
+		sg_set_page(scatterlist + i, pages[i], len, offset);
+		offset = 0;
+		count -= len;
+	}
 
 	dma_buffers = dma_map_sg(g_dev,
 				 scatterlist,
@@ -524,20 +531,20 @@ create_pagelist(char __user *buf, size_t count, unsigned short type,
 		u32 addr = sg_dma_address(sg);
 
 		/* Note: addrs is the address + page_count - 1
-		 * The firmware expects the block to be page
+		 * The firmware expects blocks after the first to be page-
 		 * aligned and a multiple of the page size
 		 */
 		WARN_ON(len == 0);
-		WARN_ON(len & ~PAGE_MASK);
-		WARN_ON(addr & ~PAGE_MASK);
+		WARN_ON(i && (i != (dma_buffers - 1)) && (len & ~PAGE_MASK));
+		WARN_ON(i && (addr & ~PAGE_MASK));
 		if (k > 0 &&
-		    ((addrs[k - 1] & PAGE_MASK) |
-			((addrs[k - 1] & ~PAGE_MASK) + 1) << PAGE_SHIFT)
-		    == addr) {
-			addrs[k - 1] += (len >> PAGE_SHIFT);
-		} else {
-			addrs[k++] = addr | ((len >> PAGE_SHIFT) - 1);
-		}
+		    ((addrs[k - 1] & PAGE_MASK) +
+		     (((addrs[k - 1] & ~PAGE_MASK) + 1) << PAGE_SHIFT))
+		    == (addr & PAGE_MASK))
+			addrs[k - 1] += ((len + PAGE_SIZE - 1) >> PAGE_SHIFT);
+		else
+			addrs[k++] = (addr & PAGE_MASK) |
+				(((len + PAGE_SIZE - 1) >> PAGE_SHIFT) - 1);
 	}
 
 	/* Partial cache lines (fragments) require special measures */
diff --git a/drivers/tee/Kconfig b/drivers/tee/Kconfig
index 2330a4eb4e8b..a6df12d88f90 100644
--- a/drivers/tee/Kconfig
+++ b/drivers/tee/Kconfig
@@ -1,6 +1,7 @@
 # Generic Trusted Execution Environment Configuration
 config TEE
 	tristate "Trusted Execution Environment support"
+	depends on HAVE_ARM_SMCCC || COMPILE_TEST
 	select DMA_SHARED_BUFFER
 	select GENERIC_ALLOCATOR
 	help
diff --git a/drivers/thermal/broadcom/Kconfig b/drivers/thermal/broadcom/Kconfig
index ab08af4654ef..42c098e86f84 100644
--- a/drivers/thermal/broadcom/Kconfig
+++ b/drivers/thermal/broadcom/Kconfig
@@ -9,8 +9,9 @@ config BCM2835_THERMAL
 config BCM_NS_THERMAL
 	tristate "Northstar thermal driver"
 	depends on ARCH_BCM_IPROC || COMPILE_TEST
+	default y if ARCH_BCM_IPROC
 	help
-	  Northstar is a family of SoCs that includes e.g. BCM4708, BCM47081,
-	  BCM4709 and BCM47094. It contains DMU (Device Management Unit) block
-	  with a thermal sensor that allows checking CPU temperature. This
-	  driver provides support for it.
+	  Support for the Northstar and Northstar Plus family of SoCs (e.g.
+	  BCM4708, BCM4709, BCM5301x, BCM95852X, etc). It contains DMU (Device
+	  Management Unit) block with a thermal sensor that allows checking CPU
+	  temperature.
diff --git a/drivers/thermal/cpu_cooling.c b/drivers/thermal/cpu_cooling.c
index 69d0f430b2d1..908a8014cf76 100644
--- a/drivers/thermal/cpu_cooling.c
+++ b/drivers/thermal/cpu_cooling.c
@@ -49,40 +49,45 @@
  */
 
 /**
- * struct power_table - frequency to power conversion
+ * struct freq_table - frequency table along with power entries
  * @frequency:	frequency in KHz
  * @power:	power in mW
  *
  * This structure is built when the cooling device registers and helps
- * in translating frequency to power and viceversa.
+ * in translating frequency to power and vice versa.
  */
-struct power_table {
+struct freq_table {
 	u32 frequency;
 	u32 power;
 };
 
 /**
+ * struct time_in_idle - Idle time stats
+ * @time: previous reading of the absolute time that this cpu was idle
+ * @timestamp: wall time of the last invocation of get_cpu_idle_time_us()
+ */
+struct time_in_idle {
+	u64 time;
+	u64 timestamp;
+};
+
+/**
  * struct cpufreq_cooling_device - data for cooling device with cpufreq
  * @id: unique integer value corresponding to each cpufreq_cooling_device
  *	registered.
- * @cool_dev: thermal_cooling_device pointer to keep track of the
- *	registered cooling device.
+ * @last_load: load measured by the latest call to cpufreq_get_requested_power()
  * @cpufreq_state: integer value representing the current state of cpufreq
  *	cooling	devices.
  * @clipped_freq: integer value representing the absolute value of the clipped
  *	frequency.
  * @max_level: maximum cooling level. One less than total number of valid
  *	cpufreq frequencies.
- * @allowed_cpus: all the cpus involved for this cpufreq_cooling_device.
+ * @freq_table: Freq table in descending order of frequencies
+ * @cdev: thermal_cooling_device pointer to keep track of the
+ *	registered cooling device.
+ * @policy: cpufreq policy.
  * @node: list_head to link all cpufreq_cooling_device together.
- * @last_load: load measured by the latest call to cpufreq_get_requested_power()
- * @time_in_idle: previous reading of the absolute time that this cpu was idle
- * @time_in_idle_timestamp: wall time of the last invocation of
- *	get_cpu_idle_time_us()
- * @dyn_power_table: array of struct power_table for frequency to power
- *	conversion, sorted in ascending order.
- * @dyn_power_table_entries: number of entries in the @dyn_power_table array
- * @cpu_dev: the first cpu_device from @allowed_cpus that has OPPs registered
+ * @idle_time: idle time stats
  * @plat_get_static_power: callback to calculate the static power
  *
  * This structure is required for keeping information of each registered
@@ -90,81 +95,45 @@ struct power_table {
  */
 struct cpufreq_cooling_device {
 	int id;
-	struct thermal_cooling_device *cool_dev;
+	u32 last_load;
 	unsigned int cpufreq_state;
 	unsigned int clipped_freq;
 	unsigned int max_level;
-	unsigned int *freq_table;	/* In descending order */
-	struct cpumask allowed_cpus;
+	struct freq_table *freq_table;	/* In descending order */
+	struct thermal_cooling_device *cdev;
+	struct cpufreq_policy *policy;
 	struct list_head node;
-	u32 last_load;
-	u64 *time_in_idle;
-	u64 *time_in_idle_timestamp;
-	struct power_table *dyn_power_table;
-	int dyn_power_table_entries;
-	struct device *cpu_dev;
+	struct time_in_idle *idle_time;
 	get_static_t plat_get_static_power;
 };
-static DEFINE_IDA(cpufreq_ida);
 
+static DEFINE_IDA(cpufreq_ida);
 static DEFINE_MUTEX(cooling_list_lock);
-static LIST_HEAD(cpufreq_dev_list);
+static LIST_HEAD(cpufreq_cdev_list);
 
 /* Below code defines functions to be used for cpufreq as cooling device */
 
 /**
  * get_level: Find the level for a particular frequency
- * @cpufreq_dev: cpufreq_dev for which the property is required
+ * @cpufreq_cdev: cpufreq_cdev for which the property is required
  * @freq: Frequency
  *
- * Return: level on success, THERMAL_CSTATE_INVALID on error.
+ * Return: level corresponding to the frequency.
  */
-static unsigned long get_level(struct cpufreq_cooling_device *cpufreq_dev,
+static unsigned long get_level(struct cpufreq_cooling_device *cpufreq_cdev,
 			       unsigned int freq)
 {
+	struct freq_table *freq_table = cpufreq_cdev->freq_table;
 	unsigned long level;
 
-	for (level = 0; level <= cpufreq_dev->max_level; level++) {
-		if (freq == cpufreq_dev->freq_table[level])
-			return level;
-
-		if (freq > cpufreq_dev->freq_table[level])
+	for (level = 1; level <= cpufreq_cdev->max_level; level++)
+		if (freq > freq_table[level].frequency)
 			break;
-	}
 
-	return THERMAL_CSTATE_INVALID;
+	return level - 1;
 }
 
 /**
- * cpufreq_cooling_get_level - for a given cpu, return the cooling level.
- * @cpu: cpu for which the level is required
- * @freq: the frequency of interest
- *
- * This function will match the cooling level corresponding to the
- * requested @freq and return it.
- *
- * Return: The matched cooling level on success or THERMAL_CSTATE_INVALID
- * otherwise.
- */
-unsigned long cpufreq_cooling_get_level(unsigned int cpu, unsigned int freq)
-{
-	struct cpufreq_cooling_device *cpufreq_dev;
-
-	mutex_lock(&cooling_list_lock);
-	list_for_each_entry(cpufreq_dev, &cpufreq_dev_list, node) {
-		if (cpumask_test_cpu(cpu, &cpufreq_dev->allowed_cpus)) {
-			mutex_unlock(&cooling_list_lock);
-			return get_level(cpufreq_dev, freq);
-		}
-	}
-	mutex_unlock(&cooling_list_lock);
-
-	pr_err("%s: cpu:%d not part of any cooling device\n", __func__, cpu);
-	return THERMAL_CSTATE_INVALID;
-}
-EXPORT_SYMBOL_GPL(cpufreq_cooling_get_level);
-
-/**
  * cpufreq_thermal_notifier - notifier callback for cpufreq policy change.
  * @nb:	struct notifier_block * with callback info.
  * @event: value showing cpufreq event for which this function invoked.
@@ -181,14 +150,18 @@ static int cpufreq_thermal_notifier(struct notifier_block *nb,
 {
 	struct cpufreq_policy *policy = data;
 	unsigned long clipped_freq;
-	struct cpufreq_cooling_device *cpufreq_dev;
+	struct cpufreq_cooling_device *cpufreq_cdev;
 
 	if (event != CPUFREQ_ADJUST)
 		return NOTIFY_DONE;
 
 	mutex_lock(&cooling_list_lock);
-	list_for_each_entry(cpufreq_dev, &cpufreq_dev_list, node) {
-		if (!cpumask_test_cpu(policy->cpu, &cpufreq_dev->allowed_cpus))
+	list_for_each_entry(cpufreq_cdev, &cpufreq_cdev_list, node) {
+		/*
+		 * A new copy of the policy is sent to the notifier and can't
+		 * compare that directly.
+		 */
+		if (policy->cpu != cpufreq_cdev->policy->cpu)
 			continue;
 
 		/*
@@ -202,7 +175,7 @@ static int cpufreq_thermal_notifier(struct notifier_block *nb,
 		 * But, if clipped_freq is greater than policy->max, we don't
 		 * need to do anything.
 		 */
-		clipped_freq = cpufreq_dev->clipped_freq;
+		clipped_freq = cpufreq_cdev->clipped_freq;
 
 		if (policy->max > clipped_freq)
 			cpufreq_verify_within_limits(policy, 0, clipped_freq);
@@ -214,63 +187,63 @@ static int cpufreq_thermal_notifier(struct notifier_block *nb,
 }
 
 /**
- * build_dyn_power_table() - create a dynamic power to frequency table
- * @cpufreq_device:	the cpufreq cooling device in which to store the table
+ * update_freq_table() - Update the freq table with power numbers
+ * @cpufreq_cdev:	the cpufreq cooling device in which to update the table
  * @capacitance: dynamic power coefficient for these cpus
  *
- * Build a dynamic power to frequency table for this cpu and store it
- * in @cpufreq_device.  This table will be used in cpu_power_to_freq() and
- * cpu_freq_to_power() to convert between power and frequency
- * efficiently.  Power is stored in mW, frequency in KHz.  The
- * resulting table is in ascending order.
+ * Update the freq table with power numbers.  This table will be used in
+ * cpu_power_to_freq() and cpu_freq_to_power() to convert between power and
+ * frequency efficiently.  Power is stored in mW, frequency in KHz.  The
+ * resulting table is in descending order.
  *
  * Return: 0 on success, -EINVAL if there are no OPPs for any CPUs,
- * -ENOMEM if we run out of memory or -EAGAIN if an OPP was
- * added/enabled while the function was executing.
+ * or -ENOMEM if we run out of memory.
  */
-static int build_dyn_power_table(struct cpufreq_cooling_device *cpufreq_device,
-				 u32 capacitance)
+static int update_freq_table(struct cpufreq_cooling_device *cpufreq_cdev,
+			     u32 capacitance)
 {
-	struct power_table *power_table;
+	struct freq_table *freq_table = cpufreq_cdev->freq_table;
 	struct dev_pm_opp *opp;
 	struct device *dev = NULL;
-	int num_opps = 0, cpu, i, ret = 0;
-	unsigned long freq;
-
-	for_each_cpu(cpu, &cpufreq_device->allowed_cpus) {
-		dev = get_cpu_device(cpu);
-		if (!dev) {
-			dev_warn(&cpufreq_device->cool_dev->device,
-				 "No cpu device for cpu %d\n", cpu);
-			continue;
-		}
+	int num_opps = 0, cpu = cpufreq_cdev->policy->cpu, i;
 
-		num_opps = dev_pm_opp_get_opp_count(dev);
-		if (num_opps > 0)
-			break;
-		else if (num_opps < 0)
-			return num_opps;
+	dev = get_cpu_device(cpu);
+	if (unlikely(!dev)) {
+		dev_warn(&cpufreq_cdev->cdev->device,
+			 "No cpu device for cpu %d\n", cpu);
+		return -ENODEV;
 	}
 
-	if (num_opps == 0)
-		return -EINVAL;
+	num_opps = dev_pm_opp_get_opp_count(dev);
+	if (num_opps < 0)
+		return num_opps;
 
-	power_table = kcalloc(num_opps, sizeof(*power_table), GFP_KERNEL);
-	if (!power_table)
-		return -ENOMEM;
+	/*
+	 * The cpufreq table is also built from the OPP table and so the count
+	 * should match.
+	 */
+	if (num_opps != cpufreq_cdev->max_level + 1) {
+		dev_warn(dev, "Number of OPPs not matching with max_levels\n");
+		return -EINVAL;
+	}
 
-	for (freq = 0, i = 0;
-	     opp = dev_pm_opp_find_freq_ceil(dev, &freq), !IS_ERR(opp);
-	     freq++, i++) {
-		u32 freq_mhz, voltage_mv;
+	for (i = 0; i <= cpufreq_cdev->max_level; i++) {
+		unsigned long freq = freq_table[i].frequency * 1000;
+		u32 freq_mhz = freq_table[i].frequency / 1000;
 		u64 power;
+		u32 voltage_mv;
 
-		if (i >= num_opps) {
-			ret = -EAGAIN;
-			goto free_power_table;
+		/*
+		 * Find ceil frequency as 'freq' may be slightly lower than OPP
+		 * freq due to truncation while converting to kHz.
+		 */
+		opp = dev_pm_opp_find_freq_ceil(dev, &freq);
+		if (IS_ERR(opp)) {
+			dev_err(dev, "failed to get opp for %lu frequency\n",
+				freq);
+			return -EINVAL;
 		}
 
-		freq_mhz = freq / 1000000;
 		voltage_mv = dev_pm_opp_get_voltage(opp) / 1000;
 		dev_pm_opp_put(opp);
 
@@ -281,89 +254,73 @@ static int build_dyn_power_table(struct cpufreq_cooling_device *cpufreq_device,
 		power = (u64)capacitance * freq_mhz * voltage_mv * voltage_mv;
 		do_div(power, 1000000000);
 
-		/* frequency is stored in power_table in KHz */
-		power_table[i].frequency = freq / 1000;
-
 		/* power is stored in mW */
-		power_table[i].power = power;
+		freq_table[i].power = power;
 	}
 
-	if (i != num_opps) {
-		ret = PTR_ERR(opp);
-		goto free_power_table;
-	}
-
-	cpufreq_device->cpu_dev = dev;
-	cpufreq_device->dyn_power_table = power_table;
-	cpufreq_device->dyn_power_table_entries = i;
-
 	return 0;
-
-free_power_table:
-	kfree(power_table);
-
-	return ret;
 }
 
-static u32 cpu_freq_to_power(struct cpufreq_cooling_device *cpufreq_device,
+static u32 cpu_freq_to_power(struct cpufreq_cooling_device *cpufreq_cdev,
 			     u32 freq)
 {
 	int i;
-	struct power_table *pt = cpufreq_device->dyn_power_table;
+	struct freq_table *freq_table = cpufreq_cdev->freq_table;
 
-	for (i = 1; i < cpufreq_device->dyn_power_table_entries; i++)
-		if (freq < pt[i].frequency)
+	for (i = 1; i <= cpufreq_cdev->max_level; i++)
+		if (freq > freq_table[i].frequency)
 			break;
 
-	return pt[i - 1].power;
+	return freq_table[i - 1].power;
 }
 
-static u32 cpu_power_to_freq(struct cpufreq_cooling_device *cpufreq_device,
+static u32 cpu_power_to_freq(struct cpufreq_cooling_device *cpufreq_cdev,
 			     u32 power)
 {
 	int i;
-	struct power_table *pt = cpufreq_device->dyn_power_table;
+	struct freq_table *freq_table = cpufreq_cdev->freq_table;
 
-	for (i = 1; i < cpufreq_device->dyn_power_table_entries; i++)
-		if (power < pt[i].power)
+	for (i = 1; i <= cpufreq_cdev->max_level; i++)
+		if (power > freq_table[i].power)
 			break;
 
-	return pt[i - 1].frequency;
+	return freq_table[i - 1].frequency;
 }
 
 /**
  * get_load() - get load for a cpu since last updated
- * @cpufreq_device:	&struct cpufreq_cooling_device for this cpu
+ * @cpufreq_cdev:	&struct cpufreq_cooling_device for this cpu
  * @cpu:	cpu number
- * @cpu_idx:	index of the cpu in cpufreq_device->allowed_cpus
+ * @cpu_idx:	index of the cpu in time_in_idle*
  *
  * Return: The average load of cpu @cpu in percentage since this
  * function was last called.
  */
-static u32 get_load(struct cpufreq_cooling_device *cpufreq_device, int cpu,
+static u32 get_load(struct cpufreq_cooling_device *cpufreq_cdev, int cpu,
 		    int cpu_idx)
 {
 	u32 load;
 	u64 now, now_idle, delta_time, delta_idle;
+	struct time_in_idle *idle_time = &cpufreq_cdev->idle_time[cpu_idx];
 
 	now_idle = get_cpu_idle_time(cpu, &now, 0);
-	delta_idle = now_idle - cpufreq_device->time_in_idle[cpu_idx];
-	delta_time = now - cpufreq_device->time_in_idle_timestamp[cpu_idx];
+	delta_idle = now_idle - idle_time->time;
+	delta_time = now - idle_time->timestamp;
 
 	if (delta_time <= delta_idle)
 		load = 0;
 	else
 		load = div64_u64(100 * (delta_time - delta_idle), delta_time);
 
-	cpufreq_device->time_in_idle[cpu_idx] = now_idle;
-	cpufreq_device->time_in_idle_timestamp[cpu_idx] = now;
+	idle_time->time = now_idle;
+	idle_time->timestamp = now;
 
 	return load;
 }
 
 /**
  * get_static_power() - calculate the static power consumed by the cpus
- * @cpufreq_device:	struct &cpufreq_cooling_device for this cpu cdev
+ * @cpufreq_cdev:	struct &cpufreq_cooling_device for this cpu cdev
  * @tz:		thermal zone device in which we're operating
  * @freq:	frequency in KHz
  * @power:	pointer in which to store the calculated static power
@@ -376,26 +333,28 @@ static u32 get_load(struct cpufreq_cooling_device *cpufreq_device, int cpu,
  *
  * Return: 0 on success, -E* on failure.
  */
-static int get_static_power(struct cpufreq_cooling_device *cpufreq_device,
+static int get_static_power(struct cpufreq_cooling_device *cpufreq_cdev,
 			    struct thermal_zone_device *tz, unsigned long freq,
 			    u32 *power)
 {
 	struct dev_pm_opp *opp;
 	unsigned long voltage;
-	struct cpumask *cpumask = &cpufreq_device->allowed_cpus;
+	struct cpufreq_policy *policy = cpufreq_cdev->policy;
+	struct cpumask *cpumask = policy->related_cpus;
 	unsigned long freq_hz = freq * 1000;
+	struct device *dev;
 
-	if (!cpufreq_device->plat_get_static_power ||
-	    !cpufreq_device->cpu_dev) {
+	if (!cpufreq_cdev->plat_get_static_power) {
 		*power = 0;
 		return 0;
 	}
 
-	opp = dev_pm_opp_find_freq_exact(cpufreq_device->cpu_dev, freq_hz,
-					 true);
+	dev = get_cpu_device(policy->cpu);
+	WARN_ON(!dev);
+
+	opp = dev_pm_opp_find_freq_exact(dev, freq_hz, true);
 	if (IS_ERR(opp)) {
-		dev_warn_ratelimited(cpufreq_device->cpu_dev,
-				     "Failed to find OPP for frequency %lu: %ld\n",
+		dev_warn_ratelimited(dev, "Failed to find OPP for frequency %lu: %ld\n",
 				     freq_hz, PTR_ERR(opp));
 		return -EINVAL;
 	}
@@ -404,31 +363,30 @@ static int get_static_power(struct cpufreq_cooling_device *cpufreq_device,
 	dev_pm_opp_put(opp);
 
 	if (voltage == 0) {
-		dev_err_ratelimited(cpufreq_device->cpu_dev,
-				    "Failed to get voltage for frequency %lu\n",
+		dev_err_ratelimited(dev, "Failed to get voltage for frequency %lu\n",
 				    freq_hz);
 		return -EINVAL;
 	}
 
-	return cpufreq_device->plat_get_static_power(cpumask, tz->passive_delay,
-						     voltage, power);
+	return cpufreq_cdev->plat_get_static_power(cpumask, tz->passive_delay,
+						  voltage, power);
 }
 
 /**
  * get_dynamic_power() - calculate the dynamic power
- * @cpufreq_device:	&cpufreq_cooling_device for this cdev
+ * @cpufreq_cdev:	&cpufreq_cooling_device for this cdev
  * @freq:	current frequency
  *
  * Return: the dynamic power consumed by the cpus described by
- * @cpufreq_device.
+ * @cpufreq_cdev.
  */
-static u32 get_dynamic_power(struct cpufreq_cooling_device *cpufreq_device,
+static u32 get_dynamic_power(struct cpufreq_cooling_device *cpufreq_cdev,
 			     unsigned long freq)
 {
 	u32 raw_cpu_power;
 
-	raw_cpu_power = cpu_freq_to_power(cpufreq_device, freq);
-	return (raw_cpu_power * cpufreq_device->last_load) / 100;
+	raw_cpu_power = cpu_freq_to_power(cpufreq_cdev, freq);
+	return (raw_cpu_power * cpufreq_cdev->last_load) / 100;
 }
 
 /* cpufreq cooling device callback functions are defined below */
@@ -446,9 +404,9 @@ static u32 get_dynamic_power(struct cpufreq_cooling_device *cpufreq_device,
 static int cpufreq_get_max_state(struct thermal_cooling_device *cdev,
 				 unsigned long *state)
 {
-	struct cpufreq_cooling_device *cpufreq_device = cdev->devdata;
+	struct cpufreq_cooling_device *cpufreq_cdev = cdev->devdata;
 
-	*state = cpufreq_device->max_level;
+	*state = cpufreq_cdev->max_level;
 	return 0;
 }
 
@@ -465,9 +423,9 @@ static int cpufreq_get_max_state(struct thermal_cooling_device *cdev,
 static int cpufreq_get_cur_state(struct thermal_cooling_device *cdev,
 				 unsigned long *state)
 {
-	struct cpufreq_cooling_device *cpufreq_device = cdev->devdata;
+	struct cpufreq_cooling_device *cpufreq_cdev = cdev->devdata;
 
-	*state = cpufreq_device->cpufreq_state;
+	*state = cpufreq_cdev->cpufreq_state;
 
 	return 0;
 }
@@ -485,23 +443,22 @@ static int cpufreq_get_cur_state(struct thermal_cooling_device *cdev,
 static int cpufreq_set_cur_state(struct thermal_cooling_device *cdev,
 				 unsigned long state)
 {
-	struct cpufreq_cooling_device *cpufreq_device = cdev->devdata;
-	unsigned int cpu = cpumask_any(&cpufreq_device->allowed_cpus);
+	struct cpufreq_cooling_device *cpufreq_cdev = cdev->devdata;
 	unsigned int clip_freq;
 
 	/* Request state should be less than max_level */
-	if (WARN_ON(state > cpufreq_device->max_level))
+	if (WARN_ON(state > cpufreq_cdev->max_level))
 		return -EINVAL;
 
 	/* Check if the old cooling action is same as new cooling action */
-	if (cpufreq_device->cpufreq_state == state)
+	if (cpufreq_cdev->cpufreq_state == state)
 		return 0;
 
-	clip_freq = cpufreq_device->freq_table[state];
-	cpufreq_device->cpufreq_state = state;
-	cpufreq_device->clipped_freq = clip_freq;
+	clip_freq = cpufreq_cdev->freq_table[state].frequency;
+	cpufreq_cdev->cpufreq_state = state;
+	cpufreq_cdev->clipped_freq = clip_freq;
 
-	cpufreq_update_policy(cpu);
+	cpufreq_update_policy(cpufreq_cdev->policy->cpu);
 
 	return 0;
 }
@@ -536,33 +493,23 @@ static int cpufreq_get_requested_power(struct thermal_cooling_device *cdev,
 	unsigned long freq;
 	int i = 0, cpu, ret;
 	u32 static_power, dynamic_power, total_load = 0;
-	struct cpufreq_cooling_device *cpufreq_device = cdev->devdata;
+	struct cpufreq_cooling_device *cpufreq_cdev = cdev->devdata;
+	struct cpufreq_policy *policy = cpufreq_cdev->policy;
 	u32 *load_cpu = NULL;
 
-	cpu = cpumask_any_and(&cpufreq_device->allowed_cpus, cpu_online_mask);
-
-	/*
-	 * All the CPUs are offline, thus the requested power by
-	 * the cdev is 0
-	 */
-	if (cpu >= nr_cpu_ids) {
-		*power = 0;
-		return 0;
-	}
-
-	freq = cpufreq_quick_get(cpu);
+	freq = cpufreq_quick_get(policy->cpu);
 
 	if (trace_thermal_power_cpu_get_power_enabled()) {
-		u32 ncpus = cpumask_weight(&cpufreq_device->allowed_cpus);
+		u32 ncpus = cpumask_weight(policy->related_cpus);
 
 		load_cpu = kcalloc(ncpus, sizeof(*load_cpu), GFP_KERNEL);
 	}
 
-	for_each_cpu(cpu, &cpufreq_device->allowed_cpus) {
+	for_each_cpu(cpu, policy->related_cpus) {
 		u32 load;
 
 		if (cpu_online(cpu))
-			load = get_load(cpufreq_device, cpu, i);
+			load = get_load(cpufreq_cdev, cpu, i);
 		else
 			load = 0;
 
@@ -573,19 +520,19 @@ static int cpufreq_get_requested_power(struct thermal_cooling_device *cdev,
 		i++;
 	}
 
-	cpufreq_device->last_load = total_load;
+	cpufreq_cdev->last_load = total_load;
 
-	dynamic_power = get_dynamic_power(cpufreq_device, freq);
-	ret = get_static_power(cpufreq_device, tz, freq, &static_power);
+	dynamic_power = get_dynamic_power(cpufreq_cdev, freq);
+	ret = get_static_power(cpufreq_cdev, tz, freq, &static_power);
 	if (ret) {
 		kfree(load_cpu);
 		return ret;
 	}
 
 	if (load_cpu) {
-		trace_thermal_power_cpu_get_power(
-			&cpufreq_device->allowed_cpus,
-			freq, load_cpu, i, dynamic_power, static_power);
+		trace_thermal_power_cpu_get_power(policy->related_cpus, freq,
+						  load_cpu, i, dynamic_power,
+						  static_power);
 
 		kfree(load_cpu);
 	}
@@ -614,38 +561,23 @@ static int cpufreq_state2power(struct thermal_cooling_device *cdev,
 			       unsigned long state, u32 *power)
 {
 	unsigned int freq, num_cpus;
-	cpumask_var_t cpumask;
 	u32 static_power, dynamic_power;
 	int ret;
-	struct cpufreq_cooling_device *cpufreq_device = cdev->devdata;
-
-	if (!alloc_cpumask_var(&cpumask, GFP_KERNEL))
-		return -ENOMEM;
-
-	cpumask_and(cpumask, &cpufreq_device->allowed_cpus, cpu_online_mask);
-	num_cpus = cpumask_weight(cpumask);
+	struct cpufreq_cooling_device *cpufreq_cdev = cdev->devdata;
 
-	/* None of our cpus are online, so no power */
-	if (num_cpus == 0) {
-		*power = 0;
-		ret = 0;
-		goto out;
-	}
+	/* Request state should be less than max_level */
+	if (WARN_ON(state > cpufreq_cdev->max_level))
+		return -EINVAL;
 
-	freq = cpufreq_device->freq_table[state];
-	if (!freq) {
-		ret = -EINVAL;
-		goto out;
-	}
+	num_cpus = cpumask_weight(cpufreq_cdev->policy->cpus);
 
-	dynamic_power = cpu_freq_to_power(cpufreq_device, freq) * num_cpus;
-	ret = get_static_power(cpufreq_device, tz, freq, &static_power);
+	freq = cpufreq_cdev->freq_table[state].frequency;
+	dynamic_power = cpu_freq_to_power(cpufreq_cdev, freq) * num_cpus;
+	ret = get_static_power(cpufreq_cdev, tz, freq, &static_power);
 	if (ret)
-		goto out;
+		return ret;
 
 	*power = static_power + dynamic_power;
-out:
-	free_cpumask_var(cpumask);
 	return ret;
 }
 
@@ -673,39 +605,27 @@ static int cpufreq_power2state(struct thermal_cooling_device *cdev,
 			       struct thermal_zone_device *tz, u32 power,
 			       unsigned long *state)
 {
-	unsigned int cpu, cur_freq, target_freq;
+	unsigned int cur_freq, target_freq;
 	int ret;
 	s32 dyn_power;
 	u32 last_load, normalised_power, static_power;
-	struct cpufreq_cooling_device *cpufreq_device = cdev->devdata;
+	struct cpufreq_cooling_device *cpufreq_cdev = cdev->devdata;
+	struct cpufreq_policy *policy = cpufreq_cdev->policy;
 
-	cpu = cpumask_any_and(&cpufreq_device->allowed_cpus, cpu_online_mask);
-
-	/* None of our cpus are online */
-	if (cpu >= nr_cpu_ids)
-		return -ENODEV;
-
-	cur_freq = cpufreq_quick_get(cpu);
-	ret = get_static_power(cpufreq_device, tz, cur_freq, &static_power);
+	cur_freq = cpufreq_quick_get(policy->cpu);
+	ret = get_static_power(cpufreq_cdev, tz, cur_freq, &static_power);
 	if (ret)
 		return ret;
 
 	dyn_power = power - static_power;
 	dyn_power = dyn_power > 0 ? dyn_power : 0;
-	last_load = cpufreq_device->last_load ?: 1;
+	last_load = cpufreq_cdev->last_load ?: 1;
 	normalised_power = (dyn_power * 100) / last_load;
-	target_freq = cpu_power_to_freq(cpufreq_device, normalised_power);
+	target_freq = cpu_power_to_freq(cpufreq_cdev, normalised_power);
 
-	*state = cpufreq_cooling_get_level(cpu, target_freq);
-	if (*state == THERMAL_CSTATE_INVALID) {
-		dev_err_ratelimited(&cdev->device,
-				    "Failed to convert %dKHz for cpu %d into a cdev state\n",
-				    target_freq, cpu);
-		return -EINVAL;
-	}
-
-	trace_thermal_power_cpu_limit(&cpufreq_device->allowed_cpus,
-				      target_freq, *state, power);
+	*state = get_level(cpufreq_cdev, target_freq);
+	trace_thermal_power_cpu_limit(policy->related_cpus, target_freq, *state,
+				      power);
 	return 0;
 }
 
@@ -748,7 +668,7 @@ static unsigned int find_next_max(struct cpufreq_frequency_table *table,
 /**
  * __cpufreq_cooling_register - helper function to create cpufreq cooling device
  * @np: a valid struct device_node to the cooling device device tree node
- * @clip_cpus: cpumask of cpus where the frequency constraints will happen.
+ * @policy: cpufreq policy
  * Normally this should be same as cpufreq policy->related_cpus.
  * @capacitance: dynamic power coefficient for these cpus
  * @plat_static_func: function to calculate the static power consumed by these
@@ -764,102 +684,68 @@ static unsigned int find_next_max(struct cpufreq_frequency_table *table,
  */
 static struct thermal_cooling_device *
 __cpufreq_cooling_register(struct device_node *np,
-			const struct cpumask *clip_cpus, u32 capacitance,
+			struct cpufreq_policy *policy, u32 capacitance,
 			get_static_t plat_static_func)
 {
-	struct cpufreq_policy *policy;
-	struct thermal_cooling_device *cool_dev;
-	struct cpufreq_cooling_device *cpufreq_dev;
+	struct thermal_cooling_device *cdev;
+	struct cpufreq_cooling_device *cpufreq_cdev;
 	char dev_name[THERMAL_NAME_LENGTH];
-	struct cpufreq_frequency_table *pos, *table;
-	cpumask_var_t temp_mask;
 	unsigned int freq, i, num_cpus;
 	int ret;
 	struct thermal_cooling_device_ops *cooling_ops;
 	bool first;
 
-	if (!alloc_cpumask_var(&temp_mask, GFP_KERNEL))
-		return ERR_PTR(-ENOMEM);
-
-	cpumask_and(temp_mask, clip_cpus, cpu_online_mask);
-	policy = cpufreq_cpu_get(cpumask_first(temp_mask));
-	if (!policy) {
-		pr_debug("%s: CPUFreq policy not found\n", __func__);
-		cool_dev = ERR_PTR(-EPROBE_DEFER);
-		goto free_cpumask;
+	if (IS_ERR_OR_NULL(policy)) {
+		pr_err("%s: cpufreq policy isn't valid: %p", __func__, policy);
+		return ERR_PTR(-EINVAL);
 	}
 
-	table = policy->freq_table;
-	if (!table) {
-		pr_debug("%s: CPUFreq table not found\n", __func__);
-		cool_dev = ERR_PTR(-ENODEV);
-		goto put_policy;
+	i = cpufreq_table_count_valid_entries(policy);
+	if (!i) {
+		pr_debug("%s: CPUFreq table not found or has no valid entries\n",
+			 __func__);
+		return ERR_PTR(-ENODEV);
 	}
 
-	cpufreq_dev = kzalloc(sizeof(*cpufreq_dev), GFP_KERNEL);
-	if (!cpufreq_dev) {
-		cool_dev = ERR_PTR(-ENOMEM);
-		goto put_policy;
-	}
+	cpufreq_cdev = kzalloc(sizeof(*cpufreq_cdev), GFP_KERNEL);
+	if (!cpufreq_cdev)
+		return ERR_PTR(-ENOMEM);
 
-	num_cpus = cpumask_weight(clip_cpus);
-	cpufreq_dev->time_in_idle = kcalloc(num_cpus,
-					    sizeof(*cpufreq_dev->time_in_idle),
-					    GFP_KERNEL);
-	if (!cpufreq_dev->time_in_idle) {
-		cool_dev = ERR_PTR(-ENOMEM);
+	cpufreq_cdev->policy = policy;
+	num_cpus = cpumask_weight(policy->related_cpus);
+	cpufreq_cdev->idle_time = kcalloc(num_cpus,
+					 sizeof(*cpufreq_cdev->idle_time),
+					 GFP_KERNEL);
+	if (!cpufreq_cdev->idle_time) {
+		cdev = ERR_PTR(-ENOMEM);
 		goto free_cdev;
 	}
 
-	cpufreq_dev->time_in_idle_timestamp =
-		kcalloc(num_cpus, sizeof(*cpufreq_dev->time_in_idle_timestamp),
-			GFP_KERNEL);
-	if (!cpufreq_dev->time_in_idle_timestamp) {
-		cool_dev = ERR_PTR(-ENOMEM);
-		goto free_time_in_idle;
-	}
-
-	/* Find max levels */
-	cpufreq_for_each_valid_entry(pos, table)
-		cpufreq_dev->max_level++;
-
-	cpufreq_dev->freq_table = kmalloc(sizeof(*cpufreq_dev->freq_table) *
-					  cpufreq_dev->max_level, GFP_KERNEL);
-	if (!cpufreq_dev->freq_table) {
-		cool_dev = ERR_PTR(-ENOMEM);
-		goto free_time_in_idle_timestamp;
-	}
-
 	/* max_level is an index, not a counter */
-	cpufreq_dev->max_level--;
-
-	cpumask_copy(&cpufreq_dev->allowed_cpus, clip_cpus);
-
-	if (capacitance) {
-		cpufreq_dev->plat_get_static_power = plat_static_func;
-
-		ret = build_dyn_power_table(cpufreq_dev, capacitance);
-		if (ret) {
-			cool_dev = ERR_PTR(ret);
-			goto free_table;
-		}
-
-		cooling_ops = &cpufreq_power_cooling_ops;
-	} else {
-		cooling_ops = &cpufreq_cooling_ops;
+	cpufreq_cdev->max_level = i - 1;
+
+	cpufreq_cdev->freq_table = kmalloc_array(i,
+					sizeof(*cpufreq_cdev->freq_table),
+					GFP_KERNEL);
+	if (!cpufreq_cdev->freq_table) {
+		cdev = ERR_PTR(-ENOMEM);
+		goto free_idle_time;
 	}
 
 	ret = ida_simple_get(&cpufreq_ida, 0, 0, GFP_KERNEL);
 	if (ret < 0) {
-		cool_dev = ERR_PTR(ret);
-		goto free_power_table;
+		cdev = ERR_PTR(ret);
+		goto free_table;
 	}
-	cpufreq_dev->id = ret;
+	cpufreq_cdev->id = ret;
+
+	snprintf(dev_name, sizeof(dev_name), "thermal-cpufreq-%d",
+		 cpufreq_cdev->id);
 
 	/* Fill freq-table in descending order of frequencies */
-	for (i = 0, freq = -1; i <= cpufreq_dev->max_level; i++) {
-		freq = find_next_max(table, freq);
-		cpufreq_dev->freq_table[i] = freq;
+	for (i = 0, freq = -1; i <= cpufreq_cdev->max_level; i++) {
+		freq = find_next_max(policy->freq_table, freq);
+		cpufreq_cdev->freq_table[i].frequency = freq;
 
 		/* Warn for duplicate entries */
 		if (!freq)
@@ -868,51 +754,54 @@ __cpufreq_cooling_register(struct device_node *np,
 			pr_debug("%s: freq:%u KHz\n", __func__, freq);
 	}
 
-	snprintf(dev_name, sizeof(dev_name), "thermal-cpufreq-%d",
-		 cpufreq_dev->id);
+	if (capacitance) {
+		cpufreq_cdev->plat_get_static_power = plat_static_func;
+
+		ret = update_freq_table(cpufreq_cdev, capacitance);
+		if (ret) {
+			cdev = ERR_PTR(ret);
+			goto remove_ida;
+		}
+
+		cooling_ops = &cpufreq_power_cooling_ops;
+	} else {
+		cooling_ops = &cpufreq_cooling_ops;
+	}
 
-	cool_dev = thermal_of_cooling_device_register(np, dev_name, cpufreq_dev,
-						      cooling_ops);
-	if (IS_ERR(cool_dev))
+	cdev = thermal_of_cooling_device_register(np, dev_name, cpufreq_cdev,
+						  cooling_ops);
+	if (IS_ERR(cdev))
 		goto remove_ida;
 
-	cpufreq_dev->clipped_freq = cpufreq_dev->freq_table[0];
-	cpufreq_dev->cool_dev = cool_dev;
+	cpufreq_cdev->clipped_freq = cpufreq_cdev->freq_table[0].frequency;
+	cpufreq_cdev->cdev = cdev;
 
 	mutex_lock(&cooling_list_lock);
 	/* Register the notifier for first cpufreq cooling device */
-	first = list_empty(&cpufreq_dev_list);
-	list_add(&cpufreq_dev->node, &cpufreq_dev_list);
+	first = list_empty(&cpufreq_cdev_list);
+	list_add(&cpufreq_cdev->node, &cpufreq_cdev_list);
 	mutex_unlock(&cooling_list_lock);
 
 	if (first)
 		cpufreq_register_notifier(&thermal_cpufreq_notifier_block,
 					  CPUFREQ_POLICY_NOTIFIER);
 
-	goto put_policy;
+	return cdev;
 
 remove_ida:
-	ida_simple_remove(&cpufreq_ida, cpufreq_dev->id);
-free_power_table:
-	kfree(cpufreq_dev->dyn_power_table);
+	ida_simple_remove(&cpufreq_ida, cpufreq_cdev->id);
 free_table:
-	kfree(cpufreq_dev->freq_table);
-free_time_in_idle_timestamp:
-	kfree(cpufreq_dev->time_in_idle_timestamp);
-free_time_in_idle:
-	kfree(cpufreq_dev->time_in_idle);
+	kfree(cpufreq_cdev->freq_table);
+free_idle_time:
+	kfree(cpufreq_cdev->idle_time);
 free_cdev:
-	kfree(cpufreq_dev);
-put_policy:
-	cpufreq_cpu_put(policy);
-free_cpumask:
-	free_cpumask_var(temp_mask);
-	return cool_dev;
+	kfree(cpufreq_cdev);
+	return cdev;
 }
 
 /**
  * cpufreq_cooling_register - function to create cpufreq cooling device.
- * @clip_cpus: cpumask of cpus where the frequency constraints will happen.
+ * @policy: cpufreq policy
  *
  * This interface function registers the cpufreq cooling device with the name
  * "thermal-cpufreq-%x". This api can support multiple instances of cpufreq
@@ -922,16 +811,16 @@ free_cpumask:
  * on failure, it returns a corresponding ERR_PTR().
  */
 struct thermal_cooling_device *
-cpufreq_cooling_register(const struct cpumask *clip_cpus)
+cpufreq_cooling_register(struct cpufreq_policy *policy)
 {
-	return __cpufreq_cooling_register(NULL, clip_cpus, 0, NULL);
+	return __cpufreq_cooling_register(NULL, policy, 0, NULL);
 }
 EXPORT_SYMBOL_GPL(cpufreq_cooling_register);
 
 /**
  * of_cpufreq_cooling_register - function to create cpufreq cooling device.
  * @np: a valid struct device_node to the cooling device device tree node
- * @clip_cpus: cpumask of cpus where the frequency constraints will happen.
+ * @policy: cpufreq policy
  *
  * This interface function registers the cpufreq cooling device with the name
  * "thermal-cpufreq-%x". This api can support multiple instances of cpufreq
@@ -943,18 +832,18 @@ EXPORT_SYMBOL_GPL(cpufreq_cooling_register);
  */
 struct thermal_cooling_device *
 of_cpufreq_cooling_register(struct device_node *np,
-			    const struct cpumask *clip_cpus)
+			    struct cpufreq_policy *policy)
 {
 	if (!np)
 		return ERR_PTR(-EINVAL);
 
-	return __cpufreq_cooling_register(np, clip_cpus, 0, NULL);
+	return __cpufreq_cooling_register(np, policy, 0, NULL);
 }
 EXPORT_SYMBOL_GPL(of_cpufreq_cooling_register);
 
 /**
  * cpufreq_power_cooling_register() - create cpufreq cooling device with power extensions
- * @clip_cpus:	cpumask of cpus where the frequency constraints will happen
+ * @policy:		cpufreq policy
  * @capacitance:	dynamic power coefficient for these cpus
  * @plat_static_func:	function to calculate the static power consumed by these
  *			cpus (optional)
@@ -974,10 +863,10 @@ EXPORT_SYMBOL_GPL(of_cpufreq_cooling_register);
  * on failure, it returns a corresponding ERR_PTR().
  */
 struct thermal_cooling_device *
-cpufreq_power_cooling_register(const struct cpumask *clip_cpus, u32 capacitance,
+cpufreq_power_cooling_register(struct cpufreq_policy *policy, u32 capacitance,
 			       get_static_t plat_static_func)
 {
-	return __cpufreq_cooling_register(NULL, clip_cpus, capacitance,
+	return __cpufreq_cooling_register(NULL, policy, capacitance,
 				plat_static_func);
 }
 EXPORT_SYMBOL(cpufreq_power_cooling_register);
@@ -985,7 +874,7 @@ EXPORT_SYMBOL(cpufreq_power_cooling_register);
 /**
  * of_cpufreq_power_cooling_register() - create cpufreq cooling device with power extensions
  * @np:	a valid struct device_node to the cooling device device tree node
- * @clip_cpus:	cpumask of cpus where the frequency constraints will happen
+ * @policy: cpufreq policy
  * @capacitance:	dynamic power coefficient for these cpus
  * @plat_static_func:	function to calculate the static power consumed by these
  *			cpus (optional)
@@ -1007,14 +896,14 @@ EXPORT_SYMBOL(cpufreq_power_cooling_register);
  */
 struct thermal_cooling_device *
 of_cpufreq_power_cooling_register(struct device_node *np,
-				  const struct cpumask *clip_cpus,
+				  struct cpufreq_policy *policy,
 				  u32 capacitance,
 				  get_static_t plat_static_func)
 {
 	if (!np)
 		return ERR_PTR(-EINVAL);
 
-	return __cpufreq_cooling_register(np, clip_cpus, capacitance,
+	return __cpufreq_cooling_register(np, policy, capacitance,
 				plat_static_func);
 }
 EXPORT_SYMBOL(of_cpufreq_power_cooling_register);
@@ -1027,30 +916,28 @@ EXPORT_SYMBOL(of_cpufreq_power_cooling_register);
  */
 void cpufreq_cooling_unregister(struct thermal_cooling_device *cdev)
 {
-	struct cpufreq_cooling_device *cpufreq_dev;
+	struct cpufreq_cooling_device *cpufreq_cdev;
 	bool last;
 
 	if (!cdev)
 		return;
 
-	cpufreq_dev = cdev->devdata;
+	cpufreq_cdev = cdev->devdata;
 
 	mutex_lock(&cooling_list_lock);
-	list_del(&cpufreq_dev->node);
+	list_del(&cpufreq_cdev->node);
 	/* Unregister the notifier for the last cpufreq cooling device */
-	last = list_empty(&cpufreq_dev_list);
+	last = list_empty(&cpufreq_cdev_list);
 	mutex_unlock(&cooling_list_lock);
 
 	if (last)
 		cpufreq_unregister_notifier(&thermal_cpufreq_notifier_block,
 					    CPUFREQ_POLICY_NOTIFIER);
 
-	thermal_cooling_device_unregister(cpufreq_dev->cool_dev);
-	ida_simple_remove(&cpufreq_ida, cpufreq_dev->id);
-	kfree(cpufreq_dev->dyn_power_table);
-	kfree(cpufreq_dev->time_in_idle_timestamp);
-	kfree(cpufreq_dev->time_in_idle);
-	kfree(cpufreq_dev->freq_table);
-	kfree(cpufreq_dev);
+	thermal_cooling_device_unregister(cpufreq_cdev->cdev);
+	ida_simple_remove(&cpufreq_ida, cpufreq_cdev->id);
+	kfree(cpufreq_cdev->idle_time);
+	kfree(cpufreq_cdev->freq_table);
+	kfree(cpufreq_cdev);
 }
 EXPORT_SYMBOL_GPL(cpufreq_cooling_unregister);
diff --git a/drivers/thermal/imx_thermal.c b/drivers/thermal/imx_thermal.c
index fb648a45754e..f7ec39f46ee4 100644
--- a/drivers/thermal/imx_thermal.c
+++ b/drivers/thermal/imx_thermal.c
@@ -8,6 +8,7 @@
  */
 
 #include <linux/clk.h>
+#include <linux/cpufreq.h>
 #include <linux/cpu_cooling.h>
 #include <linux/delay.h>
 #include <linux/device.h>
@@ -88,6 +89,7 @@ static struct thermal_soc_data thermal_imx6sx_data = {
 };
 
 struct imx_thermal_data {
+	struct cpufreq_policy *policy;
 	struct thermal_zone_device *tz;
 	struct thermal_cooling_device *cdev;
 	enum thermal_device_mode mode;
@@ -525,13 +527,18 @@ static int imx_thermal_probe(struct platform_device *pdev)
 	regmap_write(map, MISC0 + REG_SET, MISC0_REFTOP_SELBIASOFF);
 	regmap_write(map, TEMPSENSE0 + REG_SET, TEMPSENSE0_POWER_DOWN);
 
-	data->cdev = cpufreq_cooling_register(cpu_present_mask);
+	data->policy = cpufreq_cpu_get(0);
+	if (!data->policy) {
+		pr_debug("%s: CPUFreq policy not found\n", __func__);
+		return -EPROBE_DEFER;
+	}
+
+	data->cdev = cpufreq_cooling_register(data->policy);
 	if (IS_ERR(data->cdev)) {
 		ret = PTR_ERR(data->cdev);
-		if (ret != -EPROBE_DEFER)
-			dev_err(&pdev->dev,
-				"failed to register cpufreq cooling device: %d\n",
-				ret);
+		dev_err(&pdev->dev,
+			"failed to register cpufreq cooling device: %d\n", ret);
+		cpufreq_cpu_put(data->policy);
 		return ret;
 	}
 
@@ -542,6 +549,7 @@ static int imx_thermal_probe(struct platform_device *pdev)
 			dev_err(&pdev->dev,
 				"failed to get thermal clk: %d\n", ret);
 		cpufreq_cooling_unregister(data->cdev);
+		cpufreq_cpu_put(data->policy);
 		return ret;
 	}
 
@@ -556,6 +564,7 @@ static int imx_thermal_probe(struct platform_device *pdev)
 	if (ret) {
 		dev_err(&pdev->dev, "failed to enable thermal clk: %d\n", ret);
 		cpufreq_cooling_unregister(data->cdev);
+		cpufreq_cpu_put(data->policy);
 		return ret;
 	}
 
@@ -571,6 +580,7 @@ static int imx_thermal_probe(struct platform_device *pdev)
 			"failed to register thermal zone device %d\n", ret);
 		clk_disable_unprepare(data->thermal_clk);
 		cpufreq_cooling_unregister(data->cdev);
+		cpufreq_cpu_put(data->policy);
 		return ret;
 	}
 
@@ -599,6 +609,7 @@ static int imx_thermal_probe(struct platform_device *pdev)
 		clk_disable_unprepare(data->thermal_clk);
 		thermal_zone_device_unregister(data->tz);
 		cpufreq_cooling_unregister(data->cdev);
+		cpufreq_cpu_put(data->policy);
 		return ret;
 	}
 
@@ -620,6 +631,7 @@ static int imx_thermal_remove(struct platform_device *pdev)
 
 	thermal_zone_device_unregister(data->tz);
 	cpufreq_cooling_unregister(data->cdev);
+	cpufreq_cpu_put(data->policy);
 
 	return 0;
 }
diff --git a/drivers/thermal/qoriq_thermal.c b/drivers/thermal/qoriq_thermal.c
index 644ba526d9ea..4362a69ac88d 100644
--- a/drivers/thermal/qoriq_thermal.c
+++ b/drivers/thermal/qoriq_thermal.c
@@ -195,7 +195,6 @@ static struct thermal_zone_of_device_ops tmu_tz_ops = {
 static int qoriq_tmu_probe(struct platform_device *pdev)
 {
 	int ret;
-	const struct thermal_trip *trip;
 	struct qoriq_tmu_data *data;
 	struct device_node *np = pdev->dev.of_node;
 	u32 site = 0;
@@ -243,8 +242,6 @@ static int qoriq_tmu_probe(struct platform_device *pdev)
 		goto err_tmu;
 	}
 
-	trip = of_thermal_get_trip_points(data->tz);
-
 	/* Enable monitoring */
 	site |= 0x1 << (15 - data->sensor_id);
 	tmu_write(data, site | TMR_ME | TMR_ALPF, &data->regs->tmr);
diff --git a/drivers/thermal/thermal_core.c b/drivers/thermal/thermal_core.c
index b21b9cc2c8d6..5a51c740e372 100644
--- a/drivers/thermal/thermal_core.c
+++ b/drivers/thermal/thermal_core.c
@@ -359,7 +359,7 @@ static DECLARE_DELAYED_WORK(thermal_emergency_poweroff_work,
  * This may be called from any critical situation to trigger a system shutdown
  * after a known period of time. By default this is not scheduled.
  */
-void thermal_emergency_poweroff(void)
+static void thermal_emergency_poweroff(void)
 {
 	int poweroff_delay_ms = CONFIG_THERMAL_EMERGENCY_POWEROFF_DELAY_MS;
 	/*
diff --git a/drivers/thermal/ti-soc-thermal/ti-bandgap.c b/drivers/thermal/ti-soc-thermal/ti-bandgap.c
index ba9c302454fb..696ab3046b87 100644
--- a/drivers/thermal/ti-soc-thermal/ti-bandgap.c
+++ b/drivers/thermal/ti-soc-thermal/ti-bandgap.c
@@ -1010,7 +1010,7 @@ ti_bandgap_force_single_read(struct ti_bandgap *bgp, int id)
 }
 
 /**
- * ti_bandgap_set_continous_mode() - One time enabling of continuous mode
+ * ti_bandgap_set_continuous_mode() - One time enabling of continuous mode
  * @bgp: pointer to struct ti_bandgap
  *
  * Call this function only if HAS(MODE_CONFIG) is set. As this driver may
@@ -1214,22 +1214,18 @@ static struct ti_bandgap *ti_bandgap_build(struct platform_device *pdev)
 	}
 
 	bgp = devm_kzalloc(&pdev->dev, sizeof(*bgp), GFP_KERNEL);
-	if (!bgp) {
-		dev_err(&pdev->dev, "Unable to allocate mem for driver ref\n");
+	if (!bgp)
 		return ERR_PTR(-ENOMEM);
-	}
 
 	of_id = of_match_device(of_ti_bandgap_match, &pdev->dev);
 	if (of_id)
 		bgp->conf = of_id->data;
 
 	/* register shadow for context save and restore */
-	bgp->regval = devm_kzalloc(&pdev->dev, sizeof(*bgp->regval) *
-				   bgp->conf->sensor_count, GFP_KERNEL);
-	if (!bgp->regval) {
-		dev_err(&pdev->dev, "Unable to allocate mem for driver ref\n");
+	bgp->regval = devm_kcalloc(&pdev->dev, bgp->conf->sensor_count,
+				   sizeof(*bgp->regval), GFP_KERNEL);
+	if (!bgp->regval)
 		return ERR_PTR(-ENOMEM);
-	}
 
 	i = 0;
 	do {
diff --git a/drivers/thermal/ti-soc-thermal/ti-thermal-common.c b/drivers/thermal/ti-soc-thermal/ti-thermal-common.c
index 02790f69e26c..c211a8e4a210 100644
--- a/drivers/thermal/ti-soc-thermal/ti-thermal-common.c
+++ b/drivers/thermal/ti-soc-thermal/ti-thermal-common.c
@@ -28,6 +28,7 @@
 #include <linux/kernel.h>
 #include <linux/workqueue.h>
 #include <linux/thermal.h>
+#include <linux/cpufreq.h>
 #include <linux/cpumask.h>
 #include <linux/cpu_cooling.h>
 #include <linux/of.h>
@@ -37,6 +38,7 @@
 
 /* common data structures */
 struct ti_thermal_data {
+	struct cpufreq_policy *policy;
 	struct thermal_zone_device *ti_thermal;
 	struct thermal_zone_device *pcb_tz;
 	struct thermal_cooling_device *cool_dev;
@@ -247,15 +249,19 @@ int ti_thermal_register_cpu_cooling(struct ti_bandgap *bgp, int id)
 	if (!data)
 		return -EINVAL;
 
+	data->policy = cpufreq_cpu_get(0);
+	if (!data->policy) {
+		pr_debug("%s: CPUFreq policy not found\n", __func__);
+		return -EPROBE_DEFER;
+	}
+
 	/* Register cooling device */
-	data->cool_dev = cpufreq_cooling_register(cpu_present_mask);
+	data->cool_dev = cpufreq_cooling_register(data->policy);
 	if (IS_ERR(data->cool_dev)) {
 		int ret = PTR_ERR(data->cool_dev);
-
-		if (ret != -EPROBE_DEFER)
-			dev_err(bgp->dev,
-				"Failed to register cpu cooling device %d\n",
-				ret);
+		dev_err(bgp->dev, "Failed to register cpu cooling device %d\n",
+			ret);
+		cpufreq_cpu_put(data->policy);
 
 		return ret;
 	}
@@ -270,8 +276,10 @@ int ti_thermal_unregister_cpu_cooling(struct ti_bandgap *bgp, int id)
 
 	data = ti_bandgap_get_sensor_data(bgp, id);
 
-	if (data)
+	if (data) {
 		cpufreq_cooling_unregister(data->cool_dev);
+		cpufreq_cpu_put(data->policy);
+	}
 
 	return 0;
 }
diff --git a/drivers/tty/ehv_bytechan.c b/drivers/tty/ehv_bytechan.c
index 7ac9bcdf1e61..61fe8d6fd24e 100644
--- a/drivers/tty/ehv_bytechan.c
+++ b/drivers/tty/ehv_bytechan.c
@@ -764,7 +764,7 @@ static int __init ehv_bc_init(void)
 	ehv_bc_driver = alloc_tty_driver(count);
 	if (!ehv_bc_driver) {
 		ret = -ENOMEM;
-		goto error;
+		goto err_free_bcs;
 	}
 
 	ehv_bc_driver->driver_name = "ehv-bc";
@@ -778,24 +778,23 @@ static int __init ehv_bc_init(void)
 	ret = tty_register_driver(ehv_bc_driver);
 	if (ret) {
 		pr_err("ehv-bc: could not register tty driver (ret=%i)\n", ret);
-		goto error;
+		goto err_put_tty_driver;
 	}
 
 	ret = platform_driver_register(&ehv_bc_tty_driver);
 	if (ret) {
 		pr_err("ehv-bc: could not register platform driver (ret=%i)\n",
 		       ret);
-		goto error;
+		goto err_deregister_tty_driver;
 	}
 
 	return 0;
 
-error:
-	if (ehv_bc_driver) {
-		tty_unregister_driver(ehv_bc_driver);
-		put_tty_driver(ehv_bc_driver);
-	}
-
+err_deregister_tty_driver:
+	tty_unregister_driver(ehv_bc_driver);
+err_put_tty_driver:
+	put_tty_driver(ehv_bc_driver);
+err_free_bcs:
 	kfree(bcs);
 
 	return ret;
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/serdev/core.c b/drivers/tty/serdev/core.c
index 433de5ea9b02..f71b47334149 100644
--- a/drivers/tty/serdev/core.c
+++ b/drivers/tty/serdev/core.c
@@ -122,6 +122,18 @@ void serdev_device_write_wakeup(struct serdev_device *serdev)
 }
 EXPORT_SYMBOL_GPL(serdev_device_write_wakeup);
 
+int serdev_device_write_buf(struct serdev_device *serdev,
+			    const unsigned char *buf, size_t count)
+{
+	struct serdev_controller *ctrl = serdev->ctrl;
+
+	if (!ctrl || !ctrl->ops->write_buf)
+		return -EINVAL;
+
+	return ctrl->ops->write_buf(ctrl, buf, count);
+}
+EXPORT_SYMBOL_GPL(serdev_device_write_buf);
+
 int serdev_device_write(struct serdev_device *serdev,
 			const unsigned char *buf, size_t count,
 			unsigned long timeout)
diff --git a/drivers/tty/serdev/serdev-ttyport.c b/drivers/tty/serdev/serdev-ttyport.c
index 487c88f6aa0e..d0a021c93986 100644
--- a/drivers/tty/serdev/serdev-ttyport.c
+++ b/drivers/tty/serdev/serdev-ttyport.c
@@ -102,9 +102,6 @@ static int ttyport_open(struct serdev_controller *ctrl)
 		return PTR_ERR(tty);
 	serport->tty = tty;
 
-	serport->port->client_ops = &client_ops;
-	serport->port->client_data = ctrl;
-
 	if (tty->ops->open)
 		tty->ops->open(serport->tty, NULL);
 	else
@@ -215,6 +212,7 @@ struct device *serdev_tty_port_register(struct tty_port *port,
 					struct device *parent,
 					struct tty_driver *drv, int idx)
 {
+	const struct tty_port_client_operations *old_ops;
 	struct serdev_controller *ctrl;
 	struct serport *serport;
 	int ret;
@@ -233,28 +231,37 @@ struct device *serdev_tty_port_register(struct tty_port *port,
 
 	ctrl->ops = &ctrl_ops;
 
+	old_ops = port->client_ops;
+	port->client_ops = &client_ops;
+	port->client_data = ctrl;
+
 	ret = serdev_controller_add(ctrl);
 	if (ret)
-		goto err_controller_put;
+		goto err_reset_data;
 
 	dev_info(&ctrl->dev, "tty port %s%d registered\n", drv->name, idx);
 	return &ctrl->dev;
 
-err_controller_put:
+err_reset_data:
+	port->client_data = NULL;
+	port->client_ops = old_ops;
 	serdev_controller_put(ctrl);
+
 	return ERR_PTR(ret);
 }
 
-void serdev_tty_port_unregister(struct tty_port *port)
+int serdev_tty_port_unregister(struct tty_port *port)
 {
 	struct serdev_controller *ctrl = port->client_data;
 	struct serport *serport = serdev_controller_get_drvdata(ctrl);
 
 	if (!serport)
-		return;
+		return -ENODEV;
 
 	serdev_controller_remove(ctrl);
 	port->client_ops = NULL;
 	port->client_data = NULL;
 	serdev_controller_put(ctrl);
+
+	return 0;
 }
diff --git a/drivers/tty/serial/8250/8250_port.c b/drivers/tty/serial/8250/8250_port.c
index 09a65a3ec7f7..68fd045a7025 100644
--- a/drivers/tty/serial/8250/8250_port.c
+++ b/drivers/tty/serial/8250/8250_port.c
@@ -47,6 +47,7 @@
 /*
  * These are definitions for the Exar XR17V35X and XR17(C|D)15X
  */
+#define UART_EXAR_INT0		0x80
 #define UART_EXAR_SLEEP		0x8b	/* Sleep mode */
 #define UART_EXAR_DVID		0x8d	/* Device identification */
 
@@ -1337,7 +1338,7 @@ out_lock:
 	/*
 	 * Check if the device is a Fintek F81216A
 	 */
-	if (port->type == PORT_16550A)
+	if (port->type == PORT_16550A && port->iotype == UPIO_PORT)
 		fintek_8250_probe(up);
 
 	if (up->capabilities != old_capabilities) {
@@ -1869,17 +1870,13 @@ static int serial8250_default_handle_irq(struct uart_port *port)
 static int exar_handle_irq(struct uart_port *port)
 {
 	unsigned int iir = serial_port_in(port, UART_IIR);
-	int ret;
+	int ret = 0;
 
-	ret = serial8250_handle_irq(port, iir);
+	if (((port->type == PORT_XR17V35X) || (port->type == PORT_XR17D15X)) &&
+	    serial_port_in(port, UART_EXAR_INT0) != 0)
+		ret = 1;
 
-	if ((port->type == PORT_XR17V35X) ||
-	   (port->type == PORT_XR17D15X)) {
-		serial_port_in(port, 0x80);
-		serial_port_in(port, 0x81);
-		serial_port_in(port, 0x82);
-		serial_port_in(port, 0x83);
-	}
+	ret |= serial8250_handle_irq(port, iir);
 
 	return ret;
 }
@@ -2177,6 +2174,8 @@ int serial8250_do_startup(struct uart_port *port)
 	serial_port_in(port, UART_RX);
 	serial_port_in(port, UART_IIR);
 	serial_port_in(port, UART_MSR);
+	if ((port->type == PORT_XR17V35X) || (port->type == PORT_XR17D15X))
+		serial_port_in(port, UART_EXAR_INT0);
 
 	/*
 	 * At this point, there's no way the LSR could still be 0xff;
@@ -2335,6 +2334,8 @@ dont_test_tx_en:
 	serial_port_in(port, UART_RX);
 	serial_port_in(port, UART_IIR);
 	serial_port_in(port, UART_MSR);
+	if ((port->type == PORT_XR17V35X) || (port->type == PORT_XR17D15X))
+		serial_port_in(port, UART_EXAR_INT0);
 	up->lsr_saved_flags = 0;
 	up->msr_saved_flags = 0;
 
diff --git a/drivers/tty/serial/altera_jtaguart.c b/drivers/tty/serial/altera_jtaguart.c
index 18e3f8342b85..0475f5d261ce 100644
--- a/drivers/tty/serial/altera_jtaguart.c
+++ b/drivers/tty/serial/altera_jtaguart.c
@@ -478,6 +478,7 @@ static int altera_jtaguart_remove(struct platform_device *pdev)
 
 	port = &altera_jtaguart_ports[i].port;
 	uart_remove_one_port(&altera_jtaguart_driver, port);
+	iounmap(port->membase);
 
 	return 0;
 }
diff --git a/drivers/tty/serial/altera_uart.c b/drivers/tty/serial/altera_uart.c
index 46d3438a0d27..3e4b717670d7 100644
--- a/drivers/tty/serial/altera_uart.c
+++ b/drivers/tty/serial/altera_uart.c
@@ -615,6 +615,7 @@ static int altera_uart_remove(struct platform_device *pdev)
 	if (port) {
 		uart_remove_one_port(&altera_uart_driver, port);
 		port->mapbase = 0;
+		iounmap(port->membase);
 	}
 
 	return 0;
diff --git a/drivers/tty/serial/efm32-uart.c b/drivers/tty/serial/efm32-uart.c
index ebd8569f9ad5..9fff25be87f9 100644
--- a/drivers/tty/serial/efm32-uart.c
+++ b/drivers/tty/serial/efm32-uart.c
@@ -27,6 +27,7 @@
 #define UARTn_FRAME		0x04
 #define UARTn_FRAME_DATABITS__MASK	0x000f
 #define UARTn_FRAME_DATABITS(n)		((n) - 3)
+#define UARTn_FRAME_PARITY__MASK	0x0300
 #define UARTn_FRAME_PARITY_NONE		0x0000
 #define UARTn_FRAME_PARITY_EVEN		0x0200
 #define UARTn_FRAME_PARITY_ODD		0x0300
@@ -572,12 +573,16 @@ static void efm32_uart_console_get_options(struct efm32_uart_port *efm_port,
 			16 * (4 + (clkdiv >> 6)));
 
 	frame = efm32_uart_read32(efm_port, UARTn_FRAME);
-	if (frame & UARTn_FRAME_PARITY_ODD)
+	switch (frame & UARTn_FRAME_PARITY__MASK) {
+	case UARTn_FRAME_PARITY_ODD:
 		*parity = 'o';
-	else if (frame & UARTn_FRAME_PARITY_EVEN)
+		break;
+	case UARTn_FRAME_PARITY_EVEN:
 		*parity = 'e';
-	else
+		break;
+	default:
 		*parity = 'n';
+	}
 
 	*bits = (frame & UARTn_FRAME_DATABITS__MASK) -
 			UARTn_FRAME_DATABITS(4) + 4;
diff --git a/drivers/tty/serial/ifx6x60.c b/drivers/tty/serial/ifx6x60.c
index 157883653256..f190a84a0246 100644
--- a/drivers/tty/serial/ifx6x60.c
+++ b/drivers/tty/serial/ifx6x60.c
@@ -1382,9 +1382,9 @@ static struct spi_driver ifx_spi_driver = {
 static void __exit ifx_spi_exit(void)
 {
 	/* unregister */
+	spi_unregister_driver(&ifx_spi_driver);
 	tty_unregister_driver(tty_drv);
 	put_tty_driver(tty_drv);
-	spi_unregister_driver(&ifx_spi_driver);
 	unregister_reboot_notifier(&ifx_modem_reboot_notifier_block);
 }
 
diff --git a/drivers/tty/serial/imx.c b/drivers/tty/serial/imx.c
index 33509b4beaec..bbefddd92bfe 100644
--- a/drivers/tty/serial/imx.c
+++ b/drivers/tty/serial/imx.c
@@ -2184,7 +2184,9 @@ static int serial_imx_probe(struct platform_device *pdev)
 		 * and DCD (when they are outputs) or enables the respective
 		 * irqs. So set this bit early, i.e. before requesting irqs.
 		 */
-		writel(UFCR_DCEDTE, sport->port.membase + UFCR);
+		reg = readl(sport->port.membase + UFCR);
+		if (!(reg & UFCR_DCEDTE))
+			writel(reg | UFCR_DCEDTE, sport->port.membase + UFCR);
 
 		/*
 		 * Disable UCR3_RI and UCR3_DCD irqs. They are also not
@@ -2195,7 +2197,15 @@ static int serial_imx_probe(struct platform_device *pdev)
 		       sport->port.membase + UCR3);
 
 	} else {
-		writel(0, sport->port.membase + UFCR);
+		unsigned long ucr3 = UCR3_DSR;
+
+		reg = readl(sport->port.membase + UFCR);
+		if (reg & UFCR_DCEDTE)
+			writel(reg & ~UFCR_DCEDTE, sport->port.membase + UFCR);
+
+		if (!is_imx1_uart(sport))
+			ucr3 |= IMX21_UCR3_RXDMUXSEL | UCR3_ADNIMP;
+		writel(ucr3, sport->port.membase + UCR3);
 	}
 
 	clk_disable_unprepare(sport->clk_ipg);
diff --git a/drivers/tty/serial/serial_core.c b/drivers/tty/serial/serial_core.c
index 0f45b7884a2c..13bfd5dcffce 100644
--- a/drivers/tty/serial/serial_core.c
+++ b/drivers/tty/serial/serial_core.c
@@ -2083,7 +2083,7 @@ int uart_suspend_port(struct uart_driver *drv, struct uart_port *uport)
 	mutex_lock(&port->mutex);
 
 	tty_dev = device_find_child(uport->dev, &match, serial_match_port);
-	if (device_may_wakeup(tty_dev)) {
+	if (tty_dev && device_may_wakeup(tty_dev)) {
 		if (!enable_irq_wake(uport->irq))
 			uport->irq_wake = 1;
 		put_device(tty_dev);
@@ -2782,7 +2782,7 @@ int uart_add_one_port(struct uart_driver *drv, struct uart_port *uport)
 	 * Register the port whether it's detected or not.  This allows
 	 * setserial to be used to alter this port's parameters.
 	 */
-	tty_dev = tty_port_register_device_attr(port, drv->tty_driver,
+	tty_dev = tty_port_register_device_attr_serdev(port, drv->tty_driver,
 			uport->line, uport->dev, port, uport->tty_groups);
 	if (likely(!IS_ERR(tty_dev))) {
 		device_set_wakeup_capable(tty_dev, 1);
@@ -2845,7 +2845,7 @@ int uart_remove_one_port(struct uart_driver *drv, struct uart_port *uport)
 	/*
 	 * Remove the devices from the tty layer
 	 */
-	tty_unregister_device(drv->tty_driver, uport->line);
+	tty_port_unregister_device(port, drv->tty_driver, uport->line);
 
 	tty = tty_port_tty_get(port);
 	if (tty) {
diff --git a/drivers/tty/tty_port.c b/drivers/tty/tty_port.c
index 1d21a9c1d33e..4fb3165384c4 100644
--- a/drivers/tty/tty_port.c
+++ b/drivers/tty/tty_port.c
@@ -34,7 +34,9 @@ static int tty_port_default_receive_buf(struct tty_port *port,
 	if (!disc)
 		return 0;
 
+	mutex_lock(&tty->atomic_write_lock);
 	ret = tty_ldisc_receive_buf(disc, p, (char *)f, count);
+	mutex_unlock(&tty->atomic_write_lock);
 
 	tty_ldisc_deref(disc);
 
@@ -129,19 +131,85 @@ struct device *tty_port_register_device_attr(struct tty_port *port,
 		struct device *device, void *drvdata,
 		const struct attribute_group **attr_grp)
 {
+	tty_port_link_device(port, driver, index);
+	return tty_register_device_attr(driver, index, device, drvdata,
+			attr_grp);
+}
+EXPORT_SYMBOL_GPL(tty_port_register_device_attr);
+
+/**
+ * tty_port_register_device_attr_serdev - register tty or serdev device
+ * @port: tty_port of the device
+ * @driver: tty_driver for this device
+ * @index: index of the tty
+ * @device: parent if exists, otherwise NULL
+ * @drvdata: driver data for the device
+ * @attr_grp: attribute group for the device
+ *
+ * Register a serdev or tty device depending on if the parent device has any
+ * defined serdev clients or not.
+ */
+struct device *tty_port_register_device_attr_serdev(struct tty_port *port,
+		struct tty_driver *driver, unsigned index,
+		struct device *device, void *drvdata,
+		const struct attribute_group **attr_grp)
+{
 	struct device *dev;
 
 	tty_port_link_device(port, driver, index);
 
 	dev = serdev_tty_port_register(port, device, driver, index);
-	if (PTR_ERR(dev) != -ENODEV)
+	if (PTR_ERR(dev) != -ENODEV) {
 		/* Skip creating cdev if we registered a serdev device */
 		return dev;
+	}
 
 	return tty_register_device_attr(driver, index, device, drvdata,
 			attr_grp);
 }
-EXPORT_SYMBOL_GPL(tty_port_register_device_attr);
+EXPORT_SYMBOL_GPL(tty_port_register_device_attr_serdev);
+
+/**
+ * tty_port_register_device_serdev - register tty or serdev device
+ * @port: tty_port of the device
+ * @driver: tty_driver for this device
+ * @index: index of the tty
+ * @device: parent if exists, otherwise NULL
+ *
+ * Register a serdev or tty device depending on if the parent device has any
+ * defined serdev clients or not.
+ */
+struct device *tty_port_register_device_serdev(struct tty_port *port,
+		struct tty_driver *driver, unsigned index,
+		struct device *device)
+{
+	return tty_port_register_device_attr_serdev(port, driver, index,
+			device, NULL, NULL);
+}
+EXPORT_SYMBOL_GPL(tty_port_register_device_serdev);
+
+/**
+ * tty_port_unregister_device - deregister a tty or serdev device
+ * @port: tty_port of the device
+ * @driver: tty_driver for this device
+ * @index: index of the tty
+ *
+ * If a tty or serdev device is registered with a call to
+ * tty_port_register_device_serdev() then this function must be called when
+ * the device is gone.
+ */
+void tty_port_unregister_device(struct tty_port *port,
+		struct tty_driver *driver, unsigned index)
+{
+	int ret;
+
+	ret = serdev_tty_port_unregister(port);
+	if (ret == 0)
+		return;
+
+	tty_unregister_device(driver, index);
+}
+EXPORT_SYMBOL_GPL(tty_port_unregister_device);
 
 int tty_port_alloc_xmit_buf(struct tty_port *port)
 {
@@ -189,9 +257,6 @@ static void tty_port_destructor(struct kref *kref)
 	/* check if last port ref was dropped before tty release */
 	if (WARN_ON(port->itty))
 		return;
-
-	serdev_tty_port_unregister(port);
-
 	if (port->xmit_buf)
 		free_page((unsigned long)port->xmit_buf);
 	tty_port_destroy(port);
diff --git a/drivers/uio/uio.c b/drivers/uio/uio.c
index 1c196f87e9d9..ff04b7f8549f 100644
--- a/drivers/uio/uio.c
+++ b/drivers/uio/uio.c
@@ -279,7 +279,7 @@ static int uio_dev_add_attributes(struct uio_device *idev)
 		map = kzalloc(sizeof(*map), GFP_KERNEL);
 		if (!map) {
 			ret = -ENOMEM;
-			goto err_map_kobj;
+			goto err_map;
 		}
 		kobject_init(&map->kobj, &map_attr_type);
 		map->mem = mem;
@@ -289,7 +289,7 @@ static int uio_dev_add_attributes(struct uio_device *idev)
 			goto err_map_kobj;
 		ret = kobject_uevent(&map->kobj, KOBJ_ADD);
 		if (ret)
-			goto err_map;
+			goto err_map_kobj;
 	}
 
 	for (pi = 0; pi < MAX_UIO_PORT_REGIONS; pi++) {
@@ -308,7 +308,7 @@ static int uio_dev_add_attributes(struct uio_device *idev)
 		portio = kzalloc(sizeof(*portio), GFP_KERNEL);
 		if (!portio) {
 			ret = -ENOMEM;
-			goto err_portio_kobj;
+			goto err_portio;
 		}
 		kobject_init(&portio->kobj, &portio_attr_type);
 		portio->port = port;
@@ -319,7 +319,7 @@ static int uio_dev_add_attributes(struct uio_device *idev)
 			goto err_portio_kobj;
 		ret = kobject_uevent(&portio->kobj, KOBJ_ADD);
 		if (ret)
-			goto err_portio;
+			goto err_portio_kobj;
 	}
 
 	return 0;
diff --git a/drivers/usb/chipidea/core.c b/drivers/usb/chipidea/core.c
index 9e217b1361ea..fe4fe2440729 100644
--- a/drivers/usb/chipidea/core.c
+++ b/drivers/usb/chipidea/core.c
@@ -843,7 +843,10 @@ static ssize_t ci_role_show(struct device *dev, struct device_attribute *attr,
 {
 	struct ci_hdrc *ci = dev_get_drvdata(dev);
 
-	return sprintf(buf, "%s\n", ci_role(ci)->name);
+	if (ci->role != CI_ROLE_END)
+		return sprintf(buf, "%s\n", ci_role(ci)->name);
+
+	return 0;
 }
 
 static ssize_t ci_role_store(struct device *dev,
diff --git a/drivers/usb/chipidea/debug.c b/drivers/usb/chipidea/debug.c
index 6d23eede4d8c..1c31e8a08810 100644
--- a/drivers/usb/chipidea/debug.c
+++ b/drivers/usb/chipidea/debug.c
@@ -294,7 +294,8 @@ static int ci_role_show(struct seq_file *s, void *data)
 {
 	struct ci_hdrc *ci = s->private;
 
-	seq_printf(s, "%s\n", ci_role(ci)->name);
+	if (ci->role != CI_ROLE_END)
+		seq_printf(s, "%s\n", ci_role(ci)->name);
 
 	return 0;
 }
diff --git a/drivers/usb/chipidea/udc.c b/drivers/usb/chipidea/udc.c
index 56d2d3213076..d68b125796f9 100644
--- a/drivers/usb/chipidea/udc.c
+++ b/drivers/usb/chipidea/udc.c
@@ -1993,6 +1993,7 @@ static void udc_id_switch_for_host(struct ci_hdrc *ci)
 int ci_hdrc_gadget_init(struct ci_hdrc *ci)
 {
 	struct ci_role_driver *rdrv;
+	int ret;
 
 	if (!hw_read(ci, CAP_DCCPARAMS, DCCPARAMS_DC))
 		return -ENXIO;
@@ -2005,7 +2006,10 @@ int ci_hdrc_gadget_init(struct ci_hdrc *ci)
 	rdrv->stop	= udc_id_switch_for_host;
 	rdrv->irq	= udc_irq;
 	rdrv->name	= "gadget";
-	ci->roles[CI_ROLE_GADGET] = rdrv;
 
-	return udc_start(ci);
+	ret = udc_start(ci);
+	if (!ret)
+		ci->roles[CI_ROLE_GADGET] = rdrv;
+
+	return ret;
 }
diff --git a/drivers/usb/chipidea/usbmisc_imx.c b/drivers/usb/chipidea/usbmisc_imx.c
index e77a4ed4f021..9f4a0185dd60 100644
--- a/drivers/usb/chipidea/usbmisc_imx.c
+++ b/drivers/usb/chipidea/usbmisc_imx.c
@@ -108,6 +108,8 @@ struct imx_usbmisc {
 	const struct usbmisc_ops *ops;
 };
 
+static inline bool is_imx53_usbmisc(struct imx_usbmisc_data *data);
+
 static int usbmisc_imx25_init(struct imx_usbmisc_data *data)
 {
 	struct imx_usbmisc *usbmisc = dev_get_drvdata(data->dev);
@@ -242,10 +244,15 @@ static int usbmisc_imx53_init(struct imx_usbmisc_data *data)
 			val = readl(reg) | MX53_USB_UHx_CTRL_WAKE_UP_EN
 				| MX53_USB_UHx_CTRL_ULPI_INT_EN;
 			writel(val, reg);
-			/* Disable internal 60Mhz clock */
-			reg = usbmisc->base + MX53_USB_CLKONOFF_CTRL_OFFSET;
-			val = readl(reg) | MX53_USB_CLKONOFF_CTRL_H2_INT60CKOFF;
-			writel(val, reg);
+			if (is_imx53_usbmisc(data)) {
+				/* Disable internal 60Mhz clock */
+				reg = usbmisc->base +
+					MX53_USB_CLKONOFF_CTRL_OFFSET;
+				val = readl(reg) |
+					MX53_USB_CLKONOFF_CTRL_H2_INT60CKOFF;
+				writel(val, reg);
+			}
+
 		}
 		if (data->disable_oc) {
 			reg = usbmisc->base + MX53_USB_UH2_CTRL_OFFSET;
@@ -267,10 +274,15 @@ static int usbmisc_imx53_init(struct imx_usbmisc_data *data)
 			val = readl(reg) | MX53_USB_UHx_CTRL_WAKE_UP_EN
 				| MX53_USB_UHx_CTRL_ULPI_INT_EN;
 			writel(val, reg);
-			/* Disable internal 60Mhz clock */
-			reg = usbmisc->base + MX53_USB_CLKONOFF_CTRL_OFFSET;
-			val = readl(reg) | MX53_USB_CLKONOFF_CTRL_H3_INT60CKOFF;
-			writel(val, reg);
+
+			if (is_imx53_usbmisc(data)) {
+				/* Disable internal 60Mhz clock */
+				reg = usbmisc->base +
+					MX53_USB_CLKONOFF_CTRL_OFFSET;
+				val = readl(reg) |
+					MX53_USB_CLKONOFF_CTRL_H3_INT60CKOFF;
+				writel(val, reg);
+			}
 		}
 		if (data->disable_oc) {
 			reg = usbmisc->base + MX53_USB_UH3_CTRL_OFFSET;
@@ -456,6 +468,10 @@ static const struct usbmisc_ops imx27_usbmisc_ops = {
 	.init = usbmisc_imx27_init,
 };
 
+static const struct usbmisc_ops imx51_usbmisc_ops = {
+	.init = usbmisc_imx53_init,
+};
+
 static const struct usbmisc_ops imx53_usbmisc_ops = {
 	.init = usbmisc_imx53_init,
 };
@@ -479,6 +495,13 @@ static const struct usbmisc_ops imx7d_usbmisc_ops = {
 	.set_wakeup = usbmisc_imx7d_set_wakeup,
 };
 
+static inline bool is_imx53_usbmisc(struct imx_usbmisc_data *data)
+{
+	struct imx_usbmisc *usbmisc = dev_get_drvdata(data->dev);
+
+	return usbmisc->ops == &imx53_usbmisc_ops;
+}
+
 int imx_usbmisc_init(struct imx_usbmisc_data *data)
 {
 	struct imx_usbmisc *usbmisc;
@@ -536,7 +559,7 @@ static const struct of_device_id usbmisc_imx_dt_ids[] = {
 	},
 	{
 		.compatible = "fsl,imx51-usbmisc",
-		.data = &imx53_usbmisc_ops,
+		.data = &imx51_usbmisc_ops,
 	},
 	{
 		.compatible = "fsl,imx53-usbmisc",
diff --git a/drivers/usb/core/devio.c b/drivers/usb/core/devio.c
index cfc3cff6e8d5..8e6ef671be9b 100644
--- a/drivers/usb/core/devio.c
+++ b/drivers/usb/core/devio.c
@@ -475,11 +475,11 @@ static void snoop_urb(struct usb_device *udev,
 
 	if (userurb) {		/* Async */
 		if (when == SUBMIT)
-			dev_info(&udev->dev, "userurb %p, ep%d %s-%s, "
+			dev_info(&udev->dev, "userurb %pK, ep%d %s-%s, "
 					"length %u\n",
 					userurb, ep, t, d, length);
 		else
-			dev_info(&udev->dev, "userurb %p, ep%d %s-%s, "
+			dev_info(&udev->dev, "userurb %pK, ep%d %s-%s, "
 					"actual_length %u status %d\n",
 					userurb, ep, t, d, length,
 					timeout_or_status);
@@ -1895,7 +1895,7 @@ static int proc_reapurb(struct usb_dev_state *ps, void __user *arg)
 	if (as) {
 		int retval;
 
-		snoop(&ps->dev->dev, "reap %p\n", as->userurb);
+		snoop(&ps->dev->dev, "reap %pK\n", as->userurb);
 		retval = processcompl(as, (void __user * __user *)arg);
 		free_async(as);
 		return retval;
@@ -1912,7 +1912,7 @@ static int proc_reapurbnonblock(struct usb_dev_state *ps, void __user *arg)
 
 	as = async_getcompleted(ps);
 	if (as) {
-		snoop(&ps->dev->dev, "reap %p\n", as->userurb);
+		snoop(&ps->dev->dev, "reap %pK\n", as->userurb);
 		retval = processcompl(as, (void __user * __user *)arg);
 		free_async(as);
 	} else {
@@ -2043,7 +2043,7 @@ static int proc_reapurb_compat(struct usb_dev_state *ps, void __user *arg)
 	if (as) {
 		int retval;
 
-		snoop(&ps->dev->dev, "reap %p\n", as->userurb);
+		snoop(&ps->dev->dev, "reap %pK\n", as->userurb);
 		retval = processcompl_compat(as, (void __user * __user *)arg);
 		free_async(as);
 		return retval;
@@ -2060,7 +2060,7 @@ static int proc_reapurbnonblock_compat(struct usb_dev_state *ps, void __user *ar
 
 	as = async_getcompleted(ps);
 	if (as) {
-		snoop(&ps->dev->dev, "reap %p\n", as->userurb);
+		snoop(&ps->dev->dev, "reap %pK\n", as->userurb);
 		retval = processcompl_compat(as, (void __user * __user *)arg);
 		free_async(as);
 	} else {
@@ -2489,7 +2489,7 @@ static long usbdev_do_ioctl(struct file *file, unsigned int cmd,
 #endif
 
 	case USBDEVFS_DISCARDURB:
-		snoop(&dev->dev, "%s: DISCARDURB %p\n", __func__, p);
+		snoop(&dev->dev, "%s: DISCARDURB %pK\n", __func__, p);
 		ret = proc_unlinkurb(ps, p);
 		break;
 
diff --git a/drivers/usb/core/hcd.c b/drivers/usb/core/hcd.c
index 49550790a3cb..5dea98358c05 100644
--- a/drivers/usb/core/hcd.c
+++ b/drivers/usb/core/hcd.c
@@ -1723,7 +1723,7 @@ int usb_hcd_unlink_urb (struct urb *urb, int status)
 		if (retval == 0)
 			retval = -EINPROGRESS;
 		else if (retval != -EIDRM && retval != -EBUSY)
-			dev_dbg(&udev->dev, "hcd_unlink_urb %p fail %d\n",
+			dev_dbg(&udev->dev, "hcd_unlink_urb %pK fail %d\n",
 					urb, retval);
 		usb_put_dev(udev);
 	}
@@ -1890,7 +1890,7 @@ rescan:
 		/* kick hcd */
 		unlink1(hcd, urb, -ESHUTDOWN);
 		dev_dbg (hcd->self.controller,
-			"shutdown urb %p ep%d%s%s\n",
+			"shutdown urb %pK ep%d%s%s\n",
 			urb, usb_endpoint_num(&ep->desc),
 			is_in ? "in" : "out",
 			({	char *s;
@@ -2520,6 +2520,7 @@ struct usb_hcd *__usb_create_hcd(const struct hc_driver *driver,
 		hcd->bandwidth_mutex = kmalloc(sizeof(*hcd->bandwidth_mutex),
 				GFP_KERNEL);
 		if (!hcd->bandwidth_mutex) {
+			kfree(hcd->address0_mutex);
 			kfree(hcd);
 			dev_dbg(dev, "hcd bandwidth mutex alloc failed\n");
 			return NULL;
diff --git a/drivers/usb/core/hub.c b/drivers/usb/core/hub.c
index 9dca59ef18b3..b8bb20d7acdb 100644
--- a/drivers/usb/core/hub.c
+++ b/drivers/usb/core/hub.c
@@ -362,7 +362,8 @@ static void usb_set_lpm_parameters(struct usb_device *udev)
 }
 
 /* USB 2.0 spec Section 11.24.4.5 */
-static int get_hub_descriptor(struct usb_device *hdev, void *data)
+static int get_hub_descriptor(struct usb_device *hdev,
+		struct usb_hub_descriptor *desc)
 {
 	int i, ret, size;
 	unsigned dtype;
@@ -378,10 +379,18 @@ static int get_hub_descriptor(struct usb_device *hdev, void *data)
 	for (i = 0; i < 3; i++) {
 		ret = usb_control_msg(hdev, usb_rcvctrlpipe(hdev, 0),
 			USB_REQ_GET_DESCRIPTOR, USB_DIR_IN | USB_RT_HUB,
-			dtype << 8, 0, data, size,
+			dtype << 8, 0, desc, size,
 			USB_CTRL_GET_TIMEOUT);
-		if (ret >= (USB_DT_HUB_NONVAR_SIZE + 2))
+		if (hub_is_superspeed(hdev)) {
+			if (ret == size)
+				return ret;
+		} else if (ret >= USB_DT_HUB_NONVAR_SIZE + 2) {
+			/* Make sure we have the DeviceRemovable field. */
+			size = USB_DT_HUB_NONVAR_SIZE + desc->bNbrPorts / 8 + 1;
+			if (ret < size)
+				return -EMSGSIZE;
 			return ret;
+		}
 	}
 	return -EINVAL;
 }
@@ -1313,7 +1322,7 @@ static int hub_configure(struct usb_hub *hub,
 	}
 	mutex_init(&hub->status_mutex);
 
-	hub->descriptor = kmalloc(sizeof(*hub->descriptor), GFP_KERNEL);
+	hub->descriptor = kzalloc(sizeof(*hub->descriptor), GFP_KERNEL);
 	if (!hub->descriptor) {
 		ret = -ENOMEM;
 		goto fail;
@@ -1321,13 +1330,19 @@ static int hub_configure(struct usb_hub *hub,
 
 	/* Request the entire hub descriptor.
 	 * hub->descriptor can handle USB_MAXCHILDREN ports,
-	 * but the hub can/will return fewer bytes here.
+	 * but a (non-SS) hub can/will return fewer bytes here.
 	 */
 	ret = get_hub_descriptor(hdev, hub->descriptor);
 	if (ret < 0) {
 		message = "can't read hub descriptor";
 		goto fail;
-	} else if (hub->descriptor->bNbrPorts > USB_MAXCHILDREN) {
+	}
+
+	maxchild = USB_MAXCHILDREN;
+	if (hub_is_superspeed(hdev))
+		maxchild = min_t(unsigned, maxchild, USB_SS_MAXPORTS);
+
+	if (hub->descriptor->bNbrPorts > maxchild) {
 		message = "hub has too many ports!";
 		ret = -ENODEV;
 		goto fail;
diff --git a/drivers/usb/core/of.c b/drivers/usb/core/of.c
index d787f195a9a6..d563cbcf76cf 100644
--- a/drivers/usb/core/of.c
+++ b/drivers/usb/core/of.c
@@ -53,6 +53,9 @@ EXPORT_SYMBOL_GPL(usb_of_get_child_node);
  *
  * Find the companion device from platform bus.
  *
+ * Takes a reference to the returned struct device which needs to be dropped
+ * after use.
+ *
  * Return: On success, a pointer to the companion device, %NULL on failure.
  */
 struct device *usb_of_get_companion_dev(struct device *dev)
diff --git a/drivers/usb/core/urb.c b/drivers/usb/core/urb.c
index d75cb8c0f7df..47903d510955 100644
--- a/drivers/usb/core/urb.c
+++ b/drivers/usb/core/urb.c
@@ -338,7 +338,7 @@ int usb_submit_urb(struct urb *urb, gfp_t mem_flags)
 	if (!urb || !urb->complete)
 		return -EINVAL;
 	if (urb->hcpriv) {
-		WARN_ONCE(1, "URB %p submitted while active\n", urb);
+		WARN_ONCE(1, "URB %pK submitted while active\n", urb);
 		return -EBUSY;
 	}
 
diff --git a/drivers/usb/dwc3/dwc3-keystone.c b/drivers/usb/dwc3/dwc3-keystone.c
index 72664700b8a2..12ee23f53cdd 100644
--- a/drivers/usb/dwc3/dwc3-keystone.c
+++ b/drivers/usb/dwc3/dwc3-keystone.c
@@ -107,6 +107,10 @@ static int kdwc3_probe(struct platform_device *pdev)
 		return PTR_ERR(kdwc->usbss);
 
 	kdwc->clk = devm_clk_get(kdwc->dev, "usb");
+	if (IS_ERR(kdwc->clk)) {
+		dev_err(kdwc->dev, "unable to get usb clock\n");
+		return PTR_ERR(kdwc->clk);
+	}
 
 	error = clk_prepare_enable(kdwc->clk);
 	if (error < 0) {
diff --git a/drivers/usb/dwc3/dwc3-pci.c b/drivers/usb/dwc3/dwc3-pci.c
index a15ec71d0423..84a2cebfc712 100644
--- a/drivers/usb/dwc3/dwc3-pci.c
+++ b/drivers/usb/dwc3/dwc3-pci.c
@@ -39,6 +39,8 @@
 #define PCI_DEVICE_ID_INTEL_APL			0x5aaa
 #define PCI_DEVICE_ID_INTEL_KBP			0xa2b0
 #define PCI_DEVICE_ID_INTEL_GLK			0x31aa
+#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_FUNC_PMU_PWR	4
@@ -270,6 +272,8 @@ static const struct pci_device_id dwc3_pci_id_table[] = {
 	{ PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_APL), },
 	{ PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_KBP), },
 	{ PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_GLK), },
+	{ PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_CNPLP), },
+	{ PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_CNPH), },
 	{ PCI_DEVICE(PCI_VENDOR_ID_AMD, PCI_DEVICE_ID_AMD_NL_USB), },
 	{  }	/* Terminating Entry */
 };
diff --git a/drivers/usb/dwc3/gadget.c b/drivers/usb/dwc3/gadget.c
index 6f6f0b3be3ad..aea9a5b948b4 100644
--- a/drivers/usb/dwc3/gadget.c
+++ b/drivers/usb/dwc3/gadget.c
@@ -1261,14 +1261,24 @@ static int __dwc3_gadget_ep_queue(struct dwc3_ep *dep, struct dwc3_request *req)
 				__dwc3_gadget_start_isoc(dwc, dep, cur_uf);
 				dep->flags &= ~DWC3_EP_PENDING_REQUEST;
 			}
+			return 0;
 		}
-		return 0;
+
+		if ((dep->flags & DWC3_EP_BUSY) &&
+		    !(dep->flags & DWC3_EP_MISSED_ISOC)) {
+			WARN_ON_ONCE(!dep->resource_index);
+			ret = __dwc3_gadget_kick_transfer(dep,
+							  dep->resource_index);
+		}
+
+		goto out;
 	}
 
 	if (!dwc3_calc_trbs_left(dep))
 		return 0;
 
 	ret = __dwc3_gadget_kick_transfer(dep, 0);
+out:
 	if (ret == -EBUSY)
 		ret = 0;
 
@@ -3026,6 +3036,15 @@ static irqreturn_t dwc3_check_event_buf(struct dwc3_event_buffer *evt)
 		return IRQ_HANDLED;
 	}
 
+	/*
+	 * With PCIe legacy interrupt, test shows that top-half irq handler can
+	 * be called again after HW interrupt deassertion. Check if bottom-half
+	 * irq event handler completes before caching new event to prevent
+	 * losing events.
+	 */
+	if (evt->flags & DWC3_EVENT_PENDING)
+		return IRQ_HANDLED;
+
 	count = dwc3_readl(dwc->regs, DWC3_GEVNTCOUNT(0));
 	count &= DWC3_GEVNTCOUNT_MASK;
 	if (!count)
diff --git a/drivers/usb/gadget/Kconfig b/drivers/usb/gadget/Kconfig
index c164d6b788c3..b3c879b75a39 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
 
diff --git a/drivers/usb/gadget/function/f_fs.c b/drivers/usb/gadget/function/f_fs.c
index 71dd27c0d7f2..47dda3450abd 100644
--- a/drivers/usb/gadget/function/f_fs.c
+++ b/drivers/usb/gadget/function/f_fs.c
@@ -1858,12 +1858,12 @@ static int ffs_func_eps_enable(struct ffs_function *func)
 		ep->ep->driver_data = ep;
 		ep->ep->desc = ds;
 
-		comp_desc = (struct usb_ss_ep_comp_descriptor *)(ds +
-				USB_DT_ENDPOINT_SIZE);
-		ep->ep->maxburst = comp_desc->bMaxBurst + 1;
-
-		if (needs_comp_desc)
+		if (needs_comp_desc) {
+			comp_desc = (struct usb_ss_ep_comp_descriptor *)(ds +
+					USB_DT_ENDPOINT_SIZE);
+			ep->ep->maxburst = comp_desc->bMaxBurst + 1;
 			ep->ep->comp_desc = comp_desc;
+		}
 
 		ret = usb_ep_enable(ep->ep);
 		if (likely(!ret)) {
diff --git a/drivers/usb/gadget/function/u_serial.c b/drivers/usb/gadget/function/u_serial.c
index 000677c991b0..9b0805f55ad7 100644
--- a/drivers/usb/gadget/function/u_serial.c
+++ b/drivers/usb/gadget/function/u_serial.c
@@ -1256,7 +1256,7 @@ static void gserial_console_exit(void)
 	struct gscons_info *info = &gscons_info;
 
 	unregister_console(&gserial_cons);
-	if (info->console_thread != NULL)
+	if (!IS_ERR_OR_NULL(info->console_thread))
 		kthread_stop(info->console_thread);
 	gs_buf_free(&info->con_buf);
 }
diff --git a/drivers/usb/gadget/udc/dummy_hcd.c b/drivers/usb/gadget/udc/dummy_hcd.c
index c79081952ea0..ccabb51cb98d 100644
--- a/drivers/usb/gadget/udc/dummy_hcd.c
+++ b/drivers/usb/gadget/udc/dummy_hcd.c
@@ -2008,7 +2008,7 @@ ss_hub_descriptor(struct usb_hub_descriptor *desc)
 			HUB_CHAR_COMMON_OCPM);
 	desc->bNbrPorts = 1;
 	desc->u.ss.bHubHdrDecLat = 0x04; /* Worst case: 0.4 micro sec*/
-	desc->u.ss.DeviceRemovable = 0xffff;
+	desc->u.ss.DeviceRemovable = 0;
 }
 
 static inline void hub_descriptor(struct usb_hub_descriptor *desc)
@@ -2020,8 +2020,8 @@ static inline void hub_descriptor(struct usb_hub_descriptor *desc)
 			HUB_CHAR_INDV_PORT_LPSM |
 			HUB_CHAR_COMMON_OCPM);
 	desc->bNbrPorts = 1;
-	desc->u.hs.DeviceRemovable[0] = 0xff;
-	desc->u.hs.DeviceRemovable[1] = 0xff;
+	desc->u.hs.DeviceRemovable[0] = 0;
+	desc->u.hs.DeviceRemovable[1] = 0xff;	/* PortPwrCtrlMask */
 }
 
 static int dummy_hub_control(
diff --git a/drivers/usb/host/ehci-platform.c b/drivers/usb/host/ehci-platform.c
index bc7b9be12f54..f1908ea9fbd8 100644
--- a/drivers/usb/host/ehci-platform.c
+++ b/drivers/usb/host/ehci-platform.c
@@ -384,8 +384,10 @@ static int ehci_platform_resume(struct device *dev)
 	}
 
 	companion_dev = usb_of_get_companion_dev(hcd->self.controller);
-	if (companion_dev)
+	if (companion_dev) {
 		device_pm_wait_for_dev(hcd->self.controller, companion_dev);
+		put_device(companion_dev);
+	}
 
 	ehci_resume(hcd, priv->reset_on_resume);
 	return 0;
diff --git a/drivers/usb/host/r8a66597-hcd.c b/drivers/usb/host/r8a66597-hcd.c
index bfa7fa3d2eea..7bf78be1fd32 100644
--- a/drivers/usb/host/r8a66597-hcd.c
+++ b/drivers/usb/host/r8a66597-hcd.c
@@ -1269,7 +1269,7 @@ static void set_td_timer(struct r8a66597 *r8a66597, struct r8a66597_td *td)
 			time = 30;
 			break;
 		default:
-			time = 300;
+			time = 50;
 			break;
 		}
 
@@ -1785,6 +1785,7 @@ static void r8a66597_td_timer(unsigned long _r8a66597)
 		pipe = td->pipe;
 		pipe_stop(r8a66597, pipe);
 
+		/* Select a different address or endpoint */
 		new_td = td;
 		do {
 			list_move_tail(&new_td->queue,
@@ -1794,7 +1795,8 @@ static void r8a66597_td_timer(unsigned long _r8a66597)
 				new_td = td;
 				break;
 			}
-		} while (td != new_td && td->address == new_td->address);
+		} while (td != new_td && td->address == new_td->address &&
+			td->pipe->info.epnum == new_td->pipe->info.epnum);
 
 		start_transfer(r8a66597, new_td);
 
diff --git a/drivers/usb/host/xhci-hub.c b/drivers/usb/host/xhci-hub.c
index 5e3e9d4c6956..0dde49c35dd2 100644
--- a/drivers/usb/host/xhci-hub.c
+++ b/drivers/usb/host/xhci-hub.c
@@ -419,7 +419,7 @@ static int xhci_stop_device(struct xhci_hcd *xhci, int slot_id, int suspend)
 	wait_for_completion(cmd->completion);
 
 	if (cmd->status == COMP_COMMAND_ABORTED ||
-			cmd->status == COMP_STOPPED) {
+	    cmd->status == COMP_COMMAND_RING_STOPPED) {
 		xhci_warn(xhci, "Timeout while waiting for stop endpoint command\n");
 		ret = -ETIME;
 	}
diff --git a/drivers/usb/host/xhci-mem.c b/drivers/usb/host/xhci-mem.c
index bbe22bcc550a..1f1687e888d6 100644
--- a/drivers/usb/host/xhci-mem.c
+++ b/drivers/usb/host/xhci-mem.c
@@ -56,7 +56,7 @@ static struct xhci_segment *xhci_segment_alloc(struct xhci_hcd *xhci,
 	}
 
 	if (max_packet) {
-		seg->bounce_buf = kzalloc(max_packet, flags | GFP_DMA);
+		seg->bounce_buf = kzalloc(max_packet, flags);
 		if (!seg->bounce_buf) {
 			dma_pool_free(xhci->segment_pool, seg->trbs, dma);
 			kfree(seg);
@@ -1724,7 +1724,7 @@ static int scratchpad_alloc(struct xhci_hcd *xhci, gfp_t flags)
 	xhci->dcbaa->dev_context_ptrs[0] = cpu_to_le64(xhci->scratchpad->sp_dma);
 	for (i = 0; i < num_sp; i++) {
 		dma_addr_t dma;
-		void *buf = dma_alloc_coherent(dev, xhci->page_size, &dma,
+		void *buf = dma_zalloc_coherent(dev, xhci->page_size, &dma,
 				flags);
 		if (!buf)
 			goto fail_sp4;
@@ -2307,10 +2307,11 @@ static int xhci_setup_port_arrays(struct xhci_hcd *xhci, gfp_t flags)
 	/* Place limits on the number of roothub ports so that the hub
 	 * descriptors aren't longer than the USB core will allocate.
 	 */
-	if (xhci->num_usb3_ports > 15) {
+	if (xhci->num_usb3_ports > USB_SS_MAXPORTS) {
 		xhci_dbg_trace(xhci, trace_xhci_dbg_init,
-				"Limiting USB 3.0 roothub ports to 15.");
-		xhci->num_usb3_ports = 15;
+				"Limiting USB 3.0 roothub ports to %u.",
+				USB_SS_MAXPORTS);
+		xhci->num_usb3_ports = USB_SS_MAXPORTS;
 	}
 	if (xhci->num_usb2_ports > USB_MAXCHILDREN) {
 		xhci_dbg_trace(xhci, trace_xhci_dbg_init,
diff --git a/drivers/usb/host/xhci-pci.c b/drivers/usb/host/xhci-pci.c
index 7b86508ac8cf..fcf1f3f63e7a 100644
--- a/drivers/usb/host/xhci-pci.c
+++ b/drivers/usb/host/xhci-pci.c
@@ -52,6 +52,7 @@
 #define PCI_DEVICE_ID_INTEL_BROXTON_M_XHCI		0x0aa8
 #define PCI_DEVICE_ID_INTEL_BROXTON_B_XHCI		0x1aa8
 #define PCI_DEVICE_ID_INTEL_APL_XHCI			0x5aa8
+#define PCI_DEVICE_ID_INTEL_DNV_XHCI			0x19d0
 
 static const char hcd_name[] = "xhci_hcd";
 
@@ -166,7 +167,8 @@ static void xhci_pci_quirks(struct device *dev, struct xhci_hcd *xhci)
 		 pdev->device == PCI_DEVICE_ID_INTEL_CHERRYVIEW_XHCI ||
 		 pdev->device == PCI_DEVICE_ID_INTEL_BROXTON_M_XHCI ||
 		 pdev->device == PCI_DEVICE_ID_INTEL_BROXTON_B_XHCI ||
-		 pdev->device == PCI_DEVICE_ID_INTEL_APL_XHCI)) {
+		 pdev->device == PCI_DEVICE_ID_INTEL_APL_XHCI ||
+		 pdev->device == PCI_DEVICE_ID_INTEL_DNV_XHCI)) {
 		xhci->quirks |= XHCI_PME_STUCK_QUIRK;
 	}
 	if (pdev->vendor == PCI_VENDOR_ID_INTEL &&
@@ -175,7 +177,8 @@ static void xhci_pci_quirks(struct device *dev, struct xhci_hcd *xhci)
 	}
 	if (pdev->vendor == PCI_VENDOR_ID_INTEL &&
 	    (pdev->device == PCI_DEVICE_ID_INTEL_CHERRYVIEW_XHCI ||
-	     pdev->device == PCI_DEVICE_ID_INTEL_APL_XHCI))
+	     pdev->device == PCI_DEVICE_ID_INTEL_APL_XHCI ||
+	     pdev->device == PCI_DEVICE_ID_INTEL_DNV_XHCI))
 		xhci->quirks |= XHCI_MISSING_CAS;
 
 	if (pdev->vendor == PCI_VENDOR_ID_ETRON &&
diff --git a/drivers/usb/host/xhci-plat.c b/drivers/usb/host/xhci-plat.c
index 7c2a9e7c8e0f..c04144b25a67 100644
--- a/drivers/usb/host/xhci-plat.c
+++ b/drivers/usb/host/xhci-plat.c
@@ -177,7 +177,7 @@ static int xhci_plat_probe(struct platform_device *pdev)
 
 	irq = platform_get_irq(pdev, 0);
 	if (irq < 0)
-		return -ENODEV;
+		return irq;
 
 	/*
 	 * sysdev must point to a device that is known to the system firmware
diff --git a/drivers/usb/host/xhci-ring.c b/drivers/usb/host/xhci-ring.c
index 74bf5c60a260..03f63f50afb6 100644
--- a/drivers/usb/host/xhci-ring.c
+++ b/drivers/usb/host/xhci-ring.c
@@ -323,7 +323,7 @@ static void xhci_handle_stopped_cmd_ring(struct xhci_hcd *xhci,
 		if (i_cmd->status != COMP_COMMAND_ABORTED)
 			continue;
 
-		i_cmd->status = COMP_STOPPED;
+		i_cmd->status = COMP_COMMAND_RING_STOPPED;
 
 		xhci_dbg(xhci, "Turn aborted command %p to no-op\n",
 			 i_cmd->command_trb);
@@ -641,8 +641,8 @@ static void xhci_giveback_urb_in_irq(struct xhci_hcd *xhci,
 	xhci_urb_free_priv(urb_priv);
 	usb_hcd_unlink_urb_from_ep(hcd, urb);
 	spin_unlock(&xhci->lock);
-	usb_hcd_giveback_urb(hcd, urb, status);
 	trace_xhci_urb_giveback(urb);
+	usb_hcd_giveback_urb(hcd, urb, status);
 	spin_lock(&xhci->lock);
 }
 
@@ -1380,7 +1380,7 @@ static void handle_cmd_completion(struct xhci_hcd *xhci,
 	cmd_comp_code = GET_COMP_CODE(le32_to_cpu(event->status));
 
 	/* If CMD ring stopped we own the trbs between enqueue and dequeue */
-	if (cmd_comp_code == COMP_STOPPED) {
+	if (cmd_comp_code == COMP_COMMAND_RING_STOPPED) {
 		complete_all(&xhci->cmd_ring_stop_completion);
 		return;
 	}
@@ -1436,8 +1436,8 @@ static void handle_cmd_completion(struct xhci_hcd *xhci,
 		break;
 	case TRB_CMD_NOOP:
 		/* Is this an aborted command turned to NO-OP? */
-		if (cmd->status == COMP_STOPPED)
-			cmd_comp_code = COMP_STOPPED;
+		if (cmd->status == COMP_COMMAND_RING_STOPPED)
+			cmd_comp_code = COMP_COMMAND_RING_STOPPED;
 		break;
 	case TRB_RESET_EP:
 		WARN_ON(slot_id != TRB_TO_SLOT_ID(
@@ -2677,11 +2677,12 @@ irqreturn_t xhci_irq(struct usb_hcd *hcd)
 	struct xhci_hcd *xhci = hcd_to_xhci(hcd);
 	union xhci_trb *event_ring_deq;
 	irqreturn_t ret = IRQ_NONE;
+	unsigned long flags;
 	dma_addr_t deq;
 	u64 temp_64;
 	u32 status;
 
-	spin_lock(&xhci->lock);
+	spin_lock_irqsave(&xhci->lock, flags);
 	/* Check if the xHC generated the interrupt, or the irq is shared */
 	status = readl(&xhci->op_regs->status);
 	if (status == ~(u32)0) {
@@ -2707,12 +2708,9 @@ irqreturn_t xhci_irq(struct usb_hcd *hcd)
 	 */
 	status |= STS_EINT;
 	writel(status, &xhci->op_regs->status);
-	/* FIXME when MSI-X is supported and there are multiple vectors */
-	/* Clear the MSI-X event interrupt status */
 
-	if (hcd->irq) {
+	if (!hcd->msi_enabled) {
 		u32 irq_pending;
-		/* Acknowledge the PCI interrupt */
 		irq_pending = readl(&xhci->ir_set->irq_pending);
 		irq_pending |= IMAN_IP;
 		writel(irq_pending, &xhci->ir_set->irq_pending);
@@ -2757,7 +2755,7 @@ irqreturn_t xhci_irq(struct usb_hcd *hcd)
 	ret = IRQ_HANDLED;
 
 out:
-	spin_unlock(&xhci->lock);
+	spin_unlock_irqrestore(&xhci->lock, flags);
 
 	return ret;
 }
diff --git a/drivers/usb/host/xhci.c b/drivers/usb/host/xhci.c
index 2d1310220832..30f47d92a610 100644
--- a/drivers/usb/host/xhci.c
+++ b/drivers/usb/host/xhci.c
@@ -359,9 +359,10 @@ static int xhci_try_enable_msi(struct usb_hcd *hcd)
 		/* fall back to msi*/
 		ret = xhci_setup_msi(xhci);
 
-	if (!ret)
-		/* hcd->irq is 0, we have MSI */
+	if (!ret) {
+		hcd->msi_enabled = 1;
 		return 0;
+	}
 
 	if (!pdev->irq) {
 		xhci_err(xhci, "No msi-x/msi found and no IRQ in BIOS\n");
@@ -1763,7 +1764,7 @@ static int xhci_configure_endpoint_result(struct xhci_hcd *xhci,
 
 	switch (*cmd_status) {
 	case COMP_COMMAND_ABORTED:
-	case COMP_STOPPED:
+	case COMP_COMMAND_RING_STOPPED:
 		xhci_warn(xhci, "Timeout while waiting for configure endpoint command\n");
 		ret = -ETIME;
 		break;
@@ -1813,7 +1814,7 @@ static int xhci_evaluate_context_result(struct xhci_hcd *xhci,
 
 	switch (*cmd_status) {
 	case COMP_COMMAND_ABORTED:
-	case COMP_STOPPED:
+	case COMP_COMMAND_RING_STOPPED:
 		xhci_warn(xhci, "Timeout while waiting for evaluate context command\n");
 		ret = -ETIME;
 		break;
@@ -3432,7 +3433,7 @@ static int xhci_discover_or_reset_device(struct usb_hcd *hcd,
 	ret = reset_device_cmd->status;
 	switch (ret) {
 	case COMP_COMMAND_ABORTED:
-	case COMP_STOPPED:
+	case COMP_COMMAND_RING_STOPPED:
 		xhci_warn(xhci, "Timeout waiting for reset device command\n");
 		ret = -ETIME;
 		goto command_cleanup;
@@ -3817,7 +3818,7 @@ static int xhci_setup_device(struct usb_hcd *hcd, struct usb_device *udev,
 	 */
 	switch (command->status) {
 	case COMP_COMMAND_ABORTED:
-	case COMP_STOPPED:
+	case COMP_COMMAND_RING_STOPPED:
 		xhci_warn(xhci, "Timeout while waiting for setup device command\n");
 		ret = -ETIME;
 		break;
diff --git a/drivers/usb/misc/chaoskey.c b/drivers/usb/misc/chaoskey.c
index e9cae4d82af2..15d4e64d3b65 100644
--- a/drivers/usb/misc/chaoskey.c
+++ b/drivers/usb/misc/chaoskey.c
@@ -192,7 +192,7 @@ static int chaoskey_probe(struct usb_interface *interface,
 
 	dev->in_ep = in_ep;
 
-	if (udev->descriptor.idVendor != ALEA_VENDOR_ID)
+	if (le16_to_cpu(udev->descriptor.idVendor) != ALEA_VENDOR_ID)
 		dev->reads_started = 1;
 
 	dev->size = size;
diff --git a/drivers/usb/misc/iowarrior.c b/drivers/usb/misc/iowarrior.c
index 77569531b78a..83b05a287b0c 100644
--- a/drivers/usb/misc/iowarrior.c
+++ b/drivers/usb/misc/iowarrior.c
@@ -554,7 +554,7 @@ static long iowarrior_ioctl(struct file *file, unsigned int cmd,
 			info.revision = le16_to_cpu(dev->udev->descriptor.bcdDevice);
 
 			/* 0==UNKNOWN, 1==LOW(usb1.1) ,2=FULL(usb1.1), 3=HIGH(usb2.0) */
-			info.speed = le16_to_cpu(dev->udev->speed);
+			info.speed = dev->udev->speed;
 			info.if_num = dev->interface->cur_altsetting->desc.bInterfaceNumber;
 			info.report_size = dev->report_size;
 
diff --git a/drivers/usb/misc/legousbtower.c b/drivers/usb/misc/legousbtower.c
index aa3c280fdf8d..0782ac6f5edf 100644
--- a/drivers/usb/misc/legousbtower.c
+++ b/drivers/usb/misc/legousbtower.c
@@ -926,6 +926,7 @@ static int tower_probe (struct usb_interface *interface, const struct usb_device
 		 USB_MAJOR, dev->minor);
 
 exit:
+	kfree(get_version_reply);
 	return retval;
 
 error:
diff --git a/drivers/usb/misc/sisusbvga/sisusb_con.c b/drivers/usb/misc/sisusbvga/sisusb_con.c
index 3c6948af726a..f019d80ca9e4 100644
--- a/drivers/usb/misc/sisusbvga/sisusb_con.c
+++ b/drivers/usb/misc/sisusbvga/sisusb_con.c
@@ -973,7 +973,7 @@ sisusbcon_set_origin(struct vc_data *c)
 
 	mutex_unlock(&sisusb->lock);
 
-	return 1;
+	return true;
 }
 
 /* Interface routine */
diff --git a/drivers/usb/musb/musb_dsps.c b/drivers/usb/musb/musb_dsps.c
index 9c7ee26ef388..bc6a9be2ccc5 100644
--- a/drivers/usb/musb/musb_dsps.c
+++ b/drivers/usb/musb/musb_dsps.c
@@ -245,6 +245,11 @@ static int dsps_check_status(struct musb *musb, void *unused)
 		dsps_mod_timer_optional(glue);
 		break;
 	case OTG_STATE_A_WAIT_BCON:
+		/* keep VBUS on for host-only mode */
+		if (musb->port_mode == MUSB_PORT_MODE_HOST) {
+			dsps_mod_timer_optional(glue);
+			break;
+		}
 		musb_writeb(musb->mregs, MUSB_DEVCTL, 0);
 		skip_session = 1;
 		/* fall */
diff --git a/drivers/usb/musb/musb_host.c b/drivers/usb/musb/musb_host.c
index ac3a4952abb4..dbe617a735d8 100644
--- a/drivers/usb/musb/musb_host.c
+++ b/drivers/usb/musb/musb_host.c
@@ -2780,10 +2780,11 @@ int musb_host_setup(struct musb *musb, int power_budget)
 	int ret;
 	struct usb_hcd *hcd = musb->hcd;
 
-	MUSB_HST_MODE(musb);
-	musb->xceiv->otg->default_a = 1;
-	musb->xceiv->otg->state = OTG_STATE_A_IDLE;
-
+	if (musb->port_mode == MUSB_PORT_MODE_HOST) {
+		MUSB_HST_MODE(musb);
+		musb->xceiv->otg->default_a = 1;
+		musb->xceiv->otg->state = OTG_STATE_A_IDLE;
+	}
 	otg_set_host(musb->xceiv->otg, &hcd->self);
 	hcd->self.otg_port = 1;
 	musb->xceiv->otg->host = &hcd->self;
diff --git a/drivers/usb/musb/tusb6010_omap.c b/drivers/usb/musb/tusb6010_omap.c
index 8b43c4b99f04..7870b37e0ea5 100644
--- a/drivers/usb/musb/tusb6010_omap.c
+++ b/drivers/usb/musb/tusb6010_omap.c
@@ -219,6 +219,7 @@ static int tusb_omap_dma_program(struct dma_channel *channel, u16 packet_sz,
 	u32				dma_remaining;
 	int				src_burst, dst_burst;
 	u16				csr;
+	u32				psize;
 	int				ch;
 	s8				dmareq;
 	s8				sync_dev;
@@ -390,15 +391,19 @@ static int tusb_omap_dma_program(struct dma_channel *channel, u16 packet_sz,
 
 	if (chdat->tx) {
 		/* Send transfer_packet_sz packets at a time */
-		musb_writel(ep_conf, TUSB_EP_MAX_PACKET_SIZE_OFFSET,
-			chdat->transfer_packet_sz);
+		psize = musb_readl(ep_conf, TUSB_EP_MAX_PACKET_SIZE_OFFSET);
+		psize &= ~0x7ff;
+		psize |= chdat->transfer_packet_sz;
+		musb_writel(ep_conf, TUSB_EP_MAX_PACKET_SIZE_OFFSET, psize);
 
 		musb_writel(ep_conf, TUSB_EP_TX_OFFSET,
 			TUSB_EP_CONFIG_XFR_SIZE(chdat->transfer_len));
 	} else {
 		/* Receive transfer_packet_sz packets at a time */
-		musb_writel(ep_conf, TUSB_EP_MAX_PACKET_SIZE_OFFSET,
-			chdat->transfer_packet_sz << 16);
+		psize = musb_readl(ep_conf, TUSB_EP_MAX_PACKET_SIZE_OFFSET);
+		psize &= ~(0x7ff << 16);
+		psize |= (chdat->transfer_packet_sz << 16);
+		musb_writel(ep_conf, TUSB_EP_MAX_PACKET_SIZE_OFFSET, psize);
 
 		musb_writel(ep_conf, TUSB_EP_RX_OFFSET,
 			TUSB_EP_CONFIG_XFR_SIZE(chdat->transfer_len));
diff --git a/drivers/usb/serial/ftdi_sio.c b/drivers/usb/serial/ftdi_sio.c
index d38780fa8788..aba74f817dc6 100644
--- a/drivers/usb/serial/ftdi_sio.c
+++ b/drivers/usb/serial/ftdi_sio.c
@@ -809,10 +809,10 @@ static const struct usb_device_id id_table_combined[] = {
 	{ USB_DEVICE(FTDI_VID, FTDI_PROPOX_ISPCABLEIII_PID) },
 	{ USB_DEVICE(FTDI_VID, CYBER_CORTEX_AV_PID),
 		.driver_info = (kernel_ulong_t)&ftdi_jtag_quirk },
-	{ USB_DEVICE(OLIMEX_VID, OLIMEX_ARM_USB_OCD_PID),
-		.driver_info = (kernel_ulong_t)&ftdi_jtag_quirk },
-	{ USB_DEVICE(OLIMEX_VID, OLIMEX_ARM_USB_OCD_H_PID),
-		.driver_info = (kernel_ulong_t)&ftdi_jtag_quirk },
+	{ USB_DEVICE_INTERFACE_NUMBER(OLIMEX_VID, OLIMEX_ARM_USB_OCD_PID, 1) },
+	{ USB_DEVICE_INTERFACE_NUMBER(OLIMEX_VID, OLIMEX_ARM_USB_OCD_H_PID, 1) },
+	{ USB_DEVICE_INTERFACE_NUMBER(OLIMEX_VID, OLIMEX_ARM_USB_TINY_PID, 1) },
+	{ USB_DEVICE_INTERFACE_NUMBER(OLIMEX_VID, OLIMEX_ARM_USB_TINY_H_PID, 1) },
 	{ USB_DEVICE(FIC_VID, FIC_NEO1973_DEBUG_PID),
 		.driver_info = (kernel_ulong_t)&ftdi_jtag_quirk },
 	{ USB_DEVICE(FTDI_VID, FTDI_OOCDLINK_PID),
@@ -1527,9 +1527,9 @@ static int set_serial_info(struct tty_struct *tty,
 					(new_serial.flags & ASYNC_FLAGS));
 	priv->custom_divisor = new_serial.custom_divisor;
 
+check_and_exit:
 	write_latency_timer(port);
 
-check_and_exit:
 	if ((old_priv.flags & ASYNC_SPD_MASK) !=
 	     (priv->flags & ASYNC_SPD_MASK)) {
 		if ((priv->flags & ASYNC_SPD_MASK) == ASYNC_SPD_HI)
diff --git a/drivers/usb/serial/ftdi_sio_ids.h b/drivers/usb/serial/ftdi_sio_ids.h
index 71fb9e59db71..4fcf1cecb6d7 100644
--- a/drivers/usb/serial/ftdi_sio_ids.h
+++ b/drivers/usb/serial/ftdi_sio_ids.h
@@ -882,6 +882,8 @@
 /* Olimex */
 #define OLIMEX_VID			0x15BA
 #define OLIMEX_ARM_USB_OCD_PID		0x0003
+#define OLIMEX_ARM_USB_TINY_PID	0x0004
+#define OLIMEX_ARM_USB_TINY_H_PID	0x002a
 #define OLIMEX_ARM_USB_OCD_H_PID	0x002b
 
 /*
diff --git a/drivers/usb/serial/io_ti.c b/drivers/usb/serial/io_ti.c
index 87798e625d6c..6cefb9cb133d 100644
--- a/drivers/usb/serial/io_ti.c
+++ b/drivers/usb/serial/io_ti.c
@@ -2336,8 +2336,11 @@ static void change_port_settings(struct tty_struct *tty,
 	if (!baud) {
 		/* pick a default, any default... */
 		baud = 9600;
-	} else
+	} else {
+		/* Avoid a zero divisor. */
+		baud = min(baud, 461550);
 		tty_encode_baud_rate(tty, baud, baud);
+	}
 
 	edge_port->baud_rate = baud;
 	config->wBaudRate = (__u16)((461550L + baud/2) / baud);
diff --git a/drivers/usb/serial/ir-usb.c b/drivers/usb/serial/ir-usb.c
index 73956d48a0c5..f9734a96d516 100644
--- a/drivers/usb/serial/ir-usb.c
+++ b/drivers/usb/serial/ir-usb.c
@@ -197,6 +197,7 @@ static u8 ir_xbof_change(u8 xbof)
 static int ir_startup(struct usb_serial *serial)
 {
 	struct usb_irda_cs_descriptor *irda_desc;
+	int rates;
 
 	irda_desc = irda_usb_find_class_desc(serial, 0);
 	if (!irda_desc) {
@@ -205,18 +206,20 @@ static int ir_startup(struct usb_serial *serial)
 		return -ENODEV;
 	}
 
+	rates = le16_to_cpu(irda_desc->wBaudRate);
+
 	dev_dbg(&serial->dev->dev,
 		"%s - Baud rates supported:%s%s%s%s%s%s%s%s%s\n",
 		__func__,
-		(irda_desc->wBaudRate & USB_IRDA_BR_2400) ? " 2400" : "",
-		(irda_desc->wBaudRate & USB_IRDA_BR_9600) ? " 9600" : "",
-		(irda_desc->wBaudRate & USB_IRDA_BR_19200) ? " 19200" : "",
-		(irda_desc->wBaudRate & USB_IRDA_BR_38400) ? " 38400" : "",
-		(irda_desc->wBaudRate & USB_IRDA_BR_57600) ? " 57600" : "",
-		(irda_desc->wBaudRate & USB_IRDA_BR_115200) ? " 115200" : "",
-		(irda_desc->wBaudRate & USB_IRDA_BR_576000) ? " 576000" : "",
-		(irda_desc->wBaudRate & USB_IRDA_BR_1152000) ? " 1152000" : "",
-		(irda_desc->wBaudRate & USB_IRDA_BR_4000000) ? " 4000000" : "");
+		(rates & USB_IRDA_BR_2400) ? " 2400" : "",
+		(rates & USB_IRDA_BR_9600) ? " 9600" : "",
+		(rates & USB_IRDA_BR_19200) ? " 19200" : "",
+		(rates & USB_IRDA_BR_38400) ? " 38400" : "",
+		(rates & USB_IRDA_BR_57600) ? " 57600" : "",
+		(rates & USB_IRDA_BR_115200) ? " 115200" : "",
+		(rates & USB_IRDA_BR_576000) ? " 576000" : "",
+		(rates & USB_IRDA_BR_1152000) ? " 1152000" : "",
+		(rates & USB_IRDA_BR_4000000) ? " 4000000" : "");
 
 	switch (irda_desc->bmAdditionalBOFs) {
 	case USB_IRDA_AB_48:
diff --git a/drivers/usb/serial/mct_u232.c b/drivers/usb/serial/mct_u232.c
index edbc81f205c2..70f346f1aa86 100644
--- a/drivers/usb/serial/mct_u232.c
+++ b/drivers/usb/serial/mct_u232.c
@@ -189,7 +189,7 @@ static int mct_u232_set_baud_rate(struct tty_struct *tty,
 		return -ENOMEM;
 
 	divisor = mct_u232_calculate_baud_rate(serial, value, &speed);
-	put_unaligned_le32(cpu_to_le32(divisor), buf);
+	put_unaligned_le32(divisor, buf);
 	rc = usb_control_msg(serial->dev, usb_sndctrlpipe(serial->dev, 0),
 				MCT_U232_SET_BAUD_RATE_REQUEST,
 				MCT_U232_SET_REQUEST_TYPE,
diff --git a/drivers/usb/serial/option.c b/drivers/usb/serial/option.c
index af67a0de6b5d..3bf61acfc26b 100644
--- a/drivers/usb/serial/option.c
+++ b/drivers/usb/serial/option.c
@@ -281,6 +281,7 @@ static void option_instat_callback(struct urb *urb);
 #define TELIT_PRODUCT_LE922_USBCFG0		0x1042
 #define TELIT_PRODUCT_LE922_USBCFG3		0x1043
 #define TELIT_PRODUCT_LE922_USBCFG5		0x1045
+#define TELIT_PRODUCT_ME910			0x1100
 #define TELIT_PRODUCT_LE920			0x1200
 #define TELIT_PRODUCT_LE910			0x1201
 #define TELIT_PRODUCT_LE910_USBCFG4		0x1206
@@ -640,6 +641,11 @@ static const struct option_blacklist_info simcom_sim7100e_blacklist = {
 	.reserved = BIT(5) | BIT(6),
 };
 
+static const struct option_blacklist_info telit_me910_blacklist = {
+	.sendsetup = BIT(0),
+	.reserved = BIT(1) | BIT(3),
+};
+
 static const struct option_blacklist_info telit_le910_blacklist = {
 	.sendsetup = BIT(0),
 	.reserved = BIT(1) | BIT(2),
@@ -1235,6 +1241,8 @@ static const struct usb_device_id option_ids[] = {
 		.driver_info = (kernel_ulong_t)&telit_le922_blacklist_usbcfg3 },
 	{ USB_DEVICE_INTERFACE_CLASS(TELIT_VENDOR_ID, TELIT_PRODUCT_LE922_USBCFG5, 0xff),
 		.driver_info = (kernel_ulong_t)&telit_le922_blacklist_usbcfg0 },
+	{ USB_DEVICE(TELIT_VENDOR_ID, TELIT_PRODUCT_ME910),
+		.driver_info = (kernel_ulong_t)&telit_me910_blacklist },
 	{ USB_DEVICE(TELIT_VENDOR_ID, TELIT_PRODUCT_LE910),
 		.driver_info = (kernel_ulong_t)&telit_le910_blacklist },
 	{ USB_DEVICE(TELIT_VENDOR_ID, TELIT_PRODUCT_LE910_USBCFG4),
diff --git a/drivers/usb/serial/qcserial.c b/drivers/usb/serial/qcserial.c
index 38b3f0d8cd58..fd509ed6cf70 100644
--- a/drivers/usb/serial/qcserial.c
+++ b/drivers/usb/serial/qcserial.c
@@ -162,6 +162,8 @@ static const struct usb_device_id id_table[] = {
 	{DEVICE_SWI(0x1199, 0x9071)},	/* Sierra Wireless MC74xx */
 	{DEVICE_SWI(0x1199, 0x9078)},	/* Sierra Wireless EM74xx */
 	{DEVICE_SWI(0x1199, 0x9079)},	/* Sierra Wireless EM74xx */
+	{DEVICE_SWI(0x1199, 0x907a)},	/* Sierra Wireless EM74xx QDL */
+	{DEVICE_SWI(0x1199, 0x907b)},	/* Sierra Wireless EM74xx */
 	{DEVICE_SWI(0x413c, 0x81a2)},	/* Dell Wireless 5806 Gobi(TM) 4G LTE Mobile Broadband Card */
 	{DEVICE_SWI(0x413c, 0x81a3)},	/* Dell Wireless 5570 HSPA+ (42Mbps) Mobile Broadband Card */
 	{DEVICE_SWI(0x413c, 0x81a4)},	/* Dell Wireless 5570e HSPA+ (42Mbps) Mobile Broadband Card */
diff --git a/drivers/usb/storage/ene_ub6250.c b/drivers/usb/storage/ene_ub6250.c
index 369f3c24815a..44af719194b2 100644
--- a/drivers/usb/storage/ene_ub6250.c
+++ b/drivers/usb/storage/ene_ub6250.c
@@ -446,6 +446,10 @@ struct ms_lib_ctrl {
 #define SD_BLOCK_LEN  9
 
 struct ene_ub6250_info {
+
+	/* I/O bounce buffer */
+	u8		*bbuf;
+
 	/* for 6250 code */
 	struct SD_STATUS	SD_Status;
 	struct MS_STATUS	MS_Status;
@@ -493,8 +497,11 @@ static int ene_load_bincode(struct us_data *us, unsigned char flag);
 
 static void ene_ub6250_info_destructor(void *extra)
 {
+	struct ene_ub6250_info *info = (struct ene_ub6250_info *) extra;
+
 	if (!extra)
 		return;
+	kfree(info->bbuf);
 }
 
 static int ene_send_scsi_cmd(struct us_data *us, u8 fDir, void *buf, int use_sg)
@@ -860,8 +867,9 @@ static int ms_read_readpage(struct us_data *us, u32 PhyBlockAddr,
 		u8 PageNum, u32 *PageBuf, struct ms_lib_type_extdat *ExtraDat)
 {
 	struct bulk_cb_wrap *bcb = (struct bulk_cb_wrap *) us->iobuf;
+	struct ene_ub6250_info *info = (struct ene_ub6250_info *) us->extra;
+	u8 *bbuf = info->bbuf;
 	int result;
-	u8 ExtBuf[4];
 	u32 bn = PhyBlockAddr * 0x20 + PageNum;
 
 	result = ene_load_bincode(us, MS_RW_PATTERN);
@@ -901,7 +909,7 @@ static int ms_read_readpage(struct us_data *us, u32 PhyBlockAddr,
 	bcb->CDB[2]     = (unsigned char)(PhyBlockAddr>>16);
 	bcb->CDB[6]     = 0x01;
 
-	result = ene_send_scsi_cmd(us, FDIR_READ, &ExtBuf, 0);
+	result = ene_send_scsi_cmd(us, FDIR_READ, bbuf, 0);
 	if (result != USB_STOR_XFER_GOOD)
 		return USB_STOR_TRANSPORT_ERROR;
 
@@ -910,9 +918,9 @@ static int ms_read_readpage(struct us_data *us, u32 PhyBlockAddr,
 	ExtraDat->status0  = 0x10;  /* Not yet,fireware support */
 
 	ExtraDat->status1  = 0x00;  /* Not yet,fireware support */
-	ExtraDat->ovrflg   = ExtBuf[0];
-	ExtraDat->mngflg   = ExtBuf[1];
-	ExtraDat->logadr   = memstick_logaddr(ExtBuf[2], ExtBuf[3]);
+	ExtraDat->ovrflg   = bbuf[0];
+	ExtraDat->mngflg   = bbuf[1];
+	ExtraDat->logadr   = memstick_logaddr(bbuf[2], bbuf[3]);
 
 	return USB_STOR_TRANSPORT_GOOD;
 }
@@ -1332,8 +1340,9 @@ static int ms_lib_read_extra(struct us_data *us, u32 PhyBlock,
 				u8 PageNum, struct ms_lib_type_extdat *ExtraDat)
 {
 	struct bulk_cb_wrap *bcb = (struct bulk_cb_wrap *) us->iobuf;
+	struct ene_ub6250_info *info = (struct ene_ub6250_info *) us->extra;
+	u8 *bbuf = info->bbuf;
 	int result;
-	u8 ExtBuf[4];
 
 	memset(bcb, 0, sizeof(struct bulk_cb_wrap));
 	bcb->Signature = cpu_to_le32(US_BULK_CB_SIGN);
@@ -1347,7 +1356,7 @@ static int ms_lib_read_extra(struct us_data *us, u32 PhyBlock,
 	bcb->CDB[2]     = (unsigned char)(PhyBlock>>16);
 	bcb->CDB[6]     = 0x01;
 
-	result = ene_send_scsi_cmd(us, FDIR_READ, &ExtBuf, 0);
+	result = ene_send_scsi_cmd(us, FDIR_READ, bbuf, 0);
 	if (result != USB_STOR_XFER_GOOD)
 		return USB_STOR_TRANSPORT_ERROR;
 
@@ -1355,9 +1364,9 @@ static int ms_lib_read_extra(struct us_data *us, u32 PhyBlock,
 	ExtraDat->intr     = 0x80;  /* Not yet, waiting for fireware support */
 	ExtraDat->status0  = 0x10;  /* Not yet, waiting for fireware support */
 	ExtraDat->status1  = 0x00;  /* Not yet, waiting for fireware support */
-	ExtraDat->ovrflg   = ExtBuf[0];
-	ExtraDat->mngflg   = ExtBuf[1];
-	ExtraDat->logadr   = memstick_logaddr(ExtBuf[2], ExtBuf[3]);
+	ExtraDat->ovrflg   = bbuf[0];
+	ExtraDat->mngflg   = bbuf[1];
+	ExtraDat->logadr   = memstick_logaddr(bbuf[2], bbuf[3]);
 
 	return USB_STOR_TRANSPORT_GOOD;
 }
@@ -1556,9 +1565,9 @@ static int ms_lib_scan_logicalblocknumber(struct us_data *us, u16 btBlk1st)
 	u16 PhyBlock, newblk, i;
 	u16 LogStart, LogEnde;
 	struct ms_lib_type_extdat extdat;
-	u8 buf[0x200];
 	u32 count = 0, index = 0;
 	struct ene_ub6250_info *info = (struct ene_ub6250_info *) us->extra;
+	u8 *bbuf = info->bbuf;
 
 	for (PhyBlock = 0; PhyBlock < info->MS_Lib.NumberOfPhyBlock;) {
 		ms_lib_phy_to_log_range(PhyBlock, &LogStart, &LogEnde);
@@ -1572,14 +1581,16 @@ static int ms_lib_scan_logicalblocknumber(struct us_data *us, u16 btBlk1st)
 			}
 
 			if (count == PhyBlock) {
-				ms_lib_read_extrablock(us, PhyBlock, 0, 0x80, &buf);
+				ms_lib_read_extrablock(us, PhyBlock, 0, 0x80,
+						bbuf);
 				count += 0x80;
 			}
 			index = (PhyBlock % 0x80) * 4;
 
-			extdat.ovrflg = buf[index];
-			extdat.mngflg = buf[index+1];
-			extdat.logadr = memstick_logaddr(buf[index+2], buf[index+3]);
+			extdat.ovrflg = bbuf[index];
+			extdat.mngflg = bbuf[index+1];
+			extdat.logadr = memstick_logaddr(bbuf[index+2],
+					bbuf[index+3]);
 
 			if ((extdat.ovrflg & MS_REG_OVR_BKST) != MS_REG_OVR_BKST_OK) {
 				ms_lib_setacquired_errorblock(us, PhyBlock);
@@ -2062,9 +2073,9 @@ static int ene_ms_init(struct us_data *us)
 {
 	struct bulk_cb_wrap *bcb = (struct bulk_cb_wrap *) us->iobuf;
 	int result;
-	u8 buf[0x200];
 	u16 MSP_BlockSize, MSP_UserAreaBlocks;
 	struct ene_ub6250_info *info = (struct ene_ub6250_info *) us->extra;
+	u8 *bbuf = info->bbuf;
 
 	printk(KERN_INFO "transport --- ENE_MSInit\n");
 
@@ -2083,13 +2094,13 @@ static int ene_ms_init(struct us_data *us)
 	bcb->CDB[0]     = 0xF1;
 	bcb->CDB[1]     = 0x01;
 
-	result = ene_send_scsi_cmd(us, FDIR_READ, &buf, 0);
+	result = ene_send_scsi_cmd(us, FDIR_READ, bbuf, 0);
 	if (result != USB_STOR_XFER_GOOD) {
 		printk(KERN_ERR "Execution MS Init Code Fail !!\n");
 		return USB_STOR_TRANSPORT_ERROR;
 	}
 	/* the same part to test ENE */
-	info->MS_Status = *(struct MS_STATUS *)&buf[0];
+	info->MS_Status = *(struct MS_STATUS *) bbuf;
 
 	if (info->MS_Status.Insert && info->MS_Status.Ready) {
 		printk(KERN_INFO "Insert     = %x\n", info->MS_Status.Insert);
@@ -2098,15 +2109,15 @@ static int ene_ms_init(struct us_data *us)
 		printk(KERN_INFO "IsMSPHG    = %x\n", info->MS_Status.IsMSPHG);
 		printk(KERN_INFO "WtP= %x\n", info->MS_Status.WtP);
 		if (info->MS_Status.IsMSPro) {
-			MSP_BlockSize      = (buf[6] << 8) | buf[7];
-			MSP_UserAreaBlocks = (buf[10] << 8) | buf[11];
+			MSP_BlockSize      = (bbuf[6] << 8) | bbuf[7];
+			MSP_UserAreaBlocks = (bbuf[10] << 8) | bbuf[11];
 			info->MSP_TotalBlock = MSP_BlockSize * MSP_UserAreaBlocks;
 		} else {
 			ms_card_init(us); /* Card is MS (to ms.c)*/
 		}
 		usb_stor_dbg(us, "MS Init Code OK !!\n");
 	} else {
-		usb_stor_dbg(us, "MS Card Not Ready --- %x\n", buf[0]);
+		usb_stor_dbg(us, "MS Card Not Ready --- %x\n", bbuf[0]);
 		return USB_STOR_TRANSPORT_ERROR;
 	}
 
@@ -2116,9 +2127,9 @@ static int ene_ms_init(struct us_data *us)
 static int ene_sd_init(struct us_data *us)
 {
 	int result;
-	u8  buf[0x200];
 	struct bulk_cb_wrap *bcb = (struct bulk_cb_wrap *) us->iobuf;
 	struct ene_ub6250_info *info = (struct ene_ub6250_info *) us->extra;
+	u8 *bbuf = info->bbuf;
 
 	usb_stor_dbg(us, "transport --- ENE_SDInit\n");
 	/* SD Init Part-1 */
@@ -2152,17 +2163,17 @@ static int ene_sd_init(struct us_data *us)
 	bcb->Flags              = US_BULK_FLAG_IN;
 	bcb->CDB[0]             = 0xF1;
 
-	result = ene_send_scsi_cmd(us, FDIR_READ, &buf, 0);
+	result = ene_send_scsi_cmd(us, FDIR_READ, bbuf, 0);
 	if (result != USB_STOR_XFER_GOOD) {
 		usb_stor_dbg(us, "Execution SD Init Code Fail !!\n");
 		return USB_STOR_TRANSPORT_ERROR;
 	}
 
-	info->SD_Status =  *(struct SD_STATUS *)&buf[0];
+	info->SD_Status =  *(struct SD_STATUS *) bbuf;
 	if (info->SD_Status.Insert && info->SD_Status.Ready) {
 		struct SD_STATUS *s = &info->SD_Status;
 
-		ene_get_card_status(us, (unsigned char *)&buf);
+		ene_get_card_status(us, bbuf);
 		usb_stor_dbg(us, "Insert     = %x\n", s->Insert);
 		usb_stor_dbg(us, "Ready      = %x\n", s->Ready);
 		usb_stor_dbg(us, "IsMMC      = %x\n", s->IsMMC);
@@ -2170,7 +2181,7 @@ static int ene_sd_init(struct us_data *us)
 		usb_stor_dbg(us, "HiSpeed    = %x\n", s->HiSpeed);
 		usb_stor_dbg(us, "WtP        = %x\n", s->WtP);
 	} else {
-		usb_stor_dbg(us, "SD Card Not Ready --- %x\n", buf[0]);
+		usb_stor_dbg(us, "SD Card Not Ready --- %x\n", bbuf[0]);
 		return USB_STOR_TRANSPORT_ERROR;
 	}
 	return USB_STOR_TRANSPORT_GOOD;
@@ -2180,13 +2191,15 @@ static int ene_sd_init(struct us_data *us)
 static int ene_init(struct us_data *us)
 {
 	int result;
-	u8  misc_reg03 = 0;
+	u8  misc_reg03;
 	struct ene_ub6250_info *info = (struct ene_ub6250_info *)(us->extra);
+	u8 *bbuf = info->bbuf;
 
-	result = ene_get_card_type(us, REG_CARD_STATUS, &misc_reg03);
+	result = ene_get_card_type(us, REG_CARD_STATUS, bbuf);
 	if (result != USB_STOR_XFER_GOOD)
 		return USB_STOR_TRANSPORT_ERROR;
 
+	misc_reg03 = bbuf[0];
 	if (misc_reg03 & 0x01) {
 		if (!info->SD_Status.Ready) {
 			result = ene_sd_init(us);
@@ -2303,8 +2316,9 @@ static int ene_ub6250_probe(struct usb_interface *intf,
 			 const struct usb_device_id *id)
 {
 	int result;
-	u8  misc_reg03 = 0;
+	u8  misc_reg03;
 	struct us_data *us;
+	struct ene_ub6250_info *info;
 
 	result = usb_stor_probe1(&us, intf, id,
 		   (id - ene_ub6250_usb_ids) + ene_ub6250_unusual_dev_list,
@@ -2313,11 +2327,16 @@ static int ene_ub6250_probe(struct usb_interface *intf,
 		return result;
 
 	/* FIXME: where should the code alloc extra buf ? */
-	if (!us->extra) {
-		us->extra = kzalloc(sizeof(struct ene_ub6250_info), GFP_KERNEL);
-		if (!us->extra)
-			return -ENOMEM;
-		us->extra_destructor = ene_ub6250_info_destructor;
+	us->extra = kzalloc(sizeof(struct ene_ub6250_info), GFP_KERNEL);
+	if (!us->extra)
+		return -ENOMEM;
+	us->extra_destructor = ene_ub6250_info_destructor;
+
+	info = (struct ene_ub6250_info *)(us->extra);
+	info->bbuf = kmalloc(512, GFP_KERNEL);
+	if (!info->bbuf) {
+		kfree(us->extra);
+		return -ENOMEM;
 	}
 
 	us->transport_name = "ene_ub6250";
@@ -2329,12 +2348,13 @@ static int ene_ub6250_probe(struct usb_interface *intf,
 		return result;
 
 	/* probe card type */
-	result = ene_get_card_type(us, REG_CARD_STATUS, &misc_reg03);
+	result = ene_get_card_type(us, REG_CARD_STATUS, info->bbuf);
 	if (result != USB_STOR_XFER_GOOD) {
 		usb_stor_disconnect(intf);
 		return USB_STOR_TRANSPORT_ERROR;
 	}
 
+	misc_reg03 = info->bbuf[0];
 	if (!(misc_reg03 & 0x01)) {
 		pr_info("ums_eneub6250: This driver only supports SD/MS cards. "
 			"It does not support SM cards.\n");
diff --git a/drivers/usb/usbip/vhci_hcd.c b/drivers/usb/usbip/vhci_hcd.c
index 5d8b2c261940..0585078638db 100644
--- a/drivers/usb/usbip/vhci_hcd.c
+++ b/drivers/usb/usbip/vhci_hcd.c
@@ -235,14 +235,19 @@ done:
 
 static inline void hub_descriptor(struct usb_hub_descriptor *desc)
 {
+	int width;
+
 	memset(desc, 0, sizeof(*desc));
 	desc->bDescriptorType = USB_DT_HUB;
-	desc->bDescLength = 9;
 	desc->wHubCharacteristics = cpu_to_le16(
 		HUB_CHAR_INDV_PORT_LPSM | HUB_CHAR_COMMON_OCPM);
+
 	desc->bNbrPorts = VHCI_HC_PORTS;
-	desc->u.hs.DeviceRemovable[0] = 0xff;
-	desc->u.hs.DeviceRemovable[1] = 0xff;
+	BUILD_BUG_ON(VHCI_HC_PORTS > USB_MAXCHILDREN);
+	width = desc->bNbrPorts / 8 + 1;
+	desc->bDescLength = USB_DT_HUB_NONVAR_SIZE + 2 * width;
+	memset(&desc->u.hs.DeviceRemovable[0], 0, width);
+	memset(&desc->u.hs.DeviceRemovable[width], 0xff, width);
 }
 
 static int vhci_hub_control(struct usb_hcd *hcd, u16 typeReq, u16 wValue,
diff --git a/drivers/uwb/i1480/dfu/usb.c b/drivers/uwb/i1480/dfu/usb.c
index 6345e85822a4..a50cf45e530f 100644
--- a/drivers/uwb/i1480/dfu/usb.c
+++ b/drivers/uwb/i1480/dfu/usb.c
@@ -341,6 +341,7 @@ error_submit_ep1:
 static
 int i1480_usb_probe(struct usb_interface *iface, const struct usb_device_id *id)
 {
+	struct usb_device *udev = interface_to_usbdev(iface);
 	struct i1480_usb *i1480_usb;
 	struct i1480 *i1480;
 	struct device *dev = &iface->dev;
@@ -352,8 +353,8 @@ int i1480_usb_probe(struct usb_interface *iface, const struct usb_device_id *id)
 			iface->cur_altsetting->desc.bInterfaceNumber);
 		goto error;
 	}
-	if (iface->num_altsetting > 1
-	    && interface_to_usbdev(iface)->descriptor.idProduct == 0xbabe) {
+	if (iface->num_altsetting > 1 &&
+			le16_to_cpu(udev->descriptor.idProduct) == 0xbabe) {
 		/* Need altsetting #1 [HW QUIRK] or EP1 won't work */
 		result = usb_set_interface(interface_to_usbdev(iface), 0, 1);
 		if (result < 0)
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/video/fbdev/efifb.c b/drivers/video/fbdev/efifb.c
index b827a8113e26..ff01bed7112f 100644
--- a/drivers/video/fbdev/efifb.c
+++ b/drivers/video/fbdev/efifb.c
@@ -408,7 +408,7 @@ static void efifb_fixup_resources(struct pci_dev *dev)
 	if (!base)
 		return;
 
-	for (i = 0; i < PCI_STD_RESOURCE_END; i++) {
+	for (i = 0; i <= PCI_STD_RESOURCE_END; i++) {
 		struct resource *res = &dev->resource[i];
 
 		if (!(res->flags & IORESOURCE_MEM))
diff --git a/drivers/watchdog/Kconfig b/drivers/watchdog/Kconfig
index 52a70ee6014f..8b9049dac094 100644
--- a/drivers/watchdog/Kconfig
+++ b/drivers/watchdog/Kconfig
@@ -452,7 +452,7 @@ config DAVINCI_WATCHDOG
 
 config ORION_WATCHDOG
 	tristate "Orion watchdog"
-	depends on ARCH_ORION5X || ARCH_DOVE || MACH_DOVE || ARCH_MVEBU || COMPILE_TEST
+	depends on ARCH_ORION5X || ARCH_DOVE || MACH_DOVE || ARCH_MVEBU || (COMPILE_TEST && !ARCH_EBSA110)
 	depends on ARM
 	select WATCHDOG_CORE
 	help
diff --git a/drivers/watchdog/bcm_kona_wdt.c b/drivers/watchdog/bcm_kona_wdt.c
index 6fce17d5b9f1..a5775dfd8d5f 100644
--- a/drivers/watchdog/bcm_kona_wdt.c
+++ b/drivers/watchdog/bcm_kona_wdt.c
@@ -304,6 +304,8 @@ static int bcm_kona_wdt_probe(struct platform_device *pdev)
 	if (!wdt)
 		return -ENOMEM;
 
+	spin_lock_init(&wdt->lock);
+
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	wdt->base = devm_ioremap_resource(dev, res);
 	if (IS_ERR(wdt->base))
@@ -316,7 +318,6 @@ static int bcm_kona_wdt_probe(struct platform_device *pdev)
 		return ret;
 	}
 
-	spin_lock_init(&wdt->lock);
 	platform_set_drvdata(pdev, wdt);
 	watchdog_set_drvdata(&bcm_kona_wdt_wdd, wdt);
 	bcm_kona_wdt_wdd.parent = &pdev->dev;
diff --git a/drivers/watchdog/cadence_wdt.c b/drivers/watchdog/cadence_wdt.c
index 8d61e8bfe60b..86e0b5d2e761 100644
--- a/drivers/watchdog/cadence_wdt.c
+++ b/drivers/watchdog/cadence_wdt.c
@@ -49,7 +49,7 @@
 /* Counter maximum value */
 #define CDNS_WDT_COUNTER_MAX 0xFFF
 
-static int wdt_timeout = CDNS_WDT_DEFAULT_TIMEOUT;
+static int wdt_timeout;
 static int nowayout = WATCHDOG_NOWAYOUT;
 
 module_param(wdt_timeout, int, 0);
diff --git a/drivers/watchdog/iTCO_wdt.c b/drivers/watchdog/iTCO_wdt.c
index 347f0389b089..c4f65873bfa4 100644
--- a/drivers/watchdog/iTCO_wdt.c
+++ b/drivers/watchdog/iTCO_wdt.c
@@ -306,16 +306,15 @@ static int iTCO_wdt_ping(struct watchdog_device *wd_dev)
 
 	iTCO_vendor_pre_keepalive(p->smi_res, wd_dev->timeout);
 
+	/* Reset the timeout status bit so that the timer
+	 * needs to count down twice again before rebooting */
+	outw(0x0008, TCO1_STS(p));	/* write 1 to clear bit */
+
 	/* Reload the timer by writing to the TCO Timer Counter register */
-	if (p->iTCO_version >= 2) {
+	if (p->iTCO_version >= 2)
 		outw(0x01, TCO_RLD(p));
-	} else if (p->iTCO_version == 1) {
-		/* Reset the timeout status bit so that the timer
-		 * needs to count down twice again before rebooting */
-		outw(0x0008, TCO1_STS(p));	/* write 1 to clear bit */
-
+	else if (p->iTCO_version == 1)
 		outb(0x01, TCO_RLD(p));
-	}
 
 	spin_unlock(&p->io_lock);
 	return 0;
@@ -328,11 +327,8 @@ static int iTCO_wdt_set_timeout(struct watchdog_device *wd_dev, unsigned int t)
 	unsigned char val8;
 	unsigned int tmrval;
 
-	tmrval = seconds_to_ticks(p, t);
-
-	/* For TCO v1 the timer counts down twice before rebooting */
-	if (p->iTCO_version == 1)
-		tmrval /= 2;
+	/* The timer counts down twice before rebooting */
+	tmrval = seconds_to_ticks(p, t) / 2;
 
 	/* from the specs: */
 	/* "Values of 0h-3h are ignored and should not be attempted" */
@@ -385,6 +381,8 @@ static unsigned int iTCO_wdt_get_timeleft(struct watchdog_device *wd_dev)
 		spin_lock(&p->io_lock);
 		val16 = inw(TCO_RLD(p));
 		val16 &= 0x3ff;
+		if (!(inw(TCO1_STS(p)) & 0x0008))
+			val16 += (inw(TCOv2_TMR(p)) & 0x3ff);
 		spin_unlock(&p->io_lock);
 
 		time_left = ticks_to_seconds(p, val16);
diff --git a/drivers/watchdog/pcwd_usb.c b/drivers/watchdog/pcwd_usb.c
index 99ebf6ea3de6..5615f4013924 100644
--- a/drivers/watchdog/pcwd_usb.c
+++ b/drivers/watchdog/pcwd_usb.c
@@ -630,6 +630,9 @@ static int usb_pcwd_probe(struct usb_interface *interface,
 		return -ENODEV;
 	}
 
+	if (iface_desc->desc.bNumEndpoints < 1)
+		return -ENODEV;
+
 	/* check out the endpoint: it has to be Interrupt & IN */
 	endpoint = &iface_desc->endpoint[0].desc;
 
diff --git a/drivers/watchdog/sama5d4_wdt.c b/drivers/watchdog/sama5d4_wdt.c
index f709962018ac..362fd229786d 100644
--- a/drivers/watchdog/sama5d4_wdt.c
+++ b/drivers/watchdog/sama5d4_wdt.c
@@ -6,6 +6,7 @@
  * Licensed under GPLv2.
  */
 
+#include <linux/delay.h>
 #include <linux/interrupt.h>
 #include <linux/io.h>
 #include <linux/kernel.h>
@@ -29,6 +30,7 @@ struct sama5d4_wdt {
 	struct watchdog_device	wdd;
 	void __iomem		*reg_base;
 	u32			mr;
+	unsigned long		last_ping;
 };
 
 static int wdt_timeout = WDT_DEFAULT_TIMEOUT;
@@ -44,11 +46,34 @@ MODULE_PARM_DESC(nowayout,
 	"Watchdog cannot be stopped once started (default="
 	__MODULE_STRING(WATCHDOG_NOWAYOUT) ")");
 
+#define wdt_enabled (!(wdt->mr & AT91_WDT_WDDIS))
+
 #define wdt_read(wdt, field) \
 	readl_relaxed((wdt)->reg_base + (field))
 
-#define wdt_write(wtd, field, val) \
-	writel_relaxed((val), (wdt)->reg_base + (field))
+/* 4 slow clock periods is 4/32768 = 122.07µs*/
+#define WDT_DELAY	usecs_to_jiffies(123)
+
+static void wdt_write(struct sama5d4_wdt *wdt, u32 field, u32 val)
+{
+	/*
+	 * WDT_CR and WDT_MR must not be modified within three slow clock
+	 * periods following a restart of the watchdog performed by a write
+	 * access in WDT_CR.
+	 */
+	while (time_before(jiffies, wdt->last_ping + WDT_DELAY))
+		usleep_range(30, 125);
+	writel_relaxed(val, wdt->reg_base + field);
+	wdt->last_ping = jiffies;
+}
+
+static void wdt_write_nosleep(struct sama5d4_wdt *wdt, u32 field, u32 val)
+{
+	if (time_before(jiffies, wdt->last_ping + WDT_DELAY))
+		udelay(123);
+	writel_relaxed(val, wdt->reg_base + field);
+	wdt->last_ping = jiffies;
+}
 
 static int sama5d4_wdt_start(struct watchdog_device *wdd)
 {
@@ -89,7 +114,16 @@ static int sama5d4_wdt_set_timeout(struct watchdog_device *wdd,
 	wdt->mr &= ~AT91_WDT_WDD;
 	wdt->mr |= AT91_WDT_SET_WDV(value);
 	wdt->mr |= AT91_WDT_SET_WDD(value);
-	wdt_write(wdt, AT91_WDT_MR, wdt->mr);
+
+	/*
+	 * WDDIS has to be 0 when updating WDD/WDV. The datasheet states: When
+	 * setting the WDDIS bit, and while it is set, the fields WDV and WDD
+	 * must not be modified.
+	 * If the watchdog is enabled, then the timeout can be updated. Else,
+	 * wait that the user enables it.
+	 */
+	if (wdt_enabled)
+		wdt_write(wdt, AT91_WDT_MR, wdt->mr & ~AT91_WDT_WDDIS);
 
 	wdd->timeout = timeout;
 
@@ -145,23 +179,21 @@ static int of_sama5d4_wdt_init(struct device_node *np, struct sama5d4_wdt *wdt)
 
 static int sama5d4_wdt_init(struct sama5d4_wdt *wdt)
 {
-	struct watchdog_device *wdd = &wdt->wdd;
-	u32 value = WDT_SEC2TICKS(wdd->timeout);
 	u32 reg;
-
 	/*
-	 * Because the fields WDV and WDD must not be modified when the WDDIS
-	 * bit is set, so clear the WDDIS bit before writing the WDT_MR.
+	 * When booting and resuming, the bootloader may have changed the
+	 * watchdog configuration.
+	 * If the watchdog is already running, we can safely update it.
+	 * Else, we have to disable it properly.
 	 */
-	reg = wdt_read(wdt, AT91_WDT_MR);
-	reg &= ~AT91_WDT_WDDIS;
-	wdt_write(wdt, AT91_WDT_MR, reg);
-
-	wdt->mr |= AT91_WDT_SET_WDD(value);
-	wdt->mr |= AT91_WDT_SET_WDV(value);
-
-	wdt_write(wdt, AT91_WDT_MR, wdt->mr);
-
+	if (wdt_enabled) {
+		wdt_write_nosleep(wdt, AT91_WDT_MR, wdt->mr);
+	} else {
+		reg = wdt_read(wdt, AT91_WDT_MR);
+		if (!(reg & AT91_WDT_WDDIS))
+			wdt_write_nosleep(wdt, AT91_WDT_MR,
+					  reg | AT91_WDT_WDDIS);
+	}
 	return 0;
 }
 
@@ -172,6 +204,7 @@ static int sama5d4_wdt_probe(struct platform_device *pdev)
 	struct resource *res;
 	void __iomem *regs;
 	u32 irq = 0;
+	u32 timeout;
 	int ret;
 
 	wdt = devm_kzalloc(&pdev->dev, sizeof(*wdt), GFP_KERNEL);
@@ -184,6 +217,7 @@ static int sama5d4_wdt_probe(struct platform_device *pdev)
 	wdd->ops = &sama5d4_wdt_ops;
 	wdd->min_timeout = MIN_WDT_TIMEOUT;
 	wdd->max_timeout = MAX_WDT_TIMEOUT;
+	wdt->last_ping = jiffies;
 
 	watchdog_set_drvdata(wdd, wdt);
 
@@ -221,6 +255,11 @@ static int sama5d4_wdt_probe(struct platform_device *pdev)
 		return ret;
 	}
 
+	timeout = WDT_SEC2TICKS(wdd->timeout);
+
+	wdt->mr |= AT91_WDT_SET_WDD(timeout);
+	wdt->mr |= AT91_WDT_SET_WDV(timeout);
+
 	ret = sama5d4_wdt_init(wdt);
 	if (ret)
 		return ret;
@@ -263,9 +302,7 @@ static int sama5d4_wdt_resume(struct device *dev)
 {
 	struct sama5d4_wdt *wdt = dev_get_drvdata(dev);
 
-	wdt_write(wdt, AT91_WDT_MR, wdt->mr & ~AT91_WDT_WDDIS);
-	if (wdt->mr & AT91_WDT_WDDIS)
-		wdt_write(wdt, AT91_WDT_MR, wdt->mr);
+	sama5d4_wdt_init(wdt);
 
 	return 0;
 }
diff --git a/drivers/watchdog/wdt_pci.c b/drivers/watchdog/wdt_pci.c
index 48b2c058b009..bc7addc2dc06 100644
--- a/drivers/watchdog/wdt_pci.c
+++ b/drivers/watchdog/wdt_pci.c
@@ -332,7 +332,7 @@ static irqreturn_t wdtpci_interrupt(int irq, void *dev_id)
 		pr_crit("Would Reboot\n");
 #else
 		pr_crit("Initiating system reboot\n");
-		emergency_restart(NULL);
+		emergency_restart();
 #endif
 #else
 		pr_crit("Reset in 5ms\n");
diff --git a/drivers/watchdog/zx2967_wdt.c b/drivers/watchdog/zx2967_wdt.c
index e290d5a13a6d..c98252733c30 100644
--- a/drivers/watchdog/zx2967_wdt.c
+++ b/drivers/watchdog/zx2967_wdt.c
@@ -211,10 +211,8 @@ static int zx2967_wdt_probe(struct platform_device *pdev)
 
 	base = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	wdt->reg_base = devm_ioremap_resource(dev, base);
-	if (IS_ERR(wdt->reg_base)) {
-		dev_err(dev, "ioremap failed\n");
+	if (IS_ERR(wdt->reg_base))
 		return PTR_ERR(wdt->reg_base);
-	}
 
 	zx2967_wdt_reset_sysctrl(dev);