Merge branch 'u-boot/master' into 'u-boot-arm/master'

232 files changed, 14144 insertions, 2597 deletions

diff --git a/drivers/block/ahci.c b/drivers/block/ahci.c
index 88b90e0..6508648 100644
--- a/drivers/block/ahci.c
+++ b/drivers/block/ahci.c
@@ -137,10 +137,10 @@ static void sunxi_dma_init(volatile u8 *port_mmio)
 }
 #endif
 
-int ahci_reset(u32 base)
+int ahci_reset(void __iomem *base)
 {
 	int i = 1000;
-	u32 host_ctl_reg = base + HOST_CTL;
+	u32 __iomem *host_ctl_reg = base + HOST_CTL;
 	u32 tmp = readl(host_ctl_reg); /* global controller reset */
 
 	if ((tmp & HOST_RESET) == 0)
@@ -419,8 +419,9 @@ static int ahci_init_one(pci_dev_t pdev)
 	probe_ent->pio_mask = 0x1f;
 	probe_ent->udma_mask = 0x7f;	/*Fixme,assume to support UDMA6 */
 
-	pci_read_config_dword(pdev, PCI_BASE_ADDRESS_5, &probe_ent->mmio_base);
-	debug("ahci mmio_base=0x%08x\n", probe_ent->mmio_base);
+	probe_ent->mmio_base = pci_map_bar(pdev, PCI_BASE_ADDRESS_5,
+					   PCI_REGION_MEM);
+	debug("ahci mmio_base=0x%p\n", probe_ent->mmio_base);
 
 	/* Take from kernel:
 	 * JMicron-specific fixup:
@@ -939,7 +940,7 @@ void scsi_low_level_init(int busdevfunc)
 }
 
 #ifdef CONFIG_SCSI_AHCI_PLAT
-int ahci_init(u32 base)
+int ahci_init(void __iomem *base)
 {
 	int i, rc = 0;
 	u32 linkmap;
diff --git a/drivers/block/dwc_ahsata.c b/drivers/block/dwc_ahsata.c
index 01a4148..cf3ef6b 100644
--- a/drivers/block/dwc_ahsata.c
+++ b/drivers/block/dwc_ahsata.c
@@ -343,7 +343,7 @@ static int ahci_init_one(int pdev)
 				| ATA_FLAG_PIO_DMA
 				| ATA_FLAG_NO_ATAPI;
 
-	probe_ent->mmio_base = CONFIG_DWC_AHSATA_BASE_ADDR;
+	probe_ent->mmio_base = (void __iomem *)CONFIG_DWC_AHSATA_BASE_ADDR;
 
 	/* initialize adapter */
 	rc = ahci_host_init(probe_ent);
diff --git a/drivers/core/Kconfig b/drivers/core/Kconfig
index 75d182d..2861b43 100644
--- a/drivers/core/Kconfig
+++ b/drivers/core/Kconfig
@@ -46,3 +46,12 @@ config DM_STDIO
 	  Normally serial drivers register with stdio so that they can be used
 	  as normal output devices. In SPL we don't normally use stdio, so
 	  we can omit this feature.
+
+config DM_SEQ_ALIAS
+	bool "Support numbered aliases in device tree"
+	depends on DM
+	default y
+	help
+	  Most boards will have a '/aliases' node containing the path to
+	  numbered devices (e.g. serial0 = &serial0). This feature can be
+	  disabled if it is not required, to save code space in SPL.
diff --git a/drivers/core/device-remove.c b/drivers/core/device-remove.c
index 3a5f48d..6a16b4f 100644
--- a/drivers/core/device-remove.c
+++ b/drivers/core/device-remove.c
@@ -66,7 +66,7 @@ static int device_chld_remove(struct udevice *dev)
 
 int device_unbind(struct udevice *dev)
 {
-	struct driver *drv;
+	const struct driver *drv;
 	int ret;
 
 	if (!dev)
@@ -92,6 +92,10 @@ int device_unbind(struct udevice *dev)
 		free(dev->platdata);
 		dev->platdata = NULL;
 	}
+	if (dev->flags & DM_FLAG_ALLOC_UCLASS_PDATA) {
+		free(dev->uclass_platdata);
+		dev->uclass_platdata = NULL;
+	}
 	if (dev->flags & DM_FLAG_ALLOC_PARENT_PDATA) {
 		free(dev->parent_platdata);
 		dev->parent_platdata = NULL;
@@ -139,7 +143,7 @@ void device_free(struct udevice *dev)
 
 int device_remove(struct udevice *dev)
 {
-	struct driver *drv;
+	const struct driver *drv;
 	int ret;
 
 	if (!dev)
diff --git a/drivers/core/device.c b/drivers/core/device.c
index 73c3e07..85fd1fc 100644
--- a/drivers/core/device.c
+++ b/drivers/core/device.c
@@ -24,12 +24,13 @@
 
 DECLARE_GLOBAL_DATA_PTR;
 
-int device_bind(struct udevice *parent, struct driver *drv, const char *name,
-		void *platdata, int of_offset, struct udevice **devp)
+int device_bind(struct udevice *parent, const struct driver *drv,
+		const char *name, void *platdata, int of_offset,
+		struct udevice **devp)
 {
 	struct udevice *dev;
 	struct uclass *uc;
-	int ret = 0;
+	int size, ret = 0;
 
 	*devp = NULL;
 	if (!name)
@@ -55,21 +56,23 @@ int device_bind(struct udevice *parent, struct driver *drv, const char *name,
 
 	dev->seq = -1;
 	dev->req_seq = -1;
-#ifdef CONFIG_OF_CONTROL
-	/*
-	 * Some devices, such as a SPI bus, I2C bus and serial ports are
-	 * numbered using aliases.
-	 *
-	 * This is just a 'requested' sequence, and will be
-	 * resolved (and ->seq updated) when the device is probed.
-	 */
-	if (uc->uc_drv->flags & DM_UC_FLAG_SEQ_ALIAS) {
-		if (uc->uc_drv->name && of_offset != -1) {
-			fdtdec_get_alias_seq(gd->fdt_blob, uc->uc_drv->name,
-					     of_offset, &dev->req_seq);
+	if (IS_ENABLED(CONFIG_OF_CONTROL) && IS_ENABLED(CONFIG_DM_SEQ_ALIAS)) {
+		/*
+		* Some devices, such as a SPI bus, I2C bus and serial ports
+		* are numbered using aliases.
+		*
+		* This is just a 'requested' sequence, and will be
+		* resolved (and ->seq updated) when the device is probed.
+		*/
+		if (uc->uc_drv->flags & DM_UC_FLAG_SEQ_ALIAS) {
+			if (uc->uc_drv->name && of_offset != -1) {
+				fdtdec_get_alias_seq(gd->fdt_blob,
+						uc->uc_drv->name, of_offset,
+						&dev->req_seq);
+			}
 		}
 	}
-#endif
+
 	if (!dev->platdata && drv->platdata_auto_alloc_size) {
 		dev->flags |= DM_FLAG_ALLOC_PDATA;
 		dev->platdata = calloc(1, drv->platdata_auto_alloc_size);
@@ -78,9 +81,19 @@ int device_bind(struct udevice *parent, struct driver *drv, const char *name,
 			goto fail_alloc1;
 		}
 	}
-	if (parent) {
-		int size = parent->driver->per_child_platdata_auto_alloc_size;
 
+	size = uc->uc_drv->per_device_platdata_auto_alloc_size;
+	if (size) {
+		dev->flags |= DM_FLAG_ALLOC_UCLASS_PDATA;
+		dev->uclass_platdata = calloc(1, size);
+		if (!dev->uclass_platdata) {
+			ret = -ENOMEM;
+			goto fail_alloc2;
+		}
+	}
+
+	if (parent) {
+		size = parent->driver->per_child_platdata_auto_alloc_size;
 		if (!size) {
 			size = parent->uclass->uc_drv->
 					per_child_platdata_auto_alloc_size;
@@ -90,7 +103,7 @@ int device_bind(struct udevice *parent, struct driver *drv, const char *name,
 			dev->parent_platdata = calloc(1, size);
 			if (!dev->parent_platdata) {
 				ret = -ENOMEM;
-				goto fail_alloc2;
+				goto fail_alloc3;
 			}
 		}
 	}
@@ -122,21 +135,32 @@ int device_bind(struct udevice *parent, struct driver *drv, const char *name,
 	return 0;
 
 fail_child_post_bind:
-	if (drv->unbind && drv->unbind(dev)) {
-		dm_warn("unbind() method failed on dev '%s' on error path\n",
-			dev->name);
+	if (IS_ENABLED(CONFIG_DM_DEVICE_REMOVE)) {
+		if (drv->unbind && drv->unbind(dev)) {
+			dm_warn("unbind() method failed on dev '%s' on error path\n",
+				dev->name);
+		}
 	}
 
 fail_bind:
-	if (uclass_unbind_device(dev)) {
-		dm_warn("Failed to unbind dev '%s' on error path\n",
-			dev->name);
+	if (IS_ENABLED(CONFIG_DM_DEVICE_REMOVE)) {
+		if (uclass_unbind_device(dev)) {
+			dm_warn("Failed to unbind dev '%s' on error path\n",
+				dev->name);
+		}
 	}
 fail_uclass_bind:
-	list_del(&dev->sibling_node);
-	if (dev->flags & DM_FLAG_ALLOC_PARENT_PDATA) {
-		free(dev->parent_platdata);
-		dev->parent_platdata = NULL;
+	if (IS_ENABLED(CONFIG_DM_DEVICE_REMOVE)) {
+		list_del(&dev->sibling_node);
+		if (dev->flags & DM_FLAG_ALLOC_PARENT_PDATA) {
+			free(dev->parent_platdata);
+			dev->parent_platdata = NULL;
+		}
+	}
+fail_alloc3:
+	if (dev->flags & DM_FLAG_ALLOC_UCLASS_PDATA) {
+		free(dev->uclass_platdata);
+		dev->uclass_platdata = NULL;
 	}
 fail_alloc2:
 	if (dev->flags & DM_FLAG_ALLOC_PDATA) {
@@ -164,9 +188,24 @@ int device_bind_by_name(struct udevice *parent, bool pre_reloc_only,
 			   -1, devp);
 }
 
+static void *alloc_priv(int size, uint flags)
+{
+	void *priv;
+
+	if (flags & DM_FLAG_ALLOC_PRIV_DMA) {
+		priv = memalign(ARCH_DMA_MINALIGN, size);
+		if (priv)
+			memset(priv, '\0', size);
+	} else {
+		priv = calloc(1, size);
+	}
+
+	return priv;
+}
+
 int device_probe_child(struct udevice *dev, void *parent_priv)
 {
-	struct driver *drv;
+	const struct driver *drv;
 	int size = 0;
 	int ret;
 	int seq;
@@ -182,7 +221,7 @@ int device_probe_child(struct udevice *dev, void *parent_priv)
 
 	/* Allocate private data if requested */
 	if (drv->priv_auto_alloc_size) {
-		dev->priv = calloc(1, drv->priv_auto_alloc_size);
+		dev->priv = alloc_priv(drv->priv_auto_alloc_size, drv->flags);
 		if (!dev->priv) {
 			ret = -ENOMEM;
 			goto fail;
@@ -206,7 +245,7 @@ int device_probe_child(struct udevice *dev, void *parent_priv)
 					per_child_auto_alloc_size;
 		}
 		if (size) {
-			dev->parent_priv = calloc(1, size);
+			dev->parent_priv = alloc_priv(size, drv->flags);
 			if (!dev->parent_priv) {
 				ret = -ENOMEM;
 				goto fail;
@@ -227,7 +266,9 @@ int device_probe_child(struct udevice *dev, void *parent_priv)
 	}
 	dev->seq = seq;
 
-	ret = uclass_pre_probe_child(dev);
+	dev->flags |= DM_FLAG_ACTIVATED;
+
+	ret = uclass_pre_probe_device(dev);
 	if (ret)
 		goto fail;
 
@@ -243,19 +284,18 @@ int device_probe_child(struct udevice *dev, void *parent_priv)
 			goto fail;
 	}
 
+	dev->flags |= DM_FLAG_ACTIVATED;
 	if (drv->probe) {
 		ret = drv->probe(dev);
-		if (ret)
+		if (ret) {
+			dev->flags &= ~DM_FLAG_ACTIVATED;
 			goto fail;
+		}
 	}
 
-	dev->flags |= DM_FLAG_ACTIVATED;
-
 	ret = uclass_post_probe_device(dev);
-	if (ret) {
-		dev->flags &= ~DM_FLAG_ACTIVATED;
+	if (ret)
 		goto fail_uclass;
-	}
 
 	return 0;
 fail_uclass:
@@ -264,6 +304,8 @@ fail_uclass:
 			__func__, dev->name);
 	}
 fail:
+	dev->flags &= ~DM_FLAG_ACTIVATED;
+
 	dev->seq = -1;
 	device_free(dev);
 
@@ -295,6 +337,16 @@ void *dev_get_parent_platdata(struct udevice *dev)
 	return dev->parent_platdata;
 }
 
+void *dev_get_uclass_platdata(struct udevice *dev)
+{
+	if (!dev) {
+		dm_warn("%s: null device", __func__);
+		return NULL;
+	}
+
+	return dev->uclass_platdata;
+}
+
 void *dev_get_priv(struct udevice *dev)
 {
 	if (!dev) {
@@ -305,6 +357,16 @@ void *dev_get_priv(struct udevice *dev)
 	return dev->priv;
 }
 
+void *dev_get_uclass_priv(struct udevice *dev)
+{
+	if (!dev) {
+		dm_warn("%s: null device\n", __func__);
+		return NULL;
+	}
+
+	return dev->uclass_priv;
+}
+
 void *dev_get_parentdata(struct udevice *dev)
 {
 	if (!dev) {
@@ -440,9 +502,17 @@ struct udevice *dev_get_parent(struct udevice *child)
 	return child->parent;
 }
 
-ulong dev_get_of_data(struct udevice *dev)
+ulong dev_get_driver_data(struct udevice *dev)
 {
-	return dev->of_id->data;
+	return dev->driver_data;
+}
+
+const void *dev_get_driver_ops(struct udevice *dev)
+{
+	if (!dev || !dev->driver->ops)
+		return NULL;
+
+	return dev->driver->ops;
 }
 
 enum uclass_id device_get_uclass_id(struct udevice *dev)
@@ -450,6 +520,14 @@ enum uclass_id device_get_uclass_id(struct udevice *dev)
 	return dev->uclass->uc_drv->id;
 }
 
+const char *dev_get_uclass_name(struct udevice *dev)
+{
+	if (!dev)
+		return NULL;
+
+	return dev->uclass->uc_drv->name;
+}
+
 #ifdef CONFIG_OF_CONTROL
 fdt_addr_t dev_get_addr(struct udevice *dev)
 {
@@ -461,3 +539,31 @@ fdt_addr_t dev_get_addr(struct udevice *dev)
 	return FDT_ADDR_T_NONE;
 }
 #endif
+
+bool device_has_children(struct udevice *dev)
+{
+	return !list_empty(&dev->child_head);
+}
+
+bool device_has_active_children(struct udevice *dev)
+{
+	struct udevice *child;
+
+	for (device_find_first_child(dev, &child);
+	     child;
+	     device_find_next_child(&child)) {
+		if (device_active(child))
+			return true;
+	}
+
+	return false;
+}
+
+bool device_is_last_sibling(struct udevice *dev)
+{
+	struct udevice *parent = dev->parent;
+
+	if (!parent)
+		return false;
+	return list_is_last(&dev->sibling_node, &parent->child_head);
+}
diff --git a/drivers/core/lists.c b/drivers/core/lists.c
index ff115c4..647e390 100644
--- a/drivers/core/lists.c
+++ b/drivers/core/lists.c
@@ -168,7 +168,7 @@ int lists_bind_fdt(struct udevice *parent, const void *blob, int offset,
 			dm_warn("Error binding driver '%s'\n", entry->name);
 			return ret;
 		} else {
-			dev->of_id = id;
+			dev->driver_data = id->data;
 			found = true;
 			if (devp)
 				*devp = dev;
diff --git a/drivers/core/root.c b/drivers/core/root.c
index 9b5c6bb..12d0460 100644
--- a/drivers/core/root.c
+++ b/drivers/core/root.c
@@ -197,13 +197,15 @@ int dm_init_and_scan(bool pre_reloc_only)
 		debug("dm_scan_platdata() failed: %d\n", ret);
 		return ret;
 	}
-#ifdef CONFIG_OF_CONTROL
-	ret = dm_scan_fdt(gd->fdt_blob, pre_reloc_only);
-	if (ret) {
-		debug("dm_scan_fdt() failed: %d\n", ret);
-		return ret;
+
+	if (OF_CONTROL) {
+		ret = dm_scan_fdt(gd->fdt_blob, pre_reloc_only);
+		if (ret) {
+			debug("dm_scan_fdt() failed: %d\n", ret);
+			return ret;
+		}
 	}
-#endif
+
 	ret = dm_scan_other(pre_reloc_only);
 	if (ret)
 		return ret;
diff --git a/drivers/core/uclass.c b/drivers/core/uclass.c
index 289a5d2..7de8173 100644
--- a/drivers/core/uclass.c
+++ b/drivers/core/uclass.c
@@ -99,10 +99,18 @@ fail_mem:
 int uclass_destroy(struct uclass *uc)
 {
 	struct uclass_driver *uc_drv;
-	struct udevice *dev, *tmp;
+	struct udevice *dev;
 	int ret;
 
-	list_for_each_entry_safe(dev, tmp, &uc->dev_head, uclass_node) {
+	/*
+	 * We cannot use list_for_each_entry_safe() here. If a device in this
+	 * uclass has a child device also in this uclass, it will be also be
+	 * unbound (by the recursion in the call to device_unbind() below).
+	 * We can loop until the list is empty.
+	 */
+	while (!list_empty(&uc->dev_head)) {
+		dev = list_first_entry(&uc->dev_head, struct udevice,
+				       uclass_node);
 		ret = device_remove(dev);
 		if (ret)
 			return ret;
@@ -156,6 +164,60 @@ int uclass_find_device(enum uclass_id id, int index, struct udevice **devp)
 	return -ENODEV;
 }
 
+int uclass_find_first_device(enum uclass_id id, struct udevice **devp)
+{
+	struct uclass *uc;
+	int ret;
+
+	*devp = NULL;
+	ret = uclass_get(id, &uc);
+	if (ret)
+		return ret;
+	if (list_empty(&uc->dev_head))
+		return 0;
+
+	*devp = list_first_entry(&uc->dev_head, struct udevice, uclass_node);
+
+	return 0;
+}
+
+int uclass_find_next_device(struct udevice **devp)
+{
+	struct udevice *dev = *devp;
+
+	*devp = NULL;
+	if (list_is_last(&dev->uclass_node, &dev->uclass->dev_head))
+		return 0;
+
+	*devp = list_entry(dev->uclass_node.next, struct udevice, uclass_node);
+
+	return 0;
+}
+
+int uclass_find_device_by_name(enum uclass_id id, const char *name,
+			       struct udevice **devp)
+{
+	struct uclass *uc;
+	struct udevice *dev;
+	int ret;
+
+	*devp = NULL;
+	if (!name)
+		return -EINVAL;
+	ret = uclass_get(id, &uc);
+	if (ret)
+		return ret;
+
+	list_for_each_entry(dev, &uc->dev_head, uclass_node) {
+		if (!strncmp(dev->name, name, strlen(name))) {
+			*devp = dev;
+			return 0;
+		}
+	}
+
+	return -ENODEV;
+}
+
 int uclass_find_device_by_seq(enum uclass_id id, int seq_or_req_seq,
 			      bool find_req_seq, struct udevice **devp)
 {
@@ -209,22 +271,13 @@ static int uclass_find_device_by_of_offset(enum uclass_id id, int node,
 	return -ENODEV;
 }
 
-/**
- * uclass_get_device_tail() - handle the end of a get_device call
- *
- * This handles returning an error or probing a device as needed.
- *
- * @dev: Device that needs to be probed
- * @ret: Error to return. If non-zero then the device is not probed
- * @devp: Returns the value of 'dev' if there is no error
- * @return ret, if non-zero, else the result of the device_probe() call
- */
-static int uclass_get_device_tail(struct udevice *dev, int ret,
+int uclass_get_device_tail(struct udevice *dev, int ret,
 				  struct udevice **devp)
 {
 	if (ret)
 		return ret;
 
+	assert(dev);
 	ret = device_probe(dev);
 	if (ret)
 		return ret;
@@ -244,6 +297,17 @@ int uclass_get_device(enum uclass_id id, int index, struct udevice **devp)
 	return uclass_get_device_tail(dev, ret, devp);
 }
 
+int uclass_get_device_by_name(enum uclass_id id, const char *name,
+			      struct udevice **devp)
+{
+	struct udevice *dev;
+	int ret;
+
+	*devp = NULL;
+	ret = uclass_find_device_by_name(id, name, &dev);
+	return uclass_get_device_tail(dev, ret, devp);
+}
+
 int uclass_get_device_by_seq(enum uclass_id id, int seq, struct udevice **devp)
 {
 	struct udevice *dev;
@@ -274,24 +338,14 @@ int uclass_get_device_by_of_offset(enum uclass_id id, int node,
 
 int uclass_first_device(enum uclass_id id, struct udevice **devp)
 {
-	struct uclass *uc;
 	struct udevice *dev;
 	int ret;
 
 	*devp = NULL;
-	ret = uclass_get(id, &uc);
-	if (ret)
-		return ret;
-	if (list_empty(&uc->dev_head))
+	ret = uclass_find_first_device(id, &dev);
+	if (!dev)
 		return 0;
-
-	dev = list_first_entry(&uc->dev_head, struct udevice, uclass_node);
-	ret = device_probe(dev);
-	if (ret)
-		return ret;
-	*devp = dev;
-
-	return 0;
+	return uclass_get_device_tail(dev, ret, devp);
 }
 
 int uclass_next_device(struct udevice **devp)
@@ -300,17 +354,10 @@ int uclass_next_device(struct udevice **devp)
 	int ret;
 
 	*devp = NULL;
-	if (list_is_last(&dev->uclass_node, &dev->uclass->dev_head))
+	ret = uclass_find_next_device(&dev);
+	if (!dev)
 		return 0;
-
-	dev = list_entry(dev->uclass_node.next, struct udevice,
-			 uclass_node);
-	ret = device_probe(dev);
-	if (ret)
-		return ret;
-	*devp = dev;
-
-	return 0;
+	return uclass_get_device_tail(dev, ret, devp);
 }
 
 int uclass_bind_device(struct udevice *dev)
@@ -344,6 +391,7 @@ err:
 	return ret;
 }
 
+#ifdef CONFIG_DM_DEVICE_REMOVE
 int uclass_unbind_device(struct udevice *dev)
 {
 	struct uclass *uc;
@@ -359,6 +407,7 @@ int uclass_unbind_device(struct udevice *dev)
 	list_del(&dev->uclass_node);
 	return 0;
 }
+#endif
 
 int uclass_resolve_seq(struct udevice *dev)
 {
@@ -391,9 +440,17 @@ int uclass_resolve_seq(struct udevice *dev)
 	return seq;
 }
 
-int uclass_pre_probe_child(struct udevice *dev)
+int uclass_pre_probe_device(struct udevice *dev)
 {
 	struct uclass_driver *uc_drv;
+	int ret;
+
+	uc_drv = dev->uclass->uc_drv;
+	if (uc_drv->pre_probe) {
+		ret = uc_drv->pre_probe(dev);
+		if (ret)
+			return ret;
+	}
 
 	if (!dev->parent)
 		return 0;
@@ -414,6 +471,7 @@ int uclass_post_probe_device(struct udevice *dev)
 	return 0;
 }
 
+#ifdef CONFIG_DM_DEVICE_REMOVE
 int uclass_pre_remove_device(struct udevice *dev)
 {
 	struct uclass_driver *uc_drv;
@@ -435,3 +493,4 @@ int uclass_pre_remove_device(struct udevice *dev)
 
 	return 0;
 }
+#endif
diff --git a/drivers/ddr/fsl/ctrl_regs.c b/drivers/ddr/fsl/ctrl_regs.c
index 690e73d..8367c95 100644
--- a/drivers/ddr/fsl/ctrl_regs.c
+++ b/drivers/ddr/fsl/ctrl_regs.c
@@ -313,7 +313,10 @@ static void set_timing_cfg_0(const unsigned int ctrl_num,
 #ifdef CONFIG_SYS_FSL_DDR4
 	/* tXP=max(4nCK, 6ns) */
 	int txp = max((int)mclk_ps * 4, 6000); /* unit=ps */
-	trwt_mclk = 2;
+	unsigned int data_rate = get_ddr_freq(ctrl_num);
+
+	/* for faster clock, need more time for data setup */
+	trwt_mclk = (data_rate/1000000 > 1900) ? 3 : 2;
 	twrt_mclk = 1;
 	act_pd_exit_mclk = picos_to_mclk(ctrl_num, txp);
 	pre_pd_exit_mclk = act_pd_exit_mclk;
@@ -338,7 +341,7 @@ static void set_timing_cfg_0(const unsigned int ctrl_num,
 	 */
 	txp = max((int)mclk_ps * 3, (mclk_ps > 1540 ? 7500 : 6000));
 
-	ip_rev = fsl_ddr_get_version();
+	ip_rev = fsl_ddr_get_version(ctrl_num);
 	if (ip_rev >= 0x40700) {
 		/*
 		 * MRS_CYC = max(tMRD, tMOD)
@@ -544,7 +547,7 @@ static void set_timing_cfg_1(const unsigned int ctrl_num,
 	 * we need set extend bit for it at
 	 * TIMING_CFG_3[EXT_CASLAT]
 	 */
-	if (fsl_ddr_get_version() <= 0x40400)
+	if (fsl_ddr_get_version(ctrl_num) <= 0x40400)
 		caslat_ctrl = 2 * cas_latency - 1;
 	else
 		caslat_ctrl = (cas_latency - 1) << 1;
@@ -1113,16 +1116,32 @@ static void set_ddr_sdram_mode_9(fsl_ddr_cfg_regs_t *ddr,
 	int i;
 	unsigned short esdmode4 = 0;	/* Extended SDRAM mode 4 */
 	unsigned short esdmode5;	/* Extended SDRAM mode 5 */
+	int rtt_park = 0;
 
-	esdmode5 = 0x00000400;		/* Data mask enabled */
+	if (ddr->cs[0].config & SDRAM_CS_CONFIG_EN) {
+		esdmode5 = 0x00000500;	/* Data mask enable, RTT_PARK CS0 */
+		rtt_park = 1;
+	} else {
+		esdmode5 = 0x00000400;	/* Data mask enabled */
+	}
 
 	ddr->ddr_sdram_mode_9 = (0
 				 | ((esdmode4 & 0xffff) << 16)
 				 | ((esdmode5 & 0xffff) << 0)
 				);
+
+	/* only mode_9 use 0x500, others use 0x400 */
+
 	debug("FSLDDR: ddr_sdram_mode_9) = 0x%08x\n", ddr->ddr_sdram_mode_9);
 	if (unq_mrs_en) {	/* unique mode registers are supported */
 		for (i = 1; i < CONFIG_CHIP_SELECTS_PER_CTRL; i++) {
+			if (!rtt_park &&
+			    (ddr->cs[i].config & SDRAM_CS_CONFIG_EN)) {
+				esdmode5 |= 0x00000500;	/* RTT_PARK */
+				rtt_park = 1;
+			} else {
+				esdmode5 = 0x00000400;
+			}
 			switch (i) {
 			case 1:
 				ddr->ddr_sdram_mode_11 = (0
@@ -1747,9 +1766,17 @@ static void set_ddr_sdram_clk_cntl(fsl_ddr_cfg_regs_t *ddr,
 					 const memctl_options_t *popts)
 {
 	unsigned int clk_adjust;	/* Clock adjust */
+	unsigned int ss_en = 0;		/* Source synchronous enable */
 
+#if defined(CONFIG_MPC8541) || defined(CONFIG_MPC8555)
+	/* Per FSL Application Note: AN2805 */
+	ss_en = 1;
+#endif
 	clk_adjust = popts->clk_adjust;
-	ddr->ddr_sdram_clk_cntl = (clk_adjust & 0xF) << 23;
+	ddr->ddr_sdram_clk_cntl = (0
+				   | ((ss_en & 0x1) << 31)
+				   | ((clk_adjust & 0xF) << 23)
+				   );
 	debug("FSLDDR: clk_cntl = 0x%08x\n", ddr->ddr_sdram_clk_cntl);
 }
 
@@ -1962,31 +1989,41 @@ static void set_ddr_dq_mapping(fsl_ddr_cfg_regs_t *ddr,
 			       const dimm_params_t *dimm_params)
 {
 	unsigned int acc_ecc_en = (ddr->ddr_sdram_cfg >> 2) & 0x1;
+	int i;
+
+	for (i = 0; i < CONFIG_DIMM_SLOTS_PER_CTLR; i++) {
+		if (dimm_params[i].n_ranks)
+			break;
+	}
+	if (i >= CONFIG_DIMM_SLOTS_PER_CTLR) {
+		puts("DDR error: no DIMM found!\n");
+		return;
+	}
 
-	ddr->dq_map_0 = ((dimm_params->dq_mapping[0] & 0x3F) << 26) |
-			((dimm_params->dq_mapping[1] & 0x3F) << 20) |
-			((dimm_params->dq_mapping[2] & 0x3F) << 14) |
-			((dimm_params->dq_mapping[3] & 0x3F) << 8) |
-			((dimm_params->dq_mapping[4] & 0x3F) << 2);
+	ddr->dq_map_0 = ((dimm_params[i].dq_mapping[0] & 0x3F) << 26) |
+			((dimm_params[i].dq_mapping[1] & 0x3F) << 20) |
+			((dimm_params[i].dq_mapping[2] & 0x3F) << 14) |
+			((dimm_params[i].dq_mapping[3] & 0x3F) << 8) |
+			((dimm_params[i].dq_mapping[4] & 0x3F) << 2);
 
-	ddr->dq_map_1 = ((dimm_params->dq_mapping[5] & 0x3F) << 26) |
-			((dimm_params->dq_mapping[6] & 0x3F) << 20) |
-			((dimm_params->dq_mapping[7] & 0x3F) << 14) |
-			((dimm_params->dq_mapping[10] & 0x3F) << 8) |
-			((dimm_params->dq_mapping[11] & 0x3F) << 2);
+	ddr->dq_map_1 = ((dimm_params[i].dq_mapping[5] & 0x3F) << 26) |
+			((dimm_params[i].dq_mapping[6] & 0x3F) << 20) |
+			((dimm_params[i].dq_mapping[7] & 0x3F) << 14) |
+			((dimm_params[i].dq_mapping[10] & 0x3F) << 8) |
+			((dimm_params[i].dq_mapping[11] & 0x3F) << 2);
 
-	ddr->dq_map_2 = ((dimm_params->dq_mapping[12] & 0x3F) << 26) |
-			((dimm_params->dq_mapping[13] & 0x3F) << 20) |
-			((dimm_params->dq_mapping[14] & 0x3F) << 14) |
-			((dimm_params->dq_mapping[15] & 0x3F) << 8) |
-			((dimm_params->dq_mapping[16] & 0x3F) << 2);
+	ddr->dq_map_2 = ((dimm_params[i].dq_mapping[12] & 0x3F) << 26) |
+			((dimm_params[i].dq_mapping[13] & 0x3F) << 20) |
+			((dimm_params[i].dq_mapping[14] & 0x3F) << 14) |
+			((dimm_params[i].dq_mapping[15] & 0x3F) << 8) |
+			((dimm_params[i].dq_mapping[16] & 0x3F) << 2);
 
 	/* dq_map for ECC[4:7] is set to 0 if accumulated ECC is enabled */
-	ddr->dq_map_3 = ((dimm_params->dq_mapping[17] & 0x3F) << 26) |
-			((dimm_params->dq_mapping[8] & 0x3F) << 20) |
+	ddr->dq_map_3 = ((dimm_params[i].dq_mapping[17] & 0x3F) << 26) |
+			((dimm_params[i].dq_mapping[8] & 0x3F) << 20) |
 			(acc_ecc_en ? 0 :
-			 (dimm_params->dq_mapping[9] & 0x3F) << 14) |
-			dimm_params->dq_mapping_ors;
+			 (dimm_params[i].dq_mapping[9] & 0x3F) << 14) |
+			dimm_params[i].dq_mapping_ors;
 
 	debug("FSLDDR: dq_map_0 = 0x%08x\n", ddr->dq_map_0);
 	debug("FSLDDR: dq_map_1 = 0x%08x\n", ddr->dq_map_1);
@@ -2349,7 +2386,7 @@ compute_fsl_memctl_config_regs(const unsigned int ctrl_num,
 	set_ddr_cdr1(ddr, popts);
 	set_ddr_cdr2(ddr, popts);
 	set_ddr_sdram_cfg(ddr, popts, common_dimm);
-	ip_rev = fsl_ddr_get_version();
+	ip_rev = fsl_ddr_get_version(ctrl_num);
 	if (ip_rev > 0x40400)
 		unq_mrs_en = 1;
 
diff --git a/drivers/ddr/fsl/ddr4_dimm_params.c b/drivers/ddr/fsl/ddr4_dimm_params.c
index bbfb4ee..42834ca 100644
--- a/drivers/ddr/fsl/ddr4_dimm_params.c
+++ b/drivers/ddr/fsl/ddr4_dimm_params.c
@@ -135,7 +135,8 @@ unsigned int ddr_compute_dimm_parameters(const unsigned int ctrl_num,
 
 	if (spd->mem_type) {
 		if (spd->mem_type != SPD_MEMTYPE_DDR4) {
-			printf("DIMM %u: is not a DDR4 SPD.\n", dimm_number);
+			printf("Ctrl %u DIMM %u: is not a DDR4 SPD.\n",
+			       ctrl_num, dimm_number);
 			return 1;
 		}
 	} else {
diff --git a/drivers/ddr/fsl/fsl_ddr_gen4.c b/drivers/ddr/fsl/fsl_ddr_gen4.c
index d9fce7d..49e4688 100644
--- a/drivers/ddr/fsl/fsl_ddr_gen4.c
+++ b/drivers/ddr/fsl/fsl_ddr_gen4.c
@@ -1,5 +1,5 @@
 /*
- * Copyright 2014 Freescale Semiconductor, Inc.
+ * Copyright 2014-2015 Freescale Semiconductor, Inc.
  *
  * SPDX-License-Identifier:	GPL-2.0+
  */
@@ -11,6 +11,22 @@
 #include <fsl_immap.h>
 #include <fsl_ddr.h>
 
+#ifdef CONFIG_SYS_FSL_ERRATUM_A008511
+static void set_wait_for_bits_clear(void *ptr, u32 value, u32 bits)
+{
+	int timeout = 1000;
+
+	ddr_out32(ptr, value);
+
+	while (ddr_in32(ptr) & bits) {
+		udelay(100);
+		timeout--;
+	}
+	if (timeout <= 0)
+		puts("Error: A007865 wait for clear timeout.\n");
+}
+#endif /* CONFIG_SYS_FSL_ERRATUM_A008511 */
+
 #if (CONFIG_CHIP_SELECTS_PER_CTRL > 4)
 #error Invalid setting for CONFIG_CHIP_SELECTS_PER_CTRL
 #endif
@@ -36,6 +52,16 @@ void fsl_ddr_set_memctl_regs(const fsl_ddr_cfg_regs_t *regs,
 	defined(CONFIG_SYS_FSL_ERRATUM_A008514)
 	u32 *eddrtqcr1;
 #endif
+#ifdef CONFIG_SYS_FSL_ERRATUM_A008511
+	u32 temp32, mr6;
+#endif
+#ifdef CONFIG_FSL_DDR_BIST
+	u32 mtcr, err_detect, err_sbe;
+	u32 cs0_bnds, cs1_bnds, cs2_bnds, cs3_bnds, cs0_config;
+#endif
+#ifdef CONFIG_FSL_DDR_BIST
+	char buffer[CONFIG_SYS_CBSIZE];
+#endif
 
 	switch (ctrl_num) {
 	case 0:
@@ -214,6 +240,21 @@ void fsl_ddr_set_memctl_regs(const fsl_ddr_cfg_regs_t *regs,
 		ddr_setbits32(ddr->debug[28], 0x9 << 20);
 #endif
 
+#ifdef CONFIG_SYS_FSL_ERRATUM_A008511
+	/* Part 1 of 2 */
+	/* This erraum only applies to verion 5.2.0 */
+	if (fsl_ddr_get_version(ctrl_num) == 0x50200) {
+		/* Disable DRAM VRef training */
+		ddr_out32(&ddr->ddr_cdr2,
+			  regs->ddr_cdr2 & ~DDR_CDR2_VREF_TRAIN_EN);
+		/* Disable deskew */
+		ddr_out32(&ddr->debug[28], 0x400);
+		/* Disable D_INIT */
+		ddr_out32(&ddr->sdram_cfg_2,
+			  regs->ddr_sdram_cfg_2 & ~SDRAM_CFG2_D_INIT);
+		ddr_out32(&ddr->debug[25], 0x9000);
+	}
+#endif
 	/*
 	 * For RDIMMs, JEDEC spec requires clocks to be stable before reset is
 	 * deasserted. Clocks start when any chip select is enabled and clock
@@ -261,6 +302,66 @@ step2:
 	mb();
 	isb();
 
+#ifdef CONFIG_SYS_FSL_ERRATUM_A008511
+	/* Part 2 of 2 */
+	/* This erraum only applies to verion 5.2.0 */
+	if (fsl_ddr_get_version(ctrl_num) == 0x50200) {
+		/* Wait for idle */
+		timeout = 200;
+		while (!(ddr_in32(&ddr->debug[1]) & 0x2) &&
+		       (timeout > 0)) {
+			udelay(100);
+			timeout--;
+		}
+		if (timeout <= 0) {
+			printf("Controler %d timeout, debug_2 = %x\n",
+			       ctrl_num, ddr_in32(&ddr->debug[1]));
+		}
+		/* Set VREF */
+		for (i = 0; i < CONFIG_CHIP_SELECTS_PER_CTRL; i++) {
+			if (!(regs->cs[i].config & SDRAM_CS_CONFIG_EN))
+				continue;
+
+			mr6 = (regs->ddr_sdram_mode_10 >> 16)		|
+				 MD_CNTL_MD_EN				|
+				 MD_CNTL_CS_SEL(i)			|
+				 MD_CNTL_MD_SEL(6)			|
+				 0x00200000;
+			temp32 = mr6 | 0xc0;
+			set_wait_for_bits_clear(&ddr->sdram_md_cntl,
+						temp32, MD_CNTL_MD_EN);
+			udelay(1);
+			debug("MR6 = 0x%08x\n", temp32);
+			temp32 = mr6 | 0xf0;
+			set_wait_for_bits_clear(&ddr->sdram_md_cntl,
+						temp32, MD_CNTL_MD_EN);
+			udelay(1);
+			debug("MR6 = 0x%08x\n", temp32);
+			temp32 = mr6 | 0x70;
+			set_wait_for_bits_clear(&ddr->sdram_md_cntl,
+						temp32, MD_CNTL_MD_EN);
+			udelay(1);
+			debug("MR6 = 0x%08x\n", temp32);
+		}
+		ddr_out32(&ddr->sdram_md_cntl, 0);
+		ddr_out32(&ddr->debug[28], 0);		/* Enable deskew */
+		ddr_out32(&ddr->debug[1], 0x400);	/* restart deskew */
+		/* wait for idle */
+		timeout = 200;
+		while (!(ddr_in32(&ddr->debug[1]) & 0x2) &&
+		       (timeout > 0)) {
+			udelay(100);
+			timeout--;
+		}
+		if (timeout <= 0) {
+			printf("Controler %d timeout, debug_2 = %x\n",
+			       ctrl_num, ddr_in32(&ddr->debug[1]));
+		}
+		/* Restore D_INIT */
+		ddr_out32(&ddr->sdram_cfg_2, regs->ddr_sdram_cfg_2);
+	}
+#endif /* CONFIG_SYS_FSL_ERRATUM_A008511 */
+
 	total_gb_size_per_controller = 0;
 	for (i = 0; i < CONFIG_CHIP_SELECTS_PER_CTRL; i++) {
 		if (!(regs->cs[i].config & 0x80000000))
@@ -309,4 +410,70 @@ step2:
 		ddr_out32(&ddr->sdram_cfg_2, temp_sdram_cfg);
 	}
 #endif
+
+#ifdef CONFIG_FSL_DDR_BIST
+#define BIST_PATTERN1	0xFFFFFFFF
+#define BIST_PATTERN2	0x0
+#define BIST_CR		0x80010000
+#define BIST_CR_EN	0x80000000
+#define BIST_CR_STAT	0x00000001
+#define CTLR_INTLV_MASK	0x20000000
+	/* Perform build-in test on memory. Three-way interleaving is not yet
+	 * supported by this code. */
+	if (getenv_f("ddr_bist", buffer, CONFIG_SYS_CBSIZE) >= 0) {
+		puts("Running BIST test. This will take a while...");
+		cs0_config = ddr_in32(&ddr->cs0_config);
+		if (cs0_config & CTLR_INTLV_MASK) {
+			cs0_bnds = ddr_in32(&cs0_bnds);
+			cs1_bnds = ddr_in32(&cs1_bnds);
+			cs2_bnds = ddr_in32(&cs2_bnds);
+			cs3_bnds = ddr_in32(&cs3_bnds);
+			/* set bnds to non-interleaving */
+			ddr_out32(&cs0_bnds, (cs0_bnds & 0xfffefffe) >> 1);
+			ddr_out32(&cs1_bnds, (cs1_bnds & 0xfffefffe) >> 1);
+			ddr_out32(&cs2_bnds, (cs2_bnds & 0xfffefffe) >> 1);
+			ddr_out32(&cs3_bnds, (cs3_bnds & 0xfffefffe) >> 1);
+		}
+		ddr_out32(&ddr->mtp1, BIST_PATTERN1);
+		ddr_out32(&ddr->mtp2, BIST_PATTERN1);
+		ddr_out32(&ddr->mtp3, BIST_PATTERN2);
+		ddr_out32(&ddr->mtp4, BIST_PATTERN2);
+		ddr_out32(&ddr->mtp5, BIST_PATTERN1);
+		ddr_out32(&ddr->mtp6, BIST_PATTERN1);
+		ddr_out32(&ddr->mtp7, BIST_PATTERN2);
+		ddr_out32(&ddr->mtp8, BIST_PATTERN2);
+		ddr_out32(&ddr->mtp9, BIST_PATTERN1);
+		ddr_out32(&ddr->mtp10, BIST_PATTERN2);
+		mtcr = BIST_CR;
+		ddr_out32(&ddr->mtcr, mtcr);
+		timeout = 100;
+		while (timeout > 0 && (mtcr & BIST_CR_EN)) {
+			mdelay(1000);
+			timeout--;
+			mtcr = ddr_in32(&ddr->mtcr);
+		}
+		if (timeout <= 0)
+			puts("Timeout\n");
+		else
+			puts("Done\n");
+		err_detect = ddr_in32(&ddr->err_detect);
+		err_sbe = ddr_in32(&ddr->err_sbe);
+		if (mtcr & BIST_CR_STAT) {
+			printf("BIST test failed on controller %d.\n",
+			       ctrl_num);
+		}
+		if (err_detect || (err_sbe & 0xffff)) {
+			printf("ECC error detected on controller %d.\n",
+			       ctrl_num);
+		}
+
+		if (cs0_config & CTLR_INTLV_MASK) {
+			/* restore bnds registers */
+			ddr_out32(&cs0_bnds, cs0_bnds);
+			ddr_out32(&cs1_bnds, cs1_bnds);
+			ddr_out32(&cs2_bnds, cs2_bnds);
+			ddr_out32(&cs3_bnds, cs3_bnds);
+		}
+	}
+#endif
 }
diff --git a/drivers/ddr/fsl/interactive.c b/drivers/ddr/fsl/interactive.c
index 32ba6d8..d23e6e5 100644
--- a/drivers/ddr/fsl/interactive.c
+++ b/drivers/ddr/fsl/interactive.c
@@ -205,6 +205,8 @@ static void lowest_common_dimm_parameters_edit(fsl_ddr_info_t *pinfo,
 
 #define DIMM_PARM(x) {#x, offsetof(dimm_params_t, x), \
 	sizeof((dimm_params_t *)0)->x, 0}
+#define DIMM_PARM_HEX(x) {#x, offsetof(dimm_params_t, x), \
+	sizeof((dimm_params_t *)0)->x, 1}
 
 static void fsl_ddr_dimm_parameters_edit(fsl_ddr_info_t *pinfo,
 				   unsigned int ctrl_num,
@@ -220,6 +222,7 @@ static void fsl_ddr_dimm_parameters_edit(fsl_ddr_info_t *pinfo,
 		DIMM_PARM(primary_sdram_width),
 		DIMM_PARM(ec_sdram_width),
 		DIMM_PARM(registered_dimm),
+		DIMM_PARM(mirrored_dimm),
 		DIMM_PARM(device_width),
 
 		DIMM_PARM(n_row_addr),
@@ -274,7 +277,27 @@ static void fsl_ddr_dimm_parameters_edit(fsl_ddr_info_t *pinfo,
 		DIMM_PARM(tdqsq_max_ps),
 		DIMM_PARM(tqhs_ps),
 #endif
-
+#ifdef CONFIG_SYS_FSL_DDR4
+		DIMM_PARM_HEX(dq_mapping[0]),
+		DIMM_PARM_HEX(dq_mapping[1]),
+		DIMM_PARM_HEX(dq_mapping[2]),
+		DIMM_PARM_HEX(dq_mapping[3]),
+		DIMM_PARM_HEX(dq_mapping[4]),
+		DIMM_PARM_HEX(dq_mapping[5]),
+		DIMM_PARM_HEX(dq_mapping[6]),
+		DIMM_PARM_HEX(dq_mapping[7]),
+		DIMM_PARM_HEX(dq_mapping[8]),
+		DIMM_PARM_HEX(dq_mapping[9]),
+		DIMM_PARM_HEX(dq_mapping[10]),
+		DIMM_PARM_HEX(dq_mapping[11]),
+		DIMM_PARM_HEX(dq_mapping[12]),
+		DIMM_PARM_HEX(dq_mapping[13]),
+		DIMM_PARM_HEX(dq_mapping[14]),
+		DIMM_PARM_HEX(dq_mapping[15]),
+		DIMM_PARM_HEX(dq_mapping[16]),
+		DIMM_PARM_HEX(dq_mapping[17]),
+		DIMM_PARM(dq_mapping_ors),
+#endif
 		DIMM_PARM(rank_density),
 		DIMM_PARM(capacity),
 		DIMM_PARM(base_address),
@@ -296,6 +319,7 @@ static void print_dimm_parameters(const dimm_params_t *pdimm)
 		DIMM_PARM(primary_sdram_width),
 		DIMM_PARM(ec_sdram_width),
 		DIMM_PARM(registered_dimm),
+		DIMM_PARM(mirrored_dimm),
 		DIMM_PARM(device_width),
 
 		DIMM_PARM(n_row_addr),
@@ -314,6 +338,7 @@ static void print_dimm_parameters(const dimm_params_t *pdimm)
 		DIMM_PARM(tckmax_ps),
 
 		DIMM_PARM(caslat_x),
+		DIMM_PARM_HEX(caslat_x),
 		DIMM_PARM(taa_ps),
 		DIMM_PARM(caslat_x_minus_1),
 		DIMM_PARM(caslat_x_minus_2),
@@ -322,6 +347,9 @@ static void print_dimm_parameters(const dimm_params_t *pdimm)
 		DIMM_PARM(trcd_ps),
 		DIMM_PARM(trp_ps),
 		DIMM_PARM(tras_ps),
+#if defined(CONFIG_SYS_FSL_DDR4) || defined(CONFIG_SYS_FSL_DDR3)
+		DIMM_PARM(tfaw_ps),
+#endif
 #ifdef CONFIG_SYS_FSL_DDR4
 		DIMM_PARM(trfc1_ps),
 		DIMM_PARM(trfc2_ps),
@@ -347,6 +375,27 @@ static void print_dimm_parameters(const dimm_params_t *pdimm)
 		DIMM_PARM(tdqsq_max_ps),
 		DIMM_PARM(tqhs_ps),
 #endif
+#ifdef CONFIG_SYS_FSL_DDR4
+		DIMM_PARM_HEX(dq_mapping[0]),
+		DIMM_PARM_HEX(dq_mapping[1]),
+		DIMM_PARM_HEX(dq_mapping[2]),
+		DIMM_PARM_HEX(dq_mapping[3]),
+		DIMM_PARM_HEX(dq_mapping[4]),
+		DIMM_PARM_HEX(dq_mapping[5]),
+		DIMM_PARM_HEX(dq_mapping[6]),
+		DIMM_PARM_HEX(dq_mapping[7]),
+		DIMM_PARM_HEX(dq_mapping[8]),
+		DIMM_PARM_HEX(dq_mapping[9]),
+		DIMM_PARM_HEX(dq_mapping[10]),
+		DIMM_PARM_HEX(dq_mapping[11]),
+		DIMM_PARM_HEX(dq_mapping[12]),
+		DIMM_PARM_HEX(dq_mapping[13]),
+		DIMM_PARM_HEX(dq_mapping[14]),
+		DIMM_PARM_HEX(dq_mapping[15]),
+		DIMM_PARM_HEX(dq_mapping[16]),
+		DIMM_PARM_HEX(dq_mapping[17]),
+		DIMM_PARM(dq_mapping_ors),
+#endif
 	};
 	static const unsigned int n_opts = ARRAY_SIZE(options);
 
@@ -463,7 +512,7 @@ static void fsl_ddr_options_edit(fsl_ddr_info_t *pinfo,
 		CTRL_OPTIONS_CS(3, odt_rd_cfg),
 		CTRL_OPTIONS_CS(3, odt_wr_cfg),
 #endif
-#if defined(CONFIG_SYS_FSL_DDR3)
+#if defined(CONFIG_SYS_FSL_DDR3) || defined(CONFIG_SYS_FSL_DDR4)
 		CTRL_OPTIONS_CS(0, odt_rtt_norm),
 		CTRL_OPTIONS_CS(0, odt_rtt_wr),
 #if (CONFIG_CHIP_SELECTS_PER_CTRL > 1)
@@ -753,7 +802,7 @@ static void print_memctl_options(const memctl_options_t *popts)
 		CTRL_OPTIONS_CS(3, odt_rd_cfg),
 		CTRL_OPTIONS_CS(3, odt_wr_cfg),
 #endif
-#if defined(CONFIG_SYS_FSL_DDR3)
+#if defined(CONFIG_SYS_FSL_DDR3) || defined(CONFIG_SYS_FSL_DDR4)
 		CTRL_OPTIONS_CS(0, odt_rtt_norm),
 		CTRL_OPTIONS_CS(0, odt_rtt_wr),
 #if (CONFIG_CHIP_SELECTS_PER_CTRL > 1)
@@ -795,6 +844,7 @@ static void print_memctl_options(const memctl_options_t *popts)
 		CTRL_OPTIONS(twot_en),
 		CTRL_OPTIONS(threet_en),
 		CTRL_OPTIONS(registered_dimm_en),
+		CTRL_OPTIONS(mirrored_dimm),
 		CTRL_OPTIONS(ap_en),
 		CTRL_OPTIONS(x4_en),
 		CTRL_OPTIONS(bstopre),
diff --git a/drivers/ddr/fsl/lc_common_dimm_params.c b/drivers/ddr/fsl/lc_common_dimm_params.c
index b295344..b12eeb9 100644
--- a/drivers/ddr/fsl/lc_common_dimm_params.c
+++ b/drivers/ddr/fsl/lc_common_dimm_params.c
@@ -22,7 +22,7 @@ compute_cas_latency(const unsigned int ctrl_num,
 	unsigned int common_caslat;
 	unsigned int caslat_actual;
 	unsigned int retry = 16;
-	unsigned int tmp;
+	unsigned int tmp = ~0;
 	const unsigned int mclk_ps = get_memory_clk_period_ps(ctrl_num);
 #ifdef CONFIG_SYS_FSL_DDR3
 	const unsigned int taamax = 20000;
@@ -31,8 +31,7 @@ compute_cas_latency(const unsigned int ctrl_num,
 #endif
 
 	/* compute the common CAS latency supported between slots */
-	tmp = dimm_params[0].caslat_x;
-	for (i = 1; i < number_of_dimms; i++) {
+	for (i = 0; i < number_of_dimms; i++) {
 		if (dimm_params[i].n_ranks)
 			tmp &= dimm_params[i].caslat_x;
 	}
diff --git a/drivers/ddr/fsl/main.c b/drivers/ddr/fsl/main.c
index b72b242..fa22383 100644
--- a/drivers/ddr/fsl/main.c
+++ b/drivers/ddr/fsl/main.c
@@ -453,7 +453,7 @@ fsl_ddr_compute(fsl_ddr_info_t *pinfo, unsigned int start_step,
 				retval = compute_dimm_parameters(
 							i, spd, pdimm, j);
 #ifdef CONFIG_SYS_DDR_RAW_TIMING
-				if (!i && !j && retval) {
+				if (!j && retval) {
 					printf("SPD error on controller %d! "
 					"Trying fallback to raw timing "
 					"calculation\n", i);
diff --git a/drivers/ddr/fsl/options.c b/drivers/ddr/fsl/options.c
index 5beb11b..3b30fa2 100644
--- a/drivers/ddr/fsl/options.c
+++ b/drivers/ddr/fsl/options.c
@@ -728,7 +728,12 @@ unsigned int populate_memctl_options(int all_dimms_registered,
 
 	/* Choose ddr controller address mirror mode */
 #if defined(CONFIG_SYS_FSL_DDR3) || defined(CONFIG_SYS_FSL_DDR4)
-	popts->mirrored_dimm = pdimm[0].mirrored_dimm;
+	for (i = 0; i < CONFIG_DIMM_SLOTS_PER_CTLR; i++) {
+		if (pdimm[i].n_ranks) {
+			popts->mirrored_dimm = pdimm[i].mirrored_dimm;
+			break;
+		}
+	}
 #endif
 
 	/* Global Timing Parameters. */
diff --git a/drivers/ddr/fsl/util.c b/drivers/ddr/fsl/util.c
index 664081b..ce55aea 100644
--- a/drivers/ddr/fsl/util.c
+++ b/drivers/ddr/fsl/util.c
@@ -23,12 +23,34 @@
 
 #define ULL_8FS 0xFFFFFFFFULL
 
-u32 fsl_ddr_get_version(void)
+u32 fsl_ddr_get_version(unsigned int ctrl_num)
 {
 	struct ccsr_ddr __iomem *ddr;
 	u32 ver_major_minor_errata;
 
-	ddr = (void *)_DDR_ADDR;
+	switch (ctrl_num) {
+	case 0:
+		ddr = (void *)CONFIG_SYS_FSL_DDR_ADDR;
+		break;
+#if defined(CONFIG_SYS_FSL_DDR2_ADDR) && (CONFIG_NUM_DDR_CONTROLLERS > 1)
+	case 1:
+		ddr = (void *)CONFIG_SYS_FSL_DDR2_ADDR;
+		break;
+#endif
+#if defined(CONFIG_SYS_FSL_DDR3_ADDR) && (CONFIG_NUM_DDR_CONTROLLERS > 2)
+	case 2:
+		ddr = (void *)CONFIG_SYS_FSL_DDR3_ADDR;
+		break;
+#endif
+#if defined(CONFIG_SYS_FSL_DDR4_ADDR) && (CONFIG_NUM_DDR_CONTROLLERS > 3)
+	case 3:
+		ddr = (void *)CONFIG_SYS_FSL_DDR4_ADDR;
+		break;
+#endif
+	default:
+		printf("%s unexpected ctrl_num = %u\n", __func__, ctrl_num);
+		return 0;
+	}
 	ver_major_minor_errata = (ddr_in32(&ddr->ip_rev1) & 0xFFFF) << 8;
 	ver_major_minor_errata |= (ddr_in32(&ddr->ip_rev2) & 0xFF00) >> 8;
 
@@ -212,7 +234,7 @@ void print_ddr_info(unsigned int start_ctrl)
 
 	/* Calculate CAS latency based on timing cfg values */
 	cas_lat = ((ddr_in32(&ddr->timing_cfg_1) >> 16) & 0xf);
-	if (fsl_ddr_get_version() <= 0x40400)
+	if (fsl_ddr_get_version(0) <= 0x40400)
 		cas_lat += 1;
 	else
 		cas_lat += 2;
diff --git a/drivers/demo/demo-simple.c b/drivers/demo/demo-simple.c
index 2bcb7df..f069748 100644
--- a/drivers/demo/demo-simple.c
+++ b/drivers/demo/demo-simple.c
@@ -10,6 +10,7 @@
 #include <common.h>
 #include <dm.h>
 #include <dm-demo.h>
+#include <mapmem.h>
 #include <asm/io.h>
 
 static int simple_hello(struct udevice *dev, int ch)
diff --git a/drivers/gpio/Kconfig b/drivers/gpio/Kconfig
index 7b5178a..0840a30 100644
--- a/drivers/gpio/Kconfig
+++ b/drivers/gpio/Kconfig
@@ -14,3 +14,24 @@ config LPC32XX_GPIO
 	default n
 	help
 	  Support for the LPC32XX GPIO driver.
+
+config SANDBOX_GPIO
+	bool "Enable sandbox GPIO driver"
+	depends on SANDBOX && DM && DM_GPIO
+	help
+	  This driver supports some simulated GPIOs which can be adjusted
+	  using 'back door' functions like sandbox_gpio_set_value(). Then the
+	  GPIOs can be inspected through the normal get_get_value()
+	  interface. The purpose of this is to allow GPIOs to be used as
+	  normal in sandbox, perhaps with test code actually driving the
+	  behaviour of those GPIOs.
+
+config SANDBOX_GPIO_COUNT
+	int "Number of sandbox GPIOs"
+	depends on SANDBOX_GPIO
+	default 128
+	help
+	  The sandbox driver can support any number of GPIOs. Generally these
+	  are specified using the device tree. But you can also have a number
+	  of 'anonymous' GPIOs that do not belong to any device or bank.
+	  Select a suitable value depending on your needs.
diff --git a/drivers/gpio/Makefile b/drivers/gpio/Makefile
index 85f71c5..fb40e09 100644
--- a/drivers/gpio/Makefile
+++ b/drivers/gpio/Makefile
@@ -42,3 +42,5 @@ obj-$(CONFIG_TCA642X)		+= tca642x.o
 oby-$(CONFIG_SX151X)		+= sx151x.o
 obj-$(CONFIG_SUNXI_GPIO)	+= sunxi_gpio.o
 obj-$(CONFIG_LPC32XX_GPIO)	+= lpc32xx_gpio.o
+obj-$(CONFIG_STM32_GPIO)	+= stm32_gpio.o
+obj-$(CONFIG_ZYNQ_GPIO)		+= zynq_gpio.o
diff --git a/drivers/gpio/at91_gpio.c b/drivers/gpio/at91_gpio.c
index 22fbd63..75a32ee 100644
--- a/drivers/gpio/at91_gpio.c
+++ b/drivers/gpio/at91_gpio.c
@@ -511,7 +511,7 @@ static int at91_gpio_probe(struct udevice *dev)
 {
 	struct at91_port_priv *port = dev_get_priv(dev);
 	struct at91_port_platdata *plat = dev_get_platdata(dev);
-	struct gpio_dev_priv *uc_priv = dev->uclass_priv;
+	struct gpio_dev_priv *uc_priv = dev_get_uclass_priv(dev);
 
 	uc_priv->bank_name = plat->bank_name;
 	uc_priv->gpio_count = GPIO_PER_BANK;
diff --git a/drivers/gpio/bcm2835_gpio.c b/drivers/gpio/bcm2835_gpio.c
index 0244c01..fbc641d 100644
--- a/drivers/gpio/bcm2835_gpio.c
+++ b/drivers/gpio/bcm2835_gpio.c
@@ -105,7 +105,7 @@ static int bcm2835_gpio_probe(struct udevice *dev)
 {
 	struct bcm2835_gpios *gpios = dev_get_priv(dev);
 	struct bcm2835_gpio_platdata *plat = dev_get_platdata(dev);
-	struct gpio_dev_priv *uc_priv = dev->uclass_priv;
+	struct gpio_dev_priv *uc_priv = dev_get_uclass_priv(dev);
 
 	uc_priv->bank_name = "GPIO";
 	uc_priv->gpio_count = BCM2835_GPIO_COUNT;
diff --git a/drivers/gpio/gpio-uclass.c b/drivers/gpio/gpio-uclass.c
index a69bbd2..381868b 100644
--- a/drivers/gpio/gpio-uclass.c
+++ b/drivers/gpio/gpio-uclass.c
@@ -34,7 +34,7 @@ static int gpio_to_device(unsigned int gpio, struct gpio_desc *desc)
 	for (ret = uclass_first_device(UCLASS_GPIO, &dev);
 	     dev;
 	     ret = uclass_next_device(&dev)) {
-		uc_priv = dev->uclass_priv;
+		uc_priv = dev_get_uclass_priv(dev);
 		if (gpio >= uc_priv->gpio_base &&
 		    gpio < uc_priv->gpio_base + uc_priv->gpio_count) {
 			desc->dev = dev;
@@ -65,7 +65,7 @@ int gpio_lookup_name(const char *name, struct udevice **devp,
 	     ret = uclass_next_device(&dev)) {
 		int len;
 
-		uc_priv = dev->uclass_priv;
+		uc_priv = dev_get_uclass_priv(dev);
 		if (numeric != -1) {
 			offset = numeric - uc_priv->gpio_base;
 			/* Allow GPIOs to be numbered from 0 */
@@ -116,7 +116,7 @@ static int dm_gpio_request(struct gpio_desc *desc, const char *label)
 	char *str;
 	int ret;
 
-	uc_priv = dev->uclass_priv;
+	uc_priv = dev_get_uclass_priv(dev);
 	if (uc_priv->name[desc->offset])
 		return -EBUSY;
 	str = strdup(label);
@@ -195,7 +195,7 @@ int _dm_gpio_free(struct udevice *dev, uint offset)
 	struct gpio_dev_priv *uc_priv;
 	int ret;
 
-	uc_priv = dev->uclass_priv;
+	uc_priv = dev_get_uclass_priv(dev);
 	if (!uc_priv->name[offset])
 		return -ENXIO;
 	if (gpio_get_ops(dev)->free) {
@@ -232,7 +232,7 @@ int gpio_free(unsigned gpio)
 
 static int check_reserved(struct gpio_desc *desc, const char *func)
 {
-	struct gpio_dev_priv *uc_priv = desc->dev->uclass_priv;
+	struct gpio_dev_priv *uc_priv = dev_get_uclass_priv(desc->dev);
 
 	if (!uc_priv->name[desc->offset]) {
 		printf("%s: %s: error: gpio %s%d not reserved\n",
@@ -402,7 +402,7 @@ const char *gpio_get_bank_info(struct udevice *dev, int *bit_count)
 	struct gpio_dev_priv *priv;
 
 	/* Must be called on an active device */
-	priv = dev->uclass_priv;
+	priv = dev_get_uclass_priv(dev);
 	assert(priv);
 
 	*bit_count = priv->gpio_count;
@@ -420,7 +420,7 @@ static const char * const gpio_function[GPIOF_COUNT] = {
 int get_function(struct udevice *dev, int offset, bool skip_unused,
 		 const char **namep)
 {
-	struct gpio_dev_priv *uc_priv = dev->uclass_priv;
+	struct gpio_dev_priv *uc_priv = dev_get_uclass_priv(dev);
 	struct dm_gpio_ops *ops = gpio_get_ops(dev);
 
 	BUILD_BUG_ON(GPIOF_COUNT != ARRAY_SIZE(gpio_function));
@@ -468,7 +468,7 @@ int gpio_get_status(struct udevice *dev, int offset, char *buf, int buffsize)
 	BUILD_BUG_ON(GPIOF_COUNT != ARRAY_SIZE(gpio_function));
 
 	*buf = 0;
-	priv = dev->uclass_priv;
+	priv = dev_get_uclass_priv(dev);
 	ret = gpio_get_raw_function(dev, offset, NULL);
 	if (ret < 0)
 		return ret;
@@ -590,11 +590,7 @@ int gpio_request_list_by_name_nodev(const void *blob, int node,
 	int count;
 	int ret;
 
-	for (count = 0; ; count++) {
-		if (count >= max_count) {
-			ret = -ENOSPC;
-			goto err;
-		}
+	for (count = 0; count < max_count; count++) {
 		ret = _gpio_request_by_name_nodev(blob, node, list_name, count,
 						  &desc[count], flags, true);
 		if (ret == -ENOENT)
@@ -680,7 +676,7 @@ static int gpio_renumber(struct udevice *removed_dev)
 	base = 0;
 	uclass_foreach_dev(dev, uc) {
 		if (device_active(dev) && dev != removed_dev) {
-			uc_priv = dev->uclass_priv;
+			uc_priv = dev_get_uclass_priv(dev);
 			uc_priv->gpio_base = base;
 			base += uc_priv->gpio_count;
 		}
@@ -689,9 +685,21 @@ static int gpio_renumber(struct udevice *removed_dev)
 	return 0;
 }
 
+int gpio_get_number(struct gpio_desc *desc)
+{
+	struct udevice *dev = desc->dev;
+	struct gpio_dev_priv *uc_priv;
+
+	if (!dev)
+		return -1;
+	uc_priv = dev->uclass_priv;
+
+	return uc_priv->gpio_base + desc->offset;
+}
+
 static int gpio_post_probe(struct udevice *dev)
 {
-	struct gpio_dev_priv *uc_priv = dev->uclass_priv;
+	struct gpio_dev_priv *uc_priv = dev_get_uclass_priv(dev);
 
 	uc_priv->name = calloc(uc_priv->gpio_count, sizeof(char *));
 	if (!uc_priv->name)
@@ -702,7 +710,7 @@ static int gpio_post_probe(struct udevice *dev)
 
 static int gpio_pre_remove(struct udevice *dev)
 {
-	struct gpio_dev_priv *uc_priv = dev->uclass_priv;
+	struct gpio_dev_priv *uc_priv = dev_get_uclass_priv(dev);
 	int i;
 
 	for (i = 0; i < uc_priv->gpio_count; i++) {
diff --git a/drivers/gpio/intel_ich6_gpio.c b/drivers/gpio/intel_ich6_gpio.c
index 7720cc3..7e679a0 100644
--- a/drivers/gpio/intel_ich6_gpio.c
+++ b/drivers/gpio/intel_ich6_gpio.c
@@ -64,13 +64,13 @@ static int gpio_ich6_ofdata_to_platdata(struct udevice *dev)
 	pci_dev = PCI_BDF(0, 0x1f, 0);
 
 	/* Is the device present? */
-	tmpword = pci_read_config16(pci_dev, PCI_VENDOR_ID);
+	tmpword = x86_pci_read_config16(pci_dev, PCI_VENDOR_ID);
 	if (tmpword != PCI_VENDOR_ID_INTEL) {
 		debug("%s: wrong VendorID\n", __func__);
 		return -ENODEV;
 	}
 
-	tmpword = pci_read_config16(pci_dev, PCI_DEVICE_ID);
+	tmpword = x86_pci_read_config16(pci_dev, PCI_DEVICE_ID);
 	debug("Found %04x:%04x\n", PCI_VENDOR_ID_INTEL, tmpword);
 	/*
 	 * We'd like to validate the Device ID too, but pretty much any
@@ -80,34 +80,34 @@ static int gpio_ich6_ofdata_to_platdata(struct udevice *dev)
 	 */
 
 	/* I/O should already be enabled (it's a RO bit). */
-	tmpword = pci_read_config16(pci_dev, PCI_COMMAND);
+	tmpword = x86_pci_read_config16(pci_dev, PCI_COMMAND);
 	if (!(tmpword & PCI_COMMAND_IO)) {
 		debug("%s: device IO not enabled\n", __func__);
 		return -ENODEV;
 	}
 
 	/* Header Type must be normal (bits 6-0 only; see spec.) */
-	tmpbyte = pci_read_config8(pci_dev, PCI_HEADER_TYPE);
+	tmpbyte = x86_pci_read_config8(pci_dev, PCI_HEADER_TYPE);
 	if ((tmpbyte & 0x7f) != PCI_HEADER_TYPE_NORMAL) {
 		debug("%s: invalid Header type\n", __func__);
 		return -ENODEV;
 	}
 
 	/* Base Class must be a bridge device */
-	tmpbyte = pci_read_config8(pci_dev, PCI_CLASS_CODE);
+	tmpbyte = x86_pci_read_config8(pci_dev, PCI_CLASS_CODE);
 	if (tmpbyte != PCI_CLASS_CODE_BRIDGE) {
 		debug("%s: invalid class\n", __func__);
 		return -ENODEV;
 	}
 	/* Sub Class must be ISA */
-	tmpbyte = pci_read_config8(pci_dev, PCI_CLASS_SUB_CODE);
+	tmpbyte = x86_pci_read_config8(pci_dev, PCI_CLASS_SUB_CODE);
 	if (tmpbyte != PCI_CLASS_SUB_CODE_BRIDGE_ISA) {
 		debug("%s: invalid subclass\n", __func__);
 		return -ENODEV;
 	}
 
 	/* Programming Interface must be 0x00 (no others exist) */
-	tmpbyte = pci_read_config8(pci_dev, PCI_CLASS_PROG);
+	tmpbyte = x86_pci_read_config8(pci_dev, PCI_CLASS_PROG);
 	if (tmpbyte != 0x00) {
 		debug("%s: invalid interface type\n", __func__);
 		return -ENODEV;
@@ -123,7 +123,7 @@ static int gpio_ich6_ofdata_to_platdata(struct udevice *dev)
 	 * while on the Ivybridge the bit0 is used to indicate it is an
 	 * I/O space.
 	 */
-	tmplong = pci_read_config32(pci_dev, PCI_CFG_GPIOBASE);
+	tmplong = x86_pci_read_config32(pci_dev, PCI_CFG_GPIOBASE);
 	if (tmplong == 0x00000000 || tmplong == 0xffffffff) {
 		debug("%s: unexpected GPIOBASE value\n", __func__);
 		return -ENODEV;
@@ -151,7 +151,7 @@ static int gpio_ich6_ofdata_to_platdata(struct udevice *dev)
 static int ich6_gpio_probe(struct udevice *dev)
 {
 	struct ich6_bank_platdata *plat = dev_get_platdata(dev);
-	struct gpio_dev_priv *uc_priv = dev->uclass_priv;
+	struct gpio_dev_priv *uc_priv = dev_get_uclass_priv(dev);
 	struct ich6_bank_priv *bank = dev_get_priv(dev);
 
 	if (gd->arch.gpio_map) {
diff --git a/drivers/gpio/mvgpio.h b/drivers/gpio/mvgpio.h
index a3f17a0..1de7395 100644
--- a/drivers/gpio/mvgpio.h
+++ b/drivers/gpio/mvgpio.h
@@ -14,9 +14,8 @@
 
 #include <common.h>
 
-#ifdef CONFIG_SHEEVA_88SV331xV5
 /*
- * GPIO Register map for SHEEVA 88SV331xV5
+ * GPIO Register map for Marvell SOCs
  */
 struct gpio_reg {
 	u32 gplr;	/* Pin Level Register - 0x0000 */
@@ -51,8 +50,5 @@ struct gpio_reg {
 	u32 pad12[2];
 	u32 apmask;	/* Bitwise Mask of Edge Detect Register - 0x009C */
 };
-#else
-#error "CPU core subversion not defined"
-#endif
 
 #endif /* __MVGPIO_H__ */
diff --git a/drivers/gpio/mvmfp.c b/drivers/gpio/mvmfp.c
index 97bbe99..43ecf66 100644
--- a/drivers/gpio/mvmfp.c
+++ b/drivers/gpio/mvmfp.c
@@ -43,18 +43,8 @@ void mfp_config(u32 *mfp_cfgs)
 
 		/* Write a mfg register as per configuration */
 		val = 0;
-		if (cfg_val & MFP_AF_FLAG)
-			/* Abstract and program Afternate-Func Selection */
-			val |= cfg_val & MFP_AF_MASK;
-		if (cfg_val & MFP_EDGE_FLAG)
-			/* Abstract and program Edge configuration */
-			val |= cfg_val & MFP_LPM_EDGE_MASK;
-		if (cfg_val & MFP_DRIVE_FLAG)
-			/* Abstract and program Drive configuration */
-			val |= cfg_val & MFP_DRIVE_MASK;
-		if (cfg_val & MFP_PULL_FLAG)
-			/* Abstract and program Pullup/down configuration */
-			val |= cfg_val & MFP_PULL_MASK;
+		if (cfg_val & MFP_VALUE_MASK)
+			val |= cfg_val & MFP_VALUE_MASK;
 
 		writel(val, p_mfpr);
 	} while (1);
diff --git a/drivers/gpio/mxc_gpio.c b/drivers/gpio/mxc_gpio.c
index 815407b..2012f99 100644
--- a/drivers/gpio/mxc_gpio.c
+++ b/drivers/gpio/mxc_gpio.c
@@ -266,7 +266,7 @@ static int mxc_gpio_probe(struct udevice *dev)
 {
 	struct mxc_bank_info *bank = dev_get_priv(dev);
 	struct mxc_gpio_plat *plat = dev_get_platdata(dev);
-	struct gpio_dev_priv *uc_priv = dev->uclass_priv;
+	struct gpio_dev_priv *uc_priv = dev_get_uclass_priv(dev);
 	int banknum;
 	char name[18], *str;
 
diff --git a/drivers/gpio/omap_gpio.c b/drivers/gpio/omap_gpio.c
index 19fc451..0a1e124 100644
--- a/drivers/gpio/omap_gpio.c
+++ b/drivers/gpio/omap_gpio.c
@@ -309,7 +309,7 @@ static int omap_gpio_probe(struct udevice *dev)
 {
 	struct gpio_bank *bank = dev_get_priv(dev);
 	struct omap_gpio_platdata *plat = dev_get_platdata(dev);
-	struct gpio_dev_priv *uc_priv = dev->uclass_priv;
+	struct gpio_dev_priv *uc_priv = dev_get_uclass_priv(dev);
 	char name[18], *str;
 
 	sprintf(name, "GPIO%d_", plat->bank_index);
diff --git a/drivers/gpio/s5p_gpio.c b/drivers/gpio/s5p_gpio.c
index 0a245ba..49b1054 100644
--- a/drivers/gpio/s5p_gpio.c
+++ b/drivers/gpio/s5p_gpio.c
@@ -296,7 +296,7 @@ static const struct dm_gpio_ops gpio_exynos_ops = {
 
 static int gpio_exynos_probe(struct udevice *dev)
 {
-	struct gpio_dev_priv *uc_priv = dev->uclass_priv;
+	struct gpio_dev_priv *uc_priv = dev_get_uclass_priv(dev);
 	struct exynos_bank_info *priv = dev->priv;
 	struct exynos_gpio_platdata *plat = dev->platdata;
 
diff --git a/drivers/gpio/sandbox.c b/drivers/gpio/sandbox.c
index d564c25..a9b1efc 100644
--- a/drivers/gpio/sandbox.c
+++ b/drivers/gpio/sandbox.c
@@ -24,7 +24,7 @@ struct gpio_state {
 /* Access routines for GPIO state */
 static u8 *get_gpio_flags(struct udevice *dev, unsigned offset)
 {
-	struct gpio_dev_priv *uc_priv = dev->uclass_priv;
+	struct gpio_dev_priv *uc_priv = dev_get_uclass_priv(dev);
 	struct gpio_state *state = dev_get_priv(dev);
 
 	if (offset >= uc_priv->gpio_count) {
@@ -160,7 +160,7 @@ static const struct dm_gpio_ops gpio_sandbox_ops = {
 
 static int sandbox_gpio_ofdata_to_platdata(struct udevice *dev)
 {
-	struct gpio_dev_priv *uc_priv = dev->uclass_priv;
+	struct gpio_dev_priv *uc_priv = dev_get_uclass_priv(dev);
 
 	uc_priv->gpio_count = fdtdec_get_int(gd->fdt_blob, dev->of_offset,
 					     "num-gpios", 0);
@@ -172,7 +172,7 @@ static int sandbox_gpio_ofdata_to_platdata(struct udevice *dev)
 
 static int gpio_sandbox_probe(struct udevice *dev)
 {
-	struct gpio_dev_priv *uc_priv = dev->uclass_priv;
+	struct gpio_dev_priv *uc_priv = dev_get_uclass_priv(dev);
 
 	if (dev->of_offset == -1) {
 		/* Tell the uclass how many GPIOs we have */
diff --git a/drivers/gpio/stm32_gpio.c b/drivers/gpio/stm32_gpio.c
new file mode 100644
index 0000000..d3497e9
--- /dev/null
+++ b/drivers/gpio/stm32_gpio.c
@@ -0,0 +1,199 @@
+/*
+ * (C) Copyright 2011
+ * Yuri Tikhonov, Emcraft Systems, yur@emcraft.com
+ *
+ * (C) Copyright 2015
+ * Kamil Lulko, <rev13@wp.pl>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <asm/io.h>
+#include <asm/errno.h>
+#include <asm/arch/stm32.h>
+#include <asm/arch/gpio.h>
+
+DECLARE_GLOBAL_DATA_PTR;
+
+#define STM32_GPIOA_BASE	(STM32_AHB1PERIPH_BASE + 0x0000)
+#define STM32_GPIOB_BASE	(STM32_AHB1PERIPH_BASE + 0x0400)
+#define STM32_GPIOC_BASE	(STM32_AHB1PERIPH_BASE + 0x0800)
+#define STM32_GPIOD_BASE	(STM32_AHB1PERIPH_BASE + 0x0C00)
+#define STM32_GPIOE_BASE	(STM32_AHB1PERIPH_BASE + 0x1000)
+#define STM32_GPIOF_BASE	(STM32_AHB1PERIPH_BASE + 0x1400)
+#define STM32_GPIOG_BASE	(STM32_AHB1PERIPH_BASE + 0x1800)
+#define STM32_GPIOH_BASE	(STM32_AHB1PERIPH_BASE + 0x1C00)
+#define STM32_GPIOI_BASE	(STM32_AHB1PERIPH_BASE + 0x2000)
+
+static const unsigned long io_base[] = {
+	STM32_GPIOA_BASE, STM32_GPIOB_BASE, STM32_GPIOC_BASE,
+	STM32_GPIOD_BASE, STM32_GPIOE_BASE, STM32_GPIOF_BASE,
+	STM32_GPIOG_BASE, STM32_GPIOH_BASE, STM32_GPIOI_BASE
+};
+
+struct stm32_gpio_regs {
+	u32 moder;	/* GPIO port mode */
+	u32 otyper;	/* GPIO port output type */
+	u32 ospeedr;	/* GPIO port output speed */
+	u32 pupdr;	/* GPIO port pull-up/pull-down */
+	u32 idr;	/* GPIO port input data */
+	u32 odr;	/* GPIO port output data */
+	u32 bsrr;	/* GPIO port bit set/reset */
+	u32 lckr;	/* GPIO port configuration lock */
+	u32 afr[2];	/* GPIO alternate function */
+};
+
+#define CHECK_DSC(x)	(!x || x->port > 8 || x->pin > 15)
+#define CHECK_CTL(x)	(!x || x->af > 15 || x->mode > 3 || x->otype > 1 || \
+			x->pupd > 2 || x->speed > 3)
+
+int stm32_gpio_config(const struct stm32_gpio_dsc *dsc,
+		const struct stm32_gpio_ctl *ctl)
+{
+	struct stm32_gpio_regs *gpio_regs;
+	u32 i;
+	int rv;
+
+	if (CHECK_DSC(dsc)) {
+		rv = -EINVAL;
+		goto out;
+	}
+	if (CHECK_CTL(ctl)) {
+		rv = -EINVAL;
+		goto out;
+	}
+
+	gpio_regs = (struct stm32_gpio_regs *)io_base[dsc->port];
+
+	setbits_le32(&STM32_RCC->ahb1enr, 1 << dsc->port);
+
+	i = (dsc->pin & 0x07) * 4;
+	clrbits_le32(&gpio_regs->afr[dsc->pin >> 3], (0xF << i));
+	setbits_le32(&gpio_regs->afr[dsc->pin >> 3], ctl->af << i);
+
+	i = dsc->pin * 2;
+
+	clrbits_le32(&gpio_regs->moder, (0x3 << i));
+	setbits_le32(&gpio_regs->moder, ctl->mode << i);
+
+	clrbits_le32(&gpio_regs->otyper, (0x3 << i));
+	setbits_le32(&gpio_regs->otyper, ctl->otype << i);
+
+	clrbits_le32(&gpio_regs->ospeedr, (0x3 << i));
+	setbits_le32(&gpio_regs->ospeedr, ctl->speed << i);
+
+	clrbits_le32(&gpio_regs->pupdr, (0x3 << i));
+	setbits_le32(&gpio_regs->pupdr, ctl->pupd << i);
+
+	rv = 0;
+out:
+	return rv;
+}
+
+int stm32_gpout_set(const struct stm32_gpio_dsc *dsc, int state)
+{
+	struct stm32_gpio_regs	*gpio_regs;
+	int rv;
+
+	if (CHECK_DSC(dsc)) {
+		rv = -EINVAL;
+		goto out;
+	}
+
+	gpio_regs = (struct stm32_gpio_regs *)io_base[dsc->port];
+
+	if (state)
+		writel(1 << dsc->pin, &gpio_regs->bsrr);
+	else
+		writel(1 << (dsc->pin + 16), &gpio_regs->bsrr);
+
+	rv = 0;
+out:
+	return rv;
+}
+
+int stm32_gpin_get(const struct stm32_gpio_dsc *dsc)
+{
+	struct stm32_gpio_regs	*gpio_regs;
+	int rv;
+
+	if (CHECK_DSC(dsc)) {
+		rv = -EINVAL;
+		goto out;
+	}
+
+	gpio_regs = (struct stm32_gpio_regs *)io_base[dsc->port];
+	rv = readl(&gpio_regs->idr) & (1 << dsc->pin);
+out:
+	return rv;
+}
+
+/* Common GPIO API */
+
+int gpio_request(unsigned gpio, const char *label)
+{
+	return 0;
+}
+
+int gpio_free(unsigned gpio)
+{
+	return 0;
+}
+
+int gpio_direction_input(unsigned gpio)
+{
+	struct stm32_gpio_dsc dsc;
+	struct stm32_gpio_ctl ctl;
+
+	dsc.port = stm32_gpio_to_port(gpio);
+	dsc.pin = stm32_gpio_to_pin(gpio);
+	ctl.af = STM32_GPIO_AF0;
+	ctl.mode = STM32_GPIO_MODE_IN;
+	ctl.pupd = STM32_GPIO_PUPD_NO;
+	ctl.speed = STM32_GPIO_SPEED_50M;
+
+	return stm32_gpio_config(&dsc, &ctl);
+}
+
+int gpio_direction_output(unsigned gpio, int value)
+{
+	struct stm32_gpio_dsc dsc;
+	struct stm32_gpio_ctl ctl;
+	int res;
+
+	dsc.port = stm32_gpio_to_port(gpio);
+	dsc.pin = stm32_gpio_to_pin(gpio);
+	ctl.af = STM32_GPIO_AF0;
+	ctl.mode = STM32_GPIO_MODE_OUT;
+	ctl.otype = STM32_GPIO_OTYPE_PP;
+	ctl.pupd = STM32_GPIO_PUPD_NO;
+	ctl.speed = STM32_GPIO_SPEED_50M;
+
+	res = stm32_gpio_config(&dsc, &ctl);
+	if (res < 0)
+		goto out;
+	res = stm32_gpout_set(&dsc, value);
+out:
+	return res;
+}
+
+int gpio_get_value(unsigned gpio)
+{
+	struct stm32_gpio_dsc dsc;
+
+	dsc.port = stm32_gpio_to_port(gpio);
+	dsc.pin = stm32_gpio_to_pin(gpio);
+
+	return stm32_gpin_get(&dsc);
+}
+
+int gpio_set_value(unsigned gpio, int value)
+{
+	struct stm32_gpio_dsc dsc;
+
+	dsc.port = stm32_gpio_to_port(gpio);
+	dsc.pin = stm32_gpio_to_pin(gpio);
+
+	return stm32_gpout_set(&dsc, value);
+}
diff --git a/drivers/gpio/sunxi_gpio.c b/drivers/gpio/sunxi_gpio.c
index 6296092..cf5c624 100644
--- a/drivers/gpio/sunxi_gpio.c
+++ b/drivers/gpio/sunxi_gpio.c
@@ -21,6 +21,9 @@
 #ifdef CONFIG_AXP209_POWER
 #include <axp209.h>
 #endif
+#ifdef CONFIG_AXP221_POWER
+#include <axp221.h>
+#endif
 
 DECLARE_GLOBAL_DATA_PTR;
 
@@ -115,6 +118,20 @@ int gpio_set_value(unsigned gpio, int value)
 	return sunxi_gpio_output(gpio, value);
 }
 
+int sunxi_name_to_gpio_bank(const char *name)
+{
+	int group = 0;
+
+	if (*name == 'P' || *name == 'p')
+		name++;
+	if (*name >= 'A') {
+		group = *name - (*name > 'a' ? 'a' : 'A');
+		return group;
+	}
+
+	return -1;
+}
+
 int sunxi_name_to_gpio(const char *name)
 {
 	int group = 0;
@@ -125,6 +142,12 @@ int sunxi_name_to_gpio(const char *name)
 #ifdef AXP_GPIO
 	if (strncasecmp(name, "AXP0-", 5) == 0) {
 		name += 5;
+		if (strcmp(name, "VBUS-DETECT") == 0)
+			return SUNXI_GPIO_AXP0_START +
+				SUNXI_GPIO_AXP0_VBUS_DETECT;
+		if (strcmp(name, "VBUS-ENABLE") == 0)
+			return SUNXI_GPIO_AXP0_START +
+				SUNXI_GPIO_AXP0_VBUS_ENABLE;
 		pin = simple_strtol(name, &eptr, 10);
 		if (!*name || *eptr)
 			return -1;
@@ -238,7 +261,7 @@ static char *gpio_bank_name(int bank)
 static int gpio_sunxi_probe(struct udevice *dev)
 {
 	struct sunxi_gpio_platdata *plat = dev_get_platdata(dev);
-	struct gpio_dev_priv *uc_priv = dev->uclass_priv;
+	struct gpio_dev_priv *uc_priv = dev_get_uclass_priv(dev);
 
 	/* Tell the uclass how many GPIOs we have */
 	if (plat) {
diff --git a/drivers/gpio/tegra_gpio.c b/drivers/gpio/tegra_gpio.c
index f870cdb..8017e35 100644
--- a/drivers/gpio/tegra_gpio.c
+++ b/drivers/gpio/tegra_gpio.c
@@ -295,7 +295,7 @@ static const struct udevice_id tegra_gpio_ids[] = {
 
 static int gpio_tegra_probe(struct udevice *dev)
 {
-	struct gpio_dev_priv *uc_priv = dev->uclass_priv;
+	struct gpio_dev_priv *uc_priv = dev_get_uclass_priv(dev);
 	struct tegra_port_info *priv = dev->priv;
 	struct tegra_gpio_platdata *plat = dev->platdata;
 
diff --git a/drivers/gpio/zynq_gpio.c b/drivers/gpio/zynq_gpio.c
new file mode 100644
index 0000000..83a2c46
--- /dev/null
+++ b/drivers/gpio/zynq_gpio.c
@@ -0,0 +1,220 @@
+/*
+ * Xilinx Zynq GPIO device driver
+ *
+ * Copyright (C) 2015 DAVE Embedded Systems <devel@dave.eu>
+ *
+ * Most of code taken from linux kernel driver (linux/drivers/gpio/gpio-zynq.c)
+ * Copyright (C) 2009 - 2014 Xilinx, Inc.
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <asm/gpio.h>
+#include <asm/io.h>
+#include <asm/errno.h>
+
+/**
+ * zynq_gpio_get_bank_pin - Get the bank number and pin number within that bank
+ * for a given pin in the GPIO device
+ * @pin_num:	gpio pin number within the device
+ * @bank_num:	an output parameter used to return the bank number of the gpio
+ *		pin
+ * @bank_pin_num: an output parameter used to return pin number within a bank
+ *		  for the given gpio pin
+ *
+ * Returns the bank number and pin offset within the bank.
+ */
+static inline void zynq_gpio_get_bank_pin(unsigned int pin_num,
+					  unsigned int *bank_num,
+					  unsigned int *bank_pin_num)
+{
+	switch (pin_num) {
+	case ZYNQ_GPIO_BANK0_PIN_MIN ... ZYNQ_GPIO_BANK0_PIN_MAX:
+		*bank_num = 0;
+		*bank_pin_num = pin_num;
+		break;
+	case ZYNQ_GPIO_BANK1_PIN_MIN ... ZYNQ_GPIO_BANK1_PIN_MAX:
+		*bank_num = 1;
+		*bank_pin_num = pin_num - ZYNQ_GPIO_BANK1_PIN_MIN;
+		break;
+	case ZYNQ_GPIO_BANK2_PIN_MIN ... ZYNQ_GPIO_BANK2_PIN_MAX:
+		*bank_num = 2;
+		*bank_pin_num = pin_num - ZYNQ_GPIO_BANK2_PIN_MIN;
+		break;
+	case ZYNQ_GPIO_BANK3_PIN_MIN ... ZYNQ_GPIO_BANK3_PIN_MAX:
+		*bank_num = 3;
+		*bank_pin_num = pin_num - ZYNQ_GPIO_BANK3_PIN_MIN;
+		break;
+	default:
+		printf("invalid GPIO pin number: %u\n", pin_num);
+		*bank_num = 0;
+		*bank_pin_num = 0;
+		break;
+	}
+}
+
+int gpio_is_valid(unsigned gpio)
+{
+	return (gpio >= 0) && (gpio < ZYNQ_GPIO_NR_GPIOS);
+}
+
+static int check_gpio(unsigned gpio)
+{
+	if (!gpio_is_valid(gpio)) {
+		printf("ERROR : check_gpio: invalid GPIO %d\n", gpio);
+		return -1;
+	}
+	return 0;
+}
+
+/**
+ * gpio_get_value - Get the state of the specified pin of GPIO device
+ * @gpio:	gpio pin number within the device
+ *
+ * This function reads the state of the specified pin of the GPIO device.
+ *
+ * Return: 0 if the pin is low, 1 if pin is high.
+ */
+int gpio_get_value(unsigned gpio)
+{
+	u32 data;
+	unsigned int bank_num, bank_pin_num;
+
+	if (check_gpio(gpio) < 0)
+		return -1;
+
+	zynq_gpio_get_bank_pin(gpio, &bank_num, &bank_pin_num);
+
+	data = readl(ZYNQ_GPIO_BASE_ADDRESS +
+			     ZYNQ_GPIO_DATA_RO_OFFSET(bank_num));
+
+	return (data >> bank_pin_num) & 1;
+}
+
+/**
+ * gpio_set_value - Modify the value of the pin with specified value
+ * @gpio:	gpio pin number within the device
+ * @value:	value used to modify the value of the specified pin
+ *
+ * This function calculates the register offset (i.e to lower 16 bits or
+ * upper 16 bits) based on the given pin number and sets the value of a
+ * gpio pin to the specified value. The value is either 0 or non-zero.
+ */
+int gpio_set_value(unsigned gpio, int value)
+{
+	unsigned int reg_offset, bank_num, bank_pin_num;
+
+	if (check_gpio(gpio) < 0)
+		return -1;
+
+	zynq_gpio_get_bank_pin(gpio, &bank_num, &bank_pin_num);
+
+	if (bank_pin_num >= ZYNQ_GPIO_MID_PIN_NUM) {
+		/* only 16 data bits in bit maskable reg */
+		bank_pin_num -= ZYNQ_GPIO_MID_PIN_NUM;
+		reg_offset = ZYNQ_GPIO_DATA_MSW_OFFSET(bank_num);
+	} else {
+		reg_offset = ZYNQ_GPIO_DATA_LSW_OFFSET(bank_num);
+	}
+
+	/*
+	 * get the 32 bit value to be written to the mask/data register where
+	 * the upper 16 bits is the mask and lower 16 bits is the data
+	 */
+	value = !!value;
+	value = ~(1 << (bank_pin_num + ZYNQ_GPIO_MID_PIN_NUM)) &
+		((value << bank_pin_num) | ZYNQ_GPIO_UPPER_MASK);
+
+	writel(value, ZYNQ_GPIO_BASE_ADDRESS + reg_offset);
+
+	return 0;
+}
+
+/**
+ * gpio_direction_input - Set the direction of the specified GPIO pin as input
+ * @gpio:	gpio pin number within the device
+ *
+ * This function uses the read-modify-write sequence to set the direction of
+ * the gpio pin as input.
+ *
+ * Return: -1 if invalid gpio specified, 0 if successul
+ */
+int gpio_direction_input(unsigned gpio)
+{
+	u32 reg;
+	unsigned int bank_num, bank_pin_num;
+
+	if (check_gpio(gpio) < 0)
+		return -1;
+
+	zynq_gpio_get_bank_pin(gpio, &bank_num, &bank_pin_num);
+
+	/* bank 0 pins 7 and 8 are special and cannot be used as inputs */
+	if (bank_num == 0 && (bank_pin_num == 7 || bank_pin_num == 8))
+		return -1;
+
+	/* clear the bit in direction mode reg to set the pin as input */
+	reg = readl(ZYNQ_GPIO_BASE_ADDRESS + ZYNQ_GPIO_DIRM_OFFSET(bank_num));
+	reg &= ~BIT(bank_pin_num);
+	writel(reg, ZYNQ_GPIO_BASE_ADDRESS + ZYNQ_GPIO_DIRM_OFFSET(bank_num));
+
+	return 0;
+}
+
+/**
+ * gpio_direction_output - Set the direction of the specified GPIO pin as output
+ * @gpio:	gpio pin number within the device
+ * @value:	value to be written to specified pin
+ *
+ * This function sets the direction of specified GPIO pin as output, configures
+ * the Output Enable register for the pin and uses zynq_gpio_set to set
+ * the value of the pin to the value specified.
+ *
+ * Return: 0 always
+ */
+int gpio_direction_output(unsigned gpio, int value)
+{
+	u32 reg;
+	unsigned int bank_num, bank_pin_num;
+
+	if (check_gpio(gpio) < 0)
+		return -1;
+
+	zynq_gpio_get_bank_pin(gpio, &bank_num, &bank_pin_num);
+
+	/* set the GPIO pin as output */
+	reg = readl(ZYNQ_GPIO_BASE_ADDRESS + ZYNQ_GPIO_DIRM_OFFSET(bank_num));
+	reg |= BIT(bank_pin_num);
+	writel(reg, ZYNQ_GPIO_BASE_ADDRESS + ZYNQ_GPIO_DIRM_OFFSET(bank_num));
+
+	/* configure the output enable reg for the pin */
+	reg = readl(ZYNQ_GPIO_BASE_ADDRESS + ZYNQ_GPIO_OUTEN_OFFSET(bank_num));
+	reg |= BIT(bank_pin_num);
+	writel(reg, ZYNQ_GPIO_BASE_ADDRESS + ZYNQ_GPIO_OUTEN_OFFSET(bank_num));
+
+	/* set the state of the pin */
+	gpio_set_value(gpio, value);
+	return 0;
+}
+
+/**
+ * Request a gpio before using it.
+ *
+ * NOTE: Argument 'label' is unused.
+ */
+int gpio_request(unsigned gpio, const char *label)
+{
+	if (check_gpio(gpio) < 0)
+		return -1;
+
+	return 0;
+}
+
+/**
+ * Reset and free the gpio after using it.
+ */
+int gpio_free(unsigned gpio)
+{
+	return 0;
+}
diff --git a/drivers/i2c/Kconfig b/drivers/i2c/Kconfig
index 692810d..ba43019 100644
--- a/drivers/i2c/Kconfig
+++ b/drivers/i2c/Kconfig
@@ -2,16 +2,13 @@ config DM_I2C
 	bool "Enable Driver Model for I2C drivers"
 	depends on DM
 	help
-	  Enable driver model for I2C. This SPI flash interface
-	  (spi_flash_probe(), spi_flash_write(), etc.) is then
-	  implemented by the SPI flash uclass. There is one standard
-	  SPI flash driver which knows how to probe most chips
-	  supported by U-Boot. The uclass interface is defined in
-	  include/spi_flash.h, but is currently fully compatible
-	  with the old interface to avoid confusion and duplication
-	  during the transition parent. SPI and SPI flash must be
-	  enabled together (it is not possible to use driver model
-	  for one and not the other).
+	  Enable driver model for I2C. The I2C uclass interface: probe, read,
+	  write and speed, is implemented with the bus drivers operations,
+	  which provide methods for bus setting and data transfer. Each chip
+	  device (bus child) info is kept as parent platdata. The interface
+	  is defined in include/i2c.h. When i2c bus driver supports the i2c
+	  uclass, but the device drivers not, then DM_I2C_COMPAT config can
+	  be used as compatibility layer.
 
 config DM_I2C_COMPAT
 	bool "Enable I2C compatibility layer"
@@ -22,6 +19,45 @@ config DM_I2C_COMPAT
 	  to convert all code for a board in a single commit. It should not
 	  be enabled for any board in an official release.
 
+config DM_I2C_GPIO
+	bool "Enable Driver Model for software emulated I2C bus driver"
+	depends on DM_I2C && DM_GPIO
+	help
+	  Enable the i2c bus driver emulation by using the GPIOs. The bus GPIO
+	  configuration is given by the device tree. Kernel-style device tree
+	  bindings are supported.
+	  Binding info: doc/device-tree-bindings/i2c/i2c-gpio.txt
+
+config SYS_I2C_SANDBOX
+	bool "Sandbox I2C driver"
+	depends on SANDBOX && DM_I2C
+	help
+	  Enable I2C support for sandbox. This is an emulation of a real I2C
+	  bus. Devices can be attached to the bus using the device tree
+	  which specifies the driver to use. As an example, see this device
+	  tree fragment from sandbox.dts. It shows that the I2C bus has a
+	  single EEPROM at address 0x2c (7-bit address) which is emulated by
+	  the driver for "sandbox,i2c-eeprom", which is in
+	  drivers/misc/i2c_eeprom_emul.c.
+
+	  i2c@0 {
+		#address-cells = <1>;
+		#size-cells = <0>;
+		reg = <0>;
+		compatible = "sandbox,i2c";
+		clock-frequency = <400000>;
+		eeprom@2c {
+			reg = <0x2c>;
+			compatible = "i2c-eeprom";
+			emul {
+				compatible = "sandbox,i2c-eeprom";
+				sandbox,filename = "i2c.bin";
+				sandbox,size = <128>;
+			};
+		};
+	};
+
+
 config SYS_I2C_UNIPHIER
 	bool "UniPhier I2C driver"
 	depends on ARCH_UNIPHIER && DM_I2C
diff --git a/drivers/i2c/Makefile b/drivers/i2c/Makefile
index 26ea854..d9e912f 100644
--- a/drivers/i2c/Makefile
+++ b/drivers/i2c/Makefile
@@ -6,6 +6,7 @@
 #
 obj-$(CONFIG_DM_I2C) += i2c-uclass.o
 obj-$(CONFIG_DM_I2C_COMPAT) += i2c-uclass-compat.o
+obj-$(CONFIG_DM_I2C_GPIO) += i2c-gpio.o
 
 obj-$(CONFIG_SYS_I2C_ADI) += adi_i2c.o
 obj-$(CONFIG_I2C_MV) += mv_i2c.o
diff --git a/drivers/i2c/i2c-gpio.c b/drivers/i2c/i2c-gpio.c
new file mode 100644
index 0000000..ed899d4
--- /dev/null
+++ b/drivers/i2c/i2c-gpio.c
@@ -0,0 +1,346 @@
+/*
+ * (C) Copyright 2015, Samsung Electronics
+ * Przemyslaw Marczak <p.marczak@samsung.com>
+ *
+ * This file is based on: drivers/i2c/soft-i2c.c,
+ * with added driver-model support and code cleanup.
+ */
+#include <common.h>
+#include <errno.h>
+#include <dm.h>
+#include <i2c.h>
+#include <asm/gpio.h>
+
+#define DEFAULT_UDELAY	5
+#define RETRIES		0
+#define I2C_ACK		0
+#define I2C_NOACK	1
+
+DECLARE_GLOBAL_DATA_PTR;
+
+enum {
+	PIN_SDA = 0,
+	PIN_SCL,
+	PIN_COUNT,
+};
+
+struct i2c_gpio_bus {
+	/**
+	  * udelay - delay [us] between GPIO toggle operations,
+	  * which is 1/4 of I2C speed clock period.
+	 */
+	int udelay;
+	 /* sda, scl */
+	struct gpio_desc gpios[PIN_COUNT];
+};
+
+static int i2c_gpio_sda_get(struct gpio_desc *sda)
+{
+	return dm_gpio_get_value(sda);
+}
+
+static void i2c_gpio_sda_set(struct gpio_desc *sda, int bit)
+{
+	if (bit) {
+		dm_gpio_set_dir_flags(sda, GPIOD_IS_IN);
+	} else {
+		dm_gpio_set_dir_flags(sda, GPIOD_IS_OUT);
+		dm_gpio_set_value(sda, 0);
+	}
+}
+
+static void i2c_gpio_scl_set(struct gpio_desc *scl, int bit)
+{
+	dm_gpio_set_dir_flags(scl, GPIOD_IS_OUT);
+	dm_gpio_set_value(scl, bit);
+}
+
+static void i2c_gpio_write_bit(struct gpio_desc *scl, struct gpio_desc *sda,
+			       int delay, uchar bit)
+{
+	i2c_gpio_scl_set(scl, 0);
+	udelay(delay);
+	i2c_gpio_sda_set(sda, bit);
+	udelay(delay);
+	i2c_gpio_scl_set(scl, 1);
+	udelay(2 * delay);
+}
+
+static int i2c_gpio_read_bit(struct gpio_desc *scl, struct gpio_desc *sda,
+			     int delay)
+{
+	int value;
+
+	i2c_gpio_scl_set(scl, 1);
+	udelay(delay);
+	value = i2c_gpio_sda_get(sda);
+	udelay(delay);
+	i2c_gpio_scl_set(scl, 0);
+	udelay(2 * delay);
+
+	return value;
+}
+
+/* START: High -> Low on SDA while SCL is High */
+static void i2c_gpio_send_start(struct gpio_desc *scl, struct gpio_desc *sda,
+				int delay)
+{
+	udelay(delay);
+	i2c_gpio_sda_set(sda, 1);
+	udelay(delay);
+	i2c_gpio_scl_set(scl, 1);
+	udelay(delay);
+	i2c_gpio_sda_set(sda, 0);
+	udelay(delay);
+}
+
+/* STOP: Low -> High on SDA while SCL is High */
+static void i2c_gpio_send_stop(struct gpio_desc *scl, struct gpio_desc *sda,
+			       int delay)
+{
+	i2c_gpio_scl_set(scl, 0);
+	udelay(delay);
+	i2c_gpio_sda_set(sda, 0);
+	udelay(delay);
+	i2c_gpio_scl_set(scl, 1);
+	udelay(delay);
+	i2c_gpio_sda_set(sda, 1);
+	udelay(delay);
+}
+
+/* ack should be I2C_ACK or I2C_NOACK */
+static void i2c_gpio_send_ack(struct gpio_desc *scl, struct gpio_desc *sda,
+			      int delay, int ack)
+{
+	i2c_gpio_write_bit(scl, sda, delay, ack);
+	i2c_gpio_scl_set(scl, 0);
+	udelay(delay);
+}
+
+/**
+ * Send a reset sequence consisting of 9 clocks with the data signal high
+ * to clock any confused device back into an idle state.  Also send a
+ * <stop> at the end of the sequence for belts & suspenders.
+ */
+static void i2c_gpio_send_reset(struct gpio_desc *scl, struct gpio_desc *sda,
+				int delay)
+{
+	int j;
+
+	for (j = 0; j < 9; j++)
+		i2c_gpio_write_bit(scl, sda, delay, 1);
+
+	i2c_gpio_send_stop(scl, sda, delay);
+}
+
+/* Set sda high with low clock, before reading slave data */
+static void i2c_gpio_sda_high(struct gpio_desc *scl, struct gpio_desc *sda,
+			      int delay)
+{
+	i2c_gpio_scl_set(scl, 0);
+	udelay(delay);
+	i2c_gpio_sda_set(sda, 1);
+	udelay(delay);
+}
+
+/* Send 8 bits and look for an acknowledgement */
+static int i2c_gpio_write_byte(struct gpio_desc *scl, struct gpio_desc *sda,
+			       int delay, uchar data)
+{
+	int j;
+	int nack;
+
+	for (j = 0; j < 8; j++) {
+		i2c_gpio_write_bit(scl, sda, delay, data & 0x80);
+		data <<= 1;
+	}
+
+	udelay(delay);
+
+	/* Look for an <ACK>(negative logic) and return it */
+	i2c_gpio_sda_high(scl, sda, delay);
+	nack = i2c_gpio_read_bit(scl, sda, delay);
+
+	return nack;	/* not a nack is an ack */
+}
+
+/**
+ * if ack == I2C_ACK, ACK the byte so can continue reading, else
+ * send I2C_NOACK to end the read.
+ */
+static uchar i2c_gpio_read_byte(struct gpio_desc *scl, struct gpio_desc *sda,
+				int delay, int ack)
+{
+	int  data;
+	int  j;
+
+	i2c_gpio_sda_high(scl, sda, delay);
+	data = 0;
+	for (j = 0; j < 8; j++) {
+		data <<= 1;
+		data |= i2c_gpio_read_bit(scl, sda, delay);
+	}
+	i2c_gpio_send_ack(scl, sda, delay, ack);
+
+	return data;
+}
+
+/* send start and the slave chip address */
+int i2c_send_slave_addr(struct gpio_desc *scl, struct gpio_desc *sda, int delay,
+			uchar chip)
+{
+	i2c_gpio_send_start(scl, sda, delay);
+
+	if (i2c_gpio_write_byte(scl, sda, delay, chip)) {
+		i2c_gpio_send_stop(scl, sda, delay);
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static int i2c_gpio_write_data(struct i2c_gpio_bus *bus, uchar chip,
+			       uchar *buffer, int len,
+			       bool end_with_repeated_start)
+{
+	struct gpio_desc *scl = &bus->gpios[PIN_SCL];
+	struct gpio_desc *sda = &bus->gpios[PIN_SDA];
+	unsigned int delay = bus->udelay;
+	int failures = 0;
+
+	debug("%s: chip %x buffer %p len %d\n", __func__, chip, buffer, len);
+
+	if (i2c_send_slave_addr(scl, sda, delay, chip << 1)) {
+		debug("i2c_write, no chip responded %02X\n", chip);
+		return -EIO;
+	}
+
+	while (len-- > 0) {
+		if (i2c_gpio_write_byte(scl, sda, delay, *buffer++))
+			failures++;
+	}
+
+	if (!end_with_repeated_start) {
+		i2c_gpio_send_stop(scl, sda, delay);
+		return failures;
+	}
+
+	if (i2c_send_slave_addr(scl, sda, delay, (chip << 1) | 0x1)) {
+		debug("i2c_write, no chip responded %02X\n", chip);
+		return -EIO;
+	}
+
+	return failures;
+}
+
+static int i2c_gpio_read_data(struct i2c_gpio_bus *bus, uchar chip,
+			      uchar *buffer, int len)
+{
+	struct gpio_desc *scl = &bus->gpios[PIN_SCL];
+	struct gpio_desc *sda = &bus->gpios[PIN_SDA];
+	unsigned int delay = bus->udelay;
+
+	debug("%s: chip %x buffer: %p len %d\n", __func__, chip, buffer, len);
+
+	while (len-- > 0)
+		*buffer++ = i2c_gpio_read_byte(scl, sda, delay, len == 0);
+
+	i2c_gpio_send_stop(scl, sda, delay);
+
+	return 0;
+}
+
+static int i2c_gpio_xfer(struct udevice *dev, struct i2c_msg *msg, int nmsgs)
+{
+	struct i2c_gpio_bus *bus = dev_get_priv(dev);
+	int ret;
+
+	for (; nmsgs > 0; nmsgs--, msg++) {
+		bool next_is_read = nmsgs > 1 && (msg[1].flags & I2C_M_RD);
+
+		if (msg->flags & I2C_M_RD) {
+			ret = i2c_gpio_read_data(bus, msg->addr, msg->buf,
+						 msg->len);
+		} else {
+			ret = i2c_gpio_write_data(bus, msg->addr, msg->buf,
+						  msg->len, next_is_read);
+		}
+
+		if (ret)
+			return -EREMOTEIO;
+	}
+
+	return 0;
+}
+
+static int i2c_gpio_probe(struct udevice *dev, uint chip, uint chip_flags)
+{
+	struct i2c_gpio_bus *bus = dev_get_priv(dev);
+	struct gpio_desc *scl = &bus->gpios[PIN_SCL];
+	struct gpio_desc *sda = &bus->gpios[PIN_SDA];
+	unsigned int delay = bus->udelay;
+	int ret;
+
+	i2c_gpio_send_start(scl, sda, delay);
+	ret = i2c_gpio_write_byte(scl, sda, delay, (chip << 1) | 0);
+	i2c_gpio_send_stop(scl, sda, delay);
+
+	debug("%s: bus: %d (%s) chip: %x flags: %x ret: %d\n",
+	      __func__, dev->seq, dev->name, chip, chip_flags, ret);
+
+	return ret;
+}
+
+static int i2c_gpio_set_bus_speed(struct udevice *dev, unsigned int speed_hz)
+{
+	struct i2c_gpio_bus *bus = dev_get_priv(dev);
+	struct gpio_desc *scl = &bus->gpios[PIN_SCL];
+	struct gpio_desc *sda = &bus->gpios[PIN_SDA];
+
+	bus->udelay = 1000000 / (speed_hz << 2);
+
+	i2c_gpio_send_reset(scl, sda, bus->udelay);
+
+	return 0;
+}
+
+static int i2c_gpio_ofdata_to_platdata(struct udevice *dev)
+{
+	struct i2c_gpio_bus *bus = dev_get_priv(dev);
+	const void *blob = gd->fdt_blob;
+	int node = dev->of_offset;
+	int ret;
+
+	ret = gpio_request_list_by_name(dev, "gpios", bus->gpios,
+					ARRAY_SIZE(bus->gpios), 0);
+	if (ret < 0)
+		goto error;
+
+	bus->udelay = fdtdec_get_int(blob, node, "i2c-gpio,delay-us",
+				     DEFAULT_UDELAY);
+
+	return 0;
+error:
+	error("Can't get %s gpios! Error: %d", dev->name, ret);
+	return ret;
+}
+
+static const struct dm_i2c_ops i2c_gpio_ops = {
+	.xfer		= i2c_gpio_xfer,
+	.probe_chip	= i2c_gpio_probe,
+	.set_bus_speed	= i2c_gpio_set_bus_speed,
+};
+
+static const struct udevice_id i2c_gpio_ids[] = {
+	{ .compatible = "i2c-gpio" },
+	{ }
+};
+
+U_BOOT_DRIVER(i2c_gpio) = {
+	.name	= "i2c-gpio",
+	.id	= UCLASS_I2C,
+	.of_match = i2c_gpio_ids,
+	.ofdata_to_platdata = i2c_gpio_ofdata_to_platdata,
+	.priv_auto_alloc_size = sizeof(struct i2c_gpio_bus),
+	.ops	= &i2c_gpio_ops,
+};
diff --git a/drivers/i2c/i2c-uclass.c b/drivers/i2c/i2c-uclass.c
index b890806..f2e95c0 100644
--- a/drivers/i2c/i2c-uclass.c
+++ b/drivers/i2c/i2c-uclass.c
@@ -330,7 +330,7 @@ int dm_i2c_probe(struct udevice *bus, uint chip_addr, uint chip_flags,
 int dm_i2c_set_bus_speed(struct udevice *bus, unsigned int speed)
 {
 	struct dm_i2c_ops *ops = i2c_get_ops(bus);
-	struct dm_i2c_bus *i2c = bus->uclass_priv;
+	struct dm_i2c_bus *i2c = dev_get_uclass_priv(bus);
 	int ret;
 
 	/*
@@ -351,7 +351,7 @@ int dm_i2c_set_bus_speed(struct udevice *bus, unsigned int speed)
 int dm_i2c_get_bus_speed(struct udevice *bus)
 {
 	struct dm_i2c_ops *ops = i2c_get_ops(bus);
-	struct dm_i2c_bus *i2c = bus->uclass_priv;
+	struct dm_i2c_bus *i2c = dev_get_uclass_priv(bus);
 
 	if (!ops->get_bus_speed)
 		return i2c->speed_hz;
@@ -432,7 +432,7 @@ int i2c_chip_ofdata_to_platdata(const void *blob, int node,
 
 static int i2c_post_probe(struct udevice *dev)
 {
-	struct dm_i2c_bus *i2c = dev->uclass_priv;
+	struct dm_i2c_bus *i2c = dev_get_uclass_priv(dev);
 
 	i2c->speed_hz = fdtdec_get_int(gd->fdt_blob, dev->of_offset,
 				     "clock-frequency", 100000);
diff --git a/drivers/i2c/i2c-uniphier-f.c b/drivers/i2c/i2c-uniphier-f.c
index fd28c17..d29dd45 100644
--- a/drivers/i2c/i2c-uniphier-f.c
+++ b/drivers/i2c/i2c-uniphier-f.c
@@ -14,6 +14,7 @@
 #include <dm/root.h>
 #include <i2c.h>
 #include <fdtdec.h>
+#include <mapmem.h>
 
 DECLARE_GLOBAL_DATA_PTR;
 
diff --git a/drivers/i2c/i2c-uniphier.c b/drivers/i2c/i2c-uniphier.c
index 666272d..c4972ff 100644
--- a/drivers/i2c/i2c-uniphier.c
+++ b/drivers/i2c/i2c-uniphier.c
@@ -14,6 +14,7 @@
 #include <dm/root.h>
 #include <i2c.h>
 #include <fdtdec.h>
+#include <mapmem.h>
 
 DECLARE_GLOBAL_DATA_PTR;
 
diff --git a/drivers/i2c/mvtwsi.c b/drivers/i2c/mvtwsi.c
index 6f6edd5..f20d1b2 100644
--- a/drivers/i2c/mvtwsi.c
+++ b/drivers/i2c/mvtwsi.c
@@ -14,7 +14,7 @@
 #include <asm/io.h>
 
 /*
- * include a file that will provide CONFIG_I2C_MVTWSI_BASE
+ * include a file that will provide CONFIG_I2C_MVTWSI_BASE*
  * and possibly other settings
  */
 
@@ -91,11 +91,39 @@ struct  mvtwsi_registers {
 #define	MVTWSI_STATUS_IDLE		0xF8
 
 /*
- * The single instance of the controller we'll be dealing with
+ * MVTWSI controller base
  */
 
-static struct  mvtwsi_registers *twsi =
-	(struct  mvtwsi_registers *) CONFIG_I2C_MVTWSI_BASE;
+static struct mvtwsi_registers *twsi_get_base(struct i2c_adapter *adap)
+{
+	switch (adap->hwadapnr) {
+#ifdef CONFIG_I2C_MVTWSI_BASE0
+	case 0:
+		return (struct mvtwsi_registers *) CONFIG_I2C_MVTWSI_BASE0;
+#endif
+#ifdef CONFIG_I2C_MVTWSI_BASE1
+	case 1:
+		return (struct mvtwsi_registers *) CONFIG_I2C_MVTWSI_BASE1;
+#endif
+#ifdef CONFIG_I2C_MVTWSI_BASE2
+	case 2:
+		return (struct mvtwsi_registers *) CONFIG_I2C_MVTWSI_BASE2;
+#endif
+#ifdef CONFIG_I2C_MVTWSI_BASE3
+	case 3:
+		return (struct mvtwsi_registers *) CONFIG_I2C_MVTWSI_BASE3;
+#endif
+#ifdef CONFIG_I2C_MVTWSI_BASE4
+	case 4:
+		return (struct mvtwsi_registers *) CONFIG_I2C_MVTWSI_BASE4;
+#endif
+	default:
+		printf("Missing mvtwsi controller %d base\n", adap->hwadapnr);
+		break;
+	}
+
+	return NULL;
+}
 
 /*
  * Returned statuses are 0 for success and nonzero otherwise.
@@ -117,8 +145,9 @@ static struct  mvtwsi_registers *twsi =
  * Wait for IFLG to raise, or return 'timeout'; then if status is as expected,
  * return 0 (ok) or return 'wrong status'.
  */
-static int twsi_wait(int expected_status)
+static int twsi_wait(struct i2c_adapter *adap, int expected_status)
 {
+	struct mvtwsi_registers *twsi = twsi_get_base(adap);
 	int control, status;
 	int timeout = 1000;
 
@@ -153,35 +182,40 @@ static u8 twsi_control_flags = MVTWSI_CONTROL_TWSIEN;
  * Assert the START condition, either in a single I2C transaction
  * or inside back-to-back ones (repeated starts).
  */
-static int twsi_start(int expected_status)
+static int twsi_start(struct i2c_adapter *adap, int expected_status)
 {
+	struct mvtwsi_registers *twsi = twsi_get_base(adap);
+
 	/* globally set TWSIEN in case it was not */
 	twsi_control_flags |= MVTWSI_CONTROL_TWSIEN;
 	/* assert START */
 	writel(twsi_control_flags | MVTWSI_CONTROL_START, &twsi->control);
 	/* wait for controller to process START */
-	return twsi_wait(expected_status);
+	return twsi_wait(adap, expected_status);
 }
 
 /*
  * Send a byte (i2c address or data).
  */
-static int twsi_send(u8 byte, int expected_status)
+static int twsi_send(struct i2c_adapter *adap, u8 byte, int expected_status)
 {
+	struct mvtwsi_registers *twsi = twsi_get_base(adap);
+
 	/* put byte in data register for sending */
 	writel(byte, &twsi->data);
 	/* clear any pending interrupt -- that'll cause sending */
 	writel(twsi_control_flags, &twsi->control);
 	/* wait for controller to receive byte and check ACK */
-	return twsi_wait(expected_status);
+	return twsi_wait(adap, expected_status);
 }
 
 /*
  * Receive a byte.
  * Global mvtwsi_control_flags variable says if we should ack or nak.
  */
-static int twsi_recv(u8 *byte)
+static int twsi_recv(struct i2c_adapter *adap, u8 *byte)
 {
+	struct mvtwsi_registers *twsi = twsi_get_base(adap);
 	int expected_status, status;
 
 	/* compute expected status based on ACK bit in global control flags */
@@ -192,7 +226,7 @@ static int twsi_recv(u8 *byte)
 	/* acknowledge *previous state* and launch receive */
 	writel(twsi_control_flags, &twsi->control);
 	/* wait for controller to receive byte and assert ACK or NAK */
-	status = twsi_wait(expected_status);
+	status = twsi_wait(adap, expected_status);
 	/* if we did receive expected byte then store it */
 	if (status == 0)
 		*byte = readl(&twsi->data);
@@ -204,8 +238,9 @@ static int twsi_recv(u8 *byte)
  * Assert the STOP condition.
  * This is also used to force the bus back in idle (SDA=SCL=1).
  */
-static int twsi_stop(int status)
+static int twsi_stop(struct i2c_adapter *adap, int status)
 {
+	struct mvtwsi_registers *twsi = twsi_get_base(adap);
 	int control, stop_status;
 	int timeout = 1000;
 
@@ -244,6 +279,7 @@ static unsigned int twsi_calc_freq(const int n, const int m)
  */
 static void twsi_reset(struct i2c_adapter *adap)
 {
+	struct mvtwsi_registers *twsi = twsi_get_base(adap);
 	/* ensure controller will be enabled by any twsi*() function */
 	twsi_control_flags = MVTWSI_CONTROL_TWSIEN;
 	/* reset controller */
@@ -259,6 +295,7 @@ static void twsi_reset(struct i2c_adapter *adap)
 static unsigned int twsi_i2c_set_bus_speed(struct i2c_adapter *adap,
 					   unsigned int requested_speed)
 {
+	struct mvtwsi_registers *twsi = twsi_get_base(adap);
 	unsigned int tmp_speed, highest_speed, n, m;
 	unsigned int baud = 0x44; /* baudrate at controller reset */
 
@@ -281,6 +318,8 @@ static unsigned int twsi_i2c_set_bus_speed(struct i2c_adapter *adap,
 
 static void twsi_i2c_init(struct i2c_adapter *adap, int speed, int slaveadd)
 {
+	struct mvtwsi_registers *twsi = twsi_get_base(adap);
+
 	/* reset controller */
 	twsi_reset(adap);
 	/* set speed */
@@ -289,7 +328,7 @@ static void twsi_i2c_init(struct i2c_adapter *adap, int speed, int slaveadd)
 	writel(slaveadd, &twsi->slave_address);
 	writel(0, &twsi->xtnd_slave_addr);
 	/* assert STOP but don't care for the result */
-	(void) twsi_stop(0);
+	(void) twsi_stop(adap, 0);
 }
 
 /*
@@ -297,7 +336,8 @@ static void twsi_i2c_init(struct i2c_adapter *adap, int speed, int slaveadd)
  * Common to i2c_probe, i2c_read and i2c_write.
  * Expected address status will derive from direction bit (bit 0) in addr.
  */
-static int i2c_begin(int expected_start_status, u8 addr)
+static int i2c_begin(struct i2c_adapter *adap, int expected_start_status,
+		     u8 addr)
 {
 	int status, expected_addr_status;
 
@@ -307,10 +347,10 @@ static int i2c_begin(int expected_start_status, u8 addr)
 	else /* writing */
 		expected_addr_status = MVTWSI_STATUS_ADDR_W_ACK;
 	/* assert START */
-	status = twsi_start(expected_start_status);
+	status = twsi_start(adap, expected_start_status);
 	/* send out the address if the start went well */
 	if (status == 0)
-		status = twsi_send(addr, expected_addr_status);
+		status = twsi_send(adap, addr, expected_addr_status);
 	/* return ok or status of first failure to caller */
 	return status;
 }
@@ -325,12 +365,12 @@ static int twsi_i2c_probe(struct i2c_adapter *adap, uchar chip)
 	int status;
 
 	/* begin i2c read */
-	status = i2c_begin(MVTWSI_STATUS_START, (chip << 1) | 1);
+	status = i2c_begin(adap, MVTWSI_STATUS_START, (chip << 1) | 1);
 	/* dummy read was accepted: receive byte but NAK it. */
 	if (status == 0)
-		status = twsi_recv(&dummy_byte);
+		status = twsi_recv(adap, &dummy_byte);
 	/* Stop transaction */
-	twsi_stop(0);
+	twsi_stop(adap, 0);
 	/* return 0 or status of first failure */
 	return status;
 }
@@ -351,15 +391,15 @@ static int twsi_i2c_read(struct i2c_adapter *adap, uchar chip, uint addr,
 	int status;
 
 	/* begin i2c write to send the address bytes */
-	status = i2c_begin(MVTWSI_STATUS_START, (chip << 1));
+	status = i2c_begin(adap, MVTWSI_STATUS_START, (chip << 1));
 	/* send addr bytes */
 	while ((status == 0) && alen--)
-		status = twsi_send(addr >> (8*alen),
+		status = twsi_send(adap, addr >> (8*alen),
 			MVTWSI_STATUS_DATA_W_ACK);
 	/* begin i2c read to receive eeprom data bytes */
 	if (status == 0)
-		status = i2c_begin(
-			MVTWSI_STATUS_REPEATED_START, (chip << 1) | 1);
+		status = i2c_begin(adap, MVTWSI_STATUS_REPEATED_START,
+				   (chip << 1) | 1);
 	/* prepare ACK if at least one byte must be received */
 	if (length > 0)
 		twsi_control_flags |= MVTWSI_CONTROL_ACK;
@@ -369,10 +409,10 @@ static int twsi_i2c_read(struct i2c_adapter *adap, uchar chip, uint addr,
 		if (length == 0)
 			twsi_control_flags &= ~MVTWSI_CONTROL_ACK;
 		/* read current byte */
-		status = twsi_recv(data++);
+		status = twsi_recv(adap, data++);
 	}
 	/* Stop transaction */
-	status = twsi_stop(status);
+	status = twsi_stop(adap, status);
 	/* return 0 or status of first failure */
 	return status;
 }
@@ -387,21 +427,51 @@ static int twsi_i2c_write(struct i2c_adapter *adap, uchar chip, uint addr,
 	int status;
 
 	/* begin i2c write to send the eeprom adress bytes then data bytes */
-	status = i2c_begin(MVTWSI_STATUS_START, (chip << 1));
+	status = i2c_begin(adap, MVTWSI_STATUS_START, (chip << 1));
 	/* send addr bytes */
 	while ((status == 0) && alen--)
-		status = twsi_send(addr >> (8*alen),
+		status = twsi_send(adap, addr >> (8*alen),
 			MVTWSI_STATUS_DATA_W_ACK);
 	/* send data bytes */
 	while ((status == 0) && (length-- > 0))
-		status = twsi_send(*(data++), MVTWSI_STATUS_DATA_W_ACK);
+		status = twsi_send(adap, *(data++), MVTWSI_STATUS_DATA_W_ACK);
 	/* Stop transaction */
-	status = twsi_stop(status);
+	status = twsi_stop(adap, status);
 	/* return 0 or status of first failure */
 	return status;
 }
 
+#ifdef CONFIG_I2C_MVTWSI_BASE0
 U_BOOT_I2C_ADAP_COMPLETE(twsi0, twsi_i2c_init, twsi_i2c_probe,
 			 twsi_i2c_read, twsi_i2c_write,
 			 twsi_i2c_set_bus_speed,
 			 CONFIG_SYS_I2C_SPEED, CONFIG_SYS_I2C_SLAVE, 0)
+#endif
+#ifdef CONFIG_I2C_MVTWSI_BASE1
+U_BOOT_I2C_ADAP_COMPLETE(twsi1, twsi_i2c_init, twsi_i2c_probe,
+			 twsi_i2c_read, twsi_i2c_write,
+			 twsi_i2c_set_bus_speed,
+			 CONFIG_SYS_I2C_SPEED, CONFIG_SYS_I2C_SLAVE, 1)
+
+#endif
+#ifdef CONFIG_I2C_MVTWSI_BASE2
+U_BOOT_I2C_ADAP_COMPLETE(twsi2, twsi_i2c_init, twsi_i2c_probe,
+			 twsi_i2c_read, twsi_i2c_write,
+			 twsi_i2c_set_bus_speed,
+			 CONFIG_SYS_I2C_SPEED, CONFIG_SYS_I2C_SLAVE, 2)
+
+#endif
+#ifdef CONFIG_I2C_MVTWSI_BASE3
+U_BOOT_I2C_ADAP_COMPLETE(twsi3, twsi_i2c_init, twsi_i2c_probe,
+			 twsi_i2c_read, twsi_i2c_write,
+			 twsi_i2c_set_bus_speed,
+			 CONFIG_SYS_I2C_SPEED, CONFIG_SYS_I2C_SLAVE, 3)
+
+#endif
+#ifdef CONFIG_I2C_MVTWSI_BASE4
+U_BOOT_I2C_ADAP_COMPLETE(twsi4, twsi_i2c_init, twsi_i2c_probe,
+			 twsi_i2c_read, twsi_i2c_write,
+			 twsi_i2c_set_bus_speed,
+			 CONFIG_SYS_I2C_SPEED, CONFIG_SYS_I2C_SLAVE, 4)
+
+#endif
diff --git a/drivers/i2c/mxc_i2c.c b/drivers/i2c/mxc_i2c.c
index fc5ee35..42782cb 100644
--- a/drivers/i2c/mxc_i2c.c
+++ b/drivers/i2c/mxc_i2c.c
@@ -114,6 +114,9 @@ static u16 i2c_clk_div[50][2] = {
 #ifndef CONFIG_SYS_MXC_I2C3_SPEED
 #define CONFIG_SYS_MXC_I2C3_SPEED 100000
 #endif
+#ifndef CONFIG_SYS_MXC_I2C4_SPEED
+#define CONFIG_SYS_MXC_I2C4_SPEED 100000
+#endif
 
 #ifndef CONFIG_SYS_MXC_I2C1_SLAVE
 #define CONFIG_SYS_MXC_I2C1_SLAVE 0
@@ -124,6 +127,9 @@ static u16 i2c_clk_div[50][2] = {
 #ifndef CONFIG_SYS_MXC_I2C3_SLAVE
 #define CONFIG_SYS_MXC_I2C3_SLAVE 0
 #endif
+#ifndef CONFIG_SYS_MXC_I2C4_SLAVE
+#define CONFIG_SYS_MXC_I2C4_SLAVE 0
+#endif
 
 
 /*
@@ -543,12 +549,17 @@ U_BOOT_I2C_ADAP_COMPLETE(mxc1, mxc_i2c_init, mxc_i2c_probe,
 			 mxc_i2c_set_bus_speed,
 			 CONFIG_SYS_MXC_I2C2_SPEED,
 			 CONFIG_SYS_MXC_I2C2_SLAVE, 1)
-#if defined(CONFIG_MX31) || defined(CONFIG_MX35) ||\
-	defined(CONFIG_MX51) || defined(CONFIG_MX53) ||\
-	defined(CONFIG_MX6) || defined(CONFIG_LS102XA)
+#ifdef CONFIG_SYS_I2C_MXC_I2C3
 U_BOOT_I2C_ADAP_COMPLETE(mxc2, mxc_i2c_init, mxc_i2c_probe,
 			 mxc_i2c_read, mxc_i2c_write,
 			 mxc_i2c_set_bus_speed,
 			 CONFIG_SYS_MXC_I2C3_SPEED,
 			 CONFIG_SYS_MXC_I2C3_SLAVE, 2)
 #endif
+#ifdef CONFIG_SYS_I2C_MXC_I2C4
+U_BOOT_I2C_ADAP_COMPLETE(mxc3, mxc_i2c_init, mxc_i2c_probe,
+			 mxc_i2c_read, mxc_i2c_write,
+			 mxc_i2c_set_bus_speed,
+			 CONFIG_SYS_MXC_I2C4_SPEED,
+			 CONFIG_SYS_MXC_I2C4_SLAVE, 3)
+#endif
diff --git a/drivers/i2c/s3c24x0_i2c.c b/drivers/i2c/s3c24x0_i2c.c
index b4ee33f..27ff587 100644
--- a/drivers/i2c/s3c24x0_i2c.c
+++ b/drivers/i2c/s3c24x0_i2c.c
@@ -1348,7 +1348,7 @@ static int s3c_i2c_ofdata_to_platdata(struct udevice *dev)
 	struct s3c24x0_i2c_bus *i2c_bus = dev_get_priv(dev);
 	int node, flags;
 
-	i2c_bus->is_highspeed = dev->of_id->data;
+	i2c_bus->is_highspeed = dev_get_driver_data(dev);
 	node = dev->of_offset;
 
 	if (i2c_bus->is_highspeed) {
diff --git a/drivers/i2c/sandbox_i2c.c b/drivers/i2c/sandbox_i2c.c
index a943aa6..d6adc0f 100644
--- a/drivers/i2c/sandbox_i2c.c
+++ b/drivers/i2c/sandbox_i2c.c
@@ -50,7 +50,7 @@ static int get_emul(struct udevice *dev, struct udevice **devp,
 static int sandbox_i2c_xfer(struct udevice *bus, struct i2c_msg *msg,
 			    int nmsgs)
 {
-	struct dm_i2c_bus *i2c = bus->uclass_priv;
+	struct dm_i2c_bus *i2c = dev_get_uclass_priv(bus);
 	struct dm_i2c_ops *ops;
 	struct udevice *emul, *dev;
 	bool is_read;
diff --git a/drivers/i2c/tegra_i2c.c b/drivers/i2c/tegra_i2c.c
index f414287..fc95646 100644
--- a/drivers/i2c/tegra_i2c.c
+++ b/drivers/i2c/tegra_i2c.c
@@ -338,7 +338,7 @@ static int tegra_i2c_probe(struct udevice *dev)
 	bool is_dvc;
 
 	i2c_bus->id = dev->seq;
-	i2c_bus->type = dev_get_of_data(dev);
+	i2c_bus->type = dev_get_driver_data(dev);
 	i2c_bus->regs = (struct i2c_ctlr *)fdtdec_get_addr(blob, node, "reg");
 
 	/*
@@ -360,7 +360,7 @@ static int tegra_i2c_probe(struct udevice *dev)
 	if (i2c_bus->periph_id == -1)
 		return -EINVAL;
 
-	is_dvc = dev_get_of_data(dev) == TYPE_DVC;
+	is_dvc = dev_get_driver_data(dev) == TYPE_DVC;
 	if (is_dvc) {
 		i2c_bus->control =
 			&((struct dvc_ctlr *)i2c_bus->regs)->control;
@@ -469,7 +469,7 @@ int tegra_i2c_get_dvc_bus(struct udevice **busp)
 	for (uclass_first_device(UCLASS_I2C, &bus);
 	     bus;
 	     uclass_next_device(&bus)) {
-		if (dev_get_of_data(bus) == TYPE_DVC) {
+		if (dev_get_driver_data(bus) == TYPE_DVC) {
 			*busp = bus;
 			return 0;
 		}
diff --git a/drivers/input/cros_ec_keyb.c b/drivers/input/cros_ec_keyb.c
index 49ee7b2..a31aa77 100644
--- a/drivers/input/cros_ec_keyb.c
+++ b/drivers/input/cros_ec_keyb.c
@@ -198,7 +198,7 @@ static int cros_ec_keyb_decode_fdt(const void *blob, int node,
 		return -1;
 	}
 	config->ghost_filter = fdtdec_get_bool(blob, node,
-					       "google,ghost-filter");
+					       "google,needs-ghost-filter");
 	return 0;
 }
 
diff --git a/drivers/input/i8042.c b/drivers/input/i8042.c
index ca1604c..1769c5e 100644
--- a/drivers/input/i8042.c
+++ b/drivers/input/i8042.c
@@ -698,7 +698,14 @@ static int kbd_reset(void)
 
 	/* Enable Keyboard */
 	out8(I8042_COMMAND_REG, 0xae);
+	if (kbd_input_empty() == 0)
+		return -1;
+
+	out8(I8042_COMMAND_REG, 0x60);
+	if (kbd_input_empty() == 0)
+		return -1;
 
+	out8(I8042_DATA_REG, 0xf4);
 	if (kbd_input_empty() == 0)
 		return -1;
 
diff --git a/drivers/misc/Kconfig b/drivers/misc/Kconfig
index 36a8f0d..0e571d9 100644
--- a/drivers/misc/Kconfig
+++ b/drivers/misc/Kconfig
@@ -35,6 +35,15 @@ config CROS_EC_LPC
 	  through a legacy port interface, so on x86 machines the main
 	  function of the EC is power and thermal management.
 
+config CROS_EC_SANDBOX
+	bool "Enable Chrome OS EC sandbox driver"
+	depends on CROS_EC && SANDBOX
+	help
+	  Enable a sandbox emulation of the Chrome OS EC. This supports
+	  keyboard (use the -l flag to enable the LCD), verified boot context,
+	  EC flash read/write/erase support and a few other things. It is
+	  enough to perform a Chrome OS verified boot on sandbox.
+
 config CROS_EC_SPI
 	bool "Enable Chrome OS EC SPI driver"
 	depends on CROS_EC
@@ -44,16 +53,6 @@ config CROS_EC_SPI
 	  provides a faster and more robust interface than I2C but the bugs
 	  are less interesting.
 
-config DM_CROS_EC
-	bool "Enable Driver Model for Chrome OS EC"
-	depends on DM
-	help
-	  Enable driver model for the Chrome OS EC interface. This
-	  allows the cros_ec SPI driver to operate with CONFIG_DM_SPI
-	  but otherwise makes few changes. Since cros_ec also supports
-	  LPC (which doesn't support driver model yet), a full
-	  conversion is not yet possible.
-
 config CONFIG_FSL_SEC_MON
 	bool "Enable FSL SEC_MON Driver"
 	help
diff --git a/drivers/misc/Makefile b/drivers/misc/Makefile
index 6028cd4..25630c3 100644
--- a/drivers/misc/Makefile
+++ b/drivers/misc/Makefile
@@ -13,6 +13,7 @@ obj-$(CONFIG_CROS_EC_LPC) += cros_ec_lpc.o
 obj-$(CONFIG_CROS_EC_I2C) += cros_ec_i2c.o
 obj-$(CONFIG_CROS_EC_SANDBOX) += cros_ec_sandbox.o
 obj-$(CONFIG_CROS_EC_SPI) += cros_ec_spi.o
+obj-$(CONFIG_FSL_DEBUG_SERVER) += fsl_debug_server.o
 obj-$(CONFIG_FSL_IIM) += fsl_iim.o
 obj-$(CONFIG_GPIO_LED) += gpio_led.o
 obj-$(CONFIG_I2C_EEPROM) += i2c_eeprom.o
@@ -26,6 +27,7 @@ obj-$(CONFIG_SANDBOX) += i2c_eeprom_emul.o
 endif
 obj-$(CONFIG_SMSC_LPC47M) += smsc_lpc47m.o
 obj-$(CONFIG_STATUS_LED) += status_led.o
+obj-$(CONFIG_SANDBOX) += swap_case.o
 obj-$(CONFIG_TWL4030_LED) += twl4030_led.o
 obj-$(CONFIG_FSL_IFC) += fsl_ifc.o
 obj-$(CONFIG_FSL_SEC_MON) += fsl_sec_mon.o
diff --git a/drivers/misc/cros_ec.c b/drivers/misc/cros_ec.c
index 5846e76..982bac7 100644
--- a/drivers/misc/cros_ec.c
+++ b/drivers/misc/cros_ec.c
@@ -41,10 +41,6 @@ enum {
 	CROS_EC_CMD_HASH_TIMEOUT_MS = 2000,
 };
 
-#ifndef CONFIG_DM_CROS_EC
-static struct cros_ec_dev static_dev, *last_dev;
-#endif
-
 DECLARE_GLOBAL_DATA_PTR;
 
 /* Note: depends on enum ec_current_image */
@@ -211,9 +207,7 @@ static int send_command_proto3(struct cros_ec_dev *dev,
 			       const void *dout, int dout_len,
 			       uint8_t **dinp, int din_len)
 {
-#ifdef CONFIG_DM_CROS_EC
 	struct dm_cros_ec_ops *ops;
-#endif
 	int out_bytes, in_bytes;
 	int rv;
 
@@ -228,28 +222,8 @@ static int send_command_proto3(struct cros_ec_dev *dev,
 	if (in_bytes < 0)
 		return in_bytes;
 
-#ifdef CONFIG_DM_CROS_EC
 	ops = dm_cros_ec_get_ops(dev->dev);
 	rv = ops->packet ? ops->packet(dev->dev, out_bytes, in_bytes) : -ENOSYS;
-#else
-	switch (dev->interface) {
-#ifdef CONFIG_CROS_EC_SPI
-	case CROS_EC_IF_SPI:
-		rv = cros_ec_spi_packet(dev, out_bytes, in_bytes);
-		break;
-#endif
-#ifdef CONFIG_CROS_EC_SANDBOX
-	case CROS_EC_IF_SANDBOX:
-		rv = cros_ec_sandbox_packet(dev, out_bytes, in_bytes);
-		break;
-#endif
-	case CROS_EC_IF_NONE:
-	/* TODO: support protocol 3 for LPC, I2C; for now fall through */
-	default:
-		debug("%s: Unsupported interface\n", __func__);
-		rv = -1;
-	}
-#endif
 	if (rv < 0)
 		return rv;
 
@@ -261,9 +235,7 @@ static int send_command(struct cros_ec_dev *dev, uint8_t cmd, int cmd_version,
 			const void *dout, int dout_len,
 			uint8_t **dinp, int din_len)
 {
-#ifdef CONFIG_DM_CROS_EC
 	struct dm_cros_ec_ops *ops;
-#endif
 	int ret = -1;
 
 	/* Handle protocol version 3 support */
@@ -272,38 +244,9 @@ static int send_command(struct cros_ec_dev *dev, uint8_t cmd, int cmd_version,
 					   dout, dout_len, dinp, din_len);
 	}
 
-#ifdef CONFIG_DM_CROS_EC
 	ops = dm_cros_ec_get_ops(dev->dev);
 	ret = ops->command(dev->dev, cmd, cmd_version,
 			   (const uint8_t *)dout, dout_len, dinp, din_len);
-#else
-	switch (dev->interface) {
-#ifdef CONFIG_CROS_EC_SPI
-	case CROS_EC_IF_SPI:
-		ret = cros_ec_spi_command(dev, cmd, cmd_version,
-					(const uint8_t *)dout, dout_len,
-					dinp, din_len);
-		break;
-#endif
-#ifdef CONFIG_CROS_EC_I2C
-	case CROS_EC_IF_I2C:
-		ret = cros_ec_i2c_command(dev, cmd, cmd_version,
-					(const uint8_t *)dout, dout_len,
-					dinp, din_len);
-		break;
-#endif
-#ifdef CONFIG_CROS_EC_LPC
-	case CROS_EC_IF_LPC:
-		ret = cros_ec_lpc_command(dev, cmd, cmd_version,
-					(const uint8_t *)dout, dout_len,
-					dinp, din_len);
-		break;
-#endif
-	case CROS_EC_IF_NONE:
-	default:
-		ret = -1;
-	}
-#endif
 
 	return ret;
 }
@@ -681,11 +624,15 @@ static int cros_ec_check_version(struct cros_ec_dev *dev)
 	struct ec_params_hello req;
 	struct ec_response_hello *resp;
 
-#ifdef CONFIG_CROS_EC_LPC
-	/* LPC has its own way of doing this */
-	if (dev->interface == CROS_EC_IF_LPC)
-		return cros_ec_lpc_check_version(dev);
-#endif
+	struct dm_cros_ec_ops *ops;
+	int ret;
+
+	ops = dm_cros_ec_get_ops(dev->dev);
+	if (ops->check_version) {
+		ret = ops->check_version(dev->dev);
+		if (ret)
+			return ret;
+	}
 
 	/*
 	 * TODO(sjg@chromium.org).
@@ -1015,79 +962,9 @@ int cros_ec_get_ldo(struct cros_ec_dev *dev, uint8_t index, uint8_t *state)
 	return 0;
 }
 
-#ifndef CONFIG_DM_CROS_EC
-/**
- * Decode EC interface details from the device tree and allocate a suitable
- * device.
- *
- * @param blob		Device tree blob
- * @param node		Node to decode from
- * @param devp		Returns a pointer to the new allocated device
- * @return 0 if ok, -1 on error
- */
-static int cros_ec_decode_fdt(const void *blob, int node,
-		struct cros_ec_dev **devp)
-{
-	enum fdt_compat_id compat;
-	struct cros_ec_dev *dev;
-	int parent;
-
-	/* See what type of parent we are inside (this is expensive) */
-	parent = fdt_parent_offset(blob, node);
-	if (parent < 0) {
-		debug("%s: Cannot find node parent\n", __func__);
-		return -1;
-	}
-
-	dev = &static_dev;
-	dev->node = node;
-	dev->parent_node = parent;
-
-	compat = fdtdec_lookup(blob, parent);
-	switch (compat) {
-#ifdef CONFIG_CROS_EC_SPI
-	case COMPAT_SAMSUNG_EXYNOS_SPI:
-		dev->interface = CROS_EC_IF_SPI;
-		if (cros_ec_spi_decode_fdt(dev, blob))
-			return -1;
-		break;
-#endif
-#ifdef CONFIG_CROS_EC_I2C
-	case COMPAT_SAMSUNG_S3C2440_I2C:
-		dev->interface = CROS_EC_IF_I2C;
-		if (cros_ec_i2c_decode_fdt(dev, blob))
-			return -1;
-		break;
-#endif
-#ifdef CONFIG_CROS_EC_LPC
-	case COMPAT_INTEL_LPC:
-		dev->interface = CROS_EC_IF_LPC;
-		break;
-#endif
-#ifdef CONFIG_CROS_EC_SANDBOX
-	case COMPAT_SANDBOX_HOST_EMULATION:
-		dev->interface = CROS_EC_IF_SANDBOX;
-		break;
-#endif
-	default:
-		debug("%s: Unknown compat id %d\n", __func__, compat);
-		return -1;
-	}
-
-	gpio_request_by_name_nodev(blob, node, "ec-interrupt", 0, &dev->ec_int,
-				   GPIOD_IS_IN);
-	dev->optimise_flash_write = fdtdec_get_bool(blob, node,
-						    "optimise-flash-write");
-	*devp = dev;
-
-	return 0;
-}
-#endif
-
-#ifdef CONFIG_DM_CROS_EC
 int cros_ec_register(struct udevice *dev)
 {
-	struct cros_ec_dev *cdev = dev->uclass_priv;
+	struct cros_ec_dev *cdev = dev_get_uclass_priv(dev);
 	const void *blob = gd->fdt_blob;
 	int node = dev->of_offset;
 	char id[MSG_BYTES];
@@ -1113,94 +990,6 @@ int cros_ec_register(struct udevice *dev)
 
 	return 0;
 }
-#else
-int cros_ec_init(const void *blob, struct cros_ec_dev **cros_ecp)
-{
-	struct cros_ec_dev *dev;
-	char id[MSG_BYTES];
-#ifdef CONFIG_DM_CROS_EC
-	struct udevice *udev;
-	int ret;
-
-	ret = uclass_find_device(UCLASS_CROS_EC, 0, &udev);
-	if (!ret)
-		device_remove(udev);
-	ret = uclass_get_device(UCLASS_CROS_EC, 0, &udev);
-	if (ret)
-		return ret;
-	dev = udev->uclass_priv;
-	return 0;
-#else
-	int node = 0;
-
-	*cros_ecp = NULL;
-	do {
-		node = fdtdec_next_compatible(blob, node,
-					      COMPAT_GOOGLE_CROS_EC);
-		if (node < 0) {
-			debug("%s: Node not found\n", __func__);
-			return 0;
-		}
-	} while (!fdtdec_get_is_enabled(blob, node));
-
-	if (cros_ec_decode_fdt(blob, node, &dev)) {
-		debug("%s: Failed to decode device.\n", __func__);
-		return -CROS_EC_ERR_FDT_DECODE;
-	}
-
-	switch (dev->interface) {
-#ifdef CONFIG_CROS_EC_SPI
-	case CROS_EC_IF_SPI:
-		if (cros_ec_spi_init(dev, blob)) {
-			debug("%s: Could not setup SPI interface\n", __func__);
-			return -CROS_EC_ERR_DEV_INIT;
-		}
-		break;
-#endif
-#ifdef CONFIG_CROS_EC_I2C
-	case CROS_EC_IF_I2C:
-		if (cros_ec_i2c_init(dev, blob))
-			return -CROS_EC_ERR_DEV_INIT;
-		break;
-#endif
-#ifdef CONFIG_CROS_EC_LPC
-	case CROS_EC_IF_LPC:
-		if (cros_ec_lpc_init(dev, blob))
-			return -CROS_EC_ERR_DEV_INIT;
-		break;
-#endif
-#ifdef CONFIG_CROS_EC_SANDBOX
-	case CROS_EC_IF_SANDBOX:
-		if (cros_ec_sandbox_init(dev, blob))
-			return -CROS_EC_ERR_DEV_INIT;
-		break;
-#endif
-	case CROS_EC_IF_NONE:
-	default:
-		return 0;
-	}
-#endif
-
-	if (cros_ec_check_version(dev)) {
-		debug("%s: Could not detect CROS-EC version\n", __func__);
-		return -CROS_EC_ERR_CHECK_VERSION;
-	}
-
-	if (cros_ec_read_id(dev, id, sizeof(id))) {
-		debug("%s: Could not read KBC ID\n", __func__);
-		return -CROS_EC_ERR_READ_ID;
-	}
-
-	/* Remember this device for use by the cros_ec command */
-	*cros_ecp = dev;
-#ifndef CONFIG_DM_CROS_EC
-	last_dev = dev;
-#endif
-	debug("Google Chrome EC CROS-EC driver ready, id '%s'\n", id);
-
-	return 0;
-}
-#endif
 
 int cros_ec_decode_region(int argc, char * const argv[])
 {
@@ -1583,9 +1372,7 @@ static int cros_ec_i2c_passthrough(struct cros_ec_dev *dev, int flag,
 static int do_cros_ec(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
 {
 	struct cros_ec_dev *dev;
-#ifdef CONFIG_DM_CROS_EC
 	struct udevice *udev;
-#endif
 	const char *cmd;
 	int ret = 0;
 
@@ -1594,31 +1381,24 @@ static int do_cros_ec(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
 
 	cmd = argv[1];
 	if (0 == strcmp("init", cmd)) {
-#ifndef CONFIG_DM_CROS_EC
-		ret = cros_ec_init(gd->fdt_blob, &dev);
+		/* Remove any existing device */
+		ret = uclass_find_device(UCLASS_CROS_EC, 0, &udev);
+		if (!ret)
+			device_remove(udev);
+		ret = uclass_get_device(UCLASS_CROS_EC, 0, &udev);
 		if (ret) {
 			printf("Could not init cros_ec device (err %d)\n", ret);
 			return 1;
 		}
-#endif
 		return 0;
 	}
 
-#ifdef CONFIG_DM_CROS_EC
 	ret = uclass_get_device(UCLASS_CROS_EC, 0, &udev);
 	if (ret) {
 		printf("Cannot get cros-ec device (err=%d)\n", ret);
 		return 1;
 	}
-	dev = udev->uclass_priv;
-#else
-	/* Just use the last allocated device; there should be only one */
-	if (!last_dev) {
-		printf("No CROS-EC device available\n");
-		return 1;
-	}
-	dev = last_dev;
-#endif
+	dev = dev_get_uclass_priv(udev);
 	if (0 == strcmp("id", cmd)) {
 		char id[MSG_BYTES];
 
@@ -1876,10 +1656,8 @@ U_BOOT_CMD(
 );
 #endif
 
-#ifdef CONFIG_DM_CROS_EC
 UCLASS_DRIVER(cros_ec) = {
 	.id		= UCLASS_CROS_EC,
 	.name		= "cros_ec",
 	.per_device_auto_alloc_size = sizeof(struct cros_ec_dev),
 };
-#endif
diff --git a/drivers/misc/cros_ec_i2c.c b/drivers/misc/cros_ec_i2c.c
index f9bc975..3de18b2 100644
--- a/drivers/misc/cros_ec_i2c.c
+++ b/drivers/misc/cros_ec_i2c.c
@@ -28,7 +28,7 @@ static int cros_ec_i2c_command(struct udevice *udev, uint8_t cmd,
 			       int cmd_version, const uint8_t *dout,
 			       int dout_len, uint8_t **dinp, int din_len)
 {
-	struct cros_ec_dev *dev = udev->uclass_priv;
+	struct cros_ec_dev *dev = dev_get_uclass_priv(udev);
 	/* version8, cmd8, arglen8, out8[dout_len], csum8 */
 	int out_bytes = dout_len + 4;
 	/* response8, arglen8, in8[din_len], checksum8 */
@@ -139,12 +139,12 @@ static struct dm_cros_ec_ops cros_ec_ops = {
 };
 
 static const struct udevice_id cros_ec_ids[] = {
-	{ .compatible = "google,cros-ec" },
+	{ .compatible = "google,cros-ec-i2c" },
 	{ }
 };
 
 U_BOOT_DRIVER(cros_ec_i2c) = {
-	.name		= "cros_ec",
+	.name		= "cros_ec_i2c",
 	.id		= UCLASS_CROS_EC,
 	.of_match	= cros_ec_ids,
 	.probe		= cros_ec_probe,
diff --git a/drivers/misc/cros_ec_lpc.c b/drivers/misc/cros_ec_lpc.c
index 07624a1..7837841 100644
--- a/drivers/misc/cros_ec_lpc.c
+++ b/drivers/misc/cros_ec_lpc.c
@@ -14,6 +14,7 @@
  */
 
 #include <common.h>
+#include <dm.h>
 #include <command.h>
 #include <cros_ec.h>
 #include <asm/io.h>
@@ -40,10 +41,11 @@ static int wait_for_sync(struct cros_ec_dev *dev)
 	return 0;
 }
 
-int cros_ec_lpc_command(struct cros_ec_dev *dev, uint8_t cmd, int cmd_version,
+int cros_ec_lpc_command(struct udevice *udev, uint8_t cmd, int cmd_version,
 		     const uint8_t *dout, int dout_len,
 		     uint8_t **dinp, int din_len)
 {
+	struct cros_ec_dev *dev = dev_get_uclass_priv(udev);
 	const int cmd_addr = EC_LPC_ADDR_HOST_CMD;
 	const int data_addr = EC_LPC_ADDR_HOST_DATA;
 	const int args_addr = EC_LPC_ADDR_HOST_ARGS;
@@ -178,7 +180,7 @@ int cros_ec_lpc_init(struct cros_ec_dev *dev, const void *blob)
  * seeing whether the EC sets the EC_HOST_ARGS_FLAG_FROM_HOST flag
  * in args when it responds.
  */
-int cros_ec_lpc_check_version(struct cros_ec_dev *dev)
+static int cros_ec_lpc_check_version(struct udevice *dev)
 {
 	if (inb(EC_LPC_ADDR_MEMMAP + EC_MEMMAP_ID) == 'E' &&
 			inb(EC_LPC_ADDR_MEMMAP + EC_MEMMAP_ID + 1)
@@ -192,3 +194,26 @@ int cros_ec_lpc_check_version(struct cros_ec_dev *dev)
 	printf("%s: ERROR: old EC interface not supported\n", __func__);
 	return -1;
 }
+
+static int cros_ec_probe(struct udevice *dev)
+{
+	return cros_ec_register(dev);
+}
+
+static struct dm_cros_ec_ops cros_ec_ops = {
+	.command = cros_ec_lpc_command,
+	.check_version = cros_ec_lpc_check_version,
+};
+
+static const struct udevice_id cros_ec_ids[] = {
+	{ .compatible = "google,cros-ec-lpc" },
+	{ }
+};
+
+U_BOOT_DRIVER(cros_ec_lpc) = {
+	.name		= "cros_ec_lpc",
+	.id		= UCLASS_CROS_EC,
+	.of_match	= cros_ec_ids,
+	.probe		= cros_ec_probe,
+	.ops		= &cros_ec_ops,
+};
diff --git a/drivers/misc/cros_ec_sandbox.c b/drivers/misc/cros_ec_sandbox.c
index 99cc529..df41e82 100644
--- a/drivers/misc/cros_ec_sandbox.c
+++ b/drivers/misc/cros_ec_sandbox.c
@@ -467,17 +467,10 @@ static int process_cmd(struct ec_state *ec,
 	return len;
 }
 
-#ifdef CONFIG_DM_CROS_EC
 int cros_ec_sandbox_packet(struct udevice *udev, int out_bytes, int in_bytes)
 {
-	struct cros_ec_dev *dev = udev->uclass_priv;
+	struct cros_ec_dev *dev = dev_get_uclass_priv(udev);
 	struct ec_state *ec = dev_get_priv(dev->dev);
-#else
-int cros_ec_sandbox_packet(struct cros_ec_dev *dev, int out_bytes,
-			   int in_bytes)
-{
-	struct ec_state *ec = &s_state;
-#endif
 	struct ec_host_request *req_hdr = (struct ec_host_request *)dev->dout;
 	const void *req_data = req_hdr + 1;
 	struct ec_host_response *resp_hdr = (struct ec_host_response *)dev->din;
@@ -500,18 +493,9 @@ int cros_ec_sandbox_packet(struct cros_ec_dev *dev, int out_bytes,
 	return in_bytes;
 }
 
-int cros_ec_sandbox_decode_fdt(struct cros_ec_dev *dev, const void *blob)
-{
-	return 0;
-}
-
 void cros_ec_check_keyboard(struct cros_ec_dev *dev)
 {
-#ifdef CONFIG_DM_CROS_EC
 	struct ec_state *ec = dev_get_priv(dev->dev);
-#else
-	struct ec_state *ec = &s_state;
-#endif
 	ulong start;
 
 	printf("Press keys for EC to detect on reset (ESC=recovery)...");
@@ -525,7 +509,6 @@ void cros_ec_check_keyboard(struct cros_ec_dev *dev)
 	}
 }
 
-#ifdef CONFIG_DM_CROS_EC
 int cros_ec_probe(struct udevice *dev)
 {
 	struct ec_state *ec = dev->priv;
@@ -569,76 +552,20 @@ int cros_ec_probe(struct udevice *dev)
 	return cros_ec_register(dev);
 }
 
-#else
-
-/**
- * Initialize sandbox EC emulation.
- *
- * @param dev		CROS_EC device
- * @param blob		Device tree blob
- * @return 0 if ok, -1 on error
- */
-int cros_ec_sandbox_init(struct cros_ec_dev *dev, const void *blob)
-{
-	struct ec_state *ec = &s_state;
-	int node;
-	int err;
-
-	node = fdtdec_next_compatible(blob, 0, COMPAT_GOOGLE_CROS_EC);
-	if (node < 0) {
-		debug("Failed to find chrome-ec node'\n");
-		return -1;
-	}
-
-	err = cros_ec_decode_ec_flash(blob, node, &ec->ec_config);
-	if (err)
-		return err;
-
-	node = fdtdec_next_compatible(blob, 0, COMPAT_GOOGLE_CROS_EC_KEYB);
-	if (node < 0) {
-		debug("%s: No cros_ec keyboard found\n", __func__);
-	} else if (keyscan_read_fdt_matrix(ec, blob, node)) {
-		debug("%s: Could not read key matrix\n", __func__);
-		return -1;
-	}
-
-	/* If we loaded EC data, check that the length matches */
-	if (ec->flash_data &&
-	    ec->flash_data_len != ec->ec_config.flash.length) {
-		printf("EC data length is %x, expected %x, discarding data\n",
-		       ec->flash_data_len, ec->ec_config.flash.length);
-		os_free(ec->flash_data);
-		ec->flash_data = NULL;
-	}
-
-	/* Otherwise allocate the memory */
-	if (!ec->flash_data) {
-		ec->flash_data_len = ec->ec_config.flash.length;
-		ec->flash_data = os_malloc(ec->flash_data_len);
-		if (!ec->flash_data)
-			return -ENOMEM;
-	}
-
-	return 0;
-}
-#endif
-
-#ifdef CONFIG_DM_CROS_EC
 struct dm_cros_ec_ops cros_ec_ops = {
 	.packet = cros_ec_sandbox_packet,
 };
 
 static const struct udevice_id cros_ec_ids[] = {
-	{ .compatible = "google,cros-ec" },
+	{ .compatible = "google,cros-ec-sandbox" },
 	{ }
 };
 
 U_BOOT_DRIVER(cros_ec_sandbox) = {
-	.name		= "cros_ec",
+	.name		= "cros_ec_sandbox",
 	.id		= UCLASS_CROS_EC,
 	.of_match	= cros_ec_ids,
 	.probe		= cros_ec_probe,
 	.priv_auto_alloc_size = sizeof(struct ec_state),
 	.ops		= &cros_ec_ops,
 };
-#endif
diff --git a/drivers/misc/cros_ec_spi.c b/drivers/misc/cros_ec_spi.c
index 9359c56..ac2ee86 100644
--- a/drivers/misc/cros_ec_spi.c
+++ b/drivers/misc/cros_ec_spi.c
@@ -23,7 +23,7 @@ DECLARE_GLOBAL_DATA_PTR;
 
 int cros_ec_spi_packet(struct udevice *udev, int out_bytes, int in_bytes)
 {
-	struct cros_ec_dev *dev = udev->uclass_priv;
+	struct cros_ec_dev *dev = dev_get_uclass_priv(udev);
 	struct spi_slave *slave = dev_get_parentdata(dev->dev);
 	int rv;
 
@@ -66,7 +66,7 @@ int cros_ec_spi_command(struct udevice *udev, uint8_t cmd, int cmd_version,
 		     const uint8_t *dout, int dout_len,
 		     uint8_t **dinp, int din_len)
 {
-	struct cros_ec_dev *dev = udev->uclass_priv;
+	struct cros_ec_dev *dev = dev_get_uclass_priv(udev);
 	struct spi_slave *slave = dev_get_parentdata(dev->dev);
 	int in_bytes = din_len + 4;	/* status, length, checksum, trailer */
 	uint8_t *out;
@@ -165,12 +165,12 @@ static struct dm_cros_ec_ops cros_ec_ops = {
 };
 
 static const struct udevice_id cros_ec_ids[] = {
-	{ .compatible = "google,cros-ec" },
+	{ .compatible = "google,cros-ec-spi" },
 	{ }
 };
 
 U_BOOT_DRIVER(cros_ec_spi) = {
-	.name		= "cros_ec",
+	.name		= "cros_ec_spi",
 	.id		= UCLASS_CROS_EC,
 	.of_match	= cros_ec_ids,
 	.probe		= cros_ec_probe,
diff --git a/drivers/misc/fsl_debug_server.c b/drivers/misc/fsl_debug_server.c
new file mode 100644
index 0000000..e080fe6
--- /dev/null
+++ b/drivers/misc/fsl_debug_server.c
@@ -0,0 +1,246 @@
+/*
+ * Copyright (C) 2014 Freescale Semiconductor
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <errno.h>
+#include <asm/io.h>
+#include <asm/system.h>
+#include <asm/arch-fsl-lsch3/immap_lsch3.h>
+
+#include <fsl_debug_server.h>
+
+DECLARE_GLOBAL_DATA_PTR;
+static int debug_server_ver_info_maj, debug_server_ver_info_min;
+
+/**
+ * Copying Debug Server firmware to DDR
+ */
+static int debug_server_copy_image(const char *title, u64 image_addr,
+				   u32 image_size, u64 debug_server_ram_addr)
+{
+	debug("%s copied to address %p\n", title,
+	      (void *)debug_server_ram_addr);
+	memcpy((void *)debug_server_ram_addr, (void *)image_addr, image_size);
+
+	return 0;
+}
+
+/**
+ * Debug Server FIT image parser checks if the image is in FIT
+ * format, verifies integrity of the image and calculates
+ * raw image address and size values.
+ *
+ * Returns 0 if success and -1 if any of the above mentioned
+ * task fail.
+ **/
+int debug_server_parse_firmware_fit_image(const void **raw_image_addr,
+					  size_t *raw_image_size)
+{
+	int format;
+	void *fit_hdr;
+	int node_offset;
+	const void *data;
+	size_t size;
+	const char *uname = "firmware";
+	char *desc;
+	char *debug_server_ver_info;
+	char *debug_server_ver_info_major, *debug_server_ver_info_minor;
+
+	/* Check if the image is in NOR flash */
+#ifdef CONFIG_SYS_DEBUG_SERVER_FW_IN_NOR
+	fit_hdr = (void *)CONFIG_SYS_DEBUG_SERVER_FW_ADDR;
+#else
+#error "CONFIG_SYS_DEBUG_SERVER_FW_IN_NOR not defined"
+#endif
+
+	/* Check if Image is in FIT format */
+	format = genimg_get_format(fit_hdr);
+	if (format != IMAGE_FORMAT_FIT) {
+		printf("Error! Not a FIT image\n");
+		goto out_error;
+	}
+
+	if (!fit_check_format(fit_hdr)) {
+		printf("Error! Bad FIT image format\n");
+		goto out_error;
+	}
+
+	node_offset = fit_image_get_node(fit_hdr, uname);
+	if (node_offset < 0) {
+		printf("Error! Can not find %s subimage\n", uname);
+		goto out_error;
+	}
+
+	/* Verify Debug Server firmware image */
+	if (!fit_image_verify(fit_hdr, node_offset)) {
+		printf("Error! Bad Debug Server firmware hash");
+		goto out_error;
+	}
+
+	if (fit_get_desc(fit_hdr, node_offset, &desc) < 0) {
+		printf("Error! Failed to get Debug Server fw description");
+		goto out_error;
+	}
+
+	debug_server_ver_info = strstr(desc, "Version");
+	debug_server_ver_info_major = strtok(debug_server_ver_info, ".");
+	debug_server_ver_info_minor = strtok(NULL, ".");
+
+	debug_server_ver_info_maj =
+			simple_strtoul(debug_server_ver_info_major, NULL, 10);
+	debug_server_ver_info_min =
+			simple_strtoul(debug_server_ver_info_minor, NULL, 10);
+
+	/* Debug server version checking */
+	if ((debug_server_ver_info_maj < DEBUG_SERVER_VER_MAJOR) ||
+	    (debug_server_ver_info_min < DEBUG_SERVER_VER_MINOR)) {
+		printf("Debug server FW mismatches the min version required\n");
+		printf("Expected:%d.%d, Got %d.%d\n",
+		       DEBUG_SERVER_VER_MAJOR, DEBUG_SERVER_VER_MINOR,
+		       debug_server_ver_info_maj,
+		       debug_server_ver_info_min);
+		goto out_error;
+	}
+
+	/* Get address and size of raw image */
+	fit_image_get_data(fit_hdr, node_offset, &data, &size);
+
+	*raw_image_addr = data;
+	*raw_image_size = size;
+
+	return 0;
+
+out_error:
+	return -1;
+}
+
+/**
+ * Return the actual size of the Debug Server private DRAM block.
+ *
+ * NOTE: For now this function always returns the minimum required size,
+ * However, in the future, the actual size may be obtained from an environment
+ * variable.
+ */
+unsigned long debug_server_get_dram_block_size(void)
+{
+	return CONFIG_SYS_DEBUG_SERVER_DRAM_BLOCK_MIN_SIZE;
+}
+
+int debug_server_init(void)
+{
+	struct ccsr_gur __iomem *gur = (void *)(CONFIG_SYS_FSL_GUTS_ADDR);
+	int error, timeout = CONFIG_SYS_DEBUG_SERVER_TIMEOUT;
+	int debug_server_boot_status;
+	u64 debug_server_ram_addr, debug_server_ram_size;
+	const void *raw_image_addr;
+	size_t raw_image_size = 0;
+
+	debug("debug_server_init called\n");
+	/*
+	 * The Debug Server private DRAM block was already carved at the end of
+	 * DRAM by board_init_f() using CONFIG_SYS_MEM_TOP_HIDE:
+	 */
+	debug_server_ram_size = debug_server_get_dram_block_size();
+	if (gd->bd->bi_dram[1].start)
+		debug_server_ram_addr =
+			gd->bd->bi_dram[1].start + gd->bd->bi_dram[1].size;
+	else
+		debug_server_ram_addr =
+			gd->bd->bi_dram[0].start + gd->bd->bi_dram[0].size;
+
+	error = debug_server_parse_firmware_fit_image(&raw_image_addr,
+							&raw_image_size);
+	if (error != 0)
+		goto out;
+
+	debug("debug server (ram addr = 0x%llx, ram size = 0x%llx)\n",
+	      debug_server_ram_addr, debug_server_ram_size);
+	/*
+	 * Load the Debug Server FW at the beginning of the Debug Server
+	 * private DRAM block:
+	 */
+	debug_server_copy_image("Debug Server Firmware",
+				(u64)raw_image_addr, raw_image_size,
+				debug_server_ram_addr);
+
+	/* flush dcache */
+	flush_dcache_range((unsigned long)debug_server_ram_addr,
+			   (unsigned long)debug_server_ram_addr +
+			   (unsigned long)debug_server_ram_size);
+
+	/*
+	 * Tell SP that the Debug Server FW is about to be launched. Before that
+	 * populate the following:
+	 * 1. Write the size allocated to SP Memory region into Bits {31:16} of
+	 *    SCRATCHRW5.
+	 * 2. Write the start address of the SP memory regions into
+	 *    SCRATCHRW5 (Bits {15:0}, contain most significant bits, Bits
+	 *    {47:32} of the SP Memory Region physical start address
+	 *    (SoC address)) and SCRATCHRW6 (Bits {31:0}).
+	 * 3. To know the Debug Server FW boot status, set bit 0 of SCRATCHRW11
+	 *    to 1. The Debug Server sets this to 0 to indicate a
+	 *    successul boot.
+	 * 4. Wakeup SP by writing 0x1F to VSG GIC reg VIGR2.
+	 */
+
+	/* 512 MB */
+	out_le32(&gur->scratchrw[5 - 1],
+		 (u32)((u64)debug_server_ram_addr >> 32) | (0x000D << 16));
+	out_le32(&gur->scratchrw[6 - 1],
+		 ((u32)debug_server_ram_addr) & 0xFFFFFFFF);
+
+	out_le32(&gur->scratchrw[11 - 1], DEBUG_SERVER_INIT_STATUS);
+	/* Allow the changes to reflect in GUR block */
+	mb();
+
+	/*
+	 * Program VGIC to raise an interrupt to SP
+	 */
+	out_le32(CONFIG_SYS_FSL_SP_VSG_GIC_VIGR2, 0x1F);
+	/* Allow the changes to reflect in VIGR2 */
+	mb();
+
+	dmb();
+	debug("Polling for Debug server to launch ...\n");
+
+	while (1) {
+		debug_server_boot_status = in_le32(&gur->scratchrw[11 - 1]);
+		if (!(debug_server_boot_status & DEBUG_SERVER_INIT_STATUS_MASK))
+			break;
+
+		udelay(1);	/* throttle polling */
+		if (timeout-- <= 0)
+			break;
+	}
+
+	if (timeout <= 0) {
+		printf("Debug Server FW timed out (boot status: 0x%x)\n",
+		       debug_server_boot_status);
+		error = -ETIMEDOUT;
+		goto out;
+	}
+
+	if (debug_server_boot_status & DEBUG_SERVER_INIT_STATUS_MASK) {
+		printf("Debug server FW error'ed out (boot status: 0x%x)\n",
+		       debug_server_boot_status);
+		error = -ENODEV;
+		goto out;
+	}
+
+	printf("Debug server booted\n");
+	printf("Detected firmware %d.%d, (boot status: 0x0%x)\n",
+	       debug_server_ver_info_maj, debug_server_ver_info_min,
+	       debug_server_boot_status);
+
+out:
+	if (error != 0)
+		debug_server_boot_status = -error;
+	else
+		debug_server_boot_status = 0;
+
+	return debug_server_boot_status;
+}
+
diff --git a/drivers/misc/fsl_ifc.c b/drivers/misc/fsl_ifc.c
index 3902e9f..a33efdb 100644
--- a/drivers/misc/fsl_ifc.c
+++ b/drivers/misc/fsl_ifc.c
@@ -168,4 +168,25 @@ void init_final_memctl_regs(void)
 #ifdef CONFIG_SYS_CSPR0_FINAL
 	set_ifc_cspr(IFC_CS0, CONFIG_SYS_CSPR0_FINAL);
 #endif
+#ifdef CONFIG_SYS_AMASK0_FINAL
+	set_ifc_amask(IFC_CS0, CONFIG_SYS_AMASK0);
+#endif
+#ifdef CONFIG_SYS_CSPR1_FINAL
+	set_ifc_cspr(IFC_CS1, CONFIG_SYS_CSPR1_FINAL);
+#endif
+#ifdef CONFIG_SYS_AMASK1_FINAL
+	set_ifc_amask(IFC_CS1, CONFIG_SYS_AMASK1_FINAL);
+#endif
+#ifdef CONFIG_SYS_CSPR2_FINAL
+	set_ifc_cspr(IFC_CS2, CONFIG_SYS_CSPR2_FINAL);
+#endif
+#ifdef CONFIG_SYS_AMASK2_FINAL
+	set_ifc_amask(IFC_CS2, CONFIG_SYS_AMASK2);
+#endif
+#ifdef CONFIG_SYS_CSPR3_FINAL
+	set_ifc_cspr(IFC_CS3, CONFIG_SYS_CSPR3_FINAL);
+#endif
+#ifdef CONFIG_SYS_AMASK3_FINAL
+	set_ifc_amask(IFC_CS3, CONFIG_SYS_AMASK3);
+#endif
 }
diff --git a/drivers/misc/status_led.c b/drivers/misc/status_led.c
index ed9adb2..9869d98 100644
--- a/drivers/misc/status_led.c
+++ b/drivers/misc/status_led.c
@@ -53,6 +53,20 @@ led_dev_t led_dev[] = {
 	0,
     },
 #endif
+#if defined(STATUS_LED_BIT4)
+    {	STATUS_LED_BIT4,
+	STATUS_LED_STATE4,
+	STATUS_LED_PERIOD4,
+	0,
+    },
+#endif
+#if defined(STATUS_LED_BIT5)
+    {	STATUS_LED_BIT5,
+	STATUS_LED_STATE5,
+	STATUS_LED_PERIOD5,
+	0,
+    },
+#endif
 };
 
 #define MAX_LED_DEV	(sizeof(led_dev)/sizeof(led_dev_t))
diff --git a/drivers/misc/swap_case.c b/drivers/misc/swap_case.c
new file mode 100644
index 0000000..f6028ba
--- /dev/null
+++ b/drivers/misc/swap_case.c
@@ -0,0 +1,285 @@
+/*
+ * PCI emulation device which swaps the case of text
+ *
+ * Copyright (c) 2014 Google, Inc
+ * Written by Simon Glass <sjg@chromium.org>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <dm.h>
+#include <pci.h>
+#include <asm/test.h>
+#include <linux/ctype.h>
+
+/**
+ * struct swap_case_platdata - platform data for this device
+ *
+ * @command:	Current PCI command value
+ * @bar:	Current base address values
+ */
+struct swap_case_platdata {
+	u16 command;
+	u32 bar[2];
+};
+
+#define offset_to_barnum(offset)	\
+		(((offset) - PCI_BASE_ADDRESS_0) / sizeof(u32))
+
+enum {
+	MEM_TEXT_SIZE	= 0x100,
+};
+
+enum swap_case_op {
+	OP_TO_LOWER,
+	OP_TO_UPPER,
+	OP_SWAP,
+};
+
+static struct pci_bar {
+	int type;
+	u32 size;
+} barinfo[] = {
+	{ PCI_BASE_ADDRESS_SPACE_IO, 1 },
+	{ PCI_BASE_ADDRESS_MEM_TYPE_32, MEM_TEXT_SIZE },
+	{ 0, 0 },
+	{ 0, 0 },
+	{ 0, 0 },
+	{ 0, 0 },
+};
+
+struct swap_case_priv {
+	enum swap_case_op op;
+	char mem_text[MEM_TEXT_SIZE];
+};
+
+static int sandbox_swap_case_get_devfn(struct udevice *dev)
+{
+	struct pci_child_platdata *plat = dev_get_parent_platdata(dev);
+
+	return plat->devfn;
+}
+
+static int sandbox_swap_case_read_config(struct udevice *emul, uint offset,
+					 ulong *valuep, enum pci_size_t size)
+{
+	struct swap_case_platdata *plat = dev_get_platdata(emul);
+
+	switch (offset) {
+	case PCI_COMMAND:
+		*valuep = plat->command;
+		break;
+	case PCI_HEADER_TYPE:
+		*valuep = 0;
+		break;
+	case PCI_VENDOR_ID:
+		*valuep = SANDBOX_PCI_VENDOR_ID;
+		break;
+	case PCI_DEVICE_ID:
+		*valuep = SANDBOX_PCI_DEVICE_ID;
+		break;
+	case PCI_CLASS_DEVICE:
+		if (size == PCI_SIZE_8) {
+			*valuep = SANDBOX_PCI_CLASS_SUB_CODE;
+		} else {
+			*valuep = (SANDBOX_PCI_CLASS_CODE << 8) |
+					SANDBOX_PCI_CLASS_SUB_CODE;
+		}
+		break;
+	case PCI_CLASS_CODE:
+		*valuep = SANDBOX_PCI_CLASS_CODE;
+		break;
+	case PCI_BASE_ADDRESS_0:
+	case PCI_BASE_ADDRESS_1:
+	case PCI_BASE_ADDRESS_2:
+	case PCI_BASE_ADDRESS_3:
+	case PCI_BASE_ADDRESS_4:
+	case PCI_BASE_ADDRESS_5: {
+		int barnum;
+		u32 *bar, result;
+
+		barnum = offset_to_barnum(offset);
+		bar = &plat->bar[barnum];
+
+		result = *bar;
+		if (*bar == 0xffffffff) {
+			if (barinfo[barnum].type) {
+				result = (~(barinfo[barnum].size - 1) &
+					PCI_BASE_ADDRESS_IO_MASK) |
+					PCI_BASE_ADDRESS_SPACE_IO;
+			} else {
+				result = (~(barinfo[barnum].size - 1) &
+					PCI_BASE_ADDRESS_MEM_MASK) |
+					PCI_BASE_ADDRESS_MEM_TYPE_32;
+			}
+		}
+		debug("r bar %d=%x\n", barnum, result);
+		*valuep = result;
+		break;
+	}
+	}
+
+	return 0;
+}
+
+static int sandbox_swap_case_write_config(struct udevice *emul, uint offset,
+					  ulong value, enum pci_size_t size)
+{
+	struct swap_case_platdata *plat = dev_get_platdata(emul);
+
+	switch (offset) {
+	case PCI_COMMAND:
+		plat->command = value;
+		break;
+	case PCI_BASE_ADDRESS_0:
+	case PCI_BASE_ADDRESS_1: {
+		int barnum;
+		u32 *bar;
+
+		barnum = offset_to_barnum(offset);
+		bar = &plat->bar[barnum];
+
+		debug("w bar %d=%lx\n", barnum, value);
+		*bar = value;
+		break;
+	}
+	}
+
+	return 0;
+}
+
+static int sandbox_swap_case_find_bar(struct udevice *emul, unsigned int addr,
+				      int *barnump, unsigned int *offsetp)
+{
+	struct swap_case_platdata *plat = dev_get_platdata(emul);
+	int barnum;
+
+	for (barnum = 0; barnum < ARRAY_SIZE(barinfo); barnum++) {
+		unsigned int size = barinfo[barnum].size;
+
+		if (addr >= plat->bar[barnum] &&
+		    addr < plat->bar[barnum] + size) {
+			*barnump = barnum;
+			*offsetp = addr - plat->bar[barnum];
+			return 0;
+		}
+	}
+	*barnump = -1;
+
+	return -ENOENT;
+}
+
+static void sandbox_swap_case_do_op(enum swap_case_op op, char *str, int len)
+{
+	for (; len > 0; len--, str++) {
+		switch (op) {
+		case OP_TO_UPPER:
+			*str = toupper(*str);
+			break;
+		case OP_TO_LOWER:
+			*str = tolower(*str);
+			break;
+		case OP_SWAP:
+			if (isupper(*str))
+				*str = tolower(*str);
+			else
+				*str = toupper(*str);
+			break;
+		}
+	}
+}
+
+int sandbox_swap_case_read_io(struct udevice *dev, unsigned int addr,
+			      ulong *valuep, enum pci_size_t size)
+{
+	struct swap_case_priv *priv = dev_get_priv(dev);
+	unsigned int offset;
+	int barnum;
+	int ret;
+
+	ret = sandbox_swap_case_find_bar(dev, addr, &barnum, &offset);
+	if (ret)
+		return ret;
+
+	if (barnum == 0 && offset == 0)
+		*valuep = (*valuep & ~0xff) | priv->op;
+
+	return 0;
+}
+
+int sandbox_swap_case_write_io(struct udevice *dev, unsigned int addr,
+			       ulong value, enum pci_size_t size)
+{
+	struct swap_case_priv *priv = dev_get_priv(dev);
+	unsigned int offset;
+	int barnum;
+	int ret;
+
+	ret = sandbox_swap_case_find_bar(dev, addr, &barnum, &offset);
+	if (ret)
+		return ret;
+	if (barnum == 0 && offset == 0)
+		priv->op = value;
+
+	return 0;
+}
+
+static int sandbox_swap_case_map_physmem(struct udevice *dev,
+		phys_addr_t addr, unsigned long *lenp, void **ptrp)
+{
+	struct swap_case_priv *priv = dev_get_priv(dev);
+	unsigned int offset, avail;
+	int barnum;
+	int ret;
+
+	ret = sandbox_swap_case_find_bar(dev, addr, &barnum, &offset);
+	if (ret)
+		return ret;
+	if (barnum == 1) {
+		*ptrp = priv->mem_text + offset;
+		avail = barinfo[1].size - offset;
+		if (avail > barinfo[1].size)
+			*lenp = 0;
+		else
+			*lenp = min(*lenp, (ulong)avail);
+
+		return 0;
+	}
+
+	return -ENOENT;
+}
+
+static int sandbox_swap_case_unmap_physmem(struct udevice *dev,
+					   const void *vaddr, unsigned long len)
+{
+	struct swap_case_priv *priv = dev_get_priv(dev);
+
+	sandbox_swap_case_do_op(priv->op, (void *)vaddr, len);
+
+	return 0;
+}
+
+struct dm_pci_emul_ops sandbox_swap_case_emul_ops = {
+	.get_devfn = sandbox_swap_case_get_devfn,
+	.read_config = sandbox_swap_case_read_config,
+	.write_config = sandbox_swap_case_write_config,
+	.read_io = sandbox_swap_case_read_io,
+	.write_io = sandbox_swap_case_write_io,
+	.map_physmem = sandbox_swap_case_map_physmem,
+	.unmap_physmem = sandbox_swap_case_unmap_physmem,
+};
+
+static const struct udevice_id sandbox_swap_case_ids[] = {
+	{ .compatible = "sandbox,swap-case" },
+	{ }
+};
+
+U_BOOT_DRIVER(sandbox_swap_case_emul) = {
+	.name		= "sandbox_swap_case_emul",
+	.id		= UCLASS_PCI_EMUL,
+	.of_match	= sandbox_swap_case_ids,
+	.ops		= &sandbox_swap_case_emul_ops,
+	.priv_auto_alloc_size = sizeof(struct swap_case_priv),
+	.platdata_auto_alloc_size = sizeof(struct swap_case_platdata),
+};
diff --git a/drivers/mmc/fsl_esdhc.c b/drivers/mmc/fsl_esdhc.c
index db4d251..10ec216 100644
--- a/drivers/mmc/fsl_esdhc.c
+++ b/drivers/mmc/fsl_esdhc.c
@@ -105,7 +105,8 @@ static uint esdhc_xfertyp(struct mmc_cmd *cmd, struct mmc_data *data)
 	else if (cmd->resp_type & MMC_RSP_PRESENT)
 		xfertyp |= XFERTYP_RSPTYP_48;
 
-#if defined(CONFIG_MX53) || defined(CONFIG_PPC_T4240) || defined(CONFIG_LS102XA)
+#if defined(CONFIG_MX53) || defined(CONFIG_PPC_T4240) || \
+	defined(CONFIG_LS102XA) || defined(CONFIG_LS2085A)
 	if (cmd->cmdidx == MMC_CMD_STOP_TRANSMISSION)
 		xfertyp |= XFERTYP_CMDTYP_ABORT;
 #endif
@@ -183,7 +184,9 @@ static int esdhc_setup_data(struct mmc *mmc, struct mmc_data *data)
 	int timeout;
 	struct fsl_esdhc_cfg *cfg = mmc->priv;
 	struct fsl_esdhc *regs = (struct fsl_esdhc *)cfg->esdhc_base;
-
+#ifdef CONFIG_LS2085A
+	dma_addr_t addr;
+#endif
 	uint wml_value;
 
 	wml_value = data->blocksize/4;
@@ -194,8 +197,16 @@ static int esdhc_setup_data(struct mmc *mmc, struct mmc_data *data)
 
 		esdhc_clrsetbits32(&regs->wml, WML_RD_WML_MASK, wml_value);
 #ifndef CONFIG_SYS_FSL_ESDHC_USE_PIO
+#ifdef CONFIG_LS2085A
+		addr = virt_to_phys((void *)(data->dest));
+		if (upper_32_bits(addr))
+			printf("Error found for upper 32 bits\n");
+		else
+			esdhc_write32(&regs->dsaddr, lower_32_bits(addr));
+#else
 		esdhc_write32(&regs->dsaddr, (u32)data->dest);
 #endif
+#endif
 	} else {
 #ifndef CONFIG_SYS_FSL_ESDHC_USE_PIO
 		flush_dcache_range((ulong)data->src,
@@ -212,8 +223,16 @@ static int esdhc_setup_data(struct mmc *mmc, struct mmc_data *data)
 		esdhc_clrsetbits32(&regs->wml, WML_WR_WML_MASK,
 					wml_value << 16);
 #ifndef CONFIG_SYS_FSL_ESDHC_USE_PIO
+#ifdef CONFIG_LS2085A
+		addr = virt_to_phys((void *)(data->src));
+		if (upper_32_bits(addr))
+			printf("Error found for upper 32 bits\n");
+		else
+			esdhc_write32(&regs->dsaddr, lower_32_bits(addr));
+#else
 		esdhc_write32(&regs->dsaddr, (u32)data->src);
 #endif
+#endif
 	}
 
 	esdhc_write32(&regs->blkattr, data->blocks << 16 | data->blocksize);
@@ -259,10 +278,23 @@ static int esdhc_setup_data(struct mmc *mmc, struct mmc_data *data)
 static void check_and_invalidate_dcache_range
 	(struct mmc_cmd *cmd,
 	 struct mmc_data *data) {
+#ifdef CONFIG_LS2085A
+	unsigned start = 0;
+#else
 	unsigned start = (unsigned)data->dest ;
+#endif
 	unsigned size = roundup(ARCH_DMA_MINALIGN,
 				data->blocks*data->blocksize);
 	unsigned end = start+size ;
+#ifdef CONFIG_LS2085A
+	dma_addr_t addr;
+
+	addr = virt_to_phys((void *)(data->dest));
+	if (upper_32_bits(addr))
+		printf("Error found for upper 32 bits\n");
+	else
+		start = lower_32_bits(addr);
+#endif
 	invalidate_dcache_range(start, end);
 }
 #endif
diff --git a/drivers/mmc/zynq_sdhci.c b/drivers/mmc/zynq_sdhci.c
index 7887f11..d4f3882 100644
--- a/drivers/mmc/zynq_sdhci.c
+++ b/drivers/mmc/zynq_sdhci.c
@@ -25,7 +25,7 @@ int zynq_sdhci_init(phys_addr_t regbase)
 
 	host->name = "zynq_sdhci";
 	host->ioaddr = (void *)regbase;
-	host->quirks = SDHCI_QUIRK_NO_CD | SDHCI_QUIRK_WAIT_SEND_CMD |
+	host->quirks = SDHCI_QUIRK_WAIT_SEND_CMD |
 		       SDHCI_QUIRK_BROKEN_R1B;
 	host->version = sdhci_readw(host, SDHCI_HOST_VERSION);
 
diff --git a/drivers/mtd/nand/fsl_ifc_nand.c b/drivers/mtd/nand/fsl_ifc_nand.c
index 7903eeb..79fa88b 100644
--- a/drivers/mtd/nand/fsl_ifc_nand.c
+++ b/drivers/mtd/nand/fsl_ifc_nand.c
@@ -46,7 +46,7 @@ struct fsl_ifc_ctrl {
 	struct fsl_ifc_mtd *chips[MAX_BANKS];
 
 	/* device info */
-	struct fsl_ifc *regs;
+	struct fsl_ifc regs;
 	uint8_t __iomem *addr;   /* Address of assigned IFC buffer        */
 	unsigned int cs_nand;    /* On which chipsel NAND is connected	  */
 	unsigned int page;       /* Last page written to / read from      */
@@ -225,7 +225,7 @@ static void set_addr(struct mtd_info *mtd, int column, int page_addr, int oob)
 	struct nand_chip *chip = mtd->priv;
 	struct fsl_ifc_mtd *priv = chip->priv;
 	struct fsl_ifc_ctrl *ctrl = priv->ctrl;
-	struct fsl_ifc *ifc = ctrl->regs;
+	struct fsl_ifc_runtime *ifc = ctrl->regs.rregs;
 	int buf_num;
 
 	ctrl->page = page_addr;
@@ -289,10 +289,10 @@ static int fsl_ifc_run_command(struct mtd_info *mtd)
 	struct nand_chip *chip = mtd->priv;
 	struct fsl_ifc_mtd *priv = chip->priv;
 	struct fsl_ifc_ctrl *ctrl = priv->ctrl;
-	struct fsl_ifc *ifc = ctrl->regs;
+	struct fsl_ifc_runtime *ifc = ctrl->regs.rregs;
 	u32 timeo = (CONFIG_SYS_HZ * 10) / 1000;
 	u32 time_start;
-	u32 eccstat[4] = {0};
+	u32 eccstat[8] = {0};
 	int i;
 
 	/* set the chip select for NAND Transaction */
@@ -325,8 +325,15 @@ static int fsl_ifc_run_command(struct mtd_info *mtd)
 		int sector = bufnum * chip->ecc.steps;
 		int sector_end = sector + chip->ecc.steps - 1;
 
-		for (i = sector / 4; i <= sector_end / 4; i++)
+		for (i = sector / 4; i <= sector_end / 4; i++) {
+			if (i >= ARRAY_SIZE(eccstat)) {
+				printf("%s: eccstat too small for %d\n",
+				       __func__, i);
+				return -EIO;
+			}
+
 			eccstat[i] = ifc_in32(&ifc->ifc_nand.nand_eccstat[i]);
+		}
 
 		for (i = sector; i <= sector_end; i++) {
 			errors = check_read_ecc(mtd, ctrl, eccstat, i);
@@ -362,7 +369,7 @@ static void fsl_ifc_do_read(struct nand_chip *chip,
 {
 	struct fsl_ifc_mtd *priv = chip->priv;
 	struct fsl_ifc_ctrl *ctrl = priv->ctrl;
-	struct fsl_ifc *ifc = ctrl->regs;
+	struct fsl_ifc_runtime *ifc = ctrl->regs.rregs;
 
 	/* Program FIR/IFC_NAND_FCR0 for Small/Large page */
 	if (mtd->writesize > 512) {
@@ -400,7 +407,7 @@ static void fsl_ifc_cmdfunc(struct mtd_info *mtd, unsigned int command,
 	struct nand_chip *chip = mtd->priv;
 	struct fsl_ifc_mtd *priv = chip->priv;
 	struct fsl_ifc_ctrl *ctrl = priv->ctrl;
-	struct fsl_ifc *ifc = ctrl->regs;
+	struct fsl_ifc_runtime *ifc = ctrl->regs.rregs;
 
 	/* clear the read buffer */
 	ctrl->read_bytes = 0;
@@ -690,7 +697,7 @@ static int fsl_ifc_wait(struct mtd_info *mtd, struct nand_chip *chip)
 {
 	struct fsl_ifc_mtd *priv = chip->priv;
 	struct fsl_ifc_ctrl *ctrl = priv->ctrl;
-	struct fsl_ifc *ifc = ctrl->regs;
+	struct fsl_ifc_runtime *ifc = ctrl->regs.rregs;
 	u32 nand_fsr;
 
 	if (ctrl->status != IFC_NAND_EVTER_STAT_OPC)
@@ -747,24 +754,33 @@ static int fsl_ifc_write_page(struct mtd_info *mtd, struct nand_chip *chip,
 
 static void fsl_ifc_ctrl_init(void)
 {
+	uint32_t ver = 0;
 	ifc_ctrl = kzalloc(sizeof(*ifc_ctrl), GFP_KERNEL);
 	if (!ifc_ctrl)
 		return;
 
-	ifc_ctrl->regs = IFC_BASE_ADDR;
+	ifc_ctrl->regs.gregs = IFC_FCM_BASE_ADDR;
+
+	ver = ifc_in32(&ifc_ctrl->regs.gregs->ifc_rev);
+	if (ver >= FSL_IFC_V2_0_0)
+		ifc_ctrl->regs.rregs =
+			(void *)CONFIG_SYS_IFC_ADDR + IFC_RREGS_64KOFFSET;
+	else
+		ifc_ctrl->regs.rregs =
+			(void *)CONFIG_SYS_IFC_ADDR + IFC_RREGS_4KOFFSET;
 
 	/* clear event registers */
-	ifc_out32(&ifc_ctrl->regs->ifc_nand.nand_evter_stat, ~0U);
-	ifc_out32(&ifc_ctrl->regs->ifc_nand.pgrdcmpl_evt_stat, ~0U);
+	ifc_out32(&ifc_ctrl->regs.rregs->ifc_nand.nand_evter_stat, ~0U);
+	ifc_out32(&ifc_ctrl->regs.rregs->ifc_nand.pgrdcmpl_evt_stat, ~0U);
 
 	/* Enable error and event for any detected errors */
-	ifc_out32(&ifc_ctrl->regs->ifc_nand.nand_evter_en,
+	ifc_out32(&ifc_ctrl->regs.rregs->ifc_nand.nand_evter_en,
 		  IFC_NAND_EVTER_EN_OPC_EN |
 		  IFC_NAND_EVTER_EN_PGRDCMPL_EN |
 		  IFC_NAND_EVTER_EN_FTOER_EN |
 		  IFC_NAND_EVTER_EN_WPER_EN);
 
-	ifc_out32(&ifc_ctrl->regs->ifc_nand.ncfgr, 0x0);
+	ifc_out32(&ifc_ctrl->regs.rregs->ifc_nand.ncfgr, 0x0);
 }
 
 static void fsl_ifc_select_chip(struct mtd_info *mtd, int chip)
@@ -773,7 +789,7 @@ static void fsl_ifc_select_chip(struct mtd_info *mtd, int chip)
 
 static int fsl_ifc_sram_init(uint32_t ver)
 {
-	struct fsl_ifc *ifc = ifc_ctrl->regs;
+	struct fsl_ifc_runtime *ifc = ifc_ctrl->regs.rregs;
 	uint32_t cs = 0, csor = 0, csor_8k = 0, csor_ext = 0;
 	uint32_t ncfgr = 0;
 	u32 timeo = (CONFIG_SYS_HZ * 10) / 1000;
@@ -799,13 +815,13 @@ static int fsl_ifc_sram_init(uint32_t ver)
 	cs = ifc_ctrl->cs_nand >> IFC_NAND_CSEL_SHIFT;
 
 	/* Save CSOR and CSOR_ext */
-	csor = ifc_in32(&ifc_ctrl->regs->csor_cs[cs].csor);
-	csor_ext = ifc_in32(&ifc_ctrl->regs->csor_cs[cs].csor_ext);
+	csor = ifc_in32(&ifc_ctrl->regs.gregs->csor_cs[cs].csor);
+	csor_ext = ifc_in32(&ifc_ctrl->regs.gregs->csor_cs[cs].csor_ext);
 
 	/* chage PageSize 8K and SpareSize 1K*/
 	csor_8k = (csor & ~(CSOR_NAND_PGS_MASK)) | 0x0018C000;
-	ifc_out32(&ifc_ctrl->regs->csor_cs[cs].csor, csor_8k);
-	ifc_out32(&ifc_ctrl->regs->csor_cs[cs].csor_ext, 0x0000400);
+	ifc_out32(&ifc_ctrl->regs.gregs->csor_cs[cs].csor, csor_8k);
+	ifc_out32(&ifc_ctrl->regs.gregs->csor_cs[cs].csor_ext, 0x0000400);
 
 	/* READID */
 	ifc_out32(&ifc->ifc_nand.nand_fir0,
@@ -845,8 +861,8 @@ static int fsl_ifc_sram_init(uint32_t ver)
 	ifc_out32(&ifc->ifc_nand.nand_evter_stat, ifc_ctrl->status);
 
 	/* Restore CSOR and CSOR_ext */
-	ifc_out32(&ifc_ctrl->regs->csor_cs[cs].csor, csor);
-	ifc_out32(&ifc_ctrl->regs->csor_cs[cs].csor_ext, csor_ext);
+	ifc_out32(&ifc_ctrl->regs.gregs->csor_cs[cs].csor, csor);
+	ifc_out32(&ifc_ctrl->regs.gregs->csor_cs[cs].csor_ext, csor_ext);
 
 	return 0;
 }
@@ -857,6 +873,7 @@ static int fsl_ifc_chip_init(int devnum, u8 *addr)
 	struct nand_chip *nand;
 	struct fsl_ifc_mtd *priv;
 	struct nand_ecclayout *layout;
+	struct fsl_ifc_fcm *gregs = NULL;
 	uint32_t cspr = 0, csor = 0, ver = 0;
 	int ret = 0;
 
@@ -872,14 +889,15 @@ static int fsl_ifc_chip_init(int devnum, u8 *addr)
 
 	priv->ctrl = ifc_ctrl;
 	priv->vbase = addr;
+	gregs = ifc_ctrl->regs.gregs;
 
 	/* Find which chip select it is connected to.
 	 */
 	for (priv->bank = 0; priv->bank < MAX_BANKS; priv->bank++) {
 		phys_addr_t phys_addr = virt_to_phys(addr);
 
-		cspr = ifc_in32(&ifc_ctrl->regs->cspr_cs[priv->bank].cspr);
-		csor = ifc_in32(&ifc_ctrl->regs->csor_cs[priv->bank].csor);
+		cspr = ifc_in32(&gregs->cspr_cs[priv->bank].cspr);
+		csor = ifc_in32(&gregs->csor_cs[priv->bank].csor);
 
 		if ((cspr & CSPR_V) && (cspr & CSPR_MSEL) == CSPR_MSEL_NAND &&
 		    (cspr & CSPR_BA) == CSPR_PHYS_ADDR(phys_addr)) {
@@ -998,7 +1016,7 @@ static int fsl_ifc_chip_init(int devnum, u8 *addr)
 		nand->ecc.mode = NAND_ECC_SOFT;
 	}
 
-	ver = ifc_in32(&ifc_ctrl->regs->ifc_rev);
+	ver = ifc_in32(&gregs->ifc_rev);
 	if (ver >= FSL_IFC_V1_1_0)
 		ret = fsl_ifc_sram_init(ver);
 	if (ret)
diff --git a/drivers/mtd/nand/fsl_ifc_spl.c b/drivers/mtd/nand/fsl_ifc_spl.c
index fb827c5..fccbfb5 100644
--- a/drivers/mtd/nand/fsl_ifc_spl.c
+++ b/drivers/mtd/nand/fsl_ifc_spl.c
@@ -48,11 +48,25 @@ static inline int check_read_ecc(uchar *buf, u32 *eccstat,
 	return 0;
 }
 
+static inline struct fsl_ifc_runtime *runtime_regs_address(void)
+{
+	struct fsl_ifc regs = {(void *)CONFIG_SYS_IFC_ADDR, NULL};
+	int ver = 0;
+
+	ver = ifc_in32(&regs.gregs->ifc_rev);
+	if (ver >= FSL_IFC_V2_0_0)
+		regs.rregs = (void *)CONFIG_SYS_IFC_ADDR + IFC_RREGS_64KOFFSET;
+	else
+		regs.rregs = (void *)CONFIG_SYS_IFC_ADDR + IFC_RREGS_4KOFFSET;
+
+	return regs.rregs;
+}
+
 static inline void nand_wait(uchar *buf, int bufnum, int page_size)
 {
-	struct fsl_ifc *ifc = IFC_BASE_ADDR;
+	struct fsl_ifc_runtime *ifc = runtime_regs_address();
 	u32 status;
-	u32 eccstat[4];
+	u32 eccstat[8];
 	int bufperpage = page_size / 512;
 	int bufnum_end, i;
 
@@ -90,7 +104,8 @@ static inline int bad_block(uchar *marker, int port_size)
 
 int nand_spl_load_image(uint32_t offs, unsigned int uboot_size, void *vdst)
 {
-	struct fsl_ifc *ifc = IFC_BASE_ADDR;
+	struct fsl_ifc_fcm *gregs = (void *)CONFIG_SYS_IFC_ADDR;
+	struct fsl_ifc_runtime *ifc = NULL;
 	uchar *buf = (uchar *)CONFIG_SYS_NAND_BASE;
 	int page_size;
 	int port_size;
@@ -107,6 +122,8 @@ int nand_spl_load_image(uint32_t offs, unsigned int uboot_size, void *vdst)
 	int pg_no;
 	uchar *dst = vdst;
 
+	ifc = runtime_regs_address();
+
 	/* Get NAND Flash configuration */
 	csor = CONFIG_SYS_NAND_CSOR;
 	cspr = CONFIG_SYS_NAND_CSPR;
@@ -130,7 +147,7 @@ int nand_spl_load_image(uint32_t offs, unsigned int uboot_size, void *vdst)
 			bad_marker = 5;
 	}
 
-	ver = ifc_in32(&ifc->ifc_rev);
+	ver = ifc_in32(&gregs->ifc_rev);
 	if (ver >= FSL_IFC_V2_0_0)
 		bufnum_mask = (bufnum_mask * 2) + 1;
 
diff --git a/drivers/mtd/spi/Kconfig b/drivers/mtd/spi/Kconfig
index 2dc46b4..ac6d09f 100644
--- a/drivers/mtd/spi/Kconfig
+++ b/drivers/mtd/spi/Kconfig
@@ -1,6 +1,6 @@
 config DM_SPI_FLASH
 	bool "Enable Driver Model for SPI flash"
-	depends on DM && SPI
+	depends on DM && DM_SPI
 	help
 	  Enable driver model for SPI flash. This SPI flash interface
 	  (spi_flash_probe(), spi_flash_write(), etc.) is then
@@ -12,3 +12,13 @@ config DM_SPI_FLASH
 	  during the transition parent. SPI and SPI flash must be
 	  enabled together (it is not possible to use driver model
 	  for one and not the other).
+
+config SPI_FLASH_SANDBOX
+	bool "Support sandbox SPI flash device"
+	depends on SANDBOX && DM_SPI_FLASH
+	help
+	  Since sandbox cannot access real devices, an emulation mechanism is
+	  provided instead. Drivers can be connected up to the sandbox SPI
+	  bus (see CONFIG_SANDBOX_SPI) and SPI traffic will be routed to this
+	  device. Typically the contents of the emulated SPI flash device is
+	  stored in a file on the host filesystem.
diff --git a/drivers/mtd/spi/sf-uclass.c b/drivers/mtd/spi/sf-uclass.c
index 376d815..4b25902 100644
--- a/drivers/mtd/spi/sf-uclass.c
+++ b/drivers/mtd/spi/sf-uclass.c
@@ -11,6 +11,22 @@
 #include <dm/device-internal.h>
 #include "sf_internal.h"
 
+int spi_flash_read_dm(struct udevice *dev, u32 offset, size_t len, void *buf)
+{
+	return sf_get_ops(dev)->read(dev, offset, len, buf);
+}
+
+int spi_flash_write_dm(struct udevice *dev, u32 offset, size_t len,
+		       const void *buf)
+{
+	return sf_get_ops(dev)->write(dev, offset, len, buf);
+}
+
+int spi_flash_erase_dm(struct udevice *dev, u32 offset, size_t len)
+{
+	return sf_get_ops(dev)->erase(dev, offset, len);
+}
+
 /*
  * TODO(sjg@chromium.org): This is an old-style function. We should remove
  * it when all SPI flash drivers use dm
@@ -23,7 +39,7 @@ struct spi_flash *spi_flash_probe(unsigned int bus, unsigned int cs,
 	if (spi_flash_probe_bus_cs(bus, cs, max_hz, spi_mode, &dev))
 		return NULL;
 
-	return dev->uclass_priv;
+	return dev_get_uclass_priv(dev);
 }
 
 void spi_flash_free(struct spi_flash *flash)
diff --git a/drivers/mtd/spi/sf_internal.h b/drivers/mtd/spi/sf_internal.h
index 785f7a9..4158e13 100644
--- a/drivers/mtd/spi/sf_internal.h
+++ b/drivers/mtd/spi/sf_internal.h
@@ -97,10 +97,6 @@ enum {
 #define STATUS_QEB_MXIC		(1 << 6)
 #define STATUS_PEC			(1 << 7)
 
-#ifdef CONFIG_SYS_SPI_ST_ENABLE_WP_PIN
-#define STATUS_SRWD			(1 << 7) /* SR write protect */
-#endif
-
 /* Flash timeout values */
 #define SPI_FLASH_PROG_TIMEOUT		(2 * CONFIG_SYS_HZ)
 #define SPI_FLASH_PAGE_ERASE_TIMEOUT		(5 * CONFIG_SYS_HZ)
@@ -123,7 +119,8 @@ int sst_write_bp(struct spi_flash *flash, u32 offset, size_t len,
  * @name:		Device name ([MANUFLETTER][DEVTYPE][DENSITY][EXTRAINFO])
  * @jedec:		Device jedec ID (0x[1byte_manuf_id][2byte_dev_id])
  * @ext_jedec:		Device ext_jedec ID
- * @sector_size:	Sector size of this device
+ * @sector_size:	Isn't necessarily a sector size from vendor,
+ *			the size listed here is what works with CMD_ERASE_64K
  * @nr_sectors:	No.of sectors on this device
  * @e_rd_cmd:		Enum list for read commands
  * @flags:		Important param, for flash specific behaviour
diff --git a/drivers/mtd/spi/sf_ops.c b/drivers/mtd/spi/sf_ops.c
index 34bc54e..38592f5 100644
--- a/drivers/mtd/spi/sf_ops.c
+++ b/drivers/mtd/spi/sf_ops.c
@@ -154,21 +154,17 @@ static void spi_flash_dual_flash(struct spi_flash *flash, u32 *addr)
 }
 #endif
 
-int spi_flash_cmd_wait_ready(struct spi_flash *flash, unsigned long timeout)
+static int spi_flash_poll_status(struct spi_slave *spi, unsigned long timeout,
+				 u8 cmd, u8 poll_bit)
 {
-	struct spi_slave *spi = flash->spi;
 	unsigned long timebase;
 	unsigned long flags = SPI_XFER_BEGIN;
 	int ret;
 	u8 status;
 	u8 check_status = 0x0;
-	u8 poll_bit = STATUS_WIP;
-	u8 cmd = flash->poll_cmd;
 
-	if (cmd == CMD_FLAG_STATUS) {
-		poll_bit = STATUS_PEC;
+	if (cmd == CMD_FLAG_STATUS)
 		check_status = poll_bit;
-	}
 
 #ifdef CONFIG_SF_DUAL_FLASH
 	if (spi->flags & SPI_XFER_U_PAGE)
@@ -204,6 +200,28 @@ int spi_flash_cmd_wait_ready(struct spi_flash *flash, unsigned long timeout)
 	return -1;
 }
 
+int spi_flash_cmd_wait_ready(struct spi_flash *flash, unsigned long timeout)
+{
+	struct spi_slave *spi = flash->spi;
+	int ret;
+	u8 poll_bit = STATUS_WIP;
+	u8 cmd = CMD_READ_STATUS;
+
+	ret = spi_flash_poll_status(spi, timeout, cmd, poll_bit);
+	if (ret < 0)
+		return ret;
+
+	if (flash->poll_cmd == CMD_FLAG_STATUS) {
+		poll_bit = STATUS_PEC;
+		cmd = CMD_FLAG_STATUS;
+		ret = spi_flash_poll_status(spi, timeout, cmd, poll_bit);
+		if (ret < 0)
+			return ret;
+	}
+
+	return 0;
+}
+
 int spi_flash_write_common(struct spi_flash *flash, const u8 *cmd,
 		size_t cmd_len, const void *buf, size_t buf_len)
 {
diff --git a/drivers/mtd/spi/sf_probe.c b/drivers/mtd/spi/sf_probe.c
index 4103723..201471c 100644
--- a/drivers/mtd/spi/sf_probe.c
+++ b/drivers/mtd/spi/sf_probe.c
@@ -13,6 +13,7 @@
 #include <errno.h>
 #include <fdtdec.h>
 #include <malloc.h>
+#include <mapmem.h>
 #include <spi.h>
 #include <spi_flash.h>
 #include <asm/io.h>
@@ -131,6 +132,9 @@ static int spi_flash_validate_params(struct spi_slave *spi, u8 *idcode,
 	flash->name = params->name;
 	flash->memory_map = spi->memory_map;
 	flash->dual_flash = flash->spi->option;
+#ifdef CONFIG_DM_SPI_FLASH
+	flash->flags = params->flags;
+#endif
 
 	/* Assign spi_flash ops */
 #ifndef CONFIG_DM_SPI_FLASH
@@ -183,6 +187,9 @@ static int spi_flash_validate_params(struct spi_slave *spi, u8 *idcode,
 		flash->erase_size = flash->sector_size;
 	}
 
+	/* Now erase size becomes valid sector size */
+	flash->sector_size = flash->erase_size;
+
 	/* Look for the fastest read cmd */
 	cmd = fls(params->e_rd_cmd & flash->spi->op_mode_rx);
 	if (cmd) {
@@ -287,34 +294,6 @@ int spi_flash_decode_fdt(const void *blob, struct spi_flash *flash)
 }
 #endif /* CONFIG_OF_CONTROL */
 
-#ifdef CONFIG_SYS_SPI_ST_ENABLE_WP_PIN
-/* enable the W#/Vpp signal to disable writing to the status register */
-static int spi_enable_wp_pin(struct spi_flash *flash)
-{
-	u8 status;
-	int ret;
-
-	ret = spi_flash_cmd_read_status(flash, &status);
-	if (ret < 0)
-		return ret;
-
-	ret = spi_flash_cmd_write_status(flash, STATUS_SRWD);
-	if (ret < 0)
-		return ret;
-
-	ret = spi_flash_cmd_write_disable(flash);
-	if (ret < 0)
-		return ret;
-
-	return 0;
-}
-#else
-static int spi_enable_wp_pin(struct spi_flash *flash)
-{
-	return 0;
-}
-#endif
-
 /**
  * spi_flash_probe_slave() - Probe for a SPI flash device on a bus
  *
@@ -393,8 +372,6 @@ int spi_flash_probe_slave(struct spi_slave *spi, struct spi_flash *flash)
 		puts(" Full access #define CONFIG_SPI_FLASH_BAR\n");
 	}
 #endif
-	if (spi_enable_wp_pin(flash))
-		puts("Enable WP pin failed\n");
 
 	/* Release spi bus */
 	spi_release_bus(spi);
@@ -433,6 +410,8 @@ struct spi_flash *spi_flash_probe(unsigned int busnum, unsigned int cs,
 	struct spi_slave *bus;
 
 	bus = spi_setup_slave(busnum, cs, max_hz, spi_mode);
+	if (!bus)
+		return NULL;
 	return spi_flash_probe_tail(bus);
 }
 
@@ -443,6 +422,8 @@ struct spi_flash *spi_flash_probe_fdt(const void *blob, int slave_node,
 	struct spi_slave *bus;
 
 	bus = spi_setup_slave_fdt(blob, slave_node, spi_node);
+	if (!bus)
+		return NULL;
 	return spi_flash_probe_tail(bus);
 }
 #endif
@@ -458,7 +439,7 @@ void spi_flash_free(struct spi_flash *flash)
 static int spi_flash_std_read(struct udevice *dev, u32 offset, size_t len,
 			      void *buf)
 {
-	struct spi_flash *flash = dev->uclass_priv;
+	struct spi_flash *flash = dev_get_uclass_priv(dev);
 
 	return spi_flash_cmd_read_ops(flash, offset, len, buf);
 }
@@ -466,14 +447,23 @@ static int spi_flash_std_read(struct udevice *dev, u32 offset, size_t len,
 int spi_flash_std_write(struct udevice *dev, u32 offset, size_t len,
 			const void *buf)
 {
-	struct spi_flash *flash = dev->uclass_priv;
+	struct spi_flash *flash = dev_get_uclass_priv(dev);
+
+#if defined(CONFIG_SPI_FLASH_SST)
+	if (flash->flags & SST_WR) {
+		if (flash->spi->op_mode_tx & SPI_OPM_TX_BP)
+			return sst_write_bp(flash, offset, len, buf);
+		else
+			return sst_write_wp(flash, offset, len, buf);
+	}
+#endif
 
 	return spi_flash_cmd_write_ops(flash, offset, len, buf);
 }
 
 int spi_flash_std_erase(struct udevice *dev, u32 offset, size_t len)
 {
-	struct spi_flash *flash = dev->uclass_priv;
+	struct spi_flash *flash = dev_get_uclass_priv(dev);
 
 	return spi_flash_cmd_erase_ops(flash, offset, len);
 }
@@ -484,7 +474,7 @@ int spi_flash_std_probe(struct udevice *dev)
 	struct dm_spi_slave_platdata *plat = dev_get_parent_platdata(dev);
 	struct spi_flash *flash;
 
-	flash = dev->uclass_priv;
+	flash = dev_get_uclass_priv(dev);
 	flash->dev = dev;
 	debug("%s: slave=%p, cs=%d\n", __func__, slave, plat->cs);
 	return spi_flash_probe_slave(slave, flash);
diff --git a/drivers/net/4xx_enet.c b/drivers/net/4xx_enet.c
index 381ec42..3c30f42 100644
--- a/drivers/net/4xx_enet.c
+++ b/drivers/net/4xx_enet.c
@@ -1350,7 +1350,7 @@ get_speed:
 	for (i = 0; i < NUM_RX_BUFF; i++) {
 		hw_p->rx[i].ctrl = 0;
 		hw_p->rx[i].data_len = 0;
-		hw_p->rx[i].data_ptr = (char *)NetRxPackets[i];
+		hw_p->rx[i].data_ptr = (char *)net_rx_packets[i];
 		if ((NUM_RX_BUFF - 1) == i)
 			hw_p->rx[i].ctrl |= MAL_RX_CTRL_WRAP;
 		hw_p->rx[i].ctrl |= MAL_RX_CTRL_EMPTY | MAL_RX_CTRL_INTR;
@@ -1719,8 +1719,6 @@ static void mal_err (struct eth_device *dev, unsigned long isr,
 		     unsigned long uic, unsigned long maldef,
 		     unsigned long mal_errr)
 {
-	EMAC_4XX_HW_PST hw_p = dev->priv;
-
 	mtdcr (MAL0_ESR, isr);	/* clear interrupt */
 
 	/* clear DE interrupt */
@@ -1728,10 +1726,11 @@ static void mal_err (struct eth_device *dev, unsigned long isr,
 	mtdcr (MAL0_RXDEIR, 0x80000000);
 
 #ifdef INFO_4XX_ENET
-	printf ("\nMAL error occured.... ISR = %lx UIC = = %lx	MAL_DEF = %lx  MAL_ERR= %lx \n", isr, uic, maldef, mal_errr);
+	printf("\nMAL error occured.... ISR = %lx UIC = = %lx	MAL_DEF = %lx  MAL_ERR= %lx\n",
+	       isr, uic, maldef, mal_errr);
 #endif
 
-	eth_init (hw_p->bis);	/* start again... */
+	eth_init();	/* start again... */
 }
 
 /*-----------------------------------------------------------------------------+
@@ -1859,13 +1858,17 @@ static int ppc_4xx_eth_rx (struct eth_device *dev)
 
 		length = hw_p->rx[user_index].data_len & 0x0fff;
 
-		/* Pass the packet up to the protocol layers. */
-		/*	 NetReceive(NetRxPackets[rxIdx], length - 4); */
-		/*	 NetReceive(NetRxPackets[i], length); */
+		/*
+		 * Pass the packet up to the protocol layers.
+		 * net_process_received_packet(net_rx_packets[rxIdx],
+		 *			       length - 4);
+		 * net_process_received_packet(net_rx_packets[i], length);
+		 */
 		invalidate_dcache_range((u32)hw_p->rx[user_index].data_ptr,
 					(u32)hw_p->rx[user_index].data_ptr +
 					length - 4);
-		NetReceive (NetRxPackets[user_index], length - 4);
+		net_process_received_packet(net_rx_packets[user_index],
+					    length - 4);
 		/* Free Recv Buffer */
 		hw_p->rx[user_index].ctrl |= MAL_RX_CTRL_EMPTY;
 		/* Free rx buffer descriptor queue */
diff --git a/drivers/net/Kconfig b/drivers/net/Kconfig
index e69de29..973258a 100644
--- a/drivers/net/Kconfig
+++ b/drivers/net/Kconfig
@@ -0,0 +1,49 @@
+config DM_ETH
+	bool "Enable Driver Model for Ethernet drivers"
+	depends on DM
+	help
+	  Enable driver model for Ethernet.
+
+	  The eth_*() interface will be implemented by the UC_ETH class
+	  This is currently implemented in net/eth.c
+	  Look in include/net.h for details.
+
+menuconfig NETDEVICES
+	bool "Network device support"
+	depends on NET
+	help
+	  You must select Y to enable any network device support
+	  Generally if you have any networking support this is a given
+
+	  If unsure, say Y
+
+if NETDEVICES
+
+config ETH_SANDBOX
+	depends on DM_ETH && SANDBOX
+	default y
+	bool "Sandbox: Mocked Ethernet driver"
+	help
+	  This driver simply responds with fake ARP replies and ping
+	  replies that are used to verify network stack functionality
+
+	  This driver is particularly useful in the test/dm/eth.c tests
+
+config ETH_SANDBOX_RAW
+	depends on DM_ETH && SANDBOX
+	default y
+	bool "Sandbox: Bridge to Linux Raw Sockets"
+	help
+	  This driver is a bridge from the bottom of the network stack
+	  in U-Boot to the RAW AF_PACKET API in Linux. This allows real
+	  network traffic to be tested from within sandbox. See
+	  board/sandbox/README.sandbox for more details.
+
+config ETH_DESIGNWARE
+	bool "Synopsys Designware Ethernet MAC"
+	help
+	  This MAC is present in SoCs from various vendors. It supports
+	  100Mbit and 1 Gbit operation. You must enable CONFIG_PHYLIB to
+	  provide the PHY (physical media interface).
+
+endif # NETDEVICES
diff --git a/drivers/net/Makefile b/drivers/net/Makefile
index 5a5269a..150470c 100644
--- a/drivers/net/Makefile
+++ b/drivers/net/Makefile
@@ -16,7 +16,7 @@ obj-$(CONFIG_BFIN_MAC) += bfin_mac.o
 obj-$(CONFIG_CALXEDA_XGMAC) += calxedaxgmac.o
 obj-$(CONFIG_CS8900) += cs8900.o
 obj-$(CONFIG_TULIP) += dc2114x.o
-obj-$(CONFIG_DESIGNWARE_ETH) += designware.o
+obj-$(CONFIG_ETH_DESIGNWARE) += designware.o
 obj-$(CONFIG_DRIVER_DM9000) += dm9000x.o
 obj-$(CONFIG_DNET) += dnet.o
 obj-$(CONFIG_E1000) += e1000.o
@@ -51,6 +51,8 @@ obj-$(CONFIG_PCH_GBE) += pch_gbe.o
 obj-$(CONFIG_PCNET) += pcnet.o
 obj-$(CONFIG_RTL8139) += rtl8139.o
 obj-$(CONFIG_RTL8169) += rtl8169.o
+obj-$(CONFIG_ETH_SANDBOX) += sandbox.o
+obj-$(CONFIG_ETH_SANDBOX_RAW) += sandbox-raw.o
 obj-$(CONFIG_SH_ETHER) += sh_eth.o
 obj-$(CONFIG_SMC91111) += smc91111.o
 obj-$(CONFIG_SMC911X) += smc911x.o
@@ -67,4 +69,6 @@ obj-$(CONFIG_XILINX_LL_TEMAC) += xilinx_ll_temac.o xilinx_ll_temac_mdio.o \
 		xilinx_ll_temac_fifo.o xilinx_ll_temac_sdma.o
 obj-$(CONFIG_ZYNQ_GEM) += zynq_gem.o
 obj-$(CONFIG_FSL_MC_ENET) += fsl-mc/
+obj-$(CONFIG_FSL_MC_ENET) += ldpaa_eth/
+obj-$(CONFIG_FSL_MEMAC) += fm/memac_phy.o
 obj-$(CONFIG_VSC9953) += vsc9953.o
diff --git a/drivers/net/altera_tse.c b/drivers/net/altera_tse.c
index de517f8..c4fd6ec 100644
--- a/drivers/net/altera_tse.c
+++ b/drivers/net/altera_tse.c
@@ -303,16 +303,17 @@ static int tse_eth_rx(struct eth_device *dev)
 	    ALT_SGDMA_DESCRIPTOR_STATUS_TERMINATED_BY_EOP_MSK) {
 		debug("got packet\n");
 		packet_length = rx_desc->actual_bytes_transferred;
-		NetReceive(NetRxPackets[0], packet_length);
+		net_process_received_packet(net_rx_packets[0], packet_length);
 
 		/* start descriptor again */
-		flush_dcache_range((unsigned long)(NetRxPackets[0]),
-			(unsigned long)(NetRxPackets[0]) + PKTSIZE_ALIGN);
+		flush_dcache_range((unsigned long)(net_rx_packets[0]),
+				   (unsigned long)(net_rx_packets[0] +
+						   PKTSIZE_ALIGN));
 		alt_sgdma_construct_descriptor_burst(
 			(volatile struct alt_sgdma_descriptor *)&rx_desc[0],
 			(volatile struct alt_sgdma_descriptor *)&rx_desc[1],
 			(unsigned int)0x0,	/* read addr */
-			(unsigned int *)NetRxPackets[0],
+			(unsigned int *)net_rx_packets[0],
 			0x0,	/* length or EOP */
 			0x0,	/* gen eop */
 			0x0,	/* read fixed */
@@ -835,13 +836,13 @@ static int tse_eth_init(struct eth_device *dev, bd_t * bd)
 		0x0	/* channel */
 		);
 	debug("Configuring rx desc\n");
-	flush_dcache_range((unsigned long)(NetRxPackets[0]),
-			(unsigned long)(NetRxPackets[0]) + PKTSIZE_ALIGN);
+	flush_dcache_range((unsigned long)(net_rx_packets[0]),
+			   (unsigned long)(net_rx_packets[0]) + PKTSIZE_ALIGN);
 	alt_sgdma_construct_descriptor_burst(
 		(volatile struct alt_sgdma_descriptor *)&rx_desc[0],
 		(volatile struct alt_sgdma_descriptor *)&rx_desc[1],
 		(unsigned int)0x0,	/* read addr */
-		(unsigned int *)NetRxPackets[0],
+		(unsigned int *)net_rx_packets[0],
 		0x0,	/* length or EOP */
 		0x0,	/* gen eop */
 		0x0,	/* read fixed */
diff --git a/drivers/net/armada100_fec.c b/drivers/net/armada100_fec.c
index a8da6b1..e6a6252 100644
--- a/drivers/net/armada100_fec.c
+++ b/drivers/net/armada100_fec.c
@@ -639,15 +639,16 @@ static int armdfec_recv(struct eth_device *dev)
 	} else {
 		/* !!! call higher layer processing */
 		debug("ARMD100 FEC: (%s) Sending Received packet to"
-			" upper layer (NetReceive)\n", __func__);
+		      " upper layer (net_process_received_packet)\n", __func__);
 
 		/*
 		 * let the upper layer handle the packet, subtract offset
 		 * as two dummy bytes are added in received buffer see
 		 * PORT_CONFIG_EXT register bit TWO_Byte_Stuff_Mode bit.
 		 */
-		NetReceive((p_rxdesc_curr->buf_ptr + RX_BUF_OFFSET),
-			   (int)(p_rxdesc_curr->byte_cnt - RX_BUF_OFFSET));
+		net_process_received_packet(
+			p_rxdesc_curr->buf_ptr + RX_BUF_OFFSET,
+			(int)(p_rxdesc_curr->byte_cnt - RX_BUF_OFFSET));
 	}
 	/*
 	 * free these descriptors and point next in the ring
diff --git a/drivers/net/at91_emac.c b/drivers/net/at91_emac.c
index 64d4c56..d51e098 100644
--- a/drivers/net/at91_emac.c
+++ b/drivers/net/at91_emac.c
@@ -352,7 +352,7 @@ static int at91emac_init(struct eth_device *netdev, bd_t *bd)
 
 	/* Init Ethernet buffers */
 	for (i = 0; i < RBF_FRAMEMAX; i++) {
-		dev->rbfdt[i].addr = (unsigned long) NetRxPackets[i];
+		dev->rbfdt[i].addr = (unsigned long) net_rx_packets[i];
 		dev->rbfdt[i].size = 0;
 	}
 	dev->rbfdt[RBF_FRAMEMAX - 1].addr |= RBF_WRAP;
@@ -420,7 +420,7 @@ static int at91emac_recv(struct eth_device *netdev)
 	rbfp = &dev->rbfdt[dev->rbindex];
 	while (rbfp->addr & RBF_OWNER)	{
 		size = rbfp->size & RBF_SIZE;
-		NetReceive(NetRxPackets[dev->rbindex], size);
+		net_process_received_packet(net_rx_packets[dev->rbindex], size);
 
 		debug_cond(DEBUG_AT91EMAC, "Recv[%ld]: %d bytes @ %lx\n",
 			dev->rbindex, size, rbfp->addr);
diff --git a/drivers/net/ax88180.c b/drivers/net/ax88180.c
index 7f0cfe5..ded9e06 100644
--- a/drivers/net/ax88180.c
+++ b/drivers/net/ax88180.c
@@ -192,9 +192,9 @@ static void ax88180_rx_handler (struct eth_device *dev)
 	unsigned short rxcurt_ptr, rxbound_ptr, next_ptr;
 	int i;
 #if defined (CONFIG_DRIVER_AX88180_16BIT)
-	unsigned short *rxdata = (unsigned short *)NetRxPackets[0];
+	unsigned short *rxdata = (unsigned short *)net_rx_packets[0];
 #else
-	unsigned long *rxdata = (unsigned long *)NetRxPackets[0];
+	unsigned long *rxdata = (unsigned long *)net_rx_packets[0];
 #endif
 	unsigned short count;
 
@@ -237,7 +237,7 @@ static void ax88180_rx_handler (struct eth_device *dev)
 		OUTW (dev, RX_STOP_READ, RXINDICATOR);
 
 		/* Pass the packet up to the protocol layers. */
-		NetReceive (NetRxPackets[0], data_size);
+		net_process_received_packet(net_rx_packets[0], data_size);
 
 		OUTW (dev, rxbound_ptr, RXBOUND);
 
diff --git a/drivers/net/bcm-sf2-eth.c b/drivers/net/bcm-sf2-eth.c
index 5252d49..51d5146 100644
--- a/drivers/net/bcm-sf2-eth.c
+++ b/drivers/net/bcm-sf2-eth.c
@@ -103,7 +103,7 @@ static int bcm_sf2_eth_send(struct eth_device *dev, void *packet, int length)
 static int bcm_sf2_eth_receive(struct eth_device *dev)
 {
 	struct eth_dma *dma = &(((struct eth_info *)(dev->priv))->dma);
-	uint8_t *buf = (uint8_t *)NetRxPackets[0];
+	uint8_t *buf = (uint8_t *)net_rx_packets[0];
 	int rcvlen;
 	int rc = 0;
 	int i = 0;
@@ -124,11 +124,11 @@ static int bcm_sf2_eth_receive(struct eth_device *dev)
 			debug("recieved\n");
 
 			/* Forward received packet to uboot network handler */
-			NetReceive(buf, rcvlen);
+			net_process_received_packet(buf, rcvlen);
 
 			if (++i >= PKTBUFSRX)
 				i = 0;
-			buf = NetRxPackets[i];
+			buf = net_rx_packets[i];
 		}
 	}
 
diff --git a/drivers/net/bfin_mac.c b/drivers/net/bfin_mac.c
index 0c2d2ef..61cb1b0 100644
--- a/drivers/net/bfin_mac.c
+++ b/drivers/net/bfin_mac.c
@@ -189,8 +189,8 @@ static int bfin_EMAC_recv(struct eth_device *dev)
 
 		debug("%s: len = %d\n", __func__, length - 4);
 
-		NetRxPackets[rxIdx] = rxbuf[rxIdx]->FrmData->Dest;
-		NetReceive(NetRxPackets[rxIdx], length - 4);
+		net_rx_packets[rxIdx] = rxbuf[rxIdx]->FrmData->Dest;
+		net_process_received_packet(net_rx_packets[rxIdx], length - 4);
 		bfin_write_DMA1_IRQ_STATUS(DMA_DONE | DMA_ERR);
 		rxbuf[rxIdx]->StatusWord = 0x00000000;
 		if ((rxIdx + 1) >= PKTBUFSRX)
diff --git a/drivers/net/calxedaxgmac.c b/drivers/net/calxedaxgmac.c
index ff94865..c02b397 100644
--- a/drivers/net/calxedaxgmac.c
+++ b/drivers/net/calxedaxgmac.c
@@ -466,7 +466,7 @@ static int xgmac_rx(struct eth_device *dev)
 
 	length = desc_get_rx_frame_len(rxdesc);
 
-	NetReceive(desc_get_buf_addr(rxdesc), length);
+	net_process_received_packet(desc_get_buf_addr(rxdesc), length);
 
 	/* set descriptor back to owned by XGMAC */
 	desc_set_rx_owner(rxdesc);
diff --git a/drivers/net/cpsw.c b/drivers/net/cpsw.c
index 52f8da6..fb4d621 100644
--- a/drivers/net/cpsw.c
+++ b/drivers/net/cpsw.c
@@ -289,7 +289,7 @@ static inline void cpsw_ale_get_addr(u32 *ale_entry, u8 *addr)
 		addr[i] = cpsw_ale_get_field(ale_entry, 40 - 8*i, 8);
 }
 
-static inline void cpsw_ale_set_addr(u32 *ale_entry, u8 *addr)
+static inline void cpsw_ale_set_addr(u32 *ale_entry, const u8 *addr)
 {
 	int i;
 
@@ -321,7 +321,7 @@ static int cpsw_ale_write(struct cpsw_priv *priv, int idx, u32 *ale_entry)
 	return idx;
 }
 
-static int cpsw_ale_match_addr(struct cpsw_priv *priv, u8* addr)
+static int cpsw_ale_match_addr(struct cpsw_priv *priv, const u8 *addr)
 {
 	u32 ale_entry[ALE_ENTRY_WORDS];
 	int type, idx;
@@ -374,7 +374,7 @@ static int cpsw_ale_find_ageable(struct cpsw_priv *priv)
 	return -ENOENT;
 }
 
-static int cpsw_ale_add_ucast(struct cpsw_priv *priv, u8 *addr,
+static int cpsw_ale_add_ucast(struct cpsw_priv *priv, const u8 *addr,
 			      int port, int flags)
 {
 	u32 ale_entry[ALE_ENTRY_WORDS] = {0, 0, 0};
@@ -399,7 +399,8 @@ static int cpsw_ale_add_ucast(struct cpsw_priv *priv, u8 *addr,
 	return 0;
 }
 
-static int cpsw_ale_add_mcast(struct cpsw_priv *priv, u8 *addr, int port_mask)
+static int cpsw_ale_add_mcast(struct cpsw_priv *priv, const u8 *addr,
+			      int port_mask)
 {
 	u32 ale_entry[ALE_ENTRY_WORDS] = {0, 0, 0};
 	int idx, mask;
@@ -644,7 +645,7 @@ static void cpsw_slave_init(struct cpsw_slave *slave, struct cpsw_priv *priv)
 	slave_port = cpsw_get_slave_port(priv, slave->slave_num);
 	cpsw_ale_port_state(priv, slave_port, ALE_PORT_STATE_FORWARD);
 
-	cpsw_ale_add_mcast(priv, NetBcastAddr, 1 << slave_port);
+	cpsw_ale_add_mcast(priv, net_bcast_ethaddr, 1 << slave_port);
 
 	priv->phy_mask |= 1 << slave->data->phy_addr;
 }
@@ -773,7 +774,7 @@ static int cpsw_init(struct eth_device *dev, bd_t *bis)
 
 	cpsw_ale_add_ucast(priv, priv->dev->enetaddr, priv->host_port,
 			   ALE_SECURE);
-	cpsw_ale_add_mcast(priv, NetBcastAddr, 1 << priv->host_port);
+	cpsw_ale_add_mcast(priv, net_bcast_ethaddr, 1 << priv->host_port);
 
 	for_active_slave(slave, priv)
 		cpsw_slave_init(slave, priv);
@@ -845,7 +846,7 @@ static int cpsw_init(struct eth_device *dev, bd_t *bis)
 
 	/* submit rx descs */
 	for (i = 0; i < PKTBUFSRX; i++) {
-		ret = cpdma_submit(priv, &priv->rx_chan, NetRxPackets[i],
+		ret = cpdma_submit(priv, &priv->rx_chan, net_rx_packets[i],
 				   PKTSIZE);
 		if (ret < 0) {
 			printf("error %d submitting rx desc\n", ret);
@@ -904,7 +905,7 @@ static int cpsw_recv(struct eth_device *dev)
 	while (cpdma_process(priv, &priv->rx_chan, &buffer, &len) >= 0) {
 		invalidate_dcache_range((unsigned long)buffer,
 					(unsigned long)buffer + PKTSIZE_ALIGN);
-		NetReceive(buffer, len);
+		net_process_received_packet(buffer, len);
 		cpdma_submit(priv, &priv->rx_chan, buffer, PKTSIZE);
 	}
 
diff --git a/drivers/net/cs8900.c b/drivers/net/cs8900.c
index 84963c1..0713464 100644
--- a/drivers/net/cs8900.c
+++ b/drivers/net/cs8900.c
@@ -188,14 +188,13 @@ static int cs8900_recv(struct eth_device *dev)
 
 	if (rxlen > PKTSIZE_ALIGN + PKTALIGN)
 		debug("packet too big!\n");
-	for (addr = (u16 *) NetRxPackets[0], i = rxlen >> 1; i > 0;
-		 i--)
+	for (addr = (u16 *)net_rx_packets[0], i = rxlen >> 1; i > 0; i--)
 		*addr++ = REG_READ(&priv->regs->rtdata);
 	if (rxlen & 1)
 		*addr++ = REG_READ(&priv->regs->rtdata);
 
 	/* Pass the packet up to the protocol layers. */
-	NetReceive (NetRxPackets[0], rxlen);
+	net_process_received_packet(net_rx_packets[0], rxlen);
 	return rxlen;
 }
 
diff --git a/drivers/net/davinci_emac.c b/drivers/net/davinci_emac.c
index 08bc1af..427ad3e 100644
--- a/drivers/net/davinci_emac.c
+++ b/drivers/net/davinci_emac.c
@@ -700,8 +700,9 @@ static int davinci_eth_rcv_packet (struct eth_device *dev)
 			unsigned long tmp = (unsigned long)rx_curr_desc->buffer;
 
 			invalidate_dcache_range(tmp, tmp + EMAC_RXBUF_SIZE);
-			NetReceive (rx_curr_desc->buffer,
-				    (rx_curr_desc->buff_off_len & 0xffff));
+			net_process_received_packet(
+				rx_curr_desc->buffer,
+				rx_curr_desc->buff_off_len & 0xffff);
 			ret = rx_curr_desc->buff_off_len & 0xffff;
 		}
 
diff --git a/drivers/net/dc2114x.c b/drivers/net/dc2114x.c
index 799839c..8245cf5 100644
--- a/drivers/net/dc2114x.c
+++ b/drivers/net/dc2114x.c
@@ -333,9 +333,11 @@ static int dc21x4x_init(struct eth_device* dev, bd_t* bis)
 	for (i = 0; i < NUM_RX_DESC; i++) {
 		rx_ring[i].status = cpu_to_le32(R_OWN);
 		rx_ring[i].des1 = cpu_to_le32(RX_BUFF_SZ);
-		rx_ring[i].buf = cpu_to_le32(phys_to_bus((u32) NetRxPackets[i]));
+		rx_ring[i].buf = cpu_to_le32(
+			phys_to_bus((u32)net_rx_packets[i]));
 #ifdef CONFIG_TULIP_FIX_DAVICOM
-		rx_ring[i].next = cpu_to_le32(phys_to_bus((u32) &rx_ring[(i+1) % NUM_RX_DESC]));
+		rx_ring[i].next = cpu_to_le32(
+			phys_to_bus((u32)&rx_ring[(i + 1) % NUM_RX_DESC]));
 #else
 		rx_ring[i].next = 0;
 #endif
@@ -448,7 +450,8 @@ static int dc21x4x_recv(struct eth_device* dev)
 				/* Pass the packet up to the protocol
 				 * layers.
 				 */
-				NetReceive(NetRxPackets[rx_new], length - 4);
+				net_process_received_packet(
+					net_rx_packets[rx_new], length - 4);
 			}
 
 			/* Change buffer ownership for this frame, back
diff --git a/drivers/net/designware.c b/drivers/net/designware.c
index cc01604..07281a6 100644
--- a/drivers/net/designware.c
+++ b/drivers/net/designware.c
@@ -6,10 +6,12 @@
  */
 
 /*
- * Designware ethernet IP driver for u-boot
+ * Designware ethernet IP driver for U-Boot
  */
 
 #include <common.h>
+#include <dm.h>
+#include <errno.h>
 #include <miiphy.h>
 #include <malloc.h>
 #include <linux/compiler.h>
@@ -17,6 +19,8 @@
 #include <asm/io.h>
 #include "designware.h"
 
+DECLARE_GLOBAL_DATA_PTR;
+
 #if !defined(CONFIG_PHYLIB)
 # error "DesignWare Ether MAC requires PHYLIB - missing CONFIG_PHYLIB"
 #endif
@@ -40,7 +44,7 @@ static int dw_mdio_read(struct mii_dev *bus, int addr, int devad, int reg)
 		udelay(10);
 	};
 
-	return -1;
+	return -ETIMEDOUT;
 }
 
 static int dw_mdio_write(struct mii_dev *bus, int addr, int devad, int reg,
@@ -49,7 +53,7 @@ static int dw_mdio_write(struct mii_dev *bus, int addr, int devad, int reg,
 	struct eth_mac_regs *mac_p = bus->priv;
 	ulong start;
 	u16 miiaddr;
-	int ret = -1, timeout = CONFIG_MDIO_TIMEOUT;
+	int ret = -ETIMEDOUT, timeout = CONFIG_MDIO_TIMEOUT;
 
 	writel(val, &mac_p->miidata);
 	miiaddr = ((addr << MIIADDRSHIFT) & MII_ADDRMSK) |
@@ -69,27 +73,26 @@ static int dw_mdio_write(struct mii_dev *bus, int addr, int devad, int reg,
 	return ret;
 }
 
-static int dw_mdio_init(char *name, struct eth_mac_regs *mac_regs_p)
+static int dw_mdio_init(const char *name, struct eth_mac_regs *mac_regs_p)
 {
 	struct mii_dev *bus = mdio_alloc();
 
 	if (!bus) {
 		printf("Failed to allocate MDIO bus\n");
-		return -1;
+		return -ENOMEM;
 	}
 
 	bus->read = dw_mdio_read;
 	bus->write = dw_mdio_write;
-	sprintf(bus->name, name);
+	snprintf(bus->name, sizeof(bus->name), name);
 
 	bus->priv = (void *)mac_regs_p;
 
 	return mdio_register(bus);
 }
 
-static void tx_descs_init(struct eth_device *dev)
+static void tx_descs_init(struct dw_eth_dev *priv)
 {
-	struct dw_eth_dev *priv = dev->priv;
 	struct eth_dma_regs *dma_p = priv->dma_regs_p;
 	struct dmamacdescr *desc_table_p = &priv->tx_mac_descrtable[0];
 	char *txbuffs = &priv->txbuffs[0];
@@ -128,9 +131,8 @@ static void tx_descs_init(struct eth_device *dev)
 	priv->tx_currdescnum = 0;
 }
 
-static void rx_descs_init(struct eth_device *dev)
+static void rx_descs_init(struct dw_eth_dev *priv)
 {
-	struct dw_eth_dev *priv = dev->priv;
 	struct eth_dma_regs *dma_p = priv->dma_regs_p;
 	struct dmamacdescr *desc_table_p = &priv->rx_mac_descrtable[0];
 	char *rxbuffs = &priv->rxbuffs[0];
@@ -170,12 +172,10 @@ static void rx_descs_init(struct eth_device *dev)
 	priv->rx_currdescnum = 0;
 }
 
-static int dw_write_hwaddr(struct eth_device *dev)
+static int _dw_write_hwaddr(struct dw_eth_dev *priv, u8 *mac_id)
 {
-	struct dw_eth_dev *priv = dev->priv;
 	struct eth_mac_regs *mac_p = priv->mac_regs_p;
 	u32 macid_lo, macid_hi;
-	u8 *mac_id = &dev->enetaddr[0];
 
 	macid_lo = mac_id[0] + (mac_id[1] << 8) + (mac_id[2] << 16) +
 		   (mac_id[3] << 24);
@@ -213,9 +213,8 @@ static void dw_adjust_link(struct eth_mac_regs *mac_p,
 	       (phydev->port == PORT_FIBRE) ? ", fiber mode" : "");
 }
 
-static void dw_eth_halt(struct eth_device *dev)
+static void _dw_eth_halt(struct dw_eth_dev *priv)
 {
-	struct dw_eth_dev *priv = dev->priv;
 	struct eth_mac_regs *mac_p = priv->mac_regs_p;
 	struct eth_dma_regs *dma_p = priv->dma_regs_p;
 
@@ -225,12 +224,12 @@ static void dw_eth_halt(struct eth_device *dev)
 	phy_shutdown(priv->phydev);
 }
 
-static int dw_eth_init(struct eth_device *dev, bd_t *bis)
+static int _dw_eth_init(struct dw_eth_dev *priv, u8 *enetaddr)
 {
-	struct dw_eth_dev *priv = dev->priv;
 	struct eth_mac_regs *mac_p = priv->mac_regs_p;
 	struct eth_dma_regs *dma_p = priv->dma_regs_p;
 	unsigned int start;
+	int ret;
 
 	writel(readl(&dma_p->busmode) | DMAMAC_SRST, &dma_p->busmode);
 
@@ -238,7 +237,7 @@ static int dw_eth_init(struct eth_device *dev, bd_t *bis)
 	while (readl(&dma_p->busmode) & DMAMAC_SRST) {
 		if (get_timer(start) >= CONFIG_MACRESET_TIMEOUT) {
 			printf("DMA reset timeout\n");
-			return -1;
+			return -ETIMEDOUT;
 		}
 
 		mdelay(100);
@@ -246,10 +245,10 @@ static int dw_eth_init(struct eth_device *dev, bd_t *bis)
 
 	/* Soft reset above clears HW address registers.
 	 * So we have to set it here once again */
-	dw_write_hwaddr(dev);
+	_dw_write_hwaddr(priv, enetaddr);
 
-	rx_descs_init(dev);
-	tx_descs_init(dev);
+	rx_descs_init(priv);
+	tx_descs_init(priv);
 
 	writel(FIXEDBURST | PRIORXTX_41 | DMA_PBL, &dma_p->busmode);
 
@@ -268,25 +267,25 @@ static int dw_eth_init(struct eth_device *dev, bd_t *bis)
 #endif
 
 	/* Start up the PHY */
-	if (phy_startup(priv->phydev)) {
+	ret = phy_startup(priv->phydev);
+	if (ret) {
 		printf("Could not initialize PHY %s\n",
 		       priv->phydev->dev->name);
-		return -1;
+		return ret;
 	}
 
 	dw_adjust_link(mac_p, priv->phydev);
 
 	if (!priv->phydev->link)
-		return -1;
+		return -EIO;
 
 	writel(readl(&mac_p->conf) | RXENABLE | TXENABLE, &mac_p->conf);
 
 	return 0;
 }
 
-static int dw_eth_send(struct eth_device *dev, void *packet, int length)
+static int _dw_eth_send(struct dw_eth_dev *priv, void *packet, int length)
 {
-	struct dw_eth_dev *priv = dev->priv;
 	struct eth_dma_regs *dma_p = priv->dma_regs_p;
 	u32 desc_num = priv->tx_currdescnum;
 	struct dmamacdescr *desc_p = &priv->tx_mac_descrtable[desc_num];
@@ -309,7 +308,7 @@ static int dw_eth_send(struct eth_device *dev, void *packet, int length)
 	/* Check if the descriptor is owned by CPU */
 	if (desc_p->txrx_status & DESC_TXSTS_OWNBYDMA) {
 		printf("CPU not owner of tx frame\n");
-		return -1;
+		return -EPERM;
 	}
 
 	memcpy(desc_p->dmamac_addr, packet, length);
@@ -347,12 +346,11 @@ static int dw_eth_send(struct eth_device *dev, void *packet, int length)
 	return 0;
 }
 
-static int dw_eth_recv(struct eth_device *dev)
+static int _dw_eth_recv(struct dw_eth_dev *priv, uchar **packetp)
 {
-	struct dw_eth_dev *priv = dev->priv;
 	u32 status, desc_num = priv->rx_currdescnum;
 	struct dmamacdescr *desc_p = &priv->rx_mac_descrtable[desc_num];
-	int length = 0;
+	int length = -EAGAIN;
 	uint32_t desc_start = (uint32_t)desc_p;
 	uint32_t desc_end = desc_start +
 		roundup(sizeof(*desc_p), ARCH_DMA_MINALIGN);
@@ -373,31 +371,39 @@ static int dw_eth_recv(struct eth_device *dev)
 		/* Invalidate received data */
 		data_end = data_start + roundup(length, ARCH_DMA_MINALIGN);
 		invalidate_dcache_range(data_start, data_end);
+		*packetp = desc_p->dmamac_addr;
+	}
 
-		NetReceive(desc_p->dmamac_addr, length);
+	return length;
+}
 
-		/*
-		 * Make the current descriptor valid again and go to
-		 * the next one
-		 */
-		desc_p->txrx_status |= DESC_RXSTS_OWNBYDMA;
+static int _dw_free_pkt(struct dw_eth_dev *priv)
+{
+	u32 desc_num = priv->rx_currdescnum;
+	struct dmamacdescr *desc_p = &priv->rx_mac_descrtable[desc_num];
+	uint32_t desc_start = (uint32_t)desc_p;
+	uint32_t desc_end = desc_start +
+		roundup(sizeof(*desc_p), ARCH_DMA_MINALIGN);
 
-		/* Flush only status field - others weren't changed */
-		flush_dcache_range(desc_start, desc_end);
+	/*
+	 * Make the current descriptor valid again and go to
+	 * the next one
+	 */
+	desc_p->txrx_status |= DESC_RXSTS_OWNBYDMA;
 
-		/* Test the wrap-around condition. */
-		if (++desc_num >= CONFIG_RX_DESCR_NUM)
-			desc_num = 0;
-	}
+	/* Flush only status field - others weren't changed */
+	flush_dcache_range(desc_start, desc_end);
 
+	/* Test the wrap-around condition. */
+	if (++desc_num >= CONFIG_RX_DESCR_NUM)
+		desc_num = 0;
 	priv->rx_currdescnum = desc_num;
 
-	return length;
+	return 0;
 }
 
-static int dw_phy_init(struct eth_device *dev)
+static int dw_phy_init(struct dw_eth_dev *priv, void *dev)
 {
-	struct dw_eth_dev *priv = dev->priv;
 	struct phy_device *phydev;
 	int mask = 0xffffffff;
 
@@ -407,7 +413,7 @@ static int dw_phy_init(struct eth_device *dev)
 
 	phydev = phy_find_by_mask(priv->bus, mask, priv->interface);
 	if (!phydev)
-		return -1;
+		return -ENODEV;
 
 	phy_connect_dev(phydev, dev);
 
@@ -417,7 +423,43 @@ static int dw_phy_init(struct eth_device *dev)
 	priv->phydev = phydev;
 	phy_config(phydev);
 
-	return 1;
+	return 0;
+}
+
+#ifndef CONFIG_DM_ETH
+static int dw_eth_init(struct eth_device *dev, bd_t *bis)
+{
+	return _dw_eth_init(dev->priv, dev->enetaddr);
+}
+
+static int dw_eth_send(struct eth_device *dev, void *packet, int length)
+{
+	return _dw_eth_send(dev->priv, packet, length);
+}
+
+static int dw_eth_recv(struct eth_device *dev)
+{
+	uchar *packet;
+	int length;
+
+	length = _dw_eth_recv(dev->priv, &packet);
+	if (length == -EAGAIN)
+		return 0;
+	net_process_received_packet(packet, length);
+
+	_dw_free_pkt(dev->priv);
+
+	return 0;
+}
+
+static void dw_eth_halt(struct eth_device *dev)
+{
+	return _dw_eth_halt(dev->priv);
+}
+
+static int dw_write_hwaddr(struct eth_device *dev)
+{
+	return _dw_write_hwaddr(dev->priv, dev->enetaddr);
 }
 
 int designware_initialize(ulong base_addr, u32 interface)
@@ -465,5 +507,117 @@ int designware_initialize(ulong base_addr, u32 interface)
 	dw_mdio_init(dev->name, priv->mac_regs_p);
 	priv->bus = miiphy_get_dev_by_name(dev->name);
 
-	return dw_phy_init(dev);
+	return dw_phy_init(priv, dev);
+}
+#endif
+
+#ifdef CONFIG_DM_ETH
+static int designware_eth_start(struct udevice *dev)
+{
+	struct eth_pdata *pdata = dev_get_platdata(dev);
+
+	return _dw_eth_init(dev->priv, pdata->enetaddr);
+}
+
+static int designware_eth_send(struct udevice *dev, void *packet, int length)
+{
+	struct dw_eth_dev *priv = dev_get_priv(dev);
+
+	return _dw_eth_send(priv, packet, length);
+}
+
+static int designware_eth_recv(struct udevice *dev, uchar **packetp)
+{
+	struct dw_eth_dev *priv = dev_get_priv(dev);
+
+	return _dw_eth_recv(priv, packetp);
+}
+
+static int designware_eth_free_pkt(struct udevice *dev, uchar *packet,
+				   int length)
+{
+	struct dw_eth_dev *priv = dev_get_priv(dev);
+
+	return _dw_free_pkt(priv);
+}
+
+static void designware_eth_stop(struct udevice *dev)
+{
+	struct dw_eth_dev *priv = dev_get_priv(dev);
+
+	return _dw_eth_halt(priv);
+}
+
+static int designware_eth_write_hwaddr(struct udevice *dev)
+{
+	struct eth_pdata *pdata = dev_get_platdata(dev);
+	struct dw_eth_dev *priv = dev_get_priv(dev);
+
+	return _dw_write_hwaddr(priv, pdata->enetaddr);
+}
+
+static int designware_eth_probe(struct udevice *dev)
+{
+	struct eth_pdata *pdata = dev_get_platdata(dev);
+	struct dw_eth_dev *priv = dev_get_priv(dev);
+	int ret;
+
+	debug("%s, iobase=%lx, priv=%p\n", __func__, pdata->iobase, priv);
+	priv->mac_regs_p = (struct eth_mac_regs *)pdata->iobase;
+	priv->dma_regs_p = (struct eth_dma_regs *)(pdata->iobase +
+			DW_DMA_BASE_OFFSET);
+	priv->interface = pdata->phy_interface;
+
+	dw_mdio_init(dev->name, priv->mac_regs_p);
+	priv->bus = miiphy_get_dev_by_name(dev->name);
+
+	ret = dw_phy_init(priv, dev);
+	debug("%s, ret=%d\n", __func__, ret);
+
+	return ret;
 }
+
+static const struct eth_ops designware_eth_ops = {
+	.start			= designware_eth_start,
+	.send			= designware_eth_send,
+	.recv			= designware_eth_recv,
+	.free_pkt		= designware_eth_free_pkt,
+	.stop			= designware_eth_stop,
+	.write_hwaddr		= designware_eth_write_hwaddr,
+};
+
+static int designware_eth_ofdata_to_platdata(struct udevice *dev)
+{
+	struct eth_pdata *pdata = dev_get_platdata(dev);
+	const char *phy_mode;
+
+	pdata->iobase = dev_get_addr(dev);
+	pdata->phy_interface = -1;
+	phy_mode = fdt_getprop(gd->fdt_blob, dev->of_offset, "phy-mode", NULL);
+	if (phy_mode)
+		pdata->phy_interface = phy_get_interface_by_name(phy_mode);
+	if (pdata->phy_interface == -1) {
+		debug("%s: Invalid PHY interface '%s'\n", __func__, phy_mode);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static const struct udevice_id designware_eth_ids[] = {
+	{ .compatible = "allwinner,sun7i-a20-gmac" },
+	{ }
+};
+
+U_BOOT_DRIVER(eth_sandbox) = {
+	.name	= "eth_designware",
+	.id	= UCLASS_ETH,
+	.of_match = designware_eth_ids,
+	.ofdata_to_platdata = designware_eth_ofdata_to_platdata,
+	.probe	= designware_eth_probe,
+	.ops	= &designware_eth_ops,
+	.priv_auto_alloc_size = sizeof(struct dw_eth_dev),
+	.platdata_auto_alloc_size = sizeof(struct eth_pdata),
+	.flags = DM_FLAG_ALLOC_PRIV_DMA,
+};
+#endif
diff --git a/drivers/net/designware.h b/drivers/net/designware.h
index 49d900c..4b9ec39 100644
--- a/drivers/net/designware.h
+++ b/drivers/net/designware.h
@@ -228,8 +228,9 @@ struct dw_eth_dev {
 
 	struct eth_mac_regs *mac_regs_p;
 	struct eth_dma_regs *dma_regs_p;
-
+#ifndef CONFIG_DM_ETH
 	struct eth_device *dev;
+#endif
 	struct phy_device *phydev;
 	struct mii_dev *bus;
 };
diff --git a/drivers/net/dm9000x.c b/drivers/net/dm9000x.c
index 4de9d41..ccd2131 100644
--- a/drivers/net/dm9000x.c
+++ b/drivers/net/dm9000x.c
@@ -342,10 +342,10 @@ static int dm9000_init(struct eth_device *dev, bd_t *bd)
 	DM9000_iow(DM9000_ISR, ISR_ROOS | ISR_ROS | ISR_PTS | ISR_PRS);
 
 	printf("MAC: %pM\n", dev->enetaddr);
-	if (!is_valid_ether_addr(dev->enetaddr)) {
+	if (!is_valid_ethaddr(dev->enetaddr)) {
 #ifdef CONFIG_RANDOM_MACADDR
 		printf("Bad MAC address (uninitialized EEPROM?), randomizing\n");
-		eth_random_addr(dev->enetaddr);
+		net_random_ethaddr(dev->enetaddr);
 		printf("MAC: %pM\n", dev->enetaddr);
 #else
 		printf("WARNING: Bad MAC address (uninitialized EEPROM?)\n");
@@ -464,7 +464,8 @@ static void dm9000_halt(struct eth_device *netdev)
 */
 static int dm9000_rx(struct eth_device *netdev)
 {
-	u8 rxbyte, *rdptr = (u8 *) NetRxPackets[0];
+	u8 rxbyte;
+	u8 *rdptr = (u8 *)net_rx_packets[0];
 	u16 RxStatus, RxLen = 0;
 	struct board_info *db = &dm9000_info;
 
@@ -525,7 +526,7 @@ static int dm9000_rx(struct eth_device *netdev)
 			DM9000_DMP_PACKET(__func__ , rdptr, RxLen);
 
 			DM9000_DBG("passing packet to upper layer\n");
-			NetReceive(NetRxPackets[0], RxLen);
+			net_process_received_packet(net_rx_packets[0], RxLen);
 		}
 	}
 	return 0;
diff --git a/drivers/net/dnet.c b/drivers/net/dnet.c
index 944a0c0..933d1fc 100644
--- a/drivers/net/dnet.c
+++ b/drivers/net/dnet.c
@@ -188,12 +188,13 @@ static int dnet_recv(struct eth_device *netdev)
 	if (cmd_word & 0xDF180000)
 		printf("%s packet receive error %x\n", __func__, cmd_word);
 
-	data_ptr = (unsigned int *) NetRxPackets[0];
+	data_ptr = (unsigned int *)net_rx_packets[0];
 
 	for (i = 0; i < (pkt_len + 3) >> 2; i++)
 		*data_ptr++ = readl(&dnet->regs->RX_DATA_FIFO);
 
-	NetReceive(NetRxPackets[0], pkt_len + 5); /* ok + 5 ?? */
+	/* ok + 5 ?? */
+	net_process_received_packet(net_rx_packets[0], pkt_len + 5);
 
 	return 0;
 }
diff --git a/drivers/net/e1000.c b/drivers/net/e1000.c
index cd44222..96e6bb0 100644
--- a/drivers/net/e1000.c
+++ b/drivers/net/e1000.c
@@ -1197,7 +1197,7 @@ e1000_read_mac_addr(struct eth_device *nic)
 		nic->enetaddr[5] ^= 1;
 
 #ifdef CONFIG_E1000_FALLBACK_MAC
-	if (!is_valid_ether_addr(nic->enetaddr)) {
+	if (!is_valid_ethaddr(nic->enetaddr)) {
 		unsigned char fb_mac[NODE_ADDRESS_SIZE] = CONFIG_E1000_FALLBACK_MAC;
 
 		memcpy (nic->enetaddr, fb_mac, NODE_ADDRESS_SIZE);
@@ -2174,7 +2174,7 @@ e1000_copper_link_preconfig(struct e1000_hw *hw)
 		DEBUGOUT("Error, did not detect valid phy.\n");
 		return ret_val;
 	}
-	DEBUGOUT("Phy ID = %x \n", hw->phy_id);
+	DEBUGOUT("Phy ID = %x\n", hw->phy_id);
 
 	/* Set PHY to class A mode (if necessary) */
 	ret_val = e1000_set_phy_mode(hw);
@@ -3485,11 +3485,11 @@ e1000_config_fc_after_link_up(struct e1000_hw *hw)
 		 * some "sticky" (latched) bits.
 		 */
 		if (e1000_read_phy_reg(hw, PHY_STATUS, &mii_status_reg) < 0) {
-			DEBUGOUT("PHY Read Error \n");
+			DEBUGOUT("PHY Read Error\n");
 			return -E1000_ERR_PHY;
 		}
 		if (e1000_read_phy_reg(hw, PHY_STATUS, &mii_status_reg) < 0) {
-			DEBUGOUT("PHY Read Error \n");
+			DEBUGOUT("PHY Read Error\n");
 			return -E1000_ERR_PHY;
 		}
 
@@ -5152,13 +5152,13 @@ e1000_poll(struct eth_device *nic)
 
 	if (!(le32_to_cpu(rd->status)) & E1000_RXD_STAT_DD)
 		return 0;
-	/*DEBUGOUT("recv: packet len=%d \n", rd->length); */
+	/* DEBUGOUT("recv: packet len=%d\n", rd->length); */
 	/* Packet received, make sure the data are re-loaded from RAM. */
 	len = le32_to_cpu(rd->length);
 	invalidate_dcache_range((unsigned long)packet,
 				(unsigned long)packet +
 				roundup(len, ARCH_DMA_MINALIGN));
-	NetReceive((uchar *)packet, len);
+	net_process_received_packet((uchar *)packet, len);
 	fill_rx(hw);
 	return 1;
 }
diff --git a/drivers/net/eepro100.c b/drivers/net/eepro100.c
index a23a585..f2cd32c 100644
--- a/drivers/net/eepro100.c
+++ b/drivers/net/eepro100.c
@@ -674,7 +674,8 @@ static int eepro100_recv (struct eth_device *dev)
 			/* Pass the packet up to the protocol
 			 * layers.
 			 */
-			NetReceive((u8 *)rx_ring[rx_next].data, length);
+			net_process_received_packet((u8 *)rx_ring[rx_next].data,
+						    length);
 		} else {
 			/* There was an error.
 			 */
diff --git a/drivers/net/enc28j60.c b/drivers/net/enc28j60.c
index ec33764..59ea11c 100644
--- a/drivers/net/enc28j60.c
+++ b/drivers/net/enc28j60.c
@@ -21,8 +21,8 @@
  * enc_miiphy_read(), enc_miiphy_write(), enc_write_hwaddr(),
  * enc_init(), enc_recv(), enc_send(), enc_halt()
  * ALL other functions assume that the bus has already been claimed!
- * Since NetReceive() might call enc_send() in return, the bus must be
- * released, NetReceive() called and claimed again.
+ * Since net_process_received_packet() might call enc_send() in return, the bus
+ * must be released, net_process_received_packet() called and claimed again.
  */
 
 /*
@@ -415,7 +415,7 @@ static void enc_reset_rx_call(enc_dev_t *enc)
  */
 static void enc_receive(enc_dev_t *enc)
 {
-	u8 *packet = (u8 *)NetRxPackets[0];
+	u8 *packet = (u8 *)net_rx_packets[0];
 	u16 pkt_len;
 	u16 copy_len;
 	u16 status;
@@ -468,11 +468,12 @@ static void enc_receive(enc_dev_t *enc)
 			continue;
 		}
 		/*
-		 * Because NetReceive() might call enc_send(), we need to
-		 * release the SPI bus, call NetReceive(), reclaim the bus
+		 * Because net_process_received_packet() might call enc_send(),
+		 * we need to release the SPI bus, call
+		 * net_process_received_packet(), reclaim the bus.
 		 */
 		enc_release_bus(enc);
-		NetReceive(packet, pkt_len);
+		net_process_received_packet(packet, pkt_len);
 		if (enc_claim_bus(enc))
 			return;
 		(void)enc_r8(enc, CTL_REG_EIR);
diff --git a/drivers/net/ep93xx_eth.c b/drivers/net/ep93xx_eth.c
index 1c09f10..a3721c5 100644
--- a/drivers/net/ep93xx_eth.c
+++ b/drivers/net/ep93xx_eth.c
@@ -53,7 +53,7 @@ static void dump_dev(struct eth_device *dev)
 	printf("  rx_sq.end	     %p\n", priv->rx_sq.end);
 
 	for (i = 0; i < NUMRXDESC; i++)
-		printf("  rx_buffer[%2.d]      %p\n", i, NetRxPackets[i]);
+		printf("  rx_buffer[%2.d]      %p\n", i, net_rx_packets[i]);
 
 	printf("  tx_dq.base	     %p\n", priv->tx_dq.base);
 	printf("  tx_dq.current	     %p\n", priv->tx_dq.current);
@@ -237,7 +237,7 @@ static int ep93xx_eth_open(struct eth_device *dev, bd_t *bd)
 	 */
 	for (i = 0; i < NUMRXDESC; i++) {
 		/* set buffer address */
-		(priv->rx_dq.base + i)->word1 = (uint32_t)NetRxPackets[i];
+		(priv->rx_dq.base + i)->word1 = (uint32_t)net_rx_packets[i];
 
 		/* set buffer length, clear buffer index and NSOF */
 		(priv->rx_dq.base + i)->word2 = PKTSIZE_ALIGN;
@@ -310,15 +310,16 @@ static int ep93xx_eth_rcv_packet(struct eth_device *dev)
 			/*
 			 * We have a good frame. Extract the frame's length
 			 * from the current rx_status_queue entry, and copy
-			 * the frame's data into NetRxPackets[] of the
+			 * the frame's data into net_rx_packets[] of the
 			 * protocol stack. We track the total number of
 			 * bytes in the frame (nbytes_frame) which will be
 			 * used when we pass the data off to the protocol
-			 * layer via NetReceive().
+			 * layer via net_process_received_packet().
 			 */
 			len = RX_STATUS_FRAME_LEN(priv->rx_sq.current);
 
-			NetReceive((uchar *)priv->rx_dq.current->word1,	len);
+			net_process_received_packet(
+				(uchar *)priv->rx_dq.current->word1, len);
 
 			debug("reporting %d bytes...\n", len);
 		} else {
diff --git a/drivers/net/ethoc.c b/drivers/net/ethoc.c
index 46c82bb..edb3c80 100644
--- a/drivers/net/ethoc.c
+++ b/drivers/net/ethoc.c
@@ -267,7 +267,7 @@ static int ethoc_init_ring(struct eth_device *dev)
 	bd.stat = RX_BD_EMPTY | RX_BD_IRQ;
 
 	for (i = 0; i < priv->num_rx; i++) {
-		bd.addr = (u32)NetRxPackets[i];
+		bd.addr = (u32)net_rx_packets[i];
 		if (i == priv->num_rx - 1)
 			bd.stat |= RX_BD_WRAP;
 
@@ -372,7 +372,7 @@ static int ethoc_rx(struct eth_device *dev, int limit)
 		if (ethoc_update_rx_stats(&bd) == 0) {
 			int size = bd.stat >> 16;
 			size -= 4;	/* strip the CRC */
-			NetReceive((void *)bd.addr, size);
+			net_process_received_packet((void *)bd.addr, size);
 		}
 
 		/* clear the buffer descriptor so it can be reused */
diff --git a/drivers/net/fec_mxc.c b/drivers/net/fec_mxc.c
index b572470..9225d37 100644
--- a/drivers/net/fec_mxc.c
+++ b/drivers/net/fec_mxc.c
@@ -357,7 +357,7 @@ static int fec_get_hwaddr(struct eth_device *dev, int dev_id,
 						unsigned char *mac)
 {
 	imx_get_mac_from_fuse(dev_id, mac);
-	return !is_valid_ether_addr(mac);
+	return !is_valid_ethaddr(mac);
 }
 
 static int fec_set_hwaddr(struct eth_device *dev)
@@ -852,7 +852,7 @@ static int fec_recv(struct eth_device *dev)
 			swap_packet((uint32_t *)frame->data, frame_length);
 #endif
 			memcpy(buff, frame->data, frame_length);
-			NetReceive(buff, frame_length);
+			net_process_received_packet(buff, frame_length);
 			len = frame_length;
 		} else {
 			if (bd_status & FEC_RBD_ERR)
diff --git a/drivers/net/fm/eth.c b/drivers/net/fm/eth.c
index 1d1089d..d7a37f3 100644
--- a/drivers/net/fm/eth.c
+++ b/drivers/net/fm/eth.c
@@ -15,7 +15,7 @@
 #include <phy.h>
 #include <asm/fsl_dtsec.h>
 #include <asm/fsl_tgec.h>
-#include <asm/fsl_memac.h>
+#include <fsl_memac.h>
 
 #include "fm.h"
 
@@ -530,7 +530,7 @@ static int fm_eth_recv(struct eth_device *dev)
 		if (!(status & RxBD_ERROR)) {
 			data = (u8 *)rxbd->buf_ptr_lo;
 			len = rxbd->len;
-			NetReceive(data, len);
+			net_process_received_packet(data, len);
 		} else {
 			printf("%s: Rx error\n", dev->name);
 			return 0;
diff --git a/drivers/net/fm/memac.c b/drivers/net/fm/memac.c
index 60e898c..81a64bf 100644
--- a/drivers/net/fm/memac.c
+++ b/drivers/net/fm/memac.c
@@ -12,7 +12,7 @@
 #include <phy.h>
 #include <asm/types.h>
 #include <asm/io.h>
-#include <asm/fsl_memac.h>
+#include <fsl_memac.h>
 
 #include "fm.h"
 
diff --git a/drivers/net/fm/memac_phy.c b/drivers/net/fm/memac_phy.c
index a155d89..4ab78e6 100644
--- a/drivers/net/fm/memac_phy.c
+++ b/drivers/net/fm/memac_phy.c
@@ -10,9 +10,28 @@
 #include <miiphy.h>
 #include <phy.h>
 #include <asm/io.h>
-#include <asm/fsl_memac.h>
+#include <fsl_memac.h>
 #include <fm_eth.h>
 
+#ifdef CONFIG_SYS_MEMAC_LITTLE_ENDIAN
+#define memac_out_32(a, v)	out_le32(a, v)
+#define memac_clrbits_32(a, v)	clrbits_le32(a, v)
+#define memac_setbits_32(a, v)	setbits_le32(a, v)
+#else
+#define memac_out_32(a, v)	out_be32(a, v)
+#define memac_clrbits_32(a, v)	clrbits_be32(a, v)
+#define memac_setbits_32(a, v)	setbits_be32(a, v)
+#endif
+
+static u32 memac_in_32(u32 *reg)
+{
+#ifdef CONFIG_SYS_MEMAC_LITTLE_ENDIAN
+	return in_le32(reg);
+#else
+	return in_be32(reg);
+#endif
+}
+
 /*
  * Write value to the PHY for this device to the register at regnum, waiting
  * until the write is done before it returns.  All PHY configuration has to be
@@ -28,31 +47,31 @@ int memac_mdio_write(struct mii_dev *bus, int port_addr, int dev_addr,
 	if (dev_addr == MDIO_DEVAD_NONE) {
 		c45 = 0; /* clause 22 */
 		dev_addr = regnum & 0x1f;
-		clrbits_be32(&regs->mdio_stat, MDIO_STAT_ENC);
+		memac_clrbits_32(&regs->mdio_stat, MDIO_STAT_ENC);
 	} else
-		setbits_be32(&regs->mdio_stat, MDIO_STAT_ENC);
+		memac_setbits_32(&regs->mdio_stat, MDIO_STAT_ENC);
 
 	/* Wait till the bus is free */
-	while ((in_be32(&regs->mdio_stat)) & MDIO_STAT_BSY)
+	while ((memac_in_32(&regs->mdio_stat)) & MDIO_STAT_BSY)
 		;
 
 	/* Set the port and dev addr */
 	mdio_ctl = MDIO_CTL_PORT_ADDR(port_addr) | MDIO_CTL_DEV_ADDR(dev_addr);
-	out_be32(&regs->mdio_ctl, mdio_ctl);
+	memac_out_32(&regs->mdio_ctl, mdio_ctl);
 
 	/* Set the register address */
 	if (c45)
-		out_be32(&regs->mdio_addr, regnum & 0xffff);
+		memac_out_32(&regs->mdio_addr, regnum & 0xffff);
 
 	/* Wait till the bus is free */
-	while ((in_be32(&regs->mdio_stat)) & MDIO_STAT_BSY)
+	while ((memac_in_32(&regs->mdio_stat)) & MDIO_STAT_BSY)
 		;
 
 	/* Write the value to the register */
-	out_be32(&regs->mdio_data, MDIO_DATA(value));
+	memac_out_32(&regs->mdio_data, MDIO_DATA(value));
 
 	/* Wait till the MDIO write is complete */
-	while ((in_be32(&regs->mdio_data)) & MDIO_DATA_BSY)
+	while ((memac_in_32(&regs->mdio_data)) & MDIO_DATA_BSY)
 		;
 
 	return 0;
@@ -75,39 +94,39 @@ int memac_mdio_read(struct mii_dev *bus, int port_addr, int dev_addr,
 			return 0xffff;
 		c45 = 0; /* clause 22 */
 		dev_addr = regnum & 0x1f;
-		clrbits_be32(&regs->mdio_stat, MDIO_STAT_ENC);
+		memac_clrbits_32(&regs->mdio_stat, MDIO_STAT_ENC);
 	} else
-		setbits_be32(&regs->mdio_stat, MDIO_STAT_ENC);
+		memac_setbits_32(&regs->mdio_stat, MDIO_STAT_ENC);
 
 	/* Wait till the bus is free */
-	while ((in_be32(&regs->mdio_stat)) & MDIO_STAT_BSY)
+	while ((memac_in_32(&regs->mdio_stat)) & MDIO_STAT_BSY)
 		;
 
 	/* Set the Port and Device Addrs */
 	mdio_ctl = MDIO_CTL_PORT_ADDR(port_addr) | MDIO_CTL_DEV_ADDR(dev_addr);
-	out_be32(&regs->mdio_ctl, mdio_ctl);
+	memac_out_32(&regs->mdio_ctl, mdio_ctl);
 
 	/* Set the register address */
 	if (c45)
-		out_be32(&regs->mdio_addr, regnum & 0xffff);
+		memac_out_32(&regs->mdio_addr, regnum & 0xffff);
 
 	/* Wait till the bus is free */
-	while ((in_be32(&regs->mdio_stat)) & MDIO_STAT_BSY)
+	while ((memac_in_32(&regs->mdio_stat)) & MDIO_STAT_BSY)
 		;
 
 	/* Initiate the read */
 	mdio_ctl |= MDIO_CTL_READ;
-	out_be32(&regs->mdio_ctl, mdio_ctl);
+	memac_out_32(&regs->mdio_ctl, mdio_ctl);
 
 	/* Wait till the MDIO write is complete */
-	while ((in_be32(&regs->mdio_data)) & MDIO_DATA_BSY)
+	while ((memac_in_32(&regs->mdio_data)) & MDIO_DATA_BSY)
 		;
 
 	/* Return all Fs if nothing was there */
-	if (in_be32(&regs->mdio_stat) & MDIO_STAT_RD_ER)
+	if (memac_in_32(&regs->mdio_stat) & MDIO_STAT_RD_ER)
 		return 0xffff;
 
-	return in_be32(&regs->mdio_data) & 0xffff;
+	return memac_in_32(&regs->mdio_data) & 0xffff;
 }
 
 int memac_mdio_reset(struct mii_dev *bus)
@@ -143,8 +162,9 @@ int fm_memac_mdio_init(bd_t *bis, struct memac_mdio_info *info)
 	 * like T2080QDS, this bit default is '0', which leads to MDIO failure
 	 * on XAUI PHY, so set this bit definitely.
 	 */
-	setbits_be32(&((struct memac_mdio_controller *)info->regs)->mdio_stat,
-		     MDIO_STAT_CLKDIV(258) | MDIO_STAT_NEG);
+	memac_setbits_32(
+		&((struct memac_mdio_controller *)info->regs)->mdio_stat,
+		MDIO_STAT_CLKDIV(258) | MDIO_STAT_NEG);
 
 	return mdio_register(bus);
 }
diff --git a/drivers/net/fsl-mc/Makefile b/drivers/net/fsl-mc/Makefile
index 206ac6b..7563a5f 100644
--- a/drivers/net/fsl-mc/Makefile
+++ b/drivers/net/fsl-mc/Makefile
@@ -7,4 +7,8 @@
 # Layerscape MC driver
 obj-y += mc.o \
 	mc_sys.o \
-	dpmng.o
+	dpmng.o	\
+	dprc.o	\
+	dpbp.o	\
+	dpni.o
+obj-y += dpio/
diff --git a/drivers/net/fsl-mc/dpbp.c b/drivers/net/fsl-mc/dpbp.c
new file mode 100644
index 0000000..3853e58
--- /dev/null
+++ b/drivers/net/fsl-mc/dpbp.c
@@ -0,0 +1,102 @@
+/*
+ * Freescale Layerscape MC I/O wrapper
+ *
+ * Copyright (C) 2013-2015 Freescale Semiconductor, Inc.
+ * Author: German Rivera <German.Rivera@freescale.com>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+#include <fsl-mc/fsl_mc_sys.h>
+#include <fsl-mc/fsl_mc_cmd.h>
+#include <fsl-mc/fsl_dpbp.h>
+
+int dpbp_open(struct fsl_mc_io *mc_io, int dpbp_id, uint16_t *token)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPBP_CMDID_OPEN,
+					  MC_CMD_PRI_LOW, 0);
+	DPBP_CMD_OPEN(cmd, dpbp_id);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	*token = MC_CMD_HDR_READ_TOKEN(cmd.header);
+
+	return err;
+}
+
+int dpbp_close(struct fsl_mc_io *mc_io, uint16_t token)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPBP_CMDID_CLOSE, MC_CMD_PRI_HIGH,
+					  token);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+int dpbp_enable(struct fsl_mc_io *mc_io, uint16_t token)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPBP_CMDID_ENABLE, MC_CMD_PRI_LOW,
+					  token);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+int dpbp_disable(struct fsl_mc_io *mc_io, uint16_t token)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPBP_CMDID_DISABLE,
+					  MC_CMD_PRI_LOW, token);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+int dpbp_reset(struct fsl_mc_io *mc_io, uint16_t token)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPBP_CMDID_RESET,
+					  MC_CMD_PRI_LOW, token);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+int dpbp_get_attributes(struct fsl_mc_io *mc_io,
+			uint16_t token,
+			struct dpbp_attr *attr)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPBP_CMDID_GET_ATTR,
+					  MC_CMD_PRI_LOW, token);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	DPBP_RSP_GET_ATTRIBUTES(cmd, attr);
+
+	return 0;
+}
diff --git a/drivers/net/fsl-mc/dpio/Makefile b/drivers/net/fsl-mc/dpio/Makefile
new file mode 100644
index 0000000..1ccefc0
--- /dev/null
+++ b/drivers/net/fsl-mc/dpio/Makefile
@@ -0,0 +1,9 @@
+#
+# Copyright 2014 Freescale Semiconductor, Inc.
+#
+# SPDX-License-Identifier:	GPL-2.0+
+#
+
+# Layerscape MC DPIO driver
+obj-y += dpio.o	\
+	qbman_portal.o
diff --git a/drivers/net/fsl-mc/dpio/dpio.c b/drivers/net/fsl-mc/dpio/dpio.c
new file mode 100644
index 0000000..b07eff7
--- /dev/null
+++ b/drivers/net/fsl-mc/dpio/dpio.c
@@ -0,0 +1,102 @@
+/*
+ * Copyright (C) 2013-2015 Freescale Semiconductor
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <fsl-mc/fsl_mc_sys.h>
+#include <fsl-mc/fsl_mc_cmd.h>
+#include <fsl-mc/fsl_dpio.h>
+
+int dpio_open(struct fsl_mc_io *mc_io, int dpio_id, uint16_t *token)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPIO_CMDID_OPEN,
+					  MC_CMD_PRI_LOW, 0);
+	DPIO_CMD_OPEN(cmd, dpio_id);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	*token = MC_CMD_HDR_READ_TOKEN(cmd.header);
+
+	return 0;
+}
+
+int dpio_close(struct fsl_mc_io *mc_io, uint16_t token)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPIO_CMDID_CLOSE,
+					  MC_CMD_PRI_HIGH, token);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+int dpio_enable(struct fsl_mc_io *mc_io, uint16_t token)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPIO_CMDID_ENABLE,
+					  MC_CMD_PRI_LOW, token);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+int dpio_disable(struct fsl_mc_io *mc_io, uint16_t token)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPIO_CMDID_DISABLE,
+					  MC_CMD_PRI_LOW,
+					  token);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+int dpio_reset(struct fsl_mc_io *mc_io, uint16_t token)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPIO_CMDID_RESET,
+					  MC_CMD_PRI_LOW, token);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+int dpio_get_attributes(struct fsl_mc_io *mc_io,
+			uint16_t token,
+			struct dpio_attr *attr)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPIO_CMDID_GET_ATTR,
+					  MC_CMD_PRI_LOW,
+					  token);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	DPIO_RSP_GET_ATTR(cmd, attr);
+
+	return 0;
+}
diff --git a/drivers/net/fsl-mc/dpio/qbman_portal.c b/drivers/net/fsl-mc/dpio/qbman_portal.c
new file mode 100644
index 0000000..dd2a7de
--- /dev/null
+++ b/drivers/net/fsl-mc/dpio/qbman_portal.c
@@ -0,0 +1,593 @@
+/*
+ * Copyright (C) 2014 Freescale Semiconductor
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include "qbman_portal.h"
+
+/* QBMan portal management command codes */
+#define QBMAN_MC_ACQUIRE       0x30
+#define QBMAN_WQCHAN_CONFIGURE 0x46
+
+/* CINH register offsets */
+#define QBMAN_CINH_SWP_EQAR    0x8c0
+#define QBMAN_CINH_SWP_DCAP    0xac0
+#define QBMAN_CINH_SWP_SDQCR   0xb00
+#define QBMAN_CINH_SWP_RAR     0xcc0
+
+/* CENA register offsets */
+#define QBMAN_CENA_SWP_EQCR(n) (0x000 + ((uint32_t)(n) << 6))
+#define QBMAN_CENA_SWP_DQRR(n) (0x200 + ((uint32_t)(n) << 6))
+#define QBMAN_CENA_SWP_RCR(n)  (0x400 + ((uint32_t)(n) << 6))
+#define QBMAN_CENA_SWP_CR      0x600
+#define QBMAN_CENA_SWP_RR(vb)  (0x700 + ((uint32_t)(vb) >> 1))
+#define QBMAN_CENA_SWP_VDQCR   0x780
+
+/* Reverse mapping of QBMAN_CENA_SWP_DQRR() */
+#define QBMAN_IDX_FROM_DQRR(p) (((unsigned long)p & 0xff) >> 6)
+
+/*******************************/
+/* Pre-defined attribute codes */
+/*******************************/
+
+struct qb_attr_code code_generic_verb = QB_CODE(0, 0, 7);
+struct qb_attr_code code_generic_rslt = QB_CODE(0, 8, 8);
+
+/*************************/
+/* SDQCR attribute codes */
+/*************************/
+
+/* we put these here because at least some of them are required by
+ * qbman_swp_init() */
+struct qb_attr_code code_sdqcr_dct = QB_CODE(0, 24, 2);
+struct qb_attr_code code_sdqcr_fc = QB_CODE(0, 29, 1);
+struct qb_attr_code code_sdqcr_tok = QB_CODE(0, 16, 8);
+#define CODE_SDQCR_DQSRC(n) QB_CODE(0, n, 1)
+enum qbman_sdqcr_dct {
+	qbman_sdqcr_dct_null = 0,
+	qbman_sdqcr_dct_prio_ics,
+	qbman_sdqcr_dct_active_ics,
+	qbman_sdqcr_dct_active
+};
+enum qbman_sdqcr_fc {
+	qbman_sdqcr_fc_one = 0,
+	qbman_sdqcr_fc_up_to_3 = 1
+};
+
+/*********************************/
+/* Portal constructor/destructor */
+/*********************************/
+
+/* Software portals should always be in the power-on state when we initialise,
+ * due to the CCSR-based portal reset functionality that MC has. */
+struct qbman_swp *qbman_swp_init(const struct qbman_swp_desc *d)
+{
+	int ret;
+	struct qbman_swp *p = kmalloc(sizeof(*p), GFP_KERNEL);
+
+	if (!p)
+		return NULL;
+	p->desc = d;
+#ifdef QBMAN_CHECKING
+	p->mc.check = swp_mc_can_start;
+#endif
+	p->mc.valid_bit = QB_VALID_BIT;
+	p->sdq = 0;
+	qb_attr_code_encode(&code_sdqcr_dct, &p->sdq, qbman_sdqcr_dct_prio_ics);
+	qb_attr_code_encode(&code_sdqcr_fc, &p->sdq, qbman_sdqcr_fc_up_to_3);
+	qb_attr_code_encode(&code_sdqcr_tok, &p->sdq, 0xbb);
+	p->vdq.busy = 0; /* TODO: convert to atomic_t */
+	p->vdq.valid_bit = QB_VALID_BIT;
+	p->dqrr.next_idx = 0;
+	p->dqrr.valid_bit = QB_VALID_BIT;
+	ret = qbman_swp_sys_init(&p->sys, d);
+	if (ret) {
+		free(p);
+		printf("qbman_swp_sys_init() failed %d\n", ret);
+		return NULL;
+	}
+	qbman_cinh_write(&p->sys, QBMAN_CINH_SWP_SDQCR, p->sdq);
+	return p;
+}
+
+/***********************/
+/* Management commands */
+/***********************/
+
+/*
+ * Internal code common to all types of management commands.
+ */
+
+void *qbman_swp_mc_start(struct qbman_swp *p)
+{
+	void *ret;
+#ifdef QBMAN_CHECKING
+	BUG_ON(p->mc.check != swp_mc_can_start);
+#endif
+	ret = qbman_cena_write_start(&p->sys, QBMAN_CENA_SWP_CR);
+#ifdef QBMAN_CHECKING
+	if (!ret)
+		p->mc.check = swp_mc_can_submit;
+#endif
+	return ret;
+}
+
+void qbman_swp_mc_submit(struct qbman_swp *p, void *cmd, uint32_t cmd_verb)
+{
+	uint32_t *v = cmd;
+#ifdef QBMAN_CHECKING
+	BUG_ON(!p->mc.check != swp_mc_can_submit);
+#endif
+	lwsync();
+	/* TBD: "|=" is going to hurt performance. Need to move as many fields
+	 * out of word zero, and for those that remain, the "OR" needs to occur
+	 * at the caller side. This debug check helps to catch cases where the
+	 * caller wants to OR but has forgotten to do so. */
+	BUG_ON((*v & cmd_verb) != *v);
+	*v = cmd_verb | p->mc.valid_bit;
+	qbman_cena_write_complete(&p->sys, QBMAN_CENA_SWP_CR, cmd);
+	/* TODO: add prefetch support for GPP */
+#ifdef QBMAN_CHECKING
+	p->mc.check = swp_mc_can_poll;
+#endif
+}
+
+void *qbman_swp_mc_result(struct qbman_swp *p)
+{
+	uint32_t *ret, verb;
+#ifdef QBMAN_CHECKING
+	BUG_ON(p->mc.check != swp_mc_can_poll);
+#endif
+	ret = qbman_cena_read(&p->sys, QBMAN_CENA_SWP_RR(p->mc.valid_bit));
+	/* Remove the valid-bit - command completed iff the rest is non-zero */
+	verb = ret[0] & ~QB_VALID_BIT;
+	if (!verb)
+		return NULL;
+#ifdef QBMAN_CHECKING
+	p->mc.check = swp_mc_can_start;
+#endif
+	p->mc.valid_bit ^= QB_VALID_BIT;
+	return ret;
+}
+
+/***********/
+/* Enqueue */
+/***********/
+
+/* These should be const, eventually */
+static struct qb_attr_code code_eq_cmd = QB_CODE(0, 0, 2);
+static struct qb_attr_code code_eq_orp_en = QB_CODE(0, 2, 1);
+static struct qb_attr_code code_eq_tgt_id = QB_CODE(2, 0, 24);
+/* static struct qb_attr_code code_eq_tag = QB_CODE(3, 0, 32); */
+static struct qb_attr_code code_eq_qd_en = QB_CODE(0, 4, 1);
+static struct qb_attr_code code_eq_qd_bin = QB_CODE(4, 0, 16);
+static struct qb_attr_code code_eq_qd_pri = QB_CODE(4, 16, 4);
+static struct qb_attr_code code_eq_rsp_stash = QB_CODE(5, 16, 1);
+static struct qb_attr_code code_eq_rsp_lo = QB_CODE(6, 0, 32);
+static struct qb_attr_code code_eq_rsp_hi = QB_CODE(7, 0, 32);
+
+enum qbman_eq_cmd_e {
+	/* No enqueue, primarily for plugging ORP gaps for dropped frames */
+	qbman_eq_cmd_empty,
+	/* DMA an enqueue response once complete */
+	qbman_eq_cmd_respond,
+	/* DMA an enqueue response only if the enqueue fails */
+	qbman_eq_cmd_respond_reject
+};
+
+void qbman_eq_desc_clear(struct qbman_eq_desc *d)
+{
+	memset(d, 0, sizeof(*d));
+}
+
+void qbman_eq_desc_set_no_orp(struct qbman_eq_desc *d, int respond_success)
+{
+	uint32_t *cl = qb_cl(d);
+
+	qb_attr_code_encode(&code_eq_orp_en, cl, 0);
+	qb_attr_code_encode(&code_eq_cmd, cl,
+			    respond_success ? qbman_eq_cmd_respond :
+					      qbman_eq_cmd_respond_reject);
+}
+
+void qbman_eq_desc_set_response(struct qbman_eq_desc *d,
+				dma_addr_t storage_phys,
+				int stash)
+{
+	uint32_t *cl = qb_cl(d);
+
+	qb_attr_code_encode(&code_eq_rsp_lo, cl, lower32(storage_phys));
+	qb_attr_code_encode(&code_eq_rsp_hi, cl, upper32(storage_phys));
+	qb_attr_code_encode(&code_eq_rsp_stash, cl, !!stash);
+}
+
+
+void qbman_eq_desc_set_qd(struct qbman_eq_desc *d, uint32_t qdid,
+			  uint32_t qd_bin, uint32_t qd_prio)
+{
+	uint32_t *cl = qb_cl(d);
+
+	qb_attr_code_encode(&code_eq_qd_en, cl, 1);
+	qb_attr_code_encode(&code_eq_tgt_id, cl, qdid);
+	qb_attr_code_encode(&code_eq_qd_bin, cl, qd_bin);
+	qb_attr_code_encode(&code_eq_qd_pri, cl, qd_prio);
+}
+
+#define EQAR_IDX(eqar)     ((eqar) & 0x7)
+#define EQAR_VB(eqar)      ((eqar) & 0x80)
+#define EQAR_SUCCESS(eqar) ((eqar) & 0x100)
+
+int qbman_swp_enqueue(struct qbman_swp *s, const struct qbman_eq_desc *d,
+		      const struct qbman_fd *fd)
+{
+	uint32_t *p;
+	const uint32_t *cl = qb_cl(d);
+	uint32_t eqar = qbman_cinh_read(&s->sys, QBMAN_CINH_SWP_EQAR);
+	debug("EQAR=%08x\n", eqar);
+	if (!EQAR_SUCCESS(eqar))
+		return -EBUSY;
+	p = qbman_cena_write_start(&s->sys,
+				   QBMAN_CENA_SWP_EQCR(EQAR_IDX(eqar)));
+	word_copy(&p[1], &cl[1], 7);
+	word_copy(&p[8], fd, sizeof(*fd) >> 2);
+	lwsync();
+	/* Set the verb byte, have to substitute in the valid-bit */
+	p[0] = cl[0] | EQAR_VB(eqar);
+	qbman_cena_write_complete(&s->sys,
+				  QBMAN_CENA_SWP_EQCR(EQAR_IDX(eqar)),
+				  p);
+	return 0;
+}
+
+/***************************/
+/* Volatile (pull) dequeue */
+/***************************/
+
+/* These should be const, eventually */
+static struct qb_attr_code code_pull_dct = QB_CODE(0, 0, 2);
+static struct qb_attr_code code_pull_dt = QB_CODE(0, 2, 2);
+static struct qb_attr_code code_pull_rls = QB_CODE(0, 4, 1);
+static struct qb_attr_code code_pull_stash = QB_CODE(0, 5, 1);
+static struct qb_attr_code code_pull_numframes = QB_CODE(0, 8, 4);
+static struct qb_attr_code code_pull_token = QB_CODE(0, 16, 8);
+static struct qb_attr_code code_pull_dqsource = QB_CODE(1, 0, 24);
+static struct qb_attr_code code_pull_rsp_lo = QB_CODE(2, 0, 32);
+static struct qb_attr_code code_pull_rsp_hi = QB_CODE(3, 0, 32);
+
+enum qb_pull_dt_e {
+	qb_pull_dt_channel,
+	qb_pull_dt_workqueue,
+	qb_pull_dt_framequeue
+};
+
+void qbman_pull_desc_clear(struct qbman_pull_desc *d)
+{
+	memset(d, 0, sizeof(*d));
+}
+
+void qbman_pull_desc_set_storage(struct qbman_pull_desc *d,
+				 struct ldpaa_dq *storage,
+				 dma_addr_t storage_phys,
+				 int stash)
+{
+	uint32_t *cl = qb_cl(d);
+
+	/* Squiggle the pointer 'storage' into the extra 2 words of the
+	 * descriptor (which aren't copied to the hw command) */
+	*(void **)&cl[4] = storage;
+	if (!storage) {
+		qb_attr_code_encode(&code_pull_rls, cl, 0);
+		return;
+	}
+	qb_attr_code_encode(&code_pull_rls, cl, 1);
+	qb_attr_code_encode(&code_pull_stash, cl, !!stash);
+	qb_attr_code_encode(&code_pull_rsp_lo, cl, lower32(storage_phys));
+	qb_attr_code_encode(&code_pull_rsp_hi, cl, upper32(storage_phys));
+}
+
+void qbman_pull_desc_set_numframes(struct qbman_pull_desc *d, uint8_t numframes)
+{
+	uint32_t *cl = qb_cl(d);
+
+	BUG_ON(!numframes || (numframes > 16));
+	qb_attr_code_encode(&code_pull_numframes, cl,
+			    (uint32_t)(numframes - 1));
+}
+
+void qbman_pull_desc_set_token(struct qbman_pull_desc *d, uint8_t token)
+{
+	uint32_t *cl = qb_cl(d);
+
+	qb_attr_code_encode(&code_pull_token, cl, token);
+}
+
+void qbman_pull_desc_set_fq(struct qbman_pull_desc *d, uint32_t fqid)
+{
+	uint32_t *cl = qb_cl(d);
+
+	qb_attr_code_encode(&code_pull_dct, cl, 1);
+	qb_attr_code_encode(&code_pull_dt, cl, qb_pull_dt_framequeue);
+	qb_attr_code_encode(&code_pull_dqsource, cl, fqid);
+}
+
+int qbman_swp_pull(struct qbman_swp *s, struct qbman_pull_desc *d)
+{
+	uint32_t *p;
+	uint32_t *cl = qb_cl(d);
+
+	/* TODO: convert to atomic_t */
+	if (s->vdq.busy)
+		return -EBUSY;
+	s->vdq.busy = 1;
+	s->vdq.storage = *(void **)&cl[4];
+	s->vdq.token = qb_attr_code_decode(&code_pull_token, cl);
+	p = qbman_cena_write_start(&s->sys, QBMAN_CENA_SWP_VDQCR);
+	word_copy(&p[1], &cl[1], 3);
+	lwsync();
+	/* Set the verb byte, have to substitute in the valid-bit */
+	p[0] = cl[0] | s->vdq.valid_bit;
+	s->vdq.valid_bit ^= QB_VALID_BIT;
+	qbman_cena_write_complete(&s->sys, QBMAN_CENA_SWP_VDQCR, p);
+	return 0;
+}
+
+/****************/
+/* Polling DQRR */
+/****************/
+
+static struct qb_attr_code code_dqrr_verb = QB_CODE(0, 0, 8);
+static struct qb_attr_code code_dqrr_response = QB_CODE(0, 0, 7);
+static struct qb_attr_code code_dqrr_stat = QB_CODE(0, 8, 8);
+
+#define QBMAN_DQRR_RESPONSE_DQ        0x60
+#define QBMAN_DQRR_RESPONSE_FQRN      0x21
+#define QBMAN_DQRR_RESPONSE_FQRNI     0x22
+#define QBMAN_DQRR_RESPONSE_FQPN      0x24
+#define QBMAN_DQRR_RESPONSE_FQDAN     0x25
+#define QBMAN_DQRR_RESPONSE_CDAN      0x26
+#define QBMAN_DQRR_RESPONSE_CSCN_MEM  0x27
+#define QBMAN_DQRR_RESPONSE_CGCU      0x28
+#define QBMAN_DQRR_RESPONSE_BPSCN     0x29
+#define QBMAN_DQRR_RESPONSE_CSCN_WQ   0x2a
+
+
+/* NULL return if there are no unconsumed DQRR entries. Returns a DQRR entry
+ * only once, so repeated calls can return a sequence of DQRR entries, without
+ * requiring they be consumed immediately or in any particular order. */
+const struct ldpaa_dq *qbman_swp_dqrr_next(struct qbman_swp *s)
+{
+	uint32_t verb;
+	uint32_t response_verb;
+	const struct ldpaa_dq *dq = qbman_cena_read(&s->sys,
+					QBMAN_CENA_SWP_DQRR(s->dqrr.next_idx));
+	const uint32_t *p = qb_cl(dq);
+
+	verb = qb_attr_code_decode(&code_dqrr_verb, p);
+	/* If the valid-bit isn't of the expected polarity, nothing there */
+	if ((verb & QB_VALID_BIT) != s->dqrr.valid_bit) {
+		qbman_cena_invalidate_prefetch(&s->sys,
+					       QBMAN_CENA_SWP_DQRR(
+					       s->dqrr.next_idx));
+		return NULL;
+	}
+	/* There's something there. Move "next_idx" attention to the next ring
+	 * entry (and prefetch it) before returning what we found. */
+	s->dqrr.next_idx++;
+	s->dqrr.next_idx &= 3; /* Wrap around at 4 */
+	/* TODO: it's possible to do all this without conditionals, optimise it
+	 * later. */
+	if (!s->dqrr.next_idx)
+		s->dqrr.valid_bit ^= QB_VALID_BIT;
+	/* VDQCR "no longer busy" hook - if VDQCR shows "busy" and this is a
+	 * VDQCR result, mark it as non-busy. */
+	if (s->vdq.busy) {
+		uint32_t flags = ldpaa_dq_flags(dq);
+
+		response_verb = qb_attr_code_decode(&code_dqrr_response, &verb);
+		if ((response_verb == QBMAN_DQRR_RESPONSE_DQ) &&
+		    (flags & LDPAA_DQ_STAT_VOLATILE))
+			s->vdq.busy = 0;
+	}
+	qbman_cena_invalidate_prefetch(&s->sys,
+				       QBMAN_CENA_SWP_DQRR(s->dqrr.next_idx));
+	return dq;
+}
+
+/* Consume DQRR entries previously returned from qbman_swp_dqrr_next(). */
+void qbman_swp_dqrr_consume(struct qbman_swp *s, const struct ldpaa_dq *dq)
+{
+	qbman_cinh_write(&s->sys, QBMAN_CINH_SWP_DCAP, QBMAN_IDX_FROM_DQRR(dq));
+}
+
+/*********************************/
+/* Polling user-provided storage */
+/*********************************/
+
+void qbman_dq_entry_set_oldtoken(struct ldpaa_dq *dq,
+				 unsigned int num_entries,
+				 uint8_t oldtoken)
+{
+	memset(dq, oldtoken, num_entries * sizeof(*dq));
+}
+
+int qbman_dq_entry_has_newtoken(struct qbman_swp *s,
+				const struct ldpaa_dq *dq,
+				uint8_t newtoken)
+{
+	/* To avoid converting the little-endian DQ entry to host-endian prior
+	 * to us knowing whether there is a valid entry or not (and run the
+	 * risk of corrupting the incoming hardware LE write), we detect in
+	 * hardware endianness rather than host. This means we need a different
+	 * "code" depending on whether we are BE or LE in software, which is
+	 * where DQRR_TOK_OFFSET comes in... */
+	static struct qb_attr_code code_dqrr_tok_detect =
+					QB_CODE(0, DQRR_TOK_OFFSET, 8);
+	/* The user trying to poll for a result treats "dq" as const. It is
+	 * however the same address that was provided to us non-const in the
+	 * first place, for directing hardware DMA to. So we can cast away the
+	 * const because it is mutable from our perspective. */
+	uint32_t *p = qb_cl((struct ldpaa_dq *)dq);
+	uint32_t token;
+
+	token = qb_attr_code_decode(&code_dqrr_tok_detect, &p[1]);
+	if (token != newtoken)
+		return 0;
+
+	/* Only now do we convert from hardware to host endianness. Also, as we
+	 * are returning success, the user has promised not to call us again, so
+	 * there's no risk of us converting the endianness twice... */
+	make_le32_n(p, 16);
+
+	/* VDQCR "no longer busy" hook - not quite the same as DQRR, because the
+	 * fact "VDQCR" shows busy doesn't mean that the result we're looking at
+	 * is from the same command. Eg. we may be looking at our 10th dequeue
+	 * result from our first VDQCR command, yet the second dequeue command
+	 * could have been kicked off already, after seeing the 1st result. Ie.
+	 * the result we're looking at is not necessarily proof that we can
+	 * reset "busy".  We instead base the decision on whether the current
+	 * result is sitting at the first 'storage' location of the busy
+	 * command. */
+	if (s->vdq.busy && (s->vdq.storage == dq))
+		s->vdq.busy = 0;
+	return 1;
+}
+
+/********************************/
+/* Categorising dequeue entries */
+/********************************/
+
+static inline int __qbman_dq_entry_is_x(const struct ldpaa_dq *dq, uint32_t x)
+{
+	const uint32_t *p = qb_cl(dq);
+	uint32_t response_verb = qb_attr_code_decode(&code_dqrr_response, p);
+
+	return response_verb == x;
+}
+
+int qbman_dq_entry_is_DQ(const struct ldpaa_dq *dq)
+{
+	return __qbman_dq_entry_is_x(dq, QBMAN_DQRR_RESPONSE_DQ);
+}
+
+/*********************************/
+/* Parsing frame dequeue results */
+/*********************************/
+
+/* These APIs assume qbman_dq_entry_is_DQ() is TRUE */
+
+uint32_t ldpaa_dq_flags(const struct ldpaa_dq *dq)
+{
+	const uint32_t *p = qb_cl(dq);
+
+	return qb_attr_code_decode(&code_dqrr_stat, p);
+}
+
+const struct dpaa_fd *ldpaa_dq_fd(const struct ldpaa_dq *dq)
+{
+	const uint32_t *p = qb_cl(dq);
+
+	return (const struct dpaa_fd *)&p[8];
+}
+
+/******************/
+/* Buffer release */
+/******************/
+
+/* These should be const, eventually */
+/* static struct qb_attr_code code_release_num = QB_CODE(0, 0, 3); */
+static struct qb_attr_code code_release_set_me = QB_CODE(0, 5, 1);
+static struct qb_attr_code code_release_bpid = QB_CODE(0, 16, 16);
+
+void qbman_release_desc_clear(struct qbman_release_desc *d)
+{
+	uint32_t *cl;
+
+	memset(d, 0, sizeof(*d));
+	cl = qb_cl(d);
+	qb_attr_code_encode(&code_release_set_me, cl, 1);
+}
+
+void qbman_release_desc_set_bpid(struct qbman_release_desc *d, uint32_t bpid)
+{
+	uint32_t *cl = qb_cl(d);
+
+	qb_attr_code_encode(&code_release_bpid, cl, bpid);
+}
+
+#define RAR_IDX(rar)     ((rar) & 0x7)
+#define RAR_VB(rar)      ((rar) & 0x80)
+#define RAR_SUCCESS(rar) ((rar) & 0x100)
+
+int qbman_swp_release(struct qbman_swp *s, const struct qbman_release_desc *d,
+		      const uint64_t *buffers, unsigned int num_buffers)
+{
+	uint32_t *p;
+	const uint32_t *cl = qb_cl(d);
+	uint32_t rar = qbman_cinh_read(&s->sys, QBMAN_CINH_SWP_RAR);
+	debug("RAR=%08x\n", rar);
+	if (!RAR_SUCCESS(rar))
+		return -EBUSY;
+	BUG_ON(!num_buffers || (num_buffers > 7));
+	/* Start the release command */
+	p = qbman_cena_write_start(&s->sys,
+				   QBMAN_CENA_SWP_RCR(RAR_IDX(rar)));
+	/* Copy the caller's buffer pointers to the command */
+	u64_to_le32_copy(&p[2], buffers, num_buffers);
+	lwsync();
+	/* Set the verb byte, have to substitute in the valid-bit and the number
+	 * of buffers. */
+	p[0] = cl[0] | RAR_VB(rar) | num_buffers;
+	qbman_cena_write_complete(&s->sys,
+				  QBMAN_CENA_SWP_RCR(RAR_IDX(rar)),
+				  p);
+	return 0;
+}
+
+/*******************/
+/* Buffer acquires */
+/*******************/
+
+/* These should be const, eventually */
+static struct qb_attr_code code_acquire_bpid = QB_CODE(0, 16, 16);
+static struct qb_attr_code code_acquire_num = QB_CODE(1, 0, 3);
+static struct qb_attr_code code_acquire_r_num = QB_CODE(1, 0, 3);
+
+int qbman_swp_acquire(struct qbman_swp *s, uint32_t bpid, uint64_t *buffers,
+		      unsigned int num_buffers)
+{
+	uint32_t *p;
+	uint32_t verb, rslt, num;
+
+	BUG_ON(!num_buffers || (num_buffers > 7));
+
+	/* Start the management command */
+	p = qbman_swp_mc_start(s);
+
+	if (!p)
+		return -EBUSY;
+
+	/* Encode the caller-provided attributes */
+	qb_attr_code_encode(&code_acquire_bpid, p, bpid);
+	qb_attr_code_encode(&code_acquire_num, p, num_buffers);
+
+	/* Complete the management command */
+	p = qbman_swp_mc_complete(s, p, p[0] | QBMAN_MC_ACQUIRE);
+
+	/* Decode the outcome */
+	verb = qb_attr_code_decode(&code_generic_verb, p);
+	rslt = qb_attr_code_decode(&code_generic_rslt, p);
+	num = qb_attr_code_decode(&code_acquire_r_num, p);
+	BUG_ON(verb != QBMAN_MC_ACQUIRE);
+
+	/* Determine success or failure */
+	if (unlikely(rslt != QBMAN_MC_RSLT_OK)) {
+		printf("Acquire buffers from BPID 0x%x failed, code=0x%02x\n",
+		       bpid, rslt);
+		return -EIO;
+	}
+	BUG_ON(num > num_buffers);
+	/* Copy the acquired buffers to the caller's array */
+	u64_from_le32_copy(buffers, &p[2], num);
+	return (int)num;
+}
diff --git a/drivers/net/fsl-mc/dpio/qbman_portal.h b/drivers/net/fsl-mc/dpio/qbman_portal.h
new file mode 100644
index 0000000..bb67c3b
--- /dev/null
+++ b/drivers/net/fsl-mc/dpio/qbman_portal.h
@@ -0,0 +1,157 @@
+/*
+ * Copyright (C) 2014 Freescale Semiconductor
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include "qbman_private.h"
+#include <fsl-mc/fsl_qbman_portal.h>
+#include <fsl-mc/fsl_dpaa_fd.h>
+
+/* All QBMan command and result structures use this "valid bit" encoding */
+#define QB_VALID_BIT ((uint32_t)0x80)
+
+/* Management command result codes */
+#define QBMAN_MC_RSLT_OK      0xf0
+
+/* --------------------- */
+/* portal data structure */
+/* --------------------- */
+
+struct qbman_swp {
+	const struct qbman_swp_desc *desc;
+	/* The qbman_sys (ie. arch/OS-specific) support code can put anything it
+	 * needs in here. */
+	struct qbman_swp_sys sys;
+	/* Management commands */
+	struct {
+#ifdef QBMAN_CHECKING
+		enum swp_mc_check {
+			swp_mc_can_start, /* call __qbman_swp_mc_start() */
+			swp_mc_can_submit, /* call __qbman_swp_mc_submit() */
+			swp_mc_can_poll, /* call __qbman_swp_mc_result() */
+		} check;
+#endif
+		uint32_t valid_bit; /* 0x00 or 0x80 */
+	} mc;
+	/* Push dequeues */
+	uint32_t sdq;
+	/* Volatile dequeues */
+	struct {
+		/* VDQCR supports a "1 deep pipeline", meaning that if you know
+		 * the last-submitted command is already executing in the
+		 * hardware (as evidenced by at least 1 valid dequeue result),
+		 * you can write another dequeue command to the register, the
+		 * hardware will start executing it as soon as the
+		 * already-executing command terminates. (This minimises latency
+		 * and stalls.) With that in mind, this "busy" variable refers
+		 * to whether or not a command can be submitted, not whether or
+		 * not a previously-submitted command is still executing. In
+		 * other words, once proof is seen that the previously-submitted
+		 * command is executing, "vdq" is no longer "busy". TODO:
+		 * convert this to "atomic_t" so that it is thread-safe (without
+		 * locking). */
+		int busy;
+		uint32_t valid_bit; /* 0x00 or 0x80 */
+		/* We need to determine when vdq is no longer busy. This depends
+		 * on whether the "busy" (last-submitted) dequeue command is
+		 * targetting DQRR or main-memory, and detected is based on the
+		 * presence of the dequeue command's "token" showing up in
+		 * dequeue entries in DQRR or main-memory (respectively). Debug
+		 * builds will, when submitting vdq commands, verify that the
+		 * dequeue result location is not already equal to the command's
+		 * token value. */
+		struct ldpaa_dq *storage; /* NULL if DQRR */
+		uint32_t token;
+	} vdq;
+	/* DQRR */
+	struct {
+		uint32_t next_idx;
+		uint32_t valid_bit;
+	} dqrr;
+};
+
+/* -------------------------- */
+/* portal management commands */
+/* -------------------------- */
+
+/* Different management commands all use this common base layer of code to issue
+ * commands and poll for results. The first function returns a pointer to where
+ * the caller should fill in their MC command (though they should ignore the
+ * verb byte), the second function commits merges in the caller-supplied command
+ * verb (which should not include the valid-bit) and submits the command to
+ * hardware, and the third function checks for a completed response (returns
+ * non-NULL if only if the response is complete). */
+void *qbman_swp_mc_start(struct qbman_swp *p);
+void qbman_swp_mc_submit(struct qbman_swp *p, void *cmd, uint32_t cmd_verb);
+void *qbman_swp_mc_result(struct qbman_swp *p);
+
+/* Wraps up submit + poll-for-result */
+static inline void *qbman_swp_mc_complete(struct qbman_swp *swp, void *cmd,
+					  uint32_t cmd_verb)
+{
+	int loopvar;
+
+	qbman_swp_mc_submit(swp, cmd, cmd_verb);
+	DBG_POLL_START(loopvar);
+	do {
+		DBG_POLL_CHECK(loopvar);
+		cmd = qbman_swp_mc_result(swp);
+	} while (!cmd);
+	return cmd;
+}
+
+/* ------------ */
+/* qb_attr_code */
+/* ------------ */
+
+/* This struct locates a sub-field within a QBMan portal (CENA) cacheline which
+ * is either serving as a configuration command or a query result. The
+ * representation is inherently little-endian, as the indexing of the words is
+ * itself little-endian in nature and layerscape is little endian for anything
+ * that crosses a word boundary too (64-bit fields are the obvious examples).
+ */
+struct qb_attr_code {
+	unsigned int word; /* which uint32_t[] array member encodes the field */
+	unsigned int lsoffset; /* encoding offset from ls-bit */
+	unsigned int width; /* encoding width. (bool must be 1.) */
+};
+
+/* Macros to define codes */
+#define QB_CODE(a, b, c) { a, b, c}
+
+/* decode a field from a cacheline */
+static inline uint32_t qb_attr_code_decode(const struct qb_attr_code *code,
+				      const uint32_t *cacheline)
+{
+	return d32_uint32_t(code->lsoffset, code->width, cacheline[code->word]);
+}
+
+/* encode a field to a cacheline */
+static inline void qb_attr_code_encode(const struct qb_attr_code *code,
+				       uint32_t *cacheline, uint32_t val)
+{
+	cacheline[code->word] =
+		r32_uint32_t(code->lsoffset, code->width, cacheline[code->word])
+		| e32_uint32_t(code->lsoffset, code->width, val);
+}
+
+/* ---------------------- */
+/* Descriptors/cachelines */
+/* ---------------------- */
+
+/* To avoid needless dynamic allocation, the driver API often gives the caller
+ * a "descriptor" type that the caller can instantiate however they like.
+ * Ultimately though, it is just a cacheline of binary storage (or something
+ * smaller when it is known that the descriptor doesn't need all 64 bytes) for
+ * holding pre-formatted pieces of harware commands. The performance-critical
+ * code can then copy these descriptors directly into hardware command
+ * registers more efficiently than trying to construct/format commands
+ * on-the-fly. The API user sees the descriptor as an array of 32-bit words in
+ * order for the compiler to know its size, but the internal details are not
+ * exposed. The following macro is used within the driver for converting *any*
+ * descriptor pointer to a usable array pointer. The use of a macro (instead of
+ * an inline) is necessary to work with different descriptor types and to work
+ * correctly with const and non-const inputs (and similarly-qualified outputs).
+ */
+#define qb_cl(d) (&(d)->dont_manipulate_directly[0])
diff --git a/drivers/net/fsl-mc/dpio/qbman_private.h b/drivers/net/fsl-mc/dpio/qbman_private.h
new file mode 100644
index 0000000..2d2556b
--- /dev/null
+++ b/drivers/net/fsl-mc/dpio/qbman_private.h
@@ -0,0 +1,169 @@
+/*
+ * Copyright (C) 2014 Freescale Semiconductor
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+/* Perform extra checking */
+#include <common.h>
+#include <errno.h>
+#include <asm/io.h>
+#include <linux/types.h>
+#include <linux/compat.h>
+#include <malloc.h>
+#include <fsl-mc/fsl_qbman_base.h>
+
+#define QBMAN_CHECKING
+
+/* Any time there is a register interface which we poll on, this provides a
+ * "break after x iterations" scheme for it. It's handy for debugging, eg.
+ * where you don't want millions of lines of log output from a polling loop
+ * that won't, because such things tend to drown out the earlier log output
+ * that might explain what caused the problem. (NB: put ";" after each macro!)
+ * TODO: we should probably remove this once we're done sanitising the
+ * simulator...
+ */
+#define DBG_POLL_START(loopvar) (loopvar = 10)
+#define DBG_POLL_CHECK(loopvar) \
+	do {if (!(loopvar--)) BUG_ON(NULL == "DBG_POLL_CHECK"); } while (0)
+
+/* For CCSR or portal-CINH registers that contain fields at arbitrary offsets
+ * and widths, these macro-generated encode/decode/isolate/remove inlines can
+ * be used.
+ *
+ * Eg. to "d"ecode a 14-bit field out of a register (into a "uint16_t" type),
+ * where the field is located 3 bits "up" from the least-significant bit of the
+ * register (ie. the field location within the 32-bit register corresponds to a
+ * mask of 0x0001fff8), you would do;
+ *                uint16_t field = d32_uint16_t(3, 14, reg_value);
+ *
+ * Or to "e"ncode a 1-bit boolean value (input type is "int", zero is FALSE,
+ * non-zero is TRUE, so must convert all non-zero inputs to 1, hence the "!!"
+ * operator) into a register at bit location 0x00080000 (19 bits "in" from the
+ * LS bit), do;
+ *                reg_value |= e32_int(19, 1, !!field);
+ *
+ * If you wish to read-modify-write a register, such that you leave the 14-bit
+ * field as-is but have all other fields set to zero, then "i"solate the 14-bit
+ * value using;
+ *                reg_value = i32_uint16_t(3, 14, reg_value);
+ *
+ * Alternatively, you could "r"emove the 1-bit boolean field (setting it to
+ * zero) but leaving all other fields as-is;
+ *                reg_val = r32_int(19, 1, reg_value);
+ *
+ */
+#define MAKE_MASK32(width) (width == 32 ? 0xffffffff : \
+				 (uint32_t)((1 << width) - 1))
+#define DECLARE_CODEC32(t) \
+static inline uint32_t e32_##t(uint32_t lsoffset, uint32_t width, t val) \
+{ \
+	BUG_ON(width > (sizeof(t) * 8)); \
+	return ((uint32_t)val & MAKE_MASK32(width)) << lsoffset; \
+} \
+static inline t d32_##t(uint32_t lsoffset, uint32_t width, uint32_t val) \
+{ \
+	BUG_ON(width > (sizeof(t) * 8)); \
+	return (t)((val >> lsoffset) & MAKE_MASK32(width)); \
+} \
+static inline uint32_t i32_##t(uint32_t lsoffset, uint32_t width, \
+				uint32_t val) \
+{ \
+	BUG_ON(width > (sizeof(t) * 8)); \
+	return e32_##t(lsoffset, width, d32_##t(lsoffset, width, val)); \
+} \
+static inline uint32_t r32_##t(uint32_t lsoffset, uint32_t width, \
+				uint32_t val) \
+{ \
+	BUG_ON(width > (sizeof(t) * 8)); \
+	return ~(MAKE_MASK32(width) << lsoffset) & val; \
+}
+DECLARE_CODEC32(uint32_t)
+DECLARE_CODEC32(uint16_t)
+DECLARE_CODEC32(uint8_t)
+DECLARE_CODEC32(int)
+
+	/*********************/
+	/* Debugging assists */
+	/*********************/
+
+static inline void __hexdump(unsigned long start, unsigned long end,
+			unsigned long p, size_t sz, const unsigned char *c)
+{
+	while (start < end) {
+		unsigned int pos = 0;
+		char buf[64];
+		int nl = 0;
+
+		pos += sprintf(buf + pos, "%08lx: ", start);
+		do {
+			if ((start < p) || (start >= (p + sz)))
+				pos += sprintf(buf + pos, "..");
+			else
+				pos += sprintf(buf + pos, "%02x", *(c++));
+			if (!(++start & 15)) {
+				buf[pos++] = '\n';
+				nl = 1;
+			} else {
+				nl = 0;
+				if (!(start & 1))
+					buf[pos++] = ' ';
+				if (!(start & 3))
+					buf[pos++] = ' ';
+			}
+		} while (start & 15);
+		if (!nl)
+			buf[pos++] = '\n';
+		buf[pos] = '\0';
+		debug("%s", buf);
+	}
+}
+static inline void hexdump(const void *ptr, size_t sz)
+{
+	unsigned long p = (unsigned long)ptr;
+	unsigned long start = p & ~(unsigned long)15;
+	unsigned long end = (p + sz + 15) & ~(unsigned long)15;
+	const unsigned char *c = ptr;
+
+	__hexdump(start, end, p, sz, c);
+}
+
+#if defined(__BIG_ENDIAN)
+#define DQRR_TOK_OFFSET 0
+#else
+#define DQRR_TOK_OFFSET 24
+#endif
+
+/* Similarly-named functions */
+#define upper32(a) upper_32_bits(a)
+#define lower32(a) lower_32_bits(a)
+
+	/****************/
+	/* arch assists */
+	/****************/
+
+static inline void dcbz(void *ptr)
+{
+	uint32_t *p = ptr;
+	BUG_ON((unsigned long)ptr & 63);
+	p[0] = 0;
+	p[1] = 0;
+	p[2] = 0;
+	p[3] = 0;
+	p[4] = 0;
+	p[5] = 0;
+	p[6] = 0;
+	p[7] = 0;
+	p[8] = 0;
+	p[9] = 0;
+	p[10] = 0;
+	p[11] = 0;
+	p[12] = 0;
+	p[13] = 0;
+	p[14] = 0;
+	p[15] = 0;
+}
+
+#define lwsync()
+
+#include "qbman_sys.h"
diff --git a/drivers/net/fsl-mc/dpio/qbman_sys.h b/drivers/net/fsl-mc/dpio/qbman_sys.h
new file mode 100644
index 0000000..235d641
--- /dev/null
+++ b/drivers/net/fsl-mc/dpio/qbman_sys.h
@@ -0,0 +1,290 @@
+/*
+ * Copyright (C) 2014 Freescale Semiconductor
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+/* qbman_sys_decl.h and qbman_sys.h are the two platform-specific files in the
+ * driver. They are only included via qbman_private.h, which is itself a
+ * platform-independent file and is included by all the other driver source.
+ *
+ * qbman_sys_decl.h is included prior to all other declarations and logic, and
+ * it exists to provide compatibility with any linux interfaces our
+ * single-source driver code is dependent on (eg. kmalloc). Ie. this file
+ * provides linux compatibility.
+ *
+ * This qbman_sys.h header, on the other hand, is included *after* any common
+ * and platform-neutral declarations and logic in qbman_private.h, and exists to
+ * implement any platform-specific logic of the qbman driver itself. Ie. it is
+ * *not* to provide linux compatibility.
+ */
+
+/* Trace the 3 different classes of read/write access to QBMan. #undef as
+ * required. */
+#undef QBMAN_CCSR_TRACE
+#undef QBMAN_CINH_TRACE
+#undef QBMAN_CENA_TRACE
+
+/* Temporarily define this to get around the fact that cache enabled mapping is
+ * not working right now. Will remove this after uboot could map the cache
+ * enabled portal memory.
+ */
+#define QBMAN_CINH_ONLY
+
+static inline void word_copy(void *d, const void *s, unsigned int cnt)
+{
+	uint32_t *dd = d;
+	const uint32_t *ss = s;
+
+	while (cnt--)
+		*(dd++) = *(ss++);
+}
+
+/* Currently, the CENA support code expects each 32-bit word to be written in
+ * host order, and these are converted to hardware (little-endian) order on
+ * command submission. However, 64-bit quantities are must be written (and read)
+ * as two 32-bit words with the least-significant word first, irrespective of
+ * host endianness. */
+static inline void u64_to_le32_copy(void *d, const uint64_t *s,
+					unsigned int cnt)
+{
+	uint32_t *dd = d;
+	const uint32_t *ss = (const uint32_t *)s;
+
+	while (cnt--) {
+		/* TBD: the toolchain was choking on the use of 64-bit types up
+		 * until recently so this works entirely with 32-bit variables.
+		 * When 64-bit types become usable again, investigate better
+		 * ways of doing this. */
+#if defined(__BIG_ENDIAN)
+		*(dd++) = ss[1];
+		*(dd++) = ss[0];
+		ss += 2;
+#else
+		*(dd++) = *(ss++);
+		*(dd++) = *(ss++);
+#endif
+	}
+}
+static inline void u64_from_le32_copy(uint64_t *d, const void *s,
+					unsigned int cnt)
+{
+	const uint32_t *ss = s;
+	uint32_t *dd = (uint32_t *)d;
+
+	while (cnt--) {
+#if defined(__BIG_ENDIAN)
+		dd[1] = *(ss++);
+		dd[0] = *(ss++);
+		dd += 2;
+#else
+		*(dd++) = *(ss++);
+		*(dd++) = *(ss++);
+#endif
+	}
+}
+
+/* Convert a host-native 32bit value into little endian */
+#if defined(__BIG_ENDIAN)
+static inline uint32_t make_le32(uint32_t val)
+{
+	return ((val & 0xff) << 24) | ((val & 0xff00) << 8) |
+		((val & 0xff0000) >> 8) | ((val & 0xff000000) >> 24);
+}
+#else
+#define make_le32(val) (val)
+#endif
+static inline void make_le32_n(uint32_t *val, unsigned int num)
+{
+	while (num--) {
+		*val = make_le32(*val);
+		val++;
+	}
+}
+
+	/******************/
+	/* Portal access  */
+	/******************/
+struct qbman_swp_sys {
+	/* On GPP, the sys support for qbman_swp is here. The CENA region isi
+	 * not an mmap() of the real portal registers, but an allocated
+	 * place-holder, because the actual writes/reads to/from the portal are
+	 * marshalled from these allocated areas using QBMan's "MC access
+	 * registers". CINH accesses are atomic so there's no need for a
+	 * place-holder. */
+	void *cena;
+	void __iomem *addr_cena;
+	void __iomem *addr_cinh;
+};
+
+/* P_OFFSET is (ACCESS_CMD,0,12) - offset within the portal
+ * C is (ACCESS_CMD,12,1) - is inhibited? (0==CENA, 1==CINH)
+ * SWP_IDX is (ACCESS_CMD,16,10) - Software portal index
+ * P is (ACCESS_CMD,28,1) - (0==special portal, 1==any portal)
+ * T is (ACCESS_CMD,29,1) - Command type (0==READ, 1==WRITE)
+ * E is (ACCESS_CMD,31,1) - Command execute (1 to issue, poll for 0==complete)
+ */
+
+static inline void qbman_cinh_write(struct qbman_swp_sys *s, uint32_t offset,
+				    uint32_t val)
+{
+	__raw_writel(val, s->addr_cinh + offset);
+#ifdef QBMAN_CINH_TRACE
+	pr_info("qbman_cinh_write(%p:0x%03x) 0x%08x\n",
+		s->addr_cinh, offset, val);
+#endif
+}
+
+static inline uint32_t qbman_cinh_read(struct qbman_swp_sys *s, uint32_t offset)
+{
+	uint32_t reg = __raw_readl(s->addr_cinh + offset);
+
+#ifdef QBMAN_CINH_TRACE
+	pr_info("qbman_cinh_read(%p:0x%03x) 0x%08x\n",
+		s->addr_cinh, offset, reg);
+#endif
+	return reg;
+}
+
+static inline void *qbman_cena_write_start(struct qbman_swp_sys *s,
+						uint32_t offset)
+{
+	void *shadow = s->cena + offset;
+
+#ifdef QBMAN_CENA_TRACE
+	pr_info("qbman_cena_write_start(%p:0x%03x) %p\n",
+		s->addr_cena, offset, shadow);
+#endif
+	BUG_ON(offset & 63);
+	dcbz(shadow);
+	return shadow;
+}
+
+static inline void qbman_cena_write_complete(struct qbman_swp_sys *s,
+						uint32_t offset, void *cmd)
+{
+	const uint32_t *shadow = cmd;
+	int loop;
+
+#ifdef QBMAN_CENA_TRACE
+	pr_info("qbman_cena_write_complete(%p:0x%03x) %p\n",
+		s->addr_cena, offset, shadow);
+	hexdump(cmd, 64);
+#endif
+	for (loop = 15; loop >= 0; loop--)
+#ifdef QBMAN_CINH_ONLY
+		__raw_writel(shadow[loop], s->addr_cinh +
+					 offset + loop * 4);
+#else
+		__raw_writel(shadow[loop], s->addr_cena +
+					 offset + loop * 4);
+#endif
+}
+
+static inline void *qbman_cena_read(struct qbman_swp_sys *s, uint32_t offset)
+{
+	uint32_t *shadow = s->cena + offset;
+	unsigned int loop;
+
+#ifdef QBMAN_CENA_TRACE
+	pr_info("qbman_cena_read(%p:0x%03x) %p\n",
+		s->addr_cena, offset, shadow);
+#endif
+
+	for (loop = 0; loop < 16; loop++)
+#ifdef QBMAN_CINH_ONLY
+		shadow[loop] = __raw_readl(s->addr_cinh + offset
+					+ loop * 4);
+#else
+		shadow[loop] = __raw_readl(s->addr_cena + offset
+					+ loop * 4);
+#endif
+#ifdef QBMAN_CENA_TRACE
+	hexdump(shadow, 64);
+#endif
+	return shadow;
+}
+
+static inline void qbman_cena_invalidate_prefetch(struct qbman_swp_sys *s,
+						  uint32_t offset)
+{
+}
+
+	/******************/
+	/* Portal support */
+	/******************/
+
+/* The SWP_CFG portal register is special, in that it is used by the
+ * platform-specific code rather than the platform-independent code in
+ * qbman_portal.c. So use of it is declared locally here. */
+#define QBMAN_CINH_SWP_CFG   0xd00
+
+/* For MC portal use, we always configure with
+ * DQRR_MF is (SWP_CFG,20,3) - DQRR max fill (<- 0x4)
+ * EST is (SWP_CFG,16,3) - EQCR_CI stashing threshold (<- 0x0)
+ * RPM is (SWP_CFG,12,2) - RCR production notification mode (<- 0x3)
+ * DCM is (SWP_CFG,10,2) - DQRR consumption notification mode (<- 0x2)
+ * EPM is (SWP_CFG,8,2) - EQCR production notification mode (<- 0x3)
+ * SD is (SWP_CFG,5,1) - memory stashing drop enable (<- FALSE)
+ * SP is (SWP_CFG,4,1) - memory stashing priority (<- TRUE)
+ * SE is (SWP_CFG,3,1) - memory stashing enable (<- 0x0)
+ * DP is (SWP_CFG,2,1) - dequeue stashing priority (<- TRUE)
+ * DE is (SWP_CFG,1,1) - dequeue stashing enable (<- 0x0)
+ * EP is (SWP_CFG,0,1) - EQCR_CI stashing priority (<- FALSE)
+ */
+static inline uint32_t qbman_set_swp_cfg(uint8_t max_fill, uint8_t wn,
+					uint8_t est, uint8_t rpm, uint8_t dcm,
+					uint8_t epm, int sd, int sp, int se,
+					int dp, int de, int ep)
+{
+	uint32_t reg;
+
+	reg = e32_uint8_t(20, 3, max_fill) | e32_uint8_t(16, 3, est) |
+		e32_uint8_t(12, 2, rpm) | e32_uint8_t(10, 2, dcm) |
+		e32_uint8_t(8, 2, epm) | e32_int(5, 1, sd) |
+		e32_int(4, 1, sp) | e32_int(3, 1, se) | e32_int(2, 1, dp) |
+		e32_int(1, 1, de) | e32_int(0, 1, ep) |	e32_uint8_t(14, 1, wn);
+	return reg;
+}
+
+static inline int qbman_swp_sys_init(struct qbman_swp_sys *s,
+				     const struct qbman_swp_desc *d)
+{
+	uint32_t reg;
+
+	s->addr_cena = d->cena_bar;
+	s->addr_cinh = d->cinh_bar;
+	s->cena = (void *)valloc(CONFIG_SYS_PAGE_SIZE);
+	memset((void *)s->cena, 0x00, CONFIG_SYS_PAGE_SIZE);
+	if (!s->cena) {
+		printf("Could not allocate page for cena shadow\n");
+		return -1;
+	}
+
+#ifdef QBMAN_CHECKING
+	/* We should never be asked to initialise for a portal that isn't in
+	 * the power-on state. (Ie. don't forget to reset portals when they are
+	 * decommissioned!)
+	 */
+	reg = qbman_cinh_read(s, QBMAN_CINH_SWP_CFG);
+	BUG_ON(reg);
+#endif
+#ifdef QBMAN_CINH_ONLY
+	reg = qbman_set_swp_cfg(4, 1, 0, 3, 2, 3, 0, 1, 0, 1, 0, 0);
+#else
+	reg = qbman_set_swp_cfg(4, 0, 0, 3, 2, 3, 0, 1, 0, 1, 0, 0);
+#endif
+	qbman_cinh_write(s, QBMAN_CINH_SWP_CFG, reg);
+	reg = qbman_cinh_read(s, QBMAN_CINH_SWP_CFG);
+	if (!reg) {
+		printf("The portal is not enabled!\n");
+		free(s->cena);
+		return -1;
+	}
+	return 0;
+}
+
+static inline void qbman_swp_sys_finish(struct qbman_swp_sys *s)
+{
+	free((void *)s->cena);
+}
diff --git a/drivers/net/fsl-mc/dpmng.c b/drivers/net/fsl-mc/dpmng.c
index cc14c7b..01ee112 100644
--- a/drivers/net/fsl-mc/dpmng.c
+++ b/drivers/net/fsl-mc/dpmng.c
@@ -1,4 +1,4 @@
-/* Copyright 2014 Freescale Semiconductor Inc.
+/* Copyright 2013-2015 Freescale Semiconductor Inc.
  *
  * SPDX-License-Identifier:	GPL-2.0+
  */
@@ -26,66 +26,3 @@ int mc_get_version(struct fsl_mc_io *mc_io, struct mc_version *mc_ver_info)
 
 	return 0;
 }
-
-int dpmng_reset_aiop(struct fsl_mc_io *mc_io, int container_id,
-		     int aiop_tile_id)
-{
-	struct mc_command cmd = { 0 };
-
-	/* prepare command */
-	cmd.header = mc_encode_cmd_header(DPMNG_CMDID_RESET_AIOP,
-					  MC_CMD_PRI_LOW, 0);
-	DPMNG_CMD_RESET_AIOP(cmd, container_id, aiop_tile_id);
-
-	/* send command to mc*/
-	return mc_send_command(mc_io, &cmd);
-}
-
-int dpmng_load_aiop(struct fsl_mc_io *mc_io,
-		    int container_id,
-		    int aiop_tile_id,
-		    uint64_t img_iova,
-		    uint32_t img_size)
-{
-	struct mc_command cmd = { 0 };
-
-	/* prepare command */
-	cmd.header = mc_encode_cmd_header(DPMNG_CMDID_LOAD_AIOP,
-					  MC_CMD_PRI_LOW,
-					  0);
-	DPMNG_CMD_LOAD_AIOP(cmd, container_id, aiop_tile_id, img_size,
-			    img_iova);
-
-	/* send command to mc*/
-	return mc_send_command(mc_io, &cmd);
-}
-
-int dpmng_run_aiop(struct fsl_mc_io *mc_io,
-		   int container_id,
-		   int aiop_tile_id,
-		   const struct dpmng_aiop_run_cfg *cfg)
-{
-	struct mc_command cmd = { 0 };
-
-	/* prepare command */
-	cmd.header = mc_encode_cmd_header(DPMNG_CMDID_RUN_AIOP,
-					  MC_CMD_PRI_LOW,
-					  0);
-	DPMNG_CMD_RUN_AIOP(cmd, container_id, aiop_tile_id, cfg);
-
-	/* send command to mc*/
-	return mc_send_command(mc_io, &cmd);
-}
-
-int dpmng_reset_mc_portal(struct fsl_mc_io *mc_io)
-{
-	struct mc_command cmd = { 0 };
-
-	/* prepare command */
-	cmd.header = mc_encode_cmd_header(DPMNG_CMDID_RESET_MC_PORTAL,
-					  MC_CMD_PRI_LOW,
-					  0);
-
-	/* send command to mc*/
-	return mc_send_command(mc_io, &cmd);
-}
diff --git a/drivers/net/fsl-mc/dpni.c b/drivers/net/fsl-mc/dpni.c
new file mode 100644
index 0000000..b384401
--- /dev/null
+++ b/drivers/net/fsl-mc/dpni.c
@@ -0,0 +1,506 @@
+/*
+ * Copyright (C) 2013-2015 Freescale Semiconductor
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <fsl-mc/fsl_mc_sys.h>
+#include <fsl-mc/fsl_mc_cmd.h>
+#include <fsl-mc/fsl_dpni.h>
+
+int dpni_open(struct fsl_mc_io *mc_io, int dpni_id, uint16_t *token)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_OPEN,
+					  MC_CMD_PRI_LOW, 0);
+	DPNI_CMD_OPEN(cmd, dpni_id);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	*token = MC_CMD_HDR_READ_TOKEN(cmd.header);
+
+	return 0;
+}
+
+int dpni_close(struct fsl_mc_io *mc_io, uint16_t token)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_CLOSE,
+					  MC_CMD_PRI_HIGH, token);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+int dpni_set_pools(struct fsl_mc_io *mc_io,
+		   uint16_t token,
+		   const struct dpni_pools_cfg *cfg)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_SET_POOLS,
+					  MC_CMD_PRI_LOW,
+					  token);
+	DPNI_CMD_SET_POOLS(cmd, cfg);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+int dpni_enable(struct fsl_mc_io *mc_io, uint16_t token)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_ENABLE,
+					  MC_CMD_PRI_LOW, token);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+int dpni_disable(struct fsl_mc_io *mc_io, uint16_t token)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_DISABLE,
+					  MC_CMD_PRI_LOW,
+					  token);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+int dpni_reset(struct fsl_mc_io *mc_io, uint16_t token)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_RESET,
+					  MC_CMD_PRI_LOW, token);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+int dpni_get_attributes(struct fsl_mc_io *mc_io,
+			uint16_t token,
+			struct dpni_attr *attr)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_GET_ATTR,
+					  MC_CMD_PRI_LOW,
+					  token);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	DPNI_RSP_GET_ATTR(cmd, attr);
+
+	return 0;
+}
+
+int dpni_get_rx_buffer_layout(struct fsl_mc_io *mc_io,
+			      uint16_t token,
+			      struct dpni_buffer_layout *layout)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_GET_RX_BUFFER_LAYOUT,
+					  MC_CMD_PRI_LOW, token);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	DPNI_RSP_GET_RX_BUFFER_LAYOUT(cmd, layout);
+
+	return 0;
+}
+
+int dpni_set_rx_buffer_layout(struct fsl_mc_io *mc_io,
+			      uint16_t token,
+			      const struct dpni_buffer_layout *layout)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_SET_RX_BUFFER_LAYOUT,
+					  MC_CMD_PRI_LOW, token);
+	DPNI_CMD_SET_RX_BUFFER_LAYOUT(cmd, layout);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+int dpni_get_tx_buffer_layout(struct fsl_mc_io *mc_io,
+			      uint16_t token,
+			      struct dpni_buffer_layout *layout)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_GET_TX_BUFFER_LAYOUT,
+					  MC_CMD_PRI_LOW, token);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	DPNI_RSP_GET_TX_BUFFER_LAYOUT(cmd, layout);
+
+	return 0;
+}
+
+int dpni_set_tx_buffer_layout(struct fsl_mc_io *mc_io,
+			      uint16_t token,
+			      const struct dpni_buffer_layout *layout)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_SET_TX_BUFFER_LAYOUT,
+					  MC_CMD_PRI_LOW, token);
+	DPNI_CMD_SET_TX_BUFFER_LAYOUT(cmd, layout);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+int dpni_get_tx_conf_buffer_layout(struct fsl_mc_io *mc_io,
+				   uint16_t token,
+				   struct dpni_buffer_layout *layout)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_GET_TX_CONF_BUFFER_LAYOUT,
+					  MC_CMD_PRI_LOW, token);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	DPNI_RSP_GET_TX_CONF_BUFFER_LAYOUT(cmd, layout);
+
+	return 0;
+}
+
+int dpni_set_tx_conf_buffer_layout(struct fsl_mc_io *mc_io,
+				   uint16_t token,
+				   const struct dpni_buffer_layout *layout)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_SET_TX_CONF_BUFFER_LAYOUT,
+					  MC_CMD_PRI_LOW, token);
+	DPNI_CMD_SET_TX_CONF_BUFFER_LAYOUT(cmd, layout);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+int dpni_get_qdid(struct fsl_mc_io *mc_io, uint16_t token, uint16_t *qdid)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_GET_QDID,
+					  MC_CMD_PRI_LOW,
+					  token);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	DPNI_RSP_GET_QDID(cmd, *qdid);
+
+	return 0;
+}
+
+int dpni_get_tx_data_offset(struct fsl_mc_io *mc_io,
+			    uint16_t token,
+			    uint16_t *data_offset)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_GET_TX_DATA_OFFSET,
+					  MC_CMD_PRI_LOW, token);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	DPNI_RSP_GET_TX_DATA_OFFSET(cmd, *data_offset);
+
+	return 0;
+}
+
+int dpni_get_counter(struct fsl_mc_io *mc_io,
+		     uint16_t token,
+		     enum dpni_counter counter,
+		     uint64_t *value)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_GET_COUNTER,
+					  MC_CMD_PRI_LOW, token);
+	DPNI_CMD_GET_COUNTER(cmd, counter);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	DPNI_RSP_GET_COUNTER(cmd, *value);
+
+	return 0;
+}
+
+int dpni_set_counter(struct fsl_mc_io *mc_io,
+		     uint16_t token,
+		     enum dpni_counter counter,
+		     uint64_t value)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_SET_COUNTER,
+					  MC_CMD_PRI_LOW, token);
+	DPNI_CMD_SET_COUNTER(cmd, counter, value);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+int dpni_set_link_cfg(struct fsl_mc_io *mc_io,
+		      uint16_t token,
+		     struct dpni_link_cfg *cfg)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_SET_LINK_CFG,
+					  MC_CMD_PRI_LOW, token);
+	DPNI_CMD_SET_LINK_CFG(cmd, cfg);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+int dpni_get_link_state(struct fsl_mc_io *mc_io,
+			uint16_t token,
+			struct dpni_link_state *state)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_GET_LINK_STATE,
+					  MC_CMD_PRI_LOW, token);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	DPNI_RSP_GET_LINK_STATE(cmd, state);
+
+	return 0;
+}
+
+
+int dpni_set_primary_mac_addr(struct fsl_mc_io *mc_io,
+			      uint16_t token,
+			      const uint8_t mac_addr[6])
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_SET_PRIM_MAC,
+					  MC_CMD_PRI_LOW, token);
+	DPNI_CMD_SET_PRIMARY_MAC_ADDR(cmd, mac_addr);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+int dpni_get_primary_mac_addr(struct fsl_mc_io *mc_io,
+			      uint16_t token,
+			      uint8_t mac_addr[6])
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_GET_PRIM_MAC,
+					  MC_CMD_PRI_LOW, token);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	DPNI_RSP_GET_PRIMARY_MAC_ADDR(cmd, mac_addr);
+
+	return 0;
+}
+
+int dpni_add_mac_addr(struct fsl_mc_io *mc_io,
+		      uint16_t token,
+		      const uint8_t mac_addr[6])
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_ADD_MAC_ADDR,
+					  MC_CMD_PRI_LOW, token);
+	DPNI_CMD_ADD_MAC_ADDR(cmd, mac_addr);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+int dpni_remove_mac_addr(struct fsl_mc_io *mc_io,
+			 uint16_t token,
+			 const uint8_t mac_addr[6])
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_REMOVE_MAC_ADDR,
+					  MC_CMD_PRI_LOW, token);
+	DPNI_CMD_REMOVE_MAC_ADDR(cmd, mac_addr);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+int dpni_set_tx_flow(struct fsl_mc_io *mc_io,
+		     uint16_t token,
+		     uint16_t *flow_id,
+		     const struct dpni_tx_flow_cfg *cfg)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_SET_TX_FLOW,
+					  MC_CMD_PRI_LOW, token);
+	DPNI_CMD_SET_TX_FLOW(cmd, *flow_id, cfg);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	DPNI_RSP_SET_TX_FLOW(cmd, *flow_id);
+
+	return 0;
+}
+
+int dpni_get_tx_flow(struct fsl_mc_io *mc_io,
+		     uint16_t token,
+		     uint16_t flow_id,
+		     struct dpni_tx_flow_attr *attr)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_GET_TX_FLOW,
+					  MC_CMD_PRI_LOW, token);
+	DPNI_CMD_GET_TX_FLOW(cmd, flow_id);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	DPNI_RSP_GET_TX_FLOW(cmd, attr);
+
+	return 0;
+}
+
+int dpni_set_rx_flow(struct fsl_mc_io *mc_io,
+		     uint16_t token,
+		     uint8_t tc_id,
+		     uint16_t flow_id,
+		     const struct dpni_queue_cfg *cfg)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_SET_RX_FLOW,
+					  MC_CMD_PRI_LOW, token);
+	DPNI_CMD_SET_RX_FLOW(cmd, tc_id, flow_id, cfg);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+int dpni_get_rx_flow(struct fsl_mc_io *mc_io,
+		     uint16_t token,
+		     uint8_t tc_id,
+		     uint16_t flow_id,
+		     struct dpni_queue_attr *attr)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_GET_RX_FLOW,
+					  MC_CMD_PRI_LOW, token);
+	DPNI_CMD_GET_RX_FLOW(cmd, tc_id, flow_id);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	DPNI_RSP_GET_RX_FLOW(cmd, attr);
+
+	return 0;
+}
diff --git a/drivers/net/fsl-mc/dprc.c b/drivers/net/fsl-mc/dprc.c
new file mode 100644
index 0000000..d481200
--- /dev/null
+++ b/drivers/net/fsl-mc/dprc.c
@@ -0,0 +1,283 @@
+/*
+ * Freescale Layerscape MC I/O wrapper
+ *
+ * Copyright (C) 2013-2015 Freescale Semiconductor, Inc.
+ * Author: German Rivera <German.Rivera@freescale.com>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <fsl-mc/fsl_mc_sys.h>
+#include <fsl-mc/fsl_mc_cmd.h>
+#include <fsl-mc/fsl_dprc.h>
+
+int dprc_get_container_id(struct fsl_mc_io *mc_io, int *container_id)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPRC_CMDID_GET_CONT_ID,
+					  MC_CMD_PRI_LOW, 0);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	DPRC_RSP_GET_CONTAINER_ID(cmd, *container_id);
+
+	return 0;
+}
+
+int dprc_open(struct fsl_mc_io *mc_io, int container_id, uint16_t *token)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPRC_CMDID_OPEN, MC_CMD_PRI_LOW,
+					  0);
+	DPRC_CMD_OPEN(cmd, container_id);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	*token = MC_CMD_HDR_READ_TOKEN(cmd.header);
+
+	return 0;
+}
+
+int dprc_close(struct fsl_mc_io *mc_io, uint16_t token)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPRC_CMDID_CLOSE, MC_CMD_PRI_HIGH,
+					  token);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+int dprc_reset_container(struct fsl_mc_io *mc_io,
+			 uint16_t token,
+			 int child_container_id)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPRC_CMDID_RESET_CONT,
+					  MC_CMD_PRI_LOW, token);
+	DPRC_CMD_RESET_CONTAINER(cmd, child_container_id);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+int dprc_get_attributes(struct fsl_mc_io *mc_io,
+			uint16_t token,
+			struct dprc_attributes *attr)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPRC_CMDID_GET_ATTR,
+					  MC_CMD_PRI_LOW,
+					  token);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	DPRC_RSP_GET_ATTRIBUTES(cmd, attr);
+
+	return 0;
+}
+
+int dprc_get_obj_count(struct fsl_mc_io *mc_io, uint16_t token, int *obj_count)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPRC_CMDID_GET_OBJ_COUNT,
+					  MC_CMD_PRI_LOW, token);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	DPRC_RSP_GET_OBJ_COUNT(cmd, *obj_count);
+
+	return 0;
+}
+
+int dprc_get_obj(struct fsl_mc_io *mc_io,
+		 uint16_t token,
+		 int obj_index,
+		 struct dprc_obj_desc *obj_desc)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPRC_CMDID_GET_OBJ,
+					  MC_CMD_PRI_LOW,
+					  token);
+	DPRC_CMD_GET_OBJ(cmd, obj_index);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	DPRC_RSP_GET_OBJ(cmd, obj_desc);
+
+	return 0;
+}
+
+int dprc_get_res_count(struct fsl_mc_io *mc_io,
+		       uint16_t token,
+		       char *type,
+		       int *res_count)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	*res_count = 0;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPRC_CMDID_GET_RES_COUNT,
+					  MC_CMD_PRI_LOW, token);
+	DPRC_CMD_GET_RES_COUNT(cmd, type);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	DPRC_RSP_GET_RES_COUNT(cmd, *res_count);
+
+	return 0;
+}
+
+int dprc_get_res_ids(struct fsl_mc_io *mc_io,
+		     uint16_t token,
+		     char *type,
+		     struct dprc_res_ids_range_desc *range_desc)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPRC_CMDID_GET_RES_IDS,
+					  MC_CMD_PRI_LOW, token);
+	DPRC_CMD_GET_RES_IDS(cmd, range_desc, type);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	DPRC_RSP_GET_RES_IDS(cmd, range_desc);
+
+	return 0;
+}
+
+int dprc_get_obj_region(struct fsl_mc_io *mc_io,
+			uint16_t token,
+			char *obj_type,
+			int obj_id,
+			uint8_t region_index,
+			struct dprc_region_desc *region_desc)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPRC_CMDID_GET_OBJ_REG,
+					  MC_CMD_PRI_LOW, token);
+	DPRC_CMD_GET_OBJ_REGION(cmd, obj_type, obj_id, region_index);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	DPRC_RSP_GET_OBJ_REGION(cmd, region_desc);
+
+	return 0;
+}
+
+int dprc_connect(struct fsl_mc_io *mc_io,
+		 uint16_t token,
+		 const struct dprc_endpoint *endpoint1,
+		 const struct dprc_endpoint *endpoint2)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPRC_CMDID_CONNECT,
+					  MC_CMD_PRI_LOW,
+					  token);
+	DPRC_CMD_CONNECT(cmd, endpoint1, endpoint2);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+int dprc_disconnect(struct fsl_mc_io *mc_io,
+		    uint16_t token,
+		    const struct dprc_endpoint *endpoint)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPRC_CMDID_DISCONNECT,
+					  MC_CMD_PRI_LOW,
+					  token);
+	DPRC_CMD_DISCONNECT(cmd, endpoint);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+int dprc_get_connection(struct fsl_mc_io *mc_io,
+			uint16_t token,
+					const struct dprc_endpoint *endpoint1,
+					struct dprc_endpoint *endpoint2,
+					int *state)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPRC_CMDID_GET_CONNECTION,
+					  MC_CMD_PRI_LOW,
+					  token);
+	DPRC_CMD_GET_CONNECTION(cmd, endpoint1);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	DPRC_RSP_GET_CONNECTION(cmd, endpoint2, *state);
+
+	return 0;
+}
diff --git a/drivers/net/fsl-mc/fsl_dpmng_cmd.h b/drivers/net/fsl-mc/fsl_dpmng_cmd.h
index c9fe021..33f84f3 100644
--- a/drivers/net/fsl-mc/fsl_dpmng_cmd.h
+++ b/drivers/net/fsl-mc/fsl_dpmng_cmd.h
@@ -1,4 +1,4 @@
-/* Copyright 2014 Freescale Semiconductor Inc.
+/* Copyright 2013-2015 Freescale Semiconductor Inc.
  *
  * SPDX-License-Identifier:	GPL-2.0+
  */
@@ -7,10 +7,6 @@
 
 /* Command IDs */
 #define DPMNG_CMDID_GET_VERSION			0x831
-#define DPMNG_CMDID_RESET_AIOP			0x832
-#define DPMNG_CMDID_LOAD_AIOP			0x833
-#define DPMNG_CMDID_RUN_AIOP			0x834
-#define DPMNG_CMDID_RESET_MC_PORTAL		0x835
 
 /*                cmd, param, offset, width, type, arg_name */
 #define DPMNG_RSP_GET_VERSION(cmd, mc_ver_info) \
@@ -20,30 +16,4 @@ do { \
 	MC_RSP_OP(cmd, 1, 0,  32, uint32_t, mc_ver_info->minor); \
 } while (0)
 
-/*                cmd, param, offset, width, type, arg_name */
-#define DPMNG_CMD_RESET_AIOP(cmd, container_id, aiop_tile_id) \
-do { \
-	MC_CMD_OP(cmd, 0, 0,  32, int,      aiop_tile_id); \
-	MC_CMD_OP(cmd, 0, 32, 32, int,      container_id); \
-} while (0)
-
-/*                cmd, param, offset, width, type, arg_name */
-#define DPMNG_CMD_LOAD_AIOP(cmd, container_id, aiop_tile_id, img_size, \
-			    img_iova) \
-do { \
-	MC_CMD_OP(cmd, 0, 0,  32, int,      aiop_tile_id); \
-	MC_CMD_OP(cmd, 0, 32, 32, int,      container_id); \
-	MC_CMD_OP(cmd, 1, 0,  32, uint32_t, img_size); \
-	MC_CMD_OP(cmd, 2, 0,  64, uint64_t, img_iova); \
-} while (0)
-
-/*                cmd, param, offset, width, type, arg_name */
-#define DPMNG_CMD_RUN_AIOP(cmd, container_id, aiop_tile_id, cfg) \
-do { \
-	MC_CMD_OP(cmd, 0, 0,  32, int,	    aiop_tile_id); \
-	MC_CMD_OP(cmd, 0, 32, 32, int,      container_id); \
-	MC_CMD_OP(cmd, 1, 0,  32, uint32_t, cfg->cores_mask); \
-	MC_CMD_OP(cmd, 2, 0,  64, uint64_t, cfg->options); \
-} while (0)
-
 #endif /* __FSL_DPMNG_CMD_H */
diff --git a/drivers/net/fsl-mc/mc.c b/drivers/net/fsl-mc/mc.c
index 74b0085..c5c44bc 100644
--- a/drivers/net/fsl-mc/mc.c
+++ b/drivers/net/fsl-mc/mc.c
@@ -3,16 +3,75 @@
  *
  * SPDX-License-Identifier:	GPL-2.0+
  */
-
 #include <errno.h>
 #include <asm/io.h>
 #include <fsl-mc/fsl_mc.h>
 #include <fsl-mc/fsl_mc_sys.h>
+#include <fsl-mc/fsl_mc_private.h>
 #include <fsl-mc/fsl_dpmng.h>
+#include <fsl_debug_server.h>
+#include <fsl-mc/fsl_dprc.h>
+#include <fsl-mc/fsl_dpio.h>
+#include <fsl-mc/fsl_qbman_portal.h>
+
+#define MC_RAM_BASE_ADDR_ALIGNMENT  (512UL * 1024 * 1024)
+#define MC_RAM_BASE_ADDR_ALIGNMENT_MASK	(~(MC_RAM_BASE_ADDR_ALIGNMENT - 1))
+#define MC_RAM_SIZE_ALIGNMENT	    (256UL * 1024 * 1024)
+
+#define MC_MEM_SIZE_ENV_VAR	"mcmemsize"
+#define MC_BOOT_TIMEOUT_ENV_VAR	"mcboottimeout"
 
 DECLARE_GLOBAL_DATA_PTR;
 static int mc_boot_status;
+struct fsl_mc_io *dflt_mc_io = NULL;
+uint16_t dflt_dprc_handle = 0;
+struct fsl_dpbp_obj *dflt_dpbp = NULL;
+struct fsl_dpio_obj *dflt_dpio = NULL;
+uint16_t dflt_dpio_handle = 0;
+
+#ifdef DEBUG
+void dump_ram_words(const char *title, void *addr)
+{
+	int i;
+	uint32_t *words = addr;
+
+	printf("Dumping beginning of %s (%p):\n", title, addr);
+	for (i = 0; i < 16; i++)
+		printf("%#x ", words[i]);
 
+	printf("\n");
+}
+
+void dump_mc_ccsr_regs(struct mc_ccsr_registers __iomem *mc_ccsr_regs)
+{
+	printf("MC CCSR registers:\n"
+		"reg_gcr1 %#x\n"
+		"reg_gsr %#x\n"
+		"reg_sicbalr %#x\n"
+		"reg_sicbahr %#x\n"
+		"reg_sicapr %#x\n"
+		"reg_mcfbalr %#x\n"
+		"reg_mcfbahr %#x\n"
+		"reg_mcfapr %#x\n"
+		"reg_psr %#x\n",
+		mc_ccsr_regs->reg_gcr1,
+		mc_ccsr_regs->reg_gsr,
+		mc_ccsr_regs->reg_sicbalr,
+		mc_ccsr_regs->reg_sicbahr,
+		mc_ccsr_regs->reg_sicapr,
+		mc_ccsr_regs->reg_mcfbalr,
+		mc_ccsr_regs->reg_mcfbahr,
+		mc_ccsr_regs->reg_mcfapr,
+		mc_ccsr_regs->reg_psr);
+}
+#else
+
+#define dump_ram_words(title, addr)
+#define dump_mc_ccsr_regs(mc_ccsr_regs)
+
+#endif /* DEBUG */
+
+#ifndef CONFIG_SYS_LS_MC_FW_IN_DDR
 /**
  * Copying MC firmware or DPL image to DDR
  */
@@ -21,6 +80,7 @@ static int mc_copy_image(const char *title,
 {
 	debug("%s copied to address %p\n", title, (void *)mc_ram_addr);
 	memcpy((void *)mc_ram_addr, (void *)image_addr, image_size);
+	flush_dcache_range(mc_ram_addr, mc_ram_addr + image_size);
 	return 0;
 }
 
@@ -82,23 +142,254 @@ int parse_mc_firmware_fit_image(const void **raw_image_addr,
 
 	return 0;
 }
+#endif
+
+/*
+ * Calculates the values to be used to specify the address range
+ * for the MC private DRAM block, in the MCFBALR/MCFBAHR registers.
+ * It returns the highest 512MB-aligned address within the given
+ * address range, in '*aligned_base_addr', and the number of 256 MiB
+ * blocks in it, in 'num_256mb_blocks'.
+ */
+static int calculate_mc_private_ram_params(u64 mc_private_ram_start_addr,
+					   size_t mc_ram_size,
+					   u64 *aligned_base_addr,
+					   u8 *num_256mb_blocks)
+{
+	u64 addr;
+	u16 num_blocks;
+
+	if (mc_ram_size % MC_RAM_SIZE_ALIGNMENT != 0) {
+		printf("fsl-mc: ERROR: invalid MC private RAM size (%lu)\n",
+		       mc_ram_size);
+		return -EINVAL;
+	}
+
+	num_blocks = mc_ram_size / MC_RAM_SIZE_ALIGNMENT;
+	if (num_blocks < 1 || num_blocks > 0xff) {
+		printf("fsl-mc: ERROR: invalid MC private RAM size (%lu)\n",
+		       mc_ram_size);
+		return -EINVAL;
+	}
+
+	addr = (mc_private_ram_start_addr + mc_ram_size - 1) &
+		MC_RAM_BASE_ADDR_ALIGNMENT_MASK;
+
+	if (addr < mc_private_ram_start_addr) {
+		printf("fsl-mc: ERROR: bad start address %#llx\n",
+		       mc_private_ram_start_addr);
+		return -EFAULT;
+	}
+
+	*aligned_base_addr = addr;
+	*num_256mb_blocks = num_blocks;
+	return 0;
+}
+
+static int load_mc_dpc(u64 mc_ram_addr, size_t mc_ram_size)
+{
+	u64 mc_dpc_offset;
+#ifndef CONFIG_SYS_LS_MC_DPC_IN_DDR
+	int error;
+	void *dpc_fdt_hdr;
+	int dpc_size;
+#endif
+
+#ifdef CONFIG_SYS_LS_MC_DRAM_DPC_OFFSET
+	BUILD_BUG_ON((CONFIG_SYS_LS_MC_DRAM_DPC_OFFSET & 0x3) != 0 ||
+		     CONFIG_SYS_LS_MC_DRAM_DPC_OFFSET > 0xffffffff);
+
+	mc_dpc_offset = CONFIG_SYS_LS_MC_DRAM_DPC_OFFSET;
+#else
+#error "CONFIG_SYS_LS_MC_DRAM_DPC_OFFSET not defined"
+#endif
+
+	/*
+	 * Load the MC DPC blob in the MC private DRAM block:
+	 */
+#ifdef CONFIG_SYS_LS_MC_DPC_IN_DDR
+	printf("MC DPC is preloaded to %#llx\n", mc_ram_addr + mc_dpc_offset);
+#else
+	/*
+	 * Get address and size of the DPC blob stored in flash:
+	 */
+#ifdef CONFIG_SYS_LS_MC_DPC_IN_NOR
+	dpc_fdt_hdr = (void *)CONFIG_SYS_LS_MC_DPC_ADDR;
+#else
+#error "No CONFIG_SYS_LS_MC_DPC_IN_xxx defined"
+#endif
+
+	error = fdt_check_header(dpc_fdt_hdr);
+	if (error != 0) {
+		/*
+		 * Don't return with error here, since the MC firmware can
+		 * still boot without a DPC
+		 */
+		printf("fsl-mc: WARNING: No DPC image found\n");
+		return 0;
+	}
+
+	dpc_size = fdt_totalsize(dpc_fdt_hdr);
+	if (dpc_size > CONFIG_SYS_LS_MC_DPC_MAX_LENGTH) {
+		printf("fsl-mc: ERROR: Bad DPC image (too large: %d)\n",
+		       dpc_size);
+		return -EINVAL;
+	}
+
+	mc_copy_image("MC DPC blob",
+		      (u64)dpc_fdt_hdr, dpc_size, mc_ram_addr + mc_dpc_offset);
+#endif /* not defined CONFIG_SYS_LS_MC_DPC_IN_DDR */
+
+	dump_ram_words("DPC", (void *)(mc_ram_addr + mc_dpc_offset));
+	return 0;
+}
 
-int mc_init(bd_t *bis)
+static int load_mc_dpl(u64 mc_ram_addr, size_t mc_ram_size)
+{
+	u64 mc_dpl_offset;
+#ifndef CONFIG_SYS_LS_MC_DPL_IN_DDR
+	int error;
+	void *dpl_fdt_hdr;
+	int dpl_size;
+#endif
+
+#ifdef CONFIG_SYS_LS_MC_DRAM_DPL_OFFSET
+	BUILD_BUG_ON((CONFIG_SYS_LS_MC_DRAM_DPL_OFFSET & 0x3) != 0 ||
+		     CONFIG_SYS_LS_MC_DRAM_DPL_OFFSET > 0xffffffff);
+
+	mc_dpl_offset = CONFIG_SYS_LS_MC_DRAM_DPL_OFFSET;
+#else
+#error "CONFIG_SYS_LS_MC_DRAM_DPL_OFFSET not defined"
+#endif
+
+	/*
+	 * Load the MC DPL blob in the MC private DRAM block:
+	 */
+#ifdef CONFIG_SYS_LS_MC_DPL_IN_DDR
+	printf("MC DPL is preloaded to %#llx\n", mc_ram_addr + mc_dpl_offset);
+#else
+	/*
+	 * Get address and size of the DPL blob stored in flash:
+	 */
+#ifdef CONFIG_SYS_LS_MC_DPL_IN_NOR
+	dpl_fdt_hdr = (void *)CONFIG_SYS_LS_MC_DPL_ADDR;
+#else
+#error "No CONFIG_SYS_LS_MC_DPL_IN_xxx defined"
+#endif
+
+	error = fdt_check_header(dpl_fdt_hdr);
+	if (error != 0) {
+		printf("fsl-mc: ERROR: Bad DPL image (bad header)\n");
+		return error;
+	}
+
+	dpl_size = fdt_totalsize(dpl_fdt_hdr);
+	if (dpl_size > CONFIG_SYS_LS_MC_DPL_MAX_LENGTH) {
+		printf("fsl-mc: ERROR: Bad DPL image (too large: %d)\n",
+		       dpl_size);
+		return -EINVAL;
+	}
+
+	mc_copy_image("MC DPL blob",
+		      (u64)dpl_fdt_hdr, dpl_size, mc_ram_addr + mc_dpl_offset);
+#endif /* not defined CONFIG_SYS_LS_MC_DPL_IN_DDR */
+
+	dump_ram_words("DPL", (void *)(mc_ram_addr + mc_dpl_offset));
+	return 0;
+}
+
+/**
+ * Return the MC boot timeout value in milliseconds
+ */
+static unsigned long get_mc_boot_timeout_ms(void)
+{
+	unsigned long timeout_ms = CONFIG_SYS_LS_MC_BOOT_TIMEOUT_MS;
+
+	char *timeout_ms_env_var = getenv(MC_BOOT_TIMEOUT_ENV_VAR);
+
+	if (timeout_ms_env_var) {
+		timeout_ms = simple_strtoul(timeout_ms_env_var, NULL, 10);
+		if (timeout_ms == 0) {
+			printf("fsl-mc: WARNING: Invalid value for \'"
+			       MC_BOOT_TIMEOUT_ENV_VAR
+			       "\' environment variable: %lu\n",
+			       timeout_ms);
+
+			timeout_ms = CONFIG_SYS_LS_MC_BOOT_TIMEOUT_MS;
+		}
+	}
+
+	return timeout_ms;
+}
+
+static int wait_for_mc(bool booting_mc, u32 *final_reg_gsr)
+{
+	u32 reg_gsr;
+	u32 mc_fw_boot_status;
+	unsigned long timeout_ms = get_mc_boot_timeout_ms();
+	struct mc_ccsr_registers __iomem *mc_ccsr_regs = MC_CCSR_BASE_ADDR;
+
+	dmb();
+	debug("Polling mc_ccsr_regs->reg_gsr ...\n");
+	assert(timeout_ms > 0);
+	for (;;) {
+		udelay(1000);	/* throttle polling */
+		reg_gsr = in_le32(&mc_ccsr_regs->reg_gsr);
+		mc_fw_boot_status = (reg_gsr & GSR_FS_MASK);
+		if (mc_fw_boot_status & 0x1)
+			break;
+
+		timeout_ms--;
+		if (timeout_ms == 0)
+			break;
+	}
+
+	if (timeout_ms == 0) {
+		if (booting_mc)
+			printf("fsl-mc: timeout booting management complex firmware\n");
+		else
+			printf("fsl-mc: timeout deploying data path layout\n");
+
+		/* TODO: Get an error status from an MC CCSR register */
+		return -ETIMEDOUT;
+	}
+
+	if (mc_fw_boot_status != 0x1) {
+		/*
+		 * TODO: Identify critical errors from the GSR register's FS
+		 * field and for those errors, set error to -ENODEV or other
+		 * appropriate errno, so that the status property is set to
+		 * failure in the fsl,dprc device tree node.
+		 */
+		if (booting_mc) {
+			printf("fsl-mc: WARNING: Firmware booted with error (GSR: %#x)\n",
+			       reg_gsr);
+		} else {
+			printf("fsl-mc: WARNING: Data path layout deployed with error (GSR: %#x)\n",
+			       reg_gsr);
+		}
+	}
+
+	*final_reg_gsr = reg_gsr;
+	return 0;
+}
+
+int mc_init(void)
 {
 	int error = 0;
-	int timeout = 200000;
+	int portal_id = 0;
 	struct mc_ccsr_registers __iomem *mc_ccsr_regs = MC_CCSR_BASE_ADDR;
 	u64 mc_ram_addr;
-	u64 mc_dpl_offset;
 	u32 reg_gsr;
-	u32 mc_fw_boot_status;
-	void *dpl_fdt_hdr;
-	int dpl_size;
+	u32 reg_mcfbalr;
+#ifndef CONFIG_SYS_LS_MC_FW_IN_DDR
 	const void *raw_image_addr;
 	size_t raw_image_size = 0;
-	struct fsl_mc_io mc_io;
-	int portal_id;
+#endif
 	struct mc_version mc_ver_info;
+	u64 mc_ram_aligned_base_addr;
+	u8 mc_ram_num_256mb_blocks;
+	size_t mc_ram_size = mc_get_dram_block_size();
 
 	/*
 	 * The MC private DRAM block was already carved at the end of DRAM
@@ -112,6 +403,20 @@ int mc_init(bd_t *bis)
 			gd->bd->bi_dram[0].start + gd->bd->bi_dram[0].size;
 	}
 
+#ifdef CONFIG_FSL_DEBUG_SERVER
+	/*
+	 * FIXME: I don't think this is right. See get_dram_size_to_hide()
+	 */
+		mc_ram_addr -= debug_server_get_dram_block_size();
+#endif
+
+	error = calculate_mc_private_ram_params(mc_ram_addr,
+						mc_ram_size,
+						&mc_ram_aligned_base_addr,
+						&mc_ram_num_256mb_blocks);
+	if (error != 0)
+		goto out;
+
 	/*
 	 * Management Complex cores should be held at reset out of POR.
 	 * U-boot should be the first software to touch MC. To be safe,
@@ -127,6 +432,9 @@ int mc_init(bd_t *bis)
 	out_le32(&mc_ccsr_regs->reg_gcr1, 0);
 	dmb();
 
+#ifdef CONFIG_SYS_LS_MC_FW_IN_DDR
+	printf("MC firmware is preloaded to %#llx\n", mc_ram_addr);
+#else
 	error = parse_mc_firmware_fit_image(&raw_image_addr, &raw_image_size);
 	if (error != 0)
 		goto out;
@@ -135,83 +443,34 @@ int mc_init(bd_t *bis)
 	 */
 	mc_copy_image("MC Firmware",
 		      (u64)raw_image_addr, raw_image_size, mc_ram_addr);
-
-	/*
-	 * Get address and size of the DPL blob stored in flash:
-	 */
-#ifdef CONFIG_SYS_LS_MC_DPL_IN_NOR
-	dpl_fdt_hdr = (void *)CONFIG_SYS_LS_MC_DPL_ADDR;
-#else
-#error "No CONFIG_SYS_LS_MC_DPL_IN_xxx defined"
 #endif
+	dump_ram_words("firmware", (void *)mc_ram_addr);
 
-	error = fdt_check_header(dpl_fdt_hdr);
-	if (error != 0) {
-		printf("fsl-mc: ERROR: Bad DPL image (bad header)\n");
-		goto out;
-	}
-
-	dpl_size = fdt_totalsize(dpl_fdt_hdr);
-	if (dpl_size > CONFIG_SYS_LS_MC_DPL_MAX_LENGTH) {
-		printf("fsl-mc: ERROR: Bad DPL image (too large: %d)\n",
-		       dpl_size);
-		error = -EINVAL;
+	error = load_mc_dpc(mc_ram_addr, mc_ram_size);
+	if (error != 0)
 		goto out;
-	}
 
-	/*
-	 * Calculate offset in the MC private DRAM block at which the MC DPL
-	 * blob is to be placed:
-	 */
-#ifdef CONFIG_SYS_LS_MC_DRAM_DPL_OFFSET
-	BUILD_BUG_ON((CONFIG_SYS_LS_MC_DRAM_DPL_OFFSET & 0x3) != 0 ||
-		     CONFIG_SYS_LS_MC_DRAM_DPL_OFFSET > 0xffffffff);
-
-	mc_dpl_offset = CONFIG_SYS_LS_MC_DRAM_DPL_OFFSET;
-#else
-	mc_dpl_offset = mc_get_dram_block_size() -
-			roundup(CONFIG_SYS_LS_MC_DPL_MAX_LENGTH, 4096);
-
-	if ((mc_dpl_offset & 0x3) != 0 || mc_dpl_offset > 0xffffffff) {
-		printf("%s: Invalid MC DPL offset: %llu\n",
-		       __func__, mc_dpl_offset);
-		error = -EINVAL;
+	error = load_mc_dpl(mc_ram_addr, mc_ram_size);
+	if (error != 0)
 		goto out;
-	}
-#endif
-
-	/*
-	 * Load the MC DPL blob at the far end of the MC private DRAM block:
-	 *
-	 * TODO: Should we place the DPL at a different location to match
-	 * assumptions of MC firmware about its memory layout?
-	 */
-	mc_copy_image("MC DPL blob",
-		      (u64)dpl_fdt_hdr, dpl_size, mc_ram_addr + mc_dpl_offset);
 
 	debug("mc_ccsr_regs %p\n", mc_ccsr_regs);
+	dump_mc_ccsr_regs(mc_ccsr_regs);
 
 	/*
-	 * Tell MC where the MC Firmware image was loaded in DDR:
+	 * Tell MC what is the address range of the DRAM block assigned to it:
 	 */
-	out_le32(&mc_ccsr_regs->reg_mcfbalr, (u32)mc_ram_addr);
-	out_le32(&mc_ccsr_regs->reg_mcfbahr, (u32)((u64)mc_ram_addr >> 32));
+	reg_mcfbalr = (u32)mc_ram_aligned_base_addr |
+		      (mc_ram_num_256mb_blocks - 1);
+	out_le32(&mc_ccsr_regs->reg_mcfbalr, reg_mcfbalr);
+	out_le32(&mc_ccsr_regs->reg_mcfbahr,
+		 (u32)(mc_ram_aligned_base_addr >> 32));
 	out_le32(&mc_ccsr_regs->reg_mcfapr, MCFAPR_BYPASS_ICID_MASK);
 
 	/*
-	 * Tell MC where the DPL blob was loaded in DDR, by indicating
-	 * its offset relative to the beginning of the DDR block
-	 * allocated to the MC firmware. The MC firmware is responsible
-	 * for checking that there is no overlap between the DPL blob
-	 * and the runtime heap and stack of the MC firmware itself.
-	 *
-	 * NOTE: bits [31:2] of this offset need to be stored in bits [29:0] of
-	 * the GSR MC CCSR register. So, this offset is assumed to be 4-byte
-	 * aligned.
-	 * Care must be taken not to write 1s into bits 31 and 30 of the GSR in
-	 * this case as the SoC COP or PIC will be signaled.
+	 * Tell the MC that we want delayed DPL deployment.
 	 */
-	out_le32(&mc_ccsr_regs->reg_gsr, (u32)(mc_dpl_offset >> 2));
+	out_le32(&mc_ccsr_regs->reg_gsr, 0xDD00);
 
 	printf("\nfsl-mc: Booting Management Complex ...\n");
 
@@ -219,38 +478,9 @@ int mc_init(bd_t *bis)
 	 * Deassert reset and release MC core 0 to run
 	 */
 	out_le32(&mc_ccsr_regs->reg_gcr1, GCR1_P1_DE_RST | GCR1_M_ALL_DE_RST);
-	dmb();
-	debug("Polling mc_ccsr_regs->reg_gsr ...\n");
-
-	for (;;) {
-		reg_gsr = in_le32(&mc_ccsr_regs->reg_gsr);
-		mc_fw_boot_status = (reg_gsr & GSR_FS_MASK);
-		if (mc_fw_boot_status & 0x1)
-			break;
-
-		udelay(1000);	/* throttle polling */
-		if (timeout-- <= 0)
-			break;
-	}
-
-	if (timeout <= 0) {
-		printf("fsl-mc: timeout booting management complex firmware\n");
-
-		/* TODO: Get an error status from an MC CCSR register */
-		error = -ETIMEDOUT;
+	error = wait_for_mc(true, &reg_gsr);
+	if (error != 0)
 		goto out;
-	}
-
-	if (mc_fw_boot_status != 0x1) {
-		/*
-		 * TODO: Identify critical errors from the GSR register's FS
-		 * field and for those errors, set error to -ENODEV or other
-		 * appropriate errno, so that the status property is set to
-		 * failure in the fsl,dprc device tree node.
-		 */
-		printf("fsl-mc: WARNING: Firmware booted with error (GSR: %#x)\n",
-		       reg_gsr);
-	}
 
 	/*
 	 * TODO: need to obtain the portal_id for the root container from the
@@ -259,13 +489,20 @@ int mc_init(bd_t *bis)
 	portal_id = 0;
 
 	/*
-	 * Check that the MC firmware is responding portal commands:
+	 * Initialize the global default MC portal
+	 * And check that the MC firmware is responding portal commands:
 	 */
-	mc_io.mmio_regs = SOC_MC_PORTAL_ADDR(portal_id);
+	dflt_mc_io = (struct fsl_mc_io *)malloc(sizeof(struct fsl_mc_io));
+	if (!dflt_mc_io) {
+		printf(" No memory: malloc() failed\n");
+		return -ENOMEM;
+	}
+
+	dflt_mc_io->mmio_regs = SOC_MC_PORTAL_ADDR(portal_id);
 	debug("Checking access to MC portal of root DPRC container (portal_id %d, portal physical addr %p)\n",
-	      portal_id, mc_io.mmio_regs);
+	      portal_id, dflt_mc_io->mmio_regs);
 
-	error = mc_get_version(&mc_io, &mc_ver_info);
+	error = mc_get_version(dflt_mc_io, &mc_ver_info);
 	if (error != 0) {
 		printf("fsl-mc: ERROR: Firmware version check failed (error: %d)\n",
 		       error);
@@ -282,7 +519,16 @@ int mc_init(bd_t *bis)
 
 	printf("fsl-mc: Management Complex booted (version: %d.%d.%d, boot status: %#x)\n",
 	       mc_ver_info.major, mc_ver_info.minor, mc_ver_info.revision,
-	       mc_fw_boot_status);
+	       reg_gsr & GSR_FS_MASK);
+
+	/*
+	 * Tell the MC to deploy the DPL:
+	 */
+	out_le32(&mc_ccsr_regs->reg_gsr, 0x0);
+	printf("\nfsl-mc: Deploying data path layout ...\n");
+	error = wait_for_mc(false, &reg_gsr);
+	if (error != 0)
+		goto out;
 out:
 	if (error != 0)
 		mc_boot_status = -error;
@@ -299,12 +545,242 @@ int get_mc_boot_status(void)
 
 /**
  * Return the actual size of the MC private DRAM block.
- *
- * NOTE: For now this function always returns the minimum required size,
- * However, in the future, the actual size may be obtained from an environment
- * variable.
  */
 unsigned long mc_get_dram_block_size(void)
 {
-	return CONFIG_SYS_LS_MC_DRAM_BLOCK_MIN_SIZE;
+	unsigned long dram_block_size = CONFIG_SYS_LS_MC_DRAM_BLOCK_MIN_SIZE;
+
+	char *dram_block_size_env_var = getenv(MC_MEM_SIZE_ENV_VAR);
+
+	if (dram_block_size_env_var) {
+		dram_block_size = simple_strtoul(dram_block_size_env_var, NULL,
+						 10);
+
+		if (dram_block_size < CONFIG_SYS_LS_MC_DRAM_BLOCK_MIN_SIZE) {
+			printf("fsl-mc: WARNING: Invalid value for \'"
+			       MC_MEM_SIZE_ENV_VAR
+			       "\' environment variable: %lu\n",
+			       dram_block_size);
+
+			dram_block_size = CONFIG_SYS_LS_MC_DRAM_BLOCK_MIN_SIZE;
+		}
+	}
+
+	return dram_block_size;
+}
+
+int dpio_init(struct dprc_obj_desc obj_desc)
+{
+	struct qbman_swp_desc p_des;
+	struct dpio_attr attr;
+	int err = 0;
+
+	dflt_dpio = (struct fsl_dpio_obj *)malloc(sizeof(struct fsl_dpio_obj));
+	if (!dflt_dpio) {
+		printf(" No memory: malloc() failed\n");
+		return -ENOMEM;
+	}
+
+	dflt_dpio->dpio_id = obj_desc.id;
+
+	err = dpio_open(dflt_mc_io, obj_desc.id, &dflt_dpio_handle);
+	if (err) {
+		printf("dpio_open() failed\n");
+		goto err_open;
+	}
+
+	err = dpio_get_attributes(dflt_mc_io, dflt_dpio_handle, &attr);
+	if (err) {
+		printf("dpio_get_attributes() failed %d\n", err);
+		goto err_get_attr;
+	}
+
+	err = dpio_enable(dflt_mc_io, dflt_dpio_handle);
+	if (err) {
+		printf("dpio_enable() failed %d\n", err);
+		goto err_get_enable;
+	}
+	debug("ce_paddr=0x%llx, ci_paddr=0x%llx, portalid=%d, prios=%d\n",
+	      attr.qbman_portal_ce_paddr,
+	      attr.qbman_portal_ci_paddr,
+	      attr.qbman_portal_id,
+	      attr.num_priorities);
+
+	p_des.cena_bar = (void *)attr.qbman_portal_ce_paddr;
+	p_des.cinh_bar = (void *)attr.qbman_portal_ci_paddr;
+
+	dflt_dpio->sw_portal = qbman_swp_init(&p_des);
+	if (dflt_dpio->sw_portal == NULL) {
+		printf("qbman_swp_init() failed\n");
+		goto err_get_swp_init;
+	}
+	return 0;
+
+err_get_swp_init:
+err_get_enable:
+	dpio_disable(dflt_mc_io, dflt_dpio_handle);
+err_get_attr:
+	dpio_close(dflt_mc_io, dflt_dpio_handle);
+err_open:
+	free(dflt_dpio);
+	return err;
+}
+
+int dpbp_init(struct dprc_obj_desc obj_desc)
+{
+	dflt_dpbp = (struct fsl_dpbp_obj *)malloc(sizeof(struct fsl_dpbp_obj));
+	if (!dflt_dpbp) {
+		printf(" No memory: malloc() failed\n");
+		return -ENOMEM;
+	}
+	dflt_dpbp->dpbp_attr.id = obj_desc.id;
+
+	return 0;
+}
+
+int dprc_init_container_obj(struct dprc_obj_desc obj_desc, uint16_t dprc_handle)
+{
+	int error = 0, state = 0;
+	struct dprc_endpoint dpni_endpoint, dpmac_endpoint;
+	if (!strcmp(obj_desc.type, "dpbp")) {
+		if (!dflt_dpbp) {
+			error = dpbp_init(obj_desc);
+			if (error < 0)
+				printf("dpbp_init failed\n");
+		}
+	} else if (!strcmp(obj_desc.type, "dpio")) {
+		if (!dflt_dpio) {
+			error = dpio_init(obj_desc);
+			if (error < 0)
+				printf("dpio_init failed\n");
+		}
+	} else if (!strcmp(obj_desc.type, "dpni")) {
+		strcpy(dpni_endpoint.type, obj_desc.type);
+		dpni_endpoint.id = obj_desc.id;
+		error = dprc_get_connection(dflt_mc_io, dprc_handle,
+				     &dpni_endpoint, &dpmac_endpoint, &state);
+		if (!strcmp(dpmac_endpoint.type, "dpmac"))
+			error = ldpaa_eth_init(obj_desc);
+		if (error < 0)
+			printf("ldpaa_eth_init failed\n");
+	}
+
+	return error;
+}
+
+int dprc_scan_container_obj(uint16_t dprc_handle, char *obj_type, int i)
+{
+	int error = 0;
+	struct dprc_obj_desc obj_desc;
+
+	memset((void *)&obj_desc, 0x00, sizeof(struct dprc_obj_desc));
+
+	error = dprc_get_obj(dflt_mc_io, dprc_handle,
+			     i, &obj_desc);
+	if (error < 0) {
+		printf("dprc_get_obj(i=%d) failed: %d\n",
+		       i, error);
+		return error;
+	}
+
+	if (!strcmp(obj_desc.type, obj_type)) {
+		debug("Discovered object: type %s, id %d, req %s\n",
+		      obj_desc.type, obj_desc.id, obj_type);
+
+		error = dprc_init_container_obj(obj_desc, dprc_handle);
+		if (error < 0) {
+			printf("dprc_init_container_obj(i=%d) failed: %d\n",
+			       i, error);
+			return error;
+		}
+	}
+
+	return error;
+}
+
+int fsl_mc_ldpaa_init(bd_t *bis)
+{
+	int i, error = 0;
+	int dprc_opened = 0, container_id;
+	int num_child_objects = 0;
+
+	error = mc_init();
+	if (error < 0)
+		goto error;
+
+	error = dprc_get_container_id(dflt_mc_io, &container_id);
+	if (error < 0) {
+		printf("dprc_get_container_id() failed: %d\n", error);
+		goto error;
+	}
+
+	debug("fsl-mc: Container id=0x%x\n", container_id);
+
+	error = dprc_open(dflt_mc_io, container_id, &dflt_dprc_handle);
+	if (error < 0) {
+		printf("dprc_open() failed: %d\n", error);
+		goto error;
+	}
+	dprc_opened = true;
+
+	error = dprc_get_obj_count(dflt_mc_io,
+				   dflt_dprc_handle,
+				   &num_child_objects);
+	if (error < 0) {
+		printf("dprc_get_obj_count() failed: %d\n", error);
+		goto error;
+	}
+	debug("Total child in container %d = %d\n", container_id,
+	      num_child_objects);
+
+	if (num_child_objects != 0) {
+		/*
+		 * Discover objects currently in the DPRC container in the MC:
+		 */
+		for (i = 0; i < num_child_objects; i++)
+			error = dprc_scan_container_obj(dflt_dprc_handle,
+							"dpbp", i);
+
+		for (i = 0; i < num_child_objects; i++)
+			error = dprc_scan_container_obj(dflt_dprc_handle,
+							"dpio", i);
+
+		for (i = 0; i < num_child_objects; i++)
+			error = dprc_scan_container_obj(dflt_dprc_handle,
+							"dpni", i);
+	}
+error:
+	if (dprc_opened)
+		dprc_close(dflt_mc_io, dflt_dprc_handle);
+
+	return error;
+}
+
+void fsl_mc_ldpaa_exit(bd_t *bis)
+{
+	int err;
+
+	if (get_mc_boot_status() == 0) {
+		err = dpio_disable(dflt_mc_io, dflt_dpio_handle);
+		if (err < 0) {
+			printf("dpio_disable() failed: %d\n", err);
+			return;
+		}
+		err = dpio_reset(dflt_mc_io, dflt_dpio_handle);
+		if (err < 0) {
+			printf("dpio_reset() failed: %d\n", err);
+			return;
+		}
+		err = dpio_close(dflt_mc_io, dflt_dpio_handle);
+		if (err < 0) {
+			printf("dpio_close() failed: %d\n", err);
+			return;
+		}
+
+		free(dflt_dpio);
+		free(dflt_dpbp);
+	}
+
+	if (dflt_mc_io)
+		free(dflt_mc_io);
 }
diff --git a/drivers/net/fsl-mc/mc_sys.c b/drivers/net/fsl-mc/mc_sys.c
index 7c8e003..3fc1f98 100644
--- a/drivers/net/fsl-mc/mc_sys.c
+++ b/drivers/net/fsl-mc/mc_sys.c
@@ -1,7 +1,7 @@
 /*
  * Freescale Layerscape MC I/O wrapper
  *
- * Copyright (C) 2014 Freescale Semiconductor, Inc.
+ * Copyright (C) 2013-2015 Freescale Semiconductor, Inc.
  * Author: German Rivera <German.Rivera@freescale.com>
  *
  * SPDX-License-Identifier:	GPL-2.0+
@@ -32,7 +32,7 @@ int mc_send_command(struct fsl_mc_io *mc_io,
 		    struct mc_command *cmd)
 {
 	enum mc_cmd_status status;
-	int timeout = 2000;
+	int timeout = 6000;
 
 	mc_write_command(mc_io->mmio_regs, cmd);
 
@@ -52,7 +52,7 @@ int mc_send_command(struct fsl_mc_io *mc_io,
 	if (status != MC_CMD_STATUS_OK) {
 		printf("Error: MC command failed (portal: %p, obj handle: %#x, command: %#x, status: %#x)\n",
 		       mc_io->mmio_regs,
-		       (unsigned int)MC_CMD_HDR_READ_AUTHID(cmd->header),
+			(unsigned int)MC_CMD_HDR_READ_TOKEN(cmd->header),
 		       (unsigned int)MC_CMD_HDR_READ_CMDID(cmd->header),
 		       (unsigned int)status);
 
diff --git a/drivers/net/fsl_mcdmafec.c b/drivers/net/fsl_mcdmafec.c
index 6391f9b..792534b 100644
--- a/drivers/net/fsl_mcdmafec.c
+++ b/drivers/net/fsl_mcdmafec.c
@@ -244,7 +244,7 @@ static int fec_recv(struct eth_device *dev)
 	struct fec_info_dma *info = dev->priv;
 	volatile fecdma_t *fecp = (fecdma_t *) (info->iobase);
 
-	cbd_t *pRbd = &info->rxbd[info->rxIdx];
+	cbd_t *prbd = &info->rxbd[info->rxIdx];
 	u32 ievent;
 	int frame_length, len = 0;
 
@@ -276,26 +276,27 @@ static int fec_recv(struct eth_device *dev)
 		}
 	}
 
-	if (!(pRbd->cbd_sc & BD_ENET_RX_EMPTY)) {
-		if ((pRbd->cbd_sc & BD_ENET_RX_LAST)
-		    && !(pRbd->cbd_sc & BD_ENET_RX_ERR)
-		    && ((pRbd->cbd_datlen - 4) > 14)) {
+	if (!(prbd->cbd_sc & BD_ENET_RX_EMPTY)) {
+		if ((prbd->cbd_sc & BD_ENET_RX_LAST) &&
+		    !(prbd->cbd_sc & BD_ENET_RX_ERR) &&
+		    ((prbd->cbd_datlen - 4) > 14)) {
 
 			/* Get buffer address and size */
-			frame_length = pRbd->cbd_datlen - 4;
+			frame_length = prbd->cbd_datlen - 4;
 
 			/* Fill the buffer and pass it to upper layers */
-			NetReceive((uchar *)pRbd->cbd_bufaddr, frame_length);
+			net_process_received_packet((uchar *)prbd->cbd_bufaddr,
+						    frame_length);
 			len = frame_length;
 		}
 
 		/* Reset buffer descriptor as empty */
 		if ((info->rxIdx) == (PKTBUFSRX - 1))
-			pRbd->cbd_sc = (BD_ENET_RX_WRAP | BD_ENET_RX_EMPTY);
+			prbd->cbd_sc = (BD_ENET_RX_WRAP | BD_ENET_RX_EMPTY);
 		else
-			pRbd->cbd_sc = BD_ENET_RX_EMPTY;
+			prbd->cbd_sc = BD_ENET_RX_EMPTY;
 
-		pRbd->cbd_datlen = PKTSIZE_ALIGN;
+		prbd->cbd_datlen = PKTSIZE_ALIGN;
 
 		/* Now, we have an empty RxBD, restart the DMA receive task */
 		MCD_continDma(info->rxTask);
@@ -399,7 +400,7 @@ static int fec_init(struct eth_device *dev, bd_t * bd)
 	for (i = 0; i < PKTBUFSRX; i++) {
 		info->rxbd[i].cbd_sc = BD_ENET_RX_EMPTY;
 		info->rxbd[i].cbd_datlen = PKTSIZE_ALIGN;
-		info->rxbd[i].cbd_bufaddr = (uint) NetRxPackets[i];
+		info->rxbd[i].cbd_bufaddr = (uint) net_rx_packets[i];
 	}
 	info->rxbd[PKTBUFSRX - 1].cbd_sc |= BD_ENET_RX_WRAP;
 
diff --git a/drivers/net/ftgmac100.c b/drivers/net/ftgmac100.c
index 8519314..515f0b2 100644
--- a/drivers/net/ftgmac100.c
+++ b/drivers/net/ftgmac100.c
@@ -423,7 +423,7 @@ static int ftgmac100_init(struct eth_device *dev, bd_t *bd)
 	for (i = 0; i < PKTBUFSRX; i++) {
 		/* RXBUF_BADR */
 		if (!rxdes[i].rxdes2) {
-			buf = NetRxPackets[i];
+			buf = net_rx_packets[i];
 			rxdes[i].rxdes3 = virt_to_phys(buf);
 			rxdes[i].rxdes2 = (uint)buf;
 		}
@@ -493,7 +493,7 @@ static int ftgmac100_recv(struct eth_device *dev)
 	dma_map_single((void *)curr_des->rxdes2, rxlen, DMA_FROM_DEVICE);
 
 	/* pass the packet up to the protocol layers. */
-	NetReceive((void *)curr_des->rxdes2, rxlen);
+	net_process_received_packet((void *)curr_des->rxdes2, rxlen);
 
 	/* release buffer to DMA */
 	curr_des->rxdes0 &= ~FTGMAC100_RXDES0_RXPKT_RDY;
diff --git a/drivers/net/ftmac100.c b/drivers/net/ftmac100.c
index 3e148db..bd94f83 100644
--- a/drivers/net/ftmac100.c
+++ b/drivers/net/ftmac100.c
@@ -102,7 +102,7 @@ static int ftmac100_init (struct eth_device *dev, bd_t *bd)
 
 	for (i = 0; i < PKTBUFSRX; i++) {
 		/* RXBUF_BADR */
-		rxdes[i].rxdes2 = (unsigned int)NetRxPackets[i];
+		rxdes[i].rxdes2 = (unsigned int)net_rx_packets[i];
 		rxdes[i].rxdes1 |= FTMAC100_RXDES1_RXBUF_SIZE (PKTSIZE_ALIGN);
 		rxdes[i].rxdes0 = FTMAC100_RXDES0_RXDMA_OWN;
 	}
@@ -164,7 +164,7 @@ static int ftmac100_recv (struct eth_device *dev)
 
 	/* pass the packet up to the protocol layers. */
 
-	NetReceive ((void *)curr_des->rxdes2, rxlen);
+	net_process_received_packet((void *)curr_des->rxdes2, rxlen);
 
 	/* release buffer to DMA */
 
diff --git a/drivers/net/ftmac110.c b/drivers/net/ftmac110.c
index 98c4f09..4bae9ad 100644
--- a/drivers/net/ftmac110.c
+++ b/drivers/net/ftmac110.c
@@ -347,7 +347,7 @@ static int ftmac110_recv(struct eth_device *dev)
 			printf("ftmac110: rx error\n");
 		} else {
 			dma_map_single(buf, len, DMA_FROM_DEVICE);
-			NetReceive(buf, len);
+			net_process_received_packet(buf, len);
 			rlen += len;
 		}
 
@@ -425,7 +425,7 @@ int ftmac110_initialize(bd_t *bis)
 	dev->recv = ftmac110_recv;
 
 	if (!eth_getenv_enetaddr_by_index("eth", card_nr, dev->enetaddr))
-		eth_random_addr(dev->enetaddr);
+		net_random_ethaddr(dev->enetaddr);
 
 	/* allocate tx descriptors (it must be 16 bytes aligned) */
 	chip->txd = dma_alloc_coherent(
diff --git a/drivers/net/greth.c b/drivers/net/greth.c
index c817af4..a93b37a 100644
--- a/drivers/net/greth.c
+++ b/drivers/net/greth.c
@@ -533,7 +533,7 @@ int greth_recv(struct eth_device *dev)
 			sparc_dcache_flush_all();
 
 			/* pass packet on to network subsystem */
-			NetReceive((void *)d, len);
+			net_process_received_packet((void *)d, len);
 
 			/* bump stats counters */
 			greth->stats.rx_packets++;
diff --git a/drivers/net/keystone_net.c b/drivers/net/keystone_net.c
index 35f1a57..0c5fdee 100644
--- a/drivers/net/keystone_net.c
+++ b/drivers/net/keystone_net.c
@@ -505,7 +505,7 @@ static int keystone2_eth_rcv_packet(struct eth_device *dev)
 	if (hd == NULL)
 		return 0;
 
-	NetReceive((uchar *)pkt, pkt_size);
+	net_process_received_packet((uchar *)pkt, pkt_size);
 
 	ksnav_release_rxhd(&netcp_pktdma, hd);
 
diff --git a/drivers/net/ks8851_mll.c b/drivers/net/ks8851_mll.c
index 05e5b14..5b4c5b0 100644
--- a/drivers/net/ks8851_mll.c
+++ b/drivers/net/ks8851_mll.c
@@ -321,8 +321,8 @@ static void ks_rcv(struct eth_device *dev, uchar **pv_data)
 			/* read data block including CRC 4 bytes */
 			ks_read_qmu(dev, (u16 *)(*pv_data), frame_hdr->len);
 
-			/* NetRxPackets buffer size is ok (*pv_data pointer) */
-			NetReceive(*pv_data, frame_hdr->len);
+			/* net_rx_packets buffer size is ok (*pv_data) */
+			net_process_received_packet(*pv_data, frame_hdr->len);
 			pv_data++;
 		} else {
 			ks_wrreg16(dev, KS_RXQCR, (ks->rc_rxqcr | RXQCR_RRXEF));
@@ -573,7 +573,7 @@ static int ks8851_mll_recv(struct eth_device *dev)
 	ks_wrreg16(dev, KS_ISR, status);
 
 	if ((status & IRQ_RXI))
-		ks_rcv(dev, (uchar **)NetRxPackets);
+		ks_rcv(dev, (uchar **)net_rx_packets);
 
 	if ((status & IRQ_LDI)) {
 		u16 pmecr = ks_rdreg16(dev, KS_PMECR);
diff --git a/drivers/net/lan91c96.c b/drivers/net/lan91c96.c
index 229658a..495c088 100644
--- a/drivers/net/lan91c96.c
+++ b/drivers/net/lan91c96.c
@@ -568,29 +568,30 @@ static int smc_rcv(struct eth_device *dev)
 		   to send the DWORDs or the bytes first, or some
 		   mixture.  A mixture might improve already slow PIO
 		   performance  */
-		SMC_insl(dev, LAN91C96_DATA_HIGH, NetRxPackets[0],
-				packet_length >> 2);
+		SMC_insl(dev, LAN91C96_DATA_HIGH, net_rx_packets[0],
+			 packet_length >> 2);
 		/* read the left over bytes */
 		if (packet_length & 3) {
 			int i;
 
-			byte *tail = (byte *) (NetRxPackets[0] + (packet_length & ~3));
+			byte *tail = (byte *)(net_rx_packets[0] +
+				(packet_length & ~3));
 			dword leftover = SMC_inl(dev, LAN91C96_DATA_HIGH);
 
 			for (i = 0; i < (packet_length & 3); i++)
 				*tail++ = (byte) (leftover >> (8 * i)) & 0xff;
 		}
 #else
-		PRINTK3 (" Reading %d words and %d byte(s) \n",
-				 (packet_length >> 1), packet_length & 1);
-		SMC_insw(dev, LAN91C96_DATA_HIGH, NetRxPackets[0],
-				packet_length >> 1);
+		PRINTK3(" Reading %d words and %d byte(s)\n",
+			(packet_length >> 1), packet_length & 1);
+		SMC_insw(dev, LAN91C96_DATA_HIGH, net_rx_packets[0],
+			 packet_length >> 1);
 
 #endif /* USE_32_BIT */
 
 #if	SMC_DEBUG > 2
 		printf ("Receiving Packet\n");
-		print_packet((byte *)NetRxPackets[0], packet_length);
+		print_packet((byte *)net_rx_packets[0], packet_length);
 #endif
 	} else {
 		/* error ... */
@@ -609,7 +610,7 @@ static int smc_rcv(struct eth_device *dev)
 
 	if (!is_error) {
 		/* Pass the packet up to the protocol layers. */
-		NetReceive (NetRxPackets[0], packet_length);
+		net_process_received_packet(net_rx_packets[0], packet_length);
 		return packet_length;
 	} else {
 		return 0;
diff --git a/drivers/net/ldpaa_eth/Makefile b/drivers/net/ldpaa_eth/Makefile
new file mode 100644
index 0000000..c37633f
--- /dev/null
+++ b/drivers/net/ldpaa_eth/Makefile
@@ -0,0 +1,9 @@
+#
+# Copyright 2014 Freescale Semiconductor, Inc.
+#
+# SPDX-License-Identifier:	GPL-2.0+
+#
+
+obj-y += ldpaa_wriop.o
+obj-y += ldpaa_eth.o
+obj-$(CONFIG_LS2085A) += ls2085a.o
diff --git a/drivers/net/ldpaa_eth/ldpaa_eth.c b/drivers/net/ldpaa_eth/ldpaa_eth.c
new file mode 100644
index 0000000..d4be1ba
--- /dev/null
+++ b/drivers/net/ldpaa_eth/ldpaa_eth.c
@@ -0,0 +1,710 @@
+/*
+ * Copyright (C) 2014 Freescale Semiconductor
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <asm/io.h>
+#include <asm/types.h>
+#include <malloc.h>
+#include <net.h>
+#include <hwconfig.h>
+#include <phy.h>
+#include <linux/compat.h>
+
+#include "ldpaa_eth.h"
+
+#undef CONFIG_PHYLIB
+static int init_phy(struct eth_device *dev)
+{
+	/*TODO for external PHY */
+
+	return 0;
+}
+
+static void ldpaa_eth_rx(struct ldpaa_eth_priv *priv,
+			 const struct dpaa_fd *fd)
+{
+	u64 fd_addr;
+	uint16_t fd_offset;
+	uint32_t fd_length;
+	struct ldpaa_fas *fas;
+	uint32_t status, err;
+	struct qbman_release_desc releasedesc;
+	struct qbman_swp *swp = dflt_dpio->sw_portal;
+
+	fd_addr = ldpaa_fd_get_addr(fd);
+	fd_offset = ldpaa_fd_get_offset(fd);
+	fd_length = ldpaa_fd_get_len(fd);
+
+	debug("Rx frame:data addr=0x%p size=0x%x\n", (u64 *)fd_addr, fd_length);
+
+	if (fd->simple.frc & LDPAA_FD_FRC_FASV) {
+		/* Read the frame annotation status word and check for errors */
+		fas = (struct ldpaa_fas *)
+				((uint8_t *)(fd_addr) +
+				priv->buf_layout.private_data_size);
+		status = le32_to_cpu(fas->status);
+		if (status & LDPAA_ETH_RX_ERR_MASK) {
+			printf("Rx frame error(s): 0x%08x\n",
+			       status & LDPAA_ETH_RX_ERR_MASK);
+			goto error;
+		} else if (status & LDPAA_ETH_RX_UNSUPP_MASK) {
+			printf("Unsupported feature in bitmask: 0x%08x\n",
+			       status & LDPAA_ETH_RX_UNSUPP_MASK);
+			goto error;
+		}
+	}
+
+	debug("Rx frame: To Upper layer\n");
+	net_process_received_packet((uint8_t *)(fd_addr) + fd_offset,
+				    fd_length);
+
+error:
+	flush_dcache_range(fd_addr, fd_addr + LDPAA_ETH_RX_BUFFER_SIZE);
+	qbman_release_desc_clear(&releasedesc);
+	qbman_release_desc_set_bpid(&releasedesc, dflt_dpbp->dpbp_attr.bpid);
+	do {
+		/* Release buffer into the QBMAN */
+		err = qbman_swp_release(swp, &releasedesc, &fd_addr, 1);
+	} while (err == -EBUSY);
+	return;
+}
+
+static int ldpaa_eth_pull_dequeue_rx(struct eth_device *dev)
+{
+	struct ldpaa_eth_priv *priv = (struct ldpaa_eth_priv *)dev->priv;
+	const struct ldpaa_dq *dq;
+	const struct dpaa_fd *fd;
+	int i = 5, err = 0, status, loop = 20;
+	static struct qbman_pull_desc pulldesc;
+	struct qbman_swp *swp = dflt_dpio->sw_portal;
+
+	while (--i) {
+		qbman_pull_desc_clear(&pulldesc);
+		qbman_pull_desc_set_numframes(&pulldesc, 1);
+		qbman_pull_desc_set_fq(&pulldesc, priv->rx_dflt_fqid);
+
+		err = qbman_swp_pull(swp, &pulldesc);
+		if (err < 0) {
+			printf("Dequeue frames error:0x%08x\n", err);
+			continue;
+		}
+
+		do {
+			loop--;
+			dq = qbman_swp_dqrr_next(swp);
+
+			if (!loop)
+				break;
+		} while (!dq);
+
+		if (dq) {
+			/* Check for valid frame. If not sent a consume
+			 * confirmation to QBMAN otherwise give it to NADK
+			 * application and then send consume confirmation to
+			 * QBMAN.
+			 */
+			status = (uint8_t)ldpaa_dq_flags(dq);
+			if ((status & LDPAA_DQ_STAT_VALIDFRAME) == 0) {
+				debug("Dequeue RX frames:");
+				debug("No frame delivered\n");
+
+				qbman_swp_dqrr_consume(swp, dq);
+				break;
+			}
+
+			fd = ldpaa_dq_fd(dq);
+
+			/* Obtain FD and process it */
+			ldpaa_eth_rx(priv, fd);
+			qbman_swp_dqrr_consume(swp, dq);
+			break;
+		}
+	}
+
+	return err;
+}
+
+static void ldpaa_eth_tx_conf(struct ldpaa_eth_priv *priv,
+			      const struct dpaa_fd *fd)
+{
+	uint64_t fd_addr;
+	struct ldpaa_fas *fas;
+	uint32_t status, err;
+	struct qbman_release_desc releasedesc;
+	struct qbman_swp *swp = dflt_dpio->sw_portal;
+
+	fd_addr = ldpaa_fd_get_addr(fd);
+
+
+	debug("TX Conf frame:data addr=0x%p\n", (u64 *)fd_addr);
+
+	/* Check the status from the Frame Annotation */
+	if (fd->simple.frc & LDPAA_FD_FRC_FASV) {
+		fas = (struct ldpaa_fas *)
+				((uint8_t *)(fd_addr) +
+				priv->buf_layout.private_data_size);
+		status = le32_to_cpu(fas->status);
+		if (status & LDPAA_ETH_TXCONF_ERR_MASK) {
+			printf("TxConf frame error(s): 0x%08x\n",
+			       status & LDPAA_ETH_TXCONF_ERR_MASK);
+		}
+	}
+
+	qbman_release_desc_clear(&releasedesc);
+	qbman_release_desc_set_bpid(&releasedesc, dflt_dpbp->dpbp_attr.bpid);
+	do {
+		/* Release buffer into the QBMAN */
+		err = qbman_swp_release(swp, &releasedesc, &fd_addr, 1);
+	} while (err == -EBUSY);
+}
+
+static int ldpaa_eth_pull_dequeue_tx_conf(struct ldpaa_eth_priv *priv)
+{
+	const struct ldpaa_dq *dq;
+	const struct dpaa_fd *fd;
+	int err = 0;
+	int i = 5, status, loop = 20;
+	static struct qbman_pull_desc pulldesc;
+	struct qbman_swp *swp = dflt_dpio->sw_portal;
+
+	while (--i) {
+		qbman_pull_desc_clear(&pulldesc);
+		qbman_pull_desc_set_numframes(&pulldesc, 1);
+		qbman_pull_desc_set_fq(&pulldesc, priv->tx_conf_fqid);
+
+		err =  qbman_swp_pull(swp, &pulldesc);
+		if (err < 0) {
+			printf("Dequeue TX conf frames error:0x%08x\n", err);
+			continue;
+		}
+
+		do {
+			loop--;
+			dq = qbman_swp_dqrr_next(swp);
+
+			if (!loop)
+				break;
+		} while (!dq);
+
+		if (dq) {
+			/* Check for valid frame. If not sent a consume
+			 * confirmation to QBMAN otherwise give it to NADK
+			 * application and then send consume confirmation to
+			 * QBMAN.
+			 */
+			status = (uint8_t)ldpaa_dq_flags(dq);
+			if ((status & LDPAA_DQ_STAT_VALIDFRAME) == 0) {
+				debug("Dequeue TX conf frames:");
+				debug("No frame is delivered\n");
+
+				qbman_swp_dqrr_consume(swp, dq);
+				break;
+			}
+			fd = ldpaa_dq_fd(dq);
+
+			ldpaa_eth_tx_conf(priv, fd);
+			qbman_swp_dqrr_consume(swp, dq);
+			break;
+		}
+	}
+
+	return err;
+}
+
+static int ldpaa_eth_tx(struct eth_device *net_dev, void *buf, int len)
+{
+	struct ldpaa_eth_priv *priv = (struct ldpaa_eth_priv *)net_dev->priv;
+	struct dpaa_fd fd;
+	u64 buffer_start;
+	int data_offset, err;
+	struct qbman_swp *swp = dflt_dpio->sw_portal;
+	struct qbman_eq_desc ed;
+
+	/* Setup the FD fields */
+	memset(&fd, 0, sizeof(fd));
+
+	data_offset = priv->tx_data_offset;
+
+	do {
+		err = qbman_swp_acquire(dflt_dpio->sw_portal,
+					dflt_dpbp->dpbp_attr.bpid,
+					&buffer_start, 1);
+	} while (err == -EBUSY);
+
+	if (err < 0) {
+		printf("qbman_swp_acquire() failed\n");
+		return -ENOMEM;
+	}
+
+	debug("TX data: malloc buffer start=0x%p\n", (u64 *)buffer_start);
+
+	memcpy(((uint8_t *)(buffer_start) + data_offset), buf, len);
+
+	flush_dcache_range(buffer_start, buffer_start +
+					LDPAA_ETH_RX_BUFFER_SIZE);
+
+	ldpaa_fd_set_addr(&fd, (u64)buffer_start);
+	ldpaa_fd_set_offset(&fd, (uint16_t)(data_offset));
+	ldpaa_fd_set_bpid(&fd, dflt_dpbp->dpbp_attr.bpid);
+	ldpaa_fd_set_len(&fd, len);
+
+	fd.simple.ctrl = LDPAA_FD_CTRL_ASAL | LDPAA_FD_CTRL_PTA |
+				LDPAA_FD_CTRL_PTV1;
+
+	qbman_eq_desc_clear(&ed);
+	qbman_eq_desc_set_no_orp(&ed, 0);
+	qbman_eq_desc_set_qd(&ed, priv->tx_qdid, priv->tx_flow_id, 0);
+	err = qbman_swp_enqueue(swp, &ed, (const struct qbman_fd *)(&fd));
+	if (err < 0)
+		printf("error enqueueing Tx frame\n");
+
+	mdelay(1);
+
+	err = ldpaa_eth_pull_dequeue_tx_conf(priv);
+	if (err < 0)
+		printf("error Tx Conf frame\n");
+
+	return err;
+}
+
+static int ldpaa_eth_open(struct eth_device *net_dev, bd_t *bd)
+{
+	struct ldpaa_eth_priv *priv = (struct ldpaa_eth_priv *)net_dev->priv;
+	struct dpni_queue_attr rx_queue_attr;
+	struct dpni_tx_flow_attr tx_flow_attr;
+	uint8_t mac_addr[6];
+	int err;
+
+	if (net_dev->state == ETH_STATE_ACTIVE)
+		return 0;
+
+	/* DPNI initialization */
+	err = ldpaa_dpni_setup(priv);
+	if (err < 0)
+		goto err_dpni_setup;
+
+	err = ldpaa_dpbp_setup();
+	if (err < 0)
+		goto err_dpbp_setup;
+
+	/* DPNI binding DPBP */
+	err = ldpaa_dpni_bind(priv);
+	if (err)
+		goto err_bind;
+
+	err = dpni_get_primary_mac_addr(dflt_mc_io, priv->dpni_handle,
+					mac_addr);
+	if (err) {
+		printf("dpni_get_primary_mac_addr() failed\n");
+		return err;
+	}
+
+	memcpy(net_dev->enetaddr, mac_addr, 0x6);
+
+	/* setup the MAC address */
+	if (net_dev->enetaddr[0] & 0x01) {
+		printf("%s: MacAddress is multcast address\n",	__func__);
+		return 1;
+	}
+
+#ifdef CONFIG_PHYLIB
+	/* TODO Check this path */
+	err = phy_startup(priv->phydev);
+	if (err) {
+		printf("%s: Could not initialize\n", priv->phydev->dev->name);
+		return err;
+	}
+#else
+	priv->phydev->speed = SPEED_1000;
+	priv->phydev->link = 1;
+	priv->phydev->duplex = DUPLEX_FULL;
+#endif
+
+	err = dpni_enable(dflt_mc_io, priv->dpni_handle);
+	if (err < 0) {
+		printf("dpni_enable() failed\n");
+		return err;
+	}
+
+	/* TODO: support multiple Rx flows */
+	err = dpni_get_rx_flow(dflt_mc_io, priv->dpni_handle, 0, 0,
+			       &rx_queue_attr);
+	if (err) {
+		printf("dpni_get_rx_flow() failed\n");
+		goto err_rx_flow;
+	}
+
+	priv->rx_dflt_fqid = rx_queue_attr.fqid;
+
+	err = dpni_get_qdid(dflt_mc_io, priv->dpni_handle, &priv->tx_qdid);
+	if (err) {
+		printf("dpni_get_qdid() failed\n");
+		goto err_qdid;
+	}
+
+	err = dpni_get_tx_flow(dflt_mc_io, priv->dpni_handle, priv->tx_flow_id,
+			       &tx_flow_attr);
+	if (err) {
+		printf("dpni_get_tx_flow() failed\n");
+		goto err_tx_flow;
+	}
+
+	priv->tx_conf_fqid = tx_flow_attr.conf_err_attr.queue_attr.fqid;
+
+	if (!priv->phydev->link)
+		printf("%s: No link.\n", priv->phydev->dev->name);
+
+	return priv->phydev->link ? 0 : -1;
+
+err_tx_flow:
+err_qdid:
+err_rx_flow:
+	dpni_disable(dflt_mc_io, priv->dpni_handle);
+err_bind:
+	ldpaa_dpbp_free();
+err_dpbp_setup:
+	dpni_close(dflt_mc_io, priv->dpni_handle);
+err_dpni_setup:
+	return err;
+}
+
+static void ldpaa_eth_stop(struct eth_device *net_dev)
+{
+	struct ldpaa_eth_priv *priv = (struct ldpaa_eth_priv *)net_dev->priv;
+	int err = 0;
+
+	if ((net_dev->state == ETH_STATE_PASSIVE) ||
+	    (net_dev->state == ETH_STATE_INIT))
+		return;
+	/* Stop Tx and Rx traffic */
+	err = dpni_disable(dflt_mc_io, priv->dpni_handle);
+	if (err < 0)
+		printf("dpni_disable() failed\n");
+
+#ifdef CONFIG_PHYLIB
+	phy_shutdown(priv->phydev);
+#endif
+
+	ldpaa_dpbp_free();
+	dpni_reset(dflt_mc_io, priv->dpni_handle);
+	dpni_close(dflt_mc_io, priv->dpni_handle);
+}
+
+static void ldpaa_dpbp_drain_cnt(int count)
+{
+	uint64_t buf_array[7];
+	void *addr;
+	int ret, i;
+
+	BUG_ON(count > 7);
+
+	do {
+		ret = qbman_swp_acquire(dflt_dpio->sw_portal,
+					dflt_dpbp->dpbp_attr.bpid,
+					buf_array, count);
+		if (ret < 0) {
+			printf("qbman_swp_acquire() failed\n");
+			return;
+		}
+		for (i = 0; i < ret; i++) {
+			addr = (void *)buf_array[i];
+			debug("Free: buffer addr =0x%p\n", addr);
+			free(addr);
+		}
+	} while (ret);
+}
+
+static void ldpaa_dpbp_drain(void)
+{
+	int i;
+	for (i = 0; i < LDPAA_ETH_NUM_BUFS; i += 7)
+		ldpaa_dpbp_drain_cnt(7);
+}
+
+static int ldpaa_bp_add_7(uint16_t bpid)
+{
+	uint64_t buf_array[7];
+	u8 *addr;
+	int i;
+	struct qbman_release_desc rd;
+
+	for (i = 0; i < 7; i++) {
+		addr = memalign(L1_CACHE_BYTES, LDPAA_ETH_RX_BUFFER_SIZE);
+		if (!addr) {
+			printf("addr allocation failed\n");
+			goto err_alloc;
+		}
+		memset(addr, 0x00, LDPAA_ETH_RX_BUFFER_SIZE);
+		flush_dcache_range((u64)addr,
+				   (u64)(addr + LDPAA_ETH_RX_BUFFER_SIZE));
+
+		buf_array[i] = (uint64_t)addr;
+		debug("Release: buffer addr =0x%p\n", addr);
+	}
+
+release_bufs:
+	/* In case the portal is busy, retry until successful.
+	 * This function is guaranteed to succeed in a reasonable amount
+	 * of time.
+	 */
+
+	do {
+		mdelay(1);
+		qbman_release_desc_clear(&rd);
+		qbman_release_desc_set_bpid(&rd, bpid);
+	} while (qbman_swp_release(dflt_dpio->sw_portal, &rd, buf_array, i));
+
+	return i;
+
+err_alloc:
+	if (i)
+		goto release_bufs;
+
+	return 0;
+}
+
+static int ldpaa_dpbp_seed(uint16_t bpid)
+{
+	int i;
+	int count;
+
+	for (i = 0; i < LDPAA_ETH_NUM_BUFS; i += 7) {
+		count = ldpaa_bp_add_7(bpid);
+		if (count < 7)
+			printf("Buffer Seed= %d\n", count);
+	}
+
+	return 0;
+}
+
+static int ldpaa_dpbp_setup(void)
+{
+	int err;
+
+	err = dpbp_open(dflt_mc_io, dflt_dpbp->dpbp_attr.id,
+			&dflt_dpbp->dpbp_handle);
+	if (err) {
+		printf("dpbp_open() failed\n");
+		goto err_open;
+	}
+
+	err = dpbp_enable(dflt_mc_io, dflt_dpbp->dpbp_handle);
+	if (err) {
+		printf("dpbp_enable() failed\n");
+		goto err_enable;
+	}
+
+	err = dpbp_get_attributes(dflt_mc_io, dflt_dpbp->dpbp_handle,
+				  &dflt_dpbp->dpbp_attr);
+	if (err) {
+		printf("dpbp_get_attributes() failed\n");
+		goto err_get_attr;
+	}
+
+	err = ldpaa_dpbp_seed(dflt_dpbp->dpbp_attr.bpid);
+	if (err) {
+		printf("Buffer seeding failed for DPBP %d (bpid=%d)\n",
+		       dflt_dpbp->dpbp_attr.id, dflt_dpbp->dpbp_attr.bpid);
+		goto err_seed;
+	}
+
+	return 0;
+
+err_seed:
+err_get_attr:
+	dpbp_disable(dflt_mc_io, dflt_dpbp->dpbp_handle);
+err_enable:
+	dpbp_close(dflt_mc_io, dflt_dpbp->dpbp_handle);
+err_open:
+	return err;
+}
+
+static void ldpaa_dpbp_free(void)
+{
+	ldpaa_dpbp_drain();
+	dpbp_disable(dflt_mc_io, dflt_dpbp->dpbp_handle);
+	dpbp_reset(dflt_mc_io, dflt_dpbp->dpbp_handle);
+	dpbp_close(dflt_mc_io, dflt_dpbp->dpbp_handle);
+}
+
+static int ldpaa_dpni_setup(struct ldpaa_eth_priv *priv)
+{
+	int err;
+
+	/* and get a handle for the DPNI this interface is associate with */
+	err = dpni_open(dflt_mc_io, priv->dpni_id, &priv->dpni_handle);
+	if (err) {
+		printf("dpni_open() failed\n");
+		goto err_open;
+	}
+
+	err = dpni_get_attributes(dflt_mc_io, priv->dpni_handle,
+				  &priv->dpni_attrs);
+	if (err) {
+		printf("dpni_get_attributes() failed (err=%d)\n", err);
+		goto err_get_attr;
+	}
+
+	/* Configure our buffers' layout */
+	priv->buf_layout.options = DPNI_BUF_LAYOUT_OPT_PARSER_RESULT |
+				   DPNI_BUF_LAYOUT_OPT_FRAME_STATUS |
+				   DPNI_BUF_LAYOUT_OPT_PRIVATE_DATA_SIZE;
+	priv->buf_layout.pass_parser_result = true;
+	priv->buf_layout.pass_frame_status = true;
+	priv->buf_layout.private_data_size = LDPAA_ETH_SWA_SIZE;
+	/* ...rx, ... */
+	err = dpni_set_rx_buffer_layout(dflt_mc_io, priv->dpni_handle,
+					&priv->buf_layout);
+	if (err) {
+		printf("dpni_set_rx_buffer_layout() failed");
+		goto err_buf_layout;
+	}
+
+	/* ... tx, ... */
+	priv->buf_layout.options &= ~DPNI_BUF_LAYOUT_OPT_PARSER_RESULT;
+	err = dpni_set_tx_buffer_layout(dflt_mc_io, priv->dpni_handle,
+					&priv->buf_layout);
+	if (err) {
+		printf("dpni_set_tx_buffer_layout() failed");
+		goto err_buf_layout;
+	}
+
+	/* ... tx-confirm. */
+	priv->buf_layout.options &= ~DPNI_BUF_LAYOUT_OPT_PRIVATE_DATA_SIZE;
+	err = dpni_set_tx_conf_buffer_layout(dflt_mc_io, priv->dpni_handle,
+					     &priv->buf_layout);
+	if (err) {
+		printf("dpni_set_tx_conf_buffer_layout() failed");
+		goto err_buf_layout;
+	}
+
+	/* Now that we've set our tx buffer layout, retrieve the minimum
+	 * required tx data offset.
+	 */
+	err = dpni_get_tx_data_offset(dflt_mc_io, priv->dpni_handle,
+				      &priv->tx_data_offset);
+	if (err) {
+		printf("dpni_get_tx_data_offset() failed\n");
+		goto err_data_offset;
+	}
+
+	/* Warn in case TX data offset is not multiple of 64 bytes. */
+	WARN_ON(priv->tx_data_offset % 64);
+
+	/* Accomodate SWA space. */
+	priv->tx_data_offset += LDPAA_ETH_SWA_SIZE;
+	debug("priv->tx_data_offset=%d\n", priv->tx_data_offset);
+
+	return 0;
+
+err_data_offset:
+err_buf_layout:
+err_get_attr:
+	dpni_close(dflt_mc_io, priv->dpni_handle);
+err_open:
+	return err;
+}
+
+static int ldpaa_dpni_bind(struct ldpaa_eth_priv *priv)
+{
+	struct dpni_pools_cfg pools_params;
+	struct dpni_tx_flow_cfg dflt_tx_flow;
+	int err = 0;
+
+	pools_params.num_dpbp = 1;
+	pools_params.pools[0].dpbp_id = (uint16_t)dflt_dpbp->dpbp_attr.id;
+	pools_params.pools[0].buffer_size = LDPAA_ETH_RX_BUFFER_SIZE;
+	err = dpni_set_pools(dflt_mc_io, priv->dpni_handle, &pools_params);
+	if (err) {
+		printf("dpni_set_pools() failed\n");
+		return err;
+	}
+
+	priv->tx_flow_id = DPNI_NEW_FLOW_ID;
+	memset(&dflt_tx_flow, 0, sizeof(dflt_tx_flow));
+
+	err = dpni_set_tx_flow(dflt_mc_io, priv->dpni_handle,
+			       &priv->tx_flow_id, &dflt_tx_flow);
+	if (err) {
+		printf("dpni_set_tx_flow() failed\n");
+		return err;
+	}
+
+	return 0;
+}
+
+static int ldpaa_eth_netdev_init(struct eth_device *net_dev)
+{
+	int err;
+	struct ldpaa_eth_priv *priv = (struct ldpaa_eth_priv *)net_dev->priv;
+
+	sprintf(net_dev->name, "DPNI%d", priv->dpni_id);
+
+	net_dev->iobase = 0;
+	net_dev->init = ldpaa_eth_open;
+	net_dev->halt = ldpaa_eth_stop;
+	net_dev->send = ldpaa_eth_tx;
+	net_dev->recv = ldpaa_eth_pull_dequeue_rx;
+/*
+	TODO: PHY MDIO information
+	priv->bus = info->bus;
+	priv->phyaddr = info->phy_addr;
+	priv->enet_if = info->enet_if;
+*/
+
+	if (init_phy(net_dev))
+		return 0;
+
+	err = eth_register(net_dev);
+	if (err < 0) {
+		printf("eth_register() = %d\n", err);
+		return err;
+	}
+
+	return 0;
+}
+
+int ldpaa_eth_init(struct dprc_obj_desc obj_desc)
+{
+	struct eth_device		*net_dev = NULL;
+	struct ldpaa_eth_priv		*priv = NULL;
+	int				err = 0;
+
+
+	/* Net device */
+	net_dev = (struct eth_device *)malloc(sizeof(struct eth_device));
+	if (!net_dev) {
+		printf("eth_device malloc() failed\n");
+		return -ENOMEM;
+	}
+	memset(net_dev, 0, sizeof(struct eth_device));
+
+	/* alloc the ldpaa ethernet private struct */
+	priv = (struct ldpaa_eth_priv *)malloc(sizeof(struct ldpaa_eth_priv));
+	if (!priv) {
+		printf("ldpaa_eth_priv malloc() failed\n");
+		return -ENOMEM;
+	}
+	memset(priv, 0, sizeof(struct ldpaa_eth_priv));
+
+	net_dev->priv = (void *)priv;
+	priv->net_dev = (struct eth_device *)net_dev;
+	priv->dpni_id = obj_desc.id;
+
+	err = ldpaa_eth_netdev_init(net_dev);
+	if (err)
+		goto err_netdev_init;
+
+	debug("ldpaa ethernet: Probed interface %s\n", net_dev->name);
+	return 0;
+
+err_netdev_init:
+	free(priv);
+	net_dev->priv = NULL;
+	free(net_dev);
+
+	return err;
+}
diff --git a/drivers/net/ldpaa_eth/ldpaa_eth.h b/drivers/net/ldpaa_eth/ldpaa_eth.h
new file mode 100644
index 0000000..3107ab6
--- /dev/null
+++ b/drivers/net/ldpaa_eth/ldpaa_eth.h
@@ -0,0 +1,149 @@
+/*
+ * Copyright (C) 2014 Freescale Semiconductor
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#ifndef __LDPAA_ETH_H
+#define __LDPAA_ETH_H
+
+#include <linux/netdevice.h>
+#include <fsl-mc/fsl_mc.h>
+#include <fsl-mc/fsl_dpaa_fd.h>
+#include <fsl-mc/fsl_dprc.h>
+#include <fsl-mc/fsl_dpni.h>
+#include <fsl-mc/fsl_dpbp.h>
+#include <fsl-mc/fsl_dpio.h>
+#include <fsl-mc/fsl_qbman_portal.h>
+#include <fsl-mc/fsl_mc_private.h>
+
+
+enum ldpaa_eth_type {
+	LDPAA_ETH_1G_E,
+	LDPAA_ETH_10G_E,
+};
+
+/* Arbitrary values for now, but we'll need to tune */
+#define LDPAA_ETH_NUM_BUFS		(2 * 7)
+#define LDPAA_ETH_REFILL_THRESH		(LDPAA_ETH_NUM_BUFS/2)
+#define LDPAA_ETH_RX_BUFFER_SIZE	2048
+
+/* Hardware requires alignment for ingress/egress buffer addresses
+ * and ingress buffer lengths.
+ */
+#define LDPAA_ETH_BUF_ALIGN		64
+
+/* So far we're only accomodating a skb backpointer in the frame's
+ * software annotation, but the hardware options are either 0 or 64.
+ */
+#define LDPAA_ETH_SWA_SIZE		64
+
+/* Annotation valid bits in FD FRC */
+#define LDPAA_FD_FRC_FASV		0x8000
+#define LDPAA_FD_FRC_FAEADV		0x4000
+#define LDPAA_FD_FRC_FAPRV		0x2000
+#define LDPAA_FD_FRC_FAIADV		0x1000
+#define LDPAA_FD_FRC_FASWOV		0x0800
+#define LDPAA_FD_FRC_FAICFDV		0x0400
+
+/* Annotation bits in FD CTRL */
+#define LDPAA_FD_CTRL_ASAL		0x00020000	/* ASAL = 128 */
+#define LDPAA_FD_CTRL_PTA		0x00800000
+#define LDPAA_FD_CTRL_PTV1		0x00400000
+
+/* TODO: we may want to move this and other WRIOP related defines
+ * to a separate header
+ */
+/* Frame annotation status */
+struct ldpaa_fas {
+	u8 reserved;
+	u8 ppid;
+	__le16 ifpid;
+	__le32 status;
+} __packed;
+
+/* Debug frame, otherwise supposed to be discarded */
+#define LDPAA_ETH_FAS_DISC		0x80000000
+/* MACSEC frame */
+#define LDPAA_ETH_FAS_MS		0x40000000
+#define LDPAA_ETH_FAS_PTP		0x08000000
+/* Ethernet multicast frame */
+#define LDPAA_ETH_FAS_MC		0x04000000
+/* Ethernet broadcast frame */
+#define LDPAA_ETH_FAS_BC		0x02000000
+#define LDPAA_ETH_FAS_KSE		0x00040000
+#define LDPAA_ETH_FAS_EOFHE		0x00020000
+#define LDPAA_ETH_FAS_MNLE		0x00010000
+#define LDPAA_ETH_FAS_TIDE		0x00008000
+#define LDPAA_ETH_FAS_PIEE		0x00004000
+/* Frame length error */
+#define LDPAA_ETH_FAS_FLE		0x00002000
+/* Frame physical error; our favourite pastime */
+#define LDPAA_ETH_FAS_FPE		0x00001000
+#define LDPAA_ETH_FAS_PTE		0x00000080
+#define LDPAA_ETH_FAS_ISP		0x00000040
+#define LDPAA_ETH_FAS_PHE		0x00000020
+#define LDPAA_ETH_FAS_BLE		0x00000010
+/* L3 csum validation performed */
+#define LDPAA_ETH_FAS_L3CV		0x00000008
+/* L3 csum error */
+#define LDPAA_ETH_FAS_L3CE		0x00000004
+/* L4 csum validation performed */
+#define LDPAA_ETH_FAS_L4CV		0x00000002
+/* L4 csum error */
+#define LDPAA_ETH_FAS_L4CE		0x00000001
+/* These bits always signal errors */
+#define LDPAA_ETH_RX_ERR_MASK		(LDPAA_ETH_FAS_DISC	| \
+					 LDPAA_ETH_FAS_KSE	| \
+					 LDPAA_ETH_FAS_EOFHE	| \
+					 LDPAA_ETH_FAS_MNLE	| \
+					 LDPAA_ETH_FAS_TIDE	| \
+					 LDPAA_ETH_FAS_PIEE	| \
+					 LDPAA_ETH_FAS_FLE	| \
+					 LDPAA_ETH_FAS_FPE	| \
+					 LDPAA_ETH_FAS_PTE	| \
+					 LDPAA_ETH_FAS_ISP	| \
+					 LDPAA_ETH_FAS_PHE	| \
+					 LDPAA_ETH_FAS_BLE	| \
+					 LDPAA_ETH_FAS_L3CE	| \
+					 LDPAA_ETH_FAS_L4CE)
+/* Unsupported features in the ingress */
+#define LDPAA_ETH_RX_UNSUPP_MASK	LDPAA_ETH_FAS_MS
+/* TODO trim down the bitmask; not all of them apply to Tx-confirm */
+#define LDPAA_ETH_TXCONF_ERR_MASK	(LDPAA_ETH_FAS_KSE	| \
+					 LDPAA_ETH_FAS_EOFHE	| \
+					 LDPAA_ETH_FAS_MNLE	| \
+					 LDPAA_ETH_FAS_TIDE)
+
+struct ldpaa_eth_priv {
+	struct eth_device *net_dev;
+	int dpni_id;
+	uint16_t dpni_handle;
+	struct dpni_attr dpni_attrs;
+	/* Insofar as the MC is concerned, we're using one layout on all 3 types
+	 * of buffers (Rx, Tx, Tx-Conf).
+	 */
+	struct dpni_buffer_layout buf_layout;
+	uint16_t tx_data_offset;
+
+	uint32_t rx_dflt_fqid;
+	uint16_t tx_qdid;
+	uint32_t tx_conf_fqid;
+	uint16_t tx_flow_id;
+
+	enum ldpaa_eth_type type;	/* 1G or 10G ethernet */
+	struct phy_device *phydev;
+};
+
+extern struct fsl_mc_io *dflt_mc_io;
+extern struct fsl_dpbp_obj *dflt_dpbp;
+extern struct fsl_dpio_obj *dflt_dpio;
+
+static void ldpaa_dpbp_drain_cnt(int count);
+static void ldpaa_dpbp_drain(void);
+static int ldpaa_dpbp_seed(uint16_t bpid);
+static void ldpaa_dpbp_free(void);
+static int ldpaa_dpni_setup(struct ldpaa_eth_priv *priv);
+static int ldpaa_dpbp_setup(void);
+static int ldpaa_dpni_bind(struct ldpaa_eth_priv *priv);
+#endif	/* __LDPAA_H */
diff --git a/drivers/net/ldpaa_eth/ldpaa_wriop.c b/drivers/net/ldpaa_eth/ldpaa_wriop.c
new file mode 100644
index 0000000..926057a
--- /dev/null
+++ b/drivers/net/ldpaa_eth/ldpaa_wriop.c
@@ -0,0 +1,146 @@
+/*
+ * Copyright (C) 2015 Freescale Semiconductor
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <asm/io.h>
+#include <asm/types.h>
+#include <malloc.h>
+#include <net.h>
+#include <linux/compat.h>
+#include <asm/arch/fsl_serdes.h>
+#include <fsl-mc/ldpaa_wriop.h>
+
+struct wriop_dpmac_info dpmac_info[NUM_WRIOP_PORTS];
+
+__weak phy_interface_t wriop_dpmac_enet_if(int dpmac_id, int lane_prtc)
+{
+	return PHY_INTERFACE_MODE_NONE;
+}
+
+void wriop_init_dpmac(int sd, int dpmac_id, int lane_prtcl)
+{
+	phy_interface_t enet_if;
+	int index = dpmac_id + sd * 8;
+
+	dpmac_info[index].enabled = 0;
+	dpmac_info[index].id = 0;
+	dpmac_info[index].enet_if = PHY_INTERFACE_MODE_NONE;
+
+	enet_if = wriop_dpmac_enet_if(index, lane_prtcl);
+	if (enet_if != PHY_INTERFACE_MODE_NONE) {
+		dpmac_info[index].enabled = 1;
+		dpmac_info[index].id = index;
+		dpmac_info[index].enet_if = enet_if;
+	}
+}
+
+/*TODO what it do */
+static int wriop_dpmac_to_index(int dpmac_id)
+{
+	int i;
+
+	for (i = WRIOP1_DPMAC1; i < NUM_WRIOP_PORTS; i++) {
+		if (dpmac_info[i].id == dpmac_id)
+			return i;
+	}
+
+	return -1;
+}
+
+void wriop_disable_dpmac(int dpmac_id)
+{
+	int i = wriop_dpmac_to_index(dpmac_id);
+
+	if (i == -1)
+		return;
+
+	dpmac_info[i].enabled = 0;
+	wriop_dpmac_disable(dpmac_id);
+}
+
+void wriop_enable_dpmac(int dpmac_id)
+{
+	int i = wriop_dpmac_to_index(dpmac_id);
+
+	if (i == -1)
+		return;
+
+	dpmac_info[i].enabled = 1;
+	wriop_dpmac_enable(dpmac_id);
+}
+
+void wriop_set_mdio(int dpmac_id, struct mii_dev *bus)
+{
+	int i = wriop_dpmac_to_index(dpmac_id);
+
+	if (i == -1)
+		return;
+
+	dpmac_info[i].bus = bus;
+}
+
+struct mii_dev *wriop_get_mdio(int dpmac_id)
+{
+	int i = wriop_dpmac_to_index(dpmac_id);
+
+	if (i == -1)
+		return NULL;
+
+	return dpmac_info[i].bus;
+}
+
+void wriop_set_phy_address(int dpmac_id, int address)
+{
+	int i = wriop_dpmac_to_index(dpmac_id);
+
+	if (i == -1)
+		return;
+
+	dpmac_info[i].phy_addr = address;
+}
+
+int wriop_get_phy_address(int dpmac_id)
+{
+	int i = wriop_dpmac_to_index(dpmac_id);
+
+	if (i == -1)
+		return -1;
+
+	return dpmac_info[i].phy_addr;
+}
+
+void wriop_set_phy_dev(int dpmac_id, struct phy_device *phydev)
+{
+	int i = wriop_dpmac_to_index(dpmac_id);
+
+	if (i == -1)
+		return;
+
+	dpmac_info[i].phydev = phydev;
+}
+
+struct phy_device *wriop_get_phy_dev(int dpmac_id)
+{
+	int i = wriop_dpmac_to_index(dpmac_id);
+
+	if (i == -1)
+		return NULL;
+
+	return dpmac_info[i].phydev;
+}
+
+phy_interface_t wriop_get_enet_if(int dpmac_id)
+{
+	int i = wriop_dpmac_to_index(dpmac_id);
+
+	if (i == -1)
+		return PHY_INTERFACE_MODE_NONE;
+
+	if (dpmac_info[i].enabled)
+		return dpmac_info[i].enet_if;
+
+	return PHY_INTERFACE_MODE_NONE;
+}
diff --git a/drivers/net/ldpaa_eth/ls2085a.c b/drivers/net/ldpaa_eth/ls2085a.c
new file mode 100644
index 0000000..6b7960a
--- /dev/null
+++ b/drivers/net/ldpaa_eth/ls2085a.c
@@ -0,0 +1,83 @@
+/*
+ * Copyright 2015 Freescale Semiconductor, Inc.
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+#include <common.h>
+#include <phy.h>
+#include <fsl-mc/ldpaa_wriop.h>
+#include <asm/io.h>
+#include <asm/arch-fsl-lsch3/immap_lsch3.h>
+#include <asm/arch/fsl_serdes.h>
+#include <fsl-mc/ldpaa_wriop.h>
+
+u32 dpmac_to_devdisr[] = {
+	[WRIOP1_DPMAC1] = FSL_CHASSIS3_DEVDISR2_DPMAC1,
+	[WRIOP1_DPMAC2] = FSL_CHASSIS3_DEVDISR2_DPMAC2,
+	[WRIOP1_DPMAC3] = FSL_CHASSIS3_DEVDISR2_DPMAC3,
+	[WRIOP1_DPMAC4] = FSL_CHASSIS3_DEVDISR2_DPMAC4,
+	[WRIOP1_DPMAC5] = FSL_CHASSIS3_DEVDISR2_DPMAC5,
+	[WRIOP1_DPMAC6] = FSL_CHASSIS3_DEVDISR2_DPMAC6,
+	[WRIOP1_DPMAC7] = FSL_CHASSIS3_DEVDISR2_DPMAC7,
+	[WRIOP1_DPMAC8] = FSL_CHASSIS3_DEVDISR2_DPMAC8,
+	[WRIOP1_DPMAC9] = FSL_CHASSIS3_DEVDISR2_DPMAC9,
+	[WRIOP1_DPMAC10] = FSL_CHASSIS3_DEVDISR2_DPMAC10,
+	[WRIOP1_DPMAC11] = FSL_CHASSIS3_DEVDISR2_DPMAC11,
+	[WRIOP1_DPMAC12] = FSL_CHASSIS3_DEVDISR2_DPMAC12,
+	[WRIOP1_DPMAC13] = FSL_CHASSIS3_DEVDISR2_DPMAC13,
+	[WRIOP1_DPMAC14] = FSL_CHASSIS3_DEVDISR2_DPMAC14,
+	[WRIOP1_DPMAC15] = FSL_CHASSIS3_DEVDISR2_DPMAC15,
+	[WRIOP1_DPMAC16] = FSL_CHASSIS3_DEVDISR2_DPMAC16,
+	[WRIOP1_DPMAC17] = FSL_CHASSIS3_DEVDISR2_DPMAC17,
+	[WRIOP1_DPMAC18] = FSL_CHASSIS3_DEVDISR2_DPMAC18,
+	[WRIOP1_DPMAC19] = FSL_CHASSIS3_DEVDISR2_DPMAC19,
+	[WRIOP1_DPMAC20] = FSL_CHASSIS3_DEVDISR2_DPMAC20,
+	[WRIOP1_DPMAC21] = FSL_CHASSIS3_DEVDISR2_DPMAC21,
+	[WRIOP1_DPMAC22] = FSL_CHASSIS3_DEVDISR2_DPMAC22,
+	[WRIOP1_DPMAC23] = FSL_CHASSIS3_DEVDISR2_DPMAC23,
+	[WRIOP1_DPMAC24] = FSL_CHASSIS3_DEVDISR2_DPMAC24,
+};
+
+static int is_device_disabled(int dpmac_id)
+{
+	struct ccsr_gur __iomem *gur = (void *)CONFIG_SYS_FSL_GUTS_ADDR;
+	u32 devdisr2 = in_le32(&gur->devdisr2);
+
+	return dpmac_to_devdisr[dpmac_id] & devdisr2;
+}
+
+void wriop_dpmac_disable(int dpmac_id)
+{
+	struct ccsr_gur __iomem *gur = (void *)CONFIG_SYS_FSL_GUTS_ADDR;
+
+	setbits_le32(&gur->devdisr2, dpmac_to_devdisr[dpmac_id]);
+}
+
+void wriop_dpmac_enable(int dpmac_id)
+{
+	struct ccsr_gur __iomem *gur = (void *)CONFIG_SYS_FSL_GUTS_ADDR;
+
+	clrbits_le32(&gur->devdisr2, dpmac_to_devdisr[dpmac_id]);
+}
+
+phy_interface_t wriop_dpmac_enet_if(int dpmac_id, int lane_prtcl)
+{
+	enum srds_prtcl;
+
+	if (is_device_disabled(dpmac_id + 1))
+		return PHY_INTERFACE_MODE_NONE;
+
+	if (lane_prtcl >= SGMII1 && lane_prtcl <= SGMII16)
+		return PHY_INTERFACE_MODE_SGMII;
+
+	if (lane_prtcl >= XFI1 && lane_prtcl <= XFI8)
+		return PHY_INTERFACE_MODE_XGMII;
+
+	if (lane_prtcl >= XAUI1 && lane_prtcl <= XAUI2)
+		return PHY_INTERFACE_MODE_XGMII;
+
+	if (lane_prtcl >= QSGMII_A && lane_prtcl <= QSGMII_D)
+		return PHY_INTERFACE_MODE_QSGMII;
+
+	return PHY_INTERFACE_MODE_NONE;
+}
diff --git a/drivers/net/lpc32xx_eth.c b/drivers/net/lpc32xx_eth.c
index fcadf0c..8dcbb4a 100644
--- a/drivers/net/lpc32xx_eth.c
+++ b/drivers/net/lpc32xx_eth.c
@@ -419,10 +419,12 @@ static int lpc32xx_eth_recv(struct eth_device *dev)
 	rx_index = readl(&regs->rxconsumeindex);
 
 	/* if data was valid, pass it on */
-	if (!(bufs->rx_stat[rx_index].statusinfo & RX_STAT_ERRORS))
-		NetReceive(&(bufs->rx_buf[rx_index*PKTSIZE_ALIGN]),
-			   (bufs->rx_stat[rx_index].statusinfo
-			    & RX_STAT_RXSIZE) + 1);
+	if (!(bufs->rx_stat[rx_index].statusinfo & RX_STAT_ERRORS)) {
+		net_process_received_packet(
+			&(bufs->rx_buf[rx_index * PKTSIZE_ALIGN]),
+			(bufs->rx_stat[rx_index].statusinfo
+			 & RX_STAT_RXSIZE) + 1);
+	}
 
 	/* pass receive slot back to DMA engine */
 	rx_index = (rx_index + 1) % RX_BUF_COUNT;
diff --git a/drivers/net/macb.c b/drivers/net/macb.c
index 170ff06..4e1a7fe 100644
--- a/drivers/net/macb.c
+++ b/drivers/net/macb.c
@@ -347,14 +347,14 @@ static int macb_recv(struct eth_device *netdev)
 				headlen = 128 * (MACB_RX_RING_SIZE
 						 - macb->rx_tail);
 				taillen = length - headlen;
-				memcpy((void *)NetRxPackets[0],
+				memcpy((void *)net_rx_packets[0],
 				       buffer, headlen);
-				memcpy((void *)NetRxPackets[0] + headlen,
+				memcpy((void *)net_rx_packets[0] + headlen,
 				       macb->rx_buffer, taillen);
-				buffer = (void *)NetRxPackets[0];
+				buffer = (void *)net_rx_packets[0];
 			}
 
-			NetReceive(buffer, length);
+			net_process_received_packet(buffer, length);
 			if (++rx_tail >= MACB_RX_RING_SIZE)
 				rx_tail = 0;
 			reclaim_rx_buffers(macb, rx_tail);
@@ -595,7 +595,7 @@ static int macb_init(struct eth_device *netdev, bd_t *bd)
 	}
 
 	/* update the ethaddr */
-	if (is_valid_ether_addr(netdev->enetaddr)) {
+	if (is_valid_ethaddr(netdev->enetaddr)) {
 		macb_write_hwaddr(netdev);
 	} else {
 		printf("%s: mac address is not valid\n", netdev->name);
diff --git a/drivers/net/mcffec.c b/drivers/net/mcffec.c
index 7c4b210..fd73099 100644
--- a/drivers/net/mcffec.c
+++ b/drivers/net/mcffec.c
@@ -219,7 +219,8 @@ int fec_recv(struct eth_device *dev)
 
 			length -= 4;
 			/* Pass the packet up to the protocol layers. */
-			NetReceive(NetRxPackets[info->rxIdx], length);
+			net_process_received_packet(net_rx_packets[info->rxIdx],
+						    length);
 
 			fecp->eir |= FEC_EIR_RXF;
 		}
@@ -477,7 +478,7 @@ int fec_init(struct eth_device *dev, bd_t * bd)
 	for (i = 0; i < PKTBUFSRX; i++) {
 		info->rxbd[i].cbd_sc = BD_ENET_RX_EMPTY;
 		info->rxbd[i].cbd_datlen = 0;	/* Reset */
-		info->rxbd[i].cbd_bufaddr = (uint) NetRxPackets[i];
+		info->rxbd[i].cbd_bufaddr = (uint) net_rx_packets[i];
 	}
 	info->rxbd[PKTBUFSRX - 1].cbd_sc |= BD_ENET_RX_WRAP;
 
diff --git a/drivers/net/mpc512x_fec.c b/drivers/net/mpc512x_fec.c
index 427e0b8..22ea114 100644
--- a/drivers/net/mpc512x_fec.c
+++ b/drivers/net/mpc512x_fec.c
@@ -591,7 +591,8 @@ static int mpc512x_fec_recv (struct eth_device *dev)
 			rx_buff_idx = frame_length;
 
 			if (pRbd->status & FEC_RBD_LAST) {
-				NetReceive ((uchar*)rx_buff, frame_length);
+				net_process_received_packet((uchar *)rx_buff,
+							    frame_length);
 				rx_buff_idx = 0;
 			}
 		}
diff --git a/drivers/net/mpc5xxx_fec.c b/drivers/net/mpc5xxx_fec.c
index d2a8ae0..2ebd176 100644
--- a/drivers/net/mpc5xxx_fec.c
+++ b/drivers/net/mpc5xxx_fec.c
@@ -859,7 +859,7 @@ static int mpc5xxx_fec_recv(struct eth_device *dev)
 			 */
 			memcpy(buff, frame->head, 14);
 			memcpy(buff + 14, frame->data, frame_length);
-			NetReceive(buff, frame_length);
+			net_process_received_packet(buff, frame_length);
 			len = frame_length;
 		}
 		/*
diff --git a/drivers/net/mvgbe.c b/drivers/net/mvgbe.c
index 6b31a82..ab5aa68 100644
--- a/drivers/net/mvgbe.c
+++ b/drivers/net/mvgbe.c
@@ -66,12 +66,12 @@ static int smi_reg_read(const char *devname, u8 phy_adr, u8 reg_ofs, u16 * data)
 	/* check parameters */
 	if (phy_adr > PHYADR_MASK) {
 		printf("Err..(%s) Invalid PHY address %d\n",
-			__FUNCTION__, phy_adr);
+			__func__, phy_adr);
 		return -EFAULT;
 	}
 	if (reg_ofs > PHYREG_MASK) {
 		printf("Err..(%s) Invalid register offset %d\n",
-			__FUNCTION__, reg_ofs);
+			__func__, reg_ofs);
 		return -EFAULT;
 	}
 
@@ -81,7 +81,7 @@ static int smi_reg_read(const char *devname, u8 phy_adr, u8 reg_ofs, u16 * data)
 		/* read smi register */
 		smi_reg = MVGBE_REG_RD(MVGBE_SMI_REG);
 		if (timeout-- == 0) {
-			printf("Err..(%s) SMI busy timeout\n", __FUNCTION__);
+			printf("Err..(%s) SMI busy timeout\n", __func__);
 			return -EFAULT;
 		}
 	} while (smi_reg & MVGBE_PHY_SMI_BUSY_MASK);
@@ -102,7 +102,7 @@ static int smi_reg_read(const char *devname, u8 phy_adr, u8 reg_ofs, u16 * data)
 		smi_reg = MVGBE_REG_RD(MVGBE_SMI_REG);
 		if (timeout-- == 0) {
 			printf("Err..(%s) SMI read ready timeout\n",
-				__FUNCTION__);
+				__func__);
 			return -EFAULT;
 		}
 	} while (!(smi_reg & MVGBE_PHY_SMI_READ_VALID_MASK));
@@ -113,8 +113,8 @@ static int smi_reg_read(const char *devname, u8 phy_adr, u8 reg_ofs, u16 * data)
 
 	*data = (u16) (MVGBE_REG_RD(MVGBE_SMI_REG) & MVGBE_PHY_SMI_DATA_MASK);
 
-	debug("%s:(adr %d, off %d) value= %04x\n", __FUNCTION__, phy_adr,
-		reg_ofs, *data);
+	debug("%s:(adr %d, off %d) value= %04x\n", __func__, phy_adr, reg_ofs,
+	      *data);
 
 	return 0;
 }
@@ -142,11 +142,11 @@ static int smi_reg_write(const char *devname, u8 phy_adr, u8 reg_ofs, u16 data)
 
 	/* check parameters */
 	if (phy_adr > PHYADR_MASK) {
-		printf("Err..(%s) Invalid phy address\n", __FUNCTION__);
+		printf("Err..(%s) Invalid phy address\n", __func__);
 		return -EINVAL;
 	}
 	if (reg_ofs > PHYREG_MASK) {
-		printf("Err..(%s) Invalid register offset\n", __FUNCTION__);
+		printf("Err..(%s) Invalid register offset\n", __func__);
 		return -EINVAL;
 	}
 
@@ -156,7 +156,7 @@ static int smi_reg_write(const char *devname, u8 phy_adr, u8 reg_ofs, u16 data)
 		/* read smi register */
 		smi_reg = MVGBE_REG_RD(MVGBE_SMI_REG);
 		if (timeout-- == 0) {
-			printf("Err..(%s) SMI busy timeout\n", __FUNCTION__);
+			printf("Err..(%s) SMI busy timeout\n", __func__);
 			return -ETIME;
 		}
 	} while (smi_reg & MVGBE_PHY_SMI_BUSY_MASK);
@@ -583,7 +583,7 @@ static int mvgbe_send(struct eth_device *dev, void *dataptr, int datasize)
 		if ((cmd_sts & (MVGBE_ERROR_SUMMARY | MVGBE_TX_LAST_FRAME)) ==
 				(MVGBE_ERROR_SUMMARY | MVGBE_TX_LAST_FRAME) &&
 				cmd_sts & (MVGBE_UR_ERROR | MVGBE_RL_ERROR)) {
-			printf("Err..(%s) in xmit packet\n", __FUNCTION__);
+			printf("Err..(%s) in xmit packet\n", __func__);
 			return -1;
 		}
 		cmd_sts = readl(&p_txdesc->cmd_sts);
@@ -604,14 +604,14 @@ static int mvgbe_recv(struct eth_device *dev)
 		if (timeout < MVGBE_PHY_SMI_TIMEOUT)
 			timeout++;
 		else {
-			debug("%s time out...\n", __FUNCTION__);
+			debug("%s time out...\n", __func__);
 			return -1;
 		}
 	} while (readl(&p_rxdesc_curr->cmd_sts) & MVGBE_BUFFER_OWNED_BY_DMA);
 
 	if (p_rxdesc_curr->byte_cnt != 0) {
 		debug("%s: Received %d byte Packet @ 0x%x (cmd_sts= %08x)\n",
-			__FUNCTION__, (u32) p_rxdesc_curr->byte_cnt,
+			__func__, (u32) p_rxdesc_curr->byte_cnt,
 			(u32) p_rxdesc_curr->buf_ptr,
 			(u32) p_rxdesc_curr->cmd_sts);
 	}
@@ -628,21 +628,24 @@ static int mvgbe_recv(struct eth_device *dev)
 		!= (MVGBE_RX_FIRST_DESC | MVGBE_RX_LAST_DESC)) {
 
 		printf("Err..(%s) Dropping packet spread on"
-			" multiple descriptors\n", __FUNCTION__);
+			" multiple descriptors\n", __func__);
 
 	} else if (cmd_sts & MVGBE_ERROR_SUMMARY) {
 
 		printf("Err..(%s) Dropping packet with errors\n",
-			__FUNCTION__);
+			__func__);
 
 	} else {
 		/* !!! call higher layer processing */
 		debug("%s: Sending Received packet to"
-			" upper layer (NetReceive)\n", __FUNCTION__);
+		      " upper layer (net_process_received_packet)\n",
+		      __func__);
 
 		/* let the upper layer handle the packet */
-		NetReceive((p_rxdesc_curr->buf_ptr + RX_BUF_OFFSET),
-			(int)(p_rxdesc_curr->byte_cnt - RX_BUF_OFFSET));
+		net_process_received_packet((p_rxdesc_curr->buf_ptr +
+					     RX_BUF_OFFSET),
+					    (int)(p_rxdesc_curr->byte_cnt -
+						  RX_BUF_OFFSET));
 	}
 	/*
 	 * free these descriptors and point next in the ring
@@ -747,7 +750,7 @@ error2:
 			free(dmvgbe);
 error1:
 			printf("Err.. %s Failed to allocate memory\n",
-				__FUNCTION__);
+				__func__);
 			return -1;
 		}
 
@@ -767,7 +770,7 @@ error1:
 #endif
 		default:	/* this should never happen */
 			printf("Err..(%s) Invalid device number %d\n",
-				__FUNCTION__, devnum);
+				__func__, devnum);
 			return -1;
 		}
 
diff --git a/drivers/net/mvneta.c b/drivers/net/mvneta.c
index a2a69b4..efaae16 100644
--- a/drivers/net/mvneta.c
+++ b/drivers/net/mvneta.c
@@ -1572,7 +1572,7 @@ static int mvneta_recv(struct eth_device *dev)
 		 * No cache invalidation needed here, since the rx_buffer's are
 		 * located in a uncached memory region
 		 */
-		NetReceive(data, rx_bytes);
+		net_process_received_packet(data, rx_bytes);
 	}
 
 	/* Update rxq management counters */
diff --git a/drivers/net/natsemi.c b/drivers/net/natsemi.c
index 04743bd..0ed9bb5 100644
--- a/drivers/net/natsemi.c
+++ b/drivers/net/natsemi.c
@@ -841,7 +841,8 @@ natsemi_poll(struct eth_device *dev)
 		     rx_status);
 		retstat = 0;
 	} else {		/* give packet to higher level routine */
-		NetReceive((rxb + cur_rx * RX_BUF_SIZE), length);
+		net_process_received_packet((rxb + cur_rx * RX_BUF_SIZE),
+					    length);
 		retstat = 1;
 	}
 
diff --git a/drivers/net/ne2000_base.c b/drivers/net/ne2000_base.c
index ef35922..07a7cec 100644
--- a/drivers/net/ne2000_base.c
+++ b/drivers/net/ne2000_base.c
@@ -665,7 +665,7 @@ void uboot_push_packet_len(int len) {
 	dp83902a_recv(&pbuf[0], len);
 
 	/*Just pass it to the upper layer*/
-	NetReceive(&pbuf[0], len);
+	net_process_received_packet(&pbuf[0], len);
 }
 
 void uboot_push_tx_done(int key, int val) {
diff --git a/drivers/net/netconsole.c b/drivers/net/netconsole.c
index 677c89f..31042a6 100644
--- a/drivers/net/netconsole.c
+++ b/drivers/net/netconsole.c
@@ -23,7 +23,7 @@ static int input_recursion;
 static int output_recursion;
 static int net_timeout;
 static uchar nc_ether[6]; /* server enet address */
-static IPaddr_t nc_ip; /* server ip */
+static struct in_addr nc_ip; /* server ip */
 static short nc_out_port; /* target output port */
 static short nc_in_port; /* source input port */
 static const char *output_packet; /* used by first send udp */
@@ -35,42 +35,43 @@ static int output_packet_len;
 enum proto_t net_loop_last_protocol = BOOTP;
 
 static void nc_wait_arp_handler(uchar *pkt, unsigned dest,
-				 IPaddr_t sip, unsigned src,
+				 struct in_addr sip, unsigned src,
 				 unsigned len)
 {
 	net_set_state(NETLOOP_SUCCESS); /* got arp reply - quit net loop */
 }
 
-static void nc_handler(uchar *pkt, unsigned dest, IPaddr_t sip, unsigned src,
-			unsigned len)
+static void nc_handler(uchar *pkt, unsigned dest, struct in_addr sip,
+		       unsigned src, unsigned len)
 {
 	if (input_size)
 		net_set_state(NETLOOP_SUCCESS); /* got input - quit net loop */
 }
 
-static void nc_timeout(void)
+static void nc_timeout_handler(void)
 {
 	net_set_state(NETLOOP_SUCCESS);
 }
 
-static int is_broadcast(IPaddr_t ip)
+static int is_broadcast(struct in_addr ip)
 {
-	static IPaddr_t netmask;
-	static IPaddr_t our_ip;
+	static struct in_addr netmask;
+	static struct in_addr our_ip;
 	static int env_changed_id;
 	int env_id = get_env_id();
 
 	/* update only when the environment has changed */
 	if (env_changed_id != env_id) {
-		netmask = getenv_IPaddr("netmask");
-		our_ip = getenv_IPaddr("ipaddr");
+		netmask = getenv_ip("netmask");
+		our_ip = getenv_ip("ipaddr");
 
 		env_changed_id = env_id;
 	}
 
-	return (ip == ~0 ||				/* 255.255.255.255 */
-	    ((netmask & our_ip) == (netmask & ip) &&	/* on the same net */
-	    (netmask | ip) == ~0));		/* broadcast to our net */
+	return (ip.s_addr == ~0 || /* 255.255.255.255 (global bcast) */
+		((netmask.s_addr & our_ip.s_addr) ==
+		 (netmask.s_addr & ip.s_addr) && /* on the same net and */
+		 (netmask.s_addr | ip.s_addr) == ~0)); /* bcast to our net */
 }
 
 static int refresh_settings_from_env(void)
@@ -82,16 +83,17 @@ static int refresh_settings_from_env(void)
 	/* update only when the environment has changed */
 	if (env_changed_id != env_id) {
 		if (getenv("ncip")) {
-			nc_ip = getenv_IPaddr("ncip");
-			if (!nc_ip)
+			nc_ip = getenv_ip("ncip");
+			if (!nc_ip.s_addr)
 				return -1;	/* ncip is 0.0.0.0 */
 			p = strchr(getenv("ncip"), ':');
 			if (p != NULL) {
 				nc_out_port = simple_strtoul(p + 1, NULL, 10);
 				nc_in_port = nc_out_port;
 			}
-		} else
-			nc_ip = ~0; /* ncip is not set, so broadcast */
+		} else {
+			nc_ip.s_addr = ~0; /* ncip is not set, so broadcast */
+		}
 
 		p = getenv("ncoutport");
 		if (p != NULL)
@@ -111,27 +113,28 @@ static int refresh_settings_from_env(void)
 }
 
 /**
- * Called from NetLoop in net/net.c before each packet
+ * Called from net_loop in net/net.c before each packet
  */
-void NcStart(void)
+void nc_start(void)
 {
 	refresh_settings_from_env();
-	if (!output_packet_len || memcmp(nc_ether, NetEtherNullAddr, 6)) {
+	if (!output_packet_len || memcmp(nc_ether, net_null_ethaddr, 6)) {
 		/* going to check for input packet */
 		net_set_udp_handler(nc_handler);
-		NetSetTimeout(net_timeout, nc_timeout);
+		net_set_timeout_handler(net_timeout, nc_timeout_handler);
 	} else {
 		/* send arp request */
 		uchar *pkt;
 		net_set_arp_handler(nc_wait_arp_handler);
-		pkt = (uchar *)NetTxPacket + NetEthHdrSize() + IP_UDP_HDR_SIZE;
+		pkt = (uchar *)net_tx_packet + net_eth_hdr_size() +
+			IP_UDP_HDR_SIZE;
 		memcpy(pkt, output_packet, output_packet_len);
-		NetSendUDPPacket(nc_ether, nc_ip, nc_out_port, nc_in_port,
-			output_packet_len);
+		net_send_udp_packet(nc_ether, nc_ip, nc_out_port, nc_in_port,
+				    output_packet_len);
 	}
 }
 
-int nc_input_packet(uchar *pkt, IPaddr_t src_ip, unsigned dest_port,
+int nc_input_packet(uchar *pkt, struct in_addr src_ip, unsigned dest_port,
 	unsigned src_port, unsigned len)
 {
 	int end, chunk;
@@ -139,7 +142,7 @@ int nc_input_packet(uchar *pkt, IPaddr_t src_ip, unsigned dest_port,
 	if (dest_port != nc_in_port || !len)
 		return 0; /* not for us */
 
-	if (src_ip != nc_ip && !is_broadcast(nc_ip))
+	if (src_ip.s_addr != nc_ip.s_addr && !is_broadcast(nc_ip))
 		return 0; /* not from our client */
 
 	debug_cond(DEBUG_DEV_PKT, "input: \"%*.*s\"\n", len, len, pkt);
@@ -171,7 +174,7 @@ static void nc_send_packet(const char *buf, int len)
 	int inited = 0;
 	uchar *pkt;
 	uchar *ether;
-	IPaddr_t ip;
+	struct in_addr ip;
 
 	debug_cond(DEBUG_DEV_PKT, "output: \"%*.*s\"\n", len, len, buf);
 
@@ -179,13 +182,13 @@ static void nc_send_packet(const char *buf, int len)
 	if (eth == NULL)
 		return;
 
-	if (!memcmp(nc_ether, NetEtherNullAddr, 6)) {
+	if (!memcmp(nc_ether, net_null_ethaddr, 6)) {
 		if (eth->state == ETH_STATE_ACTIVE)
 			return;	/* inside net loop */
 		output_packet = buf;
 		output_packet_len = len;
 		input_recursion = 1;
-		NetLoop(NETCONS); /* wait for arp reply and send packet */
+		net_loop(NETCONS); /* wait for arp reply and send packet */
 		input_recursion = 0;
 		output_packet_len = 0;
 		return;
@@ -193,19 +196,20 @@ static void nc_send_packet(const char *buf, int len)
 
 	if (eth->state != ETH_STATE_ACTIVE) {
 		if (eth_is_on_demand_init()) {
-			if (eth_init(gd->bd) < 0)
+			if (eth_init() < 0)
 				return;
 			eth_set_last_protocol(NETCONS);
-		} else
-			eth_init_state_only(gd->bd);
+		} else {
+			eth_init_state_only();
+		}
 
 		inited = 1;
 	}
-	pkt = (uchar *)NetTxPacket + NetEthHdrSize() + IP_UDP_HDR_SIZE;
+	pkt = (uchar *)net_tx_packet + net_eth_hdr_size() + IP_UDP_HDR_SIZE;
 	memcpy(pkt, buf, len);
 	ether = nc_ether;
 	ip = nc_ip;
-	NetSendUDPPacket(ether, ip, nc_out_port, nc_in_port, len);
+	net_send_udp_packet(ether, ip, nc_out_port, nc_in_port, len);
 
 	if (inited) {
 		if (eth_is_on_demand_init())
@@ -215,7 +219,7 @@ static void nc_send_packet(const char *buf, int len)
 	}
 }
 
-static int nc_start(struct stdio_dev *dev)
+static int nc_stdio_start(struct stdio_dev *dev)
 {
 	int retval;
 
@@ -228,14 +232,14 @@ static int nc_start(struct stdio_dev *dev)
 
 	/*
 	 * Initialize the static IP settings and buffer pointers
-	 * incase we call NetSendUDPPacket before NetLoop
+	 * incase we call net_send_udp_packet before net_loop
 	 */
 	net_init();
 
 	return 0;
 }
 
-static void nc_putc(struct stdio_dev *dev, char c)
+static void nc_stdio_putc(struct stdio_dev *dev, char c)
 {
 	if (output_recursion)
 		return;
@@ -246,7 +250,7 @@ static void nc_putc(struct stdio_dev *dev, char c)
 	output_recursion = 0;
 }
 
-static void nc_puts(struct stdio_dev *dev, const char *s)
+static void nc_stdio_puts(struct stdio_dev *dev, const char *s)
 {
 	int len;
 
@@ -265,7 +269,7 @@ static void nc_puts(struct stdio_dev *dev, const char *s)
 	output_recursion = 0;
 }
 
-static int nc_getc(struct stdio_dev *dev)
+static int nc_stdio_getc(struct stdio_dev *dev)
 {
 	uchar c;
 
@@ -273,7 +277,7 @@ static int nc_getc(struct stdio_dev *dev)
 
 	net_timeout = 0;	/* no timeout */
 	while (!input_size)
-		NetLoop(NETCONS);
+		net_loop(NETCONS);
 
 	input_recursion = 0;
 
@@ -286,7 +290,7 @@ static int nc_getc(struct stdio_dev *dev)
 	return c;
 }
 
-static int nc_tstc(struct stdio_dev *dev)
+static int nc_stdio_tstc(struct stdio_dev *dev)
 {
 	struct eth_device *eth;
 
@@ -303,7 +307,7 @@ static int nc_tstc(struct stdio_dev *dev)
 	input_recursion = 1;
 
 	net_timeout = 1;
-	NetLoop(NETCONS);	/* kind of poll */
+	net_loop(NETCONS);	/* kind of poll */
 
 	input_recursion = 0;
 
@@ -319,11 +323,11 @@ int drv_nc_init(void)
 
 	strcpy(dev.name, "nc");
 	dev.flags = DEV_FLAGS_OUTPUT | DEV_FLAGS_INPUT | DEV_FLAGS_SYSTEM;
-	dev.start = nc_start;
-	dev.putc = nc_putc;
-	dev.puts = nc_puts;
-	dev.getc = nc_getc;
-	dev.tstc = nc_tstc;
+	dev.start = nc_stdio_start;
+	dev.putc = nc_stdio_putc;
+	dev.puts = nc_stdio_puts;
+	dev.getc = nc_stdio_getc;
+	dev.tstc = nc_stdio_tstc;
 
 	rc = stdio_register(&dev);
 
diff --git a/drivers/net/ns8382x.c b/drivers/net/ns8382x.c
index cfe1f34..f941c15 100644
--- a/drivers/net/ns8382x.c
+++ b/drivers/net/ns8382x.c
@@ -809,11 +809,13 @@ ns8382x_poll(struct eth_device *dev)
 
 	if ((rx_status & (DescMore | DescPktOK | DescRxLong)) != DescPktOK) {
 		/* corrupted packet received */
-		printf("ns8382x_poll: Corrupted packet, status:%lx\n", rx_status);
+		printf("ns8382x_poll: Corrupted packet, status:%lx\n",
+		       rx_status);
 		retstat = 0;
 	} else {
 		/* give packet to higher level routine */
-		NetReceive((rxb + cur_rx * RX_BUF_SIZE), length);
+		net_process_received_packet((rxb + cur_rx * RX_BUF_SIZE),
+					    length);
 		retstat = 1;
 	}
 
diff --git a/drivers/net/pch_gbe.c b/drivers/net/pch_gbe.c
index 976848d..a03bdc0 100644
--- a/drivers/net/pch_gbe.c
+++ b/drivers/net/pch_gbe.c
@@ -297,7 +297,7 @@ static int pch_gbe_recv(struct eth_device *dev)
 
 	buffer_addr = pci_mem_to_phys(priv->bdf, rx_desc->buffer_addr);
 	length = rx_desc->rx_words_eob - 3 - ETH_FCS_LEN;
-	NetReceive((uchar *)buffer_addr, length);
+	net_process_received_packet((uchar *)buffer_addr, length);
 
 	/* Test the wrap-around condition */
 	if (++priv->rx_idx >= PCH_GBE_DESC_NUM)
@@ -446,7 +446,7 @@ int pch_gbe_register(bd_t *bis)
 	dev->iobase = iobase;
 	priv->mac_regs = (struct pch_gbe_regs *)iobase;
 
-	sprintf(dev->name, "pch_gbe.%x", iobase);
+	sprintf(dev->name, "pch_gbe");
 
 	/* Read MAC address from SROM and initialize dev->enetaddr with it */
 	pch_gbe_mac_read(priv->mac_regs, dev->enetaddr);
diff --git a/drivers/net/pcnet.c b/drivers/net/pcnet.c
index 237fbba..cfcb1b4 100644
--- a/drivers/net/pcnet.c
+++ b/drivers/net/pcnet.c
@@ -507,7 +507,7 @@ static int pcnet_recv (struct eth_device *dev)
 				buf = (*lp->rx_buf)[lp->cur_rx];
 				invalidate_dcache_range((unsigned long)buf,
 					(unsigned long)buf + pkt_len);
-				NetReceive(buf, pkt_len);
+				net_process_received_packet(buf, pkt_len);
 				PCNET_DEBUG2("Rx%d: %d bytes from 0x%p\n",
 					     lp->cur_rx, pkt_len, buf);
 			}
diff --git a/drivers/net/phy/cortina.c b/drivers/net/phy/cortina.c
index 254f056..3a2b3bb 100644
--- a/drivers/net/phy/cortina.c
+++ b/drivers/net/phy/cortina.c
@@ -186,8 +186,8 @@ void cs4340_upload_firmware(struct phy_device *phydev)
 		while (*addr != 0x0a) {
 			line_temp[i++] = *addr++;
 			if (0x50 < i) {
-				printf("Not found Cortina PHY ucode at 0x%x\n",
-				       CONFIG_CORTINA_FW_ADDR);
+				printf("Not found Cortina PHY ucode at 0x%p\n",
+				       (char *)CONFIG_CORTINA_FW_ADDR);
 				return;
 			}
 		}
diff --git a/drivers/net/phy/micrel.c b/drivers/net/phy/micrel.c
index 1815b29..49f444a 100644
--- a/drivers/net/phy/micrel.c
+++ b/drivers/net/phy/micrel.c
@@ -22,6 +22,16 @@ static struct phy_driver KSZ804_driver = {
 	.shutdown = &genphy_shutdown,
 };
 
+static struct phy_driver KSZ8081_driver = {
+	.name = "Micrel KSZ8081",
+	.uid = 0x221560,
+	.mask = 0xfffff0,
+	.features = PHY_BASIC_FEATURES,
+	.config = &genphy_config,
+	.startup = &genphy_startup,
+	.shutdown = &genphy_shutdown,
+};
+
 /**
  * KSZ8895
  */
@@ -272,6 +282,7 @@ static struct phy_driver ksz9031_driver = {
 int phy_micrel_init(void)
 {
 	phy_register(&KSZ804_driver);
+	phy_register(&KSZ8081_driver);
 #ifdef CONFIG_PHY_MICREL_KSZ9021
 	phy_register(&ksz9021_driver);
 #else
diff --git a/drivers/net/phy/phy.c b/drivers/net/phy/phy.c
index df7e945..f5221a3 100644
--- a/drivers/net/phy/phy.c
+++ b/drivers/net/phy/phy.c
@@ -11,6 +11,7 @@
 
 #include <config.h>
 #include <common.h>
+#include <dm.h>
 #include <malloc.h>
 #include <net.h>
 #include <command.h>
@@ -581,7 +582,7 @@ static struct phy_device *phy_device_create(struct mii_dev *bus, int addr,
  * Description: Reads the ID registers of the PHY at @addr on the
  *   @bus, stores it in @phy_id and returns zero on success.
  */
-static int get_phy_id(struct mii_dev *bus, int addr, int devad, u32 *phy_id)
+int __weak get_phy_id(struct mii_dev *bus, int addr, int devad, u32 *phy_id)
 {
 	int phy_reg;
 
@@ -754,7 +755,11 @@ struct phy_device *phy_find_by_mask(struct mii_dev *bus, unsigned phy_mask,
 	return get_phy_device_by_mask(bus, phy_mask, interface);
 }
 
+#ifdef CONFIG_DM_ETH
+void phy_connect_dev(struct phy_device *phydev, struct udevice *dev)
+#else
 void phy_connect_dev(struct phy_device *phydev, struct eth_device *dev)
+#endif
 {
 	/* Soft Reset the PHY */
 	phy_reset(phydev);
@@ -767,8 +772,13 @@ void phy_connect_dev(struct phy_device *phydev, struct eth_device *dev)
 	debug("%s connected to %s\n", dev->name, phydev->drv->name);
 }
 
+#ifdef CONFIG_DM_ETH
+struct phy_device *phy_connect(struct mii_dev *bus, int addr,
+		struct udevice *dev, phy_interface_t interface)
+#else
 struct phy_device *phy_connect(struct mii_dev *bus, int addr,
 		struct eth_device *dev, phy_interface_t interface)
+#endif
 {
 	struct phy_device *phydev;
 
@@ -813,3 +823,15 @@ int phy_shutdown(struct phy_device *phydev)
 
 	return 0;
 }
+
+int phy_get_interface_by_name(const char *str)
+{
+	int i;
+
+	for (i = 0; i < PHY_INTERFACE_MODE_COUNT; i++) {
+		if (!strcmp(str, phy_interface_strings[i]))
+			return i;
+	}
+
+	return -1;
+}
diff --git a/drivers/net/phy/realtek.c b/drivers/net/phy/realtek.c
index a3ace68..ee97079 100644
--- a/drivers/net/phy/realtek.c
+++ b/drivers/net/phy/realtek.c
@@ -3,7 +3,7 @@
  *
  * SPDX-License-Identifier:	GPL-2.0+
  *
- * Copyright 2010-2011 Freescale Semiconductor, Inc.
+ * Copyright 2010-2011, 2015 Freescale Semiconductor, Inc.
  * author Andy Fleming
  */
 #include <config.h>
@@ -21,12 +21,28 @@
 #define MIIM_RTL8211x_PHYSTAT_SPDDONE  0x0800
 #define MIIM_RTL8211x_PHYSTAT_LINK     0x0400
 
+/* RTL8211x PHY Interrupt Enable Register */
+#define MIIM_RTL8211x_PHY_INER         0x12
+#define MIIM_RTL8211x_PHY_INTR_ENA     0x9f01
+#define MIIM_RTL8211x_PHY_INTR_DIS     0x0000
+
+/* RTL8211x PHY Interrupt Status Register */
+#define MIIM_RTL8211x_PHY_INSR         0x13
 
 /* RealTek RTL8211x */
 static int rtl8211x_config(struct phy_device *phydev)
 {
 	phy_write(phydev, MDIO_DEVAD_NONE, MII_BMCR, BMCR_RESET);
 
+	/* mask interrupt at init; if the interrupt is
+	 * needed indeed, it should be explicitly enabled
+	 */
+	phy_write(phydev, MDIO_DEVAD_NONE, MIIM_RTL8211x_PHY_INER,
+		  MIIM_RTL8211x_PHY_INTR_DIS);
+
+	/* read interrupt status just to clear it */
+	phy_read(phydev, MDIO_DEVAD_NONE, MIIM_RTL8211x_PHY_INER);
+
 	genphy_config_aneg(phydev);
 
 	return 0;
diff --git a/drivers/net/rtl8139.c b/drivers/net/rtl8139.c
index 208ce5c..ea52343 100644
--- a/drivers/net/rtl8139.c
+++ b/drivers/net/rtl8139.c
@@ -504,11 +504,11 @@ static int rtl_poll(struct eth_device *dev)
 		memcpy(rxdata, rx_ring + ring_offs + 4, semi_count);
 		memcpy(&(rxdata[semi_count]), rx_ring, rx_size-4-semi_count);
 
-		NetReceive(rxdata, length);
+		net_process_received_packet(rxdata, length);
 		debug_cond(DEBUG_RX, "rx packet %d+%d bytes",
 			semi_count, rx_size-4-semi_count);
 	} else {
-		NetReceive(rx_ring + ring_offs + 4, length);
+		net_process_received_packet(rx_ring + ring_offs + 4, length);
 		debug_cond(DEBUG_RX, "rx packet %d bytes", rx_size-4);
 	}
 	flush_cache((unsigned long)rx_ring, RX_BUF_LEN);
diff --git a/drivers/net/rtl8169.c b/drivers/net/rtl8169.c
index cea6701..958488c 100644
--- a/drivers/net/rtl8169.c
+++ b/drivers/net/rtl8169.c
@@ -55,7 +55,7 @@
 #define drv_version "v1.5"
 #define drv_date "01-17-2004"
 
-static u32 ioaddr;
+static unsigned long ioaddr;
 
 /* Condensed operations for readability. */
 #define currticks()	get_timer(0)
@@ -92,19 +92,21 @@ static int media[MAX_UNITS] = { -1, -1, -1, -1, -1, -1, -1, -1 };
 #define TX_TIMEOUT  (6*HZ)
 
 /* write/read MMIO register. Notice: {read,write}[wl] do the necessary swapping */
-#define RTL_W8(reg, val8)	writeb ((val8), ioaddr + (reg))
-#define RTL_W16(reg, val16)	writew ((val16), ioaddr + (reg))
-#define RTL_W32(reg, val32)	writel ((val32), ioaddr + (reg))
-#define RTL_R8(reg)		readb (ioaddr + (reg))
-#define RTL_R16(reg)		readw (ioaddr + (reg))
-#define RTL_R32(reg)		((unsigned long) readl (ioaddr + (reg)))
+#define RTL_W8(reg, val8)	writeb((val8), ioaddr + (reg))
+#define RTL_W16(reg, val16)	writew((val16), ioaddr + (reg))
+#define RTL_W32(reg, val32)	writel((val32), ioaddr + (reg))
+#define RTL_R8(reg)		readb(ioaddr + (reg))
+#define RTL_R16(reg)		readw(ioaddr + (reg))
+#define RTL_R32(reg)		readl(ioaddr + (reg))
 
 #define ETH_FRAME_LEN	MAX_ETH_FRAME_SIZE
 #define ETH_ALEN	MAC_ADDR_LEN
 #define ETH_ZLEN	60
 
-#define bus_to_phys(a)	pci_mem_to_phys((pci_dev_t)dev->priv, (pci_addr_t)a)
-#define phys_to_bus(a)	pci_phys_to_mem((pci_dev_t)dev->priv, (phys_addr_t)a)
+#define bus_to_phys(a)	pci_mem_to_phys((pci_dev_t)(unsigned long)dev->priv, \
+	(pci_addr_t)(unsigned long)a)
+#define phys_to_bus(a)	pci_phys_to_mem((pci_dev_t)(unsigned long)dev->priv, \
+	(phys_addr_t)a)
 
 enum RTL8169_registers {
 	MAC0 = 0,		/* Ethernet hardware address. */
@@ -538,7 +540,7 @@ static int rtl_recv(struct eth_device *dev)
 				cpu_to_le32(bus_to_phys(tpc->RxBufferRing[cur_rx]));
 			rtl_flush_rx_desc(&tpc->RxDescArray[cur_rx]);
 
-			NetReceive(rxdata, length);
+			net_process_received_packet(rxdata, length);
 		} else {
 			puts("Error Rx");
 		}
@@ -852,7 +854,7 @@ static int rtl_init(struct eth_device *dev, bd_t *bis)
 
 #ifdef DEBUG_RTL8169
 	/* Print out some hardware info */
-	printf("%s: at ioaddr 0x%x\n", dev->name, ioaddr);
+	printf("%s: at ioaddr 0x%lx\n", dev->name, ioaddr);
 #endif
 
 	/* if TBI is not endbled */
@@ -1004,7 +1006,7 @@ int rtl8169_initialize(bd_t *bis)
 		memset(dev, 0, sizeof(*dev));
 		sprintf (dev->name, "RTL8169#%d", card_number);
 
-		dev->priv = (void *) devno;
+		dev->priv = (void *)(unsigned long)devno;
 		dev->iobase = (int)pci_mem_to_phys(devno, iobase);
 
 		dev->init = rtl_reset;
diff --git a/drivers/net/sandbox-raw.c b/drivers/net/sandbox-raw.c
new file mode 100644
index 0000000..45c3b18
--- /dev/null
+++ b/drivers/net/sandbox-raw.c
@@ -0,0 +1,165 @@
+/*
+ * Copyright (c) 2015 National Instruments
+ *
+ * (C) Copyright 2015
+ * Joe Hershberger <joe.hershberger@ni.com>
+ *
+ * SPDX-License-Identifier:	GPL-2.0
+ */
+
+#include <asm/eth-raw-os.h>
+#include <common.h>
+#include <dm.h>
+#include <malloc.h>
+#include <net.h>
+
+DECLARE_GLOBAL_DATA_PTR;
+
+static int reply_arp;
+static struct in_addr arp_ip;
+
+static int sb_eth_raw_start(struct udevice *dev)
+{
+	struct eth_sandbox_raw_priv *priv = dev_get_priv(dev);
+	struct eth_pdata *pdata = dev_get_platdata(dev);
+	const char *interface;
+
+	debug("eth_sandbox_raw: Start\n");
+
+	interface = fdt_getprop(gd->fdt_blob, dev->of_offset,
+					    "host-raw-interface", NULL);
+	if (interface == NULL)
+		return -EINVAL;
+
+	if (strcmp(interface, "lo") == 0) {
+		priv->local = 1;
+		setenv("ipaddr", "127.0.0.1");
+		setenv("serverip", "127.0.0.1");
+	}
+	return sandbox_eth_raw_os_start(interface, pdata->enetaddr, priv);
+}
+
+static int sb_eth_raw_send(struct udevice *dev, void *packet, int length)
+{
+	struct eth_sandbox_raw_priv *priv = dev_get_priv(dev);
+
+	debug("eth_sandbox_raw: Send packet %d\n", length);
+
+	if (priv->local) {
+		struct ethernet_hdr *eth = packet;
+
+		if (ntohs(eth->et_protlen) == PROT_ARP) {
+			struct arp_hdr *arp = packet + ETHER_HDR_SIZE;
+
+			/**
+			 * localhost works on a higher-level API in Linux than
+			 * ARP packets, so fake it
+			 */
+			arp_ip = net_read_ip(&arp->ar_tpa);
+			reply_arp = 1;
+			return 0;
+		}
+		packet += ETHER_HDR_SIZE;
+		length -= ETHER_HDR_SIZE;
+	}
+	return sandbox_eth_raw_os_send(packet, length, priv);
+}
+
+static int sb_eth_raw_recv(struct udevice *dev, uchar **packetp)
+{
+	struct eth_pdata *pdata = dev_get_platdata(dev);
+	struct eth_sandbox_raw_priv *priv = dev_get_priv(dev);
+	int retval = 0;
+	int length;
+
+	if (reply_arp) {
+		struct arp_hdr *arp = (void *)net_rx_packets[0] +
+			ETHER_HDR_SIZE;
+
+		/*
+		 * Fake an ARP response. The u-boot network stack is sending an
+		 * ARP request (to find the MAC address to address the actual
+		 * packet to) and requires an ARP response to continue. Since
+		 * this is the localhost interface, there is no Etherent level
+		 * traffic at all, so there is no way to send an ARP request or
+		 * to get a response. For this reason we fake the response to
+		 * make the u-boot network stack happy.
+		 */
+		arp->ar_hrd = htons(ARP_ETHER);
+		arp->ar_pro = htons(PROT_IP);
+		arp->ar_hln = ARP_HLEN;
+		arp->ar_pln = ARP_PLEN;
+		arp->ar_op = htons(ARPOP_REPLY);
+		/* Any non-zero MAC address will work */
+		memset(&arp->ar_sha, 0x01, ARP_HLEN);
+		/* Use whatever IP we were looking for (always 127.0.0.1?) */
+		net_write_ip(&arp->ar_spa, arp_ip);
+		memcpy(&arp->ar_tha, pdata->enetaddr, ARP_HLEN);
+		net_write_ip(&arp->ar_tpa, net_ip);
+		length = ARP_HDR_SIZE;
+	} else {
+		/* If local, the Ethernet header won't be included; skip it */
+		uchar *pktptr = priv->local ?
+			net_rx_packets[0] + ETHER_HDR_SIZE : net_rx_packets[0];
+
+		retval = sandbox_eth_raw_os_recv(pktptr, &length, priv);
+	}
+
+	if (!retval && length) {
+		if (priv->local) {
+			struct ethernet_hdr *eth = (void *)net_rx_packets[0];
+
+			/* Fill in enough of the missing Ethernet header */
+			memcpy(eth->et_dest, pdata->enetaddr, ARP_HLEN);
+			memset(eth->et_src, 0x01, ARP_HLEN);
+			eth->et_protlen = htons(reply_arp ? PROT_ARP : PROT_IP);
+			reply_arp = 0;
+			length += ETHER_HDR_SIZE;
+		}
+
+		debug("eth_sandbox_raw: received packet %d\n",
+		      length);
+		*packetp = net_rx_packets[0];
+		return length;
+	}
+	return retval;
+}
+
+static void sb_eth_raw_stop(struct udevice *dev)
+{
+	struct eth_sandbox_raw_priv *priv = dev_get_priv(dev);
+
+	debug("eth_sandbox_raw: Stop\n");
+
+	sandbox_eth_raw_os_stop(priv);
+}
+
+static const struct eth_ops sb_eth_raw_ops = {
+	.start			= sb_eth_raw_start,
+	.send			= sb_eth_raw_send,
+	.recv			= sb_eth_raw_recv,
+	.stop			= sb_eth_raw_stop,
+};
+
+static int sb_eth_raw_ofdata_to_platdata(struct udevice *dev)
+{
+	struct eth_pdata *pdata = dev_get_platdata(dev);
+
+	pdata->iobase = dev_get_addr(dev);
+	return 0;
+}
+
+static const struct udevice_id sb_eth_raw_ids[] = {
+	{ .compatible = "sandbox,eth-raw" },
+	{ }
+};
+
+U_BOOT_DRIVER(eth_sandbox_raw) = {
+	.name	= "eth_sandbox_raw",
+	.id	= UCLASS_ETH,
+	.of_match = sb_eth_raw_ids,
+	.ofdata_to_platdata = sb_eth_raw_ofdata_to_platdata,
+	.ops	= &sb_eth_raw_ops,
+	.priv_auto_alloc_size = sizeof(struct eth_sandbox_raw_priv),
+	.platdata_auto_alloc_size = sizeof(struct eth_pdata),
+};
diff --git a/drivers/net/sandbox.c b/drivers/net/sandbox.c
new file mode 100644
index 0000000..e239ff4
--- /dev/null
+++ b/drivers/net/sandbox.c
@@ -0,0 +1,208 @@
+/*
+ * Copyright (c) 2015 National Instruments
+ *
+ * (C) Copyright 2015
+ * Joe Hershberger <joe.hershberger@ni.com>
+ *
+ * SPDX-License-Identifier:	GPL-2.0
+ */
+
+#include <common.h>
+#include <dm.h>
+#include <malloc.h>
+#include <net.h>
+
+DECLARE_GLOBAL_DATA_PTR;
+
+/**
+ * struct eth_sandbox_priv - memory for sandbox mock driver
+ *
+ * fake_host_hwaddr: MAC address of mocked machine
+ * fake_host_ipaddr: IP address of mocked machine
+ * recv_packet_buffer: buffer of the packet returned as received
+ * recv_packet_length: length of the packet returned as received
+ */
+struct eth_sandbox_priv {
+	uchar fake_host_hwaddr[ARP_HLEN];
+	struct in_addr fake_host_ipaddr;
+	uchar *recv_packet_buffer;
+	int recv_packet_length;
+};
+
+static bool disabled[8] = {false};
+
+/*
+ * sandbox_eth_disable_response()
+ *
+ * index - The alias index (also DM seq number)
+ * disable - If non-zero, ignore sent packets and don't send mock response
+ */
+void sandbox_eth_disable_response(int index, bool disable)
+{
+	disabled[index] = disable;
+}
+
+static int sb_eth_start(struct udevice *dev)
+{
+	struct eth_sandbox_priv *priv = dev_get_priv(dev);
+
+	debug("eth_sandbox: Start\n");
+
+	fdtdec_get_byte_array(gd->fdt_blob, dev->of_offset, "fake-host-hwaddr",
+			      priv->fake_host_hwaddr, ARP_HLEN);
+	priv->recv_packet_buffer = net_rx_packets[0];
+	return 0;
+}
+
+static int sb_eth_send(struct udevice *dev, void *packet, int length)
+{
+	struct eth_sandbox_priv *priv = dev_get_priv(dev);
+	struct ethernet_hdr *eth = packet;
+
+	debug("eth_sandbox: Send packet %d\n", length);
+
+	if (dev->seq >= 0 && dev->seq < ARRAY_SIZE(disabled) &&
+	    disabled[dev->seq])
+		return 0;
+
+	if (ntohs(eth->et_protlen) == PROT_ARP) {
+		struct arp_hdr *arp = packet + ETHER_HDR_SIZE;
+
+		if (ntohs(arp->ar_op) == ARPOP_REQUEST) {
+			struct ethernet_hdr *eth_recv;
+			struct arp_hdr *arp_recv;
+
+			/* store this as the assumed IP of the fake host */
+			priv->fake_host_ipaddr = net_read_ip(&arp->ar_tpa);
+			/* Formulate a fake response */
+			eth_recv = (void *)priv->recv_packet_buffer;
+			memcpy(eth_recv->et_dest, eth->et_src, ARP_HLEN);
+			memcpy(eth_recv->et_src, priv->fake_host_hwaddr,
+			       ARP_HLEN);
+			eth_recv->et_protlen = htons(PROT_ARP);
+
+			arp_recv = (void *)priv->recv_packet_buffer +
+				ETHER_HDR_SIZE;
+			arp_recv->ar_hrd = htons(ARP_ETHER);
+			arp_recv->ar_pro = htons(PROT_IP);
+			arp_recv->ar_hln = ARP_HLEN;
+			arp_recv->ar_pln = ARP_PLEN;
+			arp_recv->ar_op = htons(ARPOP_REPLY);
+			memcpy(&arp_recv->ar_sha, priv->fake_host_hwaddr,
+			       ARP_HLEN);
+			net_write_ip(&arp_recv->ar_spa, priv->fake_host_ipaddr);
+			memcpy(&arp_recv->ar_tha, &arp->ar_sha, ARP_HLEN);
+			net_copy_ip(&arp_recv->ar_tpa, &arp->ar_spa);
+
+			priv->recv_packet_length = ETHER_HDR_SIZE +
+				ARP_HDR_SIZE;
+		}
+	} else if (ntohs(eth->et_protlen) == PROT_IP) {
+		struct ip_udp_hdr *ip = packet + ETHER_HDR_SIZE;
+
+		if (ip->ip_p == IPPROTO_ICMP) {
+			struct icmp_hdr *icmp = (struct icmp_hdr *)&ip->udp_src;
+
+			if (icmp->type == ICMP_ECHO_REQUEST) {
+				struct ethernet_hdr *eth_recv;
+				struct ip_udp_hdr *ipr;
+				struct icmp_hdr *icmpr;
+
+				/* reply to the ping */
+				memcpy(priv->recv_packet_buffer, packet,
+				       length);
+				eth_recv = (void *)priv->recv_packet_buffer;
+				ipr = (void *)priv->recv_packet_buffer +
+					ETHER_HDR_SIZE;
+				icmpr = (struct icmp_hdr *)&ipr->udp_src;
+				memcpy(eth_recv->et_dest, eth->et_src,
+				       ARP_HLEN);
+				memcpy(eth_recv->et_src, priv->fake_host_hwaddr,
+				       ARP_HLEN);
+				ipr->ip_sum = 0;
+				ipr->ip_off = 0;
+				net_copy_ip((void *)&ipr->ip_dst, &ip->ip_src);
+				net_write_ip((void *)&ipr->ip_src,
+					     priv->fake_host_ipaddr);
+				ipr->ip_sum = compute_ip_checksum(ipr,
+					IP_HDR_SIZE);
+
+				icmpr->type = ICMP_ECHO_REPLY;
+				icmpr->checksum = 0;
+				icmpr->checksum = compute_ip_checksum(icmpr,
+					ICMP_HDR_SIZE);
+
+				priv->recv_packet_length = length;
+			}
+		}
+	}
+
+	return 0;
+}
+
+static int sb_eth_recv(struct udevice *dev, uchar **packetp)
+{
+	struct eth_sandbox_priv *priv = dev_get_priv(dev);
+
+	if (priv->recv_packet_length) {
+		int lcl_recv_packet_length = priv->recv_packet_length;
+
+		debug("eth_sandbox: received packet %d\n",
+		      priv->recv_packet_length);
+		priv->recv_packet_length = 0;
+		*packetp = priv->recv_packet_buffer;
+		return lcl_recv_packet_length;
+	}
+	return 0;
+}
+
+static void sb_eth_stop(struct udevice *dev)
+{
+	debug("eth_sandbox: Stop\n");
+}
+
+static int sb_eth_write_hwaddr(struct udevice *dev)
+{
+	struct eth_pdata *pdata = dev_get_platdata(dev);
+
+	debug("eth_sandbox %s: Write HW ADDR - %pM\n", dev->name,
+	      pdata->enetaddr);
+	return 0;
+}
+
+static const struct eth_ops sb_eth_ops = {
+	.start			= sb_eth_start,
+	.send			= sb_eth_send,
+	.recv			= sb_eth_recv,
+	.stop			= sb_eth_stop,
+	.write_hwaddr		= sb_eth_write_hwaddr,
+};
+
+static int sb_eth_remove(struct udevice *dev)
+{
+	return 0;
+}
+
+static int sb_eth_ofdata_to_platdata(struct udevice *dev)
+{
+	struct eth_pdata *pdata = dev_get_platdata(dev);
+
+	pdata->iobase = dev_get_addr(dev);
+	return 0;
+}
+
+static const struct udevice_id sb_eth_ids[] = {
+	{ .compatible = "sandbox,eth" },
+	{ }
+};
+
+U_BOOT_DRIVER(eth_sandbox) = {
+	.name	= "eth_sandbox",
+	.id	= UCLASS_ETH,
+	.of_match = sb_eth_ids,
+	.ofdata_to_platdata = sb_eth_ofdata_to_platdata,
+	.remove	= sb_eth_remove,
+	.ops	= &sb_eth_ops,
+	.priv_auto_alloc_size = sizeof(struct eth_sandbox_priv),
+	.platdata_auto_alloc_size = sizeof(struct eth_pdata),
+};
diff --git a/drivers/net/sh_eth.c b/drivers/net/sh_eth.c
index 4bf493e..a320b4d 100644
--- a/drivers/net/sh_eth.c
+++ b/drivers/net/sh_eth.c
@@ -127,7 +127,7 @@ int sh_eth_recv(struct eth_device *dev)
 			packet = (uchar *)
 				ADDR_TO_P2(port_info->rx_desc_cur->rd2);
 			invalidate_cache(packet, len);
-			NetReceive(packet, len);
+			net_process_received_packet(packet, len);
 		}
 
 		/* Make current descriptor available again */
diff --git a/drivers/net/smc91111.c b/drivers/net/smc91111.c
index 57c667a..ade14cd 100644
--- a/drivers/net/smc91111.c
+++ b/drivers/net/smc91111.c
@@ -756,35 +756,35 @@ static int smc_rcv(struct eth_device *dev)
 
 
 #ifdef USE_32_BIT
-		PRINTK3(" Reading %d dwords (and %d bytes) \n",
+		PRINTK3(" Reading %d dwords (and %d bytes)\n",
 			packet_length >> 2, packet_length & 3 );
 		/* QUESTION:  Like in the TX routine, do I want
 		   to send the DWORDs or the bytes first, or some
 		   mixture.  A mixture might improve already slow PIO
 		   performance	*/
-		SMC_insl( dev, SMC91111_DATA_REG, NetRxPackets[0],
-			packet_length >> 2 );
+		SMC_insl(dev, SMC91111_DATA_REG, net_rx_packets[0],
+			 packet_length >> 2);
 		/* read the left over bytes */
 		if (packet_length & 3) {
 			int i;
 
-			byte *tail = (byte *)(NetRxPackets[0] +
+			byte *tail = (byte *)(net_rx_packets[0] +
 				(packet_length & ~3));
 			dword leftover = SMC_inl(dev, SMC91111_DATA_REG);
 			for (i=0; i<(packet_length & 3); i++)
 				*tail++ = (byte) (leftover >> (8*i)) & 0xff;
 		}
 #else
-		PRINTK3(" Reading %d words and %d byte(s) \n",
+		PRINTK3(" Reading %d words and %d byte(s)\n",
 			(packet_length >> 1 ), packet_length & 1 );
-		SMC_insw(dev, SMC91111_DATA_REG , NetRxPackets[0],
-			packet_length >> 1);
+		SMC_insw(dev, SMC91111_DATA_REG , net_rx_packets[0],
+			 packet_length >> 1);
 
 #endif /* USE_32_BIT */
 
 #if	SMC_DEBUG > 2
 		printf("Receiving Packet\n");
-		print_packet( NetRxPackets[0], packet_length );
+		print_packet(net_rx_packets[0], packet_length);
 #endif
 	} else {
 		/* error ... */
@@ -815,7 +815,7 @@ static int smc_rcv(struct eth_device *dev)
 
 	if (!is_error) {
 		/* Pass the packet up to the protocol layers. */
-		NetReceive(NetRxPackets[0], packet_length);
+		net_process_received_packet(net_rx_packets[0], packet_length);
 		return packet_length;
 	} else {
 		return 0;
diff --git a/drivers/net/smc911x.c b/drivers/net/smc911x.c
index 5959672..c85a178 100644
--- a/drivers/net/smc911x.c
+++ b/drivers/net/smc911x.c
@@ -192,7 +192,7 @@ static void smc911x_halt(struct eth_device *dev)
 
 static int smc911x_rx(struct eth_device *dev)
 {
-	u32 *data = (u32 *)NetRxPackets[0];
+	u32 *data = (u32 *)net_rx_packets[0];
 	u32 pktlen, tmplen;
 	u32 status;
 
@@ -211,7 +211,7 @@ static int smc911x_rx(struct eth_device *dev)
 				": dropped bad packet. Status: 0x%08x\n",
 				status);
 		else
-			NetReceive(NetRxPackets[0], pktlen);
+			net_process_received_packet(net_rx_packets[0], pktlen);
 	}
 
 	return 0;
diff --git a/drivers/net/sunxi_emac.c b/drivers/net/sunxi_emac.c
index 5a06d68..7b31f8c 100644
--- a/drivers/net/sunxi_emac.c
+++ b/drivers/net/sunxi_emac.c
@@ -437,10 +437,10 @@ static int sunxi_emac_eth_recv(struct eth_device *dev)
 			printf("Received packet is too big (len=%d)\n", rx_len);
 		} else {
 			emac_inblk_32bit((void *)&regs->rx_io_data,
-					 NetRxPackets[0], rx_len);
+					 net_rx_packets[0], rx_len);
 
 			/* Pass to upper layer */
-			NetReceive(NetRxPackets[0], rx_len);
+			net_process_received_packet(net_rx_packets[0], rx_len);
 			return rx_len;
 		}
 	}
@@ -497,7 +497,7 @@ int sunxi_emac_initialize(void)
 
 	/* Configure pin mux settings for MII Ethernet */
 	for (pin = SUNXI_GPA(0); pin <= SUNXI_GPA(17); pin++)
-		sunxi_gpio_set_cfgpin(pin, SUNXI_GPA0_EMAC);
+		sunxi_gpio_set_cfgpin(pin, SUNXI_GPA_EMAC);
 
 	/* Set up clock gating */
 	setbits_le32(&ccm->ahb_gate0, 0x1 << AHB_GATE_OFFSET_EMAC);
diff --git a/drivers/net/tsec.c b/drivers/net/tsec.c
index dcdba4e..42d0374 100644
--- a/drivers/net/tsec.c
+++ b/drivers/net/tsec.c
@@ -287,7 +287,7 @@ void redundant_init(struct eth_device *dev)
 			}
 		}
 
-		if (!memcmp(pkt, (void *)NetRxPackets[rx_idx], sizeof(pkt)))
+		if (!memcmp(pkt, (void *)net_rx_packets[rx_idx], sizeof(pkt)))
 			fail = 0;
 
 		out_be16(&rxbd[rx_idx].length, 0);
@@ -343,7 +343,7 @@ static void startup_tsec(struct eth_device *dev)
 	for (i = 0; i < PKTBUFSRX; i++) {
 		out_be16(&rxbd[i].status, RXBD_EMPTY);
 		out_be16(&rxbd[i].length, 0);
-		out_be32(&rxbd[i].bufptr, (u32)NetRxPackets[i]);
+		out_be32(&rxbd[i].bufptr, (u32)net_rx_packets[i]);
 	}
 	status = in_be16(&rxbd[PKTBUFSRX - 1].status);
 	out_be16(&rxbd[PKTBUFSRX - 1].status, status | RXBD_WRAP);
@@ -430,7 +430,8 @@ static int tsec_recv(struct eth_device *dev)
 
 		/* Send the packet up if there were no errors */
 		if (!(status & RXBD_STATS))
-			NetReceive(NetRxPackets[rx_idx], length - 4);
+			net_process_received_packet(net_rx_packets[rx_idx],
+						    length - 4);
 		else
 			printf("Got error %x\n", (status & RXBD_STATS));
 
diff --git a/drivers/net/tsi108_eth.c b/drivers/net/tsi108_eth.c
index 72b8159..9da59a0 100644
--- a/drivers/net/tsi108_eth.c
+++ b/drivers/net/tsi108_eth.c
@@ -804,11 +804,11 @@ static int tsi108_eth_probe (struct eth_device *dev, bd_t * bis)
 	rx_descr_current = rx_descr;
 	for (index = 0; index < NUM_RX_DESC; index++) {
 		/* make sure the receive buffers are not in cache */
-		invalidate_dcache_range((unsigned long)NetRxPackets[index],
-					(unsigned long)NetRxPackets[index] +
+		invalidate_dcache_range((unsigned long)net_rx_packets[index],
+					(unsigned long)net_rx_packets[index] +
 					RX_BUFFER_SIZE);
 		rx_descr->start_addr0 =
-		    cpu_to_le32((vuint32) NetRxPackets[index]);
+		    cpu_to_le32((vuint32) net_rx_packets[index]);
 		rx_descr->start_addr1 = 0;
 		rx_descr->next_descr_addr0 =
 		    cpu_to_le32((vuint32) (rx_descr + 1));
@@ -966,7 +966,7 @@ static int tsi108_eth_recv (struct eth_device *dev)
 
 			/*** process packet ***/
 			buffer = (uchar *)(le32_to_cpu(rx_descr->start_addr0));
-			NetReceive(buffer, length);
+			net_process_received_packet(buffer, length);
 
 			invalidate_dcache_range ((unsigned long)buffer,
 						(unsigned long)buffer +
diff --git a/drivers/net/uli526x.c b/drivers/net/uli526x.c
index 9526faa..47cdb85 100644
--- a/drivers/net/uli526x.c
+++ b/drivers/net/uli526x.c
@@ -587,7 +587,8 @@ static int uli526x_rx_packet(struct eth_device *dev)
 					__FUNCTION__, i, rxptr->rx_buf_ptr[i]);
 #endif
 
-				NetReceive((uchar *)rxptr->rx_buf_ptr, rxlen);
+				net_process_received_packet(
+					(uchar *)rxptr->rx_buf_ptr, rxlen);
 				uli526x_reuse_buf(rxptr);
 
 			} else {
@@ -709,7 +710,7 @@ static void allocate_rx_buffer(struct uli526x_board_info *db)
 	u32 addr;
 
 	for (index = 0; index < RX_DESC_CNT; index++) {
-		addr = (u32)NetRxPackets[index];
+		addr = (u32)net_rx_packets[index];
 		addr += (16 - (addr & 15));
 		rxptr->rx_buf_ptr = (char *) addr;
 		rxptr->rdes2 = cpu_to_le32(addr);
diff --git a/drivers/net/vsc9953.c b/drivers/net/vsc9953.c
index 9fc3c18..fed7358 100644
--- a/drivers/net/vsc9953.c
+++ b/drivers/net/vsc9953.c
@@ -9,7 +9,7 @@
 #include <asm/io.h>
 #include <asm/fsl_serdes.h>
 #include <fm_eth.h>
-#include <asm/fsl_memac.h>
+#include <fsl_memac.h>
 #include <vsc9953.h>
 
 static struct vsc9953_info vsc9953_l2sw = {
diff --git a/drivers/net/xilinx_axi_emac.c b/drivers/net/xilinx_axi_emac.c
index 262b67b..df053fe 100644
--- a/drivers/net/xilinx_axi_emac.c
+++ b/drivers/net/xilinx_axi_emac.c
@@ -556,7 +556,7 @@ static int axiemac_recv(struct eth_device *dev)
 #endif
 	/* Pass the received frame up for processing */
 	if (length)
-		NetReceive(rxframe, length);
+		net_process_received_packet(rxframe, length);
 
 #ifdef DEBUG
 	/* It is useful to clear buffer to be sure that it is consistent */
diff --git a/drivers/net/xilinx_emaclite.c b/drivers/net/xilinx_emaclite.c
index 2a5cc44..c9afa99 100644
--- a/drivers/net/xilinx_emaclite.c
+++ b/drivers/net/xilinx_emaclite.c
@@ -322,7 +322,7 @@ static int emaclite_recv(struct eth_device *dev)
 	out_be32 (baseaddress + XEL_RSR_OFFSET, reg);
 
 	debug("Packet receive from 0x%x, length %dB\n", baseaddress, length);
-	NetReceive((uchar *) etherrxbuff, length);
+	net_process_received_packet((uchar *)etherrxbuff, length);
 	return length;
 
 }
diff --git a/drivers/net/xilinx_ll_temac_fifo.c b/drivers/net/xilinx_ll_temac_fifo.c
index b8993cd..78319d7 100644
--- a/drivers/net/xilinx_ll_temac_fifo.c
+++ b/drivers/net/xilinx_ll_temac_fifo.c
@@ -48,7 +48,7 @@ int ll_temac_reset_fifo(struct eth_device *dev)
 int ll_temac_recv_fifo(struct eth_device *dev)
 {
 	int i, length = 0;
-	u32 *buf = (u32 *)NetRxPackets[0];
+	u32 *buf = (u32 *)net_rx_packets[0];
 	struct ll_temac *ll_temac = dev->priv;
 	struct fifo_ctrl *fifo_ctrl = (void *)ll_temac->ctrladdr;
 
@@ -93,7 +93,7 @@ int ll_temac_recv_fifo(struct eth_device *dev)
 		for (i = 0; i < length; i += 4)
 			*buf++ = in_be32(&fifo_ctrl->rdfd);
 
-		NetReceive(NetRxPackets[0], length);
+		net_process_received_packet(net_rx_packets[0], length);
 	}
 
 	return 0;
diff --git a/drivers/net/xilinx_ll_temac_sdma.c b/drivers/net/xilinx_ll_temac_sdma.c
index 32a822e..07c5f6b 100644
--- a/drivers/net/xilinx_ll_temac_sdma.c
+++ b/drivers/net/xilinx_ll_temac_sdma.c
@@ -180,7 +180,7 @@ int ll_temac_init_sdma(struct eth_device *dev)
 		memset(rx_dp, 0, sizeof(*rx_dp));
 		rx_dp->next_p = rx_dp;
 		rx_dp->buf_len = PKTSIZE_ALIGN;
-		rx_dp->phys_buf_p = (u8 *)NetRxPackets[i];
+		rx_dp->phys_buf_p = (u8 *)net_rx_packets[i];
 		flush_cache((u32)rx_dp->phys_buf_p, PKTSIZE_ALIGN);
 	}
 	flush_cache((u32)cdmac_bd.rx, sizeof(cdmac_bd.rx));
@@ -316,7 +316,7 @@ int ll_temac_recv_sdma(struct eth_device *dev)
 	ll_temac->out32(ra[RX_TAILDESC_PTR], (int)&cdmac_bd.rx[rx_idx]);
 
 	if (length > 0 && pb_idx != -1)
-		NetReceive(NetRxPackets[pb_idx], length);
+		net_process_received_packet(net_rx_packets[pb_idx], length);
 
 	return 0;
 }
diff --git a/drivers/net/zynq_gem.c b/drivers/net/zynq_gem.c
index 430e228..c723dbb 100644
--- a/drivers/net/zynq_gem.c
+++ b/drivers/net/zynq_gem.c
@@ -439,7 +439,7 @@ static int zynq_gem_recv(struct eth_device *dev)
 		u32 size = roundup(frame_len, ARCH_DMA_MINALIGN);
 		invalidate_dcache_range(addr, addr + size);
 
-		NetReceive((u8 *)addr, frame_len);
+		net_process_received_packet((u8 *)addr, frame_len);
 
 		if (current_bd->status & ZYNQ_GEM_RXBUF_SOF_MASK)
 			priv->rx_first_buf = priv->rxbd_current;
@@ -513,7 +513,8 @@ int zynq_gem_initialize(bd_t *bis, phys_addr_t base_addr,
 
 	/* Align bd_space to 1MB */
 	bd_space = memalign(1 << MMU_SECTION_SHIFT, BD_SPACE);
-	mmu_set_region_dcache_behaviour((u32)bd_space, BD_SPACE, DCACHE_OFF);
+	mmu_set_region_dcache_behaviour((phys_addr_t)bd_space,
+					BD_SPACE, DCACHE_OFF);
 
 	/* Initialize the bd spaces for tx and rx bd's */
 	priv->tx_bd = (struct emac_bd *)bd_space;
diff --git a/drivers/pci/Kconfig b/drivers/pci/Kconfig
index e69de29..167d405 100644
--- a/drivers/pci/Kconfig
+++ b/drivers/pci/Kconfig
@@ -0,0 +1,22 @@
+menu "PCI"
+
+config DM_PCI
+	bool "Enable driver mode for PCI"
+	depends on DM
+	help
+	  Use driver model for PCI. Driver model is the new method for
+	  orgnising devices in U-Boot. For PCI, driver model keeps track of
+	  available PCI devices, allows scanning of PCI buses and provides
+	  device configuration support.
+
+config PCI_SANDBOX
+	bool "Sandbox PCI support"
+	depends on SANDBOX && DM_PCI
+	help
+	  Support PCI on sandbox, as an emulated bus. This permits testing of
+	  PCI feature such as bus scanning, device configuration and device
+	  access. The available (emulated) devices are defined statically in
+	  the device tree but the normal PCI scan technique is used to find
+	  then.
+
+endmenu
diff --git a/drivers/pci/Makefile b/drivers/pci/Makefile
index 50b7be5..adc238f 100644
--- a/drivers/pci/Makefile
+++ b/drivers/pci/Makefile
@@ -5,8 +5,17 @@
 # SPDX-License-Identifier:	GPL-2.0+
 #
 
+ifneq ($(CONFIG_DM_PCI),)
+obj-$(CONFIG_PCI) += pci-uclass.o pci_compat.o
+obj-$(CONFIG_PCI_SANDBOX) += pci_sandbox.o
+obj-$(CONFIG_SANDBOX) += pci-emul-uclass.o
+obj-$(CONFIG_X86) += pci_x86.o
+else
+obj-$(CONFIG_PCI) += pci.o
+endif
+obj-$(CONFIG_PCI) += pci_common.o pci_auto.o pci_rom.o
+
 obj-$(CONFIG_FSL_PCI_INIT) += fsl_pci_init.o
-obj-$(CONFIG_PCI) += pci.o pci_auto.o pci_rom.o
 obj-$(CONFIG_PCI_INDIRECT_BRIDGE) += pci_indirect.o
 obj-$(CONFIG_PCI_GT64120) += pci_gt64120.o
 obj-$(CONFIG_PCI_MSC01) += pci_msc01.o
diff --git a/drivers/pci/pci-emul-uclass.c b/drivers/pci/pci-emul-uclass.c
new file mode 100644
index 0000000..0f8e3c9
--- /dev/null
+++ b/drivers/pci/pci-emul-uclass.c
@@ -0,0 +1,67 @@
+/*
+ * Copyright (c) 2014 Google, Inc
+ * Written by Simon Glass <sjg@chromium.org>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <dm.h>
+#include <fdtdec.h>
+#include <libfdt.h>
+#include <pci.h>
+#include <dm/lists.h>
+
+DECLARE_GLOBAL_DATA_PTR;
+
+struct sandbox_pci_priv {
+	int dev_count;
+};
+
+int sandbox_pci_get_emul(struct udevice *bus, pci_dev_t find_devfn,
+			 struct udevice **emulp)
+{
+	struct udevice *dev;
+	int ret;
+
+	ret = pci_bus_find_devfn(bus, find_devfn, &dev);
+	if (ret) {
+		debug("%s: Could not find emulator for dev %x\n", __func__,
+		      find_devfn);
+		return ret;
+	}
+
+	ret = device_find_first_child(dev, emulp);
+	if (ret)
+		return ret;
+
+	return *emulp ? 0 : -ENODEV;
+}
+
+static int sandbox_pci_emul_post_probe(struct udevice *dev)
+{
+	struct sandbox_pci_priv *priv = dev->uclass->priv;
+
+	priv->dev_count++;
+	sandbox_set_enable_pci_map(true);
+
+	return 0;
+}
+
+static int sandbox_pci_emul_pre_remove(struct udevice *dev)
+{
+	struct sandbox_pci_priv *priv = dev->uclass->priv;
+
+	priv->dev_count--;
+	sandbox_set_enable_pci_map(priv->dev_count > 0);
+
+	return 0;
+}
+
+UCLASS_DRIVER(pci_emul) = {
+	.id		= UCLASS_PCI_EMUL,
+	.name		= "pci_emul",
+	.post_probe	= sandbox_pci_emul_post_probe,
+	.pre_remove	= sandbox_pci_emul_pre_remove,
+	.priv_auto_alloc_size	= sizeof(struct sandbox_pci_priv),
+};
diff --git a/drivers/pci/pci-uclass.c b/drivers/pci/pci-uclass.c
new file mode 100644
index 0000000..d48d865
--- /dev/null
+++ b/drivers/pci/pci-uclass.c
@@ -0,0 +1,639 @@
+/*
+ * Copyright (c) 2014 Google, Inc
+ * Written by Simon Glass <sjg@chromium.org>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <dm.h>
+#include <errno.h>
+#include <fdtdec.h>
+#include <inttypes.h>
+#include <pci.h>
+#include <dm/lists.h>
+#include <dm/root.h>
+#include <dm/device-internal.h>
+
+DECLARE_GLOBAL_DATA_PTR;
+
+struct pci_controller *pci_bus_to_hose(int busnum)
+{
+	struct udevice *bus;
+	int ret;
+
+	ret = uclass_get_device_by_seq(UCLASS_PCI, busnum, &bus);
+	if (ret) {
+		debug("%s: Cannot get bus %d: ret=%d\n", __func__, busnum, ret);
+		return NULL;
+	}
+	return dev_get_uclass_priv(bus);
+}
+
+/**
+ * pci_get_bus_max() - returns the bus number of the last active bus
+ *
+ * @return last bus number, or -1 if no active buses
+ */
+static int pci_get_bus_max(void)
+{
+	struct udevice *bus;
+	struct uclass *uc;
+	int ret = -1;
+
+	ret = uclass_get(UCLASS_PCI, &uc);
+	uclass_foreach_dev(bus, uc) {
+		if (bus->seq > ret)
+			ret = bus->seq;
+	}
+
+	debug("%s: ret=%d\n", __func__, ret);
+
+	return ret;
+}
+
+int pci_last_busno(void)
+{
+	struct pci_controller *hose;
+	struct udevice *bus;
+	struct uclass *uc;
+	int ret;
+
+	debug("pci_last_busno\n");
+	ret = uclass_get(UCLASS_PCI, &uc);
+	if (ret || list_empty(&uc->dev_head))
+		return -1;
+
+	/* Probe the last bus */
+	bus = list_entry(uc->dev_head.prev, struct udevice, uclass_node);
+	debug("bus = %p, %s\n", bus, bus->name);
+	assert(bus);
+	ret = device_probe(bus);
+	if (ret)
+		return ret;
+
+	/* If that bus has bridges, we may have new buses now. Get the last */
+	bus = list_entry(uc->dev_head.prev, struct udevice, uclass_node);
+	hose = dev_get_uclass_priv(bus);
+	debug("bus = %s, hose = %p\n", bus->name, hose);
+
+	return hose->last_busno;
+}
+
+int pci_get_ff(enum pci_size_t size)
+{
+	switch (size) {
+	case PCI_SIZE_8:
+		return 0xff;
+	case PCI_SIZE_16:
+		return 0xffff;
+	default:
+		return 0xffffffff;
+	}
+}
+
+int pci_bus_find_devfn(struct udevice *bus, pci_dev_t find_devfn,
+		       struct udevice **devp)
+{
+	struct udevice *dev;
+
+	for (device_find_first_child(bus, &dev);
+	     dev;
+	     device_find_next_child(&dev)) {
+		struct pci_child_platdata *pplat;
+
+		pplat = dev_get_parent_platdata(dev);
+		if (pplat && pplat->devfn == find_devfn) {
+			*devp = dev;
+			return 0;
+		}
+	}
+
+	return -ENODEV;
+}
+
+int pci_bus_find_bdf(pci_dev_t bdf, struct udevice **devp)
+{
+	struct udevice *bus;
+	int ret;
+
+	ret = uclass_get_device_by_seq(UCLASS_PCI, PCI_BUS(bdf), &bus);
+	if (ret)
+		return ret;
+	return pci_bus_find_devfn(bus, PCI_MASK_BUS(bdf), devp);
+}
+
+static int pci_device_matches_ids(struct udevice *dev,
+				  struct pci_device_id *ids)
+{
+	struct pci_child_platdata *pplat;
+	int i;
+
+	pplat = dev_get_parent_platdata(dev);
+	if (!pplat)
+		return -EINVAL;
+	for (i = 0; ids[i].vendor != 0; i++) {
+		if (pplat->vendor == ids[i].vendor &&
+		    pplat->device == ids[i].device)
+			return i;
+	}
+
+	return -EINVAL;
+}
+
+int pci_bus_find_devices(struct udevice *bus, struct pci_device_id *ids,
+			 int *indexp, struct udevice **devp)
+{
+	struct udevice *dev;
+
+	/* Scan all devices on this bus */
+	for (device_find_first_child(bus, &dev);
+	     dev;
+	     device_find_next_child(&dev)) {
+		if (pci_device_matches_ids(dev, ids) >= 0) {
+			if ((*indexp)-- <= 0) {
+				*devp = dev;
+				return 0;
+			}
+		}
+	}
+
+	return -ENODEV;
+}
+
+int pci_find_device_id(struct pci_device_id *ids, int index,
+		       struct udevice **devp)
+{
+	struct udevice *bus;
+
+	/* Scan all known buses */
+	for (uclass_first_device(UCLASS_PCI, &bus);
+	     bus;
+	     uclass_next_device(&bus)) {
+		if (!pci_bus_find_devices(bus, ids, &index, devp))
+			return 0;
+	}
+	*devp = NULL;
+
+	return -ENODEV;
+}
+
+int pci_bus_write_config(struct udevice *bus, pci_dev_t bdf, int offset,
+			 unsigned long value, enum pci_size_t size)
+{
+	struct dm_pci_ops *ops;
+
+	ops = pci_get_ops(bus);
+	if (!ops->write_config)
+		return -ENOSYS;
+	return ops->write_config(bus, bdf, offset, value, size);
+}
+
+int pci_write_config(pci_dev_t bdf, int offset, unsigned long value,
+		     enum pci_size_t size)
+{
+	struct udevice *bus;
+	int ret;
+
+	ret = uclass_get_device_by_seq(UCLASS_PCI, PCI_BUS(bdf), &bus);
+	if (ret)
+		return ret;
+
+	return pci_bus_write_config(bus, PCI_MASK_BUS(bdf), offset, value,
+				    size);
+}
+
+int pci_write_config32(pci_dev_t bdf, int offset, u32 value)
+{
+	return pci_write_config(bdf, offset, value, PCI_SIZE_32);
+}
+
+int pci_write_config16(pci_dev_t bdf, int offset, u16 value)
+{
+	return pci_write_config(bdf, offset, value, PCI_SIZE_16);
+}
+
+int pci_write_config8(pci_dev_t bdf, int offset, u8 value)
+{
+	return pci_write_config(bdf, offset, value, PCI_SIZE_8);
+}
+
+int pci_bus_read_config(struct udevice *bus, pci_dev_t bdf, int offset,
+			unsigned long *valuep, enum pci_size_t size)
+{
+	struct dm_pci_ops *ops;
+
+	ops = pci_get_ops(bus);
+	if (!ops->read_config)
+		return -ENOSYS;
+	return ops->read_config(bus, bdf, offset, valuep, size);
+}
+
+int pci_read_config(pci_dev_t bdf, int offset, unsigned long *valuep,
+		    enum pci_size_t size)
+{
+	struct udevice *bus;
+	int ret;
+
+	ret = uclass_get_device_by_seq(UCLASS_PCI, PCI_BUS(bdf), &bus);
+	if (ret)
+		return ret;
+
+	return pci_bus_read_config(bus, PCI_MASK_BUS(bdf), offset, valuep,
+				   size);
+}
+
+int pci_read_config32(pci_dev_t bdf, int offset, u32 *valuep)
+{
+	unsigned long value;
+	int ret;
+
+	ret = pci_read_config(bdf, offset, &value, PCI_SIZE_32);
+	if (ret)
+		return ret;
+	*valuep = value;
+
+	return 0;
+}
+
+int pci_read_config16(pci_dev_t bdf, int offset, u16 *valuep)
+{
+	unsigned long value;
+	int ret;
+
+	ret = pci_read_config(bdf, offset, &value, PCI_SIZE_16);
+	if (ret)
+		return ret;
+	*valuep = value;
+
+	return 0;
+}
+
+int pci_read_config8(pci_dev_t bdf, int offset, u8 *valuep)
+{
+	unsigned long value;
+	int ret;
+
+	ret = pci_read_config(bdf, offset, &value, PCI_SIZE_8);
+	if (ret)
+		return ret;
+	*valuep = value;
+
+	return 0;
+}
+
+int pci_auto_config_devices(struct udevice *bus)
+{
+	struct pci_controller *hose = bus->uclass_priv;
+	unsigned int sub_bus;
+	struct udevice *dev;
+	int ret;
+
+	sub_bus = bus->seq;
+	debug("%s: start\n", __func__);
+	pciauto_config_init(hose);
+	for (ret = device_find_first_child(bus, &dev);
+	     !ret && dev;
+	     ret = device_find_next_child(&dev)) {
+		struct pci_child_platdata *pplat;
+
+		pplat = dev_get_parent_platdata(dev);
+		unsigned int max_bus;
+		pci_dev_t bdf;
+
+		bdf = PCI_ADD_BUS(bus->seq, pplat->devfn);
+		debug("%s: device %s\n", __func__, dev->name);
+		max_bus = pciauto_config_device(hose, bdf);
+		sub_bus = max(sub_bus, max_bus);
+	}
+	debug("%s: done\n", __func__);
+
+	return sub_bus;
+}
+
+int dm_pci_hose_probe_bus(struct pci_controller *hose, pci_dev_t bdf)
+{
+	struct udevice *parent, *bus;
+	int sub_bus;
+	int ret;
+
+	debug("%s\n", __func__);
+	parent = hose->bus;
+
+	/* Find the bus within the parent */
+	ret = pci_bus_find_devfn(parent, bdf, &bus);
+	if (ret) {
+		debug("%s: Cannot find device %x on bus %s: %d\n", __func__,
+		      bdf, parent->name, ret);
+		return ret;
+	}
+
+	sub_bus = pci_get_bus_max() + 1;
+	debug("%s: bus = %d/%s\n", __func__, sub_bus, bus->name);
+	pciauto_prescan_setup_bridge(hose, bdf, bus->seq);
+
+	ret = device_probe(bus);
+	if (ret) {
+		debug("%s: Cannot probe bus bus %s: %d\n", __func__, bus->name,
+		      ret);
+		return ret;
+	}
+	if (sub_bus != bus->seq) {
+		printf("%s: Internal error, bus '%s' got seq %d, expected %d\n",
+		       __func__, bus->name, bus->seq, sub_bus);
+		return -EPIPE;
+	}
+	sub_bus = pci_get_bus_max();
+	pciauto_postscan_setup_bridge(hose, bdf, sub_bus);
+
+	return sub_bus;
+}
+
+int pci_bind_bus_devices(struct udevice *bus)
+{
+	ulong vendor, device;
+	ulong header_type;
+	pci_dev_t devfn, end;
+	bool found_multi;
+	int ret;
+
+	found_multi = false;
+	end = PCI_DEVFN(PCI_MAX_PCI_DEVICES - 1, PCI_MAX_PCI_FUNCTIONS - 1);
+	for (devfn = PCI_DEVFN(0, 0); devfn < end; devfn += PCI_DEVFN(0, 1)) {
+		struct pci_child_platdata *pplat;
+		struct udevice *dev;
+		ulong class;
+
+		if (PCI_FUNC(devfn) && !found_multi)
+			continue;
+		/* Check only the first access, we don't expect problems */
+		ret = pci_bus_read_config(bus, devfn, PCI_HEADER_TYPE,
+					  &header_type, PCI_SIZE_8);
+		if (ret)
+			goto error;
+		pci_bus_read_config(bus, devfn, PCI_VENDOR_ID, &vendor,
+				    PCI_SIZE_16);
+		if (vendor == 0xffff || vendor == 0x0000)
+			continue;
+
+		if (!PCI_FUNC(devfn))
+			found_multi = header_type & 0x80;
+
+		debug("%s: bus %d/%s: found device %x, function %d\n", __func__,
+		      bus->seq, bus->name, PCI_DEV(devfn), PCI_FUNC(devfn));
+		pci_bus_read_config(bus, devfn, PCI_DEVICE_ID, &device,
+				    PCI_SIZE_16);
+		pci_bus_read_config(bus, devfn, PCI_CLASS_DEVICE, &class,
+				    PCI_SIZE_16);
+
+		/* Find this device in the device tree */
+		ret = pci_bus_find_devfn(bus, devfn, &dev);
+
+		/* If nothing in the device tree, bind a generic device */
+		if (ret == -ENODEV) {
+			char name[30], *str;
+			const char *drv;
+
+			sprintf(name, "pci_%x:%x.%x", bus->seq,
+				PCI_DEV(devfn), PCI_FUNC(devfn));
+			str = strdup(name);
+			if (!str)
+				return -ENOMEM;
+			drv = class == PCI_CLASS_BRIDGE_PCI ?
+				"pci_bridge_drv" : "pci_generic_drv";
+			ret = device_bind_driver(bus, drv, str, &dev);
+		}
+		if (ret)
+			return ret;
+
+		/* Update the platform data */
+		pplat = dev_get_parent_platdata(dev);
+		pplat->devfn = devfn;
+		pplat->vendor = vendor;
+		pplat->device = device;
+		pplat->class = class;
+	}
+
+	return 0;
+error:
+	printf("Cannot read bus configuration: %d\n", ret);
+
+	return ret;
+}
+
+static int pci_uclass_post_bind(struct udevice *bus)
+{
+	/*
+	 * Scan the device tree for devices. This does not probe the PCI bus,
+	 * as this is not permitted while binding. It just finds devices
+	 * mentioned in the device tree.
+	 *
+	 * Before relocation, only bind devices marked for pre-relocation
+	 * use.
+	 */
+	return dm_scan_fdt_node(bus, gd->fdt_blob, bus->of_offset,
+				gd->flags & GD_FLG_RELOC ? false : true);
+}
+
+static int decode_regions(struct pci_controller *hose, const void *blob,
+			  int parent_node, int node)
+{
+	int pci_addr_cells, addr_cells, size_cells;
+	int cells_per_record;
+	const u32 *prop;
+	int len;
+	int i;
+
+	prop = fdt_getprop(blob, node, "ranges", &len);
+	if (!prop)
+		return -EINVAL;
+	pci_addr_cells = fdt_address_cells(blob, node);
+	addr_cells = fdt_address_cells(blob, parent_node);
+	size_cells = fdt_size_cells(blob, node);
+
+	/* PCI addresses are always 3-cells */
+	len /= sizeof(u32);
+	cells_per_record = pci_addr_cells + addr_cells + size_cells;
+	hose->region_count = 0;
+	debug("%s: len=%d, cells_per_record=%d\n", __func__, len,
+	      cells_per_record);
+	for (i = 0; i < MAX_PCI_REGIONS; i++, len -= cells_per_record) {
+		u64 pci_addr, addr, size;
+		int space_code;
+		u32 flags;
+		int type;
+
+		if (len < cells_per_record)
+			break;
+		flags = fdt32_to_cpu(prop[0]);
+		space_code = (flags >> 24) & 3;
+		pci_addr = fdtdec_get_number(prop + 1, 2);
+		prop += pci_addr_cells;
+		addr = fdtdec_get_number(prop, addr_cells);
+		prop += addr_cells;
+		size = fdtdec_get_number(prop, size_cells);
+		prop += size_cells;
+		debug("%s: region %d, pci_addr=%" PRIx64 ", addr=%" PRIx64
+		      ", size=%" PRIx64 ", space_code=%d\n", __func__,
+		      hose->region_count, pci_addr, addr, size, space_code);
+		if (space_code & 2) {
+			type = flags & (1U << 30) ? PCI_REGION_PREFETCH :
+					PCI_REGION_MEM;
+		} else if (space_code & 1) {
+			type = PCI_REGION_IO;
+		} else {
+			continue;
+		}
+		debug(" - type=%d\n", type);
+		pci_set_region(hose->regions + hose->region_count++, pci_addr,
+			       addr, size, type);
+	}
+
+	/* Add a region for our local memory */
+	pci_set_region(hose->regions + hose->region_count++, 0, 0,
+		       gd->ram_size, PCI_REGION_MEM | PCI_REGION_SYS_MEMORY);
+
+	return 0;
+}
+
+static int pci_uclass_pre_probe(struct udevice *bus)
+{
+	struct pci_controller *hose;
+	int ret;
+
+	debug("%s, bus=%d/%s, parent=%s\n", __func__, bus->seq, bus->name,
+	      bus->parent->name);
+	hose = bus->uclass_priv;
+
+	/* For bridges, use the top-level PCI controller */
+	if (device_get_uclass_id(bus->parent) == UCLASS_ROOT) {
+		hose->ctlr = bus;
+		ret = decode_regions(hose, gd->fdt_blob, bus->parent->of_offset,
+				bus->of_offset);
+		if (ret) {
+			debug("%s: Cannot decode regions\n", __func__);
+			return ret;
+		}
+	} else {
+		struct pci_controller *parent_hose;
+
+		parent_hose = dev_get_uclass_priv(bus->parent);
+		hose->ctlr = parent_hose->bus;
+	}
+	hose->bus = bus;
+	hose->first_busno = bus->seq;
+	hose->last_busno = bus->seq;
+
+	return 0;
+}
+
+static int pci_uclass_post_probe(struct udevice *bus)
+{
+	int ret;
+
+	/* Don't scan buses before relocation */
+	if (!(gd->flags & GD_FLG_RELOC))
+		return 0;
+
+	debug("%s: probing bus %d\n", __func__, bus->seq);
+	ret = pci_bind_bus_devices(bus);
+	if (ret)
+		return ret;
+
+#ifdef CONFIG_PCI_PNP
+	ret = pci_auto_config_devices(bus);
+#endif
+
+	return ret < 0 ? ret : 0;
+}
+
+static int pci_uclass_child_post_bind(struct udevice *dev)
+{
+	struct pci_child_platdata *pplat;
+	struct fdt_pci_addr addr;
+	int ret;
+
+	if (dev->of_offset == -1)
+		return 0;
+
+	/*
+	 * We could read vendor, device, class if available. But for now we
+	 * just check the address.
+	 */
+	pplat = dev_get_parent_platdata(dev);
+	ret = fdtdec_get_pci_addr(gd->fdt_blob, dev->of_offset,
+				  FDT_PCI_SPACE_CONFIG, "reg", &addr);
+
+	if (ret) {
+		if (ret != -ENOENT)
+			return -EINVAL;
+	} else {
+		/* extract the bdf from fdt_pci_addr */
+		pplat->devfn = addr.phys_hi & 0xffff00;
+	}
+
+	return 0;
+}
+
+int pci_bridge_read_config(struct udevice *bus, pci_dev_t devfn, uint offset,
+			   ulong *valuep, enum pci_size_t size)
+{
+	struct pci_controller *hose = bus->uclass_priv;
+	pci_dev_t bdf = PCI_ADD_BUS(bus->seq, devfn);
+
+	return pci_bus_read_config(hose->ctlr, bdf, offset, valuep, size);
+}
+
+int pci_bridge_write_config(struct udevice *bus, pci_dev_t devfn, uint offset,
+			    ulong value, enum pci_size_t size)
+{
+	struct pci_controller *hose = bus->uclass_priv;
+	pci_dev_t bdf = PCI_ADD_BUS(bus->seq, devfn);
+
+	return pci_bus_write_config(hose->ctlr, bdf, offset, value, size);
+}
+
+UCLASS_DRIVER(pci) = {
+	.id		= UCLASS_PCI,
+	.name		= "pci",
+	.post_bind	= pci_uclass_post_bind,
+	.pre_probe	= pci_uclass_pre_probe,
+	.post_probe	= pci_uclass_post_probe,
+	.child_post_bind = pci_uclass_child_post_bind,
+	.per_device_auto_alloc_size = sizeof(struct pci_controller),
+	.per_child_platdata_auto_alloc_size =
+			sizeof(struct pci_child_platdata),
+};
+
+static const struct dm_pci_ops pci_bridge_ops = {
+	.read_config	= pci_bridge_read_config,
+	.write_config	= pci_bridge_write_config,
+};
+
+static const struct udevice_id pci_bridge_ids[] = {
+	{ .compatible = "pci-bridge" },
+	{ }
+};
+
+U_BOOT_DRIVER(pci_bridge_drv) = {
+	.name		= "pci_bridge_drv",
+	.id		= UCLASS_PCI,
+	.of_match	= pci_bridge_ids,
+	.ops		= &pci_bridge_ops,
+};
+
+UCLASS_DRIVER(pci_generic) = {
+	.id		= UCLASS_PCI_GENERIC,
+	.name		= "pci_generic",
+};
+
+static const struct udevice_id pci_generic_ids[] = {
+	{ .compatible = "pci-generic" },
+	{ }
+};
+
+U_BOOT_DRIVER(pci_generic_drv) = {
+	.name		= "pci_generic_drv",
+	.id		= UCLASS_PCI_GENERIC,
+	.of_match	= pci_generic_ids,
+};
diff --git a/drivers/pci/pci.c b/drivers/pci/pci.c
index e1296ca..3babd94 100644
--- a/drivers/pci/pci.c
+++ b/drivers/pci/pci.c
@@ -101,25 +101,6 @@ PCI_READ_VIA_DWORD_OP(word, u16 *, 0x02)
 PCI_WRITE_VIA_DWORD_OP(byte, u8, 0x03, 0x000000ff)
 PCI_WRITE_VIA_DWORD_OP(word, u16, 0x02, 0x0000ffff)
 
-/* Get a virtual address associated with a BAR region */
-void *pci_map_bar(pci_dev_t pdev, int bar, int flags)
-{
-	pci_addr_t pci_bus_addr;
-	u32 bar_response;
-
-	/* read BAR address */
-	pci_read_config_dword(pdev, bar, &bar_response);
-	pci_bus_addr = (pci_addr_t)(bar_response & ~0xf);
-
-	/*
-	 * Pass "0" as the length argument to pci_bus_to_virt.  The arg
-	 * isn't actualy used on any platform because u-boot assumes a static
-	 * linear mapping.  In the future, this could read the BAR size
-	 * and pass that as the size if needed.
-	 */
-	return pci_bus_to_virt(pdev, pci_bus_addr, flags, 0, MAP_NOCACHE);
-}
-
 /*
  *
  */
@@ -187,106 +168,22 @@ int pci_last_busno(void)
 pci_dev_t pci_find_devices(struct pci_device_id *ids, int index)
 {
 	struct pci_controller * hose;
-	u16 vendor, device;
-	u8 header_type;
 	pci_dev_t bdf;
-	int i, bus, found_multi = 0;
+	int bus;
 
 	for (hose = pci_get_hose_head(); hose; hose = hose->next) {
 #ifdef CONFIG_SYS_SCSI_SCAN_BUS_REVERSE
-		for (bus = hose->last_busno; bus >= hose->first_busno; bus--)
+		for (bus = hose->last_busno; bus >= hose->first_busno; bus--) {
 #else
-		for (bus = hose->first_busno; bus <= hose->last_busno; bus++)
+		for (bus = hose->first_busno; bus <= hose->last_busno; bus++) {
 #endif
-			for (bdf = PCI_BDF(bus, 0, 0);
-			     bdf < PCI_BDF(bus + 1, 0, 0);
-			     bdf += PCI_BDF(0, 0, 1)) {
-				if (pci_skip_dev(hose, bdf))
-					continue;
-
-				if (!PCI_FUNC(bdf)) {
-					pci_read_config_byte(bdf,
-							     PCI_HEADER_TYPE,
-							     &header_type);
-
-					found_multi = header_type & 0x80;
-				} else {
-					if (!found_multi)
-						continue;
-				}
-
-				pci_read_config_word(bdf,
-						     PCI_VENDOR_ID,
-						     &vendor);
-				pci_read_config_word(bdf,
-						     PCI_DEVICE_ID,
-						     &device);
-
-				for (i = 0; ids[i].vendor != 0; i++) {
-					if (vendor == ids[i].vendor &&
-					    device == ids[i].device) {
-						if (index <= 0)
-							return bdf;
-
-						index--;
-					}
-				}
-			}
-	}
-
-	return -1;
-}
-
-pci_dev_t pci_find_class(uint find_class, int index)
-{
-	int bus;
-	int devnum;
-	pci_dev_t bdf;
-	uint32_t class;
-
-	for (bus = 0; bus <= pci_last_busno(); bus++) {
-		for (devnum = 0; devnum < PCI_MAX_PCI_DEVICES - 1; devnum++) {
-			pci_read_config_dword(PCI_BDF(bus, devnum, 0),
-					      PCI_CLASS_REVISION, &class);
-			if (class >> 16 == 0xffff)
-				continue;
-
-			for (bdf = PCI_BDF(bus, devnum, 0);
-					bdf <= PCI_BDF(bus, devnum,
-						PCI_MAX_PCI_FUNCTIONS - 1);
-					bdf += PCI_BDF(0, 0, 1)) {
-				pci_read_config_dword(bdf, PCI_CLASS_REVISION,
-						      &class);
-				class >>= 8;
-
-				if (class != find_class)
-					continue;
-				/*
-				 * Decrement the index. We want to return the
-				 * correct device, so index is 0 for the first
-				 * matching device, 1 for the second, etc.
-				 */
-				if (index) {
-					index--;
-					continue;
-				}
-				/* Return index'th controller. */
+			bdf = pci_hose_find_devices(hose, bus, ids, &index);
+			if (bdf != -1)
 				return bdf;
-			}
 		}
 	}
 
-	return -ENODEV;
-}
-
-pci_dev_t pci_find_device(unsigned int vendor, unsigned int device, int index)
-{
-	struct pci_device_id ids[2] = { {}, {0, 0} };
-
-	ids[0].vendor = vendor;
-	ids[0].device = device;
-
-	return pci_find_devices(ids, index);
+	return -1;
 }
 
 /*
@@ -355,87 +252,6 @@ pci_addr_t pci_hose_phys_to_bus (struct pci_controller *hose,
 	return bus_addr;
 }
 
-int __pci_hose_bus_to_phys(struct pci_controller *hose,
-				pci_addr_t bus_addr,
-				unsigned long flags,
-				unsigned long skip_mask,
-				phys_addr_t *pa)
-{
-	struct pci_region *res;
-	int i;
-
-	for (i = 0; i < hose->region_count; i++) {
-		res = &hose->regions[i];
-
-		if (((res->flags ^ flags) & PCI_REGION_TYPE) != 0)
-			continue;
-
-		if (res->flags & skip_mask)
-			continue;
-
-		if (bus_addr >= res->bus_start &&
-			(bus_addr - res->bus_start) < res->size) {
-			*pa = (bus_addr - res->bus_start + res->phys_start);
-			return 0;
-		}
-	}
-
-	return 1;
-}
-
-phys_addr_t pci_hose_bus_to_phys(struct pci_controller* hose,
-				 pci_addr_t bus_addr,
-				 unsigned long flags)
-{
-	phys_addr_t phys_addr = 0;
-	int ret;
-
-	if (!hose) {
-		puts("pci_hose_bus_to_phys: invalid hose\n");
-		return phys_addr;
-	}
-
-	/*
-	 * if PCI_REGION_MEM is set we do a two pass search with preference
-	 * on matches that don't have PCI_REGION_SYS_MEMORY set
-	 */
-	if ((flags & PCI_REGION_MEM) == PCI_REGION_MEM) {
-		ret = __pci_hose_bus_to_phys(hose, bus_addr,
-				flags, PCI_REGION_SYS_MEMORY, &phys_addr);
-		if (!ret)
-			return phys_addr;
-	}
-
-	ret = __pci_hose_bus_to_phys(hose, bus_addr, flags, 0, &phys_addr);
-
-	if (ret)
-		puts("pci_hose_bus_to_phys: invalid physical address\n");
-
-	return phys_addr;
-}
-
-void pci_write_bar32(struct pci_controller *hose, pci_dev_t dev, int barnum,
-		     u32 addr_and_ctrl)
-{
-	int bar;
-
-	bar = PCI_BASE_ADDRESS_0 + barnum * 4;
-	pci_hose_write_config_dword(hose, dev, bar, addr_and_ctrl);
-}
-
-u32 pci_read_bar32(struct pci_controller *hose, pci_dev_t dev, int barnum)
-{
-	u32 addr;
-	int bar;
-
-	bar = PCI_BASE_ADDRESS_0 + barnum * 4;
-	pci_hose_read_config_dword(hose, dev, bar, &addr);
-	if (addr & PCI_BASE_ADDRESS_SPACE_IO)
-		return addr & PCI_BASE_ADDRESS_IO_MASK;
-	else
-		return addr & PCI_BASE_ADDRESS_MEM_MASK;
-}
-
 int pci_hose_config_device(struct pci_controller *hose,
 			   pci_dev_t dev,
 			   unsigned long io,
@@ -576,91 +392,6 @@ void pci_cfgfunc_do_nothing(struct pci_controller *hose,
  */
 extern int pciauto_config_device(struct pci_controller *hose, pci_dev_t dev);
 
-#if defined(CONFIG_CMD_PCI) || defined(CONFIG_PCI_SCAN_SHOW)
-const char * pci_class_str(u8 class)
-{
-	switch (class) {
-	case PCI_CLASS_NOT_DEFINED:
-		return "Build before PCI Rev2.0";
-		break;
-	case PCI_BASE_CLASS_STORAGE:
-		return "Mass storage controller";
-		break;
-	case PCI_BASE_CLASS_NETWORK:
-		return "Network controller";
-		break;
-	case PCI_BASE_CLASS_DISPLAY:
-		return "Display controller";
-		break;
-	case PCI_BASE_CLASS_MULTIMEDIA:
-		return "Multimedia device";
-		break;
-	case PCI_BASE_CLASS_MEMORY:
-		return "Memory controller";
-		break;
-	case PCI_BASE_CLASS_BRIDGE:
-		return "Bridge device";
-		break;
-	case PCI_BASE_CLASS_COMMUNICATION:
-		return "Simple comm. controller";
-		break;
-	case PCI_BASE_CLASS_SYSTEM:
-		return "Base system peripheral";
-		break;
-	case PCI_BASE_CLASS_INPUT:
-		return "Input device";
-		break;
-	case PCI_BASE_CLASS_DOCKING:
-		return "Docking station";
-		break;
-	case PCI_BASE_CLASS_PROCESSOR:
-		return "Processor";
-		break;
-	case PCI_BASE_CLASS_SERIAL:
-		return "Serial bus controller";
-		break;
-	case PCI_BASE_CLASS_INTELLIGENT:
-		return "Intelligent controller";
-		break;
-	case PCI_BASE_CLASS_SATELLITE:
-		return "Satellite controller";
-		break;
-	case PCI_BASE_CLASS_CRYPT:
-		return "Cryptographic device";
-		break;
-	case PCI_BASE_CLASS_SIGNAL_PROCESSING:
-		return "DSP";
-		break;
-	case PCI_CLASS_OTHERS:
-		return "Does not fit any class";
-		break;
-	default:
-	return  "???";
-		break;
-	};
-}
-#endif /* CONFIG_CMD_PCI || CONFIG_PCI_SCAN_SHOW */
-
-__weak int pci_skip_dev(struct pci_controller *hose, pci_dev_t dev)
-{
-	/*
-	 * Check if pci device should be skipped in configuration
-	 */
-	if (dev == PCI_BDF(hose->first_busno, 0, 0)) {
-#if defined(CONFIG_PCI_CONFIG_HOST_BRIDGE) /* don't skip host bridge */
-		/*
-		 * Only skip configuration if "pciconfighost" is not set
-		 */
-		if (getenv("pciconfighost") == NULL)
-			return 1;
-#else
-		return 1;
-#endif
-	}
-
-	return 0;
-}
-
 #ifdef CONFIG_PCI_SCAN_SHOW
 __weak int pci_print_dev(struct pci_controller *hose, pci_dev_t dev)
 {
diff --git a/drivers/pci/pci_auto.c b/drivers/pci/pci_auto.c
index 378efbf..e8da977 100644
--- a/drivers/pci/pci_auto.c
+++ b/drivers/pci/pci_auto.c
@@ -432,13 +432,20 @@ int pciauto_config_device(struct pci_controller *hose, pci_dev_t dev)
 
 	switch (class) {
 	case PCI_CLASS_BRIDGE_PCI:
-		hose->current_busno++;
+		DEBUGF("PCI Autoconfig: Found P2P bridge, device %d\n",
+		       PCI_DEV(dev));
+
 		pciauto_setup_device(hose, dev, 2, hose->pci_mem,
 			hose->pci_prefetch, hose->pci_io);
 
-		DEBUGF("PCI Autoconfig: Found P2P bridge, device %d\n", PCI_DEV(dev));
-
+#ifdef CONFIG_DM_PCI
+		n = dm_pci_hose_probe_bus(hose, dev);
+		if (n < 0)
+			return n;
+		sub_bus = (unsigned int)n;
+#else
 		/* Passing in current_busno allows for sibling P2P bridges */
+		hose->current_busno++;
 		pciauto_prescan_setup_bridge(hose, dev, hose->current_busno);
 		/*
 		 * need to figure out if this is a subordinate bridge on the bus
@@ -451,6 +458,7 @@ int pciauto_config_device(struct pci_controller *hose, pci_dev_t dev)
 		pciauto_postscan_setup_bridge(hose, dev, sub_bus);
 
 		sub_bus = hose->current_busno;
+#endif
 		break;
 
 	case PCI_CLASS_STORAGE_IDE:
@@ -475,7 +483,9 @@ int pciauto_config_device(struct pci_controller *hose, pci_dev_t dev)
 		DEBUGF("PCI Autoconfig: Found P2CardBus bridge, device %d\n",
 			PCI_DEV(dev));
 
+#ifndef CONFIG_DM_PCI
 		hose->current_busno++;
+#endif
 		break;
 
 #if defined(CONFIG_PCIAUTO_SKIP_HOST_BRIDGE)
diff --git a/drivers/pci/pci_common.c b/drivers/pci/pci_common.c
new file mode 100644
index 0000000..24c66bb
--- /dev/null
+++ b/drivers/pci/pci_common.c
@@ -0,0 +1,292 @@
+/*
+ * Copyright (c) 2014 Google, Inc
+ *
+ * (C) Copyright 2001 Sysgo Real-Time Solutions, GmbH <www.elinos.com>
+ * Andreas Heppel <aheppel@sysgo.de>
+ *
+ * (C) Copyright 2002, 2003
+ * Wolfgang Denk, DENX Software Engineering, wd@denx.de.
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <errno.h>
+#include <pci.h>
+#include <asm/io.h>
+
+const char *pci_class_str(u8 class)
+{
+	switch (class) {
+	case PCI_CLASS_NOT_DEFINED:
+		return "Build before PCI Rev2.0";
+		break;
+	case PCI_BASE_CLASS_STORAGE:
+		return "Mass storage controller";
+		break;
+	case PCI_BASE_CLASS_NETWORK:
+		return "Network controller";
+		break;
+	case PCI_BASE_CLASS_DISPLAY:
+		return "Display controller";
+		break;
+	case PCI_BASE_CLASS_MULTIMEDIA:
+		return "Multimedia device";
+		break;
+	case PCI_BASE_CLASS_MEMORY:
+		return "Memory controller";
+		break;
+	case PCI_BASE_CLASS_BRIDGE:
+		return "Bridge device";
+		break;
+	case PCI_BASE_CLASS_COMMUNICATION:
+		return "Simple comm. controller";
+		break;
+	case PCI_BASE_CLASS_SYSTEM:
+		return "Base system peripheral";
+		break;
+	case PCI_BASE_CLASS_INPUT:
+		return "Input device";
+		break;
+	case PCI_BASE_CLASS_DOCKING:
+		return "Docking station";
+		break;
+	case PCI_BASE_CLASS_PROCESSOR:
+		return "Processor";
+		break;
+	case PCI_BASE_CLASS_SERIAL:
+		return "Serial bus controller";
+		break;
+	case PCI_BASE_CLASS_INTELLIGENT:
+		return "Intelligent controller";
+		break;
+	case PCI_BASE_CLASS_SATELLITE:
+		return "Satellite controller";
+		break;
+	case PCI_BASE_CLASS_CRYPT:
+		return "Cryptographic device";
+		break;
+	case PCI_BASE_CLASS_SIGNAL_PROCESSING:
+		return "DSP";
+		break;
+	case PCI_CLASS_OTHERS:
+		return "Does not fit any class";
+		break;
+	default:
+	return  "???";
+		break;
+	};
+}
+
+pci_dev_t pci_find_class(uint find_class, int index)
+{
+	int bus;
+	int devnum;
+	pci_dev_t bdf;
+	uint32_t class;
+
+	for (bus = 0; bus <= pci_last_busno(); bus++) {
+		for (devnum = 0; devnum < PCI_MAX_PCI_DEVICES - 1; devnum++) {
+			pci_read_config_dword(PCI_BDF(bus, devnum, 0),
+					      PCI_CLASS_REVISION, &class);
+			if (class >> 16 == 0xffff)
+				continue;
+
+			for (bdf = PCI_BDF(bus, devnum, 0);
+					bdf <= PCI_BDF(bus, devnum,
+						PCI_MAX_PCI_FUNCTIONS - 1);
+					bdf += PCI_BDF(0, 0, 1)) {
+				pci_read_config_dword(bdf, PCI_CLASS_REVISION,
+						      &class);
+				class >>= 8;
+
+				if (class != find_class)
+					continue;
+				/*
+				 * Decrement the index. We want to return the
+				 * correct device, so index is 0 for the first
+				 * matching device, 1 for the second, etc.
+				 */
+				if (index) {
+					index--;
+					continue;
+				}
+				/* Return index'th controller. */
+				return bdf;
+			}
+		}
+	}
+
+	return -ENODEV;
+}
+
+__weak int pci_skip_dev(struct pci_controller *hose, pci_dev_t dev)
+{
+	/*
+	 * Check if pci device should be skipped in configuration
+	 */
+	if (dev == PCI_BDF(hose->first_busno, 0, 0)) {
+#if defined(CONFIG_PCI_CONFIG_HOST_BRIDGE) /* don't skip host bridge */
+		/*
+		 * Only skip configuration if "pciconfighost" is not set
+		 */
+		if (getenv("pciconfighost") == NULL)
+			return 1;
+#else
+		return 1;
+#endif
+	}
+
+	return 0;
+}
+
+/* Get a virtual address associated with a BAR region */
+void *pci_map_bar(pci_dev_t pdev, int bar, int flags)
+{
+	pci_addr_t pci_bus_addr;
+	u32 bar_response;
+
+	/* read BAR address */
+	pci_read_config_dword(pdev, bar, &bar_response);
+	pci_bus_addr = (pci_addr_t)(bar_response & ~0xf);
+
+	/*
+	 * Pass "0" as the length argument to pci_bus_to_virt.  The arg
+	 * isn't actualy used on any platform because u-boot assumes a static
+	 * linear mapping.  In the future, this could read the BAR size
+	 * and pass that as the size if needed.
+	 */
+	return pci_bus_to_virt(pdev, pci_bus_addr, flags, 0, MAP_NOCACHE);
+}
+
+void pci_write_bar32(struct pci_controller *hose, pci_dev_t dev, int barnum,
+		     u32 addr_and_ctrl)
+{
+	int bar;
+
+	bar = PCI_BASE_ADDRESS_0 + barnum * 4;
+	pci_hose_write_config_dword(hose, dev, bar, addr_and_ctrl);
+}
+
+u32 pci_read_bar32(struct pci_controller *hose, pci_dev_t dev, int barnum)
+{
+	u32 addr;
+	int bar;
+
+	bar = PCI_BASE_ADDRESS_0 + barnum * 4;
+	pci_hose_read_config_dword(hose, dev, bar, &addr);
+	if (addr & PCI_BASE_ADDRESS_SPACE_IO)
+		return addr & PCI_BASE_ADDRESS_IO_MASK;
+	else
+		return addr & PCI_BASE_ADDRESS_MEM_MASK;
+}
+
+int __pci_hose_bus_to_phys(struct pci_controller *hose,
+				pci_addr_t bus_addr,
+				unsigned long flags,
+				unsigned long skip_mask,
+				phys_addr_t *pa)
+{
+	struct pci_region *res;
+	int i;
+
+	for (i = 0; i < hose->region_count; i++) {
+		res = &hose->regions[i];
+
+		if (((res->flags ^ flags) & PCI_REGION_TYPE) != 0)
+			continue;
+
+		if (res->flags & skip_mask)
+			continue;
+
+		if (bus_addr >= res->bus_start &&
+		    (bus_addr - res->bus_start) < res->size) {
+			*pa = (bus_addr - res->bus_start + res->phys_start);
+			return 0;
+		}
+	}
+
+	return 1;
+}
+
+phys_addr_t pci_hose_bus_to_phys(struct pci_controller *hose,
+				 pci_addr_t bus_addr,
+				 unsigned long flags)
+{
+	phys_addr_t phys_addr = 0;
+	int ret;
+
+	if (!hose) {
+		puts("pci_hose_bus_to_phys: invalid hose\n");
+		return phys_addr;
+	}
+
+	/*
+	 * if PCI_REGION_MEM is set we do a two pass search with preference
+	 * on matches that don't have PCI_REGION_SYS_MEMORY set
+	 */
+	if ((flags & PCI_REGION_MEM) == PCI_REGION_MEM) {
+		ret = __pci_hose_bus_to_phys(hose, bus_addr,
+				flags, PCI_REGION_SYS_MEMORY, &phys_addr);
+		if (!ret)
+			return phys_addr;
+	}
+
+	ret = __pci_hose_bus_to_phys(hose, bus_addr, flags, 0, &phys_addr);
+
+	if (ret)
+		puts("pci_hose_bus_to_phys: invalid physical address\n");
+
+	return phys_addr;
+}
+
+pci_dev_t pci_find_device(unsigned int vendor, unsigned int device, int index)
+{
+	struct pci_device_id ids[2] = { {}, {0, 0} };
+
+	ids[0].vendor = vendor;
+	ids[0].device = device;
+
+	return pci_find_devices(ids, index);
+}
+
+pci_dev_t pci_hose_find_devices(struct pci_controller *hose, int busnum,
+				struct pci_device_id *ids, int *indexp)
+{
+	int found_multi = 0;
+	u16 vendor, device;
+	u8 header_type;
+	pci_dev_t bdf;
+	int i;
+
+	for (bdf = PCI_BDF(busnum, 0, 0);
+	     bdf < PCI_BDF(busnum + 1, 0, 0);
+	     bdf += PCI_BDF(0, 0, 1)) {
+		if (pci_skip_dev(hose, bdf))
+			continue;
+
+		if (!PCI_FUNC(bdf)) {
+			pci_read_config_byte(bdf, PCI_HEADER_TYPE,
+					     &header_type);
+			found_multi = header_type & 0x80;
+		} else {
+			if (!found_multi)
+				continue;
+		}
+
+		pci_read_config_word(bdf, PCI_VENDOR_ID, &vendor);
+		pci_read_config_word(bdf, PCI_DEVICE_ID, &device);
+
+		for (i = 0; ids[i].vendor != 0; i++) {
+			if (vendor == ids[i].vendor &&
+			    device == ids[i].device) {
+				if ((*indexp) <= 0)
+					return bdf;
+
+				(*indexp)--;
+			}
+		}
+	}
+
+	return -1;
+}
diff --git a/drivers/pci/pci_compat.c b/drivers/pci/pci_compat.c
new file mode 100644
index 0000000..d6938c1
--- /dev/null
+++ b/drivers/pci/pci_compat.c
@@ -0,0 +1,43 @@
+/*
+ * Compatibility functions for pre-driver-model code
+ *
+ * Copyright (C) 2014 Google, Inc
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+#define DEBUG
+#include <common.h>
+#include <dm.h>
+#include <errno.h>
+#include <malloc.h>
+#include <pci.h>
+#include <dm/device-internal.h>
+#include <dm/lists.h>
+
+#define PCI_HOSE_OP(rw, name, size, type)				\
+int pci_hose_##rw##_config_##name(struct pci_controller *hose,		\
+				  pci_dev_t dev,			\
+				  int offset, type value)		\
+{									\
+	return pci_##rw##_config##size(dev, offset, value);		\
+}
+
+PCI_HOSE_OP(read, byte, 8, u8 *)
+PCI_HOSE_OP(read, word, 16, u16 *)
+PCI_HOSE_OP(read, dword, 32, u32 *)
+PCI_HOSE_OP(write, byte, 8, u8)
+PCI_HOSE_OP(write, word, 16, u16)
+PCI_HOSE_OP(write, dword, 32, u32)
+
+pci_dev_t pci_find_devices(struct pci_device_id *ids, int index)
+{
+	struct pci_child_platdata *pplat;
+	struct udevice *bus, *dev;
+
+	if (pci_find_device_id(ids, index, &dev))
+		return -1;
+	bus = dev->parent;
+	pplat = dev_get_parent_platdata(dev);
+
+	return PCI_ADD_BUS(bus->seq, pplat->devfn);
+}
diff --git a/drivers/pci/pci_sandbox.c b/drivers/pci/pci_sandbox.c
new file mode 100644
index 0000000..6de5130
--- /dev/null
+++ b/drivers/pci/pci_sandbox.c
@@ -0,0 +1,79 @@
+/*
+ * Copyright (c) 2014 Google, Inc
+ * Written by Simon Glass <sjg@chromium.org>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <dm.h>
+#include <fdtdec.h>
+#include <inttypes.h>
+#include <pci.h>
+#include <dm/root.h>
+
+DECLARE_GLOBAL_DATA_PTR;
+
+static int sandbox_pci_write_config(struct udevice *bus, pci_dev_t devfn,
+				    uint offset, ulong value,
+				    enum pci_size_t size)
+{
+	struct dm_pci_emul_ops *ops;
+	struct udevice *emul;
+	int ret;
+
+	ret = sandbox_pci_get_emul(bus, devfn, &emul);
+	if (ret)
+		return ret == -ENODEV ? 0 : ret;
+	ops = pci_get_emul_ops(emul);
+	if (!ops || !ops->write_config)
+		return -ENOSYS;
+
+	return ops->write_config(emul, offset, value, size);
+}
+
+static int sandbox_pci_read_config(struct udevice *bus, pci_dev_t devfn,
+				   uint offset, ulong *valuep,
+				   enum pci_size_t size)
+{
+	struct dm_pci_emul_ops *ops;
+	struct udevice *emul;
+	int ret;
+
+	/* Prepare the default response */
+	*valuep = pci_get_ff(size);
+	ret = sandbox_pci_get_emul(bus, devfn, &emul);
+	if (ret)
+		return ret == -ENODEV ? 0 : ret;
+	ops = pci_get_emul_ops(emul);
+	if (!ops || !ops->read_config)
+		return -ENOSYS;
+
+	return ops->read_config(emul, offset, valuep, size);
+}
+
+static int sandbox_pci_child_post_bind(struct udevice *dev)
+{
+	/* Attach an emulator if we can */
+	return dm_scan_fdt_node(dev, gd->fdt_blob, dev->of_offset, false);
+}
+
+static const struct dm_pci_ops sandbox_pci_ops = {
+	.read_config = sandbox_pci_read_config,
+	.write_config = sandbox_pci_write_config,
+};
+
+static const struct udevice_id sandbox_pci_ids[] = {
+	{ .compatible = "sandbox,pci" },
+	{ }
+};
+
+U_BOOT_DRIVER(pci_sandbox) = {
+	.name	= "pci_sandbox",
+	.id	= UCLASS_PCI,
+	.of_match = sandbox_pci_ids,
+	.ops	= &sandbox_pci_ops,
+	.child_post_bind = sandbox_pci_child_post_bind,
+	.per_child_platdata_auto_alloc_size =
+			sizeof(struct pci_child_platdata),
+};
diff --git a/drivers/pci/pci_x86.c b/drivers/pci/pci_x86.c
new file mode 100644
index 0000000..901bdca
--- /dev/null
+++ b/drivers/pci/pci_x86.c
@@ -0,0 +1,24 @@
+/*
+ * Copyright (c) 2015 Google, Inc
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <dm.h>
+#include <pci.h>
+
+static const struct dm_pci_ops x86_pci_ops = {
+};
+
+static const struct udevice_id x86_pci_ids[] = {
+	{ .compatible = "x86,pci" },
+	{ }
+};
+
+U_BOOT_DRIVER(pci_x86) = {
+	.name	= "pci_x86",
+	.id	= UCLASS_PCI,
+	.of_match = x86_pci_ids,
+	.ops	= &x86_pci_ops,
+};
diff --git a/drivers/pci/pcie_layerscape.c b/drivers/pci/pcie_layerscape.c
index bcad8f2..402c519 100644
--- a/drivers/pci/pcie_layerscape.c
+++ b/drivers/pci/pcie_layerscape.c
@@ -11,7 +11,6 @@
 #include <asm/io.h>
 #include <errno.h>
 #include <malloc.h>
-#include <asm/pcie_layerscape.h>
 
 #ifndef CONFIG_SYS_PCI_MEMORY_BUS
 #define CONFIG_SYS_PCI_MEMORY_BUS CONFIG_SYS_SDRAM_BASE
@@ -50,11 +49,20 @@
 #define PCIE_ATU_FUNC(x)		(((x) & 0x7) << 16)
 #define PCIE_ATU_UPPER_TARGET		0x91C
 
+/* LUT registers */
+#define PCIE_LUT_BASE		0x80000
+#define PCIE_LUT_DBG		0x7FC
+
+#define PCIE_DBI_RO_WR_EN	0x8bc
+
 #define PCIE_LINK_CAP		0x7c
 #define PCIE_LINK_SPEED_MASK	0xf
 #define PCIE_LINK_STA		0x82
 
-#define PCIE_DBI_SIZE		(4 * 1024) /* 4K */
+#define LTSSM_STATE_MASK	0x3f
+#define LTSSM_PCIE_L0		0x11 /* L0 state */
+
+#define PCIE_DBI_SIZE		0x100000 /* 1M */
 
 struct ls_pcie {
 	int idx;
@@ -104,8 +112,6 @@ struct ls_pcie_info {
 
 /* PEX1/2 Misc Ports Status Register */
 #define LTSSM_STATE_SHIFT	20
-#define LTSSM_STATE_MASK	0x3f
-#define LTSSM_PCIE_L0		0x11 /* L0 state */
 
 static int ls_pcie_link_state(struct ls_pcie *pcie)
 {
@@ -122,18 +128,18 @@ static int ls_pcie_link_state(struct ls_pcie *pcie)
 	return 1;
 }
 #else
-#define PCIE_LDBG 0x7FC
-
 static int ls_pcie_link_state(struct ls_pcie *pcie)
 {
 	u32 state;
 
-	state = readl(pcie->dbi + PCIE_LDBG);
-	if (state)
-		return 1;
+	state = readl(pcie->dbi + PCIE_LUT_BASE + PCIE_LUT_DBG) &
+		LTSSM_STATE_MASK;
+	if (state < LTSSM_PCIE_L0) {
+		debug("....PCIe link error. LTSSM=0x%02x.\n", state);
+		return 0;
+	}
 
-	debug("....PCIe link error.\n");
-	return 0;
+	return 1;
 }
 #endif
 
@@ -149,7 +155,11 @@ static int ls_pcie_link_up(struct ls_pcie *pcie)
 	/* Try to download speed to gen1 */
 	cap = readl(pcie->dbi + PCIE_LINK_CAP);
 	writel((cap & (~PCIE_LINK_SPEED_MASK)) | 1, pcie->dbi + PCIE_LINK_CAP);
-	udelay(2000);
+	/*
+	 * Notice: the following delay has critical impact on link training
+	 * if too short (<30ms) the link doesn't get up.
+	 */
+	mdelay(100);
 	state = ls_pcie_link_state(pcie);
 	if (state)
 		return state;
@@ -251,6 +261,10 @@ static int ls_pcie_addr_valid(struct pci_controller *hose, pci_dev_t d)
 	if (PCI_DEV(d) > 0)
 		return -EINVAL;
 
+	/* Controller does not support multi-function in RC mode */
+	if ((PCI_BUS(d) == hose->first_busno) && (PCI_FUNC(d) > 0))
+		return -EINVAL;
+
 	return 0;
 }
 
@@ -327,8 +341,12 @@ static void ls_pcie_setup_ctrl(struct ls_pcie *pcie,
 	pci_hose_write_config_dword(hose, dev, PCI_BASE_ADDRESS_0, 0);
 
 	/* program correct class for RC */
+	writel(1, pcie->dbi + PCIE_DBI_RO_WR_EN);
 	pci_hose_write_config_word(hose, dev, PCI_CLASS_DEVICE,
 				   PCI_CLASS_BRIDGE_PCI);
+#ifndef CONFIG_LS102XA
+	writel(0, pcie->dbi + PCIE_DBI_RO_WR_EN);
+#endif
 }
 
 int ls_pcie_init_ctrl(int busno, enum srds_prtcl dev, struct ls_pcie_info *info)
@@ -417,9 +435,9 @@ int ls_pcie_init_ctrl(int busno, enum srds_prtcl dev, struct ls_pcie_info *info)
 	}
 
 	/* Print the negotiated PCIe link width */
-	pci_hose_read_config_word(hose, dev, PCIE_LINK_STA, &temp16);
-		printf("x%d gen%d, regs @ 0x%lx\n", (temp16 & 0x3f0) >> 4,
-		       (temp16 & 0xf), info->regs);
+	pci_hose_read_config_word(hose, pdev, PCIE_LINK_STA, &temp16);
+	printf("x%d gen%d, regs @ 0x%lx\n", (temp16 & 0x3f0) >> 4,
+	       (temp16 & 0xf), info->regs);
 
 	if (ep_mode)
 		return busno;
@@ -486,7 +504,7 @@ static void ft_pcie_ls_setup(void *blob, const char *pci_compat,
 		fdt_set_node_status(blob, off, FDT_STATUS_DISABLED, 0);
 }
 
-void ft_pcie_setup(void *blob, bd_t *bd)
+void ft_pci_setup(void *blob, bd_t *bd)
 {
 	#ifdef CONFIG_PCIE1
 	ft_pcie_ls_setup(blob, FSL_PCIE_COMPAT, CONFIG_SYS_PCIE1_ADDR, PCIE1);
@@ -506,7 +524,7 @@ void ft_pcie_setup(void *blob, bd_t *bd)
 }
 
 #else
-void ft_pcie_setup(void *blob, bd_t *bd)
+void ft_pci_setup(void *blob, bd_t *bd)
 {
 }
 #endif
diff --git a/drivers/power/axp152.c b/drivers/power/axp152.c
index 27c2c4c..740a3b4 100644
--- a/drivers/power/axp152.c
+++ b/drivers/power/axp152.c
@@ -8,17 +8,6 @@
 #include <i2c.h>
 #include <axp152.h>
 
-enum axp152_reg {
-	AXP152_CHIP_VERSION = 0x3,
-	AXP152_DCDC2_VOLTAGE = 0x23,
-	AXP152_DCDC3_VOLTAGE = 0x27,
-	AXP152_DCDC4_VOLTAGE = 0x2B,
-	AXP152_LDO2_VOLTAGE = 0x2A,
-	AXP152_SHUTDOWN = 0x32,
-};
-
-#define AXP152_POWEROFF			(1 << 7)
-
 static int axp152_write(enum axp152_reg reg, u8 val)
 {
 	return i2c_write(0x30, reg, 1, &val, 1);
diff --git a/drivers/power/axp209.c b/drivers/power/axp209.c
index f8c9b77..1d7be49 100644
--- a/drivers/power/axp209.c
+++ b/drivers/power/axp209.c
@@ -7,45 +7,9 @@
 
 #include <common.h>
 #include <i2c.h>
+#include <asm/arch/gpio.h>
 #include <axp209.h>
 
-enum axp209_reg {
-	AXP209_POWER_STATUS = 0x00,
-	AXP209_CHIP_VERSION = 0x03,
-	AXP209_DCDC2_VOLTAGE = 0x23,
-	AXP209_DCDC3_VOLTAGE = 0x27,
-	AXP209_LDO24_VOLTAGE = 0x28,
-	AXP209_LDO3_VOLTAGE = 0x29,
-	AXP209_IRQ_ENABLE1 = 0x40,
-	AXP209_IRQ_ENABLE2 = 0x41,
-	AXP209_IRQ_ENABLE3 = 0x42,
-	AXP209_IRQ_ENABLE4 = 0x43,
-	AXP209_IRQ_ENABLE5 = 0x44,
-	AXP209_IRQ_STATUS5 = 0x4c,
-	AXP209_SHUTDOWN = 0x32,
-	AXP209_GPIO0_CTRL = 0x90,
-	AXP209_GPIO1_CTRL = 0x92,
-	AXP209_GPIO2_CTRL = 0x93,
-	AXP209_GPIO_STATE = 0x94,
-	AXP209_GPIO3_CTRL = 0x95,
-};
-
-#define AXP209_POWER_STATUS_ON_BY_DC	(1 << 0)
-
-#define AXP209_IRQ5_PEK_UP		(1 << 6)
-#define AXP209_IRQ5_PEK_DOWN		(1 << 5)
-
-#define AXP209_POWEROFF			(1 << 7)
-
-#define AXP209_GPIO_OUTPUT_LOW		0x00 /* Drive pin low */
-#define AXP209_GPIO_OUTPUT_HIGH		0x01 /* Drive pin high */
-#define AXP209_GPIO_INPUT		0x02 /* Float pin */
-
-/* GPIO3 is different from the others */
-#define AXP209_GPIO3_OUTPUT_LOW		0x00 /* Drive pin low, Output mode */
-#define AXP209_GPIO3_OUTPUT_HIGH	0x02 /* Float pin, Output mode */
-#define AXP209_GPIO3_INPUT		0x06 /* Float pin, Input mode */
-
 static int axp209_write(enum axp209_reg reg, u8 val)
 {
 	return i2c_write(0x34, reg, 1, &val, 1);
@@ -205,6 +169,9 @@ static u8 axp209_get_gpio_ctrl_reg(unsigned int pin)
 
 int axp_gpio_direction_input(unsigned int pin)
 {
+	if (pin == SUNXI_GPIO_AXP0_VBUS_DETECT)
+		return 0;
+
 	u8 reg = axp209_get_gpio_ctrl_reg(pin);
 	/* GPIO3 is "special" */
 	u8 val = (pin == 3) ? AXP209_GPIO3_INPUT : AXP209_GPIO_INPUT;
@@ -232,7 +199,10 @@ int axp_gpio_get_value(unsigned int pin)
 	u8 val, mask;
 	int rc;
 
-	if (pin == 3) {
+	if (pin == SUNXI_GPIO_AXP0_VBUS_DETECT) {
+		rc = axp209_read(AXP209_POWER_STATUS, &val);
+		mask = AXP209_POWER_STATUS_VBUS_USABLE;
+	} else if (pin == 3) {
 		rc = axp209_read(AXP209_GPIO3_CTRL, &val);
 		mask = 1;
 	} else {
diff --git a/drivers/power/axp221.c b/drivers/power/axp221.c
index c2c3988..dc3a7f1 100644
--- a/drivers/power/axp221.c
+++ b/drivers/power/axp221.c
@@ -14,6 +14,7 @@
 #include <errno.h>
 #include <asm/arch/p2wi.h>
 #include <asm/arch/rsb.h>
+#include <asm/arch/gpio.h>
 #include <axp221.h>
 
 /*
@@ -385,54 +386,66 @@ int axp221_get_sid(unsigned int *sid)
 	return 0;
 }
 
-int axp_get_vbus(void)
+int axp_gpio_direction_input(unsigned int pin)
 {
-	int ret;
-	u8 val;
-
-	ret = axp221_init();
-	if (ret)
-		return ret;
-
-	ret = pmic_bus_read(AXP221_POWER_STATUS, &val);
-	if (ret)
-		return ret;
-
-	return (val & AXP221_POWER_STATUS_VBUS_USABLE) ? 1 : 0;
+	switch (pin) {
+	case SUNXI_GPIO_AXP0_VBUS_DETECT:
+		return 0;
+	default:
+		return -EINVAL;
+	}
 }
 
-static int axp_drivebus_setup(void)
+int axp_gpio_direction_output(unsigned int pin, unsigned int val)
 {
 	int ret;
 
-	ret = axp221_init();
-	if (ret)
-		return ret;
+	switch (pin) {
+	case SUNXI_GPIO_AXP0_VBUS_ENABLE:
+		ret = axp221_clrbits(AXP221_MISC_CTRL,
+				     AXP221_MISC_CTRL_N_VBUSEN_FUNC);
+		if (ret)
+			return ret;
 
-	/* Set N_VBUSEN pin to output / DRIVEBUS function */
-	return axp221_clrbits(AXP221_MISC_CTRL, AXP221_MISC_CTRL_N_VBUSEN_FUNC);
+		return axp_gpio_set_value(pin, val);
+	default:
+		return -EINVAL;
+	}
 }
 
-int axp_drivebus_enable(void)
+int axp_gpio_get_value(unsigned int pin)
 {
 	int ret;
+	u8 val;
 
-	ret = axp_drivebus_setup();
-	if (ret)
-		return ret;
+	switch (pin) {
+	case SUNXI_GPIO_AXP0_VBUS_DETECT:
+		ret = pmic_bus_read(AXP221_POWER_STATUS, &val);
+		if (ret)
+			return ret;
 
-	/* Set DRIVEBUS high */
-	return axp221_setbits(AXP221_VBUS_IPSOUT, AXP221_VBUS_IPSOUT_DRIVEBUS);
+		return !!(val & AXP221_POWER_STATUS_VBUS_AVAIL);
+	default:
+		return -EINVAL;
+	}
 }
 
-int axp_drivebus_disable(void)
+int axp_gpio_set_value(unsigned int pin, unsigned int val)
 {
 	int ret;
 
-	ret = axp_drivebus_setup();
-	if (ret)
-		return ret;
+	switch (pin) {
+	case SUNXI_GPIO_AXP0_VBUS_ENABLE:
+		if (val)
+			ret = axp221_setbits(AXP221_VBUS_IPSOUT,
+					     AXP221_VBUS_IPSOUT_DRIVEBUS);
+		else
+			ret = axp221_clrbits(AXP221_VBUS_IPSOUT,
+					     AXP221_VBUS_IPSOUT_DRIVEBUS);
 
-	/* Set DRIVEBUS low */
-	return axp221_clrbits(AXP221_VBUS_IPSOUT, AXP221_VBUS_IPSOUT_DRIVEBUS);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
 }
diff --git a/drivers/qe/qe.c b/drivers/qe/qe.c
index d24651b..84e1433 100644
--- a/drivers/qe/qe.c
+++ b/drivers/qe/qe.c
@@ -196,6 +196,18 @@ void u_qe_init(void)
 }
 #endif
 
+#ifdef CONFIG_U_QE
+void u_qe_resume(void)
+{
+	qe_map_t *qe_immrr;
+	uint qe_base = CONFIG_SYS_IMMR + QE_IMMR_OFFSET; /* QE immr base */
+	qe_immrr = (qe_map_t *)qe_base;
+
+	u_qe_firmware_resume((const void *)CONFIG_SYS_QE_FW_ADDR, qe_immrr);
+	out_be32(&qe_immrr->iram.iready, QE_IRAM_READY);
+}
+#endif
+
 void qe_reset(void)
 {
 	qe_issue_cmd(QE_RESET, QE_CR_SUBBLOCK_INVALID,
@@ -580,6 +592,76 @@ int u_qe_upload_firmware(const struct qe_firmware *firmware)
 }
 #endif
 
+#ifdef CONFIG_U_QE
+int u_qe_firmware_resume(const struct qe_firmware *firmware, qe_map_t *qe_immrr)
+{
+	unsigned int i;
+	unsigned int j;
+	const struct qe_header *hdr;
+	const u32 *code;
+#ifdef CONFIG_DEEP_SLEEP
+#ifdef CONFIG_PPC
+	ccsr_gur_t __iomem *gur = (void *)(CONFIG_SYS_MPC85xx_GUTS_ADDR);
+#else
+	struct ccsr_gur __iomem *gur = (void *)CONFIG_SYS_FSL_GUTS_ADDR;
+#endif
+#endif
+
+	if (!firmware)
+		return -EINVAL;
+
+	hdr = &firmware->header;
+
+	/* Check the magic */
+	if ((hdr->magic[0] != 'Q') || (hdr->magic[1] != 'E') ||
+	    (hdr->magic[2] != 'F')) {
+#ifdef CONFIG_DEEP_SLEEP
+		setbits_be32(&gur->devdisr, MPC85xx_DEVDISR_QE_DISABLE);
+#endif
+		return -EPERM;
+	}
+
+	/*
+	 * If the microcode calls for it, split the I-RAM.
+	 */
+	if (!firmware->split) {
+		out_be16(&qe_immrr->cp.cercr,
+			 in_be16(&qe_immrr->cp.cercr) | QE_CP_CERCR_CIR);
+	}
+
+	/* Loop through each microcode. */
+	for (i = 0; i < firmware->count; i++) {
+		const struct qe_microcode *ucode = &firmware->microcode[i];
+
+		/* Upload a microcode if it's present */
+		if (!ucode->code_offset)
+			return 0;
+
+		code = (const void *)firmware + be32_to_cpu(ucode->code_offset);
+
+		/* Use auto-increment */
+		out_be32(&qe_immrr->iram.iadd, be32_to_cpu(ucode->iram_offset) |
+			QE_IRAM_IADD_AIE | QE_IRAM_IADD_BADDR);
+
+		for (i = 0; i < be32_to_cpu(ucode->count); i++)
+			out_be32(&qe_immrr->iram.idata, be32_to_cpu(code[i]));
+
+		/* Program the traps for this processor */
+		for (j = 0; j < 16; j++) {
+			u32 trap = be32_to_cpu(ucode->traps[j]);
+
+			if (trap)
+				out_be32(&qe_immrr->rsp[i].tibcr[j], trap);
+		}
+
+		/* Enable traps */
+		out_be32(&qe_immrr->rsp[i].eccr, be32_to_cpu(ucode->eccr));
+	}
+
+	return 0;
+}
+#endif
+
 struct qe_firmware_info *qe_get_firmware_info(void)
 {
 	return qe_firmware_uploaded ? &qe_firmware_info : NULL;
diff --git a/drivers/qe/qe.h b/drivers/qe/qe.h
index 33878f8..77b18e9 100644
--- a/drivers/qe/qe.h
+++ b/drivers/qe/qe.h
@@ -11,6 +11,9 @@
 #define __QE_H__
 
 #include "common.h"
+#ifdef CONFIG_U_QE
+#include <linux/immap_qe.h>
+#endif
 
 #define QE_NUM_OF_BRGS	16
 #define UCC_MAX_NUM	8
@@ -288,6 +291,9 @@ void qe_reset(void);
 #ifdef CONFIG_U_QE
 void u_qe_init(void);
 int u_qe_upload_firmware(const struct qe_firmware *firmware);
+void u_qe_resume(void);
+int u_qe_firmware_resume(const struct qe_firmware *firmware,
+			 qe_map_t *qe_immrr);
 #endif
 
 #endif /* __QE_H__ */
diff --git a/drivers/qe/uec.c b/drivers/qe/uec.c
index c91f084..e0ab04a 100644
--- a/drivers/qe/uec.c
+++ b/drivers/qe/uec.c
@@ -1333,7 +1333,7 @@ static int uec_recv(struct eth_device* dev)
 		if (!(status & RxBD_ERROR)) {
 			data = BD_DATA(bd);
 			len = BD_LENGTH(bd);
-			NetReceive(data, len);
+			net_process_received_packet(data, len);
 		} else {
 			printf("%s: Rx error\n", dev->name);
 		}
diff --git a/drivers/serial/Kconfig b/drivers/serial/Kconfig
index 1686a1f..54e6f26 100644
--- a/drivers/serial/Kconfig
+++ b/drivers/serial/Kconfig
@@ -66,6 +66,16 @@ config DEBUG_UART_CLOCK
 	  A default should be provided by your board, but if not you will need
 	  to use the correct value here.
 
+config DEBUG_UART_SHIFT
+	int "UART register shift"
+	depends on DEBUG_UART
+	default 0 if DEBUG_UART
+	help
+	  Some UARTs (notably ns16550) support different register layouts
+	  where the registers are spaced either as bytes, words or some other
+	  value. Use this value to specify the shift to use, where 0=byte
+	  registers, 2=32-bit word registers, etc.
+
 config UNIPHIER_SERIAL
 	bool "UniPhier on-chip UART support"
 	depends on ARCH_UNIPHIER && DM_SERIAL
diff --git a/drivers/serial/Makefile b/drivers/serial/Makefile
index b385852..d183eed 100644
--- a/drivers/serial/Makefile
+++ b/drivers/serial/Makefile
@@ -44,6 +44,7 @@ obj-$(CONFIG_TEGRA_SERIAL) += serial_tegra.o
 obj-$(CONFIG_UNIPHIER_SERIAL) += serial_uniphier.o
 obj-$(CONFIG_OMAP_SERIAL) += serial_omap.o
 obj-$(CONFIG_X86_SERIAL) += serial_x86.o
+obj-$(CONFIG_STM32_SERIAL) += serial_stm32.o
 
 ifndef CONFIG_SPL_BUILD
 obj-$(CONFIG_USB_TTY) += usbtty.o
diff --git a/drivers/serial/ns16550.c b/drivers/serial/ns16550.c
index 03beab5..3d376d7 100644
--- a/drivers/serial/ns16550.c
+++ b/drivers/serial/ns16550.c
@@ -8,6 +8,7 @@
 #include <dm.h>
 #include <errno.h>
 #include <fdtdec.h>
+#include <mapmem.h>
 #include <ns16550.h>
 #include <serial.h>
 #include <watchdog.h>
@@ -56,7 +57,7 @@ DECLARE_GLOBAL_DATA_PTR;
 
 #ifdef CONFIG_DM_SERIAL
 
-static inline void serial_out_shift(unsigned char *addr, int shift, int value)
+static inline void serial_out_shift(void *addr, int shift, int value)
 {
 #ifdef CONFIG_SYS_NS16550_PORT_MAPPED
 	outb(value, (ulong)addr);
@@ -71,7 +72,7 @@ static inline void serial_out_shift(unsigned char *addr, int shift, int value)
 #endif
 }
 
-static inline int serial_in_shift(unsigned char *addr, int shift)
+static inline int serial_in_shift(void *addr, int shift)
 {
 #ifdef CONFIG_SYS_NS16550_PORT_MAPPED
 	return inb((ulong)addr);
@@ -113,9 +114,11 @@ static int ns16550_readb(NS16550_t port, int offset)
 
 /* We can clean these up once everything is moved to driver model */
 #define serial_out(value, addr)	\
-	ns16550_writeb(com_port, addr - (unsigned char *)com_port, value)
+	ns16550_writeb(com_port, \
+		(unsigned char *)addr - (unsigned char *)com_port, value)
 #define serial_in(addr) \
-	ns16550_readb(com_port, addr - (unsigned char *)com_port)
+	ns16550_readb(com_port, \
+		(unsigned char *)addr - (unsigned char *)com_port)
 #endif
 
 static inline int calc_divisor(NS16550_t port, int clock, int baudrate)
@@ -172,7 +175,6 @@ void NS16550_init(NS16550_t com_port, int baud_divisor)
 			defined(CONFIG_TI81XX) || defined(CONFIG_AM43XX)
 	serial_out(0x7, &com_port->mdr1);	/* mode select reset TL16C750*/
 #endif
-	NS16550_setbrg(com_port, 0);
 	serial_out(UART_MCRVAL, &com_port->mcr);
 	serial_out(UART_FCRVAL, &com_port->fcr);
 	if (baud_divisor != -1)
@@ -253,15 +255,19 @@ void debug_uart_init(void)
 	 */
 	baud_divisor = calc_divisor(com_port, CONFIG_DEBUG_UART_CLOCK,
 				    CONFIG_BAUDRATE);
-
-	serial_out_shift(&com_port->ier, 0, CONFIG_SYS_NS16550_IER);
-	serial_out_shift(&com_port->mcr, 0, UART_MCRVAL);
-	serial_out_shift(&com_port->fcr, 0, UART_FCRVAL);
-
-	serial_out_shift(&com_port->lcr, 0, UART_LCR_BKSE | UART_LCRVAL);
-	serial_out_shift(&com_port->dll, 0, baud_divisor & 0xff);
-	serial_out_shift(&com_port->dlm, 0, (baud_divisor >> 8) & 0xff);
-	serial_out_shift(&com_port->lcr, 0, UART_LCRVAL);
+	serial_out_shift(&com_port->ier, CONFIG_DEBUG_UART_SHIFT,
+			 CONFIG_SYS_NS16550_IER);
+	serial_out_shift(&com_port->mcr, CONFIG_DEBUG_UART_SHIFT, UART_MCRVAL);
+	serial_out_shift(&com_port->fcr, CONFIG_DEBUG_UART_SHIFT, UART_FCRVAL);
+
+	serial_out_shift(&com_port->lcr, CONFIG_DEBUG_UART_SHIFT,
+			 UART_LCR_BKSE | UART_LCRVAL);
+	serial_out_shift(&com_port->dll, CONFIG_DEBUG_UART_SHIFT,
+			 baud_divisor & 0xff);
+	serial_out_shift(&com_port->dlm, CONFIG_DEBUG_UART_SHIFT,
+			 (baud_divisor >> 8) & 0xff);
+	serial_out_shift(&com_port->lcr, CONFIG_DEBUG_UART_SHIFT,
+			 UART_LCRVAL);
 }
 
 static inline void _debug_uart_putc(int ch)
@@ -270,7 +276,7 @@ static inline void _debug_uart_putc(int ch)
 
 	while (!(serial_in_shift(&com_port->lsr, 0) & UART_LSR_THRE))
 		;
-	serial_out_shift(&com_port->thr, 0, ch);
+	serial_out_shift(&com_port->thr, CONFIG_DEBUG_UART_SHIFT, ch);
 }
 
 DEBUG_UART_FUNCS
diff --git a/drivers/serial/serial-uclass.c b/drivers/serial/serial-uclass.c
index 2de3737..b8c2f48 100644
--- a/drivers/serial/serial-uclass.c
+++ b/drivers/serial/serial-uclass.c
@@ -70,7 +70,7 @@ static void serial_find_console_or_panic(void)
 	if (uclass_get_device_by_seq(UCLASS_SERIAL, INDEX, &dev) &&
 	    uclass_get_device(UCLASS_SERIAL, INDEX, &dev) &&
 	    (uclass_first_device(UCLASS_SERIAL, &dev) || !dev))
-		panic("No serial driver found");
+		panic_str("No serial driver found");
 #undef INDEX
 	gd->cur_serial_dev = dev;
 }
@@ -251,7 +251,7 @@ static int serial_post_probe(struct udevice *dev)
 {
 	struct dm_serial_ops *ops = serial_get_ops(dev);
 #ifdef CONFIG_DM_STDIO
-	struct serial_dev_priv *upriv = dev->uclass_priv;
+	struct serial_dev_priv *upriv = dev_get_uclass_priv(dev);
 	struct stdio_dev sdev;
 #endif
 	int ret;
@@ -299,7 +299,7 @@ static int serial_post_probe(struct udevice *dev)
 static int serial_pre_remove(struct udevice *dev)
 {
 #ifdef CONFIG_SYS_STDIO_DEREGISTER
-	struct serial_dev_priv *upriv = dev->uclass_priv;
+	struct serial_dev_priv *upriv = dev_get_uclass_priv(dev);
 
 	if (stdio_deregister_dev(upriv->sdev, 0))
 		return -EPERM;
diff --git a/drivers/serial/serial.c b/drivers/serial/serial.c
index 9f78492..699c410 100644
--- a/drivers/serial/serial.c
+++ b/drivers/serial/serial.c
@@ -154,6 +154,7 @@ serial_initfunc(sa1100_serial_initialize);
 serial_initfunc(sandbox_serial_initialize);
 serial_initfunc(sconsole_serial_initialize);
 serial_initfunc(sh_serial_initialize);
+serial_initfunc(stm32_serial_initialize);
 serial_initfunc(uartlite_serial_initialize);
 serial_initfunc(zynq_serial_initialize);
 
@@ -246,6 +247,7 @@ void serial_initialize(void)
 	sandbox_serial_initialize();
 	sconsole_serial_initialize();
 	sh_serial_initialize();
+	stm32_serial_initialize();
 	uartlite_serial_initialize();
 	zynq_serial_initialize();
 
diff --git a/drivers/serial/serial_pl01x.c b/drivers/serial/serial_pl01x.c
index 75eb6bd..2124161 100644
--- a/drivers/serial/serial_pl01x.c
+++ b/drivers/serial/serial_pl01x.c
@@ -95,7 +95,7 @@ static int pl01x_generic_serial_init(struct pl01x_regs *regs,
 	return 0;
 }
 
-static int set_line_control(struct pl01x_regs *regs)
+static int pl011_set_line_control(struct pl01x_regs *regs)
 {
 	unsigned int lcr;
 	/*
@@ -129,6 +129,9 @@ static int pl01x_generic_setbrg(struct pl01x_regs *regs, enum pl01x_type type,
 	case TYPE_PL010: {
 		unsigned int divisor;
 
+		/* disable everything */
+		writel(0, &regs->pl010_cr);
+
 		switch (baudrate) {
 		case 9600:
 			divisor = UART_PL010_BAUD_9600;
@@ -152,6 +155,12 @@ static int pl01x_generic_setbrg(struct pl01x_regs *regs, enum pl01x_type type,
 		writel((divisor & 0xf00) >> 8, &regs->pl010_lcrm);
 		writel(divisor & 0xff, &regs->pl010_lcrl);
 
+		/*
+		 * Set line control for the PL010 to be 8 bits, 1 stop bit,
+		 * no parity, fifo enabled
+		 */
+		writel(UART_PL010_LCRH_WLEN_8 | UART_PL010_LCRH_FEN,
+		       &regs->pl010_lcrh);
 		/* Finally, enable the UART */
 		writel(UART_PL010_CR_UARTEN, &regs->pl010_cr);
 		break;
@@ -178,7 +187,7 @@ static int pl01x_generic_setbrg(struct pl01x_regs *regs, enum pl01x_type type,
 		writel(divider, &regs->pl011_ibrd);
 		writel(fraction, &regs->pl011_fbrd);
 
-		set_line_control(regs);
+		pl011_set_line_control(regs);
 		/* Finally, enable the UART */
 		writel(UART_PL011_CR_UARTEN | UART_PL011_CR_TXE |
 		       UART_PL011_CR_RXE | UART_PL011_CR_RTS, &regs->pl011_cr);
diff --git a/drivers/serial/serial_stm32.c b/drivers/serial/serial_stm32.c
new file mode 100644
index 0000000..3c80096
--- /dev/null
+++ b/drivers/serial/serial_stm32.c
@@ -0,0 +1,117 @@
+/*
+ * (C) Copyright 2015
+ * Kamil Lulko, <rev13@wp.pl>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <asm/io.h>
+#include <serial.h>
+#include <asm/arch/stm32.h>
+
+#define STM32_USART1_BASE	(STM32_APB2PERIPH_BASE + 0x1000)
+#define RCC_APB2ENR_USART1EN	(1 << 4)
+
+#define USART_BASE		STM32_USART1_BASE
+#define RCC_USART_ENABLE	RCC_APB2ENR_USART1EN
+
+struct stm32_serial {
+	u32 sr;
+	u32 dr;
+	u32 brr;
+	u32 cr1;
+	u32 cr2;
+	u32 cr3;
+	u32 gtpr;
+};
+
+#define USART_CR1_RE		(1 << 2)
+#define USART_CR1_TE		(1 << 3)
+#define USART_CR1_UE		(1 << 13)
+
+#define USART_SR_FLAG_RXNE	(1 << 5)
+#define USART_SR_FLAG_TXE	(1 << 7)
+
+#define USART_BRR_F_MASK	0xF
+#define USART_BRR_M_SHIFT	4
+#define USART_BRR_M_MASK	0xFFF0
+
+DECLARE_GLOBAL_DATA_PTR;
+
+static void stm32_serial_setbrg(void)
+{
+	serial_init();
+}
+
+static int stm32_serial_init(void)
+{
+	struct stm32_serial *usart = (struct stm32_serial *)USART_BASE;
+	u32 clock, int_div, frac_div, tmp;
+
+	if ((USART_BASE & STM32_BUS_MASK) == STM32_APB1PERIPH_BASE) {
+		setbits_le32(&STM32_RCC->apb1enr, RCC_USART_ENABLE);
+		clock = clock_get(CLOCK_APB1);
+	} else if ((USART_BASE & STM32_BUS_MASK) == STM32_APB2PERIPH_BASE) {
+		setbits_le32(&STM32_RCC->apb2enr, RCC_USART_ENABLE);
+		clock = clock_get(CLOCK_APB2);
+	} else {
+		return -1;
+	}
+
+	int_div = (25 * clock) / (4 * gd->baudrate);
+	tmp = ((int_div / 100) << USART_BRR_M_SHIFT) & USART_BRR_M_MASK;
+	frac_div = int_div - (100 * (tmp >> USART_BRR_M_SHIFT));
+	tmp |= (((frac_div * 16) + 50) / 100) & USART_BRR_F_MASK;
+
+	writel(tmp, &usart->brr);
+	setbits_le32(&usart->cr1, USART_CR1_RE | USART_CR1_TE | USART_CR1_UE);
+
+	return 0;
+}
+
+static int stm32_serial_getc(void)
+{
+	struct stm32_serial *usart = (struct stm32_serial *)USART_BASE;
+	while ((readl(&usart->sr) & USART_SR_FLAG_RXNE) == 0)
+		;
+	return readl(&usart->dr);
+}
+
+static void stm32_serial_putc(const char c)
+{
+	struct stm32_serial *usart = (struct stm32_serial *)USART_BASE;
+	while ((readl(&usart->sr) & USART_SR_FLAG_TXE) == 0)
+		;
+	writel(c, &usart->dr);
+}
+
+static int stm32_serial_tstc(void)
+{
+	struct stm32_serial *usart = (struct stm32_serial *)USART_BASE;
+	u8 ret;
+
+	ret = readl(&usart->sr) & USART_SR_FLAG_RXNE;
+	return ret;
+}
+
+static struct serial_device stm32_serial_drv = {
+	.name	= "stm32_serial",
+	.start	= stm32_serial_init,
+	.stop	= NULL,
+	.setbrg	= stm32_serial_setbrg,
+	.putc	= stm32_serial_putc,
+	.puts	= default_serial_puts,
+	.getc	= stm32_serial_getc,
+	.tstc	= stm32_serial_tstc,
+};
+
+void stm32_serial_initialize(void)
+{
+	serial_register(&stm32_serial_drv);
+}
+
+__weak struct serial_device *default_serial_console(void)
+{
+	return &stm32_serial_drv;
+}
diff --git a/drivers/serial/serial_uniphier.c b/drivers/serial/serial_uniphier.c
index 98e3b81..74547eb 100644
--- a/drivers/serial/serial_uniphier.c
+++ b/drivers/serial/serial_uniphier.c
@@ -11,6 +11,7 @@
 #include <asm/errno.h>
 #include <dm/device.h>
 #include <dm/platform_data/serial-uniphier.h>
+#include <mapmem.h>
 #include <serial.h>
 #include <fdtdec.h>
 
diff --git a/drivers/serial/serial_zynq.c b/drivers/serial/serial_zynq.c
index 3e2b8dc..9278763 100644
--- a/drivers/serial/serial_zynq.c
+++ b/drivers/serial/serial_zynq.c
@@ -48,10 +48,16 @@ static void uart_zynq_serial_setbrg(const int port)
 	/* Calculation results. */
 	unsigned int calc_bauderror, bdiv, bgen;
 	unsigned long calc_baud = 0;
-	unsigned long baud = gd->baudrate;
+	unsigned long baud;
 	unsigned long clock = get_uart_clk(port);
 	struct uart_zynq *regs = uart_zynq_ports[port];
 
+	/* Covering case where input clock is so slow */
+	if (clock < 1000000 && gd->baudrate > 4800)
+		gd->baudrate = 4800;
+
+	baud = gd->baudrate;
+
 	/*                master clock
 	 * Baud rate = ------------------
 	 *              bgen * (bdiv + 1)
diff --git a/drivers/sound/Kconfig b/drivers/sound/Kconfig
index e69de29..3b96e84 100644
--- a/drivers/sound/Kconfig
+++ b/drivers/sound/Kconfig
@@ -0,0 +1,55 @@
+config SOUND
+	bool "Enable sound support"
+	help
+	  Support making sounds through an audio codec. This is normally a
+	  beep at a chosen frequency for a selected length of time. However
+	  the drivers support playing arbitrary sound samples using a
+	  PCM interface.
+
+	  Note: At present the sound setup is somewhat tangled up in that the
+	  audio codecs are called from the sound-i2s code. This could be
+	  converted to driver model.
+
+config I2S
+	bool "Enable I2S support"
+	depends on SOUND
+	help
+	  I2S is a serial bus often used to transmit audio data from the
+	  SoC to the audio codec. This option enables sound support using
+	  I2S. It calls either of the two supported codecs (no use is made
+	  of driver model at present).
+
+config I2S_SAMSUNG
+	bool "Enable I2C support for Samsung SoCs"
+	depends on SOUND
+	help
+	  Samsung Exynos SoCs support an I2S interface for sending audio
+	  data to an audio codec. This option enables support for this,
+	  using one of the available audio codec drivers. Enabling this
+	  option provides an implementation for sound_init() and
+	  sound_play().
+
+config SOUND_MAX98095
+	bool "Support Maxim max98095 audio codec"
+	depends on I2S_SAMSUNG
+	help
+	  Enable the max98095 audio codec. This is connected via I2S for
+	  audio data and I2C for codec control. At present it only works
+	  with the Samsung I2S driver.
+
+config SOUND_SANDBOX
+	bool "Support sandbox emulated audio codec"
+	depends on SANDBOX && SOUND
+	help
+	  U-Boot sandbox can emulate a sound device using SDL, playing the
+	  sound on the host machine. This option implements the sound_init()
+	  and sound_play() functions for sandbox. Note that you must install
+	  the SDL libraries for this to work.
+
+config SOUND_WM8994
+	bool "Support Wolfson Micro wm8994 audio codec"
+	depends on I2S_SAMSUNG
+	help
+	  Enable the wm8994 audio codec. This is connected via I2S for
+	  audio data and I2C for codec control. At present it only works
+	  with the Samsung I2S driver.
diff --git a/drivers/spi/Kconfig b/drivers/spi/Kconfig
index 7ae2727..357a335 100644
--- a/drivers/spi/Kconfig
+++ b/drivers/spi/Kconfig
@@ -10,3 +10,44 @@ config DM_SPI
 	  as 'parent data' to every slave on each bus. Slaves
 	  typically use driver-private data instead of extending the
 	  spi_slave structure.
+
+config SANDBOX_SPI
+	bool "Sandbox SPI driver"
+	depends on SANDBOX && DM
+	help
+	  Enable SPI support for sandbox. This is an emulation of a real SPI
+	  bus. Devices can be attached to the bus using the device tree
+	  which specifies the driver to use. As an example, see this device
+	  tree fragment from sandbox.dts. It shows that the SPI bus has a
+	  single flash device on chip select 0 which is emulated by the driver
+	  for "sandbox,spi-flash", which is in drivers/mtd/spi/sandbox.c.
+
+	  spi@0 {
+		#address-cells = <1>;
+		#size-cells = <0>;
+		reg = <0>;
+		compatible = "sandbox,spi";
+		cs-gpios = <0>, <&gpio_a 0>;
+		flash@0 {
+			reg = <0>;
+			compatible = "spansion,m25p16", "sandbox,spi-flash";
+			spi-max-frequency = <40000000>;
+			sandbox,filename = "spi.bin";
+		};
+	};
+
+config DESIGNWARE_SPI
+	bool "Designware SPI driver"
+	depends on DM_SPI
+	help
+	  Enable the Designware SPI driver. This driver can be used to
+	  access the SPI NOR flash on platforms embedding this Designware
+	  IP core.
+
+config CADENCE_QSPI
+	bool "Cadence QSPI driver"
+	depends on DM_SPI
+	help
+	  Enable the Cadence Quad-SPI (QSPI) driver. This driver can be
+	  used to access the SPI NOR flash on platforms embedding this
+	  Cadence IP core.
diff --git a/drivers/spi/Makefile b/drivers/spi/Makefile
index ce6f1cc..e288692 100644
--- a/drivers/spi/Makefile
+++ b/drivers/spi/Makefile
@@ -50,3 +50,4 @@ obj-$(CONFIG_TI_QSPI) += ti_qspi.o
 obj-$(CONFIG_XILINX_SPI) += xilinx_spi.o
 obj-$(CONFIG_ZYNQ_SPI) += zynq_spi.o
 obj-$(CONFIG_FSL_QSPI) += fsl_qspi.o
+obj-$(CONFIG_FSL_DSPI) += fsl_dspi.o
diff --git a/drivers/spi/exynos_spi.c b/drivers/spi/exynos_spi.c
index a46d8c1..67f6b2d 100644
--- a/drivers/spi/exynos_spi.c
+++ b/drivers/spi/exynos_spi.c
@@ -296,8 +296,9 @@ static int exynos_spi_probe(struct udevice *bus)
 	return 0;
 }
 
-static int exynos_spi_claim_bus(struct udevice *bus)
+static int exynos_spi_claim_bus(struct udevice *dev)
 {
+	struct udevice *bus = dev->parent;
 	struct exynos_spi_priv *priv = dev_get_priv(bus);
 
 	exynos_pinmux_config(priv->periph_id, PINMUX_FLAG_NONE);
@@ -308,8 +309,9 @@ static int exynos_spi_claim_bus(struct udevice *bus)
 	return 0;
 }
 
-static int exynos_spi_release_bus(struct udevice *bus)
+static int exynos_spi_release_bus(struct udevice *dev)
 {
+	struct udevice *bus = dev->parent;
 	struct exynos_spi_priv *priv = dev_get_priv(bus);
 
 	spi_flush_fifo(priv->regs);
diff --git a/drivers/spi/fsl_dspi.c b/drivers/spi/fsl_dspi.c
new file mode 100644
index 0000000..6476f91
--- /dev/null
+++ b/drivers/spi/fsl_dspi.c
@@ -0,0 +1,737 @@
+/*
+ * (C) Copyright 2000-2003
+ * Wolfgang Denk, DENX Software Engineering, wd@denx.de.
+ *
+ * Copyright (C) 2004-2009, 2015 Freescale Semiconductor, Inc.
+ * TsiChung Liew (Tsi-Chung.Liew@freescale.com)
+ * Chao Fu (B44548@freescale.com)
+ * Haikun Wang (B53464@freescale.com)
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+#include <dm.h>
+#include <errno.h>
+#include <common.h>
+#include <spi.h>
+#include <malloc.h>
+#include <asm/io.h>
+#include <fdtdec.h>
+#ifndef CONFIG_M68K
+#include <asm/arch/clock.h>
+#endif
+#include <fsl_dspi.h>
+
+DECLARE_GLOBAL_DATA_PTR;
+
+/* fsl_dspi_platdata flags */
+#define DSPI_FLAG_REGMAP_ENDIAN_BIG	(1 << 0)
+
+/* idle data value */
+#define DSPI_IDLE_VAL			0x0
+
+/* max chipselect signals number */
+#define FSL_DSPI_MAX_CHIPSELECT		6
+
+/* default SCK frequency, unit: HZ */
+#define FSL_DSPI_DEFAULT_SCK_FREQ	10000000
+
+/* tx/rx data wait timeout value, unit: us */
+#define DSPI_TXRX_WAIT_TIMEOUT		1000000
+
+/* CTAR register pre-configure value */
+#define DSPI_CTAR_DEFAULT_VALUE		(DSPI_CTAR_TRSZ(7) | \
+					DSPI_CTAR_PCSSCK_1CLK | \
+					DSPI_CTAR_PASC(0) | \
+					DSPI_CTAR_PDT(0) | \
+					DSPI_CTAR_CSSCK(0) | \
+					DSPI_CTAR_ASC(0) | \
+					DSPI_CTAR_DT(0))
+
+/* CTAR register pre-configure mask */
+#define DSPI_CTAR_SET_MODE_MASK		(DSPI_CTAR_TRSZ(15) | \
+					DSPI_CTAR_PCSSCK(3) | \
+					DSPI_CTAR_PASC(3) | \
+					DSPI_CTAR_PDT(3) | \
+					DSPI_CTAR_CSSCK(15) | \
+					DSPI_CTAR_ASC(15) | \
+					DSPI_CTAR_DT(15))
+
+/**
+ * struct fsl_dspi_platdata - platform data for Freescale DSPI
+ *
+ * @flags: Flags for DSPI DSPI_FLAG_...
+ * @speed_hz: Default SCK frequency
+ * @num_chipselect: Number of DSPI chipselect signals
+ * @regs_addr: Base address of DSPI registers
+ */
+struct fsl_dspi_platdata {
+	uint flags;
+	uint speed_hz;
+	uint num_chipselect;
+	fdt_addr_t regs_addr;
+};
+
+/**
+ * struct fsl_dspi_priv - private data for Freescale DSPI
+ *
+ * @flags: Flags for DSPI DSPI_FLAG_...
+ * @mode: SPI mode to use for slave device (see SPI mode flags)
+ * @mcr_val: MCR register configure value
+ * @bus_clk: DSPI input clk frequency
+ * @speed_hz: Default SCK frequency
+ * @charbit: How many bits in every transfer
+ * @num_chipselect: Number of DSPI chipselect signals
+ * @ctar_val: CTAR register configure value of per chipselect slave device
+ * @regs: Point to DSPI register structure for I/O access
+ */
+struct fsl_dspi_priv {
+	uint flags;
+	uint mode;
+	uint mcr_val;
+	uint bus_clk;
+	uint speed_hz;
+	uint charbit;
+	uint num_chipselect;
+	uint ctar_val[FSL_DSPI_MAX_CHIPSELECT];
+	struct dspi *regs;
+};
+
+#ifndef CONFIG_DM_SPI
+struct fsl_dspi {
+	struct spi_slave slave;
+	struct fsl_dspi_priv priv;
+};
+#endif
+
+__weak void cpu_dspi_port_conf(void)
+{
+}
+
+__weak int cpu_dspi_claim_bus(uint bus, uint cs)
+{
+	return 0;
+}
+
+__weak void cpu_dspi_release_bus(uint bus, uint cs)
+{
+}
+
+static uint dspi_read32(uint flags, uint *addr)
+{
+	return flags & DSPI_FLAG_REGMAP_ENDIAN_BIG ?
+		in_be32(addr) : in_le32(addr);
+}
+
+static void dspi_write32(uint flags, uint *addr, uint val)
+{
+	flags & DSPI_FLAG_REGMAP_ENDIAN_BIG ?
+		out_be32(addr, val) : out_le32(addr, val);
+}
+
+static void dspi_halt(struct fsl_dspi_priv *priv, u8 halt)
+{
+	uint mcr_val;
+
+	mcr_val = dspi_read32(priv->flags, &priv->regs->mcr);
+
+	if (halt)
+		mcr_val |= DSPI_MCR_HALT;
+	else
+		mcr_val &= ~DSPI_MCR_HALT;
+
+	dspi_write32(priv->flags, &priv->regs->mcr, mcr_val);
+}
+
+static void fsl_dspi_init_mcr(struct fsl_dspi_priv *priv, uint cfg_val)
+{
+	/* halt DSPI module */
+	dspi_halt(priv, 1);
+
+	dspi_write32(priv->flags, &priv->regs->mcr, cfg_val);
+
+	/* resume module */
+	dspi_halt(priv, 0);
+
+	priv->mcr_val = cfg_val;
+}
+
+static void fsl_dspi_cfg_cs_active_state(struct fsl_dspi_priv *priv,
+		uint cs, uint state)
+{
+	uint mcr_val;
+
+	dspi_halt(priv, 1);
+
+	mcr_val = dspi_read32(priv->flags, &priv->regs->mcr);
+	if (state & SPI_CS_HIGH)
+		/* CSx inactive state is low */
+		mcr_val &= ~DSPI_MCR_PCSIS(cs);
+	else
+		/* CSx inactive state is high */
+		mcr_val |= DSPI_MCR_PCSIS(cs);
+	dspi_write32(priv->flags, &priv->regs->mcr, mcr_val);
+
+	dspi_halt(priv, 0);
+}
+
+static int fsl_dspi_cfg_ctar_mode(struct fsl_dspi_priv *priv,
+		uint cs, uint mode)
+{
+	uint bus_setup;
+
+	bus_setup = dspi_read32(priv->flags, &priv->regs->ctar[0]);
+
+	bus_setup &= ~DSPI_CTAR_SET_MODE_MASK;
+	bus_setup |= priv->ctar_val[cs];
+	bus_setup &= ~(DSPI_CTAR_CPOL | DSPI_CTAR_CPHA | DSPI_CTAR_LSBFE);
+
+	if (mode & SPI_CPOL)
+		bus_setup |= DSPI_CTAR_CPOL;
+	if (mode & SPI_CPHA)
+		bus_setup |= DSPI_CTAR_CPHA;
+	if (mode & SPI_LSB_FIRST)
+		bus_setup |= DSPI_CTAR_LSBFE;
+
+	dspi_write32(priv->flags, &priv->regs->ctar[0], bus_setup);
+
+	priv->charbit =
+		((dspi_read32(priv->flags, &priv->regs->ctar[0]) &
+		  DSPI_CTAR_TRSZ(15)) == DSPI_CTAR_TRSZ(15)) ? 16 : 8;
+
+	return 0;
+}
+
+static void fsl_dspi_clr_fifo(struct fsl_dspi_priv *priv)
+{
+	uint mcr_val;
+
+	dspi_halt(priv, 1);
+	mcr_val = dspi_read32(priv->flags, &priv->regs->mcr);
+	/* flush RX and TX FIFO */
+	mcr_val |= (DSPI_MCR_CTXF | DSPI_MCR_CRXF);
+	dspi_write32(priv->flags, &priv->regs->mcr, mcr_val);
+	dspi_halt(priv, 0);
+}
+
+static void dspi_tx(struct fsl_dspi_priv *priv, u32 ctrl, u16 data)
+{
+	int timeout = DSPI_TXRX_WAIT_TIMEOUT;
+
+	/* wait for empty entries in TXFIFO or timeout */
+	while (DSPI_SR_TXCTR(dspi_read32(priv->flags, &priv->regs->sr)) >= 4 &&
+			timeout--)
+		udelay(1);
+
+	if (timeout >= 0)
+		dspi_write32(priv->flags, &priv->regs->tfr, (ctrl | data));
+	else
+		debug("dspi_tx: waiting timeout!\n");
+}
+
+static u16 dspi_rx(struct fsl_dspi_priv *priv)
+{
+	int timeout = DSPI_TXRX_WAIT_TIMEOUT;
+
+	/* wait for valid entries in RXFIFO or timeout */
+	while (DSPI_SR_RXCTR(dspi_read32(priv->flags, &priv->regs->sr)) == 0 &&
+			timeout--)
+		udelay(1);
+
+	if (timeout >= 0)
+		return (u16)DSPI_RFR_RXDATA(
+				dspi_read32(priv->flags, &priv->regs->rfr));
+	else {
+		debug("dspi_rx: waiting timeout!\n");
+		return (u16)(~0);
+	}
+}
+
+static int dspi_xfer(struct fsl_dspi_priv *priv, uint cs, unsigned int bitlen,
+		const void *dout, void *din, unsigned long flags)
+{
+	u16 *spi_rd16 = NULL, *spi_wr16 = NULL;
+	u8 *spi_rd = NULL, *spi_wr = NULL;
+	static u32 ctrl;
+	uint len = bitlen >> 3;
+
+	if (priv->charbit == 16) {
+		bitlen >>= 1;
+		spi_wr16 = (u16 *)dout;
+		spi_rd16 = (u16 *)din;
+	} else {
+		spi_wr = (u8 *)dout;
+		spi_rd = (u8 *)din;
+	}
+
+	if ((flags & SPI_XFER_BEGIN) == SPI_XFER_BEGIN)
+		ctrl |= DSPI_TFR_CONT;
+
+	ctrl = ctrl & DSPI_TFR_CONT;
+	ctrl = ctrl | DSPI_TFR_CTAS(0) | DSPI_TFR_PCS(cs);
+
+	if (len > 1) {
+		int tmp_len = len - 1;
+		while (tmp_len--) {
+			if (dout != NULL) {
+				if (priv->charbit == 16)
+					dspi_tx(priv, ctrl, *spi_wr16++);
+				else
+					dspi_tx(priv, ctrl, *spi_wr++);
+				dspi_rx(priv);
+			}
+
+			if (din != NULL) {
+				dspi_tx(priv, ctrl, DSPI_IDLE_VAL);
+				if (priv->charbit == 16)
+					*spi_rd16++ = dspi_rx(priv);
+				else
+					*spi_rd++ = dspi_rx(priv);
+			}
+		}
+
+		len = 1;	/* remaining byte */
+	}
+
+	if ((flags & SPI_XFER_END) == SPI_XFER_END)
+		ctrl &= ~DSPI_TFR_CONT;
+
+	if (len) {
+		if (dout != NULL) {
+			if (priv->charbit == 16)
+				dspi_tx(priv, ctrl, *spi_wr16);
+			else
+				dspi_tx(priv, ctrl, *spi_wr);
+			dspi_rx(priv);
+		}
+
+		if (din != NULL) {
+			dspi_tx(priv, ctrl, DSPI_IDLE_VAL);
+			if (priv->charbit == 16)
+				*spi_rd16 = dspi_rx(priv);
+			else
+				*spi_rd = dspi_rx(priv);
+		}
+	} else {
+		/* dummy read */
+		dspi_tx(priv, ctrl, DSPI_IDLE_VAL);
+		dspi_rx(priv);
+	}
+
+	return 0;
+}
+
+/**
+ * Calculate the divide value between input clk frequency and expected SCK frequency
+ * Formula: SCK = (clkrate/pbr) x ((1+dbr)/br)
+ * Dbr: use default value 0
+ *
+ * @pbr: return Baud Rate Prescaler value
+ * @br: return Baud Rate Scaler value
+ * @speed_hz: expected SCK frequency
+ * @clkrate: input clk frequency
+ */
+static int fsl_dspi_hz_to_spi_baud(int *pbr, int *br,
+		int speed_hz, uint clkrate)
+{
+	/* Valid baud rate pre-scaler values */
+	int pbr_tbl[4] = {2, 3, 5, 7};
+	int brs[16] = {2, 4, 6, 8,
+		16, 32, 64, 128,
+		256, 512, 1024, 2048,
+		4096, 8192, 16384, 32768};
+	int temp, i = 0, j = 0;
+
+	temp = clkrate / speed_hz;
+
+	for (i = 0; i < ARRAY_SIZE(pbr_tbl); i++)
+		for (j = 0; j < ARRAY_SIZE(brs); j++) {
+			if (pbr_tbl[i] * brs[j] >= temp) {
+				*pbr = i;
+				*br = j;
+				return 0;
+			}
+		}
+
+	debug("Can not find valid baud rate,speed_hz is %d, ", speed_hz);
+	debug("clkrate is %d, we use the max prescaler value.\n", clkrate);
+
+	*pbr = ARRAY_SIZE(pbr_tbl) - 1;
+	*br =  ARRAY_SIZE(brs) - 1;
+	return -EINVAL;
+}
+
+static int fsl_dspi_cfg_speed(struct fsl_dspi_priv *priv, uint speed)
+{
+	int ret;
+	uint bus_setup;
+	int best_i, best_j, bus_clk;
+
+	bus_clk = priv->bus_clk;
+
+	debug("DSPI set_speed: expected SCK speed %u, bus_clk %u.\n",
+	      speed, bus_clk);
+
+	bus_setup = dspi_read32(priv->flags, &priv->regs->ctar[0]);
+	bus_setup &= ~(DSPI_CTAR_DBR | DSPI_CTAR_PBR(0x3) | DSPI_CTAR_BR(0xf));
+
+	ret = fsl_dspi_hz_to_spi_baud(&best_i, &best_j, speed, bus_clk);
+	if (ret) {
+		speed = priv->speed_hz;
+		debug("DSPI set_speed use default SCK rate %u.\n", speed);
+		fsl_dspi_hz_to_spi_baud(&best_i, &best_j, speed, bus_clk);
+	}
+
+	bus_setup |= (DSPI_CTAR_PBR(best_i) | DSPI_CTAR_BR(best_j));
+	dspi_write32(priv->flags, &priv->regs->ctar[0], bus_setup);
+
+	priv->speed_hz = speed;
+
+	return 0;
+}
+#ifndef CONFIG_DM_SPI
+void spi_init(void)
+{
+	/* Nothing to do */
+}
+
+void spi_init_f(void)
+{
+	/* Nothing to do */
+}
+
+void spi_init_r(void)
+{
+	/* Nothing to do */
+}
+
+int spi_cs_is_valid(unsigned int bus, unsigned int cs)
+{
+	if (((cs >= 0) && (cs < 8)) && ((bus >= 0) && (bus < 8)))
+		return 1;
+	else
+		return 0;
+}
+
+struct spi_slave *spi_setup_slave(unsigned int bus, unsigned int cs,
+				  unsigned int max_hz, unsigned int mode)
+{
+	struct fsl_dspi *dspi;
+	uint mcr_cfg_val;
+
+	dspi = spi_alloc_slave(struct fsl_dspi, bus, cs);
+	if (!dspi)
+		return NULL;
+
+	cpu_dspi_port_conf();
+
+#ifdef CONFIG_SYS_FSL_DSPI_BE
+	dspi->priv.flags |= DSPI_FLAG_REGMAP_ENDIAN_BIG;
+#endif
+
+	dspi->priv.regs = (struct dspi *)MMAP_DSPI;
+
+#ifdef CONFIG_M68K
+	dspi->priv.bus_clk = gd->bus_clk;
+#else
+	dspi->priv.bus_clk = mxc_get_clock(MXC_DSPI_CLK);
+#endif
+	dspi->priv.speed_hz = FSL_DSPI_DEFAULT_SCK_FREQ;
+
+	/* default: all CS signals inactive state is high */
+	mcr_cfg_val = DSPI_MCR_MSTR | DSPI_MCR_PCSIS_MASK |
+		DSPI_MCR_CRXF | DSPI_MCR_CTXF;
+	fsl_dspi_init_mcr(&dspi->priv, mcr_cfg_val);
+
+	for (i = 0; i < FSL_DSPI_MAX_CHIPSELECT; i++)
+		dspi->priv.ctar_val[i] = DSPI_CTAR_DEFAULT_VALUE;
+
+#ifdef CONFIG_SYS_DSPI_CTAR0
+	if (FSL_DSPI_MAX_CHIPSELECT > 0)
+		dspi->priv.ctar_val[0] = CONFIG_SYS_DSPI_CTAR0;
+#endif
+#ifdef CONFIG_SYS_DSPI_CTAR1
+	if (FSL_DSPI_MAX_CHIPSELECT > 1)
+		dspi->priv.ctar_val[1] = CONFIG_SYS_DSPI_CTAR1;
+#endif
+#ifdef CONFIG_SYS_DSPI_CTAR2
+	if (FSL_DSPI_MAX_CHIPSELECT > 2)
+		dspi->priv.ctar_val[2] = CONFIG_SYS_DSPI_CTAR2;
+#endif
+#ifdef CONFIG_SYS_DSPI_CTAR3
+	if (FSL_DSPI_MAX_CHIPSELECT > 3)
+		dspi->priv.ctar_val[3] = CONFIG_SYS_DSPI_CTAR3;
+#endif
+#ifdef CONFIG_SYS_DSPI_CTAR4
+	if (FSL_DSPI_MAX_CHIPSELECT > 4)
+		dspi->priv.ctar_val[4] = CONFIG_SYS_DSPI_CTAR4;
+#endif
+#ifdef CONFIG_SYS_DSPI_CTAR5
+	if (FSL_DSPI_MAX_CHIPSELECT > 5)
+		dspi->priv.ctar_val[5] = CONFIG_SYS_DSPI_CTAR5;
+#endif
+#ifdef CONFIG_SYS_DSPI_CTAR6
+	if (FSL_DSPI_MAX_CHIPSELECT > 6)
+		dspi->priv.ctar_val[6] = CONFIG_SYS_DSPI_CTAR6;
+#endif
+#ifdef CONFIG_SYS_DSPI_CTAR7
+	if (FSL_DSPI_MAX_CHIPSELECT > 7)
+		dspi->priv.ctar_val[7] = CONFIG_SYS_DSPI_CTAR7;
+#endif
+
+	fsl_dspi_cfg_speed(&dspi->priv, max_hz);
+
+	/* configure transfer mode */
+	fsl_dspi_cfg_ctar_mode(&dspi->priv, cs, mode);
+
+	/* configure active state of CSX */
+	fsl_dspi_cfg_cs_active_state(&dspi->priv, cs, mode);
+
+	return &dspi->slave;
+}
+
+void spi_free_slave(struct spi_slave *slave)
+{
+	free(slave);
+}
+
+int spi_claim_bus(struct spi_slave *slave)
+{
+	uint sr_val;
+	struct fsl_dspi *dspi = (struct fsl_dspi *)slave;
+
+	cpu_dspi_claim_bus(slave->bus, slave->cs);
+
+	fsl_dspi_clr_fifo(&dspi->priv);
+
+	/* check module TX and RX status */
+	sr_val = dspi_read32(dspi->priv.flags, &dspi->priv.regs->sr);
+	if ((sr_val & DSPI_SR_TXRXS) != DSPI_SR_TXRXS) {
+		debug("DSPI RX/TX not ready!\n");
+		return -EIO;
+	}
+
+	return 0;
+}
+
+void spi_release_bus(struct spi_slave *slave)
+{
+	struct fsl_dspi *dspi = (struct fsl_dspi *)slave;
+
+	dspi_halt(&dspi->priv, 1);
+	cpu_dspi_release_bus(slave->bus.slave->cs);
+}
+
+int spi_xfer(struct spi_slave *slave, unsigned int bitlen, const void *dout,
+	     void *din, unsigned long flags)
+{
+	struct fsl_dspi *dspi = (struct fsl_dspi *)slave;
+	return dspi_xfer(&dspi->priv, slave->cs, bitlen, dout, din, flags);
+}
+#else
+static int fsl_dspi_child_pre_probe(struct udevice *dev)
+{
+	struct dm_spi_slave_platdata *slave_plat = dev_get_parent_platdata(dev);
+	struct fsl_dspi_priv *priv = dev_get_priv(dev->parent);
+
+	if (slave_plat->cs >= priv->num_chipselect) {
+		debug("DSPI invalid chipselect number %d(max %d)!\n",
+		      slave_plat->cs, priv->num_chipselect - 1);
+		return -EINVAL;
+	}
+
+	priv->ctar_val[slave_plat->cs] = DSPI_CTAR_DEFAULT_VALUE;
+
+	debug("DSPI pre_probe slave device on CS %u, max_hz %u, mode 0x%x.\n",
+	      slave_plat->cs, slave_plat->max_hz, slave_plat->mode);
+
+	return 0;
+}
+
+static int fsl_dspi_probe(struct udevice *bus)
+{
+	struct fsl_dspi_platdata *plat = dev_get_platdata(bus);
+	struct fsl_dspi_priv *priv = dev_get_priv(bus);
+	struct dm_spi_bus *dm_spi_bus;
+	uint mcr_cfg_val;
+
+	dm_spi_bus = bus->uclass_priv;
+
+	/* cpu speical pin muxing configure */
+	cpu_dspi_port_conf();
+
+	/* get input clk frequency */
+	priv->regs = (struct dspi *)plat->regs_addr;
+	priv->flags = plat->flags;
+#ifdef CONFIG_M68K
+	priv->bus_clk = gd->bus_clk;
+#else
+	priv->bus_clk = mxc_get_clock(MXC_DSPI_CLK);
+#endif
+	priv->num_chipselect = plat->num_chipselect;
+	priv->speed_hz = plat->speed_hz;
+	/* frame data length in bits, default 8bits */
+	priv->charbit = 8;
+
+	dm_spi_bus->max_hz = plat->speed_hz;
+
+	/* default: all CS signals inactive state is high */
+	mcr_cfg_val = DSPI_MCR_MSTR | DSPI_MCR_PCSIS_MASK |
+		DSPI_MCR_CRXF | DSPI_MCR_CTXF;
+	fsl_dspi_init_mcr(priv, mcr_cfg_val);
+
+	debug("%s probe done, bus-num %d.\n", bus->name, bus->seq);
+
+	return 0;
+}
+
+static int fsl_dspi_claim_bus(struct udevice *dev)
+{
+	uint sr_val;
+	struct fsl_dspi_priv *priv;
+	struct udevice *bus = dev->parent;
+	struct dm_spi_slave_platdata *slave_plat =
+		dev_get_parent_platdata(dev);
+
+	priv = dev_get_priv(bus);
+
+	/* processor special prepartion work */
+	cpu_dspi_claim_bus(bus->seq, slave_plat->cs);
+
+	/* configure transfer mode */
+	fsl_dspi_cfg_ctar_mode(priv, slave_plat->cs, priv->mode);
+
+	/* configure active state of CSX */
+	fsl_dspi_cfg_cs_active_state(priv, slave_plat->cs,
+				     priv->mode);
+
+	fsl_dspi_clr_fifo(priv);
+
+	/* check module TX and RX status */
+	sr_val = dspi_read32(priv->flags, &priv->regs->sr);
+	if ((sr_val & DSPI_SR_TXRXS) != DSPI_SR_TXRXS) {
+		debug("DSPI RX/TX not ready!\n");
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static int fsl_dspi_release_bus(struct udevice *dev)
+{
+	struct udevice *bus = dev->parent;
+	struct fsl_dspi_priv *priv = dev_get_priv(bus);
+	struct dm_spi_slave_platdata *slave_plat =
+		dev_get_parent_platdata(dev);
+
+	/* halt module */
+	dspi_halt(priv, 1);
+
+	/* processor special release work */
+	cpu_dspi_release_bus(bus->seq, slave_plat->cs);
+
+	return 0;
+}
+
+/**
+ * This function doesn't do anything except help with debugging
+ */
+static int fsl_dspi_bind(struct udevice *bus)
+{
+	debug("%s assigned req_seq %d.\n", bus->name, bus->req_seq);
+	return 0;
+}
+
+static int fsl_dspi_ofdata_to_platdata(struct udevice *bus)
+{
+	fdt_addr_t addr;
+	struct fsl_dspi_platdata *plat = bus->platdata;
+	const void *blob = gd->fdt_blob;
+	int node = bus->of_offset;
+
+	if (fdtdec_get_bool(blob, node, "big-endian"))
+		plat->flags |= DSPI_FLAG_REGMAP_ENDIAN_BIG;
+
+	plat->num_chipselect =
+		fdtdec_get_int(blob, node, "num-cs", FSL_DSPI_MAX_CHIPSELECT);
+
+	addr = fdtdec_get_addr(blob, node, "reg");
+	if (addr == FDT_ADDR_T_NONE) {
+		debug("DSPI: Can't get base address or size\n");
+		return -ENOMEM;
+	}
+	plat->regs_addr = addr;
+
+	plat->speed_hz = fdtdec_get_int(blob,
+			node, "spi-max-frequency", FSL_DSPI_DEFAULT_SCK_FREQ);
+
+	debug("DSPI: regs=0x%x, max-frequency=%d, endianess=%s, num-cs=%d\n",
+	      plat->regs_addr, plat->speed_hz,
+	      plat->flags & DSPI_FLAG_REGMAP_ENDIAN_BIG ? "be" : "le",
+	      plat->num_chipselect);
+
+	return 0;
+}
+
+static int fsl_dspi_xfer(struct udevice *dev, unsigned int bitlen,
+		const void *dout, void *din, unsigned long flags)
+{
+	struct fsl_dspi_priv *priv;
+	struct dm_spi_slave_platdata *slave_plat = dev_get_parent_platdata(dev);
+	struct udevice *bus;
+
+	bus = dev->parent;
+	priv = dev_get_priv(bus);
+
+	return dspi_xfer(priv, slave_plat->cs, bitlen, dout, din, flags);
+}
+
+static int fsl_dspi_set_speed(struct udevice *bus, uint speed)
+{
+	struct fsl_dspi_priv *priv = dev_get_priv(bus);
+
+	return fsl_dspi_cfg_speed(priv, speed);
+}
+
+static int fsl_dspi_set_mode(struct udevice *bus, uint mode)
+{
+	struct fsl_dspi_priv *priv = dev_get_priv(bus);
+
+	debug("DSPI set_mode: mode 0x%x.\n", mode);
+
+	/*
+	 * We store some chipselect special configure value in priv->ctar_val,
+	 * and we can't get the correct chipselect number here,
+	 * so just store mode value.
+	 * Do really configuration when claim_bus.
+	 */
+	priv->mode = mode;
+
+	return 0;
+}
+
+static const struct dm_spi_ops fsl_dspi_ops = {
+	.claim_bus	= fsl_dspi_claim_bus,
+	.release_bus	= fsl_dspi_release_bus,
+	.xfer		= fsl_dspi_xfer,
+	.set_speed	= fsl_dspi_set_speed,
+	.set_mode	= fsl_dspi_set_mode,
+};
+
+static const struct udevice_id fsl_dspi_ids[] = {
+	{ .compatible = "fsl,vf610-dspi" },
+	{ }
+};
+
+U_BOOT_DRIVER(fsl_dspi) = {
+	.name	= "fsl_dspi",
+	.id	= UCLASS_SPI,
+	.of_match = fsl_dspi_ids,
+	.ops	= &fsl_dspi_ops,
+	.ofdata_to_platdata = fsl_dspi_ofdata_to_platdata,
+	.platdata_auto_alloc_size = sizeof(struct fsl_dspi_platdata),
+	.priv_auto_alloc_size = sizeof(struct fsl_dspi_priv),
+	.probe	= fsl_dspi_probe,
+	.child_pre_probe = fsl_dspi_child_pre_probe,
+	.bind = fsl_dspi_bind,
+};
+#endif
diff --git a/drivers/spi/fsl_qspi.c b/drivers/spi/fsl_qspi.c
index 5e0b069..868df5f 100644
--- a/drivers/spi/fsl_qspi.c
+++ b/drivers/spi/fsl_qspi.c
@@ -1,5 +1,5 @@
 /*
- * Copyright 2013-2014 Freescale Semiconductor, Inc.
+ * Copyright 2013-2015 Freescale Semiconductor, Inc.
  *
  * Freescale Quad Serial Peripheral Interface (QSPI) driver
  *
@@ -11,8 +11,12 @@
 #include <spi.h>
 #include <asm/io.h>
 #include <linux/sizes.h>
+#include <dm.h>
+#include <errno.h>
 #include "fsl_qspi.h"
 
+DECLARE_GLOBAL_DATA_PTR;
+
 #define RX_BUFFER_SIZE		0x80
 #ifdef CONFIG_MX6SX
 #define TX_BUFFER_SIZE		0x200
@@ -63,35 +67,85 @@
 #define QSPI_CMD_PP_4B		0x12    /* Page program (up to 256 bytes) */
 #define QSPI_CMD_SE_4B		0xdc    /* Sector erase (usually 64KiB) */
 
-#ifdef CONFIG_SYS_FSL_QSPI_LE
-#define qspi_read32		in_le32
-#define qspi_write32		out_le32
-#elif defined(CONFIG_SYS_FSL_QSPI_BE)
-#define qspi_read32		in_be32
-#define qspi_write32		out_be32
-#endif
+/* fsl_qspi_platdata flags */
+#define QSPI_FLAG_REGMAP_ENDIAN_BIG	(1 << 0)
 
-static unsigned long spi_bases[] = {
-	QSPI0_BASE_ADDR,
-#ifdef CONFIG_MX6SX
-	QSPI1_BASE_ADDR,
-#endif
-};
+/* default SCK frequency, unit: HZ */
+#define FSL_QSPI_DEFAULT_SCK_FREQ	50000000
 
-static unsigned long amba_bases[] = {
-	QSPI0_AMBA_BASE,
-#ifdef CONFIG_MX6SX
-	QSPI1_AMBA_BASE,
+/* QSPI max chipselect signals number */
+#define FSL_QSPI_MAX_CHIPSELECT_NUM     4
+
+#ifdef CONFIG_DM_SPI
+/**
+ * struct fsl_qspi_platdata - platform data for Freescale QSPI
+ *
+ * @flags: Flags for QSPI QSPI_FLAG_...
+ * @speed_hz: Default SCK frequency
+ * @reg_base: Base address of QSPI registers
+ * @amba_base: Base address of QSPI memory mapping
+ * @amba_total_size: size of QSPI memory mapping
+ * @flash_num: Number of active slave devices
+ * @num_chipselect: Number of QSPI chipselect signals
+ */
+struct fsl_qspi_platdata {
+	u32 flags;
+	u32 speed_hz;
+	u32 reg_base;
+	u32 amba_base;
+	u32 amba_total_size;
+	u32 flash_num;
+	u32 num_chipselect;
+};
 #endif
+
+/**
+ * struct fsl_qspi_priv - private data for Freescale QSPI
+ *
+ * @flags: Flags for QSPI QSPI_FLAG_...
+ * @bus_clk: QSPI input clk frequency
+ * @speed_hz: Default SCK frequency
+ * @cur_seqid: current LUT table sequence id
+ * @sf_addr: flash access offset
+ * @amba_base: Base address of QSPI memory mapping of every CS
+ * @amba_total_size: size of QSPI memory mapping
+ * @cur_amba_base: Base address of QSPI memory mapping of current CS
+ * @flash_num: Number of active slave devices
+ * @num_chipselect: Number of QSPI chipselect signals
+ * @regs: Point to QSPI register structure for I/O access
+ */
+struct fsl_qspi_priv {
+	u32 flags;
+	u32 bus_clk;
+	u32 speed_hz;
+	u32 cur_seqid;
+	u32 sf_addr;
+	u32 amba_base[FSL_QSPI_MAX_CHIPSELECT_NUM];
+	u32 amba_total_size;
+	u32 cur_amba_base;
+	u32 flash_num;
+	u32 num_chipselect;
+	struct fsl_qspi_regs *regs;
 };
 
+#ifndef CONFIG_DM_SPI
 struct fsl_qspi {
 	struct spi_slave slave;
-	unsigned long reg_base;
-	unsigned long amba_base;
-	u32 sf_addr;
-	u8 cur_seqid;
+	struct fsl_qspi_priv priv;
 };
+#endif
+
+static u32 qspi_read32(u32 flags, u32 *addr)
+{
+	return flags & QSPI_FLAG_REGMAP_ENDIAN_BIG ?
+		in_be32(addr) : in_le32(addr);
+}
+
+static void qspi_write32(u32 flags, u32 *addr, u32 val)
+{
+	flags & QSPI_FLAG_REGMAP_ENDIAN_BIG ?
+		out_be32(addr, val) : out_le32(addr, val);
+}
 
 /* QSPI support swapping the flash read/write data
  * in hardware for LS102xA, but not for VF610 */
@@ -104,131 +158,135 @@ static inline u32 qspi_endian_xchg(u32 data)
 #endif
 }
 
-static inline struct fsl_qspi *to_qspi_spi(struct spi_slave *slave)
-{
-	return container_of(slave, struct fsl_qspi, slave);
-}
-
-static void qspi_set_lut(struct fsl_qspi *qspi)
+static void qspi_set_lut(struct fsl_qspi_priv *priv)
 {
-	struct fsl_qspi_regs *regs = (struct fsl_qspi_regs *)qspi->reg_base;
+	struct fsl_qspi_regs *regs = priv->regs;
 	u32 lut_base;
 
 	/* Unlock the LUT */
-	qspi_write32(&regs->lutkey, LUT_KEY_VALUE);
-	qspi_write32(&regs->lckcr, QSPI_LCKCR_UNLOCK);
+	qspi_write32(priv->flags, &regs->lutkey, LUT_KEY_VALUE);
+	qspi_write32(priv->flags, &regs->lckcr, QSPI_LCKCR_UNLOCK);
 
 	/* Write Enable */
 	lut_base = SEQID_WREN * 4;
-	qspi_write32(&regs->lut[lut_base], OPRND0(QSPI_CMD_WREN) |
+	qspi_write32(priv->flags, &regs->lut[lut_base], OPRND0(QSPI_CMD_WREN) |
 		PAD0(LUT_PAD1) | INSTR0(LUT_CMD));
-	qspi_write32(&regs->lut[lut_base + 1], 0);
-	qspi_write32(&regs->lut[lut_base + 2], 0);
-	qspi_write32(&regs->lut[lut_base + 3], 0);
+	qspi_write32(priv->flags, &regs->lut[lut_base + 1], 0);
+	qspi_write32(priv->flags, &regs->lut[lut_base + 2], 0);
+	qspi_write32(priv->flags, &regs->lut[lut_base + 3], 0);
 
 	/* Fast Read */
 	lut_base = SEQID_FAST_READ * 4;
 #ifdef CONFIG_SPI_FLASH_BAR
-	qspi_write32(&regs->lut[lut_base], OPRND0(QSPI_CMD_FAST_READ) |
-		     PAD0(LUT_PAD1) | INSTR0(LUT_CMD) | OPRND1(ADDR24BIT) |
+	qspi_write32(priv->flags, &regs->lut[lut_base],
+		     OPRND0(QSPI_CMD_FAST_READ) | PAD0(LUT_PAD1) |
+		     INSTR0(LUT_CMD) | OPRND1(ADDR24BIT) |
 		     PAD1(LUT_PAD1) | INSTR1(LUT_ADDR));
 #else
 	if (FSL_QSPI_FLASH_SIZE  <= SZ_16M)
-		qspi_write32(&regs->lut[lut_base], OPRND0(QSPI_CMD_FAST_READ) |
-			PAD0(LUT_PAD1) | INSTR0(LUT_CMD) | OPRND1(ADDR24BIT) |
-			PAD1(LUT_PAD1) | INSTR1(LUT_ADDR));
+		qspi_write32(priv->flags, &regs->lut[lut_base],
+			     OPRND0(QSPI_CMD_FAST_READ) | PAD0(LUT_PAD1) |
+			     INSTR0(LUT_CMD) | OPRND1(ADDR24BIT) |
+			     PAD1(LUT_PAD1) | INSTR1(LUT_ADDR));
 	else
-		qspi_write32(&regs->lut[lut_base],
+		qspi_write32(priv->flags, &regs->lut[lut_base],
 			     OPRND0(QSPI_CMD_FAST_READ_4B) |
 			     PAD0(LUT_PAD1) | INSTR0(LUT_CMD) |
 			     OPRND1(ADDR32BIT) | PAD1(LUT_PAD1) |
 			     INSTR1(LUT_ADDR));
 #endif
-	qspi_write32(&regs->lut[lut_base + 1], OPRND0(8) | PAD0(LUT_PAD1) |
-		INSTR0(LUT_DUMMY) | OPRND1(RX_BUFFER_SIZE) | PAD1(LUT_PAD1) |
-		INSTR1(LUT_READ));
-	qspi_write32(&regs->lut[lut_base + 2], 0);
-	qspi_write32(&regs->lut[lut_base + 3], 0);
+	qspi_write32(priv->flags, &regs->lut[lut_base + 1],
+		     OPRND0(8) | PAD0(LUT_PAD1) | INSTR0(LUT_DUMMY) |
+		     OPRND1(RX_BUFFER_SIZE) | PAD1(LUT_PAD1) |
+		     INSTR1(LUT_READ));
+	qspi_write32(priv->flags, &regs->lut[lut_base + 2], 0);
+	qspi_write32(priv->flags, &regs->lut[lut_base + 3], 0);
 
 	/* Read Status */
 	lut_base = SEQID_RDSR * 4;
-	qspi_write32(&regs->lut[lut_base], OPRND0(QSPI_CMD_RDSR) |
+	qspi_write32(priv->flags, &regs->lut[lut_base], OPRND0(QSPI_CMD_RDSR) |
 		PAD0(LUT_PAD1) | INSTR0(LUT_CMD) | OPRND1(1) |
 		PAD1(LUT_PAD1) | INSTR1(LUT_READ));
-	qspi_write32(&regs->lut[lut_base + 1], 0);
-	qspi_write32(&regs->lut[lut_base + 2], 0);
-	qspi_write32(&regs->lut[lut_base + 3], 0);
+	qspi_write32(priv->flags, &regs->lut[lut_base + 1], 0);
+	qspi_write32(priv->flags, &regs->lut[lut_base + 2], 0);
+	qspi_write32(priv->flags, &regs->lut[lut_base + 3], 0);
 
 	/* Erase a sector */
 	lut_base = SEQID_SE * 4;
 #ifdef CONFIG_SPI_FLASH_BAR
-	qspi_write32(&regs->lut[lut_base], OPRND0(QSPI_CMD_SE) |
+	qspi_write32(priv->flags, &regs->lut[lut_base], OPRND0(QSPI_CMD_SE) |
 		     PAD0(LUT_PAD1) | INSTR0(LUT_CMD) | OPRND1(ADDR24BIT) |
 		     PAD1(LUT_PAD1) | INSTR1(LUT_ADDR));
 #else
 	if (FSL_QSPI_FLASH_SIZE  <= SZ_16M)
-		qspi_write32(&regs->lut[lut_base], OPRND0(QSPI_CMD_SE) |
-			PAD0(LUT_PAD1) | INSTR0(LUT_CMD) | OPRND1(ADDR24BIT) |
-			PAD1(LUT_PAD1) | INSTR1(LUT_ADDR));
+		qspi_write32(priv->flags, &regs->lut[lut_base],
+			     OPRND0(QSPI_CMD_SE) | PAD0(LUT_PAD1) |
+			     INSTR0(LUT_CMD) | OPRND1(ADDR24BIT) |
+			     PAD1(LUT_PAD1) | INSTR1(LUT_ADDR));
 	else
-		qspi_write32(&regs->lut[lut_base], OPRND0(QSPI_CMD_SE_4B) |
-			PAD0(LUT_PAD1) | INSTR0(LUT_CMD) | OPRND1(ADDR32BIT) |
-			PAD1(LUT_PAD1) | INSTR1(LUT_ADDR));
+		qspi_write32(priv->flags, &regs->lut[lut_base],
+			     OPRND0(QSPI_CMD_SE_4B) | PAD0(LUT_PAD1) |
+			     INSTR0(LUT_CMD) | OPRND1(ADDR32BIT) |
+			     PAD1(LUT_PAD1) | INSTR1(LUT_ADDR));
 #endif
-	qspi_write32(&regs->lut[lut_base + 1], 0);
-	qspi_write32(&regs->lut[lut_base + 2], 0);
-	qspi_write32(&regs->lut[lut_base + 3], 0);
+	qspi_write32(priv->flags, &regs->lut[lut_base + 1], 0);
+	qspi_write32(priv->flags, &regs->lut[lut_base + 2], 0);
+	qspi_write32(priv->flags, &regs->lut[lut_base + 3], 0);
 
 	/* Erase the whole chip */
 	lut_base = SEQID_CHIP_ERASE * 4;
-	qspi_write32(&regs->lut[lut_base], OPRND0(QSPI_CMD_CHIP_ERASE) |
-		PAD0(LUT_PAD1) | INSTR0(LUT_CMD));
-	qspi_write32(&regs->lut[lut_base + 1], 0);
-	qspi_write32(&regs->lut[lut_base + 2], 0);
-	qspi_write32(&regs->lut[lut_base + 3], 0);
+	qspi_write32(priv->flags, &regs->lut[lut_base],
+		     OPRND0(QSPI_CMD_CHIP_ERASE) |
+		     PAD0(LUT_PAD1) | INSTR0(LUT_CMD));
+	qspi_write32(priv->flags, &regs->lut[lut_base + 1], 0);
+	qspi_write32(priv->flags, &regs->lut[lut_base + 2], 0);
+	qspi_write32(priv->flags, &regs->lut[lut_base + 3], 0);
 
 	/* Page Program */
 	lut_base = SEQID_PP * 4;
 #ifdef CONFIG_SPI_FLASH_BAR
-	qspi_write32(&regs->lut[lut_base], OPRND0(QSPI_CMD_PP) |
+	qspi_write32(priv->flags, &regs->lut[lut_base], OPRND0(QSPI_CMD_PP) |
 		     PAD0(LUT_PAD1) | INSTR0(LUT_CMD) | OPRND1(ADDR24BIT) |
 		     PAD1(LUT_PAD1) | INSTR1(LUT_ADDR));
 #else
 	if (FSL_QSPI_FLASH_SIZE  <= SZ_16M)
-		qspi_write32(&regs->lut[lut_base], OPRND0(QSPI_CMD_PP) |
-			PAD0(LUT_PAD1) | INSTR0(LUT_CMD) | OPRND1(ADDR24BIT) |
-			PAD1(LUT_PAD1) | INSTR1(LUT_ADDR));
+		qspi_write32(priv->flags, &regs->lut[lut_base],
+			     OPRND0(QSPI_CMD_PP) | PAD0(LUT_PAD1) |
+			     INSTR0(LUT_CMD) | OPRND1(ADDR24BIT) |
+			     PAD1(LUT_PAD1) | INSTR1(LUT_ADDR));
 	else
-		qspi_write32(&regs->lut[lut_base], OPRND0(QSPI_CMD_PP_4B) |
-			PAD0(LUT_PAD1) | INSTR0(LUT_CMD) | OPRND1(ADDR32BIT) |
-			PAD1(LUT_PAD1) | INSTR1(LUT_ADDR));
+		qspi_write32(priv->flags, &regs->lut[lut_base],
+			     OPRND0(QSPI_CMD_PP_4B) | PAD0(LUT_PAD1) |
+			     INSTR0(LUT_CMD) | OPRND1(ADDR32BIT) |
+			     PAD1(LUT_PAD1) | INSTR1(LUT_ADDR));
 #endif
 #ifdef CONFIG_MX6SX
 	/*
 	 * To MX6SX, OPRND0(TX_BUFFER_SIZE) can not work correctly.
 	 * So, Use IDATSZ in IPCR to determine the size and here set 0.
 	 */
-	qspi_write32(&regs->lut[lut_base + 1], OPRND0(0) |
+	qspi_write32(priv->flags, &regs->lut[lut_base + 1], OPRND0(0) |
 		     PAD0(LUT_PAD1) | INSTR0(LUT_WRITE));
 #else
-	qspi_write32(&regs->lut[lut_base + 1], OPRND0(TX_BUFFER_SIZE) |
-		PAD0(LUT_PAD1) | INSTR0(LUT_WRITE));
+	qspi_write32(priv->flags, &regs->lut[lut_base + 1],
+		     OPRND0(TX_BUFFER_SIZE) |
+		     PAD0(LUT_PAD1) | INSTR0(LUT_WRITE));
 #endif
-	qspi_write32(&regs->lut[lut_base + 2], 0);
-	qspi_write32(&regs->lut[lut_base + 3], 0);
+	qspi_write32(priv->flags, &regs->lut[lut_base + 2], 0);
+	qspi_write32(priv->flags, &regs->lut[lut_base + 3], 0);
 
 	/* READ ID */
 	lut_base = SEQID_RDID * 4;
-	qspi_write32(&regs->lut[lut_base], OPRND0(QSPI_CMD_RDID) |
+	qspi_write32(priv->flags, &regs->lut[lut_base], OPRND0(QSPI_CMD_RDID) |
 		PAD0(LUT_PAD1) | INSTR0(LUT_CMD) | OPRND1(8) |
 		PAD1(LUT_PAD1) | INSTR1(LUT_READ));
-	qspi_write32(&regs->lut[lut_base + 1], 0);
-	qspi_write32(&regs->lut[lut_base + 2], 0);
-	qspi_write32(&regs->lut[lut_base + 3], 0);
+	qspi_write32(priv->flags, &regs->lut[lut_base + 1], 0);
+	qspi_write32(priv->flags, &regs->lut[lut_base + 2], 0);
+	qspi_write32(priv->flags, &regs->lut[lut_base + 3], 0);
 
 	/* SUB SECTOR 4K ERASE */
 	lut_base = SEQID_BE_4K * 4;
-	qspi_write32(&regs->lut[lut_base], OPRND0(QSPI_CMD_BE_4K) |
+	qspi_write32(priv->flags, &regs->lut[lut_base], OPRND0(QSPI_CMD_BE_4K) |
 		     PAD0(LUT_PAD1) | INSTR0(LUT_CMD) | OPRND1(ADDR24BIT) |
 		     PAD1(LUT_PAD1) | INSTR1(LUT_ADDR));
 
@@ -239,28 +297,28 @@ static void qspi_set_lut(struct fsl_qspi *qspi)
 	 * initialization.
 	 */
 	lut_base = SEQID_BRRD * 4;
-	qspi_write32(&regs->lut[lut_base], OPRND0(QSPI_CMD_BRRD) |
+	qspi_write32(priv->flags, &regs->lut[lut_base], OPRND0(QSPI_CMD_BRRD) |
 		     PAD0(LUT_PAD1) | INSTR0(LUT_CMD) | OPRND1(1) |
 		     PAD1(LUT_PAD1) | INSTR1(LUT_READ));
 
 	lut_base = SEQID_BRWR * 4;
-	qspi_write32(&regs->lut[lut_base], OPRND0(QSPI_CMD_BRWR) |
+	qspi_write32(priv->flags, &regs->lut[lut_base], OPRND0(QSPI_CMD_BRWR) |
 		     PAD0(LUT_PAD1) | INSTR0(LUT_CMD) | OPRND1(1) |
 		     PAD1(LUT_PAD1) | INSTR1(LUT_WRITE));
 
 	lut_base = SEQID_RDEAR * 4;
-	qspi_write32(&regs->lut[lut_base], OPRND0(QSPI_CMD_RDEAR) |
+	qspi_write32(priv->flags, &regs->lut[lut_base], OPRND0(QSPI_CMD_RDEAR) |
 		     PAD0(LUT_PAD1) | INSTR0(LUT_CMD) | OPRND1(1) |
 		     PAD1(LUT_PAD1) | INSTR1(LUT_READ));
 
 	lut_base = SEQID_WREAR * 4;
-	qspi_write32(&regs->lut[lut_base], OPRND0(QSPI_CMD_WREAR) |
+	qspi_write32(priv->flags, &regs->lut[lut_base], OPRND0(QSPI_CMD_WREAR) |
 		     PAD0(LUT_PAD1) | INSTR0(LUT_CMD) | OPRND1(1) |
 		     PAD1(LUT_PAD1) | INSTR1(LUT_WRITE));
 #endif
 	/* Lock the LUT */
-	qspi_write32(&regs->lutkey, LUT_KEY_VALUE);
-	qspi_write32(&regs->lckcr, QSPI_LCKCR_LOCK);
+	qspi_write32(priv->flags, &regs->lutkey, LUT_KEY_VALUE);
+	qspi_write32(priv->flags, &regs->lckcr, QSPI_LCKCR_LOCK);
 }
 
 #if defined(CONFIG_SYS_FSL_QSPI_AHB)
@@ -270,14 +328,14 @@ static void qspi_set_lut(struct fsl_qspi *qspi)
  * the wrong data. The spec tells us reset the AHB domain and Serial Flash
  * domain at the same time.
  */
-static inline void qspi_ahb_invalid(struct fsl_qspi *q)
+static inline void qspi_ahb_invalid(struct fsl_qspi_priv *priv)
 {
-	struct fsl_qspi_regs *regs = (struct fsl_qspi_regs *)q->reg_base;
+	struct fsl_qspi_regs *regs = priv->regs;
 	u32 reg;
 
-	reg = qspi_read32(&regs->mcr);
+	reg = qspi_read32(priv->flags, &regs->mcr);
 	reg |= QSPI_MCR_SWRSTHD_MASK | QSPI_MCR_SWRSTSD_MASK;
-	qspi_write32(&regs->mcr, reg);
+	qspi_write32(priv->flags, &regs->mcr, reg);
 
 	/*
 	 * The minimum delay : 1 AHB + 2 SFCK clocks.
@@ -286,46 +344,48 @@ static inline void qspi_ahb_invalid(struct fsl_qspi *q)
 	udelay(1);
 
 	reg &= ~(QSPI_MCR_SWRSTHD_MASK | QSPI_MCR_SWRSTSD_MASK);
-	qspi_write32(&regs->mcr, reg);
+	qspi_write32(priv->flags, &regs->mcr, reg);
 }
 
 /* Read out the data from the AHB buffer. */
-static inline void qspi_ahb_read(struct fsl_qspi *q, u8 *rxbuf, int len)
+static inline void qspi_ahb_read(struct fsl_qspi_priv *priv, u8 *rxbuf, int len)
 {
-	struct fsl_qspi_regs *regs = (struct fsl_qspi_regs *)q->reg_base;
+	struct fsl_qspi_regs *regs = priv->regs;
 	u32 mcr_reg;
 
-	mcr_reg = qspi_read32(&regs->mcr);
+	mcr_reg = qspi_read32(priv->flags, &regs->mcr);
 
-	qspi_write32(&regs->mcr, QSPI_MCR_CLR_RXF_MASK | QSPI_MCR_CLR_TXF_MASK |
+	qspi_write32(priv->flags, &regs->mcr,
+		     QSPI_MCR_CLR_RXF_MASK | QSPI_MCR_CLR_TXF_MASK |
 		     QSPI_MCR_RESERVED_MASK | QSPI_MCR_END_CFD_LE);
 
 	/* Read out the data directly from the AHB buffer. */
-	memcpy(rxbuf, (u8 *)(q->amba_base + q->sf_addr), len);
+	memcpy(rxbuf, (u8 *)(priv->cur_amba_base + priv->sf_addr), len);
 
-	qspi_write32(&regs->mcr, mcr_reg);
+	qspi_write32(priv->flags, &regs->mcr, mcr_reg);
 }
 
-static void qspi_enable_ddr_mode(struct fsl_qspi_regs *regs)
+static void qspi_enable_ddr_mode(struct fsl_qspi_priv *priv)
 {
 	u32 reg, reg2;
+	struct fsl_qspi_regs *regs = priv->regs;
 
-	reg = qspi_read32(&regs->mcr);
+	reg = qspi_read32(priv->flags, &regs->mcr);
 	/* Disable the module */
-	qspi_write32(&regs->mcr, reg | QSPI_MCR_MDIS_MASK);
+	qspi_write32(priv->flags, &regs->mcr, reg | QSPI_MCR_MDIS_MASK);
 
 	/* Set the Sampling Register for DDR */
-	reg2 = qspi_read32(&regs->smpr);
+	reg2 = qspi_read32(priv->flags, &regs->smpr);
 	reg2 &= ~QSPI_SMPR_DDRSMP_MASK;
 	reg2 |= (2 << QSPI_SMPR_DDRSMP_SHIFT);
-	qspi_write32(&regs->smpr, reg2);
+	qspi_write32(priv->flags, &regs->smpr, reg2);
 
 	/* Enable the module again (enable the DDR too) */
 	reg |= QSPI_MCR_DDR_EN_MASK;
 	/* Enable bit 29 for imx6sx */
 	reg |= (1 << 29);
 
-	qspi_write32(&regs->mcr, reg);
+	qspi_write32(priv->flags, &regs->mcr, reg);
 }
 
 /*
@@ -341,180 +401,103 @@ static void qspi_enable_ddr_mode(struct fsl_qspi_regs *regs)
  * causes the controller to clear the buffer, and use the sequence pointed
  * by the QUADSPI_BFGENCR[SEQID] to initiate a read from the flash.
  */
-static void qspi_init_ahb_read(struct fsl_qspi_regs *regs)
+static void qspi_init_ahb_read(struct fsl_qspi_priv *priv)
 {
+	struct fsl_qspi_regs *regs = priv->regs;
+
 	/* AHB configuration for access buffer 0/1/2 .*/
-	qspi_write32(&regs->buf0cr, QSPI_BUFXCR_INVALID_MSTRID);
-	qspi_write32(&regs->buf1cr, QSPI_BUFXCR_INVALID_MSTRID);
-	qspi_write32(&regs->buf2cr, QSPI_BUFXCR_INVALID_MSTRID);
-	qspi_write32(&regs->buf3cr, QSPI_BUF3CR_ALLMST_MASK |
+	qspi_write32(priv->flags, &regs->buf0cr, QSPI_BUFXCR_INVALID_MSTRID);
+	qspi_write32(priv->flags, &regs->buf1cr, QSPI_BUFXCR_INVALID_MSTRID);
+	qspi_write32(priv->flags, &regs->buf2cr, QSPI_BUFXCR_INVALID_MSTRID);
+	qspi_write32(priv->flags, &regs->buf3cr, QSPI_BUF3CR_ALLMST_MASK |
 		     (0x80 << QSPI_BUF3CR_ADATSZ_SHIFT));
 
 	/* We only use the buffer3 */
-	qspi_write32(&regs->buf0ind, 0);
-	qspi_write32(&regs->buf1ind, 0);
-	qspi_write32(&regs->buf2ind, 0);
+	qspi_write32(priv->flags, &regs->buf0ind, 0);
+	qspi_write32(priv->flags, &regs->buf1ind, 0);
+	qspi_write32(priv->flags, &regs->buf2ind, 0);
 
 	/*
 	 * Set the default lut sequence for AHB Read.
 	 * Parallel mode is disabled.
 	 */
-	qspi_write32(&regs->bfgencr,
+	qspi_write32(priv->flags, &regs->bfgencr,
 		     SEQID_FAST_READ << QSPI_BFGENCR_SEQID_SHIFT);
 
 	/*Enable DDR Mode*/
-	qspi_enable_ddr_mode(regs);
+	qspi_enable_ddr_mode(priv);
 }
 #endif
 
-void spi_init()
-{
-	/* do nothing */
-}
-
-struct spi_slave *spi_setup_slave(unsigned int bus, unsigned int cs,
-		unsigned int max_hz, unsigned int mode)
-{
-	struct fsl_qspi *qspi;
-	struct fsl_qspi_regs *regs;
-	u32 smpr_val;
-	u32 total_size;
-
-	if (bus >= ARRAY_SIZE(spi_bases))
-		return NULL;
-
-	if (cs >= FSL_QSPI_FLASH_NUM)
-		return NULL;
-
-	qspi = spi_alloc_slave(struct fsl_qspi, bus, cs);
-	if (!qspi)
-		return NULL;
-
-	qspi->reg_base = spi_bases[bus];
-	/*
-	 * According cs, use different amba_base to choose the
-	 * corresponding flash devices.
-	 *
-	 * If not, only one flash device is used even if passing
-	 * different cs using `sf probe`
-	 */
-	qspi->amba_base = amba_bases[bus] + cs * FSL_QSPI_FLASH_SIZE;
-
-	qspi->slave.max_write_size = TX_BUFFER_SIZE;
-
-	regs = (struct fsl_qspi_regs *)qspi->reg_base;
-	qspi_write32(&regs->mcr, QSPI_MCR_RESERVED_MASK | QSPI_MCR_MDIS_MASK);
-
-	smpr_val = qspi_read32(&regs->smpr);
-	qspi_write32(&regs->smpr, smpr_val & ~(QSPI_SMPR_FSDLY_MASK |
-		QSPI_SMPR_FSPHS_MASK | QSPI_SMPR_HSENA_MASK));
-	qspi_write32(&regs->mcr, QSPI_MCR_RESERVED_MASK);
-
-	total_size = FSL_QSPI_FLASH_SIZE * FSL_QSPI_FLASH_NUM;
-	/*
-	 * Any read access to non-implemented addresses will provide
-	 * undefined results.
-	 *
-	 * In case single die flash devices, TOP_ADDR_MEMA2 and
-	 * TOP_ADDR_MEMB2 should be initialized/programmed to
-	 * TOP_ADDR_MEMA1 and TOP_ADDR_MEMB1 respectively - in effect,
-	 * setting the size of these devices to 0.  This would ensure
-	 * that the complete memory map is assigned to only one flash device.
-	 */
-	qspi_write32(&regs->sfa1ad, FSL_QSPI_FLASH_SIZE | amba_bases[bus]);
-	qspi_write32(&regs->sfa2ad, FSL_QSPI_FLASH_SIZE | amba_bases[bus]);
-	qspi_write32(&regs->sfb1ad, total_size | amba_bases[bus]);
-	qspi_write32(&regs->sfb2ad, total_size | amba_bases[bus]);
-
-	qspi_set_lut(qspi);
-
-	smpr_val = qspi_read32(&regs->smpr);
-	smpr_val &= ~QSPI_SMPR_DDRSMP_MASK;
-	qspi_write32(&regs->smpr, smpr_val);
-	qspi_write32(&regs->mcr, QSPI_MCR_RESERVED_MASK);
-
-#ifdef CONFIG_SYS_FSL_QSPI_AHB
-	qspi_init_ahb_read(regs);
-#endif
-	return &qspi->slave;
-}
-
-void spi_free_slave(struct spi_slave *slave)
-{
-	struct fsl_qspi *qspi = to_qspi_spi(slave);
-
-	free(qspi);
-}
-
-int spi_claim_bus(struct spi_slave *slave)
-{
-	return 0;
-}
-
 #ifdef CONFIG_SPI_FLASH_BAR
 /* Bank register read/write, EAR register read/write */
-static void qspi_op_rdbank(struct fsl_qspi *qspi, u8 *rxbuf, u32 len)
+static void qspi_op_rdbank(struct fsl_qspi_priv *priv, u8 *rxbuf, u32 len)
 {
-	struct fsl_qspi_regs *regs = (struct fsl_qspi_regs *)qspi->reg_base;
+	struct fsl_qspi_regs *regs = priv->regs;
 	u32 reg, mcr_reg, data, seqid;
 
-	mcr_reg = qspi_read32(&regs->mcr);
-	qspi_write32(&regs->mcr, QSPI_MCR_CLR_RXF_MASK | QSPI_MCR_CLR_TXF_MASK |
+	mcr_reg = qspi_read32(priv->flags, &regs->mcr);
+	qspi_write32(priv->flags, &regs->mcr,
+		     QSPI_MCR_CLR_RXF_MASK | QSPI_MCR_CLR_TXF_MASK |
 		     QSPI_MCR_RESERVED_MASK | QSPI_MCR_END_CFD_LE);
-	qspi_write32(&regs->rbct, QSPI_RBCT_RXBRD_USEIPS);
+	qspi_write32(priv->flags, &regs->rbct, QSPI_RBCT_RXBRD_USEIPS);
 
-	qspi_write32(&regs->sfar, qspi->amba_base);
+	qspi_write32(priv->flags, &regs->sfar, priv->cur_amba_base);
 
-	if (qspi->cur_seqid == QSPI_CMD_BRRD)
+	if (priv->cur_seqid == QSPI_CMD_BRRD)
 		seqid = SEQID_BRRD;
 	else
 		seqid = SEQID_RDEAR;
 
-	qspi_write32(&regs->ipcr, (seqid << QSPI_IPCR_SEQID_SHIFT) | len);
+	qspi_write32(priv->flags, &regs->ipcr,
+		     (seqid << QSPI_IPCR_SEQID_SHIFT) | len);
 
 	/* Wait previous command complete */
-	while (qspi_read32(&regs->sr) & QSPI_SR_BUSY_MASK)
+	while (qspi_read32(priv->flags, &regs->sr) & QSPI_SR_BUSY_MASK)
 		;
 
 	while (1) {
-		reg = qspi_read32(&regs->rbsr);
+		reg = qspi_read32(priv->flags, &regs->rbsr);
 		if (reg & QSPI_RBSR_RDBFL_MASK) {
-			data = qspi_read32(&regs->rbdr[0]);
+			data = qspi_read32(priv->flags, &regs->rbdr[0]);
 			data = qspi_endian_xchg(data);
 			memcpy(rxbuf, &data, len);
-			qspi_write32(&regs->mcr, qspi_read32(&regs->mcr) |
+			qspi_write32(priv->flags, &regs->mcr,
+				     qspi_read32(priv->flags, &regs->mcr) |
 				     QSPI_MCR_CLR_RXF_MASK);
 			break;
 		}
 	}
 
-	qspi_write32(&regs->mcr, mcr_reg);
+	qspi_write32(priv->flags, &regs->mcr, mcr_reg);
 }
 #endif
 
-static void qspi_op_rdid(struct fsl_qspi *qspi, u32 *rxbuf, u32 len)
+static void qspi_op_rdid(struct fsl_qspi_priv *priv, u32 *rxbuf, u32 len)
 {
-	struct fsl_qspi_regs *regs = (struct fsl_qspi_regs *)qspi->reg_base;
+	struct fsl_qspi_regs *regs = priv->regs;
 	u32 mcr_reg, rbsr_reg, data;
 	int i, size;
 
-	mcr_reg = qspi_read32(&regs->mcr);
-	qspi_write32(&regs->mcr, QSPI_MCR_CLR_RXF_MASK | QSPI_MCR_CLR_TXF_MASK |
-		QSPI_MCR_RESERVED_MASK | QSPI_MCR_END_CFD_LE);
-	qspi_write32(&regs->rbct, QSPI_RBCT_RXBRD_USEIPS);
+	mcr_reg = qspi_read32(priv->flags, &regs->mcr);
+	qspi_write32(priv->flags, &regs->mcr,
+		     QSPI_MCR_CLR_RXF_MASK | QSPI_MCR_CLR_TXF_MASK |
+		     QSPI_MCR_RESERVED_MASK | QSPI_MCR_END_CFD_LE);
+	qspi_write32(priv->flags, &regs->rbct, QSPI_RBCT_RXBRD_USEIPS);
 
-	qspi_write32(&regs->sfar, qspi->amba_base);
+	qspi_write32(priv->flags, &regs->sfar, priv->cur_amba_base);
 
-	qspi_write32(&regs->ipcr, (SEQID_RDID << QSPI_IPCR_SEQID_SHIFT) | 0);
-	while (qspi_read32(&regs->sr) & QSPI_SR_BUSY_MASK)
+	qspi_write32(priv->flags, &regs->ipcr,
+		     (SEQID_RDID << QSPI_IPCR_SEQID_SHIFT) | 0);
+	while (qspi_read32(priv->flags, &regs->sr) & QSPI_SR_BUSY_MASK)
 		;
 
 	i = 0;
 	size = len;
 	while ((RX_BUFFER_SIZE >= size) && (size > 0)) {
-		rbsr_reg = qspi_read32(&regs->rbsr);
+		rbsr_reg = qspi_read32(priv->flags, &regs->rbsr);
 		if (rbsr_reg & QSPI_RBSR_RDBFL_MASK) {
-			data = qspi_read32(&regs->rbdr[i]);
+			data = qspi_read32(priv->flags, &regs->rbdr[i]);
 			data = qspi_endian_xchg(data);
 			memcpy(rxbuf, &data, 4);
 			rxbuf++;
@@ -523,34 +506,36 @@ static void qspi_op_rdid(struct fsl_qspi *qspi, u32 *rxbuf, u32 len)
 		}
 	}
 
-	qspi_write32(&regs->mcr, mcr_reg);
+	qspi_write32(priv->flags, &regs->mcr, mcr_reg);
 }
 
 #ifndef CONFIG_SYS_FSL_QSPI_AHB
 /* If not use AHB read, read data from ip interface */
-static void qspi_op_read(struct fsl_qspi *qspi, u32 *rxbuf, u32 len)
+static void qspi_op_read(struct fsl_qspi_priv *priv, u32 *rxbuf, u32 len)
 {
-	struct fsl_qspi_regs *regs = (struct fsl_qspi_regs *)qspi->reg_base;
+	struct fsl_qspi_regs *regs = priv->regs;
 	u32 mcr_reg, data;
 	int i, size;
 	u32 to_or_from;
 
-	mcr_reg = qspi_read32(&regs->mcr);
-	qspi_write32(&regs->mcr, QSPI_MCR_CLR_RXF_MASK | QSPI_MCR_CLR_TXF_MASK |
-		QSPI_MCR_RESERVED_MASK | QSPI_MCR_END_CFD_LE);
-	qspi_write32(&regs->rbct, QSPI_RBCT_RXBRD_USEIPS);
+	mcr_reg = qspi_read32(priv->flags, &regs->mcr);
+	qspi_write32(priv->flags, &regs->mcr,
+		     QSPI_MCR_CLR_RXF_MASK | QSPI_MCR_CLR_TXF_MASK |
+		     QSPI_MCR_RESERVED_MASK | QSPI_MCR_END_CFD_LE);
+	qspi_write32(priv->flags, &regs->rbct, QSPI_RBCT_RXBRD_USEIPS);
 
-	to_or_from = qspi->sf_addr + qspi->amba_base;
+	to_or_from = priv->sf_addr + priv->cur_amba_base;
 
 	while (len > 0) {
-		qspi_write32(&regs->sfar, to_or_from);
+		qspi_write32(priv->flags, &regs->sfar, to_or_from);
 
 		size = (len > RX_BUFFER_SIZE) ?
 			RX_BUFFER_SIZE : len;
 
-		qspi_write32(&regs->ipcr,
-			(SEQID_FAST_READ << QSPI_IPCR_SEQID_SHIFT) | size);
-		while (qspi_read32(&regs->sr) & QSPI_SR_BUSY_MASK)
+		qspi_write32(priv->flags, &regs->ipcr,
+			     (SEQID_FAST_READ << QSPI_IPCR_SEQID_SHIFT) |
+			     size);
+		while (qspi_read32(priv->flags, &regs->sr) & QSPI_SR_BUSY_MASK)
 			;
 
 		to_or_from += size;
@@ -558,66 +543,69 @@ static void qspi_op_read(struct fsl_qspi *qspi, u32 *rxbuf, u32 len)
 
 		i = 0;
 		while ((RX_BUFFER_SIZE >= size) && (size > 0)) {
-			data = qspi_read32(&regs->rbdr[i]);
+			data = qspi_read32(priv->flags, &regs->rbdr[i]);
 			data = qspi_endian_xchg(data);
 			memcpy(rxbuf, &data, 4);
 			rxbuf++;
 			size -= 4;
 			i++;
 		}
-		qspi_write32(&regs->mcr, qspi_read32(&regs->mcr) |
-			QSPI_MCR_CLR_RXF_MASK);
+		qspi_write32(priv->flags, &regs->mcr,
+			     qspi_read32(priv->flags, &regs->mcr) |
+			     QSPI_MCR_CLR_RXF_MASK);
 	}
 
-	qspi_write32(&regs->mcr, mcr_reg);
+	qspi_write32(priv->flags, &regs->mcr, mcr_reg);
 }
 #endif
 
-static void qspi_op_write(struct fsl_qspi *qspi, u8 *txbuf, u32 len)
+static void qspi_op_write(struct fsl_qspi_priv *priv, u8 *txbuf, u32 len)
 {
-	struct fsl_qspi_regs *regs = (struct fsl_qspi_regs *)qspi->reg_base;
+	struct fsl_qspi_regs *regs = priv->regs;
 	u32 mcr_reg, data, reg, status_reg, seqid;
 	int i, size, tx_size;
 	u32 to_or_from = 0;
 
-	mcr_reg = qspi_read32(&regs->mcr);
-	qspi_write32(&regs->mcr, QSPI_MCR_CLR_RXF_MASK | QSPI_MCR_CLR_TXF_MASK |
-		QSPI_MCR_RESERVED_MASK | QSPI_MCR_END_CFD_LE);
-	qspi_write32(&regs->rbct, QSPI_RBCT_RXBRD_USEIPS);
+	mcr_reg = qspi_read32(priv->flags, &regs->mcr);
+	qspi_write32(priv->flags, &regs->mcr,
+		     QSPI_MCR_CLR_RXF_MASK | QSPI_MCR_CLR_TXF_MASK |
+		     QSPI_MCR_RESERVED_MASK | QSPI_MCR_END_CFD_LE);
+	qspi_write32(priv->flags, &regs->rbct, QSPI_RBCT_RXBRD_USEIPS);
 
 	status_reg = 0;
 	while ((status_reg & FLASH_STATUS_WEL) != FLASH_STATUS_WEL) {
-		qspi_write32(&regs->ipcr,
-			(SEQID_WREN << QSPI_IPCR_SEQID_SHIFT) | 0);
-		while (qspi_read32(&regs->sr) & QSPI_SR_BUSY_MASK)
+		qspi_write32(priv->flags, &regs->ipcr,
+			     (SEQID_WREN << QSPI_IPCR_SEQID_SHIFT) | 0);
+		while (qspi_read32(priv->flags, &regs->sr) & QSPI_SR_BUSY_MASK)
 			;
 
-		qspi_write32(&regs->ipcr,
-			(SEQID_RDSR << QSPI_IPCR_SEQID_SHIFT) | 1);
-		while (qspi_read32(&regs->sr) & QSPI_SR_BUSY_MASK)
+		qspi_write32(priv->flags, &regs->ipcr,
+			     (SEQID_RDSR << QSPI_IPCR_SEQID_SHIFT) | 1);
+		while (qspi_read32(priv->flags, &regs->sr) & QSPI_SR_BUSY_MASK)
 			;
 
-		reg = qspi_read32(&regs->rbsr);
+		reg = qspi_read32(priv->flags, &regs->rbsr);
 		if (reg & QSPI_RBSR_RDBFL_MASK) {
-			status_reg = qspi_read32(&regs->rbdr[0]);
+			status_reg = qspi_read32(priv->flags, &regs->rbdr[0]);
 			status_reg = qspi_endian_xchg(status_reg);
 		}
-		qspi_write32(&regs->mcr,
-			qspi_read32(&regs->mcr) | QSPI_MCR_CLR_RXF_MASK);
+		qspi_write32(priv->flags, &regs->mcr,
+			     qspi_read32(priv->flags, &regs->mcr) |
+			     QSPI_MCR_CLR_RXF_MASK);
 	}
 
 	/* Default is page programming */
 	seqid = SEQID_PP;
 #ifdef CONFIG_SPI_FLASH_BAR
-	if (qspi->cur_seqid == QSPI_CMD_BRWR)
+	if (priv->cur_seqid == QSPI_CMD_BRWR)
 		seqid = SEQID_BRWR;
-	else if (qspi->cur_seqid == QSPI_CMD_WREAR)
+	else if (priv->cur_seqid == QSPI_CMD_WREAR)
 		seqid = SEQID_WREAR;
 #endif
 
-	to_or_from = qspi->sf_addr + qspi->amba_base;
+	to_or_from = priv->sf_addr + priv->cur_amba_base;
 
-	qspi_write32(&regs->sfar, to_or_from);
+	qspi_write32(priv->flags, &regs->sfar, to_or_from);
 
 	tx_size = (len > TX_BUFFER_SIZE) ?
 		TX_BUFFER_SIZE : len;
@@ -626,7 +614,7 @@ static void qspi_op_write(struct fsl_qspi *qspi, u8 *txbuf, u32 len)
 	for (i = 0; i < size; i++) {
 		memcpy(&data, txbuf, 4);
 		data = qspi_endian_xchg(data);
-		qspi_write32(&regs->tbdr, data);
+		qspi_write32(priv->flags, &regs->tbdr, data);
 		txbuf += 4;
 	}
 
@@ -635,146 +623,273 @@ static void qspi_op_write(struct fsl_qspi *qspi, u8 *txbuf, u32 len)
 		data = 0;
 		memcpy(&data, txbuf, size);
 		data = qspi_endian_xchg(data);
-		qspi_write32(&regs->tbdr, data);
+		qspi_write32(priv->flags, &regs->tbdr, data);
 	}
 
-	qspi_write32(&regs->ipcr, (seqid << QSPI_IPCR_SEQID_SHIFT) | tx_size);
-	while (qspi_read32(&regs->sr) & QSPI_SR_BUSY_MASK)
+	qspi_write32(priv->flags, &regs->ipcr,
+		     (seqid << QSPI_IPCR_SEQID_SHIFT) | tx_size);
+	while (qspi_read32(priv->flags, &regs->sr) & QSPI_SR_BUSY_MASK)
 		;
 
-	qspi_write32(&regs->mcr, mcr_reg);
+	qspi_write32(priv->flags, &regs->mcr, mcr_reg);
 }
 
-static void qspi_op_rdsr(struct fsl_qspi *qspi, u32 *rxbuf)
+static void qspi_op_rdsr(struct fsl_qspi_priv *priv, u32 *rxbuf)
 {
-	struct fsl_qspi_regs *regs = (struct fsl_qspi_regs *)qspi->reg_base;
+	struct fsl_qspi_regs *regs = priv->regs;
 	u32 mcr_reg, reg, data;
 
-	mcr_reg = qspi_read32(&regs->mcr);
-	qspi_write32(&regs->mcr, QSPI_MCR_CLR_RXF_MASK | QSPI_MCR_CLR_TXF_MASK |
-		QSPI_MCR_RESERVED_MASK | QSPI_MCR_END_CFD_LE);
-	qspi_write32(&regs->rbct, QSPI_RBCT_RXBRD_USEIPS);
+	mcr_reg = qspi_read32(priv->flags, &regs->mcr);
+	qspi_write32(priv->flags, &regs->mcr,
+		     QSPI_MCR_CLR_RXF_MASK | QSPI_MCR_CLR_TXF_MASK |
+		     QSPI_MCR_RESERVED_MASK | QSPI_MCR_END_CFD_LE);
+	qspi_write32(priv->flags, &regs->rbct, QSPI_RBCT_RXBRD_USEIPS);
 
-	qspi_write32(&regs->sfar, qspi->amba_base);
+	qspi_write32(priv->flags, &regs->sfar, priv->cur_amba_base);
 
-	qspi_write32(&regs->ipcr,
-		(SEQID_RDSR << QSPI_IPCR_SEQID_SHIFT) | 0);
-	while (qspi_read32(&regs->sr) & QSPI_SR_BUSY_MASK)
+	qspi_write32(priv->flags, &regs->ipcr,
+		     (SEQID_RDSR << QSPI_IPCR_SEQID_SHIFT) | 0);
+	while (qspi_read32(priv->flags, &regs->sr) & QSPI_SR_BUSY_MASK)
 		;
 
 	while (1) {
-		reg = qspi_read32(&regs->rbsr);
+		reg = qspi_read32(priv->flags, &regs->rbsr);
 		if (reg & QSPI_RBSR_RDBFL_MASK) {
-			data = qspi_read32(&regs->rbdr[0]);
+			data = qspi_read32(priv->flags, &regs->rbdr[0]);
 			data = qspi_endian_xchg(data);
 			memcpy(rxbuf, &data, 4);
-			qspi_write32(&regs->mcr, qspi_read32(&regs->mcr) |
-				QSPI_MCR_CLR_RXF_MASK);
+			qspi_write32(priv->flags, &regs->mcr,
+				     qspi_read32(priv->flags, &regs->mcr) |
+				     QSPI_MCR_CLR_RXF_MASK);
 			break;
 		}
 	}
 
-	qspi_write32(&regs->mcr, mcr_reg);
+	qspi_write32(priv->flags, &regs->mcr, mcr_reg);
 }
 
-static void qspi_op_erase(struct fsl_qspi *qspi)
+static void qspi_op_erase(struct fsl_qspi_priv *priv)
 {
-	struct fsl_qspi_regs *regs = (struct fsl_qspi_regs *)qspi->reg_base;
+	struct fsl_qspi_regs *regs = priv->regs;
 	u32 mcr_reg;
 	u32 to_or_from = 0;
 
-	mcr_reg = qspi_read32(&regs->mcr);
-	qspi_write32(&regs->mcr, QSPI_MCR_CLR_RXF_MASK | QSPI_MCR_CLR_TXF_MASK |
-		QSPI_MCR_RESERVED_MASK | QSPI_MCR_END_CFD_LE);
-	qspi_write32(&regs->rbct, QSPI_RBCT_RXBRD_USEIPS);
+	mcr_reg = qspi_read32(priv->flags, &regs->mcr);
+	qspi_write32(priv->flags, &regs->mcr,
+		     QSPI_MCR_CLR_RXF_MASK | QSPI_MCR_CLR_TXF_MASK |
+		     QSPI_MCR_RESERVED_MASK | QSPI_MCR_END_CFD_LE);
+	qspi_write32(priv->flags, &regs->rbct, QSPI_RBCT_RXBRD_USEIPS);
 
-	to_or_from = qspi->sf_addr + qspi->amba_base;
-	qspi_write32(&regs->sfar, to_or_from);
+	to_or_from = priv->sf_addr + priv->cur_amba_base;
+	qspi_write32(priv->flags, &regs->sfar, to_or_from);
 
-	qspi_write32(&regs->ipcr,
-		(SEQID_WREN << QSPI_IPCR_SEQID_SHIFT) | 0);
-	while (qspi_read32(&regs->sr) & QSPI_SR_BUSY_MASK)
+	qspi_write32(priv->flags, &regs->ipcr,
+		     (SEQID_WREN << QSPI_IPCR_SEQID_SHIFT) | 0);
+	while (qspi_read32(priv->flags, &regs->sr) & QSPI_SR_BUSY_MASK)
 		;
 
-	if (qspi->cur_seqid == QSPI_CMD_SE) {
-		qspi_write32(&regs->ipcr,
+	if (priv->cur_seqid == QSPI_CMD_SE) {
+		qspi_write32(priv->flags, &regs->ipcr,
 			     (SEQID_SE << QSPI_IPCR_SEQID_SHIFT) | 0);
-	} else if (qspi->cur_seqid == QSPI_CMD_BE_4K) {
-		qspi_write32(&regs->ipcr,
+	} else if (priv->cur_seqid == QSPI_CMD_BE_4K) {
+		qspi_write32(priv->flags, &regs->ipcr,
 			     (SEQID_BE_4K << QSPI_IPCR_SEQID_SHIFT) | 0);
 	}
-	while (qspi_read32(&regs->sr) & QSPI_SR_BUSY_MASK)
+	while (qspi_read32(priv->flags, &regs->sr) & QSPI_SR_BUSY_MASK)
 		;
 
-	qspi_write32(&regs->mcr, mcr_reg);
+	qspi_write32(priv->flags, &regs->mcr, mcr_reg);
 }
 
-int spi_xfer(struct spi_slave *slave, unsigned int bitlen,
+int qspi_xfer(struct fsl_qspi_priv *priv, unsigned int bitlen,
 		const void *dout, void *din, unsigned long flags)
 {
-	struct fsl_qspi *qspi = to_qspi_spi(slave);
 	u32 bytes = DIV_ROUND_UP(bitlen, 8);
 	static u32 wr_sfaddr;
 	u32 txbuf;
 
 	if (dout) {
 		if (flags & SPI_XFER_BEGIN) {
-			qspi->cur_seqid = *(u8 *)dout;
+			priv->cur_seqid = *(u8 *)dout;
 			memcpy(&txbuf, dout, 4);
 		}
 
 		if (flags == SPI_XFER_END) {
-			qspi->sf_addr = wr_sfaddr;
-			qspi_op_write(qspi, (u8 *)dout, bytes);
+			priv->sf_addr = wr_sfaddr;
+			qspi_op_write(priv, (u8 *)dout, bytes);
 			return 0;
 		}
 
-		if (qspi->cur_seqid == QSPI_CMD_FAST_READ) {
-			qspi->sf_addr = swab32(txbuf) & OFFSET_BITS_MASK;
-		} else if ((qspi->cur_seqid == QSPI_CMD_SE) ||
-			   (qspi->cur_seqid == QSPI_CMD_BE_4K)) {
-			qspi->sf_addr = swab32(txbuf) & OFFSET_BITS_MASK;
-			qspi_op_erase(qspi);
-		} else if (qspi->cur_seqid == QSPI_CMD_PP)
+		if (priv->cur_seqid == QSPI_CMD_FAST_READ) {
+			priv->sf_addr = swab32(txbuf) & OFFSET_BITS_MASK;
+		} else if ((priv->cur_seqid == QSPI_CMD_SE) ||
+			   (priv->cur_seqid == QSPI_CMD_BE_4K)) {
+			priv->sf_addr = swab32(txbuf) & OFFSET_BITS_MASK;
+			qspi_op_erase(priv);
+		} else if (priv->cur_seqid == QSPI_CMD_PP) {
 			wr_sfaddr = swab32(txbuf) & OFFSET_BITS_MASK;
+		} else if ((priv->cur_seqid == QSPI_CMD_BRWR) ||
+			 (priv->cur_seqid == QSPI_CMD_WREAR)) {
 #ifdef CONFIG_SPI_FLASH_BAR
-		else if ((qspi->cur_seqid == QSPI_CMD_BRWR) ||
-			 (qspi->cur_seqid == QSPI_CMD_WREAR)) {
 			wr_sfaddr = 0;
-		}
 #endif
+		}
 	}
 
 	if (din) {
-		if (qspi->cur_seqid == QSPI_CMD_FAST_READ) {
+		if (priv->cur_seqid == QSPI_CMD_FAST_READ) {
 #ifdef CONFIG_SYS_FSL_QSPI_AHB
-			qspi_ahb_read(qspi, din, bytes);
+			qspi_ahb_read(priv, din, bytes);
 #else
-			qspi_op_read(qspi, din, bytes);
+			qspi_op_read(priv, din, bytes);
 #endif
-		}
-		else if (qspi->cur_seqid == QSPI_CMD_RDID)
-			qspi_op_rdid(qspi, din, bytes);
-		else if (qspi->cur_seqid == QSPI_CMD_RDSR)
-			qspi_op_rdsr(qspi, din);
+		} else if (priv->cur_seqid == QSPI_CMD_RDID)
+			qspi_op_rdid(priv, din, bytes);
+		else if (priv->cur_seqid == QSPI_CMD_RDSR)
+			qspi_op_rdsr(priv, din);
 #ifdef CONFIG_SPI_FLASH_BAR
-		else if ((qspi->cur_seqid == QSPI_CMD_BRRD) ||
-			 (qspi->cur_seqid == QSPI_CMD_RDEAR)) {
-			qspi->sf_addr = 0;
-			qspi_op_rdbank(qspi, din, bytes);
+		else if ((priv->cur_seqid == QSPI_CMD_BRRD) ||
+			 (priv->cur_seqid == QSPI_CMD_RDEAR)) {
+			priv->sf_addr = 0;
+			qspi_op_rdbank(priv, din, bytes);
 		}
 #endif
 	}
 
 #ifdef CONFIG_SYS_FSL_QSPI_AHB
-	if ((qspi->cur_seqid == QSPI_CMD_SE) ||
-	    (qspi->cur_seqid == QSPI_CMD_PP) ||
-	    (qspi->cur_seqid == QSPI_CMD_BE_4K) ||
-	    (qspi->cur_seqid == QSPI_CMD_WREAR) ||
-	    (qspi->cur_seqid == QSPI_CMD_BRWR))
-		qspi_ahb_invalid(qspi);
+	if ((priv->cur_seqid == QSPI_CMD_SE) ||
+	    (priv->cur_seqid == QSPI_CMD_PP) ||
+	    (priv->cur_seqid == QSPI_CMD_BE_4K) ||
+	    (priv->cur_seqid == QSPI_CMD_WREAR) ||
+	    (priv->cur_seqid == QSPI_CMD_BRWR))
+		qspi_ahb_invalid(priv);
+#endif
+
+	return 0;
+}
+
+void qspi_module_disable(struct fsl_qspi_priv *priv, u8 disable)
+{
+	u32 mcr_val;
+
+	mcr_val = qspi_read32(priv->flags, &priv->regs->mcr);
+	if (disable)
+		mcr_val |= QSPI_MCR_MDIS_MASK;
+	else
+		mcr_val &= ~QSPI_MCR_MDIS_MASK;
+	qspi_write32(priv->flags, &priv->regs->mcr, mcr_val);
+}
+
+void qspi_cfg_smpr(struct fsl_qspi_priv *priv, u32 clear_bits, u32 set_bits)
+{
+	u32 smpr_val;
+
+	smpr_val = qspi_read32(priv->flags, &priv->regs->smpr);
+	smpr_val &= ~clear_bits;
+	smpr_val |= set_bits;
+	qspi_write32(priv->flags, &priv->regs->smpr, smpr_val);
+}
+#ifndef CONFIG_DM_SPI
+static unsigned long spi_bases[] = {
+	QSPI0_BASE_ADDR,
+#ifdef CONFIG_MX6SX
+	QSPI1_BASE_ADDR,
+#endif
+};
+
+static unsigned long amba_bases[] = {
+	QSPI0_AMBA_BASE,
+#ifdef CONFIG_MX6SX
+	QSPI1_AMBA_BASE,
+#endif
+};
+
+static inline struct fsl_qspi *to_qspi_spi(struct spi_slave *slave)
+{
+	return container_of(slave, struct fsl_qspi, slave);
+}
+
+struct spi_slave *spi_setup_slave(unsigned int bus, unsigned int cs,
+		unsigned int max_hz, unsigned int mode)
+{
+	struct fsl_qspi *qspi;
+	struct fsl_qspi_regs *regs;
+	u32 total_size;
+
+	if (bus >= ARRAY_SIZE(spi_bases))
+		return NULL;
+
+	if (cs >= FSL_QSPI_FLASH_NUM)
+		return NULL;
+
+	qspi = spi_alloc_slave(struct fsl_qspi, bus, cs);
+	if (!qspi)
+		return NULL;
+
+#ifdef CONFIG_SYS_FSL_QSPI_BE
+	qspi->priv.flags |= QSPI_FLAG_REGMAP_ENDIAN_BIG;
+#endif
+
+	regs = (struct fsl_qspi_regs *)spi_bases[bus];
+	qspi->priv.regs = regs;
+	/*
+	 * According cs, use different amba_base to choose the
+	 * corresponding flash devices.
+	 *
+	 * If not, only one flash device is used even if passing
+	 * different cs using `sf probe`
+	 */
+	qspi->priv.cur_amba_base = amba_bases[bus] + cs * FSL_QSPI_FLASH_SIZE;
+
+	qspi->slave.max_write_size = TX_BUFFER_SIZE;
+
+	qspi_write32(qspi->priv.flags, &regs->mcr,
+		     QSPI_MCR_RESERVED_MASK | QSPI_MCR_MDIS_MASK);
+
+	qspi_cfg_smpr(&qspi->priv,
+		      ~(QSPI_SMPR_FSDLY_MASK | QSPI_SMPR_DDRSMP_MASK |
+		      QSPI_SMPR_FSPHS_MASK | QSPI_SMPR_HSENA_MASK), 0);
+
+	total_size = FSL_QSPI_FLASH_SIZE * FSL_QSPI_FLASH_NUM;
+	/*
+	 * Any read access to non-implemented addresses will provide
+	 * undefined results.
+	 *
+	 * In case single die flash devices, TOP_ADDR_MEMA2 and
+	 * TOP_ADDR_MEMB2 should be initialized/programmed to
+	 * TOP_ADDR_MEMA1 and TOP_ADDR_MEMB1 respectively - in effect,
+	 * setting the size of these devices to 0.  This would ensure
+	 * that the complete memory map is assigned to only one flash device.
+	 */
+	qspi_write32(qspi->priv.flags, &regs->sfa1ad,
+		     FSL_QSPI_FLASH_SIZE | amba_bases[bus]);
+	qspi_write32(qspi->priv.flags, &regs->sfa2ad,
+		     FSL_QSPI_FLASH_SIZE | amba_bases[bus]);
+	qspi_write32(qspi->priv.flags, &regs->sfb1ad,
+		     total_size | amba_bases[bus]);
+	qspi_write32(qspi->priv.flags, &regs->sfb2ad,
+		     total_size | amba_bases[bus]);
+
+	qspi_set_lut(&qspi->priv);
+
+#ifdef CONFIG_SYS_FSL_QSPI_AHB
+	qspi_init_ahb_read(&qspi->priv);
 #endif
 
+	qspi_module_disable(&qspi->priv, 0);
+
+	return &qspi->slave;
+}
+
+void spi_free_slave(struct spi_slave *slave)
+{
+	struct fsl_qspi *qspi = to_qspi_spi(slave);
+
+	free(qspi);
+}
+
+int spi_claim_bus(struct spi_slave *slave)
+{
 	return 0;
 }
 
@@ -782,3 +897,215 @@ void spi_release_bus(struct spi_slave *slave)
 {
 	/* Nothing to do */
 }
+
+int spi_xfer(struct spi_slave *slave, unsigned int bitlen,
+		const void *dout, void *din, unsigned long flags)
+{
+	struct fsl_qspi *qspi = to_qspi_spi(slave);
+
+	return qspi_xfer(&qspi->priv, bitlen, dout, din, flags);
+}
+
+void spi_init(void)
+{
+	/* Nothing to do */
+}
+#else
+static int fsl_qspi_child_pre_probe(struct udevice *dev)
+{
+	struct spi_slave *slave = dev_get_parentdata(dev);
+
+	slave->max_write_size = TX_BUFFER_SIZE;
+
+	return 0;
+}
+
+static int fsl_qspi_probe(struct udevice *bus)
+{
+	u32 total_size;
+	struct fsl_qspi_platdata *plat = dev_get_platdata(bus);
+	struct fsl_qspi_priv *priv = dev_get_priv(bus);
+	struct dm_spi_bus *dm_spi_bus;
+
+	dm_spi_bus = bus->uclass_priv;
+
+	dm_spi_bus->max_hz = plat->speed_hz;
+
+	priv->regs = (struct fsl_qspi_regs *)plat->reg_base;
+	priv->flags = plat->flags;
+
+	priv->speed_hz = plat->speed_hz;
+	priv->amba_base[0] = plat->amba_base;
+	priv->amba_total_size = plat->amba_total_size;
+	priv->flash_num = plat->flash_num;
+	priv->num_chipselect = plat->num_chipselect;
+
+	qspi_write32(priv->flags, &priv->regs->mcr,
+		     QSPI_MCR_RESERVED_MASK | QSPI_MCR_MDIS_MASK);
+
+	qspi_cfg_smpr(priv, ~(QSPI_SMPR_FSDLY_MASK | QSPI_SMPR_DDRSMP_MASK |
+		QSPI_SMPR_FSPHS_MASK | QSPI_SMPR_HSENA_MASK), 0);
+
+	total_size = FSL_QSPI_FLASH_SIZE * FSL_QSPI_FLASH_NUM;
+	/*
+	 * Any read access to non-implemented addresses will provide
+	 * undefined results.
+	 *
+	 * In case single die flash devices, TOP_ADDR_MEMA2 and
+	 * TOP_ADDR_MEMB2 should be initialized/programmed to
+	 * TOP_ADDR_MEMA1 and TOP_ADDR_MEMB1 respectively - in effect,
+	 * setting the size of these devices to 0.  This would ensure
+	 * that the complete memory map is assigned to only one flash device.
+	 */
+	qspi_write32(priv->flags, &priv->regs->sfa1ad,
+		     FSL_QSPI_FLASH_SIZE | priv->amba_base[0]);
+	qspi_write32(priv->flags, &priv->regs->sfa2ad,
+		     FSL_QSPI_FLASH_SIZE | priv->amba_base[0]);
+	qspi_write32(priv->flags, &priv->regs->sfb1ad,
+		     total_size | priv->amba_base[0]);
+	qspi_write32(priv->flags, &priv->regs->sfb2ad,
+		     total_size | priv->amba_base[0]);
+
+	qspi_set_lut(priv);
+
+#ifdef CONFIG_SYS_FSL_QSPI_AHB
+	qspi_init_ahb_read(priv);
+#endif
+
+	qspi_module_disable(priv, 0);
+
+	return 0;
+}
+
+static int fsl_qspi_ofdata_to_platdata(struct udevice *bus)
+{
+	struct reg_data {
+		u32 addr;
+		u32 size;
+	} regs_data[2];
+	struct fsl_qspi_platdata *plat = bus->platdata;
+	const void *blob = gd->fdt_blob;
+	int node = bus->of_offset;
+	int ret, flash_num = 0, subnode;
+
+	if (fdtdec_get_bool(blob, node, "big-endian"))
+		plat->flags |= QSPI_FLAG_REGMAP_ENDIAN_BIG;
+
+	ret = fdtdec_get_int_array(blob, node, "reg", (u32 *)regs_data,
+				   sizeof(regs_data)/sizeof(u32));
+	if (ret) {
+		debug("Error: can't get base addresses (ret = %d)!\n", ret);
+		return -ENOMEM;
+	}
+
+	/* Count flash numbers */
+	fdt_for_each_subnode(blob, subnode, node)
+		++flash_num;
+
+	if (flash_num == 0) {
+		debug("Error: Missing flashes!\n");
+		return -ENODEV;
+	}
+
+	plat->speed_hz = fdtdec_get_int(blob, node, "spi-max-frequency",
+					FSL_QSPI_DEFAULT_SCK_FREQ);
+	plat->num_chipselect = fdtdec_get_int(blob, node, "num-cs",
+					      FSL_QSPI_MAX_CHIPSELECT_NUM);
+
+	plat->reg_base = regs_data[0].addr;
+	plat->amba_base = regs_data[1].addr;
+	plat->amba_total_size = regs_data[1].size;
+	plat->flash_num = flash_num;
+
+	debug("%s: regs=<0x%x> <0x%x, 0x%x>, max-frequency=%d, endianess=%s\n",
+	      __func__,
+	      plat->reg_base,
+	      plat->amba_base,
+	      plat->amba_total_size,
+	      plat->speed_hz,
+	      plat->flags & QSPI_FLAG_REGMAP_ENDIAN_BIG ? "be" : "le"
+	      );
+
+	return 0;
+}
+
+static int fsl_qspi_xfer(struct udevice *dev, unsigned int bitlen,
+		const void *dout, void *din, unsigned long flags)
+{
+	struct fsl_qspi_priv *priv;
+	struct udevice *bus;
+
+	bus = dev->parent;
+	priv = dev_get_priv(bus);
+
+	return qspi_xfer(priv, bitlen, dout, din, flags);
+}
+
+static int fsl_qspi_claim_bus(struct udevice *dev)
+{
+	struct fsl_qspi_priv *priv;
+	struct udevice *bus;
+	struct dm_spi_slave_platdata *slave_plat = dev_get_parent_platdata(dev);
+
+	bus = dev->parent;
+	priv = dev_get_priv(bus);
+
+	priv->cur_amba_base =
+		priv->amba_base[0] + FSL_QSPI_FLASH_SIZE * slave_plat->cs;
+
+	qspi_module_disable(priv, 0);
+
+	return 0;
+}
+
+static int fsl_qspi_release_bus(struct udevice *dev)
+{
+	struct fsl_qspi_priv *priv;
+	struct udevice *bus;
+
+	bus = dev->parent;
+	priv = dev_get_priv(bus);
+
+	qspi_module_disable(priv, 1);
+
+	return 0;
+}
+
+static int fsl_qspi_set_speed(struct udevice *bus, uint speed)
+{
+	/* Nothing to do */
+	return 0;
+}
+
+static int fsl_qspi_set_mode(struct udevice *bus, uint mode)
+{
+	/* Nothing to do */
+	return 0;
+}
+
+static const struct dm_spi_ops fsl_qspi_ops = {
+	.claim_bus	= fsl_qspi_claim_bus,
+	.release_bus	= fsl_qspi_release_bus,
+	.xfer		= fsl_qspi_xfer,
+	.set_speed	= fsl_qspi_set_speed,
+	.set_mode	= fsl_qspi_set_mode,
+};
+
+static const struct udevice_id fsl_qspi_ids[] = {
+	{ .compatible = "fsl,vf610-qspi" },
+	{ .compatible = "fsl,imx6sx-qspi" },
+	{ }
+};
+
+U_BOOT_DRIVER(fsl_qspi) = {
+	.name	= "fsl_qspi",
+	.id	= UCLASS_SPI,
+	.of_match = fsl_qspi_ids,
+	.ops	= &fsl_qspi_ops,
+	.ofdata_to_platdata = fsl_qspi_ofdata_to_platdata,
+	.platdata_auto_alloc_size = sizeof(struct fsl_qspi_platdata),
+	.priv_auto_alloc_size = sizeof(struct fsl_qspi_priv),
+	.probe	= fsl_qspi_probe,
+	.child_pre_probe = fsl_qspi_child_pre_probe,
+};
+#endif
diff --git a/drivers/spi/ich.c b/drivers/spi/ich.c
index 194e882..50354fd 100644
--- a/drivers/spi/ich.c
+++ b/drivers/spi/ich.c
@@ -7,6 +7,7 @@
  */
 
 #include <common.h>
+#include <dm.h>
 #include <errno.h>
 #include <malloc.h>
 #include <spi.h>
@@ -19,154 +20,99 @@
 #define SPI_OPCODE_WREN      0x06
 #define SPI_OPCODE_FAST_READ 0x0b
 
-struct ich_ctlr {
+struct ich_spi_platdata {
 	pci_dev_t dev;		/* PCI device number */
 	int ich_version;	/* Controller version, 7 or 9 */
 	bool use_sbase;		/* Use SBASE instead of RCB */
+};
+
+struct ich_spi_priv {
 	int ichspi_lock;
 	int locked;
-	uint8_t *opmenu;
+	int opmenu;
 	int menubytes;
 	void *base;		/* Base of register set */
-	uint16_t *preop;
-	uint16_t *optype;
-	uint32_t *addr;
-	uint8_t *data;
+	int preop;
+	int optype;
+	int addr;
+	int data;
 	unsigned databytes;
-	uint8_t *status;
-	uint16_t *control;
-	uint32_t *bbar;
+	int status;
+	int control;
+	int bbar;
 	uint32_t *pr;		/* only for ich9 */
-	uint8_t *speed;		/* pointer to speed control */
+	int speed;		/* pointer to speed control */
 	ulong max_speed;	/* Maximum bus speed in MHz */
+	ulong cur_speed;	/* Current bus speed */
+	struct spi_trans trans;	/* current transaction in progress */
 };
 
-struct ich_ctlr ctlr;
-
-static inline struct ich_spi_slave *to_ich_spi(struct spi_slave *slave)
-{
-	return container_of(slave, struct ich_spi_slave, slave);
-}
-
-static unsigned int ich_reg(const void *addr)
-{
-	return (unsigned)(addr - ctlr.base) & 0xffff;
-}
-
-static u8 ich_readb(const void *addr)
+static u8 ich_readb(struct ich_spi_priv *priv, int reg)
 {
-	u8 value = readb(addr);
+	u8 value = readb(priv->base + reg);
 
-	debug("read %2.2x from %4.4x\n", value, ich_reg(addr));
+	debug("read %2.2x from %4.4x\n", value, reg);
 
 	return value;
 }
 
-static u16 ich_readw(const void *addr)
+static u16 ich_readw(struct ich_spi_priv *priv, int reg)
 {
-	u16 value = readw(addr);
+	u16 value = readw(priv->base + reg);
 
-	debug("read %4.4x from %4.4x\n", value, ich_reg(addr));
+	debug("read %4.4x from %4.4x\n", value, reg);
 
 	return value;
 }
 
-static u32 ich_readl(const void *addr)
+static u32 ich_readl(struct ich_spi_priv *priv, int reg)
 {
-	u32 value = readl(addr);
+	u32 value = readl(priv->base + reg);
 
-	debug("read %8.8x from %4.4x\n", value, ich_reg(addr));
+	debug("read %8.8x from %4.4x\n", value, reg);
 
 	return value;
 }
 
-static void ich_writeb(u8 value, void *addr)
+static void ich_writeb(struct ich_spi_priv *priv, u8 value, int reg)
 {
-	writeb(value, addr);
-	debug("wrote %2.2x to %4.4x\n", value, ich_reg(addr));
+	writeb(value, priv->base + reg);
+	debug("wrote %2.2x to %4.4x\n", value, reg);
 }
 
-static void ich_writew(u16 value, void *addr)
+static void ich_writew(struct ich_spi_priv *priv, u16 value, int reg)
 {
-	writew(value, addr);
-	debug("wrote %4.4x to %4.4x\n", value, ich_reg(addr));
+	writew(value, priv->base + reg);
+	debug("wrote %4.4x to %4.4x\n", value, reg);
 }
 
-static void ich_writel(u32 value, void *addr)
+static void ich_writel(struct ich_spi_priv *priv, u32 value, int reg)
 {
-	writel(value, addr);
-	debug("wrote %8.8x to %4.4x\n", value, ich_reg(addr));
+	writel(value, priv->base + reg);
+	debug("wrote %8.8x to %4.4x\n", value, reg);
 }
 
-static void write_reg(const void *value, void *dest, uint32_t size)
+static void write_reg(struct ich_spi_priv *priv, const void *value,
+		      int dest_reg, uint32_t size)
 {
-	memcpy_toio(dest, value, size);
+	memcpy_toio(priv->base + dest_reg, value, size);
 }
 
-static void read_reg(const void *src, void *value, uint32_t size)
+static void read_reg(struct ich_spi_priv *priv, int src_reg, void *value,
+		     uint32_t size)
 {
-	memcpy_fromio(value, src, size);
+	memcpy_fromio(value, priv->base + src_reg, size);
 }
 
-static void ich_set_bbar(struct ich_ctlr *ctlr, uint32_t minaddr)
+static void ich_set_bbar(struct ich_spi_priv *ctlr, uint32_t minaddr)
 {
 	const uint32_t bbar_mask = 0x00ffff00;
 	uint32_t ichspi_bbar;
 
 	minaddr &= bbar_mask;
-	ichspi_bbar = ich_readl(ctlr->bbar) & ~bbar_mask;
+	ichspi_bbar = ich_readl(ctlr, ctlr->bbar) & ~bbar_mask;
 	ichspi_bbar |= minaddr;
-	ich_writel(ichspi_bbar, ctlr->bbar);
-}
-
-int spi_cs_is_valid(unsigned int bus, unsigned int cs)
-{
-	puts("spi_cs_is_valid used but not implemented\n");
-	return 0;
-}
-
-struct spi_slave *spi_setup_slave(unsigned int bus, unsigned int cs,
-		unsigned int max_hz, unsigned int mode)
-{
-	struct ich_spi_slave *ich;
-
-	ich = spi_alloc_slave(struct ich_spi_slave, bus, cs);
-	if (!ich) {
-		puts("ICH SPI: Out of memory\n");
-		return NULL;
-	}
-
-	/*
-	 * Yes this controller can only write a small number of bytes at
-	 * once! The limit is typically 64 bytes.
-	 */
-	ich->slave.max_write_size = ctlr.databytes;
-	ich->speed = max_hz;
-
-	/*
-	 * ICH 7 SPI controller only supports array read command
-	 * and byte program command for SST flash
-	 */
-	if (ctlr.ich_version == 7 || ctlr.use_sbase) {
-		ich->slave.op_mode_rx = SPI_OPM_RX_AS;
-		ich->slave.op_mode_tx = SPI_OPM_TX_BP;
-	}
-
-	return &ich->slave;
-}
-
-struct spi_slave *spi_setup_slave_fdt(const void *blob, int slave_node,
-				      int spi_node)
-{
-	/* We only support a single SPI at present */
-	return spi_setup_slave(0, 0, 20000000, 0);
-}
-
-void spi_free_slave(struct spi_slave *slave)
-{
-	struct ich_spi_slave *ich = to_ich_spi(slave);
-
-	free(ich);
+	ich_writel(ctlr, ichspi_bbar, ctlr->bbar);
 }
 
 /*
@@ -185,7 +131,8 @@ static int get_ich_version(uint16_t device_id)
 	     device_id <= PCI_DEVICE_ID_INTEL_COUGARPOINT_LPC_MAX) ||
 	    (device_id >= PCI_DEVICE_ID_INTEL_PANTHERPOINT_LPC_MIN &&
 	     device_id <= PCI_DEVICE_ID_INTEL_PANTHERPOINT_LPC_MAX) ||
-	    device_id == PCI_DEVICE_ID_INTEL_VALLEYVIEW_LPC)
+	    device_id == PCI_DEVICE_ID_INTEL_VALLEYVIEW_LPC ||
+	    device_id == PCI_DEVICE_ID_INTEL_LYNXPOINT_LPC)
 		return 9;
 
 	return 0;
@@ -208,7 +155,7 @@ static int ich9_can_do_33mhz(pci_dev_t dev)
 	return speed == 1;
 }
 
-static int ich_find_spi_controller(struct ich_ctlr *ich)
+static int ich_find_spi_controller(struct ich_spi_platdata *ich)
 {
 	int last_bus = pci_last_busno();
 	int bus;
@@ -241,131 +188,77 @@ static int ich_find_spi_controller(struct ich_ctlr *ich)
 	return -ENODEV;
 }
 
-static int ich_init_controller(struct ich_ctlr *ctlr)
+static int ich_init_controller(struct ich_spi_platdata *plat,
+			       struct ich_spi_priv *ctlr)
 {
 	uint8_t *rcrb; /* Root Complex Register Block */
 	uint32_t rcba; /* Root Complex Base Address */
 	uint32_t sbase_addr;
 	uint8_t *sbase;
 
-	pci_read_config_dword(ctlr->dev, 0xf0, &rcba);
+	pci_read_config_dword(plat->dev, 0xf0, &rcba);
 	/* Bits 31-14 are the base address, 13-1 are reserved, 0 is enable. */
 	rcrb = (uint8_t *)(rcba & 0xffffc000);
 
 	/* SBASE is similar */
-	pci_read_config_dword(ctlr->dev, 0x54, &sbase_addr);
+	pci_read_config_dword(plat->dev, 0x54, &sbase_addr);
 	sbase = (uint8_t *)(sbase_addr & 0xfffffe00);
 
-	if (ctlr->ich_version == 7) {
+	if (plat->ich_version == 7) {
 		struct ich7_spi_regs *ich7_spi;
 
 		ich7_spi = (struct ich7_spi_regs *)(rcrb + 0x3020);
-		ctlr->ichspi_lock = ich_readw(&ich7_spi->spis) & SPIS_LOCK;
-		ctlr->opmenu = ich7_spi->opmenu;
+		ctlr->ichspi_lock = readw(&ich7_spi->spis) & SPIS_LOCK;
+		ctlr->opmenu = offsetof(struct ich7_spi_regs, opmenu);
 		ctlr->menubytes = sizeof(ich7_spi->opmenu);
-		ctlr->optype = &ich7_spi->optype;
-		ctlr->addr = &ich7_spi->spia;
-		ctlr->data = (uint8_t *)ich7_spi->spid;
+		ctlr->optype = offsetof(struct ich7_spi_regs, optype);
+		ctlr->addr = offsetof(struct ich7_spi_regs, spia);
+		ctlr->data = offsetof(struct ich7_spi_regs, spid);
 		ctlr->databytes = sizeof(ich7_spi->spid);
-		ctlr->status = (uint8_t *)&ich7_spi->spis;
-		ctlr->control = &ich7_spi->spic;
-		ctlr->bbar = &ich7_spi->bbar;
-		ctlr->preop = &ich7_spi->preop;
+		ctlr->status = offsetof(struct ich7_spi_regs, spis);
+		ctlr->control = offsetof(struct ich7_spi_regs, spic);
+		ctlr->bbar = offsetof(struct ich7_spi_regs, bbar);
+		ctlr->preop = offsetof(struct ich7_spi_regs, preop);
 		ctlr->base = ich7_spi;
-	} else if (ctlr->ich_version == 9) {
+	} else if (plat->ich_version == 9) {
 		struct ich9_spi_regs *ich9_spi;
 
-		if (ctlr->use_sbase)
+		if (plat->use_sbase)
 			ich9_spi = (struct ich9_spi_regs *)sbase;
 		else
 			ich9_spi = (struct ich9_spi_regs *)(rcrb + 0x3800);
-		ctlr->ichspi_lock = ich_readw(&ich9_spi->hsfs) & HSFS_FLOCKDN;
-		ctlr->opmenu = ich9_spi->opmenu;
+		ctlr->ichspi_lock = readw(&ich9_spi->hsfs) & HSFS_FLOCKDN;
+		ctlr->opmenu = offsetof(struct ich9_spi_regs, opmenu);
 		ctlr->menubytes = sizeof(ich9_spi->opmenu);
-		ctlr->optype = &ich9_spi->optype;
-		ctlr->addr = &ich9_spi->faddr;
-		ctlr->data = (uint8_t *)ich9_spi->fdata;
+		ctlr->optype = offsetof(struct ich9_spi_regs, optype);
+		ctlr->addr = offsetof(struct ich9_spi_regs, faddr);
+		ctlr->data = offsetof(struct ich9_spi_regs, fdata);
 		ctlr->databytes = sizeof(ich9_spi->fdata);
-		ctlr->status = &ich9_spi->ssfs;
-		ctlr->control = (uint16_t *)ich9_spi->ssfc;
-		ctlr->speed = ich9_spi->ssfc + 2;
-		ctlr->bbar = &ich9_spi->bbar;
-		ctlr->preop = &ich9_spi->preop;
+		ctlr->status = offsetof(struct ich9_spi_regs, ssfs);
+		ctlr->control = offsetof(struct ich9_spi_regs, ssfc);
+		ctlr->speed = ctlr->control + 2;
+		ctlr->bbar = offsetof(struct ich9_spi_regs, bbar);
+		ctlr->preop = offsetof(struct ich9_spi_regs, preop);
 		ctlr->pr = &ich9_spi->pr[0];
 		ctlr->base = ich9_spi;
 	} else {
-		debug("ICH SPI: Unrecognized ICH version %d.\n",
-		      ctlr->ich_version);
-		return -1;
+		debug("ICH SPI: Unrecognised ICH version %d\n",
+		      plat->ich_version);
+		return -EINVAL;
 	}
 
 	/* Work out the maximum speed we can support */
 	ctlr->max_speed = 20000000;
-	if (ctlr->ich_version == 9 && ich9_can_do_33mhz(ctlr->dev))
+	if (plat->ich_version == 9 && ich9_can_do_33mhz(plat->dev))
 		ctlr->max_speed = 33000000;
 	debug("ICH SPI: Version %d detected at %p, speed %ld\n",
-	      ctlr->ich_version, ctlr->base, ctlr->max_speed);
+	      plat->ich_version, ctlr->base, ctlr->max_speed);
 
 	ich_set_bbar(ctlr, 0);
 
 	return 0;
 }
 
-void spi_init(void)
-{
-	uint8_t bios_cntl;
-
-	if (ich_find_spi_controller(&ctlr)) {
-		printf("ICH SPI: Cannot find device\n");
-		return;
-	}
-
-	if (ich_init_controller(&ctlr)) {
-		printf("ICH SPI: Cannot setup controller\n");
-		return;
-	}
-
-	/*
-	 * Disable the BIOS write protect so write commands are allowed.  On
-	 * v9, deassert SMM BIOS Write Protect Disable.
-	 */
-	if (ctlr.use_sbase) {
-		struct ich9_spi_regs *ich9_spi;
-
-		ich9_spi = (struct ich9_spi_regs *)ctlr.base;
-		bios_cntl = ich_readb(&ich9_spi->bcr);
-		bios_cntl &= ~(1 << 5);	/* clear Enable InSMM_STS (EISS) */
-		bios_cntl |= 1;		/* Write Protect Disable (WPD) */
-		ich_writeb(bios_cntl, &ich9_spi->bcr);
-	} else {
-		pci_read_config_byte(ctlr.dev, 0xdc, &bios_cntl);
-		if (ctlr.ich_version == 9)
-			bios_cntl &= ~(1 << 5);
-		pci_write_config_byte(ctlr.dev, 0xdc, bios_cntl | 0x1);
-	}
-}
-
-int spi_claim_bus(struct spi_slave *slave)
-{
-	/* Handled by ICH automatically. */
-	return 0;
-}
-
-void spi_release_bus(struct spi_slave *slave)
-{
-	/* Handled by ICH automatically. */
-}
-
-void spi_cs_activate(struct spi_slave *slave)
-{
-	/* Handled by ICH automatically. */
-}
-
-void spi_cs_deactivate(struct spi_slave *slave)
-{
-	/* Handled by ICH automatically. */
-}
-
 static inline void spi_use_out(struct spi_trans *trans, unsigned bytes)
 {
 	trans->out += bytes;
@@ -411,19 +304,19 @@ static void spi_setup_type(struct spi_trans *trans, int data_bytes)
 	}
 }
 
-static int spi_setup_opcode(struct spi_trans *trans)
+static int spi_setup_opcode(struct ich_spi_priv *ctlr, struct spi_trans *trans)
 {
 	uint16_t optypes;
-	uint8_t opmenu[ctlr.menubytes];
+	uint8_t opmenu[ctlr->menubytes];
 
 	trans->opcode = trans->out[0];
 	spi_use_out(trans, 1);
-	if (!ctlr.ichspi_lock) {
+	if (!ctlr->ichspi_lock) {
 		/* The lock is off, so just use index 0. */
-		ich_writeb(trans->opcode, ctlr.opmenu);
-		optypes = ich_readw(ctlr.optype);
+		ich_writeb(ctlr, trans->opcode, ctlr->opmenu);
+		optypes = ich_readw(ctlr, ctlr->optype);
 		optypes = (optypes & 0xfffc) | (trans->type & 0x3);
-		ich_writew(optypes, ctlr.optype);
+		ich_writew(ctlr, optypes, ctlr->optype);
 		return 0;
 	} else {
 		/* The lock is on. See if what we need is on the menu. */
@@ -434,20 +327,20 @@ static int spi_setup_opcode(struct spi_trans *trans)
 		if (trans->opcode == SPI_OPCODE_WREN)
 			return 0;
 
-		read_reg(ctlr.opmenu, opmenu, sizeof(opmenu));
-		for (opcode_index = 0; opcode_index < ctlr.menubytes;
+		read_reg(ctlr, ctlr->opmenu, opmenu, sizeof(opmenu));
+		for (opcode_index = 0; opcode_index < ctlr->menubytes;
 				opcode_index++) {
 			if (opmenu[opcode_index] == trans->opcode)
 				break;
 		}
 
-		if (opcode_index == ctlr.menubytes) {
+		if (opcode_index == ctlr->menubytes) {
 			printf("ICH SPI: Opcode %x not found\n",
 			       trans->opcode);
-			return -1;
+			return -EINVAL;
 		}
 
-		optypes = ich_readw(ctlr.optype);
+		optypes = ich_readw(ctlr, ctlr->optype);
 		optype = (optypes >> (opcode_index * 2)) & 0x3;
 		if (trans->type == SPI_OPCODE_TYPE_WRITE_NO_ADDRESS &&
 		    optype == SPI_OPCODE_TYPE_WRITE_WITH_ADDRESS &&
@@ -458,7 +351,7 @@ static int spi_setup_opcode(struct spi_trans *trans)
 		if (optype != trans->type) {
 			printf("ICH SPI: Transaction doesn't fit type %d\n",
 			       optype);
-			return -1;
+			return -ENOSPC;
 		}
 		return opcode_index;
 	}
@@ -480,7 +373,7 @@ static int spi_setup_offset(struct spi_trans *trans)
 		return 1;
 	default:
 		printf("Unrecognized SPI transaction type %#x\n", trans->type);
-		return -1;
+		return -EPROTO;
 	}
 }
 
@@ -491,16 +384,19 @@ static int spi_setup_offset(struct spi_trans *trans)
  *
  * Return the last read status value on success or -1 on failure.
  */
-static int ich_status_poll(u16 bitmask, int wait_til_set)
+static int ich_status_poll(struct ich_spi_priv *ctlr, u16 bitmask,
+			   int wait_til_set)
 {
 	int timeout = 600000; /* This will result in 6s */
 	u16 status = 0;
 
 	while (timeout--) {
-		status = ich_readw(ctlr.status);
+		status = ich_readw(ctlr, ctlr->status);
 		if (wait_til_set ^ ((status & bitmask) == 0)) {
-			if (wait_til_set)
-				ich_writew((status & bitmask), ctlr.status);
+			if (wait_til_set) {
+				ich_writew(ctlr, status & bitmask,
+					   ctlr->status);
+			}
 			return status;
 		}
 		udelay(10);
@@ -508,30 +404,28 @@ static int ich_status_poll(u16 bitmask, int wait_til_set)
 
 	printf("ICH SPI: SCIP timeout, read %x, expected %x\n",
 	       status, bitmask);
-	return -1;
+	return -ETIMEDOUT;
 }
 
-/*
-int spi_xfer(struct spi_slave *slave, const void *dout,
-		unsigned int bitsout, void *din, unsigned int bitsin)
-*/
-int spi_xfer(struct spi_slave *slave, unsigned int bitlen, const void *dout,
-		void *din, unsigned long flags)
+static int ich_spi_xfer(struct udevice *dev, unsigned int bitlen,
+			const void *dout, void *din, unsigned long flags)
 {
-	struct ich_spi_slave *ich = to_ich_spi(slave);
+	struct udevice *bus = dev_get_parent(dev);
+	struct ich_spi_priv *ctlr = dev_get_priv(bus);
 	uint16_t control;
 	int16_t opcode_index;
 	int with_address;
 	int status;
 	int bytes = bitlen / 8;
-	struct spi_trans *trans = &ich->trans;
+	struct spi_trans *trans = &ctlr->trans;
 	unsigned type = flags & (SPI_XFER_BEGIN | SPI_XFER_END);
 	int using_cmd = 0;
+	int ret;
 
 	/* Ee don't support writing partial bytes. */
 	if (bitlen % 8) {
 		debug("ICH SPI: Accessing partial bytes not supported\n");
-		return -1;
+		return -EPROTONOSUPPORT;
 	}
 
 	/* An empty end transaction can be ignored */
@@ -545,7 +439,7 @@ int spi_xfer(struct spi_slave *slave, unsigned int bitlen, const void *dout,
 	if (dout && type == SPI_XFER_BEGIN) {
 		if (bytes > ICH_MAX_CMD_LEN) {
 			debug("ICH SPI: Command length limit exceeded\n");
-			return -1;
+			return -ENOSPC;
 		}
 		memcpy(trans->cmd, dout, bytes);
 		trans->cmd_len = bytes;
@@ -576,21 +470,22 @@ int spi_xfer(struct spi_slave *slave, unsigned int bitlen, const void *dout,
 	/* There has to always at least be an opcode. */
 	if (!trans->bytesout) {
 		debug("ICH SPI: No opcode for transfer\n");
-		return -1;
+		return -EPROTO;
 	}
 
-	if (ich_status_poll(SPIS_SCIP, 0) == -1)
-		return -1;
+	ret = ich_status_poll(ctlr, SPIS_SCIP, 0);
+	if (ret < 0)
+		return ret;
 
-	ich_writew(SPIS_CDS | SPIS_FCERR, ctlr.status);
+	ich_writew(ctlr, SPIS_CDS | SPIS_FCERR, ctlr->status);
 
 	spi_setup_type(trans, using_cmd ? bytes : 0);
-	opcode_index = spi_setup_opcode(trans);
+	opcode_index = spi_setup_opcode(ctlr, trans);
 	if (opcode_index < 0)
-		return -1;
+		return -EINVAL;
 	with_address = spi_setup_offset(trans);
 	if (with_address < 0)
-		return -1;
+		return -EINVAL;
 
 	if (trans->opcode == SPI_OPCODE_WREN) {
 		/*
@@ -598,20 +493,20 @@ int spi_xfer(struct spi_slave *slave, unsigned int bitlen, const void *dout,
 		 * in order to prevent the Management Engine from
 		 * issuing a transaction between WREN and DATA.
 		 */
-		if (!ctlr.ichspi_lock)
-			ich_writew(trans->opcode, ctlr.preop);
+		if (!ctlr->ichspi_lock)
+			ich_writew(ctlr, trans->opcode, ctlr->preop);
 		return 0;
 	}
 
-	if (ctlr.speed && ctlr.max_speed >= 33000000) {
+	if (ctlr->speed && ctlr->max_speed >= 33000000) {
 		int byte;
 
-		byte = ich_readb(ctlr.speed);
-		if (ich->speed >= 33000000)
+		byte = ich_readb(ctlr, ctlr->speed);
+		if (ctlr->cur_speed >= 33000000)
 			byte |= SSFC_SCF_33MHZ;
 		else
 			byte &= ~SSFC_SCF_33MHZ;
-		ich_writeb(byte, ctlr.speed);
+		ich_writeb(ctlr, byte, ctlr->speed);
 	}
 
 	/* See if we have used up the command data */
@@ -622,35 +517,36 @@ int spi_xfer(struct spi_slave *slave, unsigned int bitlen, const void *dout,
 	}
 
 	/* Preset control fields */
-	control = ich_readw(ctlr.control);
+	control = ich_readw(ctlr, ctlr->control);
 	control &= ~SSFC_RESERVED;
 	control = SPIC_SCGO | ((opcode_index & 0x07) << 4);
 
 	/* Issue atomic preop cycle if needed */
-	if (ich_readw(ctlr.preop))
+	if (ich_readw(ctlr, ctlr->preop))
 		control |= SPIC_ACS;
 
 	if (!trans->bytesout && !trans->bytesin) {
 		/* SPI addresses are 24 bit only */
-		if (with_address)
-			ich_writel(trans->offset & 0x00FFFFFF, ctlr.addr);
-
+		if (with_address) {
+			ich_writel(ctlr, trans->offset & 0x00FFFFFF,
+				   ctlr->addr);
+		}
 		/*
 		 * This is a 'no data' command (like Write Enable), its
 		 * bitesout size was 1, decremented to zero while executing
 		 * spi_setup_opcode() above. Tell the chip to send the
 		 * command.
 		 */
-		ich_writew(control, ctlr.control);
+		ich_writew(ctlr, control, ctlr->control);
 
 		/* wait for the result */
-		status = ich_status_poll(SPIS_CDS | SPIS_FCERR, 1);
-		if (status == -1)
-			return -1;
+		status = ich_status_poll(ctlr, SPIS_CDS | SPIS_FCERR, 1);
+		if (status < 0)
+			return status;
 
 		if (status & SPIS_FCERR) {
 			debug("ICH SPI: Command transaction error\n");
-			return -1;
+			return -EIO;
 		}
 
 		return 0;
@@ -663,9 +559,9 @@ int spi_xfer(struct spi_slave *slave, unsigned int bitlen, const void *dout,
 	 * and followed by other SPI commands, and this sequence is controlled
 	 * by the SPI chip driver.
 	 */
-	if (trans->bytesout > ctlr.databytes) {
+	if (trans->bytesout > ctlr->databytes) {
 		debug("ICH SPI: Too much to write. This should be prevented by the driver's max_write_size?\n");
-		return -1;
+		return -EPROTO;
 	}
 
 	/*
@@ -676,41 +572,41 @@ int spi_xfer(struct spi_slave *slave, unsigned int bitlen, const void *dout,
 		uint32_t data_length;
 
 		/* SPI addresses are 24 bit only */
-		ich_writel(trans->offset & 0x00FFFFFF, ctlr.addr);
+		ich_writel(ctlr, trans->offset & 0x00FFFFFF, ctlr->addr);
 
 		if (trans->bytesout)
-			data_length = min(trans->bytesout, ctlr.databytes);
+			data_length = min(trans->bytesout, ctlr->databytes);
 		else
-			data_length = min(trans->bytesin, ctlr.databytes);
+			data_length = min(trans->bytesin, ctlr->databytes);
 
 		/* Program data into FDATA0 to N */
 		if (trans->bytesout) {
-			write_reg(trans->out, ctlr.data, data_length);
+			write_reg(ctlr, trans->out, ctlr->data, data_length);
 			spi_use_out(trans, data_length);
 			if (with_address)
 				trans->offset += data_length;
 		}
 
 		/* Add proper control fields' values */
-		control &= ~((ctlr.databytes - 1) << 8);
+		control &= ~((ctlr->databytes - 1) << 8);
 		control |= SPIC_DS;
 		control |= (data_length - 1) << 8;
 
 		/* write it */
-		ich_writew(control, ctlr.control);
+		ich_writew(ctlr, control, ctlr->control);
 
 		/* Wait for Cycle Done Status or Flash Cycle Error. */
-		status = ich_status_poll(SPIS_CDS | SPIS_FCERR, 1);
-		if (status == -1)
-			return -1;
+		status = ich_status_poll(ctlr, SPIS_CDS | SPIS_FCERR, 1);
+		if (status < 0)
+			return status;
 
 		if (status & SPIS_FCERR) {
 			debug("ICH SPI: Data transaction error\n");
-			return -1;
+			return -EIO;
 		}
 
 		if (trans->bytesin) {
-			read_reg(ctlr.data, trans->in, data_length);
+			read_reg(ctlr, ctlr->data, trans->in, data_length);
 			spi_use_in(trans, data_length);
 			if (with_address)
 				trans->offset += data_length;
@@ -718,7 +614,7 @@ int spi_xfer(struct spi_slave *slave, unsigned int bitlen, const void *dout,
 	}
 
 	/* Clear atomic preop now that xfer is done */
-	ich_writew(0, ctlr.preop);
+	ich_writew(ctlr, 0, ctlr->preop);
 
 	return 0;
 }
@@ -730,15 +626,18 @@ int spi_xfer(struct spi_slave *slave, unsigned int bitlen, const void *dout,
  * don't actually take effect until the HSFS[FLOCKDN] bit is set, but that's
  * done elsewhere.
  */
-int spi_write_protect_region(uint32_t lower_limit, uint32_t length, int hint)
+int spi_write_protect_region(struct udevice *dev, uint32_t lower_limit,
+			     uint32_t length, int hint)
 {
+	struct udevice *bus = dev->parent;
+	struct ich_spi_priv *ctlr = dev_get_priv(bus);
 	uint32_t tmplong;
 	uint32_t upper_limit;
 
-	if (!ctlr.pr) {
+	if (!ctlr->pr) {
 		printf("%s: operation not supported on this chipset\n",
 		       __func__);
-		return -1;
+		return -ENOSYS;
 	}
 
 	if (length == 0 ||
@@ -746,7 +645,7 @@ int spi_write_protect_region(uint32_t lower_limit, uint32_t length, int hint)
 	    hint < 0 || hint > 4) {
 		printf("%s(0x%x, 0x%x, %d): invalid args\n", __func__,
 		       lower_limit, length, hint);
-		return -1;
+		return -EPERM;
 	}
 
 	upper_limit = lower_limit + length - 1;
@@ -765,8 +664,121 @@ int spi_write_protect_region(uint32_t lower_limit, uint32_t length, int hint)
 		((lower_limit & 0x01fff000) >> 12);
 
 	printf("%s: writing 0x%08x to %p\n", __func__, tmplong,
-	       &ctlr.pr[hint]);
-	ctlr.pr[hint] = tmplong;
+	       &ctlr->pr[hint]);
+	ctlr->pr[hint] = tmplong;
+
+	return 0;
+}
+
+static int ich_spi_probe(struct udevice *bus)
+{
+	struct ich_spi_platdata *plat = dev_get_platdata(bus);
+	struct ich_spi_priv *priv = dev_get_priv(bus);
+	uint8_t bios_cntl;
+	int ret;
+
+	ret = ich_init_controller(plat, priv);
+	if (ret)
+		return ret;
+	/*
+	 * Disable the BIOS write protect so write commands are allowed.  On
+	 * v9, deassert SMM BIOS Write Protect Disable.
+	 */
+	if (plat->use_sbase) {
+		struct ich9_spi_regs *ich9_spi;
+
+		ich9_spi = priv->base;
+		bios_cntl = ich_readb(priv, ich9_spi->bcr);
+		bios_cntl &= ~(1 << 5);	/* clear Enable InSMM_STS (EISS) */
+		bios_cntl |= 1;		/* Write Protect Disable (WPD) */
+		ich_writeb(priv, bios_cntl, ich9_spi->bcr);
+	} else {
+		pci_read_config_byte(plat->dev, 0xdc, &bios_cntl);
+		if (plat->ich_version == 9)
+			bios_cntl &= ~(1 << 5);
+		pci_write_config_byte(plat->dev, 0xdc, bios_cntl | 0x1);
+	}
+
+	priv->cur_speed = priv->max_speed;
+
+	return 0;
+}
+
+static int ich_spi_ofdata_to_platdata(struct udevice *bus)
+{
+	struct ich_spi_platdata *plat = dev_get_platdata(bus);
+	int ret;
+
+	ret = ich_find_spi_controller(plat);
+	if (ret)
+		return ret;
 
 	return 0;
 }
+
+static int ich_spi_set_speed(struct udevice *bus, uint speed)
+{
+	struct ich_spi_priv *priv = dev_get_priv(bus);
+
+	priv->cur_speed = speed;
+
+	return 0;
+}
+
+static int ich_spi_set_mode(struct udevice *bus, uint mode)
+{
+	debug("%s: mode=%d\n", __func__, mode);
+
+	return 0;
+}
+
+static int ich_spi_child_pre_probe(struct udevice *dev)
+{
+	struct udevice *bus = dev_get_parent(dev);
+	struct ich_spi_platdata *plat = dev_get_platdata(bus);
+	struct ich_spi_priv *priv = dev_get_priv(bus);
+	struct spi_slave *slave = dev_get_parentdata(dev);
+
+	/*
+	 * Yes this controller can only write a small number of bytes at
+	 * once! The limit is typically 64 bytes.
+	 */
+	slave->max_write_size = priv->databytes;
+	/*
+	 * ICH 7 SPI controller only supports array read command
+	 * and byte program command for SST flash
+	 */
+	if (plat->ich_version == 7) {
+		slave->op_mode_rx = SPI_OPM_RX_AS;
+		slave->op_mode_tx = SPI_OPM_TX_BP;
+	}
+
+	return 0;
+}
+
+static const struct dm_spi_ops ich_spi_ops = {
+	.xfer		= ich_spi_xfer,
+	.set_speed	= ich_spi_set_speed,
+	.set_mode	= ich_spi_set_mode,
+	/*
+	 * cs_info is not needed, since we require all chip selects to be
+	 * in the device tree explicitly
+	 */
+};
+
+static const struct udevice_id ich_spi_ids[] = {
+	{ .compatible = "intel,ich-spi" },
+	{ }
+};
+
+U_BOOT_DRIVER(ich_spi) = {
+	.name	= "ich_spi",
+	.id	= UCLASS_SPI,
+	.of_match = ich_spi_ids,
+	.ops	= &ich_spi_ops,
+	.ofdata_to_platdata = ich_spi_ofdata_to_platdata,
+	.platdata_auto_alloc_size = sizeof(struct ich_spi_platdata),
+	.priv_auto_alloc_size = sizeof(struct ich_spi_priv),
+	.child_pre_probe = ich_spi_child_pre_probe,
+	.probe	= ich_spi_probe,
+};
diff --git a/drivers/spi/omap3_spi.c b/drivers/spi/omap3_spi.c
index 651e46e..85f9e85 100644
--- a/drivers/spi/omap3_spi.c
+++ b/drivers/spi/omap3_spi.c
@@ -20,7 +20,7 @@
 #include <asm/io.h>
 #include "omap3_spi.h"
 
-#define SPI_WAIT_TIMEOUT 3000000
+#define SPI_WAIT_TIMEOUT 10
 
 static void spi_reset(struct omap3_spi_slave *ds)
 {
@@ -227,7 +227,7 @@ int omap3_spi_write(struct spi_slave *slave, unsigned int len, const void *txp,
 {
 	struct omap3_spi_slave *ds = to_omap3_spi(slave);
 	int i;
-	int timeout = SPI_WAIT_TIMEOUT;
+	ulong start;
 	int chconf = readl(&ds->regs->channel[ds->slave.cs].chconf);
 
 	/* Enable the channel */
@@ -241,9 +241,10 @@ int omap3_spi_write(struct spi_slave *slave, unsigned int len, const void *txp,
 
 	for (i = 0; i < len; i++) {
 		/* wait till TX register is empty (TXS == 1) */
+		start = get_timer(0);
 		while (!(readl(&ds->regs->channel[ds->slave.cs].chstat) &
 			 OMAP3_MCSPI_CHSTAT_TXS)) {
-			if (--timeout <= 0) {
+			if (get_timer(start) > SPI_WAIT_TIMEOUT) {
 				printf("SPI TXS timed out, status=0x%08x\n",
 				       readl(&ds->regs->channel[ds->slave.cs].chstat));
 				return -1;
@@ -280,7 +281,7 @@ int omap3_spi_read(struct spi_slave *slave, unsigned int len, void *rxp,
 {
 	struct omap3_spi_slave *ds = to_omap3_spi(slave);
 	int i;
-	int timeout = SPI_WAIT_TIMEOUT;
+	ulong start;
 	int chconf = readl(&ds->regs->channel[ds->slave.cs].chconf);
 
 	/* Enable the channel */
@@ -295,10 +296,11 @@ int omap3_spi_read(struct spi_slave *slave, unsigned int len, void *rxp,
 	writel(0, &ds->regs->channel[ds->slave.cs].tx);
 
 	for (i = 0; i < len; i++) {
+		start = get_timer(0);
 		/* Wait till RX register contains data (RXS == 1) */
 		while (!(readl(&ds->regs->channel[ds->slave.cs].chstat) &
 			 OMAP3_MCSPI_CHSTAT_RXS)) {
-			if (--timeout <= 0) {
+			if (get_timer(start) > SPI_WAIT_TIMEOUT) {
 				printf("SPI RXS timed out, status=0x%08x\n",
 				       readl(&ds->regs->channel[ds->slave.cs].chstat));
 				return -1;
@@ -332,7 +334,7 @@ int omap3_spi_txrx(struct spi_slave *slave, unsigned int len,
 		   const void *txp, void *rxp, unsigned long flags)
 {
 	struct omap3_spi_slave *ds = to_omap3_spi(slave);
-	int timeout = SPI_WAIT_TIMEOUT;
+	ulong start;
 	int chconf = readl(&ds->regs->channel[ds->slave.cs].chconf);
 	int irqstatus = readl(&ds->regs->irqstatus);
 	int i=0;
@@ -350,9 +352,10 @@ int omap3_spi_txrx(struct spi_slave *slave, unsigned int len,
 	for (i=0; i < len; i++){
 		/* Write: wait for TX empty (TXS == 1)*/
 		irqstatus |= (1<< (4*(ds->slave.bus)));
+		start = get_timer(0);
 		while (!(readl(&ds->regs->channel[ds->slave.cs].chstat) &
 			 OMAP3_MCSPI_CHSTAT_TXS)) {
-			if (--timeout <= 0) {
+			if (get_timer(start) > SPI_WAIT_TIMEOUT) {
 				printf("SPI TXS timed out, status=0x%08x\n",
 				       readl(&ds->regs->channel[ds->slave.cs].chstat));
 				return -1;
@@ -368,9 +371,10 @@ int omap3_spi_txrx(struct spi_slave *slave, unsigned int len,
 			writel(((u8 *)txp)[i], tx);
 
 		/*Read: wait for RX containing data (RXS == 1)*/
+		start = get_timer(0);
 		while (!(readl(&ds->regs->channel[ds->slave.cs].chstat) &
 			 OMAP3_MCSPI_CHSTAT_RXS)) {
-			if (--timeout <= 0) {
+			if (get_timer(start) > SPI_WAIT_TIMEOUT) {
 				printf("SPI RXS timed out, status=0x%08x\n",
 				       readl(&ds->regs->channel[ds->slave.cs].chstat));
 				return -1;
diff --git a/drivers/spi/spi-uclass.c b/drivers/spi/spi-uclass.c
index 63a6217..83fe8e0 100644
--- a/drivers/spi/spi-uclass.c
+++ b/drivers/spi/spi-uclass.c
@@ -50,7 +50,7 @@ int spi_claim_bus(struct spi_slave *slave)
 	struct udevice *dev = slave->dev;
 	struct udevice *bus = dev->parent;
 	struct dm_spi_ops *ops = spi_get_ops(bus);
-	struct dm_spi_bus *spi = bus->uclass_priv;
+	struct dm_spi_bus *spi = dev_get_uclass_priv(bus);
 	int speed;
 	int ret;
 
@@ -67,7 +67,7 @@ int spi_claim_bus(struct spi_slave *slave)
 	if (ret)
 		return ret;
 
-	return ops->claim_bus ? ops->claim_bus(bus) : 0;
+	return ops->claim_bus ? ops->claim_bus(dev) : 0;
 }
 
 void spi_release_bus(struct spi_slave *slave)
@@ -77,7 +77,7 @@ void spi_release_bus(struct spi_slave *slave)
 	struct dm_spi_ops *ops = spi_get_ops(bus);
 
 	if (ops->release_bus)
-		ops->release_bus(bus);
+		ops->release_bus(dev);
 }
 
 int spi_xfer(struct spi_slave *slave, unsigned int bitlen,
@@ -110,7 +110,7 @@ int spi_child_post_bind(struct udevice *dev)
 
 int spi_post_probe(struct udevice *bus)
 {
-	struct dm_spi_bus *spi = bus->uclass_priv;
+	struct dm_spi_bus *spi = dev_get_uclass_priv(bus);
 
 	spi->max_hz = fdtdec_get_int(gd->fdt_blob, bus->of_offset,
 				     "spi-max-frequency", 0);
diff --git a/drivers/spi/tegra114_spi.c b/drivers/spi/tegra114_spi.c
index 53ff9ea..4bec663 100644
--- a/drivers/spi/tegra114_spi.c
+++ b/drivers/spi/tegra114_spi.c
@@ -153,8 +153,9 @@ static int tegra114_spi_probe(struct udevice *bus)
 	return 0;
 }
 
-static int tegra114_spi_claim_bus(struct udevice *bus)
+static int tegra114_spi_claim_bus(struct udevice *dev)
 {
+	struct udevice *bus = dev->parent;
 	struct tegra114_spi_priv *priv = dev_get_priv(bus);
 	struct spi_regs *regs = priv->regs;
 
diff --git a/drivers/spi/tegra20_sflash.c b/drivers/spi/tegra20_sflash.c
index 78c74cd..82c1b84 100644
--- a/drivers/spi/tegra20_sflash.c
+++ b/drivers/spi/tegra20_sflash.c
@@ -125,8 +125,9 @@ static int tegra20_sflash_probe(struct udevice *bus)
 	return 0;
 }
 
-static int tegra20_sflash_claim_bus(struct udevice *bus)
+static int tegra20_sflash_claim_bus(struct udevice *dev)
 {
+	struct udevice *bus = dev->parent;
 	struct tegra20_sflash_priv *priv = dev_get_priv(bus);
 	struct spi_regs *regs = priv->regs;
 	u32 reg;
diff --git a/drivers/spi/tegra20_slink.c b/drivers/spi/tegra20_slink.c
index 597d6ad..f6fb89b 100644
--- a/drivers/spi/tegra20_slink.c
+++ b/drivers/spi/tegra20_slink.c
@@ -141,8 +141,9 @@ static int tegra30_spi_probe(struct udevice *bus)
 	return 0;
 }
 
-static int tegra30_spi_claim_bus(struct udevice *bus)
+static int tegra30_spi_claim_bus(struct udevice *dev)
 {
+	struct udevice *bus = dev->parent;
 	struct tegra30_spi_priv *priv = dev_get_priv(bus);
 	struct spi_regs *regs = priv->regs;
 	u32 reg;
diff --git a/drivers/spi/zynq_spi.c b/drivers/spi/zynq_spi.c
index 5da8759..e9129da 100644
--- a/drivers/spi/zynq_spi.c
+++ b/drivers/spi/zynq_spi.c
@@ -227,9 +227,6 @@ int spi_xfer(struct spi_slave *slave, unsigned int bitlen, const void *dout,
 	debug("spi_xfer: bus:%i cs:%i bitlen:%i len:%i flags:%lx\n",
 	      slave->bus, slave->cs, bitlen, len, flags);
 
-	if (bitlen == 0)
-		return -1;
-
 	if (bitlen % 8) {
 		debug("spi_xfer: Non byte aligned SPI transfer\n");
 		return -1;
diff --git a/drivers/tpm/Kconfig b/drivers/tpm/Kconfig
index e69de29..f408b8a 100644
--- a/drivers/tpm/Kconfig
+++ b/drivers/tpm/Kconfig
@@ -0,0 +1,7 @@
+config TPM_TIS_SANDBOX
+	bool "Enable sandbox TPM driver"
+	help
+	  This driver emulates a TPM, providing access to base functions
+	  such as reading and writing TPM private data. This is enough to
+	  support Chrome OS verified boot. Extend functionality is not
+	  implemented.
diff --git a/drivers/usb/Kconfig b/drivers/usb/Kconfig
index b4a9442..637ef3d 100644
--- a/drivers/usb/Kconfig
+++ b/drivers/usb/Kconfig
@@ -35,8 +35,24 @@ config USB
 
 if USB
 
+config DM_USB
+	bool "Enable driver model for USB"
+	depends on USB && DM
+	help
+	  Enable driver model for USB. The USB interface is then implemented
+	  by the USB uclass. Multiple USB controllers of different types
+	  (XHCI, EHCI) can be attached and used. The 'usb' command works as
+	  normal. OCHI is not supported at present.
+
+	  Much of the code is shared but with this option enabled the USB
+	  uclass takes care of device enumeration. USB devices can be
+	  declared with the USB_DEVICE() macro and will be automatically
+	  probed when found on the bus.
+
 source "drivers/usb/host/Kconfig"
 
+source "drivers/usb/emul/Kconfig"
+
 config USB_STORAGE
 	bool "USB Mass Storage support"
 	---help---
diff --git a/drivers/usb/emul/Kconfig b/drivers/usb/emul/Kconfig
new file mode 100644
index 0000000..ae1ab23
--- /dev/null
+++ b/drivers/usb/emul/Kconfig
@@ -0,0 +1,8 @@
+config USB_EMUL
+	bool "Support for USB device emulation"
+	depends on DM_USB && SANDBOX
+	help
+	  Since sandbox does not have access to a real USB bus, it is possible
+	  to use device emulators instead. This allows testing of the USB
+	  stack on sandbox without needing a real device, or any host machine
+	  USB resources.
diff --git a/drivers/usb/emul/Makefile b/drivers/usb/emul/Makefile
new file mode 100644
index 0000000..8fd83d5
--- /dev/null
+++ b/drivers/usb/emul/Makefile
@@ -0,0 +1,10 @@
+#
+# (C) Copyright 2015 Google, Inc
+# Written by Simon Glass <sjg@chromium.org>
+#
+# SPDX-License-Identifier:	GPL-2.0+
+#
+
+obj-$(CONFIG_USB_EMUL) += sandbox_flash.o
+obj-$(CONFIG_USB_EMUL) += sandbox_hub.o
+obj-$(CONFIG_USB_EMUL) += usb-emul-uclass.o
diff --git a/drivers/usb/emul/sandbox_flash.c b/drivers/usb/emul/sandbox_flash.c
new file mode 100644
index 0000000..6e0808d
--- /dev/null
+++ b/drivers/usb/emul/sandbox_flash.c
@@ -0,0 +1,423 @@
+/*
+ * (C) Copyright 2015 Google, Inc
+ * Written by Simon Glass <sjg@chromium.org>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <dm.h>
+#include <os.h>
+#include <scsi.h>
+#include <usb.h>
+
+DECLARE_GLOBAL_DATA_PTR;
+
+/*
+ * This driver emulates a flash stick using the UFI command specification and
+ * the BBB (bulk/bulk/bulk) protocol. It supports only a single logical unit
+ * number (LUN 0).
+ */
+
+enum {
+	SANDBOX_FLASH_EP_OUT		= 1,	/* endpoints */
+	SANDBOX_FLASH_EP_IN		= 2,
+	SANDBOX_FLASH_BLOCK_LEN		= 512,
+};
+
+enum cmd_phase {
+	PHASE_START,
+	PHASE_DATA,
+	PHASE_STATUS,
+};
+
+/**
+ * struct sandbox_flash_priv - private state for this driver
+ *
+ * @error:	true if there is an error condition
+ * @alloc_len:	Allocation length from the last incoming command
+ * @transfer_len: Transfer length from CBW header
+ * @read_len:	Number of blocks of data left in the current read command
+ * @tag:	Tag value from last command
+ * @fd:		File descriptor of backing file
+ * @file_size:	Size of file in bytes
+ * @status_buff:	Data buffer for outgoing status
+ * @buff_used:	Number of bytes ready to transfer back to host
+ * @buff:	Data buffer for outgoing data
+ */
+struct sandbox_flash_priv {
+	bool error;
+	int alloc_len;
+	int transfer_len;
+	int read_len;
+	enum cmd_phase phase;
+	u32 tag;
+	int fd;
+	loff_t file_size;
+	struct umass_bbb_csw status;
+	int buff_used;
+	u8 buff[512];
+};
+
+struct sandbox_flash_plat {
+	const char *pathname;
+};
+
+struct scsi_inquiry_resp {
+	u8 type;
+	u8 flags;
+	u8 version;
+	u8 data_format;
+	u8 additional_len;
+	u8 spare[3];
+	char vendor[8];
+	char product[16];
+	char revision[4];
+};
+
+struct scsi_read_capacity_resp {
+	u32 last_block_addr;
+	u32 block_len;
+};
+
+struct __packed scsi_read10_req {
+	u8 cmd;
+	u8 lun_flags;
+	u32 lba;
+	u8 spare;
+	u16 transfer_len;
+	u8 spare2[3];
+};
+
+enum {
+	STRINGID_MANUFACTURER = 1,
+	STRINGID_PRODUCT,
+	STRINGID_SERIAL,
+
+	STRINGID_COUNT,
+};
+
+static struct usb_string flash_strings[] = {
+	{STRINGID_MANUFACTURER,	"sandbox"},
+	{STRINGID_PRODUCT,	"flash"},
+	{STRINGID_SERIAL,	"2345"},
+	{},
+};
+
+static struct usb_device_descriptor flash_device_desc = {
+	.bLength =		sizeof(flash_device_desc),
+	.bDescriptorType =	USB_DT_DEVICE,
+
+	.bcdUSB =		__constant_cpu_to_le16(0x0200),
+
+	.bDeviceClass =		0,
+	.bDeviceSubClass =	0,
+	.bDeviceProtocol =	0,
+
+	.idVendor =		__constant_cpu_to_le16(0x1234),
+	.idProduct =		__constant_cpu_to_le16(0x5678),
+	.iManufacturer =	STRINGID_MANUFACTURER,
+	.iProduct =		STRINGID_PRODUCT,
+	.iSerialNumber =	STRINGID_SERIAL,
+	.bNumConfigurations =	1,
+};
+
+static struct usb_config_descriptor flash_config0 = {
+	.bLength		= sizeof(flash_config0),
+	.bDescriptorType	= USB_DT_CONFIG,
+
+	/* wTotalLength is set up by usb-emul-uclass */
+	.bNumInterfaces		= 1,
+	.bConfigurationValue	= 0,
+	.iConfiguration		= 0,
+	.bmAttributes		= 1 << 7,
+	.bMaxPower		= 50,
+};
+
+static struct usb_interface_descriptor flash_interface0 = {
+	.bLength		= sizeof(flash_interface0),
+	.bDescriptorType	= USB_DT_INTERFACE,
+
+	.bInterfaceNumber	= 0,
+	.bAlternateSetting	= 0,
+	.bNumEndpoints		= 2,
+	.bInterfaceClass	= USB_CLASS_MASS_STORAGE,
+	.bInterfaceSubClass	= US_SC_UFI,
+	.bInterfaceProtocol	= US_PR_BULK,
+	.iInterface		= 0,
+};
+
+static struct usb_endpoint_descriptor flash_endpoint0_out = {
+	.bLength		= USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType	= USB_DT_ENDPOINT,
+
+	.bEndpointAddress	= SANDBOX_FLASH_EP_OUT,
+	.bmAttributes		= USB_ENDPOINT_XFER_BULK,
+	.wMaxPacketSize		= __constant_cpu_to_le16(1024),
+	.bInterval		= 0,
+};
+
+static struct usb_endpoint_descriptor flash_endpoint1_in = {
+	.bLength		= USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType	= USB_DT_ENDPOINT,
+
+	.bEndpointAddress	= SANDBOX_FLASH_EP_IN | USB_ENDPOINT_DIR_MASK,
+	.bmAttributes		= USB_ENDPOINT_XFER_BULK,
+	.wMaxPacketSize		= __constant_cpu_to_le16(1024),
+	.bInterval		= 0,
+};
+
+static void *flash_desc_list[] = {
+	&flash_device_desc,
+	&flash_config0,
+	&flash_interface0,
+	&flash_endpoint0_out,
+	&flash_endpoint1_in,
+	NULL,
+};
+
+static int sandbox_flash_control(struct udevice *dev, struct usb_device *udev,
+				 unsigned long pipe, void *buff, int len,
+				 struct devrequest *setup)
+{
+	struct sandbox_flash_priv *priv = dev_get_priv(dev);
+
+	if (pipe == usb_rcvctrlpipe(udev, 0)) {
+		switch (setup->request) {
+		case US_BBB_RESET:
+			priv->error = false;
+			return 0;
+		case US_BBB_GET_MAX_LUN:
+			*(char *)buff = '\0';
+			return 1;
+		default:
+			debug("request=%x\n", setup->request);
+			break;
+		}
+	}
+	debug("pipe=%lx\n", pipe);
+
+	return -EIO;
+}
+
+static void setup_fail_response(struct sandbox_flash_priv *priv)
+{
+	struct umass_bbb_csw *csw = &priv->status;
+
+	csw->dCSWSignature = CSWSIGNATURE;
+	csw->dCSWTag = priv->tag;
+	csw->dCSWDataResidue = 0;
+	csw->bCSWStatus = CSWSTATUS_FAILED;
+	priv->buff_used = 0;
+}
+
+/**
+ * setup_response() - set up a response to send back to the host
+ *
+ * @priv:	Sandbox flash private data
+ * @resp:	Response to send, or NULL if none
+ * @size:	Size of response
+ */
+static void setup_response(struct sandbox_flash_priv *priv, void *resp,
+			   int size)
+{
+	struct umass_bbb_csw *csw = &priv->status;
+
+	csw->dCSWSignature = CSWSIGNATURE;
+	csw->dCSWTag = priv->tag;
+	csw->dCSWDataResidue = 0;
+	csw->bCSWStatus = CSWSTATUS_GOOD;
+
+	assert(!resp || resp == priv->buff);
+	priv->buff_used = size;
+}
+
+static void handle_read(struct sandbox_flash_priv *priv, ulong lba,
+			ulong transfer_len)
+{
+	debug("%s: lba=%lx, transfer_len=%lx\n", __func__, lba, transfer_len);
+	if (priv->fd != -1) {
+		os_lseek(priv->fd, lba * SANDBOX_FLASH_BLOCK_LEN, OS_SEEK_SET);
+		priv->read_len = transfer_len;
+		setup_response(priv, priv->buff,
+			       transfer_len * SANDBOX_FLASH_BLOCK_LEN);
+	} else {
+		setup_fail_response(priv);
+	}
+}
+
+static int handle_ufi_command(struct sandbox_flash_priv *priv, const void *buff,
+			      int len)
+{
+	const struct SCSI_cmd_block *req = buff;
+
+	switch (*req->cmd) {
+	case SCSI_INQUIRY: {
+		struct scsi_inquiry_resp *resp = (void *)priv->buff;
+
+		priv->alloc_len = req->cmd[4];
+		memset(resp, '\0', sizeof(*resp));
+		resp->data_format = 1;
+		resp->additional_len = 0x1f;
+		strncpy(resp->vendor,
+			flash_strings[STRINGID_MANUFACTURER -  1].s,
+			sizeof(resp->vendor));
+		strncpy(resp->product, flash_strings[STRINGID_PRODUCT - 1].s,
+			sizeof(resp->product));
+		strncpy(resp->revision, "1.0", sizeof(resp->revision));
+		setup_response(priv, resp, sizeof(*resp));
+		break;
+	}
+	case SCSI_TST_U_RDY:
+		setup_response(priv, NULL, 0);
+		break;
+	case SCSI_RD_CAPAC: {
+		struct scsi_read_capacity_resp *resp = (void *)priv->buff;
+		uint blocks;
+
+		if (priv->file_size)
+			blocks = priv->file_size / SANDBOX_FLASH_BLOCK_LEN - 1;
+		else
+			blocks = 0;
+		resp->last_block_addr = cpu_to_be32(blocks);
+		resp->block_len = cpu_to_be32(SANDBOX_FLASH_BLOCK_LEN);
+		setup_response(priv, resp, sizeof(*resp));
+		break;
+	}
+	case SCSI_READ10: {
+		struct scsi_read10_req *req = (void *)buff;
+
+		handle_read(priv, be32_to_cpu(req->lba),
+			    be16_to_cpu(req->transfer_len));
+		break;
+	}
+	default:
+		debug("Command not supported: %x\n", req->cmd[0]);
+		return -EPROTONOSUPPORT;
+	}
+
+	priv->phase = priv->transfer_len ? PHASE_DATA : PHASE_STATUS;
+	return 0;
+}
+
+static int sandbox_flash_bulk(struct udevice *dev, struct usb_device *udev,
+			      unsigned long pipe, void *buff, int len)
+{
+	struct sandbox_flash_priv *priv = dev_get_priv(dev);
+	int ep = usb_pipeendpoint(pipe);
+	struct umass_bbb_cbw *cbw = buff;
+
+	debug("%s: dev=%s, pipe=%lx, ep=%x, len=%x, phase=%d\n", __func__,
+	      dev->name, pipe, ep, len, priv->phase);
+	switch (ep) {
+	case SANDBOX_FLASH_EP_OUT:
+		switch (priv->phase) {
+		case PHASE_START:
+			priv->alloc_len = 0;
+			priv->read_len = 0;
+			if (priv->error || len != UMASS_BBB_CBW_SIZE ||
+			    cbw->dCBWSignature != CBWSIGNATURE)
+				goto err;
+			if ((cbw->bCBWFlags & CBWFLAGS_SBZ) ||
+			    cbw->bCBWLUN != 0)
+				goto err;
+			if (cbw->bCDBLength < 1 || cbw->bCDBLength >= 0x10)
+				goto err;
+			priv->transfer_len = cbw->dCBWDataTransferLength;
+			priv->tag = cbw->dCBWTag;
+			return handle_ufi_command(priv, cbw->CBWCDB,
+						  cbw->bCDBLength);
+		case PHASE_DATA:
+			debug("data out\n");
+			break;
+		default:
+			break;
+		}
+	case SANDBOX_FLASH_EP_IN:
+		switch (priv->phase) {
+		case PHASE_DATA:
+			debug("data in, len=%x, alloc_len=%x, priv->read_len=%x\n",
+			      len, priv->alloc_len, priv->read_len);
+			if (priv->read_len) {
+				ulong bytes_read;
+
+				bytes_read = os_read(priv->fd, buff, len);
+				if (bytes_read != len)
+					return -EIO;
+				priv->read_len -= len / SANDBOX_FLASH_BLOCK_LEN;
+				if (!priv->read_len)
+					priv->phase = PHASE_STATUS;
+			} else {
+				if (priv->alloc_len && len > priv->alloc_len)
+					len = priv->alloc_len;
+				memcpy(buff, priv->buff, len);
+				priv->phase = PHASE_STATUS;
+			}
+			return len;
+		case PHASE_STATUS:
+			debug("status in, len=%x\n", len);
+			if (len > sizeof(priv->status))
+				len = sizeof(priv->status);
+			memcpy(buff, &priv->status, len);
+			priv->phase = PHASE_START;
+			return len;
+		default:
+			break;
+		}
+	}
+err:
+	priv->error = true;
+	debug("%s: Detected transfer error\n", __func__);
+	return 0;
+}
+
+static int sandbox_flash_ofdata_to_platdata(struct udevice *dev)
+{
+	struct sandbox_flash_plat *plat = dev_get_platdata(dev);
+	const void *blob = gd->fdt_blob;
+
+	plat->pathname = fdt_getprop(blob, dev->of_offset, "sandbox,filepath",
+				     NULL);
+
+	return 0;
+}
+
+static int sandbox_flash_bind(struct udevice *dev)
+{
+	return usb_emul_setup_device(dev, PACKET_SIZE_64, flash_strings,
+				     flash_desc_list);
+}
+
+static int sandbox_flash_probe(struct udevice *dev)
+{
+	struct sandbox_flash_plat *plat = dev_get_platdata(dev);
+	struct sandbox_flash_priv *priv = dev_get_priv(dev);
+
+	priv->fd = os_open(plat->pathname, OS_O_RDONLY);
+	if (priv->fd != -1)
+		return os_get_filesize(plat->pathname, &priv->file_size);
+
+	return 0;
+}
+
+static const struct dm_usb_ops sandbox_usb_flash_ops = {
+	.control	= sandbox_flash_control,
+	.bulk		= sandbox_flash_bulk,
+};
+
+static const struct udevice_id sandbox_usb_flash_ids[] = {
+	{ .compatible = "sandbox,usb-flash" },
+	{ }
+};
+
+U_BOOT_DRIVER(usb_sandbox_flash) = {
+	.name	= "usb_sandbox_flash",
+	.id	= UCLASS_USB_EMUL,
+	.of_match = sandbox_usb_flash_ids,
+	.bind	= sandbox_flash_bind,
+	.probe	= sandbox_flash_probe,
+	.ofdata_to_platdata = sandbox_flash_ofdata_to_platdata,
+	.ops	= &sandbox_usb_flash_ops,
+	.priv_auto_alloc_size = sizeof(struct sandbox_flash_priv),
+	.platdata_auto_alloc_size = sizeof(struct sandbox_flash_plat),
+};
diff --git a/drivers/usb/emul/sandbox_hub.c b/drivers/usb/emul/sandbox_hub.c
new file mode 100644
index 0000000..baf8bdc
--- /dev/null
+++ b/drivers/usb/emul/sandbox_hub.c
@@ -0,0 +1,304 @@
+/*
+ * (C) Copyright 2015 Google, Inc
+ * Written by Simon Glass <sjg@chromium.org>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <dm.h>
+#include <usb.h>
+#include <dm/device-internal.h>
+
+DECLARE_GLOBAL_DATA_PTR;
+
+/* We only support up to 8 */
+#define SANDBOX_NUM_PORTS	2
+
+struct sandbox_hub_platdata {
+	struct usb_dev_platdata plat;
+	int port;	/* Port number (numbered from 0) */
+};
+
+enum {
+	STRING_MANUFACTURER = 1,
+	STRING_PRODUCT,
+	STRING_SERIAL,
+
+	STRING_count,
+};
+
+static struct usb_string hub_strings[] = {
+	{STRING_MANUFACTURER,	"sandbox"},
+	{STRING_PRODUCT,	"hub"},
+	{STRING_SERIAL,		"2345"},
+	{},
+};
+
+static struct usb_device_descriptor hub_device_desc = {
+	.bLength =		sizeof(hub_device_desc),
+	.bDescriptorType =	USB_DT_DEVICE,
+
+	.bcdUSB =		__constant_cpu_to_le16(0x0200),
+
+	.bDeviceClass =		USB_CLASS_HUB,
+	.bDeviceSubClass =	0,
+	.bDeviceProtocol =	0,
+
+	.idVendor =		__constant_cpu_to_le16(0x1234),
+	.idProduct =		__constant_cpu_to_le16(0x5678),
+	.iManufacturer =	STRING_MANUFACTURER,
+	.iProduct =		STRING_PRODUCT,
+	.iSerialNumber =	STRING_SERIAL,
+	.bNumConfigurations =	1,
+};
+
+static struct usb_config_descriptor hub_config1 = {
+	.bLength		= sizeof(hub_config1),
+	.bDescriptorType	= USB_DT_CONFIG,
+
+	/* wTotalLength is set up by usb-emul-uclass */
+	.bNumInterfaces		= 1,
+	.bConfigurationValue	= 0,
+	.iConfiguration		= 0,
+	.bmAttributes		= 1 << 7,
+	.bMaxPower		= 50,
+};
+
+static struct usb_interface_descriptor hub_interface0 = {
+	.bLength		= sizeof(hub_interface0),
+	.bDescriptorType	= USB_DT_INTERFACE,
+
+	.bInterfaceNumber	= 0,
+	.bAlternateSetting	= 0,
+	.bNumEndpoints		= 1,
+	.bInterfaceClass	= USB_CLASS_HUB,
+	.bInterfaceSubClass	= 0,
+	.bInterfaceProtocol	= US_PR_CB,
+	.iInterface		= 0,
+};
+
+static struct usb_endpoint_descriptor hub_endpoint0_in = {
+	.bLength		= USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType	= USB_DT_ENDPOINT,
+
+	.bEndpointAddress	= 1 | USB_DIR_IN,
+	.bmAttributes		= USB_ENDPOINT_XFER_INT,
+	.wMaxPacketSize		= __constant_cpu_to_le16(1024),
+	.bInterval		= 0,
+};
+
+static struct usb_hub_descriptor hub_desc = {
+	.bLength		= sizeof(hub_desc),
+	.bDescriptorType	= USB_DT_HUB,
+	.bNbrPorts		= SANDBOX_NUM_PORTS,
+	.wHubCharacteristics	= __constant_cpu_to_le16(1 << 0 | 1 << 3 |
+								1 << 7),
+	.bPwrOn2PwrGood		= 2,
+	.bHubContrCurrent	= 5,
+	.DeviceRemovable	= {0, 0xff}, /* all ports removeable */
+#if SANDBOX_NUM_PORTS > 8
+#error "This code sets up an incorrect mask"
+#endif
+};
+
+static void *hub_desc_list[] = {
+	&hub_device_desc,
+	&hub_config1,
+	&hub_interface0,
+	&hub_endpoint0_in,
+	&hub_desc,
+	NULL,
+};
+
+struct sandbox_hub_priv {
+	int status[SANDBOX_NUM_PORTS];
+	int change[SANDBOX_NUM_PORTS];
+};
+
+static struct udevice *hub_find_device(struct udevice *hub, int port)
+{
+	struct udevice *dev;
+
+	for (device_find_first_child(hub, &dev);
+	     dev;
+	     device_find_next_child(&dev)) {
+		struct sandbox_hub_platdata *plat;
+
+		plat = dev_get_parent_platdata(dev);
+		if (plat->port == port)
+			return dev;
+	}
+
+	return NULL;
+}
+
+static int clrset_post_state(struct udevice *hub, int port, int clear, int set)
+{
+	struct sandbox_hub_priv *priv = dev_get_priv(hub);
+	int *status = &priv->status[port];
+	int *change = &priv->change[port];
+	int ret = 0;
+
+	if ((clear | set) & USB_PORT_STAT_POWER) {
+		struct udevice *dev = hub_find_device(hub, port);
+
+		if (dev) {
+			if (set & USB_PORT_STAT_POWER) {
+				ret = device_probe(dev);
+				debug("%s: %s: power on, probed, ret=%d\n",
+				      __func__, dev->name, ret);
+				if (!ret) {
+					set |= USB_PORT_STAT_CONNECTION |
+						USB_PORT_STAT_ENABLE;
+				}
+
+			} else if (clear & USB_PORT_STAT_POWER) {
+				debug("%s: %s: power off, removed, ret=%d\n",
+				      __func__, dev->name, ret);
+				ret = device_remove(dev);
+				clear |= USB_PORT_STAT_CONNECTION;
+			}
+		}
+	}
+	*change |= *status & clear;
+	*change |= ~*status & set;
+	*change &= 0x1f;
+	*status = (*status & ~clear) | set;
+
+	return ret;
+}
+
+static int sandbox_hub_submit_control_msg(struct udevice *bus,
+					  struct usb_device *udev,
+					  unsigned long pipe,
+					  void *buffer, int length,
+					  struct devrequest *setup)
+{
+	struct sandbox_hub_priv *priv = dev_get_priv(bus);
+	int ret = 0;
+
+	if (pipe == usb_rcvctrlpipe(udev, 0)) {
+		switch (setup->requesttype) {
+		case USB_RT_HUB | USB_DIR_IN:
+			switch (setup->request) {
+			case USB_REQ_GET_STATUS: {
+				struct usb_hub_status *hubsts = buffer;
+
+				hubsts->wHubStatus = 0;
+				hubsts->wHubChange = 0;
+				udev->status = 0;
+				udev->act_len = sizeof(*hubsts);
+				return 0;
+			}
+			default:
+				debug("%s: rx ctl requesttype=%x, request=%x\n",
+				      __func__, setup->requesttype,
+				      setup->request);
+				break;
+			}
+		case USB_RT_PORT | USB_DIR_IN:
+			switch (setup->request) {
+			case USB_REQ_GET_STATUS: {
+				struct usb_port_status *portsts = buffer;
+				int port;
+
+				port = (setup->index & USB_HUB_PORT_MASK) - 1;
+				portsts->wPortStatus = priv->status[port];
+				portsts->wPortChange = priv->change[port];
+				udev->status = 0;
+				udev->act_len = sizeof(*portsts);
+				return 0;
+			}
+			}
+		default:
+			debug("%s: rx ctl requesttype=%x, request=%x\n",
+			      __func__, setup->requesttype, setup->request);
+			break;
+		}
+	} else if (pipe == usb_sndctrlpipe(udev, 0)) {
+		switch (setup->requesttype) {
+		case USB_RT_PORT:
+			switch (setup->request) {
+			case USB_REQ_SET_FEATURE: {
+				int port;
+
+				port = (setup->index & USB_HUB_PORT_MASK) - 1;
+				debug("set feature port=%x, feature=%x\n",
+				      port, setup->value);
+				if (setup->value < USB_PORT_FEAT_C_CONNECTION) {
+					ret = clrset_post_state(bus, port, 0,
+							1 << setup->value);
+				} else {
+					debug("  ** Invalid feature\n");
+				}
+				return ret;
+			}
+			case USB_REQ_CLEAR_FEATURE: {
+				int port;
+
+				port = (setup->index & USB_HUB_PORT_MASK) - 1;
+				debug("clear feature port=%x, feature=%x\n",
+				      port, setup->value);
+				if (setup->value < USB_PORT_FEAT_C_CONNECTION) {
+					ret = clrset_post_state(bus, port,
+							1 << setup->value, 0);
+				} else {
+					priv->change[port] &= 1 <<
+						(setup->value - 16);
+				}
+				udev->status = 0;
+				return 0;
+			}
+			default:
+				debug("%s: tx ctl requesttype=%x, request=%x\n",
+				      __func__, setup->requesttype,
+				      setup->request);
+				break;
+			}
+		default:
+			debug("%s: tx ctl requesttype=%x, request=%x\n",
+			      __func__, setup->requesttype, setup->request);
+			break;
+		}
+	}
+	debug("pipe=%lx\n", pipe);
+
+	return -EIO;
+}
+
+static int sandbox_hub_bind(struct udevice *dev)
+{
+	return usb_emul_setup_device(dev, PACKET_SIZE_64, hub_strings,
+				     hub_desc_list);
+}
+
+static int sandbox_child_post_bind(struct udevice *dev)
+{
+	struct sandbox_hub_platdata *plat = dev_get_parent_platdata(dev);
+
+	plat->port = fdtdec_get_int(gd->fdt_blob, dev->of_offset, "reg", -1);
+
+	return 0;
+}
+
+static const struct dm_usb_ops sandbox_usb_hub_ops = {
+	.control	= sandbox_hub_submit_control_msg,
+};
+
+static const struct udevice_id sandbox_usb_hub_ids[] = {
+	{ .compatible = "sandbox,usb-hub" },
+	{ }
+};
+
+U_BOOT_DRIVER(usb_sandbox_hub) = {
+	.name	= "usb_sandbox_hub",
+	.id	= UCLASS_USB_EMUL,
+	.of_match = sandbox_usb_hub_ids,
+	.bind	= sandbox_hub_bind,
+	.ops	= &sandbox_usb_hub_ops,
+	.priv_auto_alloc_size = sizeof(struct sandbox_hub_priv),
+	.per_child_platdata_auto_alloc_size =
+			sizeof(struct sandbox_hub_platdata),
+	.child_post_bind = sandbox_child_post_bind,
+};
diff --git a/drivers/usb/emul/usb-emul-uclass.c b/drivers/usb/emul/usb-emul-uclass.c
new file mode 100644
index 0000000..205f2c5
--- /dev/null
+++ b/drivers/usb/emul/usb-emul-uclass.c
@@ -0,0 +1,263 @@
+/*
+ * (C) Copyright 2015 Google, Inc
+ * Written by Simon Glass <sjg@chromium.org>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <dm.h>
+#include <usb.h>
+#include <dm/root.h>
+#include <dm/device-internal.h>
+
+DECLARE_GLOBAL_DATA_PTR;
+
+static int copy_to_unicode(char *buff, int length, const char *str)
+{
+	int ptr;
+	int i;
+
+	if (length < 2)
+		return 0;
+	buff[1] = USB_DT_STRING;
+	for (ptr = 2, i = 0; ptr + 1 < length && *str; i++, ptr += 2) {
+		buff[ptr] = str[i];
+		buff[ptr + 1] = 0;
+	}
+	buff[0] = ptr;
+
+	return ptr;
+}
+
+static int usb_emul_get_string(struct usb_string *strings, int index,
+			       char *buff, int length)
+{
+	if (index == 0) {
+		char *desc = buff;
+
+		desc[0] = 4;
+		desc[1] = USB_DT_STRING;
+		desc[2] = 0x09;
+		desc[3] = 0x14;
+		return 4;
+	} else if (strings) {
+		struct usb_string *ptr;
+
+		for (ptr = strings; ptr->s; ptr++) {
+			if (ptr->id == index)
+				return copy_to_unicode(buff, length, ptr->s);
+		}
+	}
+
+	return -EINVAL;
+}
+
+static struct usb_generic_descriptor **find_descriptor(
+		struct usb_generic_descriptor **ptr, int type, int index)
+{
+	debug("%s: type=%x, index=%d\n", __func__, type, index);
+	for (; *ptr; ptr++) {
+		if ((*ptr)->bDescriptorType != type)
+			continue;
+		switch (type) {
+		case USB_DT_CONFIG: {
+			struct usb_config_descriptor *cdesc;
+
+			cdesc = (struct usb_config_descriptor *)*ptr;
+			if (cdesc && cdesc->bConfigurationValue == index)
+				return ptr;
+			break;
+		}
+		default:
+			return ptr;
+		}
+	}
+	debug("%s: config ptr=%p\n", __func__, *ptr);
+
+	return ptr;
+}
+
+static int usb_emul_get_descriptor(struct usb_dev_platdata *plat, int value,
+				   void *buffer, int length)
+{
+	struct usb_generic_descriptor **ptr;
+	int type = value >> 8;
+	int index = value & 0xff;
+	int upto, todo;
+
+	debug("%s: type=%d, index=%d, plat=%p\n", __func__, type, index, plat);
+	if (type == USB_DT_STRING) {
+		return usb_emul_get_string(plat->strings, index, buffer,
+					   length);
+	}
+
+	ptr = find_descriptor((struct usb_generic_descriptor **)plat->desc_list,
+			      type, index);
+	if (!ptr) {
+		debug("%s: Could not find descriptor type %d, index %d\n",
+		      __func__, type, index);
+		return -ENOENT;
+	}
+	for (upto = 0; *ptr && upto < length; ptr++, upto += todo) {
+		todo = min(length - upto, (int)(*ptr)->bLength);
+
+		memcpy(buffer + upto, *ptr, todo);
+	}
+
+	return upto ? upto : length ? -EIO : 0;
+}
+
+int usb_emul_find(struct udevice *bus, ulong pipe, struct udevice **emulp)
+{
+	int devnum = usb_pipedevice(pipe);
+	struct udevice *dev;
+	struct uclass *uc;
+	int ret;
+
+	*emulp = NULL;
+	ret = uclass_get(UCLASS_USB_EMUL, &uc);
+	if (ret)
+		return ret;
+	uclass_foreach_dev(dev, uc) {
+		struct usb_dev_platdata *udev = dev_get_parent_platdata(dev);
+
+		if (udev->devnum == devnum) {
+			debug("%s: Found emulator '%s', addr %d\n", __func__,
+			      dev->name, udev->devnum);
+			*emulp = dev;
+			return 0;
+		}
+	}
+
+	debug("%s: No emulator found, addr %d\n", __func__, devnum);
+	return -ENOENT;
+}
+
+int usb_emul_control(struct udevice *emul, struct usb_device *udev,
+		     unsigned long pipe, void *buffer, int length,
+		     struct devrequest *setup)
+{
+	struct dm_usb_ops *ops = usb_get_emul_ops(emul);
+	struct usb_dev_platdata *plat;
+	int ret;
+
+	/* We permit getting the descriptor before we are probed */
+	plat = dev_get_parent_platdata(emul);
+	if (!ops->control)
+		return -ENOSYS;
+	debug("%s: dev=%s\n", __func__, emul->name);
+	if (pipe == usb_rcvctrlpipe(udev, 0)) {
+		switch (setup->request) {
+		case USB_REQ_GET_DESCRIPTOR: {
+			return usb_emul_get_descriptor(plat, setup->value,
+						       buffer, length);
+		}
+		default:
+			ret = device_probe(emul);
+			if (ret)
+				return ret;
+			return ops->control(emul, udev, pipe, buffer, length,
+					    setup);
+		}
+	} else if (pipe == usb_snddefctrl(udev)) {
+		switch (setup->request) {
+		case USB_REQ_SET_ADDRESS:
+			debug("   ** set address %s %d\n", emul->name,
+			      setup->value);
+			plat->devnum = setup->value;
+			return 0;
+		default:
+			debug("requestsend =%x\n", setup->request);
+			break;
+		}
+	} else if (pipe == usb_sndctrlpipe(udev, 0)) {
+		switch (setup->request) {
+		case USB_REQ_SET_CONFIGURATION:
+			plat->configno = setup->value;
+			return 0;
+		default:
+			ret = device_probe(emul);
+			if (ret)
+				return ret;
+			return ops->control(emul, udev, pipe, buffer, length,
+					    setup);
+		}
+	}
+	debug("pipe=%lx\n", pipe);
+
+	return -EIO;
+}
+
+int usb_emul_bulk(struct udevice *emul, struct usb_device *udev,
+		  unsigned long pipe, void *buffer, int length)
+{
+	struct dm_usb_ops *ops = usb_get_emul_ops(emul);
+	int ret;
+
+	/* We permit getting the descriptor before we are probed */
+	if (!ops->bulk)
+		return -ENOSYS;
+	debug("%s: dev=%s\n", __func__, emul->name);
+	ret = device_probe(emul);
+	if (ret)
+		return ret;
+	return ops->bulk(emul, udev, pipe, buffer, length);
+}
+
+int usb_emul_setup_device(struct udevice *dev, int maxpacketsize,
+			  struct usb_string *strings, void **desc_list)
+{
+	struct usb_dev_platdata *plat = dev_get_parent_platdata(dev);
+	struct usb_generic_descriptor **ptr;
+	struct usb_config_descriptor *cdesc;
+	int upto;
+
+	plat->strings = strings;
+	plat->desc_list = (struct usb_generic_descriptor **)desc_list;
+
+	/* Fill in wTotalLength for each configuration descriptor */
+	ptr = plat->desc_list;
+	for (cdesc = NULL, upto = 0; *ptr; upto += (*ptr)->bLength, ptr++) {
+		debug("   - upto=%d, type=%d\n", upto, (*ptr)->bDescriptorType);
+		if ((*ptr)->bDescriptorType == USB_DT_CONFIG) {
+			if (cdesc) {
+				cdesc->wTotalLength = upto;
+				debug("%s: config %d length %d\n", __func__,
+				      cdesc->bConfigurationValue,
+				      cdesc->bLength);
+			}
+			cdesc = (struct usb_config_descriptor *)*ptr;
+			upto = 0;
+		}
+	}
+	if (cdesc) {
+		cdesc->wTotalLength = upto;
+		debug("%s: config %d length %d\n", __func__,
+		      cdesc->bConfigurationValue, cdesc->wTotalLength);
+	}
+
+	return 0;
+}
+
+int usb_emul_post_bind(struct udevice *dev)
+{
+	/* Scan the bus for devices */
+	return dm_scan_fdt_node(dev, gd->fdt_blob, dev->of_offset, false);
+}
+
+void usb_emul_reset(struct udevice *dev)
+{
+	struct usb_dev_platdata *plat = dev_get_parent_platdata(dev);
+
+	plat->devnum = 0;
+	plat->configno = 0;
+}
+
+UCLASS_DRIVER(usb_emul) = {
+	.id		= UCLASS_USB_EMUL,
+	.name		= "usb_emul",
+	.post_bind	= usb_emul_post_bind,
+	.per_child_auto_alloc_size = sizeof(struct usb_device),
+	.per_child_platdata_auto_alloc_size = sizeof(struct usb_dev_platdata),
+};
diff --git a/drivers/usb/eth/asix.c b/drivers/usb/eth/asix.c
index 1cd179b..c8697ae 100644
--- a/drivers/usb/eth/asix.c
+++ b/drivers/usb/eth/asix.c
@@ -534,7 +534,8 @@ static int asix_recv(struct eth_device *eth)
 		}
 
 		/* Notify net stack */
-		NetReceive(buf_ptr + sizeof(packet_len), packet_len);
+		net_process_received_packet(buf_ptr + sizeof(packet_len),
+					    packet_len);
 
 		/* Adjust for next iteration. Packets are padded to 16-bits */
 		if (packet_len & 1)
diff --git a/drivers/usb/eth/asix88179.c b/drivers/usb/eth/asix88179.c
index 0ef85db..94dfe85 100644
--- a/drivers/usb/eth/asix88179.c
+++ b/drivers/usb/eth/asix88179.c
@@ -558,7 +558,7 @@ static int asix_recv(struct eth_device *eth)
 
 		frame_pos += 2;
 
-		NetReceive(recv_buf + frame_pos, pkt_len);
+		net_process_received_packet(recv_buf + frame_pos, pkt_len);
 
 		pkt_hdr++;
 		frame_pos += ((pkt_len + 7) & 0xFFF8)-2;
diff --git a/drivers/usb/eth/mcs7830.c b/drivers/usb/eth/mcs7830.c
index 8e738d4..c1b7086 100644
--- a/drivers/usb/eth/mcs7830.c
+++ b/drivers/usb/eth/mcs7830.c
@@ -600,7 +600,7 @@ static int mcs7830_recv(struct eth_device *eth)
 
 	if (sts == STAT_RX_FRAME_CORRECT) {
 		debug("%s() got a frame, len=%d\n", __func__, gotlen);
-		NetReceive(buf, gotlen);
+		net_process_received_packet(buf, gotlen);
 		return 0;
 	}
 
diff --git a/drivers/usb/eth/smsc95xx.c b/drivers/usb/eth/smsc95xx.c
index 6bca34d..a7e50d6 100644
--- a/drivers/usb/eth/smsc95xx.c
+++ b/drivers/usb/eth/smsc95xx.c
@@ -355,7 +355,7 @@ static int smsc95xx_init_mac_address(struct eth_device *eth,
 	/* try reading mac address from EEPROM */
 	if (smsc95xx_read_eeprom(dev, EEPROM_MAC_OFFSET, ETH_ALEN,
 			eth->enetaddr) == 0) {
-		if (is_valid_ether_addr(eth->enetaddr)) {
+		if (is_valid_ethaddr(eth->enetaddr)) {
 			/* eeprom values are valid so use them */
 			debug("MAC address read from EEPROM\n");
 			return 0;
@@ -760,7 +760,8 @@ static int smsc95xx_recv(struct eth_device *eth)
 		}
 
 		/* Notify net stack */
-		NetReceive(buf_ptr + sizeof(packet_len), packet_len - 4);
+		net_process_received_packet(buf_ptr + sizeof(packet_len),
+					    packet_len - 4);
 
 		/* Adjust for next iteration */
 		actual_len -= sizeof(packet_len) + packet_len;
diff --git a/drivers/usb/eth/usb_ether.c b/drivers/usb/eth/usb_ether.c
index 7cb96e3..c72b7e4 100644
--- a/drivers/usb/eth/usb_ether.c
+++ b/drivers/usb/eth/usb_ether.c
@@ -5,7 +5,9 @@
  */
 
 #include <common.h>
+#include <dm.h>
 #include <usb.h>
+#include <dm/device-internal.h>
 
 #include "usb_ether.h"
 
@@ -118,8 +120,6 @@ static void probe_valid_drivers(struct usb_device *dev)
 int usb_host_eth_scan(int mode)
 {
 	int i, old_async;
-	struct usb_device *dev;
-
 
 	if (mode == 1)
 		printf("       scanning usb for ethernet devices... ");
@@ -138,23 +138,59 @@ int usb_host_eth_scan(int mode)
 	}
 
 	usb_max_eth_dev = 0;
+#ifdef CONFIG_DM_USB
+	/*
+	 * TODO: We should add USB_DEVICE() declarations to each USB ethernet
+	 * driver and then most of this file can be removed.
+	 */
+	struct udevice *bus;
+	struct uclass *uc;
+	int ret;
+
+	ret = uclass_get(UCLASS_USB, &uc);
+	if (ret)
+		return ret;
+	uclass_foreach_dev(bus, uc) {
+		for (i = 0; i < USB_MAX_DEVICE; i++) {
+			struct usb_device *dev;
+
+			dev = usb_get_dev_index(bus, i); /* get device */
+			debug("i=%d, %s\n", i, dev ? dev->dev->name : "(done)");
+			if (!dev)
+				break; /* no more devices available */
+
+			/*
+			 * find valid usb_ether driver for this device,
+			 * if any
+			 */
+			probe_valid_drivers(dev);
+
+			/* check limit */
+			if (usb_max_eth_dev == USB_MAX_ETH_DEV)
+				break;
+		} /* for */
+	}
+#else
 	for (i = 0; i < USB_MAX_DEVICE; i++) {
+		struct usb_device *dev;
+
 		dev = usb_get_dev_index(i); /* get device */
 		debug("i=%d\n", i);
-		if (dev == NULL)
+		if (!dev)
 			break; /* no more devices available */
 
 		/* find valid usb_ether driver for this device, if any */
 		probe_valid_drivers(dev);
 
 		/* check limit */
-		if (usb_max_eth_dev == USB_MAX_ETH_DEV) {
-			printf("max USB Ethernet Device reached: %d stopping\n",
-				usb_max_eth_dev);
+		if (usb_max_eth_dev == USB_MAX_ETH_DEV)
 			break;
-		}
 	} /* for */
-
+#endif
+	if (usb_max_eth_dev == USB_MAX_ETH_DEV) {
+		printf("max USB Ethernet Device reached: %d stopping\n",
+		       usb_max_eth_dev);
+	}
 	usb_disable_asynch(old_async); /* restore asynch value */
 	printf("%d Ethernet Device(s) found\n", usb_max_eth_dev);
 	if (usb_max_eth_dev > 0)
diff --git a/drivers/usb/gadget/ci_udc.c b/drivers/usb/gadget/ci_udc.c
index 3b7024c..22d288c 100644
--- a/drivers/usb/gadget/ci_udc.c
+++ b/drivers/usb/gadget/ci_udc.c
@@ -883,7 +883,11 @@ int usb_gadget_register_driver(struct usb_gadget_driver *driver)
 	if (driver->speed != USB_SPEED_FULL && driver->speed != USB_SPEED_HIGH)
 		return -EINVAL;
 
+#ifdef CONFIG_DM_USB
+	ret = usb_setup_ehci_gadget(&controller.ctrl);
+#else
 	ret = usb_lowlevel_init(0, USB_INIT_DEVICE, (void **)&controller.ctrl);
+#endif
 	if (ret)
 		return ret;
 
diff --git a/drivers/usb/gadget/ether.c b/drivers/usb/gadget/ether.c
index 7e3b3ed..141ff8b 100644
--- a/drivers/usb/gadget/ether.c
+++ b/drivers/usb/gadget/ether.c
@@ -1522,7 +1522,7 @@ static int rx_submit(struct eth_dev *dev, struct usb_request *req,
 	 * RNDIS headers involve variable numbers of LE32 values.
 	 */
 
-	req->buf = (u8 *) NetRxPackets[0];
+	req->buf = (u8 *)net_rx_packets[0];
 	req->length = size;
 	req->complete = rx_complete;
 
@@ -1645,13 +1645,13 @@ static int eth_start_xmit (struct sk_buff *skb, struct net_device *net)
 	if (!eth_is_promisc (dev)) {
 		u8		*dest = skb->data;
 
-		if (is_multicast_ether_addr(dest)) {
+		if (is_multicast_ethaddr(dest)) {
 			u16	type;
 
 			/* ignores USB_CDC_PACKET_TYPE_MULTICAST and host
 			 * SET_ETHERNET_MULTICAST_FILTERS requests
 			 */
-			if (is_broadcast_ether_addr(dest))
+			if (is_broadcast_ethaddr(dest))
 				type = USB_CDC_PACKET_TYPE_BROADCAST;
 			else
 				type = USB_CDC_PACKET_TYPE_ALL_MULTICAST;
@@ -1942,7 +1942,7 @@ static int is_eth_addr_valid(char *str)
 		}
 
 		/* Now check the contents. */
-		return is_valid_ether_addr(ea);
+		return is_valid_ethaddr(ea);
 	}
 	return 0;
 }
@@ -1971,7 +1971,7 @@ static int get_ether_addr(const char *str, u8 *dev_addr)
 			num |= (nibble(*str++));
 			dev_addr[i] = num;
 		}
-		if (is_valid_ether_addr(dev_addr))
+		if (is_valid_ethaddr(dev_addr))
 			return 0;
 	}
 	return 1;
@@ -2446,7 +2446,8 @@ static int usb_eth_recv(struct eth_device *netdev)
 	if (packet_received) {
 		debug("%s: packet received\n", __func__);
 		if (dev->rx_req) {
-			NetReceive(NetRxPackets[0], dev->rx_req->length);
+			net_process_received_packet(net_rx_packets[0],
+						    dev->rx_req->length);
 			packet_received = 0;
 
 			rx_submit(dev, dev->rx_req, 0);
diff --git a/drivers/usb/host/Makefile b/drivers/usb/host/Makefile
index eb6f34b..3b57e56 100644
--- a/drivers/usb/host/Makefile
+++ b/drivers/usb/host/Makefile
@@ -5,6 +5,11 @@
 # SPDX-License-Identifier:	GPL-2.0+
 #
 
+ifdef CONFIG_DM_USB
+obj-$(CONFIG_CMD_USB) += usb-uclass.o
+obj-$(CONFIG_SANDBOX) += usb-sandbox.o
+endif
+
 # ohci
 obj-$(CONFIG_USB_OHCI_NEW) += ohci-hcd.o
 obj-$(CONFIG_USB_ATMEL) += ohci-at91.o
@@ -39,6 +44,7 @@ obj-$(CONFIG_USB_EHCI_SUNXI) += ehci-sunxi.o
 obj-$(CONFIG_USB_EHCI_TEGRA) += ehci-tegra.o
 obj-$(CONFIG_USB_EHCI_UNIPHIER) += ehci-uniphier.o
 obj-$(CONFIG_USB_EHCI_VCT) += ehci-vct.o
+obj-$(CONFIG_USB_EHCI_VF) += ehci-vf.o
 obj-$(CONFIG_USB_EHCI_RMOBILE) += ehci-rmobile.o
 obj-$(CONFIG_USB_EHCI_ZYNQ) += ehci-zynq.o
 
diff --git a/drivers/usb/host/ehci-exynos.c b/drivers/usb/host/ehci-exynos.c
index f3c077d..86cf631 100644
--- a/drivers/usb/host/ehci-exynos.c
+++ b/drivers/usb/host/ehci-exynos.c
@@ -8,6 +8,7 @@
  */
 
 #include <common.h>
+#include <dm.h>
 #include <fdtdec.h>
 #include <libfdt.h>
 #include <malloc.h>
@@ -24,19 +25,73 @@
 /* Declare global data pointer */
 DECLARE_GLOBAL_DATA_PTR;
 
+#ifdef CONFIG_DM_USB
+struct exynos_ehci_platdata {
+	struct usb_platdata usb_plat;
+	fdt_addr_t hcd_base;
+	fdt_addr_t phy_base;
+	struct gpio_desc vbus_gpio;
+};
+#endif
+
 /**
  * Contains pointers to register base addresses
  * for the usb controller.
  */
 struct exynos_ehci {
+	struct ehci_ctrl ctrl;
 	struct exynos_usb_phy *usb;
 	struct ehci_hccr *hcd;
+#ifndef CONFIG_DM_USB
 	struct gpio_desc vbus_gpio;
+#endif
 };
 
+#ifndef CONFIG_DM_USB
 static struct exynos_ehci exynos;
+#endif
 
-#ifdef CONFIG_OF_CONTROL
+#ifdef CONFIG_DM_USB
+static int ehci_usb_ofdata_to_platdata(struct udevice *dev)
+{
+	struct exynos_ehci_platdata *plat = dev_get_platdata(dev);
+	const void *blob = gd->fdt_blob;
+	unsigned int node;
+	int depth;
+
+	/*
+	 * Get the base address for XHCI controller from the device node
+	 */
+	plat->hcd_base = dev_get_addr(dev);
+	if (plat->hcd_base == FDT_ADDR_T_NONE) {
+		debug("Can't get the XHCI register base address\n");
+		return -ENXIO;
+	}
+
+	depth = 0;
+	node = fdtdec_next_compatible_subnode(blob, dev->of_offset,
+				COMPAT_SAMSUNG_EXYNOS_USB_PHY, &depth);
+	if (node <= 0) {
+		debug("XHCI: Can't get device node for usb3-phy controller\n");
+		return -ENODEV;
+	}
+
+	/*
+	 * Get the base address for usbphy from the device node
+	 */
+	plat->phy_base = fdtdec_get_addr(blob, node, "reg");
+	if (plat->phy_base == FDT_ADDR_T_NONE) {
+		debug("Can't get the usbphy register address\n");
+		return -ENXIO;
+	}
+
+	/* Vbus gpio */
+	gpio_request_by_name(dev, "samsung,vbus-gpio", 0,
+			     &plat->vbus_gpio, GPIOD_IS_OUT);
+
+	return 0;
+}
+#else
 static int exynos_usb_parse_dt(const void *blob, struct exynos_ehci *exynos)
 {
 	fdt_addr_t addr;
@@ -215,6 +270,7 @@ static void reset_usb_phy(struct exynos_usb_phy *usb)
 	set_usbhost_phy_ctrl(POWER_USB_HOST_PHY_CTRL_DISABLE);
 }
 
+#ifndef CONFIG_DM_USB
 /*
  * EHCI-initialization
  * Create the appropriate control structures to manage
@@ -268,3 +324,57 @@ int ehci_hcd_stop(int index)
 
 	return 0;
 }
+#endif
+
+#ifdef CONFIG_DM_USB
+static int ehci_usb_probe(struct udevice *dev)
+{
+	struct exynos_ehci_platdata *plat = dev_get_platdata(dev);
+	struct exynos_ehci *ctx = dev_get_priv(dev);
+	struct ehci_hcor *hcor;
+
+	ctx->hcd = (struct ehci_hccr *)plat->hcd_base;
+	ctx->usb = (struct exynos_usb_phy *)plat->phy_base;
+	hcor = (struct ehci_hcor *)((uint32_t)ctx->hcd +
+			HC_LENGTH(ehci_readl(&ctx->hcd->cr_capbase)));
+
+	/* setup the Vbus gpio here */
+	if (dm_gpio_is_valid(&plat->vbus_gpio))
+		dm_gpio_set_value(&plat->vbus_gpio, 1);
+
+	setup_usb_phy(ctx->usb);
+
+	return ehci_register(dev, ctx->hcd, hcor, NULL, 0, USB_INIT_HOST);
+}
+
+static int ehci_usb_remove(struct udevice *dev)
+{
+	struct exynos_ehci *ctx = dev_get_priv(dev);
+	int ret;
+
+	ret = ehci_deregister(dev);
+	if (ret)
+		return ret;
+	reset_usb_phy(ctx->usb);
+
+	return 0;
+}
+
+static const struct udevice_id ehci_usb_ids[] = {
+	{ .compatible = "samsung,exynos-ehci" },
+	{ }
+};
+
+U_BOOT_DRIVER(usb_ehci) = {
+	.name	= "ehci_exynos",
+	.id	= UCLASS_USB,
+	.of_match = ehci_usb_ids,
+	.ofdata_to_platdata = ehci_usb_ofdata_to_platdata,
+	.probe = ehci_usb_probe,
+	.remove = ehci_usb_remove,
+	.ops	= &ehci_usb_ops,
+	.priv_auto_alloc_size = sizeof(struct exynos_ehci),
+	.platdata_auto_alloc_size = sizeof(struct exynos_ehci_platdata),
+	.flags	= DM_FLAG_ALLOC_PRIV_DMA,
+};
+#endif
diff --git a/drivers/usb/host/ehci-faraday.c b/drivers/usb/host/ehci-faraday.c
index 3b761bc..821222c 100644
--- a/drivers/usb/host/ehci-faraday.c
+++ b/drivers/usb/host/ehci-faraday.c
@@ -29,6 +29,59 @@ static inline int ehci_is_fotg2xx(union ehci_faraday_regs *regs)
 	return !readl(&regs->usb.easstr);
 }
 
+void faraday_ehci_set_usbmode(struct ehci_ctrl *ctrl)
+{
+	/* nothing needs to be done */
+}
+
+int faraday_ehci_get_port_speed(struct ehci_ctrl *ctrl, uint32_t reg)
+{
+	int spd, ret = PORTSC_PSPD_HS;
+	union ehci_faraday_regs *regs;
+
+	ret = (void __iomem *)((ulong)ctrl->hcor - 0x10);
+	if (ehci_is_fotg2xx(regs))
+		spd = OTGCSR_SPD(readl(&regs->otg.otgcsr));
+	else
+		spd = BMCSR_SPD(readl(&regs->usb.bmcsr));
+
+	switch (spd) {
+	case 0:    /* full speed */
+		ret = PORTSC_PSPD_FS;
+		break;
+	case 1:    /* low  speed */
+		ret = PORTSC_PSPD_LS;
+		break;
+	case 2:    /* high speed */
+		ret = PORTSC_PSPD_HS;
+		break;
+	default:
+		printf("ehci-faraday: invalid device speed\n");
+		break;
+	}
+
+	return ret;
+}
+
+uint32_t *faraday_ehci_get_portsc_register(struct ehci_ctrl *ctrl, int port)
+{
+	/* Faraday EHCI has one and only one portsc register */
+	if (port) {
+		/* Printing the message would cause a scan failure! */
+		debug("The request port(%d) is not configured\n", port);
+		return NULL;
+	}
+
+	/* Faraday EHCI PORTSC register offset is 0x20 from hcor */
+	return (uint32_t *)((uint8_t *)ctrl->hcor + 0x20);
+}
+
+static const struct ehci_ops faraday_ehci_ops = {
+	.set_usb_mode		= faraday_ehci_set_usbmode,
+	.get_port_speed		= faraday_ehci_get_port_speed,
+	.get_portsc_register	= faraday_ehci_get_portsc_register,
+};
+
 /*
  * Create the appropriate control structures to manage
  * a new EHCI host controller.
@@ -43,6 +96,7 @@ int ehci_hcd_init(int index, enum usb_init_type init,
 
 	if (index < 0 || index >= ARRAY_SIZE(base_list))
 		return -1;
+	ehci_set_controller_priv(index, NULL, &faraday_ehci_ops);
 	regs = (void __iomem *)base_list[index];
 	hccr = (struct ehci_hccr *)&regs->usb.hccr;
 	hcor = (struct ehci_hcor *)&regs->usb.hcor;
@@ -87,61 +141,3 @@ int ehci_hcd_stop(int index)
 {
 	return 0;
 }
-
-/*
- * This ehci_set_usbmode() overrides the weak function
- * in "ehci-hcd.c".
- */
-void ehci_set_usbmode(int index)
-{
-	/* nothing needs to be done */
-}
-
-/*
- * This ehci_get_port_speed() overrides the weak function
- * in "ehci-hcd.c".
- */
-int ehci_get_port_speed(struct ehci_hcor *hcor, uint32_t reg)
-{
-	int spd, ret = PORTSC_PSPD_HS;
-	union ehci_faraday_regs *regs = (void __iomem *)((ulong)hcor - 0x10);
-
-	if (ehci_is_fotg2xx(regs))
-		spd = OTGCSR_SPD(readl(&regs->otg.otgcsr));
-	else
-		spd = BMCSR_SPD(readl(&regs->usb.bmcsr));
-
-	switch (spd) {
-	case 0:    /* full speed */
-		ret = PORTSC_PSPD_FS;
-		break;
-	case 1:    /* low  speed */
-		ret = PORTSC_PSPD_LS;
-		break;
-	case 2:    /* high speed */
-		ret = PORTSC_PSPD_HS;
-		break;
-	default:
-		printf("ehci-faraday: invalid device speed\n");
-		break;
-	}
-
-	return ret;
-}
-
-/*
- * This ehci_get_portsc_register() overrides the weak function
- * in "ehci-hcd.c".
- */
-uint32_t *ehci_get_portsc_register(struct ehci_hcor *hcor, int port)
-{
-	/* Faraday EHCI has one and only one portsc register */
-	if (port) {
-		/* Printing the message would cause a scan failure! */
-		debug("The request port(%d) is not configured\n", port);
-		return NULL;
-	}
-
-	/* Faraday EHCI PORTSC register offset is 0x20 from hcor */
-	return (uint32_t *)((uint8_t *)hcor + 0x20);
-}
diff --git a/drivers/usb/host/ehci-fsl.c b/drivers/usb/host/ehci-fsl.c
index 5d4288d..2dca524 100644
--- a/drivers/usb/host/ehci-fsl.c
+++ b/drivers/usb/host/ehci-fsl.c
@@ -259,10 +259,11 @@ static int fdt_fixup_usb_erratum(void *blob, const char *prop_erratum,
 void fdt_fixup_dr_usb(void *blob, bd_t *bd)
 {
 	static const char * const modes[] = { "host", "peripheral", "otg" };
-	static const char * const phys[] = { "ulpi", "utmi" };
+	static const char * const phys[] = { "ulpi", "utmi", "utmi_dual" };
 	int usb_erratum_a006261_off = -1;
 	int usb_erratum_a007075_off = -1;
 	int usb_erratum_a007792_off = -1;
+	int usb_erratum_a005697_off = -1;
 	int usb_mode_off = -1;
 	int usb_phy_off = -1;
 	char str[5];
@@ -303,6 +304,9 @@ void fdt_fixup_dr_usb(void *blob, bd_t *bd)
 				dr_phy_type = phys[phy_idx];
 		}
 
+		if (has_dual_phy())
+			dr_phy_type = phys[2];
+
 		usb_mode_off = fdt_fixup_usb_mode_phy_type(blob,
 							   dr_mode_type, NULL,
 							   usb_mode_off);
@@ -325,6 +329,7 @@ void fdt_fixup_dr_usb(void *blob, bd_t *bd)
 			if (usb_erratum_a006261_off < 0)
 				return;
 		}
+
 		if (has_erratum_a007075()) {
 			usb_erratum_a007075_off =  fdt_fixup_usb_erratum
 						   (blob,
@@ -333,6 +338,7 @@ void fdt_fixup_dr_usb(void *blob, bd_t *bd)
 			if (usb_erratum_a007075_off < 0)
 				return;
 		}
+
 		if (has_erratum_a007792()) {
 			usb_erratum_a007792_off =  fdt_fixup_usb_erratum
 						   (blob,
@@ -341,6 +347,14 @@ void fdt_fixup_dr_usb(void *blob, bd_t *bd)
 			if (usb_erratum_a007792_off < 0)
 				return;
 		}
+		if (has_erratum_a005697()) {
+			usb_erratum_a005697_off =  fdt_fixup_usb_erratum
+						   (blob,
+						    "fsl,usb-erratum-a005697",
+						    usb_erratum_a005697_off);
+			if (usb_erratum_a005697_off < 0)
+				return;
+		}
 	}
 }
 #endif
diff --git a/drivers/usb/host/ehci-hcd.c b/drivers/usb/host/ehci-hcd.c
index 86f1646..bd9861d 100644
--- a/drivers/usb/host/ehci-hcd.c
+++ b/drivers/usb/host/ehci-hcd.c
@@ -21,6 +21,7 @@
  * MA 02111-1307 USA
  */
 #include <common.h>
+#include <dm.h>
 #include <errno.h>
 #include <asm/byteorder.h>
 #include <asm/unaligned.h>
@@ -42,7 +43,9 @@
  */
 #define HCHALT_TIMEOUT (8 * 1000)
 
+#ifndef CONFIG_DM_USB
 static struct ehci_ctrl ehcic[CONFIG_USB_MAX_CONTROLLER_COUNT];
+#endif
 
 #define ALIGN_END_ADDR(type, ptr, size)			\
 	((unsigned long)(ptr) + roundup((size) * sizeof(type), USB_DMA_MINALIGN))
@@ -119,17 +122,33 @@ static struct descriptor {
 #define ehci_is_TDI()	(0)
 #endif
 
-__weak int ehci_get_port_speed(struct ehci_hcor *hcor, uint32_t reg)
+static struct ehci_ctrl *ehci_get_ctrl(struct usb_device *udev)
+{
+#ifdef CONFIG_DM_USB
+	struct udevice *dev;
+
+	/* Find the USB controller */
+	for (dev = udev->dev;
+	     device_get_uclass_id(dev) != UCLASS_USB;
+	     dev = dev->parent)
+		;
+	return dev_get_priv(dev);
+#else
+	return udev->controller;
+#endif
+}
+
+static int ehci_get_port_speed(struct ehci_ctrl *ctrl, uint32_t reg)
 {
 	return PORTSC_PSPD(reg);
 }
 
-__weak void ehci_set_usbmode(int index)
+static void ehci_set_usbmode(struct ehci_ctrl *ctrl)
 {
 	uint32_t tmp;
 	uint32_t *reg_ptr;
 
-	reg_ptr = (uint32_t *)((u8 *)&ehcic[index].hcor->or_usbcmd + USBMODE);
+	reg_ptr = (uint32_t *)((u8 *)&ctrl->hcor->or_usbcmd + USBMODE);
 	tmp = ehci_readl(reg_ptr);
 	tmp |= USBMODE_CM_HC;
 #if defined(CONFIG_EHCI_MMIO_BIG_ENDIAN)
@@ -138,11 +157,23 @@ __weak void ehci_set_usbmode(int index)
 	ehci_writel(reg_ptr, tmp);
 }
 
-__weak void ehci_powerup_fixup(uint32_t *status_reg, uint32_t *reg)
+static void ehci_powerup_fixup(struct ehci_ctrl *ctrl, uint32_t *status_reg,
+			       uint32_t *reg)
 {
 	mdelay(50);
 }
 
+static uint32_t *ehci_get_portsc_register(struct ehci_ctrl *ctrl, int port)
+{
+	if (port < 0 || port >= CONFIG_SYS_USB_EHCI_MAX_ROOT_PORTS) {
+		/* Printing the message would cause a scan failure! */
+		debug("The request port(%u) is not configured\n", port);
+		return NULL;
+	}
+
+	return (uint32_t *)&ctrl->hcor->or_portsc[port];
+}
+
 static int handshake(uint32_t *ptr, uint32_t mask, uint32_t done, int usec)
 {
 	uint32_t result;
@@ -159,15 +190,15 @@ static int handshake(uint32_t *ptr, uint32_t mask, uint32_t done, int usec)
 	return -1;
 }
 
-static int ehci_reset(int index)
+static int ehci_reset(struct ehci_ctrl *ctrl)
 {
 	uint32_t cmd;
 	int ret = 0;
 
-	cmd = ehci_readl(&ehcic[index].hcor->or_usbcmd);
+	cmd = ehci_readl(&ctrl->hcor->or_usbcmd);
 	cmd = (cmd & ~CMD_RUN) | CMD_RESET;
-	ehci_writel(&ehcic[index].hcor->or_usbcmd, cmd);
-	ret = handshake((uint32_t *)&ehcic[index].hcor->or_usbcmd,
+	ehci_writel(&ctrl->hcor->or_usbcmd, cmd);
+	ret = handshake((uint32_t *)&ctrl->hcor->or_usbcmd,
 			CMD_RESET, 0, 250 * 1000);
 	if (ret < 0) {
 		printf("EHCI fail to reset\n");
@@ -175,13 +206,13 @@ static int ehci_reset(int index)
 	}
 
 	if (ehci_is_TDI())
-		ehci_set_usbmode(index);
+		ctrl->ops.set_usb_mode(ctrl);
 
 #ifdef CONFIG_USB_EHCI_TXFIFO_THRESH
-	cmd = ehci_readl(&ehcic[index].hcor->or_txfilltuning);
+	cmd = ehci_readl(&ctrl->hcor->or_txfilltuning);
 	cmd &= ~TXFIFO_THRESH_MASK;
 	cmd |= TXFIFO_THRESH(CONFIG_USB_EHCI_TXFIFO_THRESH);
-	ehci_writel(&ehcic[index].hcor->or_txfilltuning, cmd);
+	ehci_writel(&ctrl->hcor->or_txfilltuning, cmd);
 #endif
 out:
 	return ret;
@@ -264,12 +295,13 @@ static inline u8 ehci_encode_speed(enum usb_device_speed speed)
 	return QH_FULL_SPEED;
 }
 
-static void ehci_update_endpt2_dev_n_port(struct usb_device *dev,
+static void ehci_update_endpt2_dev_n_port(struct usb_device *udev,
 					  struct QH *qh)
 {
 	struct usb_device *ttdev;
+	int parent_devnum;
 
-	if (dev->speed != USB_SPEED_LOW && dev->speed != USB_SPEED_FULL)
+	if (udev->speed != USB_SPEED_LOW && udev->speed != USB_SPEED_FULL)
 		return;
 
 	/*
@@ -277,14 +309,35 @@ static void ehci_update_endpt2_dev_n_port(struct usb_device *dev,
 	 * the tt, so of the first upstream usb-2 hub, there may be usb-1 hubs
 	 * in the tree before that one!
 	 */
-	ttdev = dev;
+#ifdef CONFIG_DM_USB
+	struct udevice *parent;
+
+	for (ttdev = udev; ; ) {
+		struct udevice *dev = ttdev->dev;
+
+		if (dev->parent &&
+		    device_get_uclass_id(dev->parent) == UCLASS_USB_HUB)
+			parent = dev->parent;
+		else
+			parent = NULL;
+		if (!parent)
+			return;
+		ttdev = dev_get_parentdata(parent);
+		if (!ttdev->speed != USB_SPEED_HIGH)
+			break;
+	}
+	parent_devnum = ttdev->devnum;
+#else
+	ttdev = udev;
 	while (ttdev->parent && ttdev->parent->speed != USB_SPEED_HIGH)
 		ttdev = ttdev->parent;
 	if (!ttdev->parent)
 		return;
+	parent_devnum = ttdev->parent->devnum;
+#endif
 
 	qh->qh_endpt2 |= cpu_to_hc32(QH_ENDPT2_PORTNUM(ttdev->portnr) |
-				     QH_ENDPT2_HUBADDR(ttdev->parent->devnum));
+				     QH_ENDPT2_HUBADDR(parent_devnum));
 }
 
 static int
@@ -303,7 +356,7 @@ ehci_submit_async(struct usb_device *dev, unsigned long pipe, void *buffer,
 	uint32_t cmd;
 	int timeout;
 	int ret = 0;
-	struct ehci_ctrl *ctrl = dev->controller;
+	struct ehci_ctrl *ctrl = ehci_get_ctrl(dev);
 
 	debug("dev=%p, pipe=%lx, buffer=%p, length=%d, req=%p\n", dev, pipe,
 	      buffer, length, req);
@@ -649,20 +702,8 @@ fail:
 	return -1;
 }
 
-__weak uint32_t *ehci_get_portsc_register(struct ehci_hcor *hcor, int port)
-{
-	if (port < 0 || port >= CONFIG_SYS_USB_EHCI_MAX_ROOT_PORTS) {
-		/* Printing the message would cause a scan failure! */
-		debug("The request port(%u) is not configured\n", port);
-		return NULL;
-	}
-
-	return (uint32_t *)&hcor->or_portsc[port];
-}
-
-int
-ehci_submit_root(struct usb_device *dev, unsigned long pipe, void *buffer,
-		 int length, struct devrequest *req)
+static int ehci_submit_root(struct usb_device *dev, unsigned long pipe,
+			    void *buffer, int length, struct devrequest *req)
 {
 	uint8_t tmpbuf[4];
 	u16 typeReq;
@@ -671,7 +712,7 @@ ehci_submit_root(struct usb_device *dev, unsigned long pipe, void *buffer,
 	uint32_t reg;
 	uint32_t *status_reg;
 	int port = le16_to_cpu(req->index) & 0xff;
-	struct ehci_ctrl *ctrl = dev->controller;
+	struct ehci_ctrl *ctrl = ehci_get_ctrl(dev);
 
 	srclen = 0;
 
@@ -686,7 +727,7 @@ ehci_submit_root(struct usb_device *dev, unsigned long pipe, void *buffer,
 	case USB_REQ_GET_STATUS | ((USB_RT_PORT | USB_DIR_IN) << 8):
 	case USB_REQ_SET_FEATURE | ((USB_DIR_OUT | USB_RT_PORT) << 8):
 	case USB_REQ_CLEAR_FEATURE | ((USB_DIR_OUT | USB_RT_PORT) << 8):
-		status_reg = ehci_get_portsc_register(ctrl->hcor, port - 1);
+		status_reg = ctrl->ops.get_portsc_register(ctrl, port - 1);
 		if (!status_reg)
 			return -1;
 		break;
@@ -781,7 +822,7 @@ ehci_submit_root(struct usb_device *dev, unsigned long pipe, void *buffer,
 			tmpbuf[1] |= USB_PORT_STAT_POWER >> 8;
 
 		if (ehci_is_TDI()) {
-			switch (ehci_get_port_speed(ctrl->hcor, reg)) {
+			switch (ctrl->ops.get_port_speed(ctrl, reg)) {
 			case PORTSC_PSPD_FS:
 				break;
 			case PORTSC_PSPD_LS:
@@ -843,7 +884,7 @@ ehci_submit_root(struct usb_device *dev, unsigned long pipe, void *buffer,
 				 * usb 2.0 specification say 50 ms resets on
 				 * root
 				 */
-				ehci_powerup_fixup(status_reg, &reg);
+				ctrl->ops.powerup_fixup(ctrl, status_reg, &reg);
 
 				ehci_writel(status_reg, reg & ~EHCI_PS_PR);
 				/*
@@ -930,41 +971,59 @@ unknown:
 	return -1;
 }
 
-int usb_lowlevel_stop(int index)
+const struct ehci_ops default_ehci_ops = {
+	.set_usb_mode		= ehci_set_usbmode,
+	.get_port_speed		= ehci_get_port_speed,
+	.powerup_fixup		= ehci_powerup_fixup,
+	.get_portsc_register	= ehci_get_portsc_register,
+};
+
+static void ehci_setup_ops(struct ehci_ctrl *ctrl, const struct ehci_ops *ops)
 {
-	ehci_shutdown(&ehcic[index]);
-	return ehci_hcd_stop(index);
+	if (!ops) {
+		ctrl->ops = default_ehci_ops;
+	} else {
+		ctrl->ops = *ops;
+		if (!ctrl->ops.set_usb_mode)
+			ctrl->ops.set_usb_mode = ehci_set_usbmode;
+		if (!ctrl->ops.get_port_speed)
+			ctrl->ops.get_port_speed = ehci_get_port_speed;
+		if (!ctrl->ops.powerup_fixup)
+			ctrl->ops.powerup_fixup = ehci_powerup_fixup;
+		if (!ctrl->ops.get_portsc_register)
+			ctrl->ops.get_portsc_register =
+					ehci_get_portsc_register;
+	}
 }
 
-int usb_lowlevel_init(int index, enum usb_init_type init, void **controller)
+#ifndef CONFIG_DM_USB
+void ehci_set_controller_priv(int index, void *priv, const struct ehci_ops *ops)
+{
+	struct ehci_ctrl *ctrl = &ehcic[index];
+
+	ctrl->priv = priv;
+	ehci_setup_ops(ctrl, ops);
+}
+
+void *ehci_get_controller_priv(int index)
+{
+	return ehcic[index].priv;
+}
+#endif
+
+static int ehci_common_init(struct ehci_ctrl *ctrl, uint tweaks)
 {
-	uint32_t reg;
-	uint32_t cmd;
 	struct QH *qh_list;
 	struct QH *periodic;
+	uint32_t reg;
+	uint32_t cmd;
 	int i;
-	int rc;
-
-	rc = ehci_hcd_init(index, init, &ehcic[index].hccr, &ehcic[index].hcor);
-	if (rc)
-		return rc;
-	if (init == USB_INIT_DEVICE)
-		goto done;
 
-	/* EHCI spec section 4.1 */
-	if (ehci_reset(index))
-		return -1;
-
-#if defined(CONFIG_EHCI_HCD_INIT_AFTER_RESET)
-	rc = ehci_hcd_init(index, init, &ehcic[index].hccr, &ehcic[index].hcor);
-	if (rc)
-		return rc;
-#endif
 	/* Set the high address word (aka segment) for 64-bit controller */
-	if (ehci_readl(&ehcic[index].hccr->cr_hccparams) & 1)
-		ehci_writel(&ehcic[index].hcor->or_ctrldssegment, 0);
+	if (ehci_readl(&ctrl->hccr->cr_hccparams) & 1)
+		ehci_writel(&ctrl->hcor->or_ctrldssegment, 0);
 
-	qh_list = &ehcic[index].qh_list;
+	qh_list = &ctrl->qh_list;
 
 	/* Set head of reclaim list */
 	memset(qh_list, 0, sizeof(*qh_list));
@@ -980,14 +1039,14 @@ int usb_lowlevel_init(int index, enum usb_init_type init, void **controller)
 			   ALIGN_END_ADDR(struct QH, qh_list, 1));
 
 	/* Set async. queue head pointer. */
-	ehci_writel(&ehcic[index].hcor->or_asynclistaddr, (unsigned long)qh_list);
+	ehci_writel(&ctrl->hcor->or_asynclistaddr, (unsigned long)qh_list);
 
 	/*
 	 * Set up periodic list
 	 * Step 1: Parent QH for all periodic transfers.
 	 */
-	ehcic[index].periodic_schedules = 0;
-	periodic = &ehcic[index].periodic_queue;
+	ctrl->periodic_schedules = 0;
+	periodic = &ctrl->periodic_queue;
 	memset(periodic, 0, sizeof(*periodic));
 	periodic->qh_link = cpu_to_hc32(QH_LINK_TERMINATE);
 	periodic->qh_overlay.qt_next = cpu_to_hc32(QT_NEXT_TERMINATE);
@@ -1005,25 +1064,25 @@ int usb_lowlevel_init(int index, enum usb_init_type init, void **controller)
 	 *         Split Transactions will be spread across microframes using
 	 *         S-mask and C-mask.
 	 */
-	if (ehcic[index].periodic_list == NULL)
-		ehcic[index].periodic_list = memalign(4096, 1024 * 4);
+	if (ctrl->periodic_list == NULL)
+		ctrl->periodic_list = memalign(4096, 1024 * 4);
 
-	if (!ehcic[index].periodic_list)
+	if (!ctrl->periodic_list)
 		return -ENOMEM;
 	for (i = 0; i < 1024; i++) {
-		ehcic[index].periodic_list[i] = cpu_to_hc32((unsigned long)periodic
+		ctrl->periodic_list[i] = cpu_to_hc32((unsigned long)periodic
 						| QH_LINK_TYPE_QH);
 	}
 
-	flush_dcache_range((unsigned long)ehcic[index].periodic_list,
-			   ALIGN_END_ADDR(uint32_t, ehcic[index].periodic_list,
+	flush_dcache_range((unsigned long)ctrl->periodic_list,
+			   ALIGN_END_ADDR(uint32_t, ctrl->periodic_list,
 					  1024));
 
 	/* Set periodic list base address */
-	ehci_writel(&ehcic[index].hcor->or_periodiclistbase,
-		(unsigned long)ehcic[index].periodic_list);
+	ehci_writel(&ctrl->hcor->or_periodiclistbase,
+		(unsigned long)ctrl->periodic_list);
 
-	reg = ehci_readl(&ehcic[index].hccr->cr_hcsparams);
+	reg = ehci_readl(&ctrl->hccr->cr_hcsparams);
 	descriptor.hub.bNbrPorts = HCS_N_PORTS(reg);
 	debug("Register %x NbrPorts %d\n", reg, descriptor.hub.bNbrPorts);
 	/* Port Indicators */
@@ -1036,37 +1095,81 @@ int usb_lowlevel_init(int index, enum usb_init_type init, void **controller)
 				| 0x01, &descriptor.hub.wHubCharacteristics);
 
 	/* Start the host controller. */
-	cmd = ehci_readl(&ehcic[index].hcor->or_usbcmd);
+	cmd = ehci_readl(&ctrl->hcor->or_usbcmd);
 	/*
 	 * Philips, Intel, and maybe others need CMD_RUN before the
 	 * root hub will detect new devices (why?); NEC doesn't
 	 */
 	cmd &= ~(CMD_LRESET|CMD_IAAD|CMD_PSE|CMD_ASE|CMD_RESET);
 	cmd |= CMD_RUN;
-	ehci_writel(&ehcic[index].hcor->or_usbcmd, cmd);
+	ehci_writel(&ctrl->hcor->or_usbcmd, cmd);
 
-#ifndef CONFIG_USB_EHCI_FARADAY
-	/* take control over the ports */
-	cmd = ehci_readl(&ehcic[index].hcor->or_configflag);
-	cmd |= FLAG_CF;
-	ehci_writel(&ehcic[index].hcor->or_configflag, cmd);
-#endif
+	if (!(tweaks & EHCI_TWEAK_NO_INIT_CF)) {
+		/* take control over the ports */
+		cmd = ehci_readl(&ctrl->hcor->or_configflag);
+		cmd |= FLAG_CF;
+		ehci_writel(&ctrl->hcor->or_configflag, cmd);
+	}
 
 	/* unblock posted write */
-	cmd = ehci_readl(&ehcic[index].hcor->or_usbcmd);
+	cmd = ehci_readl(&ctrl->hcor->or_usbcmd);
 	mdelay(5);
-	reg = HC_VERSION(ehci_readl(&ehcic[index].hccr->cr_capbase));
+	reg = HC_VERSION(ehci_readl(&ctrl->hccr->cr_capbase));
 	printf("USB EHCI %x.%02x\n", reg >> 8, reg & 0xff);
 
-	ehcic[index].rootdev = 0;
+	return 0;
+}
+
+#ifndef CONFIG_DM_USB
+int usb_lowlevel_stop(int index)
+{
+	ehci_shutdown(&ehcic[index]);
+	return ehci_hcd_stop(index);
+}
+
+int usb_lowlevel_init(int index, enum usb_init_type init, void **controller)
+{
+	struct ehci_ctrl *ctrl = &ehcic[index];
+	uint tweaks = 0;
+	int rc;
+
+	/**
+	 * Set ops to default_ehci_ops, ehci_hcd_init should call
+	 * ehci_set_controller_priv to change any of these function pointers.
+	 */
+	ctrl->ops = default_ehci_ops;
+
+	rc = ehci_hcd_init(index, init, &ctrl->hccr, &ctrl->hcor);
+	if (rc)
+		return rc;
+	if (init == USB_INIT_DEVICE)
+		goto done;
+
+	/* EHCI spec section 4.1 */
+	if (ehci_reset(ctrl))
+		return -1;
+
+#if defined(CONFIG_EHCI_HCD_INIT_AFTER_RESET)
+	rc = ehci_hcd_init(index, init, &ctrl->hccr, &ctrl->hcor);
+	if (rc)
+		return rc;
+#endif
+#ifdef CONFIG_USB_EHCI_FARADAY
+	tweaks |= EHCI_TWEAK_NO_INIT_CF;
+#endif
+	rc = ehci_common_init(ctrl, tweaks);
+	if (rc)
+		return rc;
+
+	ctrl->rootdev = 0;
 done:
 	*controller = &ehcic[index];
 	return 0;
 }
+#endif
 
-int
-submit_bulk_msg(struct usb_device *dev, unsigned long pipe, void *buffer,
-		int length)
+static int _ehci_submit_bulk_msg(struct usb_device *dev, unsigned long pipe,
+				 void *buffer, int length)
 {
 
 	if (usb_pipetype(pipe) != PIPE_BULK) {
@@ -1076,11 +1179,11 @@ submit_bulk_msg(struct usb_device *dev, unsigned long pipe, void *buffer,
 	return ehci_submit_async(dev, pipe, buffer, length, NULL);
 }
 
-int
-submit_control_msg(struct usb_device *dev, unsigned long pipe, void *buffer,
-		   int length, struct devrequest *setup)
+static int _ehci_submit_control_msg(struct usb_device *dev, unsigned long pipe,
+				    void *buffer, int length,
+				    struct devrequest *setup)
 {
-	struct ehci_ctrl *ctrl = dev->controller;
+	struct ehci_ctrl *ctrl = ehci_get_ctrl(dev);
 
 	if (usb_pipetype(pipe) != PIPE_CONTROL) {
 		debug("non-control pipe (type=%lu)", usb_pipetype(pipe));
@@ -1150,7 +1253,7 @@ struct int_queue *
 create_int_queue(struct usb_device *dev, unsigned long pipe, int queuesize,
 		 int elementsize, void *buffer, int interval)
 {
-	struct ehci_ctrl *ctrl = dev->controller;
+	struct ehci_ctrl *ctrl = ehci_get_ctrl(dev);
 	struct int_queue *result = NULL;
 	int i;
 
@@ -1342,7 +1445,7 @@ void *poll_int_queue(struct usb_device *dev, struct int_queue *queue)
 int
 destroy_int_queue(struct usb_device *dev, struct int_queue *queue)
 {
-	struct ehci_ctrl *ctrl = dev->controller;
+	struct ehci_ctrl *ctrl = ehci_get_ctrl(dev);
 	int result = -1;
 	unsigned long timeout;
 
@@ -1386,9 +1489,8 @@ out:
 	return result;
 }
 
-int
-submit_int_msg(struct usb_device *dev, unsigned long pipe, void *buffer,
-	       int length, int interval)
+static int _ehci_submit_int_msg(struct usb_device *dev, unsigned long pipe,
+				void *buffer, int length, int interval)
 {
 	void *backbuffer;
 	struct int_queue *queue;
@@ -1423,3 +1525,98 @@ submit_int_msg(struct usb_device *dev, unsigned long pipe, void *buffer,
 	/* everything worked out fine */
 	return result;
 }
+
+#ifndef CONFIG_DM_USB
+int submit_bulk_msg(struct usb_device *dev, unsigned long pipe,
+			    void *buffer, int length)
+{
+	return _ehci_submit_bulk_msg(dev, pipe, buffer, length);
+}
+
+int submit_control_msg(struct usb_device *dev, unsigned long pipe, void *buffer,
+		   int length, struct devrequest *setup)
+{
+	return _ehci_submit_control_msg(dev, pipe, buffer, length, setup);
+}
+
+int submit_int_msg(struct usb_device *dev, unsigned long pipe,
+		   void *buffer, int length, int interval)
+{
+	return _ehci_submit_int_msg(dev, pipe, buffer, length, interval);
+}
+#endif
+
+#ifdef CONFIG_DM_USB
+static int ehci_submit_control_msg(struct udevice *dev, struct usb_device *udev,
+				   unsigned long pipe, void *buffer, int length,
+				   struct devrequest *setup)
+{
+	debug("%s: dev='%s', udev=%p, udev->dev='%s', portnr=%d\n", __func__,
+	      dev->name, udev, udev->dev->name, udev->portnr);
+
+	return _ehci_submit_control_msg(udev, pipe, buffer, length, setup);
+}
+
+static int ehci_submit_bulk_msg(struct udevice *dev, struct usb_device *udev,
+				unsigned long pipe, void *buffer, int length)
+{
+	debug("%s: dev='%s', udev=%p\n", __func__, dev->name, udev);
+	return _ehci_submit_bulk_msg(udev, pipe, buffer, length);
+}
+
+static int ehci_submit_int_msg(struct udevice *dev, struct usb_device *udev,
+			       unsigned long pipe, void *buffer, int length,
+			       int interval)
+{
+	debug("%s: dev='%s', udev=%p\n", __func__, dev->name, udev);
+	return _ehci_submit_int_msg(udev, pipe, buffer, length, interval);
+}
+
+int ehci_register(struct udevice *dev, struct ehci_hccr *hccr,
+		  struct ehci_hcor *hcor, const struct ehci_ops *ops,
+		  uint tweaks, enum usb_init_type init)
+{
+	struct ehci_ctrl *ctrl = dev_get_priv(dev);
+	int ret;
+
+	debug("%s: dev='%s', ctrl=%p, hccr=%p, hcor=%p, init=%d\n", __func__,
+	      dev->name, ctrl, hccr, hcor, init);
+
+	ehci_setup_ops(ctrl, ops);
+	ctrl->hccr = hccr;
+	ctrl->hcor = hcor;
+	ctrl->priv = ctrl;
+
+	if (init == USB_INIT_DEVICE)
+		goto done;
+	ret = ehci_reset(ctrl);
+	if (ret)
+		goto err;
+
+	ret = ehci_common_init(ctrl, tweaks);
+	if (ret)
+		goto err;
+done:
+	return 0;
+err:
+	free(ctrl);
+	debug("%s: failed, ret=%d\n", __func__, ret);
+	return ret;
+}
+
+int ehci_deregister(struct udevice *dev)
+{
+	struct ehci_ctrl *ctrl = dev_get_priv(dev);
+
+	ehci_shutdown(ctrl);
+
+	return 0;
+}
+
+struct dm_usb_ops ehci_usb_ops = {
+	.control = ehci_submit_control_msg,
+	.bulk = ehci_submit_bulk_msg,
+	.interrupt = ehci_submit_int_msg,
+};
+
+#endif
diff --git a/drivers/usb/host/ehci-mx5.c b/drivers/usb/host/ehci-mx5.c
index 7566c61..d319962 100644
--- a/drivers/usb/host/ehci-mx5.c
+++ b/drivers/usb/host/ehci-mx5.c
@@ -218,11 +218,23 @@ void __weak board_ehci_hcd_postinit(struct usb_ehci *ehci, int port)
 {
 }
 
+__weak void mx5_ehci_powerup_fixup(struct ehci_ctrl *ctrl, uint32_t *status_reg,
+				   uint32_t *reg)
+{
+	mdelay(50);
+}
+
+static const struct ehci_ops mx5_ehci_ops = {
+	.powerup_fixup		= mx5_ehci_powerup_fixup,
+};
+
 int ehci_hcd_init(int index, enum usb_init_type init,
 		struct ehci_hccr **hccr, struct ehci_hcor **hcor)
 {
 	struct usb_ehci *ehci;
 
+	/* The only user for this is efikamx-usb */
+	ehci_set_controller_priv(index, NULL, &mx5_ehci_ops);
 	set_usboh3_clk();
 	enable_usboh3_clk(true);
 	set_usb_phy_clk();
diff --git a/drivers/usb/host/ehci-tegra.c b/drivers/usb/host/ehci-tegra.c
index c6bfbe3..27705d6 100644
--- a/drivers/usb/host/ehci-tegra.c
+++ b/drivers/usb/host/ehci-tegra.c
@@ -7,6 +7,7 @@
  */
 
 #include <common.h>
+#include <dm.h>
 #include <asm/errno.h>
 #include <asm/io.h>
 #include <asm-generic/gpio.h>
@@ -20,6 +21,8 @@
 
 #include "ehci.h"
 
+DECLARE_GLOBAL_DATA_PTR;
+
 #define USB1_ADDR_MASK	0xFFFF0000
 
 #define HOSTPC1_DEVLC	0x84
@@ -32,9 +35,11 @@
 	#endif
 #endif
 
+#ifndef CONFIG_DM_USB
 enum {
 	USB_PORTS_MAX	= 3,		/* Maximum ports we allow */
 };
+#endif
 
 /* Parameters we need for USB */
 enum {
@@ -61,14 +66,26 @@ enum dr_mode {
 	DR_MODE_OTG,		/* supports both */
 };
 
+enum usb_ctlr_type {
+	USB_CTLR_T20,
+	USB_CTLR_T30,
+	USB_CTLR_T114,
+
+	USB_CTRL_COUNT,
+};
+
 /* Information about a USB port */
 struct fdt_usb {
+	struct ehci_ctrl ehci;
 	struct usb_ctlr *reg;	/* address of registers in physical memory */
 	unsigned utmi:1;	/* 1 if port has external tranceiver, else 0 */
 	unsigned ulpi:1;	/* 1 if port has external ULPI transceiver */
 	unsigned enabled:1;	/* 1 to enable, 0 to disable */
 	unsigned has_legacy_mode:1; /* 1 if this port has legacy mode */
+#ifndef CONFIG_DM_USB
 	unsigned initialized:1; /* has this port already been initialized? */
+#endif
+	enum usb_ctlr_type type;
 	enum usb_init_type init_type;
 	enum dr_mode dr_mode;	/* dual role mode */
 	enum periph_id periph_id;/* peripheral id */
@@ -76,10 +93,10 @@ struct fdt_usb {
 	struct gpio_desc phy_reset_gpio; /* GPIO to reset ULPI phy */
 };
 
+#ifndef CONFIG_DM_USB
 static struct fdt_usb port[USB_PORTS_MAX];	/* List of valid USB ports */
 static unsigned port_count;			/* Number of available ports */
-/* Port that needs to clear CSC after Port Reset */
-static u32 port_addr_clear_csc;
+#endif
 
 /*
  * This table has USB timing parameters for each Oscillator frequency we
@@ -156,13 +173,14 @@ static const u8 utmip_elastic_limit = 16;
 static const u8 utmip_hs_sync_start_delay = 9;
 
 struct fdt_usb_controller {
+	/* TODO(sjg@chromium.org): Remove when we only use driver model */
 	int compat;
 	/* flag to determine whether controller supports hostpc register */
 	u32 has_hostpc:1;
 	const unsigned *pll_parameter;
 };
 
-static struct fdt_usb_controller fdt_usb_controllers[] = {
+static struct fdt_usb_controller fdt_usb_controllers[USB_CTRL_COUNT] = {
 	{
 		.compat		= COMPAT_NVIDIA_TEGRA20_USB,
 		.has_hostpc	= 0,
@@ -180,40 +198,36 @@ static struct fdt_usb_controller fdt_usb_controllers[] = {
 	},
 };
 
-static struct fdt_usb_controller *controller;
-
 /*
  * A known hardware issue where Connect Status Change bit of PORTSC register
  * of USB1 controller will be set after Port Reset.
  * We have to clear it in order for later device enumeration to proceed.
- * This ehci_powerup_fixup overrides the weak function ehci_powerup_fixup
- * in "ehci-hcd.c".
  */
-void ehci_powerup_fixup(uint32_t *status_reg, uint32_t *reg)
+static void tegra_ehci_powerup_fixup(struct ehci_ctrl *ctrl,
+				     uint32_t *status_reg, uint32_t *reg)
 {
+	struct fdt_usb *config = ctrl->priv;
+	struct fdt_usb_controller *controller;
+
+	controller = &fdt_usb_controllers[config->type];
 	mdelay(50);
 	/* This is to avoid PORT_ENABLE bit to be cleared in "ehci-hcd.c". */
 	if (controller->has_hostpc)
 		*reg |= EHCI_PS_PE;
 
-	if (((unsigned long)status_reg & TEGRA_USB_ADDR_MASK) != port_addr_clear_csc)
+	if (!config->has_legacy_mode)
 		return;
 	/* For EHCI_PS_CSC to be cleared in ehci_hcd.c */
 	if (ehci_readl(status_reg) & EHCI_PS_CSC)
 		*reg |= EHCI_PS_CSC;
 }
 
-/*
- * This ehci_set_usbmode overrides the weak function ehci_set_usbmode
- * in "ehci-hcd.c".
- */
-void ehci_set_usbmode(int index)
+static void tegra_ehci_set_usbmode(struct ehci_ctrl *ctrl)
 {
-	struct fdt_usb *config;
+	struct fdt_usb *config = ctrl->priv;
 	struct usb_ctlr *usbctlr;
 	uint32_t tmp;
 
-	config = &port[index];
 	usbctlr = config->reg;
 
 	tmp = ehci_readl(&usbctlr->usb_mode);
@@ -221,17 +235,17 @@ void ehci_set_usbmode(int index)
 	ehci_writel(&usbctlr->usb_mode, tmp);
 }
 
-/*
- * This ehci_get_port_speed overrides the weak function ehci_get_port_speed
- * in "ehci-hcd.c".
- */
-int ehci_get_port_speed(struct ehci_hcor *hcor, uint32_t reg)
+static int tegra_ehci_get_port_speed(struct ehci_ctrl *ctrl, uint32_t reg)
 {
+	struct fdt_usb *config = ctrl->priv;
+	struct fdt_usb_controller *controller;
 	uint32_t tmp;
 	uint32_t *reg_ptr;
 
+	controller = &fdt_usb_controllers[config->type];
 	if (controller->has_hostpc) {
-		reg_ptr = (uint32_t *)((u8 *)&hcor->or_usbcmd + HOSTPC1_DEVLC);
+		reg_ptr = (uint32_t *)((u8 *)&ctrl->hcor->or_usbcmd +
+				HOSTPC1_DEVLC);
 		tmp = ehci_readl(reg_ptr);
 		return HOSTPC1_PSPD(tmp);
 	} else
@@ -263,7 +277,8 @@ static void set_up_vbus(struct fdt_usb *config, enum usb_init_type init)
 	}
 }
 
-void usbf_reset_controller(struct fdt_usb *config, struct usb_ctlr *usbctlr)
+static void usbf_reset_controller(struct fdt_usb *config,
+				  struct usb_ctlr *usbctlr)
 {
 	/* Reset the USB controller with 2us delay */
 	reset_periph(config->periph_id, 2);
@@ -283,7 +298,7 @@ void usbf_reset_controller(struct fdt_usb *config, struct usb_ctlr *usbctlr)
 		setbits_le32(&usbctlr->susp_ctrl, UTMIP_PHY_ENB);
 }
 
-static const unsigned *get_pll_timing(void)
+static const unsigned *get_pll_timing(struct fdt_usb_controller *controller)
 {
 	const unsigned *timing;
 
@@ -330,6 +345,7 @@ static void init_phy_mux(struct fdt_usb *config, uint pts,
 static int init_utmi_usb_controller(struct fdt_usb *config,
 				    enum usb_init_type init)
 {
+	struct fdt_usb_controller *controller;
 	u32 b_sess_valid_mask, val;
 	int loop_count;
 	const unsigned *timing;
@@ -362,11 +378,14 @@ static int init_utmi_usb_controller(struct fdt_usb *config,
 			VBUS_SENSE_CTL_MASK,
 			VBUS_SENSE_CTL_A_SESS_VLD << VBUS_SENSE_CTL_SHIFT);
 
+	controller = &fdt_usb_controllers[config->type];
+	debug("controller=%p, type=%d\n", controller, config->type);
+
 	/*
 	 * PLL Delay CONFIGURATION settings. The following parameters control
 	 * the bring up of the plls.
 	 */
-	timing = get_pll_timing();
+	timing = get_pll_timing(controller);
 
 	if (!controller->has_hostpc) {
 		val = readl(&usbctlr->utmip_misc_cfg1);
@@ -517,7 +536,7 @@ static int init_utmi_usb_controller(struct fdt_usb *config,
 		udelay(1);
 	}
 	if (!loop_count)
-		return -1;
+		return -ETIMEDOUT;
 
 	/* Disable ICUSB FS/LS transceiver */
 	clrbits_le32(&usbctlr->icusb_ctrl, IC_ENB1);
@@ -560,6 +579,7 @@ static int init_ulpi_usb_controller(struct fdt_usb *config,
 	int loop_count;
 	struct ulpi_viewport ulpi_vp;
 	struct usb_ctlr *usbctlr = config->reg;
+	int ret;
 
 	/* set up ULPI reference clock on pllp_out4 */
 	clock_enable(PERIPH_ID_DEV2_OUT);
@@ -605,9 +625,10 @@ static int init_ulpi_usb_controller(struct fdt_usb *config,
 	ulpi_vp.port_num = 0;
 	ulpi_vp.viewport_addr = (u32)&usbctlr->ulpi_viewport;
 
-	if (ulpi_init(&ulpi_vp)) {
+	ret = ulpi_init(&ulpi_vp);
+	if (ret) {
 		printf("Tegra ULPI viewport init failed\n");
-		return -1;
+		return ret;
 	}
 
 	ulpi_set_vbus(&ulpi_vp, 1, 1);
@@ -624,7 +645,7 @@ static int init_ulpi_usb_controller(struct fdt_usb *config,
 		udelay(1);
 	}
 	if (!loop_count)
-		return -1;
+		return -ETIMEDOUT;
 	clrbits_le32(&usbctlr->susp_ctrl, USB_SUSP_CLR);
 
 	return 0;
@@ -635,7 +656,7 @@ static int init_ulpi_usb_controller(struct fdt_usb *config,
 {
 	printf("No code to set up ULPI controller, please enable"
 			"CONFIG_USB_ULPI and CONFIG_USB_ULPI_VIEWPORT");
-	return -1;
+	return -ENOSYS;
 }
 #endif
 
@@ -662,7 +683,7 @@ static int fdt_decode_usb(const void *blob, int node, struct fdt_usb *config)
 		else {
 			debug("%s: Cannot decode dr_mode '%s'\n", __func__,
 			      mode);
-			return -FDT_ERR_NOTFOUND;
+			return -EINVAL;
 		}
 	} else {
 		config->dr_mode = DR_MODE_HOST;
@@ -674,12 +695,10 @@ static int fdt_decode_usb(const void *blob, int node, struct fdt_usb *config)
 	config->enabled = fdtdec_get_is_enabled(blob, node);
 	config->has_legacy_mode = fdtdec_get_bool(blob, node,
 						  "nvidia,has-legacy-mode");
-	if (config->has_legacy_mode)
-		port_addr_clear_csc = (unsigned long)config->reg;
 	config->periph_id = clock_decode_periph_id(blob, node);
 	if (config->periph_id == PERIPH_ID_NONE) {
 		debug("%s: Missing/invalid peripheral ID\n", __func__);
-		return -FDT_ERR_NOTFOUND;
+		return -EINVAL;
 	}
 	gpio_request_by_name_nodev(blob, node, "nvidia,vbus-gpio", 0,
 				   &config->vbus_gpio, GPIOD_IS_OUT);
@@ -695,16 +714,101 @@ static int fdt_decode_usb(const void *blob, int node, struct fdt_usb *config)
 	return 0;
 }
 
+int usb_common_init(struct fdt_usb *config, enum usb_init_type init)
+{
+	int ret = 0;
+
+	switch (init) {
+	case USB_INIT_HOST:
+		switch (config->dr_mode) {
+		case DR_MODE_HOST:
+		case DR_MODE_OTG:
+			break;
+		default:
+			printf("tegrausb: Invalid dr_mode %d for host mode\n",
+			       config->dr_mode);
+			return -1;
+		}
+		break;
+	case USB_INIT_DEVICE:
+		if (config->periph_id != PERIPH_ID_USBD) {
+			printf("tegrausb: Device mode only supported on first USB controller\n");
+			return -1;
+		}
+		if (!config->utmi) {
+			printf("tegrausb: Device mode only supported with UTMI PHY\n");
+			return -1;
+		}
+		switch (config->dr_mode) {
+		case DR_MODE_DEVICE:
+		case DR_MODE_OTG:
+			break;
+		default:
+			printf("tegrausb: Invalid dr_mode %d for device mode\n",
+			       config->dr_mode);
+			return -1;
+		}
+		break;
+	default:
+		printf("tegrausb: Unknown USB_INIT_* %d\n", init);
+		return -1;
+	}
+
+#ifndef CONFIG_DM_USB
+	/* skip init, if the port is already initialized */
+	if (config->initialized && config->init_type == init)
+		return 0;
+#endif
+
+	debug("%d, %d\n", config->utmi, config->ulpi);
+	if (config->utmi)
+		ret = init_utmi_usb_controller(config, init);
+	else if (config->ulpi)
+		ret = init_ulpi_usb_controller(config, init);
+	if (ret)
+		return ret;
+
+	set_up_vbus(config, init);
+
+	config->init_type = init;
+
+	return 0;
+}
+
+void usb_common_uninit(struct fdt_usb *priv)
+{
+	struct usb_ctlr *usbctlr;
+
+	usbctlr = priv->reg;
+
+	/* Stop controller */
+	writel(0, &usbctlr->usb_cmd);
+	udelay(1000);
+
+	/* Initiate controller reset */
+	writel(2, &usbctlr->usb_cmd);
+	udelay(1000);
+}
+
+static const struct ehci_ops tegra_ehci_ops = {
+	.set_usb_mode		= tegra_ehci_set_usbmode,
+	.get_port_speed		= tegra_ehci_get_port_speed,
+	.powerup_fixup		= tegra_ehci_powerup_fixup,
+};
+
+#ifndef CONFIG_DM_USB
 /*
  * process_usb_nodes() - Process a list of USB nodes, adding them to our list
  *			of USB ports.
  * @blob:	fdt blob
  * @node_list:	list of nodes to process (any <=0 are ignored)
  * @count:	number of nodes to process
+ * @id:		controller type (enum usb_ctlr_type)
  *
  * Return:	0 - ok, -1 - error
  */
-static int process_usb_nodes(const void *blob, int node_list[], int count)
+static int process_usb_nodes(const void *blob, int node_list[], int count,
+			     enum usb_ctlr_type id)
 {
 	struct fdt_usb config;
 	int node, i;
@@ -728,9 +832,11 @@ static int process_usb_nodes(const void *blob, int node_list[], int count)
 			return -1;
 		}
 		if (!clk_done) {
-			config_clock(get_pll_timing());
+			config_clock(get_pll_timing(
+					&fdt_usb_controllers[id]));
 			clk_done = 1;
 		}
+		config.type = id;
 		config.initialized = 0;
 
 		/* add new USB port to the list of available ports */
@@ -747,20 +853,17 @@ int usb_process_devicetree(const void *blob)
 	int i;
 
 	for (i = 0; i < ARRAY_SIZE(fdt_usb_controllers); i++) {
-		controller = &fdt_usb_controllers[i];
-
 		count = fdtdec_find_aliases_for_id(blob, "usb",
-			controller->compat, node_list, USB_PORTS_MAX);
+			fdt_usb_controllers[i].compat, node_list,
+			USB_PORTS_MAX);
 		if (count) {
-			err = process_usb_nodes(blob, node_list, count);
+			err = process_usb_nodes(blob, node_list, count, i);
 			if (err)
 				printf("%s: Error processing USB node!\n",
 				       __func__);
 			return err;
 		}
 	}
-	if (i == ARRAY_SIZE(fdt_usb_controllers))
-		controller = NULL;
 
 	return err;
 }
@@ -780,68 +883,22 @@ int ehci_hcd_init(int index, enum usb_init_type init,
 {
 	struct fdt_usb *config;
 	struct usb_ctlr *usbctlr;
+	int ret;
 
 	if (index >= port_count)
 		return -1;
 
 	config = &port[index];
+	ehci_set_controller_priv(index, config, &tegra_ehci_ops);
 
-	switch (init) {
-	case USB_INIT_HOST:
-		switch (config->dr_mode) {
-		case DR_MODE_HOST:
-		case DR_MODE_OTG:
-			break;
-		default:
-			printf("tegrausb: Invalid dr_mode %d for host mode\n",
-			       config->dr_mode);
-			return -1;
-		}
-		break;
-	case USB_INIT_DEVICE:
-		if (config->periph_id != PERIPH_ID_USBD) {
-			printf("tegrausb: Device mode only supported on first USB controller\n");
-			return -1;
-		}
-		if (!config->utmi) {
-			printf("tegrausb: Device mode only supported with UTMI PHY\n");
-			return -1;
-		}
-		switch (config->dr_mode) {
-		case DR_MODE_DEVICE:
-		case DR_MODE_OTG:
-			break;
-		default:
-			printf("tegrausb: Invalid dr_mode %d for device mode\n",
-			       config->dr_mode);
-			return -1;
-		}
-		break;
-	default:
-		printf("tegrausb: Unknown USB_INIT_* %d\n", init);
-		return -1;
-	}
-
-	/* skip init, if the port is already initialized */
-	if (config->initialized && config->init_type == init)
-		goto success;
-
-	if (config->utmi && init_utmi_usb_controller(config, init)) {
-		printf("tegrausb: Cannot init port %d\n", index);
-		return -1;
-	}
-
-	if (config->ulpi && init_ulpi_usb_controller(config, init)) {
+	ret = usb_common_init(config, init);
+	if (ret) {
 		printf("tegrausb: Cannot init port %d\n", index);
-		return -1;
+		return ret;
 	}
 
-	set_up_vbus(config, init);
-
 	config->initialized = 1;
-	config->init_type = init;
 
-success:
 	usbctlr = config->reg;
 	*hccr = (struct ehci_hccr *)&usbctlr->cap_length;
 	*hcor = (struct ehci_hcor *)&usbctlr->usb_cmd;
@@ -854,19 +911,80 @@ success:
  */
 int ehci_hcd_stop(int index)
 {
-	struct usb_ctlr *usbctlr;
+	usb_common_uninit(&port[index]);
 
-	usbctlr = port[index].reg;
+	port[index].initialized = 0;
 
-	/* Stop controller */
-	writel(0, &usbctlr->usb_cmd);
-	udelay(1000);
+	return 0;
+}
+#endif /* !CONFIG_DM_USB */
 
-	/* Initiate controller reset */
-	writel(2, &usbctlr->usb_cmd);
-	udelay(1000);
+#ifdef CONFIG_DM_USB
+static int ehci_usb_ofdata_to_platdata(struct udevice *dev)
+{
+	struct fdt_usb *priv = dev_get_priv(dev);
+	int ret;
 
-	port[index].initialized = 0;
+	ret = fdt_decode_usb(gd->fdt_blob, dev->of_offset, priv);
+	if (ret)
+		return ret;
+
+	priv->type = dev_get_driver_data(dev);
+
+	return 0;
+}
+
+static int ehci_usb_probe(struct udevice *dev)
+{
+	struct usb_platdata *plat = dev_get_platdata(dev);
+	struct fdt_usb *priv = dev_get_priv(dev);
+	struct ehci_hccr *hccr;
+	struct ehci_hcor *hcor;
+	static bool clk_done;
+	int ret;
+
+	ret = usb_common_init(priv, plat->init_type);
+	if (ret)
+		return ret;
+	hccr = (struct ehci_hccr *)&priv->reg->cap_length;
+	hcor = (struct ehci_hcor *)&priv->reg->usb_cmd;
+	if (!clk_done) {
+		config_clock(get_pll_timing(&fdt_usb_controllers[priv->type]));
+		clk_done = true;
+	}
+
+	return ehci_register(dev, hccr, hcor, &tegra_ehci_ops, 0,
+			     plat->init_type);
+}
+
+static int ehci_usb_remove(struct udevice *dev)
+{
+	int ret;
+
+	ret = ehci_deregister(dev);
+	if (ret)
+		return ret;
 
 	return 0;
 }
+
+static const struct udevice_id ehci_usb_ids[] = {
+	{ .compatible = "nvidia,tegra20-ehci", .data = USB_CTLR_T20 },
+	{ .compatible = "nvidia,tegra30-ehci", .data = USB_CTLR_T30 },
+	{ .compatible = "nvidia,tegra114-ehci", .data = USB_CTLR_T114 },
+	{ }
+};
+
+U_BOOT_DRIVER(usb_ehci) = {
+	.name	= "ehci_tegra",
+	.id	= UCLASS_USB,
+	.of_match = ehci_usb_ids,
+	.ofdata_to_platdata = ehci_usb_ofdata_to_platdata,
+	.probe = ehci_usb_probe,
+	.remove = ehci_usb_remove,
+	.ops	= &ehci_usb_ops,
+	.platdata_auto_alloc_size = sizeof(struct usb_platdata),
+	.priv_auto_alloc_size = sizeof(struct fdt_usb),
+	.flags	= DM_FLAG_ALLOC_PRIV_DMA,
+};
+#endif
diff --git a/drivers/usb/host/ehci-vf.c b/drivers/usb/host/ehci-vf.c
new file mode 100644
index 0000000..5454855
--- /dev/null
+++ b/drivers/usb/host/ehci-vf.c
@@ -0,0 +1,164 @@
+/*
+ * Copyright (c) 2015 Sanchayan Maity <sanchayan.maity@toradex.com>
+ * Copyright (C) 2015 Toradex AG
+ *
+ * Based on ehci-mx6 driver
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <usb.h>
+#include <errno.h>
+#include <linux/compiler.h>
+#include <asm/io.h>
+#include <asm/arch/clock.h>
+#include <asm/arch/imx-regs.h>
+#include <asm/arch/crm_regs.h>
+#include <asm/imx-common/iomux-v3.h>
+#include <asm/imx-common/regs-usbphy.h>
+#include <usb/ehci-fsl.h>
+
+#include "ehci.h"
+
+#define USB_NC_REG_OFFSET				0x00000800
+
+#define ANADIG_PLL_CTRL_EN_USB_CLKS		(1 << 6)
+
+#define UCTRL_OVER_CUR_POL	(1 << 8) /* OTG Polarity of Overcurrent */
+#define UCTRL_OVER_CUR_DIS	(1 << 7) /* Disable OTG Overcurrent Detection */
+
+/* USBCMD */
+#define UCMD_RUN_STOP		(1 << 0) /* controller run/stop */
+#define UCMD_RESET			(1 << 1) /* controller reset */
+
+static const unsigned phy_bases[] = {
+	USB_PHY0_BASE_ADDR,
+	USB_PHY1_BASE_ADDR,
+};
+
+static const unsigned nc_reg_bases[] = {
+	USBC0_BASE_ADDR,
+	USBC1_BASE_ADDR,
+};
+
+static void usb_internal_phy_clock_gate(int index)
+{
+	void __iomem *phy_reg;
+
+	phy_reg = (void __iomem *)phy_bases[index];
+	clrbits_le32(phy_reg + USBPHY_CTRL, USBPHY_CTRL_CLKGATE);
+}
+
+static void usb_power_config(int index)
+{
+	struct anadig_reg __iomem *anadig =
+		(struct anadig_reg __iomem *)ANADIG_BASE_ADDR;
+	void __iomem *pll_ctrl;
+
+	switch (index) {
+	case 0:
+		pll_ctrl = &anadig->pll3_ctrl;
+		clrbits_le32(pll_ctrl, ANADIG_PLL3_CTRL_BYPASS);
+		setbits_le32(pll_ctrl, ANADIG_PLL3_CTRL_ENABLE
+			 | ANADIG_PLL3_CTRL_POWERDOWN
+			 | ANADIG_PLL_CTRL_EN_USB_CLKS);
+		break;
+	case 1:
+		pll_ctrl = &anadig->pll7_ctrl;
+		clrbits_le32(pll_ctrl, ANADIG_PLL7_CTRL_BYPASS);
+		setbits_le32(pll_ctrl, ANADIG_PLL7_CTRL_ENABLE
+			 | ANADIG_PLL7_CTRL_POWERDOWN
+			 | ANADIG_PLL_CTRL_EN_USB_CLKS);
+		break;
+	default:
+		return;
+	}
+}
+
+static void usb_phy_enable(int index, struct usb_ehci *ehci)
+{
+	void __iomem *phy_reg;
+	void __iomem *phy_ctrl;
+	void __iomem *usb_cmd;
+
+	phy_reg = (void __iomem *)phy_bases[index];
+	phy_ctrl = (void __iomem *)(phy_reg + USBPHY_CTRL);
+	usb_cmd = (void __iomem *)&ehci->usbcmd;
+
+	/* Stop then Reset */
+	clrbits_le32(usb_cmd, UCMD_RUN_STOP);
+	while (readl(usb_cmd) & UCMD_RUN_STOP)
+		;
+
+	setbits_le32(usb_cmd, UCMD_RESET);
+	while (readl(usb_cmd) & UCMD_RESET)
+		;
+
+	/* Reset USBPHY module */
+	setbits_le32(phy_ctrl, USBPHY_CTRL_SFTRST);
+	udelay(10);
+
+	/* Remove CLKGATE and SFTRST */
+	clrbits_le32(phy_ctrl, USBPHY_CTRL_CLKGATE | USBPHY_CTRL_SFTRST);
+	udelay(10);
+
+	/* Power up the PHY */
+	writel(0, phy_reg + USBPHY_PWD);
+
+	/* Enable FS/LS device */
+	setbits_le32(phy_ctrl, USBPHY_CTRL_ENUTMILEVEL2 |
+		 USBPHY_CTRL_ENUTMILEVEL3);
+}
+
+static void usb_oc_config(int index)
+{
+	void __iomem *ctrl;
+
+	ctrl = (void __iomem *)(nc_reg_bases[index] + USB_NC_REG_OFFSET);
+
+	setbits_le32(ctrl, UCTRL_OVER_CUR_POL);
+	setbits_le32(ctrl, UCTRL_OVER_CUR_DIS);
+}
+
+int ehci_hcd_init(int index, enum usb_init_type init,
+		struct ehci_hccr **hccr, struct ehci_hcor **hcor)
+{
+	struct usb_ehci *ehci;
+
+	if (index >= ARRAY_SIZE(nc_reg_bases))
+		return -EINVAL;
+
+	if (init == USB_INIT_DEVICE && index == 1)
+		return -ENODEV;
+	if (init == USB_INIT_HOST && index == 0)
+		return -ENODEV;
+
+	ehci = (struct usb_ehci *)nc_reg_bases[index];
+
+	usb_power_config(index);
+	usb_oc_config(index);
+	usb_internal_phy_clock_gate(index);
+	usb_phy_enable(index, ehci);
+
+	*hccr = (struct ehci_hccr *)((uint32_t)&ehci->caplength);
+	*hcor = (struct ehci_hcor *)((uint32_t)*hccr +
+			HC_LENGTH(ehci_readl(&(*hccr)->cr_capbase)));
+
+	if (init == USB_INIT_DEVICE) {
+		setbits_le32(&ehci->usbmode, CM_DEVICE);
+		writel((PORT_PTS_UTMI | PORT_PTS_PTW), &ehci->portsc);
+		setbits_le32(&ehci->portsc, USB_EN);
+	} else if (init == USB_INIT_HOST) {
+		setbits_le32(&ehci->usbmode, CM_HOST);
+		writel((PORT_PTS_UTMI | PORT_PTS_PTW), &ehci->portsc);
+		setbits_le32(&ehci->portsc, USB_EN);
+	}
+
+	return 0;
+}
+
+int ehci_hcd_stop(int index)
+{
+	return 0;
+}
diff --git a/drivers/usb/host/ehci.h b/drivers/usb/host/ehci.h
index 79aecd4..774282d 100644
--- a/drivers/usb/host/ehci.h
+++ b/drivers/usb/host/ehci.h
@@ -238,6 +238,22 @@ struct QH {
 	};
 };
 
+/* Tweak flags for EHCI, used to control operation */
+enum {
+	/* don't use or_configflag in init */
+	EHCI_TWEAK_NO_INIT_CF		= 1 << 0,
+};
+
+struct ehci_ctrl;
+
+struct ehci_ops {
+	void (*set_usb_mode)(struct ehci_ctrl *ctrl);
+	int (*get_port_speed)(struct ehci_ctrl *ctrl, uint32_t reg);
+	void (*powerup_fixup)(struct ehci_ctrl *ctrl, uint32_t *status_reg,
+			      uint32_t *reg);
+	uint32_t *(*get_portsc_register)(struct ehci_ctrl *ctrl, int port);
+};
+
 struct ehci_ctrl {
 	struct ehci_hccr *hccr;	/* R/O registers, not need for volatile */
 	struct ehci_hcor *hcor;
@@ -248,11 +264,42 @@ struct ehci_ctrl {
 	uint32_t *periodic_list;
 	int periodic_schedules;
 	int ntds;
+	struct ehci_ops ops;
+	void *priv;	/* client's private data */
 };
 
+/**
+ * ehci_set_controller_info() - Set up private data for the controller
+ *
+ * This function can be called in ehci_hcd_init() to tell the EHCI layer
+ * about the controller's private data pointer. Then in the above functions
+ * this can be accessed given the struct ehci_ctrl pointer. Also special
+ * EHCI operation methods can be provided if required
+ *
+ * @index:	Controller number to set
+ * @priv:	Controller pointer
+ * @ops:	Controller operations, or NULL to use default
+ */
+void ehci_set_controller_priv(int index, void *priv,
+			      const struct ehci_ops *ops);
+
+/**
+ * ehci_get_controller_priv() - Get controller private data
+ *
+ * @index	Controller number to get
+ * @return controller pointer for this index
+ */
+void *ehci_get_controller_priv(int index);
+
 /* Low level init functions */
 int ehci_hcd_init(int index, enum usb_init_type init,
 		struct ehci_hccr **hccr, struct ehci_hcor **hcor);
 int ehci_hcd_stop(int index);
 
+int ehci_register(struct udevice *dev, struct ehci_hccr *hccr,
+		  struct ehci_hcor *hcor, const struct ehci_ops *ops,
+		  uint tweaks, enum usb_init_type init);
+int ehci_deregister(struct udevice *dev);
+extern struct dm_usb_ops ehci_usb_ops;
+
 #endif /* USB_EHCI_H */
diff --git a/drivers/usb/host/usb-sandbox.c b/drivers/usb/host/usb-sandbox.c
new file mode 100644
index 0000000..c5f9822
--- /dev/null
+++ b/drivers/usb/host/usb-sandbox.c
@@ -0,0 +1,117 @@
+/*
+ * (C) Copyright 2015 Google, Inc
+ * Written by Simon Glass <sjg@chromium.org>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <dm.h>
+#include <usb.h>
+#include <dm/root.h>
+
+DECLARE_GLOBAL_DATA_PTR;
+
+static void usbmon_trace(struct udevice *bus, ulong pipe,
+			 struct devrequest *setup, struct udevice *emul)
+{
+	static const char types[] = "ZICB";
+	int type;
+
+	type = (pipe & USB_PIPE_TYPE_MASK) >> USB_PIPE_TYPE_SHIFT;
+	debug("0 0 S %c%c:%d:%03ld:%ld", types[type],
+	      pipe & USB_DIR_IN ? 'i' : 'o',
+	      bus->seq,
+	      (pipe & USB_PIPE_DEV_MASK) >> USB_PIPE_DEV_SHIFT,
+	      (pipe & USB_PIPE_EP_MASK) >> USB_PIPE_EP_SHIFT);
+	if (setup) {
+		debug(" s %02x %02x %04x %04x %04x", setup->requesttype,
+		      setup->request, setup->value, setup->index,
+		      setup->length);
+	}
+	debug(" %s", emul ? emul->name : "(no emul found)");
+
+	debug("\n");
+}
+
+static int sandbox_submit_control(struct udevice *bus,
+				      struct usb_device *udev,
+				      unsigned long pipe,
+				      void *buffer, int length,
+				      struct devrequest *setup)
+{
+	struct udevice *emul;
+	int ret;
+
+	/* Just use child of dev as emulator? */
+	debug("%s: bus=%s\n", __func__, bus->name);
+	ret = usb_emul_find(bus, pipe, &emul);
+	usbmon_trace(bus, pipe, setup, emul);
+	if (ret)
+		return ret;
+	ret = usb_emul_control(emul, udev, pipe, buffer, length, setup);
+	if (ret < 0) {
+		debug("ret=%d\n", ret);
+		udev->status = ret;
+		udev->act_len = 0;
+	} else {
+		udev->status = 0;
+		udev->act_len = ret;
+	}
+
+	return ret;
+}
+
+static int sandbox_submit_bulk(struct udevice *bus, struct usb_device *udev,
+			       unsigned long pipe, void *buffer, int length)
+{
+	struct udevice *emul;
+	int ret;
+
+	/* Just use child of dev as emulator? */
+	debug("%s: bus=%s\n", __func__, bus->name);
+	ret = usb_emul_find(bus, pipe, &emul);
+	usbmon_trace(bus, pipe, NULL, emul);
+	if (ret)
+		return ret;
+	ret = usb_emul_bulk(emul, udev, pipe, buffer, length);
+	if (ret < 0) {
+		debug("ret=%d\n", ret);
+		udev->status = ret;
+		udev->act_len = 0;
+	} else {
+		udev->status = 0;
+		udev->act_len = ret;
+	}
+
+	return ret;
+}
+
+static int sandbox_alloc_device(struct udevice *dev, struct usb_device *udev)
+{
+	return 0;
+}
+
+static int sandbox_usb_probe(struct udevice *dev)
+{
+	return 0;
+}
+
+static const struct dm_usb_ops sandbox_usb_ops = {
+	.control	= sandbox_submit_control,
+	.bulk		= sandbox_submit_bulk,
+	.alloc_device	= sandbox_alloc_device,
+};
+
+static const struct udevice_id sandbox_usb_ids[] = {
+	{ .compatible = "sandbox,usb" },
+	{ }
+};
+
+U_BOOT_DRIVER(usb_sandbox) = {
+	.name	= "usb_sandbox",
+	.id	= UCLASS_USB,
+	.of_match = sandbox_usb_ids,
+	.probe = sandbox_usb_probe,
+	.ops	= &sandbox_usb_ops,
+};
diff --git a/drivers/usb/host/usb-uclass.c b/drivers/usb/host/usb-uclass.c
new file mode 100644
index 0000000..714bc0e
--- /dev/null
+++ b/drivers/usb/host/usb-uclass.c
@@ -0,0 +1,645 @@
+/*
+ * (C) Copyright 2015 Google, Inc
+ * Written by Simon Glass <sjg@chromium.org>
+ *
+ * usb_match_device() modified from Linux kernel v4.0.
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <dm.h>
+#include <errno.h>
+#include <usb.h>
+#include <dm/device-internal.h>
+#include <dm/lists.h>
+#include <dm/root.h>
+#include <dm/uclass-internal.h>
+
+DECLARE_GLOBAL_DATA_PTR;
+
+extern bool usb_started; /* flag for the started/stopped USB status */
+static bool asynch_allowed;
+
+int usb_disable_asynch(int disable)
+{
+	int old_value = asynch_allowed;
+
+	asynch_allowed = !disable;
+	return old_value;
+}
+
+int submit_int_msg(struct usb_device *udev, unsigned long pipe, void *buffer,
+		   int length, int interval)
+{
+	struct udevice *bus = udev->controller_dev;
+	struct dm_usb_ops *ops = usb_get_ops(bus);
+
+	if (!ops->interrupt)
+		return -ENOSYS;
+
+	return ops->interrupt(bus, udev, pipe, buffer, length, interval);
+}
+
+int submit_control_msg(struct usb_device *udev, unsigned long pipe,
+		       void *buffer, int length, struct devrequest *setup)
+{
+	struct udevice *bus = udev->controller_dev;
+	struct dm_usb_ops *ops = usb_get_ops(bus);
+
+	if (!ops->control)
+		return -ENOSYS;
+
+	return ops->control(bus, udev, pipe, buffer, length, setup);
+}
+
+int submit_bulk_msg(struct usb_device *udev, unsigned long pipe, void *buffer,
+		    int length)
+{
+	struct udevice *bus = udev->controller_dev;
+	struct dm_usb_ops *ops = usb_get_ops(bus);
+
+	if (!ops->bulk)
+		return -ENOSYS;
+
+	return ops->bulk(bus, udev, pipe, buffer, length);
+}
+
+int usb_alloc_device(struct usb_device *udev)
+{
+	struct udevice *bus = udev->controller_dev;
+	struct dm_usb_ops *ops = usb_get_ops(bus);
+
+	/* This is only requird by some controllers - current XHCI */
+	if (!ops->alloc_device)
+		return 0;
+
+	return ops->alloc_device(bus, udev);
+}
+
+int usb_stop(void)
+{
+	struct udevice *bus;
+	struct uclass *uc;
+	int err = 0, ret;
+
+	/* De-activate any devices that have been activated */
+	ret = uclass_get(UCLASS_USB, &uc);
+	if (ret)
+		return ret;
+	uclass_foreach_dev(bus, uc) {
+		ret = device_remove(bus);
+		if (ret && !err)
+			err = ret;
+	}
+
+#ifdef CONFIG_SANDBOX
+	struct udevice *dev;
+
+	/* Reset all enulation devices */
+	ret = uclass_get(UCLASS_USB_EMUL, &uc);
+	if (ret)
+		return ret;
+
+	uclass_foreach_dev(dev, uc)
+		usb_emul_reset(dev);
+#endif
+	usb_stor_reset();
+	usb_hub_reset();
+	usb_started = 0;
+
+	return err;
+}
+
+static int usb_scan_bus(struct udevice *bus, bool recurse)
+{
+	struct usb_bus_priv *priv;
+	struct udevice *dev;
+	int ret;
+
+	priv = dev_get_uclass_priv(bus);
+
+	assert(recurse);	/* TODO: Support non-recusive */
+
+	ret = usb_scan_device(bus, 0, USB_SPEED_FULL, &dev);
+	if (ret)
+		return ret;
+
+	return priv->next_addr;
+}
+
+int usb_init(void)
+{
+	int controllers_initialized = 0;
+	struct udevice *bus;
+	struct uclass *uc;
+	int count = 0;
+	int ret;
+
+	asynch_allowed = 1;
+	usb_hub_reset();
+
+	ret = uclass_get(UCLASS_USB, &uc);
+	if (ret)
+		return ret;
+
+	uclass_foreach_dev(bus, uc) {
+		/* init low_level USB */
+		count++;
+		printf("USB");
+		printf("%d:   ", bus->seq);
+		ret = device_probe(bus);
+		if (ret == -ENODEV) {	/* No such device. */
+			puts("Port not available.\n");
+			controllers_initialized++;
+			continue;
+		}
+
+		if (ret) {		/* Other error. */
+			printf("probe failed, error %d\n", ret);
+			continue;
+		}
+		/*
+		 * lowlevel init is OK, now scan the bus for devices
+		 * i.e. search HUBs and configure them
+		 */
+		controllers_initialized++;
+		printf("scanning bus %d for devices... ", bus->seq);
+		debug("\n");
+		ret = usb_scan_bus(bus, true);
+		if (ret < 0)
+			printf("failed, error %d\n", ret);
+		else if (!ret)
+			printf("No USB Device found\n");
+		else
+			printf("%d USB Device(s) found\n", ret);
+		usb_started = true;
+	}
+
+	debug("scan end\n");
+	/* if we were not able to find at least one working bus, bail out */
+	if (!count)
+		printf("No controllers found\n");
+	else if (controllers_initialized == 0)
+		printf("USB error: all controllers failed lowlevel init\n");
+
+	return usb_started ? 0 : -1;
+}
+
+int usb_reset_root_port(void)
+{
+	return -ENOSYS;
+}
+
+static struct usb_device *find_child_devnum(struct udevice *parent, int devnum)
+{
+	struct usb_device *udev;
+	struct udevice *dev;
+
+	if (!device_active(parent))
+		return NULL;
+	udev = dev_get_parentdata(parent);
+	if (udev->devnum == devnum)
+		return udev;
+
+	for (device_find_first_child(parent, &dev);
+	     dev;
+	     device_find_next_child(&dev)) {
+		udev = find_child_devnum(dev, devnum);
+		if (udev)
+			return udev;
+	}
+
+	return NULL;
+}
+
+struct usb_device *usb_get_dev_index(struct udevice *bus, int index)
+{
+	struct udevice *hub;
+	int devnum = index + 1; /* Addresses are allocated from 1 on USB */
+
+	device_find_first_child(bus, &hub);
+	if (device_get_uclass_id(hub) == UCLASS_USB_HUB)
+		return find_child_devnum(hub, devnum);
+
+	return NULL;
+}
+
+int usb_post_bind(struct udevice *dev)
+{
+	/* Scan the bus for devices */
+	return dm_scan_fdt_node(dev, gd->fdt_blob, dev->of_offset, false);
+}
+
+int usb_port_reset(struct usb_device *parent, int portnr)
+{
+	unsigned short portstatus;
+	int ret;
+
+	debug("%s: start\n", __func__);
+
+	if (parent) {
+		/* reset the port for the second time */
+		assert(portnr > 0);
+		debug("%s: reset %d\n", __func__, portnr - 1);
+		ret = legacy_hub_port_reset(parent, portnr - 1, &portstatus);
+		if (ret < 0) {
+			printf("\n     Couldn't reset port %i\n", portnr);
+			return ret;
+		}
+	} else {
+		debug("%s: reset root\n", __func__);
+		usb_reset_root_port();
+	}
+
+	return 0;
+}
+
+int usb_legacy_port_reset(struct usb_device *parent, int portnr)
+{
+	return usb_port_reset(parent, portnr);
+}
+
+int usb_setup_ehci_gadget(struct ehci_ctrl **ctlrp)
+{
+	struct usb_platdata *plat;
+	struct udevice *dev;
+	int ret;
+
+	/* Find the old device and remove it */
+	ret = uclass_find_device_by_seq(UCLASS_USB, 0, true, &dev);
+	if (ret)
+		return ret;
+	ret = device_remove(dev);
+	if (ret)
+		return ret;
+
+	plat = dev_get_platdata(dev);
+	plat->init_type = USB_INIT_DEVICE;
+	ret = device_probe(dev);
+	if (ret)
+		return ret;
+	*ctlrp = dev_get_priv(dev);
+
+	return 0;
+}
+
+/* returns 0 if no match, 1 if match */
+int usb_match_device(const struct usb_device_descriptor *desc,
+		     const struct usb_device_id *id)
+{
+	if ((id->match_flags & USB_DEVICE_ID_MATCH_VENDOR) &&
+	    id->idVendor != le16_to_cpu(desc->idVendor))
+		return 0;
+
+	if ((id->match_flags & USB_DEVICE_ID_MATCH_PRODUCT) &&
+	    id->idProduct != le16_to_cpu(desc->idProduct))
+		return 0;
+
+	/* No need to test id->bcdDevice_lo != 0, since 0 is never
+	   greater than any unsigned number. */
+	if ((id->match_flags & USB_DEVICE_ID_MATCH_DEV_LO) &&
+	    (id->bcdDevice_lo > le16_to_cpu(desc->bcdDevice)))
+		return 0;
+
+	if ((id->match_flags & USB_DEVICE_ID_MATCH_DEV_HI) &&
+	    (id->bcdDevice_hi < le16_to_cpu(desc->bcdDevice)))
+		return 0;
+
+	if ((id->match_flags & USB_DEVICE_ID_MATCH_DEV_CLASS) &&
+	    (id->bDeviceClass != desc->bDeviceClass))
+		return 0;
+
+	if ((id->match_flags & USB_DEVICE_ID_MATCH_DEV_SUBCLASS) &&
+	    (id->bDeviceSubClass != desc->bDeviceSubClass))
+		return 0;
+
+	if ((id->match_flags & USB_DEVICE_ID_MATCH_DEV_PROTOCOL) &&
+	    (id->bDeviceProtocol != desc->bDeviceProtocol))
+		return 0;
+
+	return 1;
+}
+
+/* returns 0 if no match, 1 if match */
+int usb_match_one_id_intf(const struct usb_device_descriptor *desc,
+			  const struct usb_interface_descriptor *int_desc,
+			  const struct usb_device_id *id)
+{
+	/* The interface class, subclass, protocol and number should never be
+	 * checked for a match if the device class is Vendor Specific,
+	 * unless the match record specifies the Vendor ID. */
+	if (desc->bDeviceClass == USB_CLASS_VENDOR_SPEC &&
+	    !(id->match_flags & USB_DEVICE_ID_MATCH_VENDOR) &&
+	    (id->match_flags & (USB_DEVICE_ID_MATCH_INT_CLASS |
+				USB_DEVICE_ID_MATCH_INT_SUBCLASS |
+				USB_DEVICE_ID_MATCH_INT_PROTOCOL |
+				USB_DEVICE_ID_MATCH_INT_NUMBER)))
+		return 0;
+
+	if ((id->match_flags & USB_DEVICE_ID_MATCH_INT_CLASS) &&
+	    (id->bInterfaceClass != int_desc->bInterfaceClass))
+		return 0;
+
+	if ((id->match_flags & USB_DEVICE_ID_MATCH_INT_SUBCLASS) &&
+	    (id->bInterfaceSubClass != int_desc->bInterfaceSubClass))
+		return 0;
+
+	if ((id->match_flags & USB_DEVICE_ID_MATCH_INT_PROTOCOL) &&
+	    (id->bInterfaceProtocol != int_desc->bInterfaceProtocol))
+		return 0;
+
+	if ((id->match_flags & USB_DEVICE_ID_MATCH_INT_NUMBER) &&
+	    (id->bInterfaceNumber != int_desc->bInterfaceNumber))
+		return 0;
+
+	return 1;
+}
+
+/* returns 0 if no match, 1 if match */
+int usb_match_one_id(struct usb_device_descriptor *desc,
+		     struct usb_interface_descriptor *int_desc,
+		     const struct usb_device_id *id)
+{
+	if (!usb_match_device(desc, id))
+		return 0;
+
+	return usb_match_one_id_intf(desc, int_desc, id);
+}
+
+/**
+ * usb_find_and_bind_driver() - Find and bind the right USB driver
+ *
+ * This only looks at certain fields in the descriptor.
+ */
+static int usb_find_and_bind_driver(struct udevice *parent,
+				    struct usb_device_descriptor *desc,
+				    struct usb_interface_descriptor *iface,
+				    int bus_seq, int devnum,
+				    struct udevice **devp)
+{
+	struct usb_driver_entry *start, *entry;
+	int n_ents;
+	int ret;
+	char name[30], *str;
+
+	*devp = NULL;
+	debug("%s: Searching for driver\n", __func__);
+	start = ll_entry_start(struct usb_driver_entry, usb_driver_entry);
+	n_ents = ll_entry_count(struct usb_driver_entry, usb_driver_entry);
+	for (entry = start; entry != start + n_ents; entry++) {
+		const struct usb_device_id *id;
+		struct udevice *dev;
+		const struct driver *drv;
+		struct usb_dev_platdata *plat;
+
+		for (id = entry->match; id->match_flags; id++) {
+			if (!usb_match_one_id(desc, iface, id))
+				continue;
+
+			drv = entry->driver;
+			/*
+			 * We could pass the descriptor to the driver as
+			 * platdata (instead of NULL) and allow its bind()
+			 * method to return -ENOENT if it doesn't support this
+			 * device. That way we could continue the search to
+			 * find another driver. For now this doesn't seem
+			 * necesssary, so just bind the first match.
+			 */
+			ret = device_bind(parent, drv, drv->name, NULL, -1,
+					  &dev);
+			if (ret)
+				goto error;
+			debug("%s: Match found: %s\n", __func__, drv->name);
+			dev->driver_data = id->driver_info;
+			plat = dev_get_parent_platdata(dev);
+			plat->id = *id;
+			*devp = dev;
+			return 0;
+		}
+	}
+
+	/* Bind a generic driver so that the device can be used */
+	snprintf(name, sizeof(name), "generic_bus_%x_dev_%x", bus_seq, devnum);
+	str = strdup(name);
+	if (!str)
+		return -ENOMEM;
+	ret = device_bind_driver(parent, "usb_dev_generic_drv", str, devp);
+
+error:
+	debug("%s: No match found: %d\n", __func__, ret);
+	return ret;
+}
+
+/**
+ * usb_find_child() - Find an existing device which matches our needs
+ *
+ *
+ */
+static int usb_find_child(struct udevice *parent,
+			  struct usb_device_descriptor *desc,
+			  struct usb_interface_descriptor *iface,
+			  struct udevice **devp)
+{
+	struct udevice *dev;
+
+	*devp = NULL;
+	for (device_find_first_child(parent, &dev);
+	     dev;
+	     device_find_next_child(&dev)) {
+		struct usb_dev_platdata *plat = dev_get_parent_platdata(dev);
+
+		/* If this device is already in use, skip it */
+		if (device_active(dev))
+			continue;
+		debug("   %s: name='%s', plat=%d, desc=%d\n", __func__,
+		      dev->name, plat->id.bDeviceClass, desc->bDeviceClass);
+		if (usb_match_one_id(desc, iface, &plat->id)) {
+			*devp = dev;
+			return 0;
+		}
+	}
+
+	return -ENOENT;
+}
+
+int usb_scan_device(struct udevice *parent, int port,
+		    enum usb_device_speed speed, struct udevice **devp)
+{
+	struct udevice *dev;
+	bool created = false;
+	struct usb_dev_platdata *plat;
+	struct usb_bus_priv *priv;
+	struct usb_device *parent_udev;
+	int ret;
+	ALLOC_CACHE_ALIGN_BUFFER(struct usb_device, udev, 1);
+	struct usb_interface_descriptor *iface = &udev->config.if_desc[0].desc;
+
+	*devp = NULL;
+	memset(udev, '\0', sizeof(*udev));
+	ret = usb_get_bus(parent, &udev->controller_dev);
+	if (ret)
+		return ret;
+	priv = dev_get_uclass_priv(udev->controller_dev);
+
+	/*
+	 * Somewhat nasty, this. We create a local device and use the normal
+	 * USB stack to read its descriptor. Then we know what type of device
+	 * to create for real.
+	 *
+	 * udev->dev is set to the parent, since we don't have a real device
+	 * yet. The USB stack should not access udev.dev anyway, except perhaps
+	 * to find the controller, and the controller will either be @parent,
+	 * or some parent of @parent.
+	 *
+	 * Another option might be to create the device as a generic USB
+	 * device, then morph it into the correct one when we know what it
+	 * should be. This means that a generic USB device would morph into
+	 * a network controller, or a USB flash stick, for example. However,
+	 * we don't support such morphing and it isn't clear that it would
+	 * be easy to do.
+	 *
+	 * Yet another option is to split out the USB stack parts of udev
+	 * into something like a 'struct urb' (as Linux does) which can exist
+	 * independently of any device. This feels cleaner, but calls for quite
+	 * a big change to the USB stack.
+	 *
+	 * For now, the approach is to set up an empty udev, read its
+	 * descriptor and assign it an address, then bind a real device and
+	 * stash the resulting information into the device's parent
+	 * platform data. Then when we probe it, usb_child_pre_probe() is called
+	 * and it will pull the information out of the stash.
+	 */
+	udev->dev = parent;
+	udev->speed = speed;
+	udev->devnum = priv->next_addr + 1;
+	udev->portnr = port;
+	debug("Calling usb_setup_device(), portnr=%d\n", udev->portnr);
+	parent_udev = device_get_uclass_id(parent) == UCLASS_USB_HUB ?
+		dev_get_parentdata(parent) : NULL;
+	ret = usb_setup_device(udev, priv->desc_before_addr, parent_udev, port);
+	debug("read_descriptor for '%s': ret=%d\n", parent->name, ret);
+	if (ret)
+		return ret;
+	ret = usb_find_child(parent, &udev->descriptor, iface, &dev);
+	debug("** usb_find_child returns %d\n", ret);
+	if (ret) {
+		if (ret != -ENOENT)
+			return ret;
+		ret = usb_find_and_bind_driver(parent, &udev->descriptor, iface,
+					       udev->controller_dev->seq,
+					       udev->devnum, &dev);
+		if (ret)
+			return ret;
+		created = true;
+	}
+	plat = dev_get_parent_platdata(dev);
+	debug("%s: Probing '%s', plat=%p\n", __func__, dev->name, plat);
+	plat->devnum = udev->devnum;
+	plat->speed = udev->speed;
+	plat->slot_id = udev->slot_id;
+	plat->portnr = port;
+	debug("** device '%s': stashing slot_id=%d\n", dev->name,
+	      plat->slot_id);
+	priv->next_addr++;
+	ret = device_probe(dev);
+	if (ret) {
+		debug("%s: Device '%s' probe failed\n", __func__, dev->name);
+		priv->next_addr--;
+		if (created)
+			device_unbind(dev);
+		return ret;
+	}
+	*devp = dev;
+
+	return 0;
+}
+
+int usb_child_post_bind(struct udevice *dev)
+{
+	struct usb_dev_platdata *plat = dev_get_parent_platdata(dev);
+	const void *blob = gd->fdt_blob;
+	int val;
+
+	if (dev->of_offset == -1)
+		return 0;
+
+	/* We only support matching a few things */
+	val = fdtdec_get_int(blob, dev->of_offset, "usb,device-class", -1);
+	if (val != -1) {
+		plat->id.match_flags |= USB_DEVICE_ID_MATCH_DEV_CLASS;
+		plat->id.bDeviceClass = val;
+	}
+	val = fdtdec_get_int(blob, dev->of_offset, "usb,interface-class", -1);
+	if (val != -1) {
+		plat->id.match_flags |= USB_DEVICE_ID_MATCH_INT_CLASS;
+		plat->id.bInterfaceClass = val;
+	}
+
+	return 0;
+}
+
+int usb_get_bus(struct udevice *dev, struct udevice **busp)
+{
+	struct udevice *bus;
+
+	*busp = NULL;
+	for (bus = dev; bus && device_get_uclass_id(bus) != UCLASS_USB; )
+		bus = bus->parent;
+	if (!bus) {
+		/* By design this cannot happen */
+		assert(bus);
+		debug("USB HUB '%s' does not have a controller\n", dev->name);
+		return -EXDEV;
+	}
+	*busp = bus;
+
+	return 0;
+}
+
+int usb_child_pre_probe(struct udevice *dev)
+{
+	struct udevice *bus;
+	struct usb_device *udev = dev_get_parentdata(dev);
+	struct usb_dev_platdata *plat = dev_get_parent_platdata(dev);
+	int ret;
+
+	ret = usb_get_bus(dev, &bus);
+	if (ret)
+		return ret;
+	udev->controller_dev = bus;
+	udev->dev = dev;
+	udev->devnum = plat->devnum;
+	udev->slot_id = plat->slot_id;
+	udev->portnr = plat->portnr;
+	udev->speed = plat->speed;
+	debug("** device '%s': getting slot_id=%d\n", dev->name, plat->slot_id);
+
+	ret = usb_select_config(udev);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+UCLASS_DRIVER(usb) = {
+	.id		= UCLASS_USB,
+	.name		= "usb",
+	.flags		= DM_UC_FLAG_SEQ_ALIAS,
+	.post_bind	= usb_post_bind,
+	.per_child_auto_alloc_size = sizeof(struct usb_device),
+	.per_device_auto_alloc_size = sizeof(struct usb_bus_priv),
+	.child_post_bind = usb_child_post_bind,
+	.child_pre_probe = usb_child_pre_probe,
+	.per_child_platdata_auto_alloc_size = sizeof(struct usb_dev_platdata),
+};
+
+UCLASS_DRIVER(usb_dev_generic) = {
+	.id		= UCLASS_USB_DEV_GENERIC,
+	.name		= "usb_dev_generic",
+};
+
+U_BOOT_DRIVER(usb_dev_generic_drv) = {
+	.id		= UCLASS_USB_DEV_GENERIC,
+	.name		= "usb_dev_generic_drv",
+};
diff --git a/drivers/usb/host/xhci-exynos5.c b/drivers/usb/host/xhci-exynos5.c
index 3f86fdc..23c7ecc 100644
--- a/drivers/usb/host/xhci-exynos5.c
+++ b/drivers/usb/host/xhci-exynos5.c
@@ -14,6 +14,7 @@
  */
 
 #include <common.h>
+#include <dm.h>
 #include <fdtdec.h>
 #include <libfdt.h>
 #include <malloc.h>
@@ -32,20 +33,76 @@
 /* Declare global data pointer */
 DECLARE_GLOBAL_DATA_PTR;
 
+#ifdef CONFIG_DM_USB
+struct exynos_xhci_platdata {
+	fdt_addr_t hcd_base;
+	fdt_addr_t phy_base;
+	struct gpio_desc vbus_gpio;
+};
+#endif
+
 /**
  * Contains pointers to register base addresses
  * for the usb controller.
  */
 struct exynos_xhci {
+#ifdef CONFIG_DM_USB
+	struct usb_platdata usb_plat;
+#endif
+	struct xhci_ctrl ctrl;
 	struct exynos_usb3_phy *usb3_phy;
 	struct xhci_hccr *hcd;
 	struct dwc3 *dwc3_reg;
+#ifndef CONFIG_DM_USB
 	struct gpio_desc vbus_gpio;
+#endif
 };
 
+#ifndef CONFIG_DM_USB
 static struct exynos_xhci exynos;
+#endif
 
-#ifdef CONFIG_OF_CONTROL
+#ifdef CONFIG_DM_USB
+static int xhci_usb_ofdata_to_platdata(struct udevice *dev)
+{
+	struct exynos_xhci_platdata *plat = dev_get_platdata(dev);
+	const void *blob = gd->fdt_blob;
+	unsigned int node;
+	int depth;
+
+	/*
+	 * Get the base address for XHCI controller from the device node
+	 */
+	plat->hcd_base = fdtdec_get_addr(blob, dev->of_offset, "reg");
+	if (plat->hcd_base == FDT_ADDR_T_NONE) {
+		debug("Can't get the XHCI register base address\n");
+		return -ENXIO;
+	}
+
+	depth = 0;
+	node = fdtdec_next_compatible_subnode(blob, dev->of_offset,
+				COMPAT_SAMSUNG_EXYNOS5_USB3_PHY, &depth);
+	if (node <= 0) {
+		debug("XHCI: Can't get device node for usb3-phy controller\n");
+		return -ENODEV;
+	}
+
+	/*
+	 * Get the base address for usbphy from the device node
+	 */
+	plat->phy_base = fdtdec_get_addr(blob, node, "reg");
+	if (plat->phy_base == FDT_ADDR_T_NONE) {
+		debug("Can't get the usbphy register address\n");
+		return -ENXIO;
+	}
+
+	/* Vbus gpio */
+	gpio_request_by_name(dev, "samsung,vbus-gpio", 0,
+			     &plat->vbus_gpio, GPIOD_IS_OUT);
+
+	return 0;
+}
+#else
 static int exynos_usb3_parse_dt(const void *blob, struct exynos_xhci *exynos)
 {
 	fdt_addr_t addr;
@@ -283,6 +340,7 @@ static void exynos_xhci_core_exit(struct exynos_xhci *exynos)
 	exynos5_usb3_phy_exit(exynos->usb3_phy);
 }
 
+#ifndef CONFIG_DM_USB
 int xhci_hcd_init(int index, struct xhci_hccr **hccr, struct xhci_hcor **hcor)
 {
 	struct exynos_xhci *ctx = &exynos;
@@ -326,3 +384,63 @@ void xhci_hcd_stop(int index)
 
 	exynos_xhci_core_exit(ctx);
 }
+#endif
+
+#ifdef CONFIG_DM_USB
+static int xhci_usb_probe(struct udevice *dev)
+{
+	struct exynos_xhci_platdata *plat = dev_get_platdata(dev);
+	struct exynos_xhci *ctx = dev_get_priv(dev);
+	struct xhci_hcor *hcor;
+	int ret;
+
+	ctx->hcd = (struct xhci_hccr *)plat->hcd_base;
+	ctx->usb3_phy = (struct exynos_usb3_phy *)plat->phy_base;
+	ctx->dwc3_reg = (struct dwc3 *)((char *)(ctx->hcd) + DWC3_REG_OFFSET);
+	hcor = (struct xhci_hcor *)((uint32_t)ctx->hcd +
+			HC_LENGTH(xhci_readl(&ctx->hcd->cr_capbase)));
+
+	/* setup the Vbus gpio here */
+	if (dm_gpio_is_valid(&plat->vbus_gpio))
+		dm_gpio_set_value(&plat->vbus_gpio, 1);
+
+	ret = exynos_xhci_core_init(ctx);
+	if (ret) {
+		puts("XHCI: failed to initialize controller\n");
+		return -EINVAL;
+	}
+
+	return xhci_register(dev, ctx->hcd, hcor);
+}
+
+static int xhci_usb_remove(struct udevice *dev)
+{
+	struct exynos_xhci *ctx = dev_get_priv(dev);
+	int ret;
+
+	ret = xhci_deregister(dev);
+	if (ret)
+		return ret;
+	exynos_xhci_core_exit(ctx);
+
+	return 0;
+}
+
+static const struct udevice_id xhci_usb_ids[] = {
+	{ .compatible = "samsung,exynos5250-xhci" },
+	{ }
+};
+
+U_BOOT_DRIVER(usb_xhci) = {
+	.name	= "xhci_exynos",
+	.id	= UCLASS_USB,
+	.of_match = xhci_usb_ids,
+	.ofdata_to_platdata = xhci_usb_ofdata_to_platdata,
+	.probe = xhci_usb_probe,
+	.remove = xhci_usb_remove,
+	.ops	= &xhci_usb_ops,
+	.platdata_auto_alloc_size = sizeof(struct exynos_xhci_platdata),
+	.priv_auto_alloc_size = sizeof(struct exynos_xhci),
+	.flags	= DM_FLAG_ALLOC_PRIV_DMA,
+};
+#endif
diff --git a/drivers/usb/host/xhci-mem.c b/drivers/usb/host/xhci-mem.c
index 10f11cd..3744452 100644
--- a/drivers/usb/host/xhci-mem.c
+++ b/drivers/usb/host/xhci-mem.c
@@ -15,6 +15,7 @@
  */
 
 #include <common.h>
+#include <dm.h>
 #include <asm/byteorder.h>
 #include <usb.h>
 #include <malloc.h>
@@ -352,12 +353,10 @@ static struct xhci_container_ctx
  * @param udev	pointer to USB deivce structure
  * @return 0 on success else -1 on failure
  */
-int xhci_alloc_virt_device(struct usb_device *udev)
+int xhci_alloc_virt_device(struct xhci_ctrl *ctrl, unsigned int slot_id)
 {
 	u64 byte_64 = 0;
-	unsigned int slot_id = udev->slot_id;
 	struct xhci_virt_device *virt_dev;
-	struct xhci_ctrl *ctrl = udev->controller;
 
 	/* Slot ID 0 is reserved */
 	if (ctrl->devs[slot_id]) {
@@ -627,17 +626,16 @@ void xhci_slot_copy(struct xhci_ctrl *ctrl, struct xhci_container_ctx *in_ctx,
  * @param udev pointer to the Device Data Structure
  * @return returns negative value on failure else 0 on success
  */
-void xhci_setup_addressable_virt_dev(struct usb_device *udev)
+void xhci_setup_addressable_virt_dev(struct xhci_ctrl *ctrl, int slot_id,
+				     int speed, int hop_portnr)
 {
-	struct usb_device *hop = udev;
 	struct xhci_virt_device *virt_dev;
 	struct xhci_ep_ctx *ep0_ctx;
 	struct xhci_slot_ctx *slot_ctx;
 	u32 port_num = 0;
 	u64 trb_64 = 0;
-	struct xhci_ctrl *ctrl = udev->controller;
 
-	virt_dev = ctrl->devs[udev->slot_id];
+	virt_dev = ctrl->devs[slot_id];
 
 	BUG_ON(!virt_dev);
 
@@ -648,7 +646,7 @@ void xhci_setup_addressable_virt_dev(struct usb_device *udev)
 	/* Only the control endpoint is valid - one endpoint context */
 	slot_ctx->dev_info |= cpu_to_le32(LAST_CTX(1) | 0);
 
-	switch (udev->speed) {
+	switch (speed) {
 	case USB_SPEED_SUPER:
 		slot_ctx->dev_info |= cpu_to_le32(SLOT_SPEED_SS);
 		break;
@@ -666,11 +664,7 @@ void xhci_setup_addressable_virt_dev(struct usb_device *udev)
 		BUG();
 	}
 
-	/* Extract the root hub port number */
-	if (hop->parent)
-		while (hop->parent->parent)
-			hop = hop->parent;
-	port_num = hop->portnr;
+	port_num = hop_portnr;
 	debug("port_num = %d\n", port_num);
 
 	slot_ctx->dev_info2 |=
@@ -680,9 +674,9 @@ void xhci_setup_addressable_virt_dev(struct usb_device *udev)
 	/* Step 4 - ring already allocated */
 	/* Step 5 */
 	ep0_ctx->ep_info2 = cpu_to_le32(CTRL_EP << EP_TYPE_SHIFT);
-	debug("SPEED = %d\n", udev->speed);
+	debug("SPEED = %d\n", speed);
 
-	switch (udev->speed) {
+	switch (speed) {
 	case USB_SPEED_SUPER:
 		ep0_ctx->ep_info2 |= cpu_to_le32(((512 & MAX_PACKET_MASK) <<
 					MAX_PACKET_SHIFT));
diff --git a/drivers/usb/host/xhci-ring.c b/drivers/usb/host/xhci-ring.c
index f3759d4..5a1391f 100644
--- a/drivers/usb/host/xhci-ring.c
+++ b/drivers/usb/host/xhci-ring.c
@@ -353,7 +353,7 @@ static void giveback_first_trb(struct usb_device *udev, int ep_index,
 				int start_cycle,
 				struct xhci_generic_trb *start_trb)
 {
-	struct xhci_ctrl *ctrl = udev->controller;
+	struct xhci_ctrl *ctrl = xhci_get_ctrl(udev);
 
 	/*
 	 * Pass all the TRBs to the hardware at once and make sure this write
@@ -477,7 +477,7 @@ union xhci_trb *xhci_wait_for_event(struct xhci_ctrl *ctrl, trb_type expected)
  */
 static void abort_td(struct usb_device *udev, int ep_index)
 {
-	struct xhci_ctrl *ctrl = udev->controller;
+	struct xhci_ctrl *ctrl = xhci_get_ctrl(udev);
 	struct xhci_ring *ring =  ctrl->devs[udev->slot_id]->eps[ep_index].ring;
 	union xhci_trb *event;
 	u32 field;
@@ -554,7 +554,7 @@ int xhci_bulk_tx(struct usb_device *udev, unsigned long pipe,
 	int start_cycle;
 	u32 field = 0;
 	u32 length_field = 0;
-	struct xhci_ctrl *ctrl = udev->controller;
+	struct xhci_ctrl *ctrl = xhci_get_ctrl(udev);
 	int slot_id = udev->slot_id;
 	int ep_index;
 	struct xhci_virt_device *virt_dev;
@@ -748,7 +748,7 @@ int xhci_ctrl_tx(struct usb_device *udev, unsigned long pipe,
 	u32 length_field;
 	u64 buf_64 = 0;
 	struct xhci_generic_trb *start_trb;
-	struct xhci_ctrl *ctrl = udev->controller;
+	struct xhci_ctrl *ctrl = xhci_get_ctrl(udev);
 	int slot_id = udev->slot_id;
 	int ep_index;
 	u32 trb_fields[4];
diff --git a/drivers/usb/host/xhci.c b/drivers/usb/host/xhci.c
index f8b5ce4..0b09643 100644
--- a/drivers/usb/host/xhci.c
+++ b/drivers/usb/host/xhci.c
@@ -21,6 +21,7 @@
  */
 
 #include <common.h>
+#include <dm.h>
 #include <asm/byteorder.h>
 #include <usb.h>
 #include <malloc.h>
@@ -108,7 +109,25 @@ static struct descriptor {
 	},
 };
 
+#ifndef CONFIG_DM_USB
 static struct xhci_ctrl xhcic[CONFIG_USB_MAX_CONTROLLER_COUNT];
+#endif
+
+struct xhci_ctrl *xhci_get_ctrl(struct usb_device *udev)
+{
+#ifdef CONFIG_DM_USB
+	struct udevice *dev;
+
+	/* Find the USB controller */
+	for (dev = udev->dev;
+	     device_get_uclass_id(dev) != UCLASS_USB;
+	     dev = dev->parent)
+		;
+	return dev_get_priv(dev);
+#else
+	return udev->controller;
+#endif
+}
 
 /**
  * Waits for as per specified amount of time
@@ -250,7 +269,7 @@ static int xhci_configure_endpoints(struct usb_device *udev, bool ctx_change)
 {
 	struct xhci_container_ctx *in_ctx;
 	struct xhci_virt_device *virt_dev;
-	struct xhci_ctrl *ctrl = udev->controller;
+	struct xhci_ctrl *ctrl = xhci_get_ctrl(udev);
 	union xhci_trb *event;
 
 	virt_dev = ctrl->devs[udev->slot_id];
@@ -298,7 +317,7 @@ static int xhci_set_configuration(struct usb_device *udev)
 	int ep_index;
 	unsigned int dir;
 	unsigned int ep_type;
-	struct xhci_ctrl *ctrl = udev->controller;
+	struct xhci_ctrl *ctrl = xhci_get_ctrl(udev);
 	int num_of_ep;
 	int ep_flag = 0;
 	u64 trb_64 = 0;
@@ -379,10 +398,10 @@ static int xhci_set_configuration(struct usb_device *udev)
  * @param udev pointer to the Device Data Structure
  * @return 0 if successful else error code on failure
  */
-static int xhci_address_device(struct usb_device *udev)
+static int xhci_address_device(struct usb_device *udev, int root_portnr)
 {
 	int ret = 0;
-	struct xhci_ctrl *ctrl = udev->controller;
+	struct xhci_ctrl *ctrl = xhci_get_ctrl(udev);
 	struct xhci_slot_ctx *slot_ctx;
 	struct xhci_input_control_ctx *ctrl_ctx;
 	struct xhci_virt_device *virt_dev;
@@ -395,8 +414,9 @@ static int xhci_address_device(struct usb_device *udev)
 	 * This is the first Set Address since device plug-in
 	 * so setting up the slot context.
 	 */
-	debug("Setting up addressable devices\n");
-	xhci_setup_addressable_virt_dev(udev);
+	debug("Setting up addressable devices %p\n", ctrl->dcbaa);
+	xhci_setup_addressable_virt_dev(ctrl, udev->slot_id, udev->speed,
+					root_portnr);
 
 	ctrl_ctx = xhci_get_input_control_ctx(virt_dev->in_ctx);
 	ctrl_ctx->add_flags = cpu_to_le32(SLOT_FLAG | EP0_FLAG);
@@ -461,10 +481,10 @@ static int xhci_address_device(struct usb_device *udev)
  * @param udev	pointer to the Device Data Structure
  * @return Returns 0 on succes else return error code on failure
  */
-int usb_alloc_device(struct usb_device *udev)
+int _xhci_alloc_device(struct usb_device *udev)
 {
+	struct xhci_ctrl *ctrl = xhci_get_ctrl(udev);
 	union xhci_trb *event;
-	struct xhci_ctrl *ctrl = udev->controller;
 	int ret;
 
 	/*
@@ -486,7 +506,7 @@ int usb_alloc_device(struct usb_device *udev)
 
 	xhci_acknowledge_event(ctrl);
 
-	ret = xhci_alloc_virt_device(udev);
+	ret = xhci_alloc_virt_device(ctrl, udev->slot_id);
 	if (ret < 0) {
 		/*
 		 * TODO: Unsuccessful Address Device command shall leave
@@ -499,6 +519,13 @@ int usb_alloc_device(struct usb_device *udev)
 	return 0;
 }
 
+#ifndef CONFIG_DM_USB
+int usb_alloc_device(struct usb_device *udev)
+{
+	return _xhci_alloc_device(udev);
+}
+#endif
+
 /*
  * Full speed devices may have a max packet size greater than 8 bytes, but the
  * USB core doesn't know that until it reads the first 8 bytes of the
@@ -510,7 +537,7 @@ int usb_alloc_device(struct usb_device *udev)
  */
 int xhci_check_maxpacket(struct usb_device *udev)
 {
-	struct xhci_ctrl *ctrl = udev->controller;
+	struct xhci_ctrl *ctrl = xhci_get_ctrl(udev);
 	unsigned int slot_id = udev->slot_id;
 	int ep_index = 0;	/* control endpoint */
 	struct xhci_container_ctx *in_ctx;
@@ -640,7 +667,7 @@ static int xhci_submit_root(struct usb_device *udev, unsigned long pipe,
 	int len, srclen;
 	uint32_t reg;
 	volatile uint32_t *status_reg;
-	struct xhci_ctrl *ctrl = udev->controller;
+	struct xhci_ctrl *ctrl = xhci_get_ctrl(udev);
 	struct xhci_hcor *hcor = ctrl->hcor;
 
 	if ((req->requesttype & USB_RT_PORT) &&
@@ -677,7 +704,7 @@ static int xhci_submit_root(struct usb_device *udev, unsigned long pipe,
 				srclen = 4;
 				break;
 			case 1:	/* Vendor String  */
-				srcptr = "\16\3u\0-\0b\0o\0o\0t\0";
+				srcptr = "\16\3U\0-\0B\0o\0o\0t\0";
 				srclen = 14;
 				break;
 			case 2:	/* Product Name */
@@ -858,9 +885,8 @@ unknown:
  * @param interval	interval of the interrupt
  * @return 0
  */
-int
-submit_int_msg(struct usb_device *udev, unsigned long pipe, void *buffer,
-						int length, int interval)
+static int _xhci_submit_int_msg(struct usb_device *udev, unsigned long pipe,
+				void *buffer, int length, int interval)
 {
 	/*
 	 * TODO: Not addressing any interrupt type transfer requests
@@ -878,9 +904,8 @@ submit_int_msg(struct usb_device *udev, unsigned long pipe, void *buffer,
  * @param length	length of the buffer
  * @return returns 0 if successful else -1 on failure
  */
-int
-submit_bulk_msg(struct usb_device *udev, unsigned long pipe, void *buffer,
-								int length)
+static int _xhci_submit_bulk_msg(struct usb_device *udev, unsigned long pipe,
+				 void *buffer, int length)
 {
 	if (usb_pipetype(pipe) != PIPE_BULK) {
 		printf("non-bulk pipe (type=%lu)", usb_pipetype(pipe));
@@ -898,13 +923,14 @@ submit_bulk_msg(struct usb_device *udev, unsigned long pipe, void *buffer,
  * @param buffer	buffer to be read/written based on the request
  * @param length	length of the buffer
  * @param setup		Request type
+ * @param root_portnr	Root port number that this device is on
  * @return returns 0 if successful else -1 on failure
  */
-int
-submit_control_msg(struct usb_device *udev, unsigned long pipe, void *buffer,
-					int length, struct devrequest *setup)
+static int _xhci_submit_control_msg(struct usb_device *udev, unsigned long pipe,
+				    void *buffer, int length,
+				    struct devrequest *setup, int root_portnr)
 {
-	struct xhci_ctrl *ctrl = udev->controller;
+	struct xhci_ctrl *ctrl = xhci_get_ctrl(udev);
 	int ret = 0;
 
 	if (usb_pipetype(pipe) != PIPE_CONTROL) {
@@ -916,7 +942,7 @@ submit_control_msg(struct usb_device *udev, unsigned long pipe, void *buffer,
 		return xhci_submit_root(udev, pipe, buffer, setup);
 
 	if (setup->request == USB_REQ_SET_ADDRESS)
-		return xhci_address_device(udev);
+		return xhci_address_device(udev, root_portnr);
 
 	if (setup->request == USB_REQ_SET_CONFIGURATION) {
 		ret = xhci_set_configuration(udev);
@@ -929,33 +955,16 @@ submit_control_msg(struct usb_device *udev, unsigned long pipe, void *buffer,
 	return xhci_ctrl_tx(udev, pipe, setup, length, buffer);
 }
 
-/**
- * Intialises the XHCI host controller
- * and allocates the necessary data structures
- *
- * @param index	index to the host controller data structure
- * @return pointer to the intialised controller
- */
-int usb_lowlevel_init(int index, enum usb_init_type init, void **controller)
+static int xhci_lowlevel_init(struct xhci_ctrl *ctrl)
 {
+	struct xhci_hccr *hccr;
+	struct xhci_hcor *hcor;
 	uint32_t val;
 	uint32_t val2;
 	uint32_t reg;
-	struct xhci_hccr *hccr;
-	struct xhci_hcor *hcor;
-	struct xhci_ctrl *ctrl;
-
-	if (xhci_hcd_init(index, &hccr, (struct xhci_hcor **)&hcor) != 0)
-		return -ENODEV;
-
-	if (xhci_reset(hcor) != 0)
-		return -ENODEV;
-
-	ctrl = &xhcic[index];
-
-	ctrl->hccr = hccr;
-	ctrl->hcor = hcor;
 
+	hccr = ctrl->hccr;
+	hcor = ctrl->hcor;
 	/*
 	 * Program the Number of Device Slots Enabled field in the CONFIG
 	 * register with the max value of slots the HC can handle.
@@ -997,11 +1006,82 @@ int usb_lowlevel_init(int index, enum usb_init_type init, void **controller)
 	reg = HC_VERSION(xhci_readl(&hccr->cr_capbase));
 	printf("USB XHCI %x.%02x\n", reg >> 8, reg & 0xff);
 
-	*controller = &xhcic[index];
+	return 0;
+}
+
+static int xhci_lowlevel_stop(struct xhci_ctrl *ctrl)
+{
+	u32 temp;
+
+	xhci_reset(ctrl->hcor);
+
+	debug("// Disabling event ring interrupts\n");
+	temp = xhci_readl(&ctrl->hcor->or_usbsts);
+	xhci_writel(&ctrl->hcor->or_usbsts, temp & ~STS_EINT);
+	temp = xhci_readl(&ctrl->ir_set->irq_pending);
+	xhci_writel(&ctrl->ir_set->irq_pending, ER_IRQ_DISABLE(temp));
 
 	return 0;
 }
 
+#ifndef CONFIG_DM_USB
+int submit_control_msg(struct usb_device *udev, unsigned long pipe,
+		       void *buffer, int length, struct devrequest *setup)
+{
+	struct usb_device *hop = udev;
+
+	if (hop->parent)
+		while (hop->parent->parent)
+			hop = hop->parent;
+
+	return _xhci_submit_control_msg(udev, pipe, buffer, length, setup,
+					hop->portnr);
+}
+
+int submit_bulk_msg(struct usb_device *udev, unsigned long pipe, void *buffer,
+		    int length)
+{
+	return _xhci_submit_bulk_msg(udev, pipe, buffer, length);
+}
+
+int submit_int_msg(struct usb_device *udev, unsigned long pipe, void *buffer,
+		   int length, int interval)
+{
+	return _xhci_submit_int_msg(udev, pipe, buffer, length, interval);
+}
+
+/**
+ * Intialises the XHCI host controller
+ * and allocates the necessary data structures
+ *
+ * @param index	index to the host controller data structure
+ * @return pointer to the intialised controller
+ */
+int usb_lowlevel_init(int index, enum usb_init_type init, void **controller)
+{
+	struct xhci_hccr *hccr;
+	struct xhci_hcor *hcor;
+	struct xhci_ctrl *ctrl;
+	int ret;
+
+	if (xhci_hcd_init(index, &hccr, (struct xhci_hcor **)&hcor) != 0)
+		return -ENODEV;
+
+	if (xhci_reset(hcor) != 0)
+		return -ENODEV;
+
+	ctrl = &xhcic[index];
+
+	ctrl->hccr = hccr;
+	ctrl->hcor = hcor;
+
+	ret = xhci_lowlevel_init(ctrl);
+
+	*controller = &xhcic[index];
+
+	return ret;
+}
+
 /**
  * Stops the XHCI host controller
  * and cleans up all the related data structures
@@ -1012,19 +1092,143 @@ int usb_lowlevel_init(int index, enum usb_init_type init, void **controller)
 int usb_lowlevel_stop(int index)
 {
 	struct xhci_ctrl *ctrl = (xhcic + index);
-	u32 temp;
 
-	xhci_reset(ctrl->hcor);
+	xhci_lowlevel_stop(ctrl);
+	xhci_hcd_stop(index);
+	xhci_cleanup(ctrl);
 
-	debug("// Disabling event ring interrupts\n");
-	temp = xhci_readl(&ctrl->hcor->or_usbsts);
-	xhci_writel(&ctrl->hcor->or_usbsts, temp & ~STS_EINT);
-	temp = xhci_readl(&ctrl->ir_set->irq_pending);
-	xhci_writel(&ctrl->ir_set->irq_pending, ER_IRQ_DISABLE(temp));
+	return 0;
+}
+#endif /* CONFIG_DM_USB */
 
-	xhci_hcd_stop(index);
+#ifdef CONFIG_DM_USB
+/*
+static struct usb_device *get_usb_device(struct udevice *dev)
+{
+	struct usb_device *udev;
 
+	if (device_get_uclass_id(dev) == UCLASS_USB)
+		udev = dev_get_uclass_priv(dev);
+	else
+		udev = dev_get_parentdata(dev);
+
+	return udev;
+}
+*/
+static bool is_root_hub(struct udevice *dev)
+{
+	if (device_get_uclass_id(dev->parent) != UCLASS_USB_HUB)
+		return true;
+
+	return false;
+}
+
+static int xhci_submit_control_msg(struct udevice *dev, struct usb_device *udev,
+				   unsigned long pipe, void *buffer, int length,
+				   struct devrequest *setup)
+{
+	struct usb_device *uhop;
+	struct udevice *hub;
+	int root_portnr = 0;
+
+	debug("%s: dev='%s', udev=%p, udev->dev='%s', portnr=%d\n", __func__,
+	      dev->name, udev, udev->dev->name, udev->portnr);
+	hub = udev->dev;
+	if (device_get_uclass_id(hub) == UCLASS_USB_HUB) {
+		/* Figure out our port number on the root hub */
+		if (is_root_hub(hub)) {
+			root_portnr = udev->portnr;
+		} else {
+			while (!is_root_hub(hub->parent))
+				hub = hub->parent;
+			uhop = dev_get_parentdata(hub);
+			root_portnr = uhop->portnr;
+		}
+	}
+/*
+	struct usb_device *hop = udev;
+
+	if (hop->parent)
+		while (hop->parent->parent)
+			hop = hop->parent;
+*/
+	return _xhci_submit_control_msg(udev, pipe, buffer, length, setup,
+					root_portnr);
+}
+
+static int xhci_submit_bulk_msg(struct udevice *dev, struct usb_device *udev,
+				unsigned long pipe, void *buffer, int length)
+{
+	debug("%s: dev='%s', udev=%p\n", __func__, dev->name, udev);
+	return _xhci_submit_bulk_msg(udev, pipe, buffer, length);
+}
+
+static int xhci_submit_int_msg(struct udevice *dev, struct usb_device *udev,
+			       unsigned long pipe, void *buffer, int length,
+			       int interval)
+{
+	debug("%s: dev='%s', udev=%p\n", __func__, dev->name, udev);
+	return _xhci_submit_int_msg(udev, pipe, buffer, length, interval);
+}
+
+static int xhci_alloc_device(struct udevice *dev, struct usb_device *udev)
+{
+	debug("%s: dev='%s', udev=%p\n", __func__, dev->name, udev);
+	return _xhci_alloc_device(udev);
+}
+
+int xhci_register(struct udevice *dev, struct xhci_hccr *hccr,
+		  struct xhci_hcor *hcor)
+{
+	struct xhci_ctrl *ctrl = dev_get_priv(dev);
+	struct usb_bus_priv *priv = dev_get_uclass_priv(dev);
+	int ret;
+
+	debug("%s: dev='%s', ctrl=%p, hccr=%p, hcor=%p\n", __func__, dev->name,
+	      ctrl, hccr, hcor);
+
+	ctrl->dev = dev;
+
+	/*
+	 * XHCI needs to issue a Address device command to setup
+	 * proper device context structures, before it can interact
+	 * with the device. So a get_descriptor will fail before any
+	 * of that is done for XHCI unlike EHCI.
+	 */
+	priv->desc_before_addr = false;
+
+	ret = xhci_reset(hcor);
+	if (ret)
+		goto err;
+
+	ctrl->hccr = hccr;
+	ctrl->hcor = hcor;
+	ret = xhci_lowlevel_init(ctrl);
+	if (ret)
+		goto err;
+
+	return 0;
+err:
+	free(ctrl);
+	debug("%s: failed, ret=%d\n", __func__, ret);
+	return ret;
+}
+
+int xhci_deregister(struct udevice *dev)
+{
+	struct xhci_ctrl *ctrl = dev_get_priv(dev);
+
+	xhci_lowlevel_stop(ctrl);
 	xhci_cleanup(ctrl);
 
 	return 0;
 }
+
+struct dm_usb_ops xhci_usb_ops = {
+	.control = xhci_submit_control_msg,
+	.bulk = xhci_submit_bulk_msg,
+	.interrupt = xhci_submit_int_msg,
+	.alloc_device = xhci_alloc_device,
+};
+
+#endif
diff --git a/drivers/usb/host/xhci.h b/drivers/usb/host/xhci.h
index 0951e87..2afa386 100644
--- a/drivers/usb/host/xhci.h
+++ b/drivers/usb/host/xhci.h
@@ -1209,6 +1209,9 @@ void xhci_hcd_stop(int index);
 #define XHCI_STS_CNR		(1 << 11)
 
 struct xhci_ctrl {
+#ifdef CONFIG_DM_USB
+	struct udevice *dev;
+#endif
 	struct xhci_hccr *hccr;	/* R/O registers, not need for volatile */
 	struct xhci_hcor *hcor;
 	struct xhci_doorbell_array *dba;
@@ -1241,7 +1244,8 @@ void xhci_endpoint_copy(struct xhci_ctrl *ctrl,
 void xhci_slot_copy(struct xhci_ctrl *ctrl,
 		    struct xhci_container_ctx *in_ctx,
 		    struct xhci_container_ctx *out_ctx);
-void xhci_setup_addressable_virt_dev(struct usb_device *udev);
+void xhci_setup_addressable_virt_dev(struct xhci_ctrl *ctrl, int slot_id,
+				     int speed, int hop_portnr);
 void xhci_queue_command(struct xhci_ctrl *ctrl, u8 *ptr,
 			u32 slot_id, u32 ep_index, trb_type cmd);
 void xhci_acknowledge_event(struct xhci_ctrl *ctrl);
@@ -1255,8 +1259,31 @@ void xhci_flush_cache(uintptr_t addr, u32 type_len);
 void xhci_inval_cache(uintptr_t addr, u32 type_len);
 void xhci_cleanup(struct xhci_ctrl *ctrl);
 struct xhci_ring *xhci_ring_alloc(unsigned int num_segs, bool link_trbs);
-int xhci_alloc_virt_device(struct usb_device *udev);
+int xhci_alloc_virt_device(struct xhci_ctrl *ctrl, unsigned int slot_id);
 int xhci_mem_init(struct xhci_ctrl *ctrl, struct xhci_hccr *hccr,
 		  struct xhci_hcor *hcor);
 
+/**
+ * xhci_deregister() - Unregister an XHCI controller
+ *
+ * @dev:	Controller device
+ * @return 0 if registered, -ve on error
+ */
+int xhci_deregister(struct udevice *dev);
+
+/**
+ * xhci_register() - Register a new XHCI controller
+ *
+ * @dev:	Controller device
+ * @hccr:	Host controller control registers
+ * @hcor:	Not sure what this means
+ * @return 0 if registered, -ve on error
+ */
+int xhci_register(struct udevice *dev, struct xhci_hccr *hccr,
+		  struct xhci_hcor *hcor);
+
+extern struct dm_usb_ops xhci_usb_ops;
+
+struct xhci_ctrl *xhci_get_ctrl(struct usb_device *udev);
+
 #endif /* HOST_XHCI_H_ */
diff --git a/drivers/usb/musb-new/musb_uboot.c b/drivers/usb/musb-new/musb_uboot.c
index 053d945..7d90ebc 100644
--- a/drivers/usb/musb-new/musb_uboot.c
+++ b/drivers/usb/musb-new/musb_uboot.c
@@ -180,7 +180,7 @@ void *poll_int_queue(struct usb_device *dev, struct int_queue *queue)
 	return NULL; /* URB still pending */
 }
 
-void usb_reset_root_port(void)
+int usb_reset_root_port(void)
 {
 	void *mbase = host->mregs;
 	u8 power;
@@ -208,6 +208,8 @@ void usb_reset_root_port(void)
 			(musb_readb(mbase, MUSB_DEVCTL) & MUSB_DEVCTL_FSDEV) ?
 			USB_SPEED_FULL : USB_SPEED_LOW;
 	mdelay((host_speed == USB_SPEED_LOW) ? 200 : 50);
+
+	return 0;
 }
 
 int usb_lowlevel_init(int index, enum usb_init_type init, void **controller)
diff --git a/drivers/usb/musb-new/sunxi.c b/drivers/usb/musb-new/sunxi.c
index 90aaec6..c9a6a16 100644
--- a/drivers/usb/musb-new/sunxi.c
+++ b/drivers/usb/musb-new/sunxi.c
@@ -235,52 +235,19 @@ static int sunxi_musb_init(struct musb *musb)
 
 	pr_debug("%s():\n", __func__);
 
-	if (is_host_enabled(musb)) {
-		int vbus_det = sunxi_name_to_gpio(CONFIG_USB0_VBUS_DET);
-
-#ifdef AXP_VBUS_DETECT
-		if (!strcmp(CONFIG_USB0_VBUS_DET, "axp_vbus_detect")) {
-			err = axp_get_vbus();
-			if (err < 0)
-				return err;
-		} else {
-#endif
-			if (vbus_det == -1) {
-				eprintf("Error invalid Vusb-det pin\n");
-				return -EINVAL;
-			}
-
-			err = gpio_request(vbus_det, "vbus0_det");
-			if (err)
-				return err;
-
-			err = gpio_direction_input(vbus_det);
-			if (err) {
-				gpio_free(vbus_det);
-				return err;
-			}
-
-			err = gpio_get_value(vbus_det);
-			if (err < 0) {
-				gpio_free(vbus_det);
-				return -EIO;
-			}
-
-			gpio_free(vbus_det);
-#ifdef AXP_VBUS_DETECT
-		}
-#endif
+	err = sunxi_usbc_request_resources(0);
+	if (err)
+		return err;
 
+	if (is_host_enabled(musb)) {
+		err = sunxi_usbc_vbus_detect(0);
 		if (err) {
 			eprintf("Error: A charger is plugged into the OTG\n");
+			sunxi_usbc_free_resources(0);
 			return -EIO;
 		}
 	}
 
-	err = sunxi_usbc_request_resources(0);
-	if (err)
-		return err;
-
 	musb->isr = sunxi_musb_interrupt;
 	sunxi_usbc_enable(0);
 
diff --git a/drivers/usb/phy/omap_usb_phy.c b/drivers/usb/phy/omap_usb_phy.c
index 52a3664..63d9301 100644
--- a/drivers/usb/phy/omap_usb_phy.c
+++ b/drivers/usb/phy/omap_usb_phy.c
@@ -131,17 +131,6 @@ static void omap_enable_usb3_phy(struct omap_xhci *omap)
 {
 	u32	val;
 
-	/* Setting OCP2SCP1 register */
-	setbits_le32((*prcm)->cm_l3init_ocp2scp1_clkctrl,
-		     OCP2SCP1_CLKCTRL_MODULEMODE_HW);
-
-	/* Turn on 32K AON clk */
-	setbits_le32((*prcm)->cm_coreaon_usb_phy_core_clkctrl,
-		     USBPHY_CORE_CLKCTRL_OPTFCLKEN_CLK32K);
-
-	/* Setting CM_L3INIT_CLKSTCTRL to 0x0 i.e NO sleep */
-	writel(0x0, (*prcm)->cm_l3init_clkstctrl);
-
 	val = (USBOTGSS_DMADISABLE |
 			USBOTGSS_STANDBYMODE_SMRT_WKUP |
 			USBOTGSS_IDLEMODE_NOIDLE);
@@ -169,11 +158,6 @@ static void omap_enable_usb3_phy(struct omap_xhci *omap)
 	writel(val, &omap->otg_wrapper->irqstatus_1);
 	val = readl(&omap->otg_wrapper->irqstatus_0);
 	writel(val, &omap->otg_wrapper->irqstatus_0);
-
-	/* Enable the USB OTG Super speed clocks */
-	val = (OPTFCLKEN_REFCLK960M | OTG_SS_CLKCTRL_MODULEMODE_HW);
-	setbits_le32((*prcm)->cm_l3init_usb_otg_ss_clkctrl, val);
-
 };
 #endif /* CONFIG_OMAP_USB3PHY1_HOST */
 
diff --git a/drivers/video/Makefile b/drivers/video/Makefile
index 22a316b..f64918e 100644
--- a/drivers/video/Makefile
+++ b/drivers/video/Makefile
@@ -45,5 +45,6 @@ obj-$(CONFIG_VIDEO_TEGRA) += tegra.o
 obj-$(CONFIG_VIDEO_VCXK) += bus_vcxk.o
 obj-$(CONFIG_VIDEO_VESA) += vesa_fb.o
 obj-$(CONFIG_FORMIKE) += formike.o
+obj-$(CONFIG_LG4573) += lg4573.o
 obj-$(CONFIG_AM335X_LCD) += am335x-fb.o
 obj-$(CONFIG_VIDEO_PARADE) += parade.o
diff --git a/drivers/video/cfb_console.c b/drivers/video/cfb_console.c
index a81affa..f4231b8 100644
--- a/drivers/video/cfb_console.c
+++ b/drivers/video/cfb_console.c
@@ -87,6 +87,7 @@
  */
 
 #include <common.h>
+#include <fdtdec.h>
 #include <version.h>
 #include <malloc.h>
 #include <linux/compiler.h>
@@ -2251,6 +2252,7 @@ int drv_video_init(void)
 {
 	int skip_dev_init;
 	struct stdio_dev console_dev;
+	bool have_keyboard;
 
 	/* Check if video initialization should be skipped */
 	if (board_video_skip())
@@ -2262,11 +2264,20 @@ int drv_video_init(void)
 	if (board_cfb_skip())
 		return 0;
 
+#if defined(CONFIG_VGA_AS_SINGLE_DEVICE)
+	have_keyboard = false;
+#elif defined(CONFIG_OF_CONTROL)
+	have_keyboard = !fdtdec_get_config_bool(gd->fdt_blob,
+						"u-boot,no-keyboard");
+#else
+	have_keyboard = true;
+#endif
+	if (have_keyboard) {
+		debug("KBD: Keyboard init ...\n");
 #if !defined(CONFIG_VGA_AS_SINGLE_DEVICE)
-	debug("KBD: Keyboard init ...\n");
-	skip_dev_init |= (VIDEO_KBD_INIT_FCT == -1);
+		skip_dev_init |= (VIDEO_KBD_INIT_FCT == -1);
 #endif
-
+	}
 	if (skip_dev_init)
 		return 0;
 
@@ -2279,11 +2290,13 @@ int drv_video_init(void)
 	console_dev.puts = video_puts;	/* 'puts' function */
 
 #if !defined(CONFIG_VGA_AS_SINGLE_DEVICE)
-	/* Also init console device */
-	console_dev.flags |= DEV_FLAGS_INPUT;
-	console_dev.tstc = VIDEO_TSTC_FCT;	/* 'tstc' function */
-	console_dev.getc = VIDEO_GETC_FCT;	/* 'getc' function */
-#endif /* CONFIG_VGA_AS_SINGLE_DEVICE */
+	if (have_keyboard) {
+		/* Also init console device */
+		console_dev.flags |= DEV_FLAGS_INPUT;
+		console_dev.tstc = VIDEO_TSTC_FCT;	/* 'tstc' function */
+		console_dev.getc = VIDEO_GETC_FCT;	/* 'getc' function */
+	}
+#endif
 
 	if (stdio_register(&console_dev) != 0)
 		return 0;
diff --git a/drivers/video/ipu.h b/drivers/video/ipu.h
index 091b58f..348be58 100644
--- a/drivers/video/ipu.h
+++ b/drivers/video/ipu.h
@@ -265,5 +265,4 @@ int ipu_dp_init(ipu_channel_t channel, uint32_t in_pixel_fmt,
 void ipu_dp_uninit(ipu_channel_t channel);
 void ipu_dp_dc_disable(ipu_channel_t channel, unsigned char swap);
 ipu_color_space_t format_to_colorspace(uint32_t fmt);
-
 #endif
diff --git a/drivers/video/ipu_common.c b/drivers/video/ipu_common.c
index 5873531..9f85102 100644
--- a/drivers/video/ipu_common.c
+++ b/drivers/video/ipu_common.c
@@ -210,9 +210,13 @@ static struct clk ipu_clk = {
 	.usecount = 0,
 };
 
+#if !defined CONFIG_SYS_LDB_CLOCK
+#define CONFIG_SYS_LDB_CLOCK 65000000
+#endif
+
 static struct clk ldb_clk = {
 	.name = "ldb_clk",
-	.rate = 65000000,
+	.rate = CONFIG_SYS_LDB_CLOCK,
 	.usecount = 0,
 };
 
@@ -1194,3 +1198,11 @@ ipu_color_space_t format_to_colorspace(uint32_t fmt)
 	}
 	return RGB;
 }
+
+/* should be removed when clk framework is availiable */
+int ipu_set_ldb_clock(int rate)
+{
+	ldb_clk.rate = rate;
+
+	return 0;
+}
diff --git a/drivers/video/lg4573.c b/drivers/video/lg4573.c
new file mode 100644
index 0000000..43670fc
--- /dev/null
+++ b/drivers/video/lg4573.c
@@ -0,0 +1,231 @@
+/*
+ * LCD: LG4573, TFT 4.3", 480x800, RGB24
+ * LCD initialization via SPI
+ *
+ * SPDX-License-Identifier:	GPL-2.0
+ *
+ */
+#include <common.h>
+#include <errno.h>
+#include <spi.h>
+
+#define PWR_ON_DELAY_MSECS  120
+
+static int lb043wv_spi_write_u16(struct spi_slave *spi, u16 val)
+{
+	unsigned long flags = SPI_XFER_BEGIN;
+	unsigned short buf16 = htons(val);
+	int ret = 0;
+
+	flags |= SPI_XFER_END;
+
+	ret = spi_xfer(spi, 16, &buf16, NULL, flags);
+	if (ret)
+		debug("%s: Failed to send: %d\n", __func__, ret);
+
+	return ret;
+}
+
+static void lb043wv_spi_write_u16_array(struct spi_slave *spi, u16 *buff,
+					int size)
+{
+	int i;
+
+	for (i = 0; i < size; i++)
+		lb043wv_spi_write_u16(spi, buff[i]);
+}
+
+static void lb043wv_display_mode_settings(struct spi_slave *spi)
+{
+	static u16 display_mode_settings[] = {
+	  0x703A,
+	  0x7270,
+	  0x70B1,
+	  0x7208,
+	  0x723B,
+	  0x720F,
+	  0x70B2,
+	  0x7200,
+	  0x72C8,
+	  0x70B3,
+	  0x7200,
+	  0x70B4,
+	  0x7200,
+	  0x70B5,
+	  0x7242,
+	  0x7210,
+	  0x7210,
+	  0x7200,
+	  0x7220,
+	  0x70B6,
+	  0x720B,
+	  0x720F,
+	  0x723C,
+	  0x7213,
+	  0x7213,
+	  0x72E8,
+	  0x70B7,
+	  0x7246,
+	  0x7206,
+	  0x720C,
+	  0x7200,
+	  0x7200,
+	};
+
+	debug("transfer display mode settings\n");
+	lb043wv_spi_write_u16_array(spi, display_mode_settings,
+				    ARRAY_SIZE(display_mode_settings));
+}
+
+static void lb043wv_power_settings(struct spi_slave *spi)
+{
+	static u16 power_settings[] = {
+	  0x70C0,
+	  0x7201,
+	  0x7211,
+	  0x70C3,
+	  0x7207,
+	  0x7203,
+	  0x7204,
+	  0x7204,
+	  0x7204,
+	  0x70C4,
+	  0x7212,
+	  0x7224,
+	  0x7218,
+	  0x7218,
+	  0x7202,
+	  0x7249,
+	  0x70C5,
+	  0x726F,
+	  0x70C6,
+	  0x7241,
+	  0x7263,
+	};
+
+	debug("transfer power settings\n");
+	lb043wv_spi_write_u16_array(spi, power_settings,
+				    ARRAY_SIZE(power_settings));
+}
+
+static void lb043wv_gamma_settings(struct spi_slave *spi)
+{
+	static u16 gamma_settings[] = {
+	  0x70D0,
+	  0x7203,
+	  0x7207,
+	  0x7273,
+	  0x7235,
+	  0x7200,
+	  0x7201,
+	  0x7220,
+	  0x7200,
+	  0x7203,
+	  0x70D1,
+	  0x7203,
+	  0x7207,
+	  0x7273,
+	  0x7235,
+	  0x7200,
+	  0x7201,
+	  0x7220,
+	  0x7200,
+	  0x7203,
+	  0x70D2,
+	  0x7203,
+	  0x7207,
+	  0x7273,
+	  0x7235,
+	  0x7200,
+	  0x7201,
+	  0x7220,
+	  0x7200,
+	  0x7203,
+	  0x70D3,
+	  0x7203,
+	  0x7207,
+	  0x7273,
+	  0x7235,
+	  0x7200,
+	  0x7201,
+	  0x7220,
+	  0x7200,
+	  0x7203,
+	  0x70D4,
+	  0x7203,
+	  0x7207,
+	  0x7273,
+	  0x7235,
+	  0x7200,
+	  0x7201,
+	  0x7220,
+	  0x7200,
+	  0x7203,
+	  0x70D5,
+	  0x7203,
+	  0x7207,
+	  0x7273,
+	  0x7235,
+	  0x7200,
+	  0x7201,
+	  0x7220,
+	  0x7200,
+	  0x7203,
+	};
+
+	debug("transfer gamma settings\n");
+	lb043wv_spi_write_u16_array(spi, gamma_settings,
+				    ARRAY_SIZE(gamma_settings));
+}
+
+static void lb043wv_display_on(struct spi_slave *spi)
+{
+	static u16 sleep_out = 0x7011;
+	static u16 display_on = 0x7029;
+
+	lb043wv_spi_write_u16(spi, sleep_out);
+	mdelay(PWR_ON_DELAY_MSECS);
+	lb043wv_spi_write_u16(spi, display_on);
+}
+
+int lg4573_spi_startup(unsigned int bus, unsigned int cs,
+	unsigned int max_hz, unsigned int spi_mode)
+{
+	struct spi_slave *spi;
+	int ret;
+
+	spi = spi_setup_slave(bus, cs, max_hz, spi_mode);
+	if (!spi) {
+		debug("%s: Failed to set up slave\n", __func__);
+		return -1;
+	}
+
+	ret = spi_claim_bus(spi);
+	if (ret) {
+		debug("%s: Failed to claim SPI bus: %d\n", __func__, ret);
+		goto err_claim_bus;
+	}
+
+	lb043wv_display_mode_settings(spi);
+	lb043wv_power_settings(spi);
+	lb043wv_gamma_settings(spi);
+
+	lb043wv_display_on(spi);
+	return 0;
+err_claim_bus:
+	spi_free_slave(spi);
+	return -1;
+}
+
+static int do_lgset(cmd_tbl_t *cmdtp, int flag, int argc,
+		       char * const argv[])
+{
+	lg4573_spi_startup(0, 0, 10000000, SPI_MODE_0);
+	return 0;
+}
+
+U_BOOT_CMD(
+	lgset,	2,	1,	do_lgset,
+	"set lgdisplay",
+	""
+);
diff --git a/drivers/video/sunxi_display.c b/drivers/video/sunxi_display.c
index 4e12150..d2341b0 100644
--- a/drivers/video/sunxi_display.c
+++ b/drivers/video/sunxi_display.c
@@ -665,10 +665,10 @@ static void sunxi_lcdc_tcon0_mode_set(const struct ctfb_res_modes *mode,
 
 	for (pin = SUNXI_GPD(0); pin <= SUNXI_GPD(27); pin++)
 #ifdef CONFIG_VIDEO_LCD_IF_PARALLEL
-		sunxi_gpio_set_cfgpin(pin, SUNXI_GPD0_LCD0);
+		sunxi_gpio_set_cfgpin(pin, SUNXI_GPD_LCD0);
 #endif
 #ifdef CONFIG_VIDEO_LCD_IF_LVDS
-		sunxi_gpio_set_cfgpin(pin, SUNXI_GPD0_LVDS0);
+		sunxi_gpio_set_cfgpin(pin, SUNXI_GPD_LVDS0);
 #endif
 
 	sunxi_lcdc_pll_set(0, mode->pixclock_khz, &clk_div, &clk_double);
@@ -779,8 +779,8 @@ static void sunxi_lcdc_tcon1_mode_set(const struct ctfb_res_modes *mode,
 	       &lcdc->tcon1_timing_sync);
 
 	if (use_portd_hvsync) {
-		sunxi_gpio_set_cfgpin(SUNXI_GPD(26), SUNXI_GPD0_LCD0);
-		sunxi_gpio_set_cfgpin(SUNXI_GPD(27), SUNXI_GPD0_LCD0);
+		sunxi_gpio_set_cfgpin(SUNXI_GPD(26), SUNXI_GPD_LCD0);
+		sunxi_gpio_set_cfgpin(SUNXI_GPD(27), SUNXI_GPD_LCD0);
 
 		val = 0;
 		if (mode->sync & FB_SYNC_HOR_HIGH_ACT)