Merge branch 'master' of git://git.denx.de/u-boot

Signed-off-by: Stefano Babic <sbabic@denx.de>

169 files changed, 8372 insertions, 4691 deletions

diff --git a/drivers/Kconfig b/drivers/Kconfig
index 118b66e..f2a137a 100644
--- a/drivers/Kconfig
+++ b/drivers/Kconfig
@@ -30,6 +30,8 @@ source "drivers/input/Kconfig"
 
 source "drivers/led/Kconfig"
 
+source "drivers/mailbox/Kconfig"
+
 source "drivers/memory/Kconfig"
 
 source "drivers/misc/Kconfig"
diff --git a/drivers/Makefile b/drivers/Makefile
index 99dd07f..f6295d2 100644
--- a/drivers/Makefile
+++ b/drivers/Makefile
@@ -64,6 +64,7 @@ obj-y += video/
 obj-y += watchdog/
 obj-$(CONFIG_QE) += qe/
 obj-$(CONFIG_U_QE) += qe/
+obj-y += mailbox/
 obj-y += memory/
 obj-y += pwm/
 obj-y += input/
diff --git a/drivers/clk/Kconfig b/drivers/clk/Kconfig
index a98b74b..6eee8eb 100644
--- a/drivers/clk/Kconfig
+++ b/drivers/clk/Kconfig
@@ -21,5 +21,6 @@ config SPL_CLK
 	  used as U-Boot proper.
 
 source "drivers/clk/uniphier/Kconfig"
+source "drivers/clk/exynos/Kconfig"
 
 endmenu
diff --git a/drivers/clk/Makefile b/drivers/clk/Makefile
index c51db15..81fe600 100644
--- a/drivers/clk/Makefile
+++ b/drivers/clk/Makefile
@@ -11,3 +11,4 @@ obj-$(CONFIG_ROCKCHIP_RK3288) += clk_rk3288.o
 obj-$(CONFIG_SANDBOX) += clk_sandbox.o
 obj-$(CONFIG_MACH_PIC32) += clk_pic32.o
 obj-$(CONFIG_CLK_UNIPHIER) += uniphier/
+obj-$(CONFIG_CLK_EXYNOS) += exynos/
diff --git a/drivers/clk/clk_rk3036.c b/drivers/clk/clk_rk3036.c
index bd5f22a..7ec65bd 100644
--- a/drivers/clk/clk_rk3036.c
+++ b/drivers/clk/clk_rk3036.c
@@ -407,7 +407,7 @@ static int rk3036_clk_bind(struct udevice *dev)
 	}
 
 	/* The reset driver does not have a device node, so bind it here */
-	ret = device_bind_driver(gd->dm_root, "rk3036_reset", "reset", &dev);
+	ret = device_bind_driver(gd->dm_root, "rk3036_sysreset", "reset", &dev);
 	if (ret)
 		debug("Warning: No RK3036 reset driver: ret=%d\n", ret);
 
diff --git a/drivers/clk/clk_rk3288.c b/drivers/clk/clk_rk3288.c
index 2a85e93..d88893c 100644
--- a/drivers/clk/clk_rk3288.c
+++ b/drivers/clk/clk_rk3288.c
@@ -326,6 +326,17 @@ static int pll_para_config(ulong freq_hz, struct pll_div *div, uint *ext_div)
 	return 0;
 }
 
+static int rockchip_mac_set_clk(struct rk3288_cru *cru,
+				  int periph, uint freq)
+{
+	/* Assuming mac_clk is fed by an external clock */
+	rk_clrsetreg(&cru->cru_clksel_con[21],
+		     RMII_EXTCLK_MASK << RMII_EXTCLK_SHIFT,
+		     RMII_EXTCLK_SELECT_EXT_CLK << RMII_EXTCLK_SHIFT);
+
+	 return 0;
+}
+
 static int rockchip_vop_set_clk(struct rk3288_cru *cru, struct rk3288_grf *grf,
 				int periph, unsigned int rate_hz)
 {
@@ -759,6 +770,9 @@ static ulong rk3288_set_periph_rate(struct udevice *dev, int periph, ulong rate)
 		new_rate = rockchip_spi_set_clk(cru, gclk_rate, periph, rate);
 		break;
 #ifndef CONFIG_SPL_BUILD
+	case SCLK_MAC:
+		new_rate = rockchip_mac_set_clk(priv->cru, periph, rate);
+		break;
 	case DCLK_VOP0:
 	case DCLK_VOP1:
 		new_rate = rockchip_vop_set_clk(cru, priv->grf, periph, rate);
@@ -877,7 +891,7 @@ static int rk3288_clk_bind(struct udevice *dev)
 	}
 
 	/* The reset driver does not have a device node, so bind it here */
-	ret = device_bind_driver(gd->dm_root, "rk3288_reset", "reset", &dev);
+	ret = device_bind_driver(gd->dm_root, "rk3288_sysreset", "reset", &dev);
 	if (ret)
 		debug("Warning: No RK3288 reset driver: ret=%d\n", ret);
 
diff --git a/drivers/clk/exynos/Kconfig b/drivers/clk/exynos/Kconfig
new file mode 100644
index 0000000..eb0efa9
--- /dev/null
+++ b/drivers/clk/exynos/Kconfig
@@ -0,0 +1,18 @@
+config CLK_EXYNOS
+	bool
+	select CLK
+	help
+	  This enables support for common clock driver API on Samsung
+	  Exynos SoCs.
+
+menu "Clock drivers for Exynos SoCs"
+	depends on CLK_EXYNOS
+
+config CLK_EXYNOS7420
+	bool "Clock driver for Samsung's Exynos7420 SoC"
+	default y
+	help
+	  This enables common clock driver support for platforms based
+	  on Samsung Exynos7420 SoC.
+
+endmenu
diff --git a/drivers/clk/exynos/Makefile b/drivers/clk/exynos/Makefile
new file mode 100644
index 0000000..1df10fe
--- /dev/null
+++ b/drivers/clk/exynos/Makefile
@@ -0,0 +1,9 @@
+#
+# Copyright (C) 2016 Samsung Electronics
+# Thomas Abraham <thomas.ab@samsung.com>
+#
+# SPDX-License-Identifier:	GPL-2.0+
+#
+
+obj-y				+= clk-pll.o
+obj-$(CONFIG_CLK_EXYNOS7420)	+= clk-exynos7420.o
diff --git a/drivers/clk/exynos/clk-exynos7420.c b/drivers/clk/exynos/clk-exynos7420.c
new file mode 100644
index 0000000..bf5d0e6
--- /dev/null
+++ b/drivers/clk/exynos/clk-exynos7420.c
@@ -0,0 +1,236 @@
+/*
+ * Samsung Exynos7420 clock driver.
+ * Copyright (C) 2016 Samsung Electronics
+ * Thomas Abraham <thomas.ab@samsung.com>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <dm.h>
+#include <errno.h>
+#include <clk.h>
+#include <asm/io.h>
+#include <dt-bindings/clock/exynos7420-clk.h>
+#include "clk-pll.h"
+
+DECLARE_GLOBAL_DATA_PTR;
+
+#define DIVIDER(reg, shift, mask)	\
+	(((readl(reg) >> shift) & mask) + 1)
+
+/* CMU TOPC block device structure */
+struct exynos7420_clk_cmu_topc {
+	unsigned int	rsvd1[68];
+	unsigned int	bus0_pll_con[2];
+	unsigned int	rsvd2[2];
+	unsigned int	bus1_pll_con[2];
+	unsigned int	rsvd3[54];
+	unsigned int	mux_sel[6];
+	unsigned int	rsvd4[250];
+	unsigned int	div[4];
+};
+
+/* CMU TOP0 block device structure */
+struct exynos7420_clk_cmu_top0 {
+	unsigned int	rsvd0[128];
+	unsigned int	mux_sel[7];
+	unsigned int	rsvd1[261];
+	unsigned int	div_peric[5];
+};
+
+/**
+ * struct exynos7420_clk_topc_priv - private data for CMU topc clock driver.
+ *
+ * @topc: base address of the memory mapped CMU TOPC controller.
+ * @fin_freq: frequency of the Oscillator clock.
+ * @sclk_bus0_pll_a: frequency of sclk_bus0_pll_a clock.
+ * @sclk_bus1_pll_a: frequency of sclk_bus1_pll_a clock.
+ */
+struct exynos7420_clk_topc_priv {
+	struct exynos7420_clk_cmu_topc *topc;
+	unsigned long fin_freq;
+	unsigned long sclk_bus0_pll_a;
+	unsigned long sclk_bus1_pll_a;
+};
+
+/**
+ * struct exynos7420_clk_top0_priv - private data for CMU top0 clock driver.
+ *
+ * @top0: base address of the memory mapped CMU TOP0 controller.
+ * @mout_top0_bus0_pll_half: frequency of mout_top0_bus0_pll_half clock
+ * @sclk_uart2: frequency of sclk_uart2 clock.
+ */
+struct exynos7420_clk_top0_priv {
+	struct exynos7420_clk_cmu_top0 *top0;
+	unsigned long mout_top0_bus0_pll_half;
+	unsigned long sclk_uart2;
+};
+
+static ulong exynos7420_topc_get_periph_rate(struct udevice *dev, int periph)
+{
+	struct exynos7420_clk_topc_priv *priv = dev_get_priv(dev);
+
+	switch (periph) {
+	case DOUT_SCLK_BUS0_PLL:
+	case SCLK_BUS0_PLL_A:
+	case SCLK_BUS0_PLL_B:
+		return priv->sclk_bus0_pll_a;
+	case DOUT_SCLK_BUS1_PLL:
+	case SCLK_BUS1_PLL_A:
+	case SCLK_BUS1_PLL_B:
+		return priv->sclk_bus1_pll_a;
+	default:
+		return 0;
+	}
+}
+
+static struct clk_ops exynos7420_clk_topc_ops = {
+	.get_periph_rate	= exynos7420_topc_get_periph_rate,
+};
+
+static int exynos7420_clk_topc_probe(struct udevice *dev)
+{
+	struct exynos7420_clk_topc_priv *priv = dev_get_priv(dev);
+	struct exynos7420_clk_cmu_topc *topc;
+	struct udevice *clk_dev;
+	unsigned long rate;
+	fdt_addr_t base;
+	int ret;
+
+	base = dev_get_addr(dev);
+	if (base == FDT_ADDR_T_NONE)
+		return -EINVAL;
+
+	topc = (struct exynos7420_clk_cmu_topc *)base;
+	priv->topc = topc;
+
+	ret = clk_get_by_index(dev, 0, &clk_dev);
+	if (ret >= 0)
+		priv->fin_freq = clk_get_rate(clk_dev);
+
+	rate = pll145x_get_rate(&topc->bus0_pll_con[0], priv->fin_freq);
+	if (readl(&topc->mux_sel[1]) & (1 << 16))
+		rate >>= 1;
+	rate /= DIVIDER(&topc->div[3], 0, 0xf);
+	priv->sclk_bus0_pll_a = rate;
+
+	rate = pll145x_get_rate(&topc->bus1_pll_con[0], priv->fin_freq) /
+			DIVIDER(&topc->div[3], 8, 0xf);
+	priv->sclk_bus1_pll_a = rate;
+
+	return 0;
+}
+
+static ulong exynos7420_top0_get_periph_rate(struct udevice *dev, int periph)
+{
+	struct exynos7420_clk_top0_priv *priv = dev_get_priv(dev);
+	struct exynos7420_clk_cmu_top0 *top0 = priv->top0;
+
+	switch (periph) {
+	case CLK_SCLK_UART2:
+		return priv->mout_top0_bus0_pll_half /
+			DIVIDER(&top0->div_peric[3], 8, 0xf);
+	default:
+		return 0;
+	}
+}
+
+static struct clk_ops exynos7420_clk_top0_ops = {
+	.get_periph_rate	= exynos7420_top0_get_periph_rate,
+};
+
+static int exynos7420_clk_top0_probe(struct udevice *dev)
+{
+	struct exynos7420_clk_top0_priv *priv;
+	struct exynos7420_clk_cmu_top0 *top0;
+	struct udevice *clk_dev;
+	fdt_addr_t base;
+	int ret;
+
+	priv = dev_get_priv(dev);
+	if (!priv)
+		return -EINVAL;
+
+	base = dev_get_addr(dev);
+	if (base == FDT_ADDR_T_NONE)
+		return -EINVAL;
+
+	top0 = (struct exynos7420_clk_cmu_top0 *)base;
+	priv->top0 = top0;
+
+	ret = clk_get_by_index(dev, 1, &clk_dev);
+	if (ret >= 0) {
+		priv->mout_top0_bus0_pll_half =
+			clk_get_periph_rate(clk_dev, ret);
+		if (readl(&top0->mux_sel[1]) & (1 << 16))
+			priv->mout_top0_bus0_pll_half >>= 1;
+	}
+
+	return 0;
+}
+
+static ulong exynos7420_peric1_get_periph_rate(struct udevice *dev, int periph)
+{
+	struct udevice *clk_dev;
+	unsigned int ret;
+	unsigned long freq = 0;
+
+	switch (periph) {
+	case SCLK_UART2:
+		ret = clk_get_by_index(dev, 3, &clk_dev);
+		if (ret < 0)
+			return ret;
+		freq = clk_get_periph_rate(clk_dev, ret);
+		break;
+	}
+
+	return freq;
+}
+
+static struct clk_ops exynos7420_clk_peric1_ops = {
+	.get_periph_rate	= exynos7420_peric1_get_periph_rate,
+};
+
+static const struct udevice_id exynos7420_clk_topc_compat[] = {
+	{ .compatible = "samsung,exynos7-clock-topc" },
+	{ }
+};
+
+U_BOOT_DRIVER(exynos7420_clk_topc) = {
+	.name = "exynos7420-clock-topc",
+	.id = UCLASS_CLK,
+	.of_match = exynos7420_clk_topc_compat,
+	.probe = exynos7420_clk_topc_probe,
+	.priv_auto_alloc_size = sizeof(struct exynos7420_clk_topc_priv),
+	.ops = &exynos7420_clk_topc_ops,
+	.flags = DM_FLAG_PRE_RELOC,
+};
+
+static const struct udevice_id exynos7420_clk_top0_compat[] = {
+	{ .compatible = "samsung,exynos7-clock-top0" },
+	{ }
+};
+
+U_BOOT_DRIVER(exynos7420_clk_top0) = {
+	.name = "exynos7420-clock-top0",
+	.id = UCLASS_CLK,
+	.of_match = exynos7420_clk_top0_compat,
+	.probe = exynos7420_clk_top0_probe,
+	.priv_auto_alloc_size = sizeof(struct exynos7420_clk_top0_priv),
+	.ops = &exynos7420_clk_top0_ops,
+	.flags = DM_FLAG_PRE_RELOC,
+};
+
+static const struct udevice_id exynos7420_clk_peric1_compat[] = {
+	{ .compatible = "samsung,exynos7-clock-peric1" },
+	{ }
+};
+
+U_BOOT_DRIVER(exynos7420_clk_peric1) = {
+	.name = "exynos7420-clock-peric1",
+	.id = UCLASS_CLK,
+	.of_match = exynos7420_clk_peric1_compat,
+	.ops = &exynos7420_clk_peric1_ops,
+	.flags = DM_FLAG_PRE_RELOC,
+};
diff --git a/drivers/clk/exynos/clk-pll.c b/drivers/clk/exynos/clk-pll.c
new file mode 100644
index 0000000..27220c5
--- /dev/null
+++ b/drivers/clk/exynos/clk-pll.c
@@ -0,0 +1,33 @@
+/*
+ * Exynos PLL helper functions for clock drivers.
+ * Copyright (C) 2016 Samsung Electronics
+ * Thomas Abraham <thomas.ab@samsung.com>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <asm/io.h>
+#include <div64.h>
+
+#define PLL145X_MDIV_SHIFT	16
+#define PLL145X_MDIV_MASK	0x3ff
+#define PLL145X_PDIV_SHIFT	8
+#define PLL145X_PDIV_MASK	0x3f
+#define PLL145X_SDIV_SHIFT	0
+#define PLL145X_SDIV_MASK	0x7
+
+unsigned long pll145x_get_rate(unsigned int *con1, unsigned long fin_freq)
+{
+	unsigned long pll_con1 = readl(con1);
+	unsigned long mdiv, sdiv, pdiv;
+	uint64_t fvco = fin_freq;
+
+	mdiv = (pll_con1 >> PLL145X_MDIV_SHIFT) & PLL145X_MDIV_MASK;
+	pdiv = (pll_con1 >> PLL145X_PDIV_SHIFT) & PLL145X_PDIV_MASK;
+	sdiv = (pll_con1 >> PLL145X_SDIV_SHIFT) & PLL145X_SDIV_MASK;
+
+	fvco *= mdiv;
+	do_div(fvco, (pdiv << sdiv));
+	return (unsigned long)fvco;
+}
diff --git a/drivers/clk/exynos/clk-pll.h b/drivers/clk/exynos/clk-pll.h
new file mode 100644
index 0000000..631d035
--- /dev/null
+++ b/drivers/clk/exynos/clk-pll.h
@@ -0,0 +1,9 @@
+/*
+ * Exynos PLL helper functions for clock drivers.
+ * Copyright (C) 2016 Samsung Electronics
+ * Thomas Abraham <thomas.ab@samsung.com>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+unsigned long pll145x_get_rate(unsigned int *con1, unsigned long fin_freq);
diff --git a/drivers/clk/uniphier/clk-uniphier-mio.c b/drivers/clk/uniphier/clk-uniphier-mio.c
index a2b0e80..c6ecd11 100644
--- a/drivers/clk/uniphier/clk-uniphier-mio.c
+++ b/drivers/clk/uniphier/clk-uniphier-mio.c
@@ -165,6 +165,10 @@ static const struct udevice_id uniphier_mio_clk_match[] = {
 		.data = (ulong)&uniphier_mio_clk_data,
 	},
 	{
+		.compatible = "socionext,ph1-ld11-mioctrl",
+		.data = (ulong)&uniphier_mio_clk_data,
+	},
+	{
 		.compatible = "socionext,ph1-ld20-mioctrl",
 		.data = (ulong)&uniphier_mio_clk_data,
 	},
diff --git a/drivers/core/Kconfig b/drivers/core/Kconfig
index c5c9d2a..8749561 100644
--- a/drivers/core/Kconfig
+++ b/drivers/core/Kconfig
@@ -178,4 +178,27 @@ config SPL_OF_TRANSLATE
 	  used for the address translation. This function is faster and
 	  smaller in size than fdt_translate_address().
 
+config OF_ISA_BUS
+	bool
+	depends on OF_TRANSLATE
+	help
+	  Is this option is enabled then support for the ISA bus will
+	  be included for addresses read from DT. This is something that
+	  should be known to be required or not based upon the board
+	  being targetted, and whether or not it makes use of an ISA bus.
+
+	  The bus is matched based upon its node name equalling "isa". The
+	  busses #address-cells should equal 2, with the first cell being
+	  used to hold flags & flag 0x1 indicating that the address range
+	  should be accessed using I/O port in/out accessors. The second
+	  cell holds the offset into ISA bus address space. The #size-cells
+	  property should equal 1, and of course holds the size of the
+	  address range used by a device.
+
+	  If this option is not enabled then support for the ISA bus is
+	  not included and any such busses used in DT will be treated as
+	  typical simple-bus compatible busses. This will lead to
+	  mistranslation of device addresses, so ensure that this is
+	  enabled if your board does include an ISA bus.
+
 endmenu
diff --git a/drivers/core/device.c b/drivers/core/device.c
index 5c2dc70..eb75b17 100644
--- a/drivers/core/device.c
+++ b/drivers/core/device.c
@@ -26,9 +26,10 @@
 
 DECLARE_GLOBAL_DATA_PTR;
 
-int device_bind(struct udevice *parent, const struct driver *drv,
-		const char *name, void *platdata, int of_offset,
-		struct udevice **devp)
+static int device_bind_common(struct udevice *parent, const struct driver *drv,
+			      const char *name, void *platdata,
+			      ulong driver_data, int of_offset,
+			      struct udevice **devp)
 {
 	struct udevice *dev;
 	struct uclass *uc;
@@ -56,6 +57,7 @@ int device_bind(struct udevice *parent, const struct driver *drv,
 	INIT_LIST_HEAD(&dev->devres_head);
 #endif
 	dev->platdata = platdata;
+	dev->driver_data = driver_data;
 	dev->name = name;
 	dev->of_offset = of_offset;
 	dev->parent = parent;
@@ -193,6 +195,23 @@ fail_alloc1:
 	return ret;
 }
 
+int device_bind_with_driver_data(struct udevice *parent,
+				 const struct driver *drv, const char *name,
+				 ulong driver_data, int of_offset,
+				 struct udevice **devp)
+{
+	return device_bind_common(parent, drv, name, NULL, driver_data,
+				  of_offset, devp);
+}
+
+int device_bind(struct udevice *parent, const struct driver *drv,
+		const char *name, void *platdata, int of_offset,
+		struct udevice **devp)
+{
+	return device_bind_common(parent, drv, name, platdata, 0, of_offset,
+				  devp);
+}
+
 int device_bind_by_name(struct udevice *parent, bool pre_reloc_only,
 			const struct driver_info *info, struct udevice **devp)
 {
@@ -721,3 +740,17 @@ int device_set_name(struct udevice *dev, const char *name)
 
 	return 0;
 }
+
+bool of_device_is_compatible(struct udevice *dev, const char *compat)
+{
+	const void *fdt = gd->fdt_blob;
+
+	return !fdt_node_check_compatible(fdt, dev->of_offset, compat);
+}
+
+bool of_machine_is_compatible(const char *compat)
+{
+	const void *fdt = gd->fdt_blob;
+
+	return !fdt_node_check_compatible(fdt, 0, compat);
+}
diff --git a/drivers/core/lists.c b/drivers/core/lists.c
index a72db13..0c27717 100644
--- a/drivers/core/lists.c
+++ b/drivers/core/lists.c
@@ -170,7 +170,8 @@ int lists_bind_fdt(struct udevice *parent, const void *blob, int offset,
 		}
 
 		dm_dbg("   - found match at '%s'\n", entry->name);
-		ret = device_bind(parent, entry, name, NULL, offset, &dev);
+		ret = device_bind_with_driver_data(parent, entry, name,
+						   id->data, offset, &dev);
 		if (ret == -ENODEV) {
 			dm_dbg("Driver '%s' refuses to bind\n", entry->name);
 			continue;
@@ -180,7 +181,6 @@ int lists_bind_fdt(struct udevice *parent, const void *blob, int offset,
 				ret);
 			return ret;
 		} else {
-			dev->driver_data = id->data;
 			found = true;
 			if (devp)
 				*devp = dev;
diff --git a/drivers/crypto/fsl/jr.c b/drivers/crypto/fsl/jr.c
index 8bc517d..510fa4e 100644
--- a/drivers/crypto/fsl/jr.c
+++ b/drivers/crypto/fsl/jr.c
@@ -19,11 +19,26 @@
 #define CIRC_CNT(head, tail, size)	(((head) - (tail)) & (size - 1))
 #define CIRC_SPACE(head, tail, size)	CIRC_CNT((tail), (head) + 1, (size))
 
-struct jobring jr;
+uint32_t sec_offset[CONFIG_SYS_FSL_MAX_NUM_OF_SEC] = {
+	0,
+#if defined(CONFIG_PPC_C29X)
+	CONFIG_SYS_FSL_SEC_IDX_OFFSET,
+	2 * CONFIG_SYS_FSL_SEC_IDX_OFFSET
+#endif
+};
+
+#define SEC_ADDR(idx)	\
+	((CONFIG_SYS_FSL_SEC_ADDR + sec_offset[idx]))
+
+#define SEC_JR0_ADDR(idx)	\
+	(SEC_ADDR(idx) +	\
+	 (CONFIG_SYS_FSL_JR0_OFFSET - CONFIG_SYS_FSL_SEC_OFFSET))
+
+struct jobring jr0[CONFIG_SYS_FSL_MAX_NUM_OF_SEC];
 
-static inline void start_jr0(void)
+static inline void start_jr0(uint8_t sec_idx)
 {
-	ccsr_sec_t *sec = (void *)CONFIG_SYS_FSL_SEC_ADDR;
+	ccsr_sec_t *sec = (void *)SEC_ADDR(sec_idx);
 	u32 ctpr_ms = sec_in32(&sec->ctpr_ms);
 	u32 scfgr = sec_in32(&sec->scfgr);
 
@@ -42,15 +57,15 @@ static inline void start_jr0(void)
 	}
 }
 
-static inline void jr_reset_liodn(void)
+static inline void jr_reset_liodn(uint8_t sec_idx)
 {
-	ccsr_sec_t *sec = (void *)CONFIG_SYS_FSL_SEC_ADDR;
+	ccsr_sec_t *sec = (void *)SEC_ADDR(sec_idx);
 	sec_out32(&sec->jrliodnr[0].ls, 0);
 }
 
-static inline void jr_disable_irq(void)
+static inline void jr_disable_irq(uint8_t sec_idx)
 {
-	struct jr_regs *regs = (struct jr_regs *)CONFIG_SYS_FSL_JR0_ADDR;
+	struct jr_regs *regs = (struct jr_regs *)SEC_JR0_ADDR(sec_idx);
 	uint32_t jrcfg = sec_in32(&regs->jrcfg1);
 
 	jrcfg = jrcfg | JR_INTMASK;
@@ -58,11 +73,12 @@ static inline void jr_disable_irq(void)
 	sec_out32(&regs->jrcfg1, jrcfg);
 }
 
-static void jr_initregs(void)
+static void jr_initregs(uint8_t sec_idx)
 {
-	struct jr_regs *regs = (struct jr_regs *)CONFIG_SYS_FSL_JR0_ADDR;
-	phys_addr_t ip_base = virt_to_phys((void *)jr.input_ring);
-	phys_addr_t op_base = virt_to_phys((void *)jr.output_ring);
+	struct jr_regs *regs = (struct jr_regs *)SEC_JR0_ADDR(sec_idx);
+	struct jobring *jr = &jr0[sec_idx];
+	phys_addr_t ip_base = virt_to_phys((void *)jr->input_ring);
+	phys_addr_t op_base = virt_to_phys((void *)jr->output_ring);
 
 #ifdef CONFIG_PHYS_64BIT
 	sec_out32(&regs->irba_h, ip_base >> 32);
@@ -79,59 +95,63 @@ static void jr_initregs(void)
 	sec_out32(&regs->ors, JR_SIZE);
 	sec_out32(&regs->irs, JR_SIZE);
 
-	if (!jr.irq)
-		jr_disable_irq();
+	if (!jr->irq)
+		jr_disable_irq(sec_idx);
 }
 
-static int jr_init(void)
+static int jr_init(uint8_t sec_idx)
 {
-	memset(&jr, 0, sizeof(struct jobring));
+	struct jobring *jr = &jr0[sec_idx];
 
-	jr.jq_id = DEFAULT_JR_ID;
-	jr.irq = DEFAULT_IRQ;
+	memset(jr, 0, sizeof(struct jobring));
+
+	jr->jq_id = DEFAULT_JR_ID;
+	jr->irq = DEFAULT_IRQ;
 
 #ifdef CONFIG_FSL_CORENET
-	jr.liodn = DEFAULT_JR_LIODN;
+	jr->liodn = DEFAULT_JR_LIODN;
 #endif
-	jr.size = JR_SIZE;
-	jr.input_ring = (dma_addr_t *)memalign(ARCH_DMA_MINALIGN,
+	jr->size = JR_SIZE;
+	jr->input_ring = (dma_addr_t *)memalign(ARCH_DMA_MINALIGN,
 				JR_SIZE * sizeof(dma_addr_t));
-	if (!jr.input_ring)
+	if (!jr->input_ring)
 		return -1;
 
-	jr.op_size = roundup(JR_SIZE * sizeof(struct op_ring),
-			     ARCH_DMA_MINALIGN);
-	jr.output_ring =
-	    (struct op_ring *)memalign(ARCH_DMA_MINALIGN, jr.op_size);
-	if (!jr.output_ring)
+	jr->op_size = roundup(JR_SIZE * sizeof(struct op_ring),
+			      ARCH_DMA_MINALIGN);
+	jr->output_ring =
+	    (struct op_ring *)memalign(ARCH_DMA_MINALIGN, jr->op_size);
+	if (!jr->output_ring)
 		return -1;
 
-	memset(jr.input_ring, 0, JR_SIZE * sizeof(dma_addr_t));
-	memset(jr.output_ring, 0, jr.op_size);
+	memset(jr->input_ring, 0, JR_SIZE * sizeof(dma_addr_t));
+	memset(jr->output_ring, 0, jr->op_size);
 
-	start_jr0();
+	start_jr0(sec_idx);
 
-	jr_initregs();
+	jr_initregs(sec_idx);
 
 	return 0;
 }
 
-static int jr_sw_cleanup(void)
+static int jr_sw_cleanup(uint8_t sec_idx)
 {
-	jr.head = 0;
-	jr.tail = 0;
-	jr.read_idx = 0;
-	jr.write_idx = 0;
-	memset(jr.info, 0, sizeof(jr.info));
-	memset(jr.input_ring, 0, jr.size * sizeof(dma_addr_t));
-	memset(jr.output_ring, 0, jr.size * sizeof(struct op_ring));
+	struct jobring *jr = &jr0[sec_idx];
+
+	jr->head = 0;
+	jr->tail = 0;
+	jr->read_idx = 0;
+	jr->write_idx = 0;
+	memset(jr->info, 0, sizeof(jr->info));
+	memset(jr->input_ring, 0, jr->size * sizeof(dma_addr_t));
+	memset(jr->output_ring, 0, jr->size * sizeof(struct op_ring));
 
 	return 0;
 }
 
-static int jr_hw_reset(void)
+static int jr_hw_reset(uint8_t sec_idx)
 {
-	struct jr_regs *regs = (struct jr_regs *)CONFIG_SYS_FSL_JR0_ADDR;
+	struct jr_regs *regs = (struct jr_regs *)SEC_JR0_ADDR(sec_idx);
 	uint32_t timeout = 100000;
 	uint32_t jrint, jrcr;
 
@@ -161,10 +181,11 @@ static int jr_hw_reset(void)
 /* -1 --- error, can't enqueue -- no space available */
 static int jr_enqueue(uint32_t *desc_addr,
 	       void (*callback)(uint32_t status, void *arg),
-	       void *arg)
+	       void *arg, uint8_t sec_idx)
 {
-	struct jr_regs *regs = (struct jr_regs *)CONFIG_SYS_FSL_JR0_ADDR;
-	int head = jr.head;
+	struct jr_regs *regs = (struct jr_regs *)SEC_JR0_ADDR(sec_idx);
+	struct jobring *jr = &jr0[sec_idx];
+	int head = jr->head;
 	uint32_t desc_word;
 	int length = desc_len(desc_addr);
 	int i;
@@ -184,18 +205,14 @@ static int jr_enqueue(uint32_t *desc_addr,
 
 	phys_addr_t desc_phys_addr = virt_to_phys(desc_addr);
 
-	if (sec_in32(&regs->irsa) == 0 ||
-	    CIRC_SPACE(jr.head, jr.tail, jr.size) <= 0)
-		return -1;
-
-	jr.info[head].desc_phys_addr = desc_phys_addr;
-	jr.info[head].callback = (void *)callback;
-	jr.info[head].arg = arg;
-	jr.info[head].op_done = 0;
+	jr->info[head].desc_phys_addr = desc_phys_addr;
+	jr->info[head].callback = (void *)callback;
+	jr->info[head].arg = arg;
+	jr->info[head].op_done = 0;
 
-	unsigned long start = (unsigned long)&jr.info[head] &
+	unsigned long start = (unsigned long)&jr->info[head] &
 					~(ARCH_DMA_MINALIGN - 1);
-	unsigned long end = ALIGN((unsigned long)&jr.info[head] +
+	unsigned long end = ALIGN((unsigned long)&jr->info[head] +
 				  sizeof(struct jr_info), ARCH_DMA_MINALIGN);
 	flush_dcache_range(start, end);
 
@@ -205,11 +222,11 @@ static int jr_enqueue(uint32_t *desc_addr,
 	 * depend on endianness of SEC block.
 	 */
 #ifdef CONFIG_SYS_FSL_SEC_LE
-	addr_lo = (uint32_t *)(&jr.input_ring[head]);
-	addr_hi = (uint32_t *)(&jr.input_ring[head]) + 1;
+	addr_lo = (uint32_t *)(&jr->input_ring[head]);
+	addr_hi = (uint32_t *)(&jr->input_ring[head]) + 1;
 #elif defined(CONFIG_SYS_FSL_SEC_BE)
-	addr_hi = (uint32_t *)(&jr.input_ring[head]);
-	addr_lo = (uint32_t *)(&jr.input_ring[head]) + 1;
+	addr_hi = (uint32_t *)(&jr->input_ring[head]);
+	addr_lo = (uint32_t *)(&jr->input_ring[head]) + 1;
 #endif /* ifdef CONFIG_SYS_FSL_SEC_LE */
 
 	sec_out32(addr_hi, (uint32_t)(desc_phys_addr >> 32));
@@ -217,21 +234,21 @@ static int jr_enqueue(uint32_t *desc_addr,
 
 #else
 	/* Write the 32 bit Descriptor address on Input Ring. */
-	sec_out32(&jr.input_ring[head], desc_phys_addr);
+	sec_out32(&jr->input_ring[head], desc_phys_addr);
 #endif /* ifdef CONFIG_PHYS_64BIT */
 
-	start = (unsigned long)&jr.input_ring[head] & ~(ARCH_DMA_MINALIGN - 1);
-	end = ALIGN((unsigned long)&jr.input_ring[head] +
+	start = (unsigned long)&jr->input_ring[head] & ~(ARCH_DMA_MINALIGN - 1);
+	end = ALIGN((unsigned long)&jr->input_ring[head] +
 		     sizeof(dma_addr_t), ARCH_DMA_MINALIGN);
 	flush_dcache_range(start, end);
 
-	jr.head = (head + 1) & (jr.size - 1);
+	jr->head = (head + 1) & (jr->size - 1);
 
 	/* Invalidate output ring */
-	start = (unsigned long)jr.output_ring &
+	start = (unsigned long)jr->output_ring &
 					~(ARCH_DMA_MINALIGN - 1);
-	end = ALIGN((unsigned long)jr.output_ring + jr.op_size,
-		     ARCH_DMA_MINALIGN);
+	end = ALIGN((unsigned long)jr->output_ring + jr->op_size,
+		    ARCH_DMA_MINALIGN);
 	invalidate_dcache_range(start, end);
 
 	sec_out32(&regs->irja, 1);
@@ -239,11 +256,12 @@ static int jr_enqueue(uint32_t *desc_addr,
 	return 0;
 }
 
-static int jr_dequeue(void)
+static int jr_dequeue(int sec_idx)
 {
-	struct jr_regs *regs = (struct jr_regs *)CONFIG_SYS_FSL_JR0_ADDR;
-	int head = jr.head;
-	int tail = jr.tail;
+	struct jr_regs *regs = (struct jr_regs *)SEC_JR0_ADDR(sec_idx);
+	struct jobring *jr = &jr0[sec_idx];
+	int head = jr->head;
+	int tail = jr->tail;
 	int idx, i, found;
 	void (*callback)(uint32_t status, void *arg);
 	void *arg = NULL;
@@ -253,7 +271,8 @@ static int jr_dequeue(void)
 	uint32_t *addr;
 #endif
 
-	while (sec_in32(&regs->orsf) && CIRC_CNT(jr.head, jr.tail, jr.size)) {
+	while (sec_in32(&regs->orsf) && CIRC_CNT(jr->head, jr->tail,
+						 jr->size)) {
 
 		found = 0;
 
@@ -264,11 +283,11 @@ static int jr_dequeue(void)
 		 * depend on endianness of SEC block.
 		 */
 	#ifdef CONFIG_SYS_FSL_SEC_LE
-		addr_lo = (uint32_t *)(&jr.output_ring[jr.tail].desc);
-		addr_hi = (uint32_t *)(&jr.output_ring[jr.tail].desc) + 1;
+		addr_lo = (uint32_t *)(&jr->output_ring[jr->tail].desc);
+		addr_hi = (uint32_t *)(&jr->output_ring[jr->tail].desc) + 1;
 	#elif defined(CONFIG_SYS_FSL_SEC_BE)
-		addr_hi = (uint32_t *)(&jr.output_ring[jr.tail].desc);
-		addr_lo = (uint32_t *)(&jr.output_ring[jr.tail].desc) + 1;
+		addr_hi = (uint32_t *)(&jr->output_ring[jr->tail].desc);
+		addr_lo = (uint32_t *)(&jr->output_ring[jr->tail].desc) + 1;
 	#endif /* ifdef CONFIG_SYS_FSL_SEC_LE */
 
 		op_desc = ((u64)sec_in32(addr_hi) << 32) |
@@ -276,15 +295,15 @@ static int jr_dequeue(void)
 
 	#else
 		/* Read the 32 bit Descriptor address from Output Ring. */
-		addr = (uint32_t *)&jr.output_ring[jr.tail].desc;
+		addr = (uint32_t *)&jr->output_ring[jr->tail].desc;
 		op_desc = sec_in32(addr);
 	#endif /* ifdef CONFIG_PHYS_64BIT */
 
-		uint32_t status = sec_in32(&jr.output_ring[jr.tail].status);
+		uint32_t status = sec_in32(&jr->output_ring[jr->tail].status);
 
-		for (i = 0; CIRC_CNT(head, tail + i, jr.size) >= 1; i++) {
-			idx = (tail + i) & (jr.size - 1);
-			if (op_desc == jr.info[idx].desc_phys_addr) {
+		for (i = 0; CIRC_CNT(head, tail + i, jr->size) >= 1; i++) {
+			idx = (tail + i) & (jr->size - 1);
+			if (op_desc == jr->info[idx].desc_phys_addr) {
 				found = 1;
 				break;
 			}
@@ -294,9 +313,9 @@ static int jr_dequeue(void)
 		if (!found)
 			return -1;
 
-		jr.info[idx].op_done = 1;
-		callback = (void *)jr.info[idx].callback;
-		arg = jr.info[idx].arg;
+		jr->info[idx].op_done = 1;
+		callback = (void *)jr->info[idx].callback;
+		arg = jr->info[idx].arg;
 
 		/* When the job on tail idx gets done, increment
 		 * tail till the point where job completed out of oredr has
@@ -304,14 +323,14 @@ static int jr_dequeue(void)
 		 */
 		if (idx == tail)
 			do {
-				tail = (tail + 1) & (jr.size - 1);
-			} while (jr.info[tail].op_done);
+				tail = (tail + 1) & (jr->size - 1);
+			} while (jr->info[tail].op_done);
 
-		jr.tail = tail;
-		jr.read_idx = (jr.read_idx + 1) & (jr.size - 1);
+		jr->tail = tail;
+		jr->read_idx = (jr->read_idx + 1) & (jr->size - 1);
 
 		sec_out32(&regs->orjr, 1);
-		jr.info[idx].op_done = 0;
+		jr->info[idx].op_done = 0;
 
 		callback(status, arg);
 	}
@@ -327,7 +346,7 @@ static void desc_done(uint32_t status, void *arg)
 	x->done = 1;
 }
 
-int run_descriptor_jr(uint32_t *desc)
+static inline int run_descriptor_jr_idx(uint32_t *desc, uint8_t sec_idx)
 {
 	unsigned long long timeval = get_ticks();
 	unsigned long long timeout = usec2ticks(CONFIG_SEC_DEQ_TIMEOUT);
@@ -336,7 +355,7 @@ int run_descriptor_jr(uint32_t *desc)
 
 	memset(&op, 0, sizeof(op));
 
-	ret = jr_enqueue(desc, desc_done, &op);
+	ret = jr_enqueue(desc, desc_done, &op, sec_idx);
 	if (ret) {
 		debug("Error in SEC enq\n");
 		ret = JQ_ENQ_ERR;
@@ -346,7 +365,7 @@ int run_descriptor_jr(uint32_t *desc)
 	timeval = get_ticks();
 	timeout = usec2ticks(CONFIG_SEC_DEQ_TIMEOUT);
 	while (op.done != 1) {
-		ret = jr_dequeue();
+		ret = jr_dequeue(sec_idx);
 		if (ret) {
 			debug("Error in SEC deq\n");
 			ret = JQ_DEQ_ERR;
@@ -368,20 +387,30 @@ out:
 	return ret;
 }
 
-int jr_reset(void)
+int run_descriptor_jr(uint32_t *desc)
+{
+	return run_descriptor_jr_idx(desc, 0);
+}
+
+static inline int jr_reset_sec(uint8_t sec_idx)
 {
-	if (jr_hw_reset() < 0)
+	if (jr_hw_reset(sec_idx) < 0)
 		return -1;
 
 	/* Clean up the jobring structure maintained by software */
-	jr_sw_cleanup();
+	jr_sw_cleanup(sec_idx);
 
 	return 0;
 }
 
-int sec_reset(void)
+int jr_reset(void)
 {
-	ccsr_sec_t *sec = (void *)CONFIG_SYS_FSL_SEC_ADDR;
+	return jr_reset_sec(0);
+}
+
+static inline int sec_reset_idx(uint8_t sec_idx)
+{
+	ccsr_sec_t *sec = (void *)SEC_ADDR(sec_idx);
 	uint32_t mcfgr = sec_in32(&sec->mcfgr);
 	uint32_t timeout = 100000;
 
@@ -408,14 +437,13 @@ int sec_reset(void)
 	return 0;
 }
 
-static int instantiate_rng(void)
+static int instantiate_rng(uint8_t sec_idx)
 {
 	struct result op;
 	u32 *desc;
 	u32 rdsta_val;
 	int ret = 0;
-	ccsr_sec_t __iomem *sec =
-			(ccsr_sec_t __iomem *)CONFIG_SYS_FSL_SEC_ADDR;
+	ccsr_sec_t __iomem *sec = (ccsr_sec_t __iomem *)SEC_ADDR(sec_idx);
 	struct rng4tst __iomem *rng =
 			(struct rng4tst __iomem *)&sec->rng;
 
@@ -432,7 +460,7 @@ static int instantiate_rng(void)
 	flush_dcache_range((unsigned long)desc,
 			   (unsigned long)desc + size);
 
-	ret = run_descriptor_jr(desc);
+	ret = run_descriptor_jr_idx(desc, sec_idx);
 
 	if (ret)
 		printf("RNG: Instantiation failed with error %x\n", ret);
@@ -444,9 +472,14 @@ static int instantiate_rng(void)
 	return ret;
 }
 
-static u8 get_rng_vid(void)
+int sec_reset(void)
+{
+	return sec_reset_idx(0);
+}
+
+static u8 get_rng_vid(uint8_t sec_idx)
 {
-	ccsr_sec_t *sec = (void *)CONFIG_SYS_FSL_SEC_ADDR;
+	ccsr_sec_t *sec = (void *)SEC_ADDR(sec_idx);
 	u32 cha_vid = sec_in32(&sec->chavid_ls);
 
 	return (cha_vid & SEC_CHAVID_RNG_LS_MASK) >> SEC_CHAVID_LS_RNG_SHIFT;
@@ -456,10 +489,9 @@ static u8 get_rng_vid(void)
  * By default, the TRNG runs for 200 clocks per sample;
  * 1200 clocks per sample generates better entropy.
  */
-static void kick_trng(int ent_delay)
+static void kick_trng(int ent_delay, uint8_t sec_idx)
 {
-	ccsr_sec_t __iomem *sec =
-			(ccsr_sec_t __iomem *)CONFIG_SYS_FSL_SEC_ADDR;
+	ccsr_sec_t __iomem *sec = (ccsr_sec_t __iomem *)SEC_ADDR(sec_idx);
 	struct rng4tst __iomem *rng =
 			(struct rng4tst __iomem *)&sec->rng;
 	u32 val;
@@ -486,11 +518,10 @@ static void kick_trng(int ent_delay)
 	sec_clrbits32(&rng->rtmctl, RTMCTL_PRGM);
 }
 
-static int rng_init(void)
+static int rng_init(uint8_t sec_idx)
 {
 	int ret, ent_delay = RTSDCTL_ENT_DLY_MIN;
-	ccsr_sec_t __iomem *sec =
-			(ccsr_sec_t __iomem *)CONFIG_SYS_FSL_SEC_ADDR;
+	ccsr_sec_t __iomem *sec = (ccsr_sec_t __iomem *)SEC_ADDR(sec_idx);
 	struct rng4tst __iomem *rng =
 			(struct rng4tst __iomem *)&sec->rng;
 
@@ -509,7 +540,7 @@ static int rng_init(void)
 		 * Also, if a handle was instantiated, do not change
 		 * the TRNG parameters.
 		 */
-		kick_trng(ent_delay);
+		kick_trng(ent_delay, sec_idx);
 		ent_delay += 400;
 		/*
 		 * if instantiate_rng(...) fails, the loop will rerun
@@ -518,7 +549,7 @@ static int rng_init(void)
 		 * interval, leading to a sucessful initialization of
 		 * the RNG.
 		 */
-		ret = instantiate_rng();
+		ret = instantiate_rng(sec_idx);
 	} while ((ret == -1) && (ent_delay < RTSDCTL_ENT_DLY_MAX));
 	if (ret) {
 		printf("RNG: Failed to instantiate RNG\n");
@@ -531,9 +562,9 @@ static int rng_init(void)
 	return ret;
 }
 
-int sec_init(void)
+int sec_init_idx(uint8_t sec_idx)
 {
-	ccsr_sec_t *sec = (void *)CONFIG_SYS_FSL_SEC_ADDR;
+	ccsr_sec_t *sec = (void *)SEC_ADDR(sec_idx);
 	uint32_t mcr = sec_in32(&sec->mcfgr);
 	int ret = 0;
 
@@ -543,6 +574,11 @@ int sec_init(void)
 	uint32_t liodn_s;
 #endif
 
+	if (!(sec_idx < CONFIG_SYS_FSL_MAX_NUM_OF_SEC)) {
+		printf("SEC initialization failed\n");
+		return -1;
+	}
+
 	/*
 	 * Modifying CAAM Read/Write Attributes
 	 * For LS2080A
@@ -568,7 +604,7 @@ int sec_init(void)
 	liodn_s = (liodnr & JRSLIODN_MASK) >> JRSLIODN_SHIFT;
 #endif
 
-	ret = jr_init();
+	ret = jr_init(sec_idx);
 	if (ret < 0) {
 		printf("SEC initialization failed\n");
 		return -1;
@@ -582,13 +618,18 @@ int sec_init(void)
 	pamu_enable();
 #endif
 
-	if (get_rng_vid() >= 4) {
-		if (rng_init() < 0) {
-			printf("RNG instantiation failed\n");
+	if (get_rng_vid(sec_idx) >= 4) {
+		if (rng_init(sec_idx) < 0) {
+			printf("SEC%u: RNG instantiation failed\n", sec_idx);
 			return -1;
 		}
-		printf("SEC: RNG instantiated\n");
+		printf("SEC%u: RNG instantiated\n", sec_idx);
 	}
 
 	return ret;
 }
+
+int sec_init(void)
+{
+	return sec_init_idx(0);
+}
diff --git a/drivers/crypto/fsl/jr.h b/drivers/crypto/fsl/jr.h
index 1642dbb..d897e57 100644
--- a/drivers/crypto/fsl/jr.h
+++ b/drivers/crypto/fsl/jr.h
@@ -90,6 +90,9 @@ struct jobring {
 	/* This ring can be on the stack */
 	struct jr_info info[JR_SIZE];
 	struct op_ring *output_ring;
+	/* Offset in CCSR to the SEC engine to which this JR belongs */
+	uint32_t sec_offset;
+
 };
 
 struct result {
diff --git a/drivers/ddr/fsl/ctrl_regs.c b/drivers/ddr/fsl/ctrl_regs.c
index 9073917..1d5cec6 100644
--- a/drivers/ddr/fsl/ctrl_regs.c
+++ b/drivers/ddr/fsl/ctrl_regs.c
@@ -709,7 +709,7 @@ static void set_timing_cfg_2(const unsigned int ctrl_num,
 		| ((add_lat_mclk & 0xf) << 28)
 		| ((cpo & 0x1f) << 23)
 		| ((wr_lat & 0xf) << 19)
-		| ((wr_lat & 0x10) << 14)
+		| ((wr_lat & 0x10) << 18)
 		| ((rd_to_pre & RD_TO_PRE_MASK) << RD_TO_PRE_SHIFT)
 		| ((wr_data_delay & WR_DATA_DELAY_MASK) << WR_DATA_DELAY_SHIFT)
 		| ((cke_pls & 0x7) << 6)
@@ -1835,10 +1835,17 @@ static void set_ddr_sdram_clk_cntl(fsl_ddr_cfg_regs_t *ddr,
 	/* Per FSL Application Note: AN2805 */
 	ss_en = 1;
 #endif
-	clk_adjust = popts->clk_adjust;
+	if (fsl_ddr_get_version(0) >= 0x40701) {
+		/* clk_adjust in 5-bits on T-series and LS-series */
+		clk_adjust = (popts->clk_adjust & 0x1F) << 22;
+	} else {
+		/* clk_adjust in 4-bits on earlier MPC85xx and P-series */
+		clk_adjust = (popts->clk_adjust & 0xF) << 23;
+	}
+
 	ddr->ddr_sdram_clk_cntl = (0
 				   | ((ss_en & 0x1) << 31)
-				   | ((clk_adjust & 0xF) << 23)
+				   | clk_adjust
 				   );
 	debug("FSLDDR: clk_cntl = 0x%08x\n", ddr->ddr_sdram_clk_cntl);
 }
diff --git a/drivers/ddr/fsl/fsl_ddr_gen4.c b/drivers/ddr/fsl/fsl_ddr_gen4.c
index 608810d..d37e247 100644
--- a/drivers/ddr/fsl/fsl_ddr_gen4.c
+++ b/drivers/ddr/fsl/fsl_ddr_gen4.c
@@ -56,7 +56,8 @@ void fsl_ddr_set_memctl_regs(const fsl_ddr_cfg_regs_t *regs,
 	u32 vref_seq2[3] = {0xc0, 0xf0, 0x70};	/* for range 2 */
 	u32 *vref_seq = vref_seq1;
 #endif
-#ifdef CONFIG_SYS_FSL_ERRATUM_A009942
+#if defined(CONFIG_SYS_FSL_ERRATUM_A009942) | \
+	defined(CONFIG_SYS_FSL_ERRATUM_A010165)
 	ulong ddr_freq;
 	u32 tmp;
 #endif
@@ -205,12 +206,14 @@ void fsl_ddr_set_memctl_regs(const fsl_ddr_cfg_regs_t *regs,
 
 #ifdef CONFIG_SYS_FSL_ERRATUM_A009803
 	/* part 1 of 2 */
-	if (regs->ddr_sdram_cfg & SDRAM_CFG_RD_EN) { /* for RDIMM */
-		ddr_out32(&ddr->ddr_sdram_rcw_2,
-			  regs->ddr_sdram_rcw_2 & ~0x0f000000);
+	if (regs->ddr_sdram_cfg_2 & SDRAM_CFG2_AP_EN) {
+		if (regs->ddr_sdram_cfg & SDRAM_CFG_RD_EN) { /* for RDIMM */
+			ddr_out32(&ddr->ddr_sdram_rcw_2,
+				  regs->ddr_sdram_rcw_2 & ~0x0f000000);
+		}
+		ddr_out32(&ddr->err_disable, regs->err_disable |
+			  DDR_ERR_DISABLE_APED);
 	}
-
-	ddr_out32(&ddr->err_disable, regs->err_disable | DDR_ERR_DISABLE_APED);
 #else
 	ddr_out32(&ddr->err_disable, regs->err_disable);
 #endif
@@ -240,8 +243,10 @@ void fsl_ddr_set_memctl_regs(const fsl_ddr_cfg_regs_t *regs,
 		/* 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 transmit bit deskew */
+		temp32 = ddr_in32(&ddr->debug[28]);
+		temp32 |= DDR_TX_BD_DIS;
+		ddr_out32(&ddr->debug[28], temp32);
 		/* Disable D_INIT */
 		ddr_out32(&ddr->sdram_cfg_2,
 			  regs->ddr_sdram_cfg_2 & ~SDRAM_CFG2_D_INIT);
@@ -249,6 +254,13 @@ void fsl_ddr_set_memctl_regs(const fsl_ddr_cfg_regs_t *regs,
 	}
 #endif
 
+#ifdef CONFIG_SYS_FSL_ERRATUM_A009801
+	temp32 = ddr_in32(&ddr->debug[25]);
+	temp32 &= ~DDR_CAS_TO_PRE_SUB_MASK;
+	temp32 |= 9 << DDR_CAS_TO_PRE_SUB_SHIFT;
+	ddr_out32(&ddr->debug[25], temp32);
+#endif
+
 #ifdef CONFIG_SYS_FSL_ERRATUM_A009942
 	ddr_freq = get_ddr_freq(ctrl_num) / 1000000;
 	tmp = ddr_in32(&ddr->debug[28]);
@@ -262,6 +274,13 @@ void fsl_ddr_set_memctl_regs(const fsl_ddr_cfg_regs_t *regs,
 		ddr_out32(&ddr->debug[28], tmp | 0x0060007b);
 #endif
 
+#ifdef CONFIG_SYS_FSL_ERRATUM_A010165
+	ddr_freq = get_ddr_freq(ctrl_num) / 1000000;
+	if ((ddr_freq > 1900) && (ddr_freq < 2300)) {
+		tmp = ddr_in32(&ddr->debug[28]);
+		ddr_out32(&ddr->debug[28], tmp | 0x000a0000);
+	}
+#endif
 	/*
 	 * For RDIMMs, JEDEC spec requires clocks to be stable before reset is
 	 * deasserted. Clocks start when any chip select is enabled and clock
@@ -358,7 +377,9 @@ step2:
 			debug("MR6 = 0x%08x\n", temp32);
 		}
 		ddr_out32(&ddr->sdram_md_cntl, 0);
-		ddr_out32(&ddr->debug[28], 0);		/* Enable deskew */
+		temp32 = ddr_in32(&ddr->debug[28]);
+		temp32 &= ~DDR_TX_BD_DIS; /* Enable deskew */
+		ddr_out32(&ddr->debug[28], temp32);
 		ddr_out32(&ddr->debug[1], 0x400);	/* restart deskew */
 		/* wait for idle */
 		timeout = 40;
@@ -376,22 +397,24 @@ step2:
 #endif /* CONFIG_SYS_FSL_ERRATUM_A008511 */
 
 #ifdef CONFIG_SYS_FSL_ERRATUM_A009803
-		/* if it's RDIMM */
-		if (regs->ddr_sdram_cfg & SDRAM_CFG_RD_EN) {
-			for (i = 0; i < CONFIG_CHIP_SELECTS_PER_CTRL; i++) {
-				if (!(regs->cs[i].config & SDRAM_CS_CONFIG_EN))
-					continue;
-				set_wait_for_bits_clear(&ddr->sdram_md_cntl,
-							MD_CNTL_MD_EN |
-							MD_CNTL_CS_SEL(i) |
-							0x070000ed,
-							MD_CNTL_MD_EN);
-				udelay(1);
+		if (regs->ddr_sdram_cfg_2 & SDRAM_CFG2_AP_EN) {
+			/* if it's RDIMM */
+			if (regs->ddr_sdram_cfg & SDRAM_CFG_RD_EN) {
+				for (i = 0; i < CONFIG_CHIP_SELECTS_PER_CTRL; i++) {
+					if (!(regs->cs[i].config & SDRAM_CS_CONFIG_EN))
+						continue;
+					set_wait_for_bits_clear(&ddr->sdram_md_cntl,
+								MD_CNTL_MD_EN |
+								MD_CNTL_CS_SEL(i) |
+								0x070000ed,
+								MD_CNTL_MD_EN);
+					udelay(1);
+				}
 			}
-		}
 
-		ddr_out32(&ddr->err_disable,
-			  regs->err_disable & ~DDR_ERR_DISABLE_APED);
+			ddr_out32(&ddr->err_disable,
+				  regs->err_disable & ~DDR_ERR_DISABLE_APED);
+		}
 #endif
 	}
 #endif
diff --git a/drivers/ddr/fsl/options.c b/drivers/ddr/fsl/options.c
index d0075ff..793d12a 100644
--- a/drivers/ddr/fsl/options.c
+++ b/drivers/ddr/fsl/options.c
@@ -886,7 +886,8 @@ unsigned int populate_memctl_options(const common_timing_params_t *common_dimm,
 	} else
 		popts->ecc_mode = 1;
 #endif
-	popts->ecc_init_using_memctl = 1; /* 0 = use DMA, 1 = use memctl */
+	/* 1 = use memory controler to init data */
+	popts->ecc_init_using_memctl = popts->ecc_mode ? 1 : 0;
 
 	/*
 	 * Choose DQS config
diff --git a/drivers/dfu/dfu_nand.c b/drivers/dfu/dfu_nand.c
index a975492..9fb874c 100644
--- a/drivers/dfu/dfu_nand.c
+++ b/drivers/dfu/dfu_nand.c
@@ -25,7 +25,7 @@ static int nand_block_op(enum dfu_op op, struct dfu_entity *dfu,
 	loff_t start, lim;
 	size_t count, actual;
 	int ret;
-	nand_info_t *nand;
+	struct mtd_info *mtd;
 
 	/* if buf == NULL return total size of the area */
 	if (buf == NULL) {
@@ -39,16 +39,16 @@ static int nand_block_op(enum dfu_op op, struct dfu_entity *dfu,
 
 	if (nand_curr_device < 0 ||
 	    nand_curr_device >= CONFIG_SYS_MAX_NAND_DEVICE ||
-	    !nand_info[nand_curr_device].name) {
+	    !nand_info[nand_curr_device]->name) {
 		printf("%s: invalid nand device\n", __func__);
 		return -1;
 	}
 
-	nand = &nand_info[nand_curr_device];
+	mtd = nand_info[nand_curr_device];
 
 	if (op == DFU_OP_READ) {
-		ret = nand_read_skip_bad(nand, start, &count, &actual,
-				lim, buf);
+		ret = nand_read_skip_bad(mtd, start, &count, &actual,
+					 lim, buf);
 	} else {
 		nand_erase_options_t opts;
 
@@ -59,12 +59,12 @@ static int nand_block_op(enum dfu_op op, struct dfu_entity *dfu,
 		opts.quiet = 1;
 		opts.lim = lim;
 		/* first erase */
-		ret = nand_erase_opts(nand, &opts);
+		ret = nand_erase_opts(mtd, &opts);
 		if (ret)
 			return ret;
 		/* then write */
-		ret = nand_write_skip_bad(nand, start, &count, &actual,
-				lim, buf, WITH_WR_VERIFY);
+		ret = nand_write_skip_bad(mtd, start, &count, &actual,
+					  lim, buf, WITH_WR_VERIFY);
 	}
 
 	if (ret != 0) {
@@ -139,27 +139,37 @@ static int dfu_read_medium_nand(struct dfu_entity *dfu, u64 offset, void *buf,
 static int dfu_flush_medium_nand(struct dfu_entity *dfu)
 {
 	int ret = 0;
+	u64 off;
 
 	/* in case of ubi partition, erase rest of the partition */
 	if (dfu->data.nand.ubi) {
-		nand_info_t *nand;
+		struct mtd_info *mtd;
 		nand_erase_options_t opts;
 
 		if (nand_curr_device < 0 ||
 		    nand_curr_device >= CONFIG_SYS_MAX_NAND_DEVICE ||
-		    !nand_info[nand_curr_device].name) {
+		    !nand_info[nand_curr_device]->name) {
 			printf("%s: invalid nand device\n", __func__);
 			return -1;
 		}
 
-		nand = &nand_info[nand_curr_device];
+		mtd = nand_info[nand_curr_device];
 
 		memset(&opts, 0, sizeof(opts));
-		opts.offset = dfu->data.nand.start + dfu->offset +
+		off = dfu->offset;
+		if ((off & (mtd->erasesize - 1)) != 0) {
+			/*
+			 * last write ended with unaligned length
+			 * sector is erased, jump to next
+			 */
+			off = off & ~((mtd->erasesize - 1));
+			off += mtd->erasesize;
+		}
+		opts.offset = dfu->data.nand.start + off +
 				dfu->bad_skip;
 		opts.length = dfu->data.nand.start +
 				dfu->data.nand.size - opts.offset;
-		ret = nand_erase_opts(nand, &opts);
+		ret = nand_erase_opts(mtd, &opts);
 		if (ret != 0)
 			printf("Failure erase: %d\n", ret);
 	}
diff --git a/drivers/fpga/fpga.c b/drivers/fpga/fpga.c
index d94eb5c..7e2f3e1 100644
--- a/drivers/fpga/fpga.c
+++ b/drivers/fpga/fpga.c
@@ -120,7 +120,7 @@ static int fpga_dev_info(int devnum)
 }
 
 /*
- * fgpa_init is usually called from misc_init_r() and MUST be called
+ * fpga_init is usually called from misc_init_r() and MUST be called
  * before any of the other fpga functions are used.
  */
 void fpga_init(void)
diff --git a/drivers/gpio/Kconfig b/drivers/gpio/Kconfig
index 93a7e8c..73b862d 100644
--- a/drivers/gpio/Kconfig
+++ b/drivers/gpio/Kconfig
@@ -109,6 +109,21 @@ config SANDBOX_GPIO_COUNT
 	  of 'anonymous' GPIOs that do not belong to any device or bank.
 	  Select a suitable value depending on your needs.
 
+config TEGRA_GPIO
+	bool "Tegra20..210 GPIO driver"
+	depends on DM_GPIO
+	help
+	  Support for the GPIO controller contained in NVIDIA Tegra20 through
+	  Tegra210.
+
+config TEGRA186_GPIO
+	bool "Tegra186 GPIO driver"
+	depends on DM_GPIO
+	help
+	  Support for the GPIO controller contained in NVIDIA Tegra186. This
+	  covers both the "main" and "AON" controller instances, even though
+	  they have slightly different register layout.
+
 config GPIO_UNIPHIER
 	bool "UniPhier GPIO"
 	depends on ARCH_UNIPHIER
@@ -173,4 +188,30 @@ config DM_PCA953X
 
 	  Now, max 24 bits chips and PCA953X compatible chips are
 	  supported
+
+config MPC85XX_GPIO
+	bool "Freescale MPC85XX GPIO driver"
+	depends on DM_GPIO
+	help
+	  This driver supports the built-in GPIO controller of MPC85XX CPUs.
+	  Each GPIO bank is identified by its own entry in the device tree,
+	  i.e.
+
+	  gpio-controller@fc00 {
+		#gpio-cells = <2>;
+		compatible = "fsl,pq3-gpio";
+		reg = <0xfc00 0x100>
+	  }
+
+	  By default, each bank is assumed to have 32 GPIOs, but the ngpios
+	  setting is honored, so the number of GPIOs for each bank is
+	  configurable to match the actual GPIO count of the SoC (e.g. the
+	  32/32/23 banks of the P1022 SoC).
+
+	  Aside from the standard functions of input/output mode, and output
+	  value setting, the open-drain feature, which can configure individual
+	  GPIOs to work as open-drain outputs, is supported.
+
+	  The driver has been tested on MPC85XX, but it is likely that other
+	  PowerQUICC III devices will work as well.
 endmenu
diff --git a/drivers/gpio/Makefile b/drivers/gpio/Makefile
index ddec1ef..792d191 100644
--- a/drivers/gpio/Makefile
+++ b/drivers/gpio/Makefile
@@ -31,10 +31,12 @@ obj-$(CONFIG_S5P)		+= s5p_gpio.o
 obj-$(CONFIG_SANDBOX_GPIO)	+= sandbox.o
 obj-$(CONFIG_SPEAR_GPIO)	+= spear_gpio.o
 obj-$(CONFIG_TEGRA_GPIO)	+= tegra_gpio.o
+obj-$(CONFIG_TEGRA186_GPIO)	+= tegra186_gpio.o
 obj-$(CONFIG_DA8XX_GPIO)	+= da8xx_gpio.o
 obj-$(CONFIG_DM644X_GPIO)	+= da8xx_gpio.o
 obj-$(CONFIG_ALTERA_PIO)	+= altera_pio.o
 obj-$(CONFIG_MPC83XX_GPIO)	+= mpc83xx_gpio.o
+obj-$(CONFIG_MPC85XX_GPIO)	+= mpc85xx_gpio.o
 obj-$(CONFIG_SH_GPIO_PFC)	+= sh_pfc.o
 obj-$(CONFIG_OMAP_GPIO)	+= omap_gpio.o
 obj-$(CONFIG_DB8500_GPIO)	+= db8500_gpio.o
diff --git a/drivers/gpio/gpio-uclass.c b/drivers/gpio/gpio-uclass.c
index 732b6c2..4559739 100644
--- a/drivers/gpio/gpio-uclass.c
+++ b/drivers/gpio/gpio-uclass.c
@@ -367,6 +367,38 @@ int dm_gpio_set_value(const struct gpio_desc *desc, int value)
 	return 0;
 }
 
+int dm_gpio_get_open_drain(struct gpio_desc *desc)
+{
+	struct dm_gpio_ops *ops = gpio_get_ops(desc->dev);
+	int ret;
+
+	ret = check_reserved(desc, "get_open_drain");
+	if (ret)
+		return ret;
+
+	if (ops->set_open_drain)
+		return ops->get_open_drain(desc->dev, desc->offset);
+	else
+		return -ENOSYS;
+}
+
+int dm_gpio_set_open_drain(struct gpio_desc *desc, int value)
+{
+	struct dm_gpio_ops *ops = gpio_get_ops(desc->dev);
+	int ret;
+
+	ret = check_reserved(desc, "set_open_drain");
+	if (ret)
+		return ret;
+
+	if (ops->set_open_drain)
+		ret = ops->set_open_drain(desc->dev, desc->offset, value);
+	else
+		return 0; /* feature not supported -> ignore setting */
+
+	return ret;
+}
+
 int dm_gpio_set_dir_flags(struct gpio_desc *desc, ulong flags)
 {
 	struct udevice *dev = desc->dev;
diff --git a/drivers/gpio/mpc85xx_gpio.c b/drivers/gpio/mpc85xx_gpio.c
new file mode 100644
index 0000000..04773e2
--- /dev/null
+++ b/drivers/gpio/mpc85xx_gpio.c
@@ -0,0 +1,228 @@
+/*
+ * (C) Copyright 2016
+ * Mario Six, Guntermann & Drunck GmbH, six@gdsys.de
+ *
+ * based on arch/powerpc/include/asm/mpc85xx_gpio.h, which is
+ *
+ * Copyright 2010 eXMeritus, A Boeing Company
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <dm.h>
+#include <asm/gpio.h>
+#include <mapmem.h>
+
+DECLARE_GLOBAL_DATA_PTR;
+
+struct ccsr_gpio {
+	u32	gpdir;
+	u32	gpodr;
+	u32	gpdat;
+	u32	gpier;
+	u32	gpimr;
+	u32	gpicr;
+};
+
+struct mpc85xx_gpio_data {
+	/* The bank's register base in memory */
+	struct ccsr_gpio __iomem *base;
+	/* The address of the registers; used to identify the bank */
+	ulong addr;
+	/* The GPIO count of the bank */
+	uint gpio_count;
+	/* The GPDAT register cannot be used to determine the value of output
+	 * pins on MPC8572/MPC8536, so we shadow it and use the shadowed value
+	 * for output pins */
+	u32 dat_shadow;
+};
+
+inline u32 gpio_mask(unsigned gpio) {
+	return (1U << (31 - (gpio)));
+}
+
+static inline u32 mpc85xx_gpio_get_val(struct ccsr_gpio *base, u32 mask)
+{
+	return in_be32(&base->gpdat) & mask;
+}
+
+static inline u32 mpc85xx_gpio_get_dir(struct ccsr_gpio *base, u32 mask)
+{
+	return in_be32(&base->gpdir) & mask;
+}
+
+static inline void mpc85xx_gpio_set_in(struct ccsr_gpio *base, u32 gpios)
+{
+	clrbits_be32(&base->gpdat, gpios);
+	/* GPDIR register 0 -> input */
+	clrbits_be32(&base->gpdir, gpios);
+}
+
+static inline void mpc85xx_gpio_set_low(struct ccsr_gpio *base, u32 gpios)
+{
+	clrbits_be32(&base->gpdat, gpios);
+	/* GPDIR register 1 -> output */
+	setbits_be32(&base->gpdir, gpios);
+}
+
+static inline void mpc85xx_gpio_set_high(struct ccsr_gpio *base, u32 gpios)
+{
+	setbits_be32(&base->gpdat, gpios);
+	/* GPDIR register 1 -> output */
+	setbits_be32(&base->gpdir, gpios);
+}
+
+static inline int mpc85xx_gpio_open_drain_val(struct ccsr_gpio *base, u32 mask)
+{
+	return in_be32(&base->gpodr) & mask;
+}
+
+static inline void mpc85xx_gpio_open_drain_on(struct ccsr_gpio *base, u32
+					      gpios)
+{
+	/* GPODR register 1 -> open drain on */
+	setbits_be32(&base->gpodr, gpios);
+}
+
+static inline void mpc85xx_gpio_open_drain_off(struct ccsr_gpio *base,
+					       u32 gpios)
+{
+	/* GPODR register 0 -> open drain off (actively driven) */
+	clrbits_be32(&base->gpodr, gpios);
+}
+
+static int mpc85xx_gpio_direction_input(struct udevice *dev, unsigned gpio)
+{
+	struct mpc85xx_gpio_data *data = dev_get_priv(dev);
+
+	mpc85xx_gpio_set_in(data->base, gpio_mask(gpio));
+	return 0;
+}
+
+static int mpc85xx_gpio_set_value(struct udevice *dev, unsigned gpio,
+				  int value)
+{
+	struct mpc85xx_gpio_data *data = dev_get_priv(dev);
+
+	if (value) {
+		data->dat_shadow |= gpio_mask(gpio);
+		mpc85xx_gpio_set_high(data->base, gpio_mask(gpio));
+	} else {
+		data->dat_shadow &= ~gpio_mask(gpio);
+		mpc85xx_gpio_set_low(data->base, gpio_mask(gpio));
+	}
+	return 0;
+}
+
+static int mpc85xx_gpio_direction_output(struct udevice *dev, unsigned gpio,
+					 int value)
+{
+	return mpc85xx_gpio_set_value(dev, gpio, value);
+}
+
+static int mpc85xx_gpio_get_value(struct udevice *dev, unsigned gpio)
+{
+	struct mpc85xx_gpio_data *data = dev_get_priv(dev);
+
+	if (!!mpc85xx_gpio_get_dir(data->base, gpio_mask(gpio))) {
+		/* Output -> use shadowed value */
+		return !!(data->dat_shadow & gpio_mask(gpio));
+	} else {
+		/* Input -> read value from GPDAT register */
+		return !!mpc85xx_gpio_get_val(data->base, gpio_mask(gpio));
+	}
+}
+
+static int mpc85xx_gpio_get_open_drain(struct udevice *dev, unsigned gpio)
+{
+	struct mpc85xx_gpio_data *data = dev_get_priv(dev);
+
+	return !!mpc85xx_gpio_open_drain_val(data->base, gpio_mask(gpio));
+}
+
+static int mpc85xx_gpio_set_open_drain(struct udevice *dev, unsigned gpio,
+				       int value)
+{
+	struct mpc85xx_gpio_data *data = dev_get_priv(dev);
+
+	if (value) {
+		mpc85xx_gpio_open_drain_on(data->base, gpio_mask(gpio));
+	} else {
+		mpc85xx_gpio_open_drain_off(data->base, gpio_mask(gpio));
+	}
+	return 0;
+}
+
+static int mpc85xx_gpio_get_function(struct udevice *dev, unsigned gpio)
+{
+	struct mpc85xx_gpio_data *data = dev_get_priv(dev);
+	int dir;
+
+	dir = !!mpc85xx_gpio_get_dir(data->base, gpio_mask(gpio));
+	return dir ? GPIOF_OUTPUT : GPIOF_INPUT;
+}
+
+static int mpc85xx_gpio_ofdata_to_platdata(struct udevice *dev) {
+	struct mpc85xx_gpio_data *data = dev_get_priv(dev);
+	fdt_addr_t addr;
+	fdt_size_t size;
+
+	addr = fdtdec_get_addr_size_auto_noparent(gd->fdt_blob, dev->of_offset,
+						  "reg", 0, &size);
+
+	data->addr = addr;
+	data->base = map_sysmem(CONFIG_SYS_IMMR + addr, size);
+
+	if (!data->base)
+		return -ENOMEM;
+
+	data->gpio_count = fdtdec_get_int(gd->fdt_blob, dev->of_offset,
+					  "ngpios", 32);
+	data->dat_shadow = 0;
+
+	return 0;
+}
+
+static int mpc85xx_gpio_probe(struct udevice *dev)
+{
+	struct gpio_dev_priv *uc_priv = dev_get_uclass_priv(dev);
+	struct mpc85xx_gpio_data *data = dev_get_priv(dev);
+	char name[32], *str;
+
+	snprintf(name, sizeof(name), "MPC@%lx_", data->addr);
+	str = strdup(name);
+
+	if (!str)
+		return -ENOMEM;
+
+	uc_priv->bank_name = str;
+	uc_priv->gpio_count = data->gpio_count;
+
+	return 0;
+}
+
+static const struct dm_gpio_ops gpio_mpc85xx_ops = {
+	.direction_input	= mpc85xx_gpio_direction_input,
+	.direction_output	= mpc85xx_gpio_direction_output,
+	.get_value		= mpc85xx_gpio_get_value,
+	.set_value		= mpc85xx_gpio_set_value,
+	.get_open_drain		= mpc85xx_gpio_get_open_drain,
+	.set_open_drain		= mpc85xx_gpio_set_open_drain,
+	.get_function 		= mpc85xx_gpio_get_function,
+};
+
+static const struct udevice_id mpc85xx_gpio_ids[] = {
+	{ .compatible = "fsl,pq3-gpio" },
+	{ /* sentinel */ }
+};
+
+U_BOOT_DRIVER(gpio_mpc85xx) = {
+	.name	= "gpio_mpc85xx",
+	.id	= UCLASS_GPIO,
+	.ops	= &gpio_mpc85xx_ops,
+	.ofdata_to_platdata = mpc85xx_gpio_ofdata_to_platdata,
+	.of_match = mpc85xx_gpio_ids,
+	.probe	= mpc85xx_gpio_probe,
+	.priv_auto_alloc_size = sizeof(struct mpc85xx_gpio_data),
+};
diff --git a/drivers/gpio/mxs_gpio.c b/drivers/gpio/mxs_gpio.c
index b54a10b..c25b4c1 100644
--- a/drivers/gpio/mxs_gpio.c
+++ b/drivers/gpio/mxs_gpio.c
@@ -8,7 +8,6 @@
  */
 
 #include <common.h>
-#include <netdev.h>
 #include <asm/errno.h>
 #include <asm/io.h>
 #include <asm/arch/iomux.h>
diff --git a/drivers/gpio/pca953x_gpio.c b/drivers/gpio/pca953x_gpio.c
index 987d10e..065b181 100644
--- a/drivers/gpio/pca953x_gpio.c
+++ b/drivers/gpio/pca953x_gpio.c
@@ -16,8 +16,8 @@
  *
  * TODO:
  * 1. Support PCA957X_TYPE
- * 2. Support max 40 gpio pins
- * 3. Support Plolarity Inversion
+ * 2. Support 24 gpio pins
+ * 3. Support Polarity Inversion
  */
 
 #include <common.h>
@@ -47,7 +47,7 @@ enum {
 	PCA953X_DIRECTION_OUT,
 };
 
-#define MAX_BANK 3
+#define MAX_BANK 5
 #define BANK_SZ 8
 
 DECLARE_GLOBAL_DATA_PTR;
@@ -121,6 +121,9 @@ static int pca953x_read_regs(struct udevice *dev, int reg, u8 *val)
 		ret = dm_i2c_read(dev, reg, val, 1);
 	} else if (info->gpio_count <= 16) {
 		ret = dm_i2c_read(dev, reg << 1, val, info->bank_count);
+	} else if (info->gpio_count == 40) {
+		/* Auto increment */
+		ret = dm_i2c_read(dev, (reg << 3) | 0x80, val, info->bank_count);
 	} else {
 		dev_err(dev, "Unsupported now\n");
 		return -EINVAL;
diff --git a/drivers/gpio/rk_gpio.c b/drivers/gpio/rk_gpio.c
index fefe3ca..64abcba 100644
--- a/drivers/gpio/rk_gpio.c
+++ b/drivers/gpio/rk_gpio.c
@@ -8,7 +8,6 @@
  */
 
 #include <common.h>
-#include <clk.h>
 #include <dm.h>
 #include <syscon.h>
 #include <asm/errno.h>
diff --git a/drivers/gpio/sandbox.c b/drivers/gpio/sandbox.c
index a9b1efc..f6435a0 100644
--- a/drivers/gpio/sandbox.c
+++ b/drivers/gpio/sandbox.c
@@ -15,6 +15,7 @@ DECLARE_GLOBAL_DATA_PTR;
 /* Flags for each GPIO */
 #define GPIOF_OUTPUT	(1 << 0)	/* Currently set as an output */
 #define GPIOF_HIGH	(1 << 1)	/* Currently set high */
+#define GPIOF_ODR	(1 << 2)	/* Currently set to open drain mode */
 
 struct gpio_state {
 	const char *label;	/* label given by requester */
@@ -70,6 +71,16 @@ int sandbox_gpio_set_value(struct udevice *dev, unsigned offset, int value)
 	return set_gpio_flag(dev, offset, GPIOF_HIGH, value);
 }
 
+int sandbox_gpio_get_open_drain(struct udevice *dev, unsigned offset)
+{
+	return get_gpio_flag(dev, offset, GPIOF_ODR);
+}
+
+int sandbox_gpio_set_open_drain(struct udevice *dev, unsigned offset, int value)
+{
+	return set_gpio_flag(dev, offset, GPIOF_ODR, value);
+}
+
 int sandbox_gpio_get_direction(struct udevice *dev, unsigned offset)
 {
 	return get_gpio_flag(dev, offset, GPIOF_OUTPUT);
@@ -124,6 +135,28 @@ static int sb_gpio_set_value(struct udevice *dev, unsigned offset, int value)
 	return sandbox_gpio_set_value(dev, offset, value);
 }
 
+/* read GPIO ODR value of port 'offset' */
+static int sb_gpio_get_open_drain(struct udevice *dev, unsigned offset)
+{
+	debug("%s: offset:%u\n", __func__, offset);
+
+	return sandbox_gpio_get_open_drain(dev, offset);
+}
+
+/* write GPIO ODR value to port 'offset' */
+static int sb_gpio_set_open_drain(struct udevice *dev, unsigned offset, int value)
+{
+	debug("%s: offset:%u, value = %d\n", __func__, offset, value);
+
+	if (!sandbox_gpio_get_direction(dev, offset)) {
+		printf("sandbox_gpio: error: set_open_drain on input gpio %u\n",
+		       offset);
+		return -1;
+	}
+
+	return sandbox_gpio_set_open_drain(dev, offset, value);
+}
+
 static int sb_gpio_get_function(struct udevice *dev, unsigned offset)
 {
 	if (get_gpio_flag(dev, offset, GPIOF_OUTPUT))
@@ -154,6 +187,8 @@ static const struct dm_gpio_ops gpio_sandbox_ops = {
 	.direction_output	= sb_gpio_direction_output,
 	.get_value		= sb_gpio_get_value,
 	.set_value		= sb_gpio_set_value,
+	.get_open_drain		= sb_gpio_get_open_drain,
+	.set_open_drain		= sb_gpio_set_open_drain,
 	.get_function		= sb_gpio_get_function,
 	.xlate			= sb_gpio_xlate,
 };
diff --git a/drivers/gpio/sunxi_gpio.c b/drivers/gpio/sunxi_gpio.c
index a7cec18..94abbeb 100644
--- a/drivers/gpio/sunxi_gpio.c
+++ b/drivers/gpio/sunxi_gpio.c
@@ -258,43 +258,30 @@ static int gpio_sunxi_probe(struct udevice *dev)
 
 	return 0;
 }
+
+struct sunxi_gpio_soc_data {
+	int start;
+	int no_banks;
+};
+
 /**
  * We have a top-level GPIO device with no actual GPIOs. It has a child
  * device for each Sunxi bank.
  */
 static int gpio_sunxi_bind(struct udevice *parent)
 {
+	struct sunxi_gpio_soc_data *soc_data =
+		(struct sunxi_gpio_soc_data *)dev_get_driver_data(parent);
 	struct sunxi_gpio_platdata *plat = parent->platdata;
 	struct sunxi_gpio_reg *ctlr;
-	int bank, no_banks, ret, start;
+	int bank, ret;
 
 	/* If this is a child device, there is nothing to do here */
 	if (plat)
 		return 0;
 
-	if (fdt_node_check_compatible(gd->fdt_blob, parent->of_offset,
-				"allwinner,sun6i-a31-r-pinctrl") == 0) {
-		start = 'L' - 'A';
-		no_banks = 2; /* L & M */
-	} else if (fdt_node_check_compatible(gd->fdt_blob, parent->of_offset,
-				"allwinner,sun8i-a23-r-pinctrl") == 0 ||
-		   fdt_node_check_compatible(gd->fdt_blob, parent->of_offset,
-				"allwinner,sun8i-a83t-r-pinctrl") == 0 ||
-		   fdt_node_check_compatible(gd->fdt_blob, parent->of_offset,
-				"allwinner,sun8i-h3-r-pinctrl") == 0) {
-		start = 'L' - 'A';
-		no_banks = 1; /* L only */
-	} else if (fdt_node_check_compatible(gd->fdt_blob, parent->of_offset,
-				"allwinner,sun9i-a80-r-pinctrl") == 0) {
-		start = 'L' - 'A';
-		no_banks = 3; /* L, M & N */
-	} else {
-		start = 0;
-		no_banks = SUNXI_GPIO_BANKS;
-	}
-
 	ctlr = (struct sunxi_gpio_reg *)dev_get_addr(parent);
-	for (bank = 0; bank < no_banks; bank++) {
+	for (bank = 0; bank < soc_data->no_banks; bank++) {
 		struct sunxi_gpio_platdata *plat;
 		struct udevice *dev;
 
@@ -302,7 +289,7 @@ static int gpio_sunxi_bind(struct udevice *parent)
 		if (!plat)
 			return -ENOMEM;
 		plat->regs = &ctlr->gpio_bank[bank];
-		plat->bank_name = gpio_bank_name(start + bank);
+		plat->bank_name = gpio_bank_name(soc_data->start + bank);
 		plat->gpio_count = SUNXI_GPIOS_PER_BANK;
 
 		ret = device_bind(parent, parent->driver,
@@ -315,23 +302,46 @@ static int gpio_sunxi_bind(struct udevice *parent)
 	return 0;
 }
 
+static const struct sunxi_gpio_soc_data soc_data_a_all = {
+	.start = 0,
+	.no_banks = SUNXI_GPIO_BANKS,
+};
+
+static const struct sunxi_gpio_soc_data soc_data_l_1 = {
+	.start = 'L' - 'A',
+	.no_banks = 1,
+};
+
+static const struct sunxi_gpio_soc_data soc_data_l_2 = {
+	.start = 'L' - 'A',
+	.no_banks = 2,
+};
+
+static const struct sunxi_gpio_soc_data soc_data_l_3 = {
+	.start = 'L' - 'A',
+	.no_banks = 3,
+};
+
+#define ID(_compat_, _soc_data_) \
+	{ .compatible = _compat_, .data = (ulong)&soc_data_##_soc_data_ }
+
 static const struct udevice_id sunxi_gpio_ids[] = {
-	{ .compatible = "allwinner,sun4i-a10-pinctrl" },
-	{ .compatible = "allwinner,sun5i-a10s-pinctrl" },
-	{ .compatible = "allwinner,sun5i-a13-pinctrl" },
-	{ .compatible = "allwinner,sun6i-a31-pinctrl" },
-	{ .compatible = "allwinner,sun6i-a31s-pinctrl" },
-	{ .compatible = "allwinner,sun7i-a20-pinctrl" },
-	{ .compatible = "allwinner,sun8i-a23-pinctrl" },
-	{ .compatible = "allwinner,sun8i-a33-pinctrl" },
-	{ .compatible = "allwinner,sun8i-a83t-pinctrl", },
-	{ .compatible = "allwinner,sun8i-h3-pinctrl" },
-	{ .compatible = "allwinner,sun9i-a80-pinctrl" },
-	{ .compatible = "allwinner,sun6i-a31-r-pinctrl" },
-	{ .compatible = "allwinner,sun8i-a23-r-pinctrl" },
-	{ .compatible = "allwinner,sun8i-a83t-r-pinctrl" },
-	{ .compatible = "allwinner,sun8i-h3-r-pinctrl", },
-	{ .compatible = "allwinner,sun9i-a80-r-pinctrl", },
+	ID("allwinner,sun4i-a10-pinctrl",	a_all),
+	ID("allwinner,sun5i-a10s-pinctrl",	a_all),
+	ID("allwinner,sun5i-a13-pinctrl",	a_all),
+	ID("allwinner,sun6i-a31-pinctrl",	a_all),
+	ID("allwinner,sun6i-a31s-pinctrl",	a_all),
+	ID("allwinner,sun7i-a20-pinctrl",	a_all),
+	ID("allwinner,sun8i-a23-pinctrl",	a_all),
+	ID("allwinner,sun8i-a33-pinctrl",	a_all),
+	ID("allwinner,sun8i-a83t-pinctrl",	a_all),
+	ID("allwinner,sun8i-h3-pinctrl",	a_all),
+	ID("allwinner,sun9i-a80-pinctrl",	a_all),
+	ID("allwinner,sun6i-a31-r-pinctrl",	l_2),
+	ID("allwinner,sun8i-a23-r-pinctrl",	l_1),
+	ID("allwinner,sun8i-a83t-r-pinctrl",	l_1),
+	ID("allwinner,sun8i-h3-r-pinctrl",	l_1),
+	ID("allwinner,sun9i-a80-r-pinctrl",	l_3),
 	{ }
 };
 
diff --git a/drivers/gpio/tegra186_gpio.c b/drivers/gpio/tegra186_gpio.c
new file mode 100644
index 0000000..1c68151
--- /dev/null
+++ b/drivers/gpio/tegra186_gpio.c
@@ -0,0 +1,288 @@
+/*
+ * Copyright (c) 2010-2016, NVIDIA CORPORATION.
+ * (based on tegra_gpio.c)
+ *
+ * SPDX-License-Identifier: GPL-2.0
+ */
+
+#include <common.h>
+#include <dm.h>
+#include <malloc.h>
+#include <errno.h>
+#include <fdtdec.h>
+#include <asm/io.h>
+#include <asm/bitops.h>
+#include <asm/gpio.h>
+#include <dm/device-internal.h>
+#include <dt-bindings/gpio/gpio.h>
+#include "tegra186_gpio_priv.h"
+
+DECLARE_GLOBAL_DATA_PTR;
+
+struct tegra186_gpio_port_data {
+	const char *name;
+	uint32_t offset;
+};
+
+struct tegra186_gpio_ctlr_data {
+	const struct tegra186_gpio_port_data *ports;
+	uint32_t port_count;
+};
+
+struct tegra186_gpio_platdata {
+	const char *name;
+	uint32_t *regs;
+};
+
+static uint32_t *tegra186_gpio_reg(struct udevice *dev, uint32_t reg,
+				   uint32_t gpio)
+{
+	struct tegra186_gpio_platdata *plat = dev->platdata;
+	uint32_t index = (reg + (gpio * TEGRA186_GPIO_PER_GPIO_STRIDE)) / 4;
+
+	return &(plat->regs[index]);
+}
+
+static int tegra186_gpio_set_out(struct udevice *dev, unsigned offset,
+				 bool output)
+{
+	uint32_t *reg;
+	uint32_t rval;
+
+	reg = tegra186_gpio_reg(dev, TEGRA186_GPIO_OUTPUT_CONTROL, offset);
+	rval = readl(reg);
+	if (output)
+		rval &= ~TEGRA186_GPIO_OUTPUT_CONTROL_FLOATED;
+	else
+		rval |= TEGRA186_GPIO_OUTPUT_CONTROL_FLOATED;
+	writel(rval, reg);
+
+	reg = tegra186_gpio_reg(dev, TEGRA186_GPIO_ENABLE_CONFIG, offset);
+	rval = readl(reg);
+	if (output)
+		rval |= TEGRA186_GPIO_ENABLE_CONFIG_OUT;
+	else
+		rval &= ~TEGRA186_GPIO_ENABLE_CONFIG_OUT;
+	rval |= TEGRA186_GPIO_ENABLE_CONFIG_ENABLE;
+	writel(rval, reg);
+
+	return 0;
+}
+
+static int tegra186_gpio_set_val(struct udevice *dev, unsigned offset, bool val)
+{
+	uint32_t *reg;
+	uint32_t rval;
+
+	reg = tegra186_gpio_reg(dev, TEGRA186_GPIO_OUTPUT_VALUE, offset);
+	rval = readl(reg);
+	if (val)
+		rval |= TEGRA186_GPIO_OUTPUT_VALUE_HIGH;
+	else
+		rval &= ~TEGRA186_GPIO_OUTPUT_VALUE_HIGH;
+	writel(rval, reg);
+
+	return 0;
+}
+
+static int tegra186_gpio_direction_input(struct udevice *dev, unsigned offset)
+{
+	return tegra186_gpio_set_out(dev, offset, false);
+}
+
+static int tegra186_gpio_direction_output(struct udevice *dev, unsigned offset,
+				       int value)
+{
+	int ret;
+
+	ret = tegra186_gpio_set_val(dev, offset, value != 0);
+	if (ret)
+		return ret;
+	return tegra186_gpio_set_out(dev, offset, true);
+}
+
+static int tegra186_gpio_get_value(struct udevice *dev, unsigned offset)
+{
+	uint32_t *reg;
+	uint32_t rval;
+
+	reg = tegra186_gpio_reg(dev, TEGRA186_GPIO_ENABLE_CONFIG, offset);
+	rval = readl(reg);
+
+	if (rval & TEGRA186_GPIO_ENABLE_CONFIG_OUT)
+		reg = tegra186_gpio_reg(dev, TEGRA186_GPIO_OUTPUT_VALUE,
+					offset);
+	else
+		reg = tegra186_gpio_reg(dev, TEGRA186_GPIO_INPUT, offset);
+
+	rval = readl(reg);
+	return !!rval;
+}
+
+static int tegra186_gpio_set_value(struct udevice *dev, unsigned offset,
+				   int value)
+{
+	return tegra186_gpio_set_val(dev, offset, value != 0);
+}
+
+static int tegra186_gpio_get_function(struct udevice *dev, unsigned offset)
+{
+	uint32_t *reg;
+	uint32_t rval;
+
+	reg = tegra186_gpio_reg(dev, TEGRA186_GPIO_ENABLE_CONFIG, offset);
+	rval = readl(reg);
+	if (rval & TEGRA186_GPIO_ENABLE_CONFIG_OUT)
+		return GPIOF_OUTPUT;
+	else
+		return GPIOF_INPUT;
+}
+
+static int tegra186_gpio_xlate(struct udevice *dev, struct gpio_desc *desc,
+			    struct fdtdec_phandle_args *args)
+{
+	int gpio, port, ret;
+
+	gpio = args->args[0];
+	port = gpio / TEGRA186_GPIO_PER_GPIO_COUNT;
+	ret = device_get_child(dev, port, &desc->dev);
+	if (ret)
+		return ret;
+	desc->offset = gpio % TEGRA186_GPIO_PER_GPIO_COUNT;
+	desc->flags = args->args[1] & GPIO_ACTIVE_LOW ? GPIOD_ACTIVE_LOW : 0;
+
+	return 0;
+}
+
+static const struct dm_gpio_ops tegra186_gpio_ops = {
+	.direction_input	= tegra186_gpio_direction_input,
+	.direction_output	= tegra186_gpio_direction_output,
+	.get_value		= tegra186_gpio_get_value,
+	.set_value		= tegra186_gpio_set_value,
+	.get_function		= tegra186_gpio_get_function,
+	.xlate			= tegra186_gpio_xlate,
+};
+
+/**
+ * We have a top-level GPIO device with no actual GPIOs. It has a child device
+ * for each port within the controller.
+ */
+static int tegra186_gpio_bind(struct udevice *parent)
+{
+	struct tegra186_gpio_platdata *parent_plat = parent->platdata;
+	struct tegra186_gpio_ctlr_data *ctlr_data =
+		(struct tegra186_gpio_ctlr_data *)dev_get_driver_data(parent);
+	uint32_t *regs;
+	int port, ret;
+
+	/* If this is a child device, there is nothing to do here */
+	if (parent_plat)
+		return 0;
+
+	regs = (uint32_t *)dev_get_addr_name(parent, "gpio");
+	if (regs == (uint32_t *)FDT_ADDR_T_NONE)
+		return -ENODEV;
+
+	for (port = 0; port < ctlr_data->port_count; port++) {
+		struct tegra186_gpio_platdata *plat;
+		struct udevice *dev;
+
+		plat = calloc(1, sizeof(*plat));
+		if (!plat)
+			return -ENOMEM;
+		plat->name = ctlr_data->ports[port].name;
+		plat->regs = &(regs[ctlr_data->ports[port].offset / 4]);
+
+		ret = device_bind(parent, parent->driver, plat->name, plat,
+				  -1, &dev);
+		if (ret)
+			return ret;
+		dev->of_offset = parent->of_offset;
+	}
+
+	return 0;
+}
+
+static int tegra186_gpio_probe(struct udevice *dev)
+{
+	struct tegra186_gpio_platdata *plat = dev->platdata;
+	struct gpio_dev_priv *uc_priv = dev_get_uclass_priv(dev);
+
+	/* Only child devices have ports */
+	if (!plat)
+		return 0;
+
+	uc_priv->gpio_count = TEGRA186_GPIO_PER_GPIO_COUNT;
+	uc_priv->bank_name = plat->name;
+
+	return 0;
+}
+
+static const struct tegra186_gpio_port_data tegra186_gpio_main_ports[] = {
+	{"A",  0x2000},
+	{"B",  0x3000},
+	{"C",  0x3200},
+	{"D",  0x3400},
+	{"E",  0x2200},
+	{"F",  0x2400},
+	{"G",  0x4200},
+	{"H",  0x1000},
+	{"I",  0x0800},
+	{"J",  0x5000},
+	{"K",  0x5200},
+	{"L",  0x1200},
+	{"M",  0x5600},
+	{"N",  0x0000},
+	{"O",  0x0200},
+	{"P",  0x4000},
+	{"Q",  0x0400},
+	{"R",  0x0a00},
+	{"T",  0x0600},
+	{"X",  0x1400},
+	{"Y",  0x1600},
+	{"BB", 0x2600},
+	{"CC", 0x5400},
+};
+
+static const struct tegra186_gpio_ctlr_data tegra186_gpio_main_data = {
+	.ports = tegra186_gpio_main_ports,
+	.port_count = ARRAY_SIZE(tegra186_gpio_main_ports),
+};
+
+static const struct tegra186_gpio_port_data tegra186_gpio_aon_ports[] = {
+	{"S",  0x0200},
+	{"U",  0x0400},
+	{"V",  0x0800},
+	{"W",  0x0a00},
+	{"Z",  0x0e00},
+	{"AA", 0x0c00},
+	{"EE", 0x0600},
+	{"FF", 0x0000},
+};
+
+static const struct tegra186_gpio_ctlr_data tegra186_gpio_aon_data = {
+	.ports = tegra186_gpio_aon_ports,
+	.port_count = ARRAY_SIZE(tegra186_gpio_aon_ports),
+};
+
+static const struct udevice_id tegra186_gpio_ids[] = {
+	{
+		.compatible = "nvidia,tegra186-gpio",
+		.data = (ulong)&tegra186_gpio_main_data,
+	},
+	{
+		.compatible = "nvidia,tegra186-gpio-aon",
+		.data = (ulong)&tegra186_gpio_aon_data,
+	},
+	{ }
+};
+
+U_BOOT_DRIVER(tegra186_gpio) = {
+	.name = "tegra186_gpio",
+	.id = UCLASS_GPIO,
+	.of_match = tegra186_gpio_ids,
+	.bind = tegra186_gpio_bind,
+	.probe = tegra186_gpio_probe,
+	.ops = &tegra186_gpio_ops,
+	.flags = DM_FLAG_PRE_RELOC,
+};
diff --git a/drivers/gpio/tegra186_gpio_priv.h b/drivers/gpio/tegra186_gpio_priv.h
new file mode 100644
index 0000000..9e85a434
--- /dev/null
+++ b/drivers/gpio/tegra186_gpio_priv.h
@@ -0,0 +1,61 @@
+/*
+ * Copyright (c) 2016, NVIDIA CORPORATION.
+ *
+ * SPDX-License-Identifier: GPL-2.0
+ */
+
+#ifndef _TEGRA186_GPIO_PRIV_H_
+#define _TEGRA186_GPIO_PRIV_H_
+
+/*
+ * For each GPIO, there are a set of registers than affect it, all packed
+ * back-to-back.
+ */
+#define TEGRA186_GPIO_ENABLE_CONFIG				0x00
+#define TEGRA186_GPIO_ENABLE_CONFIG_ENABLE			BIT(0)
+#define TEGRA186_GPIO_ENABLE_CONFIG_OUT				BIT(1)
+#define TEGRA186_GPIO_ENABLE_CONFIG_TRIGGER_TYPE_SHIFT		2
+#define TEGRA186_GPIO_ENABLE_CONFIG_TRIGGER_TYPE_MASK		3
+#define TEGRA186_GPIO_ENABLE_CONFIG_TRIGGER_TYPE_NONE		0
+#define TEGRA186_GPIO_ENABLE_CONFIG_TRIGGER_TYPE_LEVEL		1
+#define TEGRA186_GPIO_ENABLE_CONFIG_TRIGGER_TYPE_SINGLE_EDGE	2
+#define TEGRA186_GPIO_ENABLE_CONFIG_TRIGGER_TYPE_DOUBLE_EDGE	3
+#define TEGRA186_GPIO_ENABLE_CONFIG_TRIGGER_LEVEL_HIGH_RISING	BIT(4)
+#define TEGRA186_GPIO_ENABLE_CONFIG_DEBOUNCE_ENABLE		BIT(5)
+#define TEGRA186_GPIO_ENABLE_CONFIG_INTERRUPT_ENABLE		BIT(6)
+#define TEGRA186_GPIO_ENABLE_CONFIG_TIMESTAMPING_ENABLE		BIT(7)
+
+#define TEGRA186_GPIO_DEBOUNCE_THRESHOLD			0x04
+
+#define TEGRA186_GPIO_INPUT					0x08
+
+#define TEGRA186_GPIO_OUTPUT_CONTROL				0x0c
+#define TEGRA186_GPIO_OUTPUT_CONTROL_FLOATED			BIT(0)
+
+#define TEGRA186_GPIO_OUTPUT_VALUE				0x10
+#define TEGRA186_GPIO_OUTPUT_VALUE_HIGH				1
+
+#define TEGRA186_GPIO_INTERRUPT_CLEAR				0x14
+
+/*
+ * 8 GPIOs are packed into a port. Their registers appear back-to-back in the
+ * port's address space.
+ */
+#define TEGRA186_GPIO_PER_GPIO_STRIDE				0x20
+#define TEGRA186_GPIO_PER_GPIO_COUNT				8
+
+/*
+ * Per-port registers are packed immediately following all of a port's
+ * per-GPIO registers.
+ */
+#define TEGRA186_GPIO_INTERRUPT_STATUS_G			0x100
+#define TEGRA186_GPIO_INTERRUPT_STATUS_G_STRIDE			4
+#define TEGRA186_GPIO_INTERRUPT_STATUS_G_COUNT			8
+
+/*
+ * The registers for multiple ports are packed together back-to-back to form
+ * the overall controller.
+ */
+#define TEGRA186_GPIO_PER_PORT_STRIDE				0x200
+
+#endif
diff --git a/drivers/gpio/zynq_gpio.c b/drivers/gpio/zynq_gpio.c
index 3a995f6..4ab2356 100644
--- a/drivers/gpio/zynq_gpio.c
+++ b/drivers/gpio/zynq_gpio.c
@@ -299,11 +299,33 @@ static int zynq_gpio_direction_output(struct udevice *dev, unsigned gpio,
 	return 0;
 }
 
+static int zynq_gpio_get_function(struct udevice *dev, unsigned offset)
+{
+	u32 reg;
+	unsigned int bank_num, bank_pin_num;
+	struct zynq_gpio_privdata *priv = dev_get_priv(dev);
+
+	if (check_gpio(offset, dev) < 0)
+		return -1;
+
+	zynq_gpio_get_bank_pin(offset, &bank_num, &bank_pin_num, dev);
+
+	/* set the GPIO pin as output */
+	reg = readl(priv->base + ZYNQ_GPIO_DIRM_OFFSET(bank_num));
+	reg &= BIT(bank_pin_num);
+	if (reg)
+		return GPIOF_OUTPUT;
+	else
+		return GPIOF_INPUT;
+}
+
 static const struct dm_gpio_ops gpio_zynq_ops = {
 	.direction_input	= zynq_gpio_direction_input,
 	.direction_output	= zynq_gpio_direction_output,
 	.get_value		= zynq_gpio_get_value,
 	.set_value		= zynq_gpio_set_value,
+	.get_function		= zynq_gpio_get_function,
+
 };
 
 static const struct udevice_id zynq_gpio_ids[] = {
diff --git a/drivers/mailbox/Kconfig b/drivers/mailbox/Kconfig
new file mode 100644
index 0000000..9087512
--- /dev/null
+++ b/drivers/mailbox/Kconfig
@@ -0,0 +1,20 @@
+menu "Mailbox Controller Support"
+
+config DM_MAILBOX
+	bool "Enable mailbox controllers using Driver Model"
+	depends on DM && OF_CONTROL
+	help
+	  Enable support for the mailbox driver class. Mailboxes provide the
+	  ability to transfer small messages and/or notifications from one
+	  CPU to another CPU, or sometimes to dedicated HW modules. They form
+	  the basis of a variety of inter-process/inter-CPU communication
+	  protocols.
+
+config SANDBOX_MBOX
+	bool "Enable the sandbox mailbox test driver"
+	depends on DM_MAILBOX && SANDBOX
+	help
+	  Enable support for a test mailbox implementation, which simply echos
+	  back a modified version of any message that is sent.
+
+endmenu
diff --git a/drivers/mailbox/Makefile b/drivers/mailbox/Makefile
new file mode 100644
index 0000000..bbae4de
--- /dev/null
+++ b/drivers/mailbox/Makefile
@@ -0,0 +1,7 @@
+# Copyright (c) 2016, NVIDIA CORPORATION.
+#
+# SPDX-License-Identifier: GPL-2.0
+
+obj-$(CONFIG_DM_MAILBOX) += mailbox-uclass.o
+obj-$(CONFIG_SANDBOX_MBOX) += sandbox-mbox.o
+obj-$(CONFIG_SANDBOX_MBOX) += sandbox-mbox-test.o
diff --git a/drivers/mailbox/mailbox-uclass.c b/drivers/mailbox/mailbox-uclass.c
new file mode 100644
index 0000000..73fa328
--- /dev/null
+++ b/drivers/mailbox/mailbox-uclass.c
@@ -0,0 +1,145 @@
+/*
+ * Copyright (c) 2016, NVIDIA CORPORATION.
+ *
+ * SPDX-License-Identifier: GPL-2.0
+ */
+
+#include <common.h>
+#include <dm.h>
+#include <fdtdec.h>
+#include <mailbox_client.h>
+#include <mailbox_uclass.h>
+
+DECLARE_GLOBAL_DATA_PTR;
+
+static inline struct mbox_ops *mbox_dev_ops(struct udevice *dev)
+{
+	return (struct mbox_ops *)dev->driver->ops;
+}
+
+static int mbox_of_xlate_default(struct mbox_chan *chan,
+				 struct fdtdec_phandle_args *args)
+{
+	debug("%s(chan=%p)\n", __func__, chan);
+
+	if (args->args_count != 1) {
+		debug("Invaild args_count: %d\n", args->args_count);
+		return -EINVAL;
+	}
+
+	chan->id = args->args[0];
+
+	return 0;
+}
+
+int mbox_get_by_index(struct udevice *dev, int index, struct mbox_chan *chan)
+{
+	struct fdtdec_phandle_args args;
+	int ret;
+	struct udevice *dev_mbox;
+	struct mbox_ops *ops;
+
+	debug("%s(dev=%p, index=%d, chan=%p)\n", __func__, dev, index, chan);
+
+	ret = fdtdec_parse_phandle_with_args(gd->fdt_blob, dev->of_offset,
+					     "mboxes", "#mbox-cells", 0,
+					     index, &args);
+	if (ret) {
+		debug("%s: fdtdec_parse_phandle_with_args failed: %d\n",
+		      __func__, ret);
+		return ret;
+	}
+
+	ret = uclass_get_device_by_of_offset(UCLASS_MAILBOX, args.node,
+					     &dev_mbox);
+	if (ret) {
+		debug("%s: uclass_get_device_by_of_offset failed: %d\n",
+		      __func__, ret);
+		return ret;
+	}
+	ops = mbox_dev_ops(dev_mbox);
+
+	chan->dev = dev_mbox;
+	if (ops->of_xlate)
+		ret = ops->of_xlate(chan, &args);
+	else
+		ret = mbox_of_xlate_default(chan, &args);
+	if (ret) {
+		debug("of_xlate() failed: %d\n", ret);
+		return ret;
+	}
+
+	ret = ops->request(chan);
+	if (ret) {
+		debug("ops->request() failed: %d\n", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+int mbox_get_by_name(struct udevice *dev, const char *name,
+		     struct mbox_chan *chan)
+{
+	int index;
+
+	debug("%s(dev=%p, name=%s, chan=%p)\n", __func__, dev, name, chan);
+
+	index = fdt_find_string(gd->fdt_blob, dev->of_offset, "mbox-names",
+				name);
+	if (index < 0) {
+		debug("fdt_find_string() failed: %d\n", index);
+		return index;
+	}
+
+	return mbox_get_by_index(dev, index, chan);
+}
+
+int mbox_free(struct mbox_chan *chan)
+{
+	struct mbox_ops *ops = mbox_dev_ops(chan->dev);
+
+	debug("%s(chan=%p)\n", __func__, chan);
+
+	return ops->free(chan);
+}
+
+int mbox_send(struct mbox_chan *chan, const void *data)
+{
+	struct mbox_ops *ops = mbox_dev_ops(chan->dev);
+
+	debug("%s(chan=%p, data=%p)\n", __func__, chan, data);
+
+	return ops->send(chan, data);
+}
+
+int mbox_recv(struct mbox_chan *chan, void *data, ulong timeout_us)
+{
+	struct mbox_ops *ops = mbox_dev_ops(chan->dev);
+	ulong start_time;
+	int ret;
+
+	debug("%s(chan=%p, data=%p, timeout_us=%ld)\n", __func__, chan, data,
+	      timeout_us);
+
+	start_time = timer_get_us();
+	/*
+	 * Account for partial us ticks, but if timeout_us is 0, ensure we
+	 * still don't wait at all.
+	 */
+	if (timeout_us)
+		timeout_us++;
+
+	for (;;) {
+		ret = ops->recv(chan, data);
+		if (ret != -ENODATA)
+			return ret;
+		if ((timer_get_us() - start_time) >= timeout_us)
+			return -ETIMEDOUT;
+	}
+}
+
+UCLASS_DRIVER(mailbox) = {
+	.id		= UCLASS_MAILBOX,
+	.name		= "mailbox",
+};
diff --git a/drivers/mailbox/sandbox-mbox-test.c b/drivers/mailbox/sandbox-mbox-test.c
new file mode 100644
index 0000000..02d161a
--- /dev/null
+++ b/drivers/mailbox/sandbox-mbox-test.c
@@ -0,0 +1,54 @@
+/*
+ * Copyright (c) 2016, NVIDIA CORPORATION.
+ *
+ * SPDX-License-Identifier: GPL-2.0
+ */
+
+#include <common.h>
+#include <dm.h>
+#include <mailbox_client.h>
+#include <asm/io.h>
+
+struct sandbox_mbox_test {
+	struct mbox_chan chan;
+};
+
+int sandbox_mbox_test_get(struct udevice *dev)
+{
+	struct sandbox_mbox_test *sbmt = dev_get_priv(dev);
+
+	return mbox_get_by_name(dev, "test", &sbmt->chan);
+}
+
+int sandbox_mbox_test_send(struct udevice *dev, uint32_t msg)
+{
+	struct sandbox_mbox_test *sbmt = dev_get_priv(dev);
+
+	return mbox_send(&sbmt->chan, &msg);
+}
+
+int sandbox_mbox_test_recv(struct udevice *dev, uint32_t *msg)
+{
+	struct sandbox_mbox_test *sbmt = dev_get_priv(dev);
+
+	return mbox_recv(&sbmt->chan, msg, 100);
+}
+
+int sandbox_mbox_test_free(struct udevice *dev)
+{
+	struct sandbox_mbox_test *sbmt = dev_get_priv(dev);
+
+	return mbox_free(&sbmt->chan);
+}
+
+static const struct udevice_id sandbox_mbox_test_ids[] = {
+	{ .compatible = "sandbox,mbox-test" },
+	{ }
+};
+
+U_BOOT_DRIVER(sandbox_mbox_test) = {
+	.name = "sandbox_mbox_test",
+	.id = UCLASS_MISC,
+	.of_match = sandbox_mbox_test_ids,
+	.priv_auto_alloc_size = sizeof(struct sandbox_mbox_test),
+};
diff --git a/drivers/mailbox/sandbox-mbox.c b/drivers/mailbox/sandbox-mbox.c
new file mode 100644
index 0000000..1b7ac23
--- /dev/null
+++ b/drivers/mailbox/sandbox-mbox.c
@@ -0,0 +1,104 @@
+/*
+ * Copyright (c) 2016, NVIDIA CORPORATION.
+ *
+ * SPDX-License-Identifier: GPL-2.0
+ */
+
+#include <common.h>
+#include <dm.h>
+#include <mailbox_uclass.h>
+#include <asm/io.h>
+#include <asm/mbox.h>
+
+#define SANDBOX_MBOX_CHANNELS 2
+
+struct sandbox_mbox_chan {
+	bool rx_msg_valid;
+	uint32_t rx_msg;
+};
+
+struct sandbox_mbox {
+	struct sandbox_mbox_chan chans[SANDBOX_MBOX_CHANNELS];
+};
+
+static int sandbox_mbox_request(struct mbox_chan *chan)
+{
+	debug("%s(chan=%p)\n", __func__, chan);
+
+	if (chan->id >= SANDBOX_MBOX_CHANNELS)
+		return -EINVAL;
+
+	return 0;
+}
+
+static int sandbox_mbox_free(struct mbox_chan *chan)
+{
+	debug("%s(chan=%p)\n", __func__, chan);
+
+	return 0;
+}
+
+static int sandbox_mbox_send(struct mbox_chan *chan, const void *data)
+{
+	struct sandbox_mbox *sbm = dev_get_priv(chan->dev);
+	const uint32_t *pmsg = data;
+
+	debug("%s(chan=%p, data=%p)\n", __func__, chan, data);
+
+	sbm->chans[chan->id].rx_msg = *pmsg ^ SANDBOX_MBOX_PING_XOR;
+	sbm->chans[chan->id].rx_msg_valid = true;
+
+	return 0;
+}
+
+static int sandbox_mbox_recv(struct mbox_chan *chan, void *data)
+{
+	struct sandbox_mbox *sbm = dev_get_priv(chan->dev);
+	uint32_t *pmsg = data;
+
+	debug("%s(chan=%p, data=%p)\n", __func__, chan, data);
+
+	if (!sbm->chans[chan->id].rx_msg_valid)
+		return -ENODATA;
+
+	*pmsg = sbm->chans[chan->id].rx_msg;
+	sbm->chans[chan->id].rx_msg_valid = false;
+
+	return 0;
+}
+
+static int sandbox_mbox_bind(struct udevice *dev)
+{
+	debug("%s(dev=%p)\n", __func__, dev);
+
+	return 0;
+}
+
+static int sandbox_mbox_probe(struct udevice *dev)
+{
+	debug("%s(dev=%p)\n", __func__, dev);
+
+	return 0;
+}
+
+static const struct udevice_id sandbox_mbox_ids[] = {
+	{ .compatible = "sandbox,mbox" },
+	{ }
+};
+
+struct mbox_ops sandbox_mbox_mbox_ops = {
+	.request = sandbox_mbox_request,
+	.free = sandbox_mbox_free,
+	.send = sandbox_mbox_send,
+	.recv = sandbox_mbox_recv,
+};
+
+U_BOOT_DRIVER(sandbox_mbox) = {
+	.name = "sandbox_mbox",
+	.id = UCLASS_MAILBOX,
+	.of_match = sandbox_mbox_ids,
+	.bind = sandbox_mbox_bind,
+	.probe = sandbox_mbox_probe,
+	.priv_auto_alloc_size = sizeof(struct sandbox_mbox),
+	.ops = &sandbox_mbox_mbox_ops,
+};
diff --git a/drivers/misc/Kconfig b/drivers/misc/Kconfig
index af8667f..2373037 100644
--- a/drivers/misc/Kconfig
+++ b/drivers/misc/Kconfig
@@ -121,13 +121,13 @@ config PCA9551_I2C_ADDR
 	help
 	  The I2C address of the PCA9551 LED controller.
 
-config RESET
-	bool "Enable support for reset drivers"
+config SYSRESET
+	bool "Enable support for system reset drivers"
 	depends on DM
 	help
-	  Enable reset drivers which can be used to reset the CPU or board.
-	  Each driver can provide a reset method which will be called to
-	  effect a reset. The uclass will try all available drivers when
+	  Enable system reset drivers which can be used to reset the CPU or
+	  board. Each driver can provide a reset method which will be called
+	  to effect a reset. The uclass will try all available drivers when
 	  reset_walk() is called.
 
 config WINBOND_W83627
@@ -138,4 +138,10 @@ config WINBOND_W83627
 	  legacy UART or other devices in the Winbond Super IO chips
 	  on X86 platforms.
 
+config QFW
+	bool
+	help
+	  Hidden option to enable QEMU fw_cfg interface. This will be selected by
+	  either CONFIG_CMD_QFW or CONFIG_GENERATE_ACPI_TABLE.
+
 endmenu
diff --git a/drivers/misc/Makefile b/drivers/misc/Makefile
index 5969d34..066639b 100644
--- a/drivers/misc/Makefile
+++ b/drivers/misc/Makefile
@@ -27,7 +27,7 @@ obj-$(CONFIG_MXS_OCOTP) += mxs_ocotp.o
 obj-$(CONFIG_NS87308) += ns87308.o
 obj-$(CONFIG_PDSP188x) += pdsp188x.o
 obj-$(CONFIG_$(SPL_)PWRSEQ) += pwrseq-uclass.o
-obj-$(CONFIG_SANDBOX) += reset_sandbox.o
+obj-$(CONFIG_SANDBOX) += sysreset_sandbox.o
 ifdef CONFIG_DM_I2C
 obj-$(CONFIG_SANDBOX) += i2c_eeprom_emul.o
 endif
@@ -40,6 +40,7 @@ obj-$(CONFIG_TWL4030_LED) += twl4030_led.o
 obj-$(CONFIG_FSL_IFC) += fsl_ifc.o
 obj-$(CONFIG_FSL_SEC_MON) += fsl_sec_mon.o
 obj-$(CONFIG_PCA9551_LED) += pca9551_led.o
-obj-$(CONFIG_RESET) += reset-uclass.o
+obj-$(CONFIG_SYSRESET) += sysreset-uclass.o
 obj-$(CONFIG_FSL_DEVICE_DISABLE) += fsl_devdis.o
 obj-$(CONFIG_WINBOND_W83627) += winbond_w83627.o
+obj-$(CONFIG_QFW) += qfw.o
diff --git a/drivers/misc/qfw.c b/drivers/misc/qfw.c
new file mode 100644
index 0000000..d43d1d3
--- /dev/null
+++ b/drivers/misc/qfw.c
@@ -0,0 +1,386 @@
+/*
+ * (C) Copyright 2015 Miao Yan <yanmiaobest@gmail.com>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <command.h>
+#include <errno.h>
+#include <malloc.h>
+#include <qfw.h>
+#include <asm/io.h>
+#ifdef CONFIG_GENERATE_ACPI_TABLE
+#include <asm/tables.h>
+#endif
+#include <linux/list.h>
+
+static bool fwcfg_present;
+static bool fwcfg_dma_present;
+static struct fw_cfg_arch_ops *fwcfg_arch_ops;
+
+static LIST_HEAD(fw_list);
+
+#ifdef CONFIG_GENERATE_ACPI_TABLE
+/*
+ * This function allocates memory for ACPI tables
+ *
+ * @entry : BIOS linker command entry which tells where to allocate memory
+ *          (either high memory or low memory)
+ * @addr  : The address that should be used for low memory allcation. If the
+ *          memory allocation request is 'ZONE_HIGH' then this parameter will
+ *          be ignored.
+ * @return: 0 on success, or negative value on failure
+ */
+static int bios_linker_allocate(struct bios_linker_entry *entry, u32 *addr)
+{
+	uint32_t size, align;
+	struct fw_file *file;
+	unsigned long aligned_addr;
+
+	align = le32_to_cpu(entry->alloc.align);
+	/* align must be power of 2 */
+	if (align & (align - 1)) {
+		printf("error: wrong alignment %u\n", align);
+		return -EINVAL;
+	}
+
+	file = qemu_fwcfg_find_file(entry->alloc.file);
+	if (!file) {
+		printf("error: can't find file %s\n", entry->alloc.file);
+		return -ENOENT;
+	}
+
+	size = be32_to_cpu(file->cfg.size);
+
+	/*
+	 * ZONE_HIGH means we need to allocate from high memory, since
+	 * malloc space is already at the end of RAM, so we directly use it.
+	 * If allocation zone is ZONE_FSEG, then we use the 'addr' passed
+	 * in which is low memory
+	 */
+	if (entry->alloc.zone == BIOS_LINKER_LOADER_ALLOC_ZONE_HIGH) {
+		aligned_addr = (unsigned long)memalign(align, size);
+		if (!aligned_addr) {
+			printf("error: allocating resource\n");
+			return -ENOMEM;
+		}
+	} else if (entry->alloc.zone == BIOS_LINKER_LOADER_ALLOC_ZONE_FSEG) {
+		aligned_addr = ALIGN(*addr, align);
+	} else {
+		printf("error: invalid allocation zone\n");
+		return -EINVAL;
+	}
+
+	debug("bios_linker_allocate: allocate file %s, size %u, zone %d, align %u, addr 0x%lx\n",
+	      file->cfg.name, size, entry->alloc.zone, align, aligned_addr);
+
+	qemu_fwcfg_read_entry(be16_to_cpu(file->cfg.select),
+			      size, (void *)aligned_addr);
+	file->addr = aligned_addr;
+
+	/* adjust address for low memory allocation */
+	if (entry->alloc.zone == BIOS_LINKER_LOADER_ALLOC_ZONE_FSEG)
+		*addr = (aligned_addr + size);
+
+	return 0;
+}
+
+/*
+ * This function patches ACPI tables previously loaded
+ * by bios_linker_allocate()
+ *
+ * @entry : BIOS linker command entry which tells how to patch
+ *          ACPI tables
+ * @return: 0 on success, or negative value on failure
+ */
+static int bios_linker_add_pointer(struct bios_linker_entry *entry)
+{
+	struct fw_file *dest, *src;
+	uint32_t offset = le32_to_cpu(entry->pointer.offset);
+	uint64_t pointer = 0;
+
+	dest = qemu_fwcfg_find_file(entry->pointer.dest_file);
+	if (!dest || !dest->addr)
+		return -ENOENT;
+	src = qemu_fwcfg_find_file(entry->pointer.src_file);
+	if (!src || !src->addr)
+		return -ENOENT;
+
+	debug("bios_linker_add_pointer: dest->addr 0x%lx, src->addr 0x%lx, offset 0x%x size %u, 0x%llx\n",
+	      dest->addr, src->addr, offset, entry->pointer.size, pointer);
+
+	memcpy(&pointer, (char *)dest->addr + offset, entry->pointer.size);
+	pointer	= le64_to_cpu(pointer);
+	pointer += (unsigned long)src->addr;
+	pointer	= cpu_to_le64(pointer);
+	memcpy((char *)dest->addr + offset, &pointer, entry->pointer.size);
+
+	return 0;
+}
+
+/*
+ * This function updates checksum fields of ACPI tables previously loaded
+ * by bios_linker_allocate()
+ *
+ * @entry : BIOS linker command entry which tells where to update ACPI table
+ *          checksums
+ * @return: 0 on success, or negative value on failure
+ */
+static int bios_linker_add_checksum(struct bios_linker_entry *entry)
+{
+	struct fw_file *file;
+	uint8_t *data, cksum = 0;
+	uint8_t *cksum_start;
+
+	file = qemu_fwcfg_find_file(entry->cksum.file);
+	if (!file || !file->addr)
+		return -ENOENT;
+
+	data = (uint8_t *)(file->addr + le32_to_cpu(entry->cksum.offset));
+	cksum_start = (uint8_t *)(file->addr + le32_to_cpu(entry->cksum.start));
+	cksum = table_compute_checksum(cksum_start,
+				       le32_to_cpu(entry->cksum.length));
+	*data = cksum;
+
+	return 0;
+}
+
+/* This function loads and patches ACPI tables provided by QEMU */
+u32 write_acpi_tables(u32 addr)
+{
+	int i, ret = 0;
+	struct fw_file *file;
+	struct bios_linker_entry *table_loader;
+	struct bios_linker_entry *entry;
+	uint32_t size;
+
+	/* make sure fw_list is loaded */
+	ret = qemu_fwcfg_read_firmware_list();
+	if (ret) {
+		printf("error: can't read firmware file list\n");
+		return addr;
+	}
+
+	file = qemu_fwcfg_find_file("etc/table-loader");
+	if (!file) {
+		printf("error: can't find etc/table-loader\n");
+		return addr;
+	}
+
+	size = be32_to_cpu(file->cfg.size);
+	if ((size % sizeof(*entry)) != 0) {
+		printf("error: table-loader maybe corrupted\n");
+		return addr;
+	}
+
+	table_loader = malloc(size);
+	if (!table_loader) {
+		printf("error: no memory for table-loader\n");
+		return addr;
+	}
+
+	qemu_fwcfg_read_entry(be16_to_cpu(file->cfg.select),
+			      size, table_loader);
+
+	for (i = 0; i < (size / sizeof(*entry)); i++) {
+		entry = table_loader + i;
+		switch (le32_to_cpu(entry->command)) {
+		case BIOS_LINKER_LOADER_COMMAND_ALLOCATE:
+			ret = bios_linker_allocate(entry, &addr);
+			if (ret)
+				goto out;
+			break;
+		case BIOS_LINKER_LOADER_COMMAND_ADD_POINTER:
+			ret = bios_linker_add_pointer(entry);
+			if (ret)
+				goto out;
+			break;
+		case BIOS_LINKER_LOADER_COMMAND_ADD_CHECKSUM:
+			ret = bios_linker_add_checksum(entry);
+			if (ret)
+				goto out;
+			break;
+		default:
+			break;
+		}
+	}
+
+out:
+	if (ret) {
+		struct fw_cfg_file_iter iter;
+		for (file = qemu_fwcfg_file_iter_init(&iter);
+		     !qemu_fwcfg_file_iter_end(&iter);
+		     file = qemu_fwcfg_file_iter_next(&iter)) {
+			if (file->addr) {
+				free((void *)file->addr);
+				file->addr = 0;
+			}
+		}
+	}
+
+	free(table_loader);
+	return addr;
+}
+#endif
+
+/* Read configuration item using fw_cfg PIO interface */
+static void qemu_fwcfg_read_entry_pio(uint16_t entry,
+		uint32_t size, void *address)
+{
+	debug("qemu_fwcfg_read_entry_pio: entry 0x%x, size %u address %p\n",
+	      entry, size, address);
+
+	return fwcfg_arch_ops->arch_read_pio(entry, size, address);
+}
+
+/* Read configuration item using fw_cfg DMA interface */
+static void qemu_fwcfg_read_entry_dma(uint16_t entry,
+		uint32_t size, void *address)
+{
+	struct fw_cfg_dma_access dma;
+
+	dma.length = cpu_to_be32(size);
+	dma.address = cpu_to_be64((uintptr_t)address);
+	dma.control = cpu_to_be32(FW_CFG_DMA_READ);
+
+	/*
+	 * writting FW_CFG_INVALID will cause read operation to resume at
+	 * last offset, otherwise read will start at offset 0
+	 */
+	if (entry != FW_CFG_INVALID)
+		dma.control |= cpu_to_be32(FW_CFG_DMA_SELECT | (entry << 16));
+
+	barrier();
+
+	debug("qemu_fwcfg_read_entry_dma: entry 0x%x, size %u address %p, control 0x%x\n",
+	      entry, size, address, be32_to_cpu(dma.control));
+
+	fwcfg_arch_ops->arch_read_dma(&dma);
+}
+
+bool qemu_fwcfg_present(void)
+{
+	return fwcfg_present;
+}
+
+bool qemu_fwcfg_dma_present(void)
+{
+	return fwcfg_dma_present;
+}
+
+void qemu_fwcfg_read_entry(uint16_t entry, uint32_t length, void *address)
+{
+	if (fwcfg_dma_present)
+		qemu_fwcfg_read_entry_dma(entry, length, address);
+	else
+		qemu_fwcfg_read_entry_pio(entry, length, address);
+}
+
+int qemu_fwcfg_online_cpus(void)
+{
+	uint16_t nb_cpus;
+
+	if (!fwcfg_present)
+		return -ENODEV;
+
+	qemu_fwcfg_read_entry(FW_CFG_NB_CPUS, 2, &nb_cpus);
+
+	return le16_to_cpu(nb_cpus);
+}
+
+int qemu_fwcfg_read_firmware_list(void)
+{
+	int i;
+	uint32_t count;
+	struct fw_file *file;
+	struct list_head *entry;
+
+	/* don't read it twice */
+	if (!list_empty(&fw_list))
+		return 0;
+
+	qemu_fwcfg_read_entry(FW_CFG_FILE_DIR, 4, &count);
+	if (!count)
+		return 0;
+
+	count = be32_to_cpu(count);
+	for (i = 0; i < count; i++) {
+		file = malloc(sizeof(*file));
+		if (!file) {
+			printf("error: allocating resource\n");
+			goto err;
+		}
+		qemu_fwcfg_read_entry(FW_CFG_INVALID,
+				      sizeof(struct fw_cfg_file), &file->cfg);
+		file->addr = 0;
+		list_add_tail(&file->list, &fw_list);
+	}
+
+	return 0;
+
+err:
+	list_for_each(entry, &fw_list) {
+		file = list_entry(entry, struct fw_file, list);
+		free(file);
+	}
+
+	return -ENOMEM;
+}
+
+struct fw_file *qemu_fwcfg_find_file(const char *name)
+{
+	struct list_head *entry;
+	struct fw_file *file;
+
+	list_for_each(entry, &fw_list) {
+		file = list_entry(entry, struct fw_file, list);
+		if (!strcmp(file->cfg.name, name))
+			return file;
+	}
+
+	return NULL;
+}
+
+struct fw_file *qemu_fwcfg_file_iter_init(struct fw_cfg_file_iter *iter)
+{
+	iter->entry = fw_list.next;
+	return list_entry((struct list_head *)iter->entry,
+			  struct fw_file, list);
+}
+
+struct fw_file *qemu_fwcfg_file_iter_next(struct fw_cfg_file_iter *iter)
+{
+	iter->entry = ((struct list_head *)iter->entry)->next;
+	return list_entry((struct list_head *)iter->entry,
+			  struct fw_file, list);
+}
+
+bool qemu_fwcfg_file_iter_end(struct fw_cfg_file_iter *iter)
+{
+	return iter->entry == &fw_list;
+}
+
+void qemu_fwcfg_init(struct fw_cfg_arch_ops *ops)
+{
+	uint32_t qemu;
+	uint32_t dma_enabled;
+
+	fwcfg_present = false;
+	fwcfg_dma_present = false;
+	fwcfg_arch_ops = NULL;
+
+	if (!ops || !ops->arch_read_pio || !ops->arch_read_dma)
+		return;
+	fwcfg_arch_ops = ops;
+
+	qemu_fwcfg_read_entry_pio(FW_CFG_SIGNATURE, 4, &qemu);
+	if (be32_to_cpu(qemu) == QEMU_FW_CFG_SIGNATURE)
+		fwcfg_present = true;
+
+	if (fwcfg_present) {
+		qemu_fwcfg_read_entry_pio(FW_CFG_ID, 1, &dma_enabled);
+		if (dma_enabled & FW_CFG_DMA_ENABLED)
+			fwcfg_dma_present = true;
+	}
+}
diff --git a/drivers/misc/reset_sandbox.c b/drivers/misc/reset_sandbox.c
deleted file mode 100644
index 2691bb0..0000000
--- a/drivers/misc/reset_sandbox.c
+++ /dev/null
@@ -1,100 +0,0 @@
-/*
- * Copyright (c) 2015 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 <reset.h>
-#include <asm/state.h>
-#include <asm/test.h>
-
-DECLARE_GLOBAL_DATA_PTR;
-
-static int sandbox_warm_reset_request(struct udevice *dev, enum reset_t type)
-{
-	struct sandbox_state *state = state_get_current();
-
-	switch (type) {
-	case RESET_WARM:
-		state->last_reset = type;
-		break;
-	default:
-		return -ENOSYS;
-	}
-	if (!state->reset_allowed[type])
-		return -EACCES;
-
-	return -EINPROGRESS;
-}
-
-static int sandbox_reset_request(struct udevice *dev, enum reset_t type)
-{
-	struct sandbox_state *state = state_get_current();
-
-	/*
-	 * If we have a device tree, the device we created from platform data
-	 * (see the U_BOOT_DEVICE() declaration below) should not do anything.
-	 * If we are that device, return an error.
-	 */
-	if (state->fdt_fname && dev->of_offset == -1)
-		return -ENODEV;
-
-	switch (type) {
-	case RESET_COLD:
-		state->last_reset = type;
-		break;
-	case RESET_POWER:
-		state->last_reset = type;
-		if (!state->reset_allowed[type])
-			return -EACCES;
-		sandbox_exit();
-		break;
-	default:
-		return -ENOSYS;
-	}
-	if (!state->reset_allowed[type])
-		return -EACCES;
-
-	return -EINPROGRESS;
-}
-
-static struct reset_ops sandbox_reset_ops = {
-	.request	= sandbox_reset_request,
-};
-
-static const struct udevice_id sandbox_reset_ids[] = {
-	{ .compatible = "sandbox,reset" },
-	{ }
-};
-
-U_BOOT_DRIVER(reset_sandbox) = {
-	.name		= "reset_sandbox",
-	.id		= UCLASS_RESET,
-	.of_match	= sandbox_reset_ids,
-	.ops		= &sandbox_reset_ops,
-};
-
-static struct reset_ops sandbox_warm_reset_ops = {
-	.request	= sandbox_warm_reset_request,
-};
-
-static const struct udevice_id sandbox_warm_reset_ids[] = {
-	{ .compatible = "sandbox,warm-reset" },
-	{ }
-};
-
-U_BOOT_DRIVER(warm_reset_sandbox) = {
-	.name		= "warm_reset_sandbox",
-	.id		= UCLASS_RESET,
-	.of_match	= sandbox_warm_reset_ids,
-	.ops		= &sandbox_warm_reset_ops,
-};
-
-/* This is here in case we don't have a device tree */
-U_BOOT_DEVICE(reset_sandbox_non_fdt) = {
-	.name = "reset_sandbox",
-};
diff --git a/drivers/misc/reset-uclass.c b/drivers/misc/sysreset-uclass.c
index fdb5c6f..3566d17 100644
--- a/drivers/misc/reset-uclass.c
+++ b/drivers/misc/sysreset-uclass.c
@@ -6,7 +6,7 @@
  */
 
 #include <common.h>
-#include <reset.h>
+#include <sysreset.h>
 #include <dm.h>
 #include <errno.h>
 #include <regmap.h>
@@ -15,9 +15,9 @@
 #include <dm/root.h>
 #include <linux/err.h>
 
-int reset_request(struct udevice *dev, enum reset_t type)
+int sysreset_request(struct udevice *dev, enum sysreset_t type)
 {
-	struct reset_ops *ops = reset_get_ops(dev);
+	struct sysreset_ops *ops = sysreset_get_ops(dev);
 
 	if (!ops->request)
 		return -ENOSYS;
@@ -25,16 +25,16 @@ int reset_request(struct udevice *dev, enum reset_t type)
 	return ops->request(dev, type);
 }
 
-int reset_walk(enum reset_t type)
+int sysreset_walk(enum sysreset_t type)
 {
 	struct udevice *dev;
 	int ret = -ENOSYS;
 
-	while (ret != -EINPROGRESS && type < RESET_COUNT) {
-		for (uclass_first_device(UCLASS_RESET, &dev);
+	while (ret != -EINPROGRESS && type < SYSRESET_COUNT) {
+		for (uclass_first_device(UCLASS_SYSRESET, &dev);
 		     dev;
 		     uclass_next_device(&dev)) {
-			ret = reset_request(dev, type);
+			ret = sysreset_request(dev, type);
 			if (ret == -EINPROGRESS)
 				break;
 		}
@@ -44,38 +44,38 @@ int reset_walk(enum reset_t type)
 	return ret;
 }
 
-void reset_walk_halt(enum reset_t type)
+void sysreset_walk_halt(enum sysreset_t type)
 {
 	int ret;
 
-	ret = reset_walk(type);
+	ret = sysreset_walk(type);
 
 	/* Wait for the reset to take effect */
 	if (ret == -EINPROGRESS)
 		mdelay(100);
 
 	/* Still no reset? Give up */
-	printf("Reset not supported on this platform\n");
+	debug("System reset not supported on this platform\n");
 	hang();
 }
 
 /**
- * reset_cpu() - calls reset_walk(RESET_WARM)
+ * reset_cpu() - calls sysreset_walk(SYSRESET_WARM)
  */
 void reset_cpu(ulong addr)
 {
-	reset_walk_halt(RESET_WARM);
+	sysreset_walk_halt(SYSRESET_WARM);
 }
 
 
 int do_reset(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
 {
-	reset_walk_halt(RESET_WARM);
+	sysreset_walk_halt(SYSRESET_WARM);
 
 	return 0;
 }
 
-UCLASS_DRIVER(reset) = {
-	.id		= UCLASS_RESET,
-	.name		= "reset",
+UCLASS_DRIVER(sysreset) = {
+	.id		= UCLASS_SYSRESET,
+	.name		= "sysreset",
 };
diff --git a/drivers/misc/sysreset_sandbox.c b/drivers/misc/sysreset_sandbox.c
new file mode 100644
index 0000000..7ae7f38
--- /dev/null
+++ b/drivers/misc/sysreset_sandbox.c
@@ -0,0 +1,101 @@
+/*
+ * Copyright (c) 2015 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 <sysreset.h>
+#include <asm/state.h>
+#include <asm/test.h>
+
+DECLARE_GLOBAL_DATA_PTR;
+
+static int sandbox_warm_sysreset_request(struct udevice *dev,
+					 enum sysreset_t type)
+{
+	struct sandbox_state *state = state_get_current();
+
+	switch (type) {
+	case SYSRESET_WARM:
+		state->last_sysreset = type;
+		break;
+	default:
+		return -ENOSYS;
+	}
+	if (!state->sysreset_allowed[type])
+		return -EACCES;
+
+	return -EINPROGRESS;
+}
+
+static int sandbox_sysreset_request(struct udevice *dev, enum sysreset_t type)
+{
+	struct sandbox_state *state = state_get_current();
+
+	/*
+	 * If we have a device tree, the device we created from platform data
+	 * (see the U_BOOT_DEVICE() declaration below) should not do anything.
+	 * If we are that device, return an error.
+	 */
+	if (state->fdt_fname && dev->of_offset == -1)
+		return -ENODEV;
+
+	switch (type) {
+	case SYSRESET_COLD:
+		state->last_sysreset = type;
+		break;
+	case SYSRESET_POWER:
+		state->last_sysreset = type;
+		if (!state->sysreset_allowed[type])
+			return -EACCES;
+		sandbox_exit();
+		break;
+	default:
+		return -ENOSYS;
+	}
+	if (!state->sysreset_allowed[type])
+		return -EACCES;
+
+	return -EINPROGRESS;
+}
+
+static struct sysreset_ops sandbox_sysreset_ops = {
+	.request	= sandbox_sysreset_request,
+};
+
+static const struct udevice_id sandbox_sysreset_ids[] = {
+	{ .compatible = "sandbox,reset" },
+	{ }
+};
+
+U_BOOT_DRIVER(sysreset_sandbox) = {
+	.name		= "sysreset_sandbox",
+	.id		= UCLASS_SYSRESET,
+	.of_match	= sandbox_sysreset_ids,
+	.ops		= &sandbox_sysreset_ops,
+};
+
+static struct sysreset_ops sandbox_warm_sysreset_ops = {
+	.request	= sandbox_warm_sysreset_request,
+};
+
+static const struct udevice_id sandbox_warm_sysreset_ids[] = {
+	{ .compatible = "sandbox,warm-reset" },
+	{ }
+};
+
+U_BOOT_DRIVER(warm_sysreset_sandbox) = {
+	.name		= "warm_sysreset_sandbox",
+	.id		= UCLASS_SYSRESET,
+	.of_match	= sandbox_warm_sysreset_ids,
+	.ops		= &sandbox_warm_sysreset_ops,
+};
+
+/* This is here in case we don't have a device tree */
+U_BOOT_DEVICE(sysreset_sandbox_non_fdt) = {
+	.name = "sysreset_sandbox",
+};
diff --git a/drivers/mmc/dw_mmc.c b/drivers/mmc/dw_mmc.c
index 7329f40..74a2663 100644
--- a/drivers/mmc/dw_mmc.c
+++ b/drivers/mmc/dw_mmc.c
@@ -454,27 +454,40 @@ static const struct mmc_ops dwmci_ops = {
 	.init		= dwmci_init,
 };
 
-int add_dwmci(struct dwmci_host *host, u32 max_clk, u32 min_clk)
+void dwmci_setup_cfg(struct mmc_config *cfg, const char *name, int buswidth,
+		     uint caps, u32 max_clk, u32 min_clk)
 {
-	host->cfg.name = host->name;
-	host->cfg.ops = &dwmci_ops;
-	host->cfg.f_min = min_clk;
-	host->cfg.f_max = max_clk;
+	cfg->name = name;
+	cfg->ops = &dwmci_ops;
+	cfg->f_min = min_clk;
+	cfg->f_max = max_clk;
 
-	host->cfg.voltages = MMC_VDD_32_33 | MMC_VDD_33_34 | MMC_VDD_165_195;
+	cfg->voltages = MMC_VDD_32_33 | MMC_VDD_33_34 | MMC_VDD_165_195;
 
-	host->cfg.host_caps = host->caps;
+	cfg->host_caps = caps;
 
-	if (host->buswidth == 8) {
-		host->cfg.host_caps |= MMC_MODE_8BIT;
-		host->cfg.host_caps &= ~MMC_MODE_4BIT;
+	if (buswidth == 8) {
+		cfg->host_caps |= MMC_MODE_8BIT;
+		cfg->host_caps &= ~MMC_MODE_4BIT;
 	} else {
-		host->cfg.host_caps |= MMC_MODE_4BIT;
-		host->cfg.host_caps &= ~MMC_MODE_8BIT;
+		cfg->host_caps |= MMC_MODE_4BIT;
+		cfg->host_caps &= ~MMC_MODE_8BIT;
 	}
-	host->cfg.host_caps |= MMC_MODE_HS | MMC_MODE_HS_52MHz;
+	cfg->host_caps |= MMC_MODE_HS | MMC_MODE_HS_52MHz;
+
+	cfg->b_max = CONFIG_SYS_MMC_MAX_BLK_COUNT;
+}
 
-	host->cfg.b_max = CONFIG_SYS_MMC_MAX_BLK_COUNT;
+#ifdef CONFIG_BLK
+int dwmci_bind(struct udevice *dev, struct mmc *mmc, struct mmc_config *cfg)
+{
+	return mmc_bind(dev, mmc, cfg);
+}
+#else
+int add_dwmci(struct dwmci_host *host, u32 max_clk, u32 min_clk)
+{
+	dwmci_setup_cfg(&host->cfg, host->name, host->buswidth, host->caps,
+			max_clk, min_clk);
 
 	host->mmc = mmc_create(&host->cfg, host);
 	if (host->mmc == NULL)
@@ -482,3 +495,4 @@ int add_dwmci(struct dwmci_host *host, u32 max_clk, u32 min_clk)
 
 	return 0;
 }
+#endif
diff --git a/drivers/mmc/fsl_esdhc.c b/drivers/mmc/fsl_esdhc.c
index 3acf9e8..57ad975 100644
--- a/drivers/mmc/fsl_esdhc.c
+++ b/drivers/mmc/fsl_esdhc.c
@@ -308,14 +308,10 @@ 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_FSL_LAYERSCAPE
 	unsigned start = 0;
-#else
-	unsigned start = (unsigned)data->dest ;
-#endif
+	unsigned end = 0;
 	unsigned size = roundup(ARCH_DMA_MINALIGN,
 				data->blocks*data->blocksize);
-	unsigned end = start+size ;
 #ifdef CONFIG_FSL_LAYERSCAPE
 	dma_addr_t addr;
 
@@ -324,7 +320,10 @@ static void check_and_invalidate_dcache_range
 		printf("Error found for upper 32 bits\n");
 	else
 		start = lower_32_bits(addr);
+#else
+	start = (unsigned)data->dest;
 #endif
+	end = start + size;
 	invalidate_dcache_range(start, end);
 }
 
diff --git a/drivers/mmc/mmc.c b/drivers/mmc/mmc.c
index 74b3d68..7586558 100644
--- a/drivers/mmc/mmc.c
+++ b/drivers/mmc/mmc.c
@@ -155,8 +155,6 @@ int mmc_send_status(struct mmc *mmc, int timeout)
 #endif
 		return TIMEOUT;
 	}
-	if (cmd.response[0] & MMC_STATUS_SWITCH_ERROR)
-		return SWITCH_ERR;
 
 	return 0;
 }
@@ -516,7 +514,7 @@ static int mmc_change_freq(struct mmc *mmc)
 	err = mmc_switch(mmc, EXT_CSD_CMD_SET_NORMAL, EXT_CSD_HS_TIMING, 1);
 
 	if (err)
-		return err == SWITCH_ERR ? 0 : err;
+		return err;
 
 	/* Now check to see that it worked */
 	err = mmc_send_ext_csd(mmc, ext_csd);
@@ -984,7 +982,7 @@ static const int fbase[] = {
 /* Multiplier values for TRAN_SPEED.  Multiplied by 10 to be nice
  * to platforms without floating point.
  */
-static const int multipliers[] = {
+static const u8 multipliers[] = {
 	0,	/* reserved */
 	10,
 	12,
@@ -1531,15 +1529,6 @@ static int mmc_send_if_cond(struct mmc *mmc)
 	return 0;
 }
 
-/* not used any more */
-int __deprecated mmc_register(struct mmc *mmc)
-{
-#if !defined(CONFIG_SPL_BUILD) || defined(CONFIG_SPL_LIBCOMMON_SUPPORT)
-	printf("%s is deprecated! use mmc_create() instead.\n", __func__);
-#endif
-	return -1;
-}
-
 #ifdef CONFIG_BLK
 int mmc_bind(struct udevice *dev, struct mmc *mmc, const struct mmc_config *cfg)
 {
@@ -1566,7 +1555,7 @@ int mmc_bind(struct udevice *dev, struct mmc *mmc, const struct mmc_config *cfg)
 	bdesc->removable = 1;
 
 	/* setup initial part type */
-	bdesc->part_type = mmc->cfg->part_type;
+	bdesc->part_type = cfg->part_type;
 	mmc->dev = dev;
 
 	return 0;
diff --git a/drivers/mmc/mmc_private.h b/drivers/mmc/mmc_private.h
index 27b9e5f..9f0d5c2 100644
--- a/drivers/mmc/mmc_private.h
+++ b/drivers/mmc/mmc_private.h
@@ -37,6 +37,19 @@ ulong mmc_bwrite(struct blk_desc *block_dev, lbaint_t start, lbaint_t blkcnt,
 
 /* SPL will never write or erase, declare dummies to reduce code size. */
 
+#ifdef CONFIG_BLK
+static inline unsigned long mmc_berase(struct udevice *dev,
+				       lbaint_t start, lbaint_t blkcnt)
+{
+	return 0;
+}
+
+static inline ulong mmc_bwrite(struct udevice *dev, lbaint_t start,
+			       lbaint_t blkcnt, const void *src)
+{
+	return 0;
+}
+#else
 static inline unsigned long mmc_berase(struct blk_desc *block_dev,
 				       lbaint_t start, lbaint_t blkcnt)
 {
@@ -48,6 +61,7 @@ static inline ulong mmc_bwrite(struct blk_desc *block_dev, lbaint_t start,
 {
 	return 0;
 }
+#endif
 
 #endif /* CONFIG_SPL_BUILD */
 
diff --git a/drivers/mmc/omap_hsmmc.c b/drivers/mmc/omap_hsmmc.c
index be34057..d007b56 100644
--- a/drivers/mmc/omap_hsmmc.c
+++ b/drivers/mmc/omap_hsmmc.c
@@ -701,6 +701,7 @@ int omap_mmc_init(int dev_index, uint host_caps_mask, uint f_max, int cd_gpio,
 		priv_data->base_addr = (struct hsmmc *)OMAP_HSMMC2_BASE;
 #if (defined(CONFIG_OMAP44XX) || defined(CONFIG_OMAP54XX) || \
 	defined(CONFIG_DRA7XX) || defined(CONFIG_AM57XX) || \
+	defined(CONFIG_AM33XX) || \
 	defined(CONFIG_AM43XX) || defined(CONFIG_SOC_KEYSTONE)) && \
 		defined(CONFIG_HSMMC2_8BIT)
 		/* Enable 8-bit interface for eMMC on OMAP4/5 or DRA7XX */
diff --git a/drivers/mmc/rockchip_dw_mmc.c b/drivers/mmc/rockchip_dw_mmc.c
index 0a261c5..750ab9f 100644
--- a/drivers/mmc/rockchip_dw_mmc.c
+++ b/drivers/mmc/rockchip_dw_mmc.c
@@ -18,6 +18,11 @@
 
 DECLARE_GLOBAL_DATA_PTR;
 
+struct rockchip_mmc_plat {
+	struct mmc_config cfg;
+	struct mmc mmc;
+};
+
 struct rockchip_dwmmc_priv {
 	struct udevice *clk;
 	int periph;
@@ -62,6 +67,9 @@ static int rockchip_dwmmc_ofdata_to_platdata(struct udevice *dev)
 
 static int rockchip_dwmmc_probe(struct udevice *dev)
 {
+#ifdef CONFIG_BLK
+	struct rockchip_mmc_plat *plat = dev_get_platdata(dev);
+#endif
 	struct mmc_uclass_priv *upriv = dev_get_uclass_priv(dev);
 	struct rockchip_dwmmc_priv *priv = dev_get_priv(dev);
 	struct dwmci_host *host = &priv->host;
@@ -100,16 +108,37 @@ static int rockchip_dwmmc_probe(struct udevice *dev)
 			return ret;
 	}
 #endif
+#ifdef CONFIG_BLK
+	dwmci_setup_cfg(&plat->cfg, dev->name, host->buswidth, host->caps,
+			minmax[1], minmax[0]);
+	host->mmc = &plat->mmc;
+#else
 	ret = add_dwmci(host, minmax[1], minmax[0]);
 	if (ret)
 		return ret;
 
+#endif
+	host->mmc->priv = &priv->host;
 	host->mmc->dev = dev;
 	upriv->mmc = host->mmc;
 
 	return 0;
 }
 
+static int rockchip_dwmmc_bind(struct udevice *dev)
+{
+#ifdef CONFIG_BLK
+	struct rockchip_mmc_plat *plat = dev_get_platdata(dev);
+	int ret;
+
+	ret = dwmci_bind(dev, &plat->mmc, &plat->cfg);
+	if (ret)
+		return ret;
+#endif
+
+	return 0;
+}
+
 static const struct udevice_id rockchip_dwmmc_ids[] = {
 	{ .compatible = "rockchip,rk3288-dw-mshc" },
 	{ }
@@ -120,8 +149,10 @@ U_BOOT_DRIVER(rockchip_dwmmc_drv) = {
 	.id		= UCLASS_MMC,
 	.of_match	= rockchip_dwmmc_ids,
 	.ofdata_to_platdata = rockchip_dwmmc_ofdata_to_platdata,
+	.bind		= rockchip_dwmmc_bind,
 	.probe		= rockchip_dwmmc_probe,
 	.priv_auto_alloc_size = sizeof(struct rockchip_dwmmc_priv),
+	.platdata_auto_alloc_size = sizeof(struct rockchip_mmc_plat),
 };
 
 #ifdef CONFIG_PWRSEQ
diff --git a/drivers/mmc/sdhci.c b/drivers/mmc/sdhci.c
index ef7e615..5c71ab8 100644
--- a/drivers/mmc/sdhci.c
+++ b/drivers/mmc/sdhci.c
@@ -137,7 +137,7 @@ static int sdhci_send_command(struct mmc *mmc, struct mmc_cmd *cmd,
 	int trans_bytes = 0, is_aligned = 1;
 	u32 mask, flags, mode;
 	unsigned int time = 0, start_addr = 0;
-	int mmc_dev = mmc->block_dev.devnum;
+	int mmc_dev = mmc_get_blk_desc(mmc)->devnum;
 	unsigned start = get_timer(0);
 
 	/* Timeout unit - ms */
diff --git a/drivers/mmc/tegra_mmc.c b/drivers/mmc/tegra_mmc.c
index 573819a..c9d9432 100644
--- a/drivers/mmc/tegra_mmc.c
+++ b/drivers/mmc/tegra_mmc.c
@@ -11,8 +11,10 @@
 #include <common.h>
 #include <asm/gpio.h>
 #include <asm/io.h>
+#ifndef CONFIG_TEGRA186
 #include <asm/arch/clock.h>
 #include <asm/arch-tegra/clk_rst.h>
+#endif
 #include <asm/arch-tegra/mmc.h>
 #include <asm/arch-tegra/tegra_mmc.h>
 #include <mmc.h>
@@ -357,8 +359,12 @@ static void mmc_change_clock(struct mmc_host *host, uint clock)
 	 */
 	if (clock == 0)
 		goto out;
+#ifndef CONFIG_TEGRA186
 	clock_adjust_periph_pll_div(host->mmc_id, CLOCK_ID_PERIPH, clock,
 				    &div);
+#else
+	div = (20000000 + clock - 1) / clock;
+#endif
 	debug("div = %d\n", div);
 
 	writew(0, &host->reg->clkcon);
@@ -543,7 +549,9 @@ static int do_mmc_init(int dev_index, bool removable)
 	      gpio_get_number(&host->cd_gpio));
 
 	host->clock = 0;
+#ifndef CONFIG_TEGRA186
 	clock_start_periph_pll(host->mmc_id, CLOCK_ID_PERIPH, 20000000);
+#endif
 
 	if (dm_gpio_is_valid(&host->pwr_gpio))
 		dm_gpio_set_value(&host->pwr_gpio, 1);
@@ -568,7 +576,11 @@ static int do_mmc_init(int dev_index, bool removable)
 	 *  (actually 52MHz)
 	 */
 	host->cfg.f_min = 375000;
+#ifndef CONFIG_TEGRA186
 	host->cfg.f_max = 48000000;
+#else
+	host->cfg.f_max = 375000;
+#endif
 
 	host->cfg.b_max = CONFIG_SYS_MMC_MAX_BLK_COUNT;
 
@@ -600,11 +612,13 @@ static int mmc_get_config(const void *blob, int node, struct mmc_host *host,
 		return -FDT_ERR_NOTFOUND;
 	}
 
+#ifndef CONFIG_TEGRA186
 	host->mmc_id = clock_decode_periph_id(blob, node);
 	if (host->mmc_id == PERIPH_ID_NONE) {
 		debug("%s: could not decode periph id\n", __func__);
 		return -FDT_ERR_NOTFOUND;
 	}
+#endif
 
 	/*
 	 * NOTE: mmc->bus_width is determined by mmc.c dynamically.
@@ -624,7 +638,13 @@ static int mmc_get_config(const void *blob, int node, struct mmc_host *host,
 	*removablep = !fdtdec_get_bool(blob, node, "non-removable");
 
 	debug("%s: found controller at %p, width = %d, periph_id = %d\n",
-		__func__, host->reg, host->width, host->mmc_id);
+		__func__, host->reg, host->width,
+#ifndef CONFIG_TEGRA186
+		host->mmc_id
+#else
+		-1
+#endif
+	);
 	return 0;
 }
 
@@ -668,6 +688,16 @@ void tegra_mmc_init(void)
 	const void *blob = gd->fdt_blob;
 	debug("%s entry\n", __func__);
 
+	/* See if any Tegra186 MMC controllers are present */
+	count = fdtdec_find_aliases_for_id(blob, "sdhci",
+		COMPAT_NVIDIA_TEGRA186_SDMMC, node_list,
+		CONFIG_SYS_MMC_MAX_DEVICE);
+	debug("%s: count of Tegra186 sdhci nodes is %d\n", __func__, count);
+	if (process_nodes(blob, node_list, count)) {
+		printf("%s: Error processing T186 mmc node(s)!\n", __func__);
+		return;
+	}
+
 	/* See if any Tegra210 MMC controllers are present */
 	count = fdtdec_find_aliases_for_id(blob, "sdhci",
 		COMPAT_NVIDIA_TEGRA210_SDMMC, node_list,
diff --git a/drivers/mtd/nand/Makefile b/drivers/mtd/nand/Makefile
index 6fb3718..837d397 100644
--- a/drivers/mtd/nand/Makefile
+++ b/drivers/mtd/nand/Makefile
@@ -13,7 +13,6 @@ endif
 
 obj-$(CONFIG_SPL_NAND_AM33XX_BCH) += am335x_spl_bch.o
 obj-$(CONFIG_SPL_NAND_DENALI) += denali_spl.o
-obj-$(CONFIG_SPL_NAND_DOCG4) += docg4_spl.o
 obj-$(CONFIG_SPL_NAND_SIMPLE) += nand_spl_simple.o
 obj-$(CONFIG_SPL_NAND_LOAD) += nand_spl_load.o
 obj-$(CONFIG_SPL_NAND_ECC) += nand_ecc.o
@@ -50,7 +49,6 @@ obj-$(CONFIG_NAND_FSL_ELBC) += fsl_elbc_nand.o
 obj-$(CONFIG_NAND_FSL_IFC) += fsl_ifc_nand.o
 obj-$(CONFIG_NAND_FSL_UPM) += fsl_upm.o
 obj-$(CONFIG_NAND_FSMC) += fsmc_nand.o
-obj-$(CONFIG_NAND_JZ4740) += jz4740_nand.o
 obj-$(CONFIG_NAND_KB9202) += kb9202_nand.o
 obj-$(CONFIG_NAND_KIRKWOOD) += kirkwood_nand.o
 obj-$(CONFIG_NAND_KMETER1) += kmeter1_nand.o
@@ -68,7 +66,6 @@ obj-$(CONFIG_TEGRA_NAND) += tegra_nand.o
 obj-$(CONFIG_NAND_OMAP_GPMC) += omap_gpmc.o
 obj-$(CONFIG_NAND_OMAP_ELM) += omap_elm.o
 obj-$(CONFIG_NAND_PLAT) += nand_plat.o
-obj-$(CONFIG_NAND_DOCG4) += docg4.o
 
 else  # minimal SPL drivers
 
diff --git a/drivers/mtd/nand/am335x_spl_bch.c b/drivers/mtd/nand/am335x_spl_bch.c
index bf8b2ee..f8770e0 100644
--- a/drivers/mtd/nand/am335x_spl_bch.c
+++ b/drivers/mtd/nand/am335x_spl_bch.c
@@ -16,7 +16,7 @@
 #include <linux/mtd/nand_ecc.h>
 
 static int nand_ecc_pos[] = CONFIG_SYS_NAND_ECCPOS;
-nand_info_t nand_info[1];
+static struct mtd_info *mtd;
 static struct nand_chip nand_chip;
 
 #define ECCSTEPS	(CONFIG_SYS_NAND_PAGE_SIZE / \
@@ -30,12 +30,12 @@ static struct nand_chip nand_chip;
 static int nand_command(int block, int page, uint32_t offs,
 	u8 cmd)
 {
-	struct nand_chip *this = nand_info[0].priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 	int page_addr = page + block * CONFIG_SYS_NAND_PAGE_COUNT;
 	void (*hwctrl)(struct mtd_info *mtd, int cmd,
 			unsigned int ctrl) = this->cmd_ctrl;
 
-	while (!this->dev_ready(&nand_info[0]))
+	while (!this->dev_ready(mtd))
 		;
 
 	/* Emulate NAND_CMD_READOOB */
@@ -45,11 +45,11 @@ static int nand_command(int block, int page, uint32_t offs,
 	}
 
 	/* Begin command latch cycle */
-	hwctrl(&nand_info[0], cmd, NAND_CTRL_CLE | NAND_CTRL_CHANGE);
+	hwctrl(mtd, cmd, NAND_CTRL_CLE | NAND_CTRL_CHANGE);
 
 	if (cmd == NAND_CMD_RESET) {
-		hwctrl(&nand_info[0], NAND_CMD_NONE, NAND_NCE | NAND_CTRL_CHANGE);
-		while (!this->dev_ready(&nand_info[0]))
+		hwctrl(mtd, NAND_CMD_NONE, NAND_NCE | NAND_CTRL_CHANGE);
+		while (!this->dev_ready(mtd))
 			;
 		return 0;
 	}
@@ -60,39 +60,39 @@ static int nand_command(int block, int page, uint32_t offs,
 
 	/* Set ALE and clear CLE to start address cycle */
 	/* Column address */
-	hwctrl(&nand_info[0], offs & 0xff,
+	hwctrl(mtd, offs & 0xff,
 		       NAND_CTRL_ALE | NAND_CTRL_CHANGE); /* A[7:0] */
-	hwctrl(&nand_info[0], (offs >> 8) & 0xff, NAND_CTRL_ALE); /* A[11:9] */
+	hwctrl(mtd, (offs >> 8) & 0xff, NAND_CTRL_ALE); /* A[11:9] */
 	/* Row address */
 	if (cmd != NAND_CMD_RNDOUT) {
-		hwctrl(&nand_info[0], (page_addr & 0xff),
+		hwctrl(mtd, (page_addr & 0xff),
 		       NAND_CTRL_ALE); /* A[19:12] */
-		hwctrl(&nand_info[0], ((page_addr >> 8) & 0xff),
+		hwctrl(mtd, ((page_addr >> 8) & 0xff),
 		       NAND_CTRL_ALE); /* A[27:20] */
 #ifdef CONFIG_SYS_NAND_5_ADDR_CYCLE
 		/* One more address cycle for devices > 128MiB */
-		hwctrl(&nand_info[0], (page_addr >> 16) & 0x0f,
+		hwctrl(mtd, (page_addr >> 16) & 0x0f,
 		       NAND_CTRL_ALE); /* A[31:28] */
 #endif
 	}
 
-	hwctrl(&nand_info[0], NAND_CMD_NONE, NAND_NCE | NAND_CTRL_CHANGE);
+	hwctrl(mtd, NAND_CMD_NONE, NAND_NCE | NAND_CTRL_CHANGE);
 
 	if (cmd == NAND_CMD_READ0) {
 		/* Latch in address */
-		hwctrl(&nand_info[0], NAND_CMD_READSTART,
+		hwctrl(mtd, NAND_CMD_READSTART,
 			   NAND_CTRL_CLE | NAND_CTRL_CHANGE);
-		hwctrl(&nand_info[0], NAND_CMD_NONE, NAND_NCE | NAND_CTRL_CHANGE);
+		hwctrl(mtd, NAND_CMD_NONE, NAND_NCE | NAND_CTRL_CHANGE);
 
 		/*
 		 * Wait a while for the data to be ready
 		 */
-		while (!this->dev_ready(&nand_info[0]))
+		while (!this->dev_ready(mtd))
 			;
 	} else if (cmd == NAND_CMD_RNDOUT) {
-		hwctrl(&nand_info[0], NAND_CMD_RNDOUTSTART, NAND_CTRL_CLE |
+		hwctrl(mtd, NAND_CMD_RNDOUTSTART, NAND_CTRL_CLE |
 					NAND_CTRL_CHANGE);
-		hwctrl(&nand_info[0], NAND_CMD_NONE, NAND_NCE | NAND_CTRL_CHANGE);
+		hwctrl(mtd, NAND_CMD_NONE, NAND_NCE | NAND_CTRL_CHANGE);
 	}
 
 	return 0;
@@ -100,7 +100,7 @@ static int nand_command(int block, int page, uint32_t offs,
 
 static int nand_is_bad_block(int block)
 {
-	struct nand_chip *this = nand_info[0].priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 
 	nand_command(block, 0, CONFIG_SYS_NAND_BAD_BLOCK_POS,
 		NAND_CMD_READOOB);
@@ -121,7 +121,7 @@ static int nand_is_bad_block(int block)
 
 static int nand_read_page(int block, int page, void *dst)
 {
-	struct nand_chip *this = nand_info[0].priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 	u_char ecc_calc[ECCTOTAL];
 	u_char ecc_code[ECCTOTAL];
 	u_char oob_data[CONFIG_SYS_NAND_OOBSIZE];
@@ -137,15 +137,15 @@ static int nand_read_page(int block, int page, void *dst)
 	nand_command(block, page, 0, NAND_CMD_READ0);
 
 	for (i = 0; eccsteps; eccsteps--, i += eccbytes, p += eccsize) {
-		this->ecc.hwctl(&nand_info[0], NAND_ECC_READ);
+		this->ecc.hwctl(mtd, NAND_ECC_READ);
 		nand_command(block, page, data_pos, NAND_CMD_RNDOUT);
 
-		this->read_buf(&nand_info[0], p, eccsize);
+		this->read_buf(mtd, p, eccsize);
 
 		nand_command(block, page, oob_pos, NAND_CMD_RNDOUT);
 
-		this->read_buf(&nand_info[0], oob, eccbytes);
-		this->ecc.calculate(&nand_info[0], p, &ecc_calc[i]);
+		this->read_buf(mtd, oob, eccbytes);
+		this->ecc.calculate(mtd, p, &ecc_calc[i]);
 
 		data_pos += eccsize;
 		oob_pos += eccbytes;
@@ -164,7 +164,7 @@ static int nand_read_page(int block, int page, void *dst)
 		 * from correct_data(). We just hope that all possible errors
 		 * are corrected by this routine.
 		 */
-		this->ecc.correct(&nand_info[0], p, &ecc_code[i], &ecc_calc[i]);
+		this->ecc.correct(mtd, p, &ecc_code[i], &ecc_calc[i]);
 	}
 
 	return 0;
@@ -173,7 +173,7 @@ static int nand_read_page(int block, int page, void *dst)
 int nand_spl_load_image(uint32_t offs, unsigned int size, void *dst)
 {
 	unsigned int block, lastblock;
-	unsigned int page;
+	unsigned int page, page_offset;
 
 	/*
 	 * offs has to be aligned to a page address!
@@ -181,6 +181,7 @@ int nand_spl_load_image(uint32_t offs, unsigned int size, void *dst)
 	block = offs / CONFIG_SYS_NAND_BLOCK_SIZE;
 	lastblock = (offs + size - 1) / CONFIG_SYS_NAND_BLOCK_SIZE;
 	page = (offs % CONFIG_SYS_NAND_BLOCK_SIZE) / CONFIG_SYS_NAND_PAGE_SIZE;
+	page_offset = offs % CONFIG_SYS_NAND_PAGE_SIZE;
 
 	while (block <= lastblock) {
 		if (!nand_is_bad_block(block)) {
@@ -189,6 +190,18 @@ int nand_spl_load_image(uint32_t offs, unsigned int size, void *dst)
 			 */
 			while (page < CONFIG_SYS_NAND_PAGE_COUNT) {
 				nand_read_page(block, page, dst);
+				/*
+				 * When offs is not aligned to page address the
+				 * extra offset is copied to dst as well. Copy
+				 * the image such that its first byte will be
+				 * at the dst.
+				 */
+				if (unlikely(page_offset)) {
+					memmove(dst, dst + page_offset,
+						CONFIG_SYS_NAND_PAGE_SIZE);
+					dst = (void *)((int)dst - page_offset);
+					page_offset = 0;
+				}
 				dst += CONFIG_SYS_NAND_PAGE_SIZE;
 				page++;
 			}
@@ -210,13 +223,13 @@ void nand_init(void)
 	/*
 	 * Init board specific nand support
 	 */
-	nand_info[0].priv = &nand_chip;
+	mtd = &nand_chip.mtd;
 	nand_chip.IO_ADDR_R = nand_chip.IO_ADDR_W =
 		(void  __iomem *)CONFIG_SYS_NAND_BASE;
 	board_nand_init(&nand_chip);
 
 	if (nand_chip.select_chip)
-		nand_chip.select_chip(&nand_info[0], 0);
+		nand_chip.select_chip(mtd, 0);
 
 	/* NAND chip may require reset after power-on */
 	nand_command(0, 0, 0, NAND_CMD_RESET);
@@ -226,5 +239,5 @@ void nand_init(void)
 void nand_deselect(void)
 {
 	if (nand_chip.select_chip)
-		nand_chip.select_chip(&nand_info[0], -1);
+		nand_chip.select_chip(mtd, -1);
 }
diff --git a/drivers/mtd/nand/arasan_nfc.c b/drivers/mtd/nand/arasan_nfc.c
index 2d73a05..320cbaa 100644
--- a/drivers/mtd/nand/arasan_nfc.c
+++ b/drivers/mtd/nand/arasan_nfc.c
@@ -230,7 +230,7 @@ static void arasan_nand_enable_ecc(void)
 static u8 arasan_nand_get_addrcycle(struct mtd_info *mtd)
 {
 	u8 addrcycles;
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 
 	switch (curr_cmd->addr_cycles) {
 	case NAND_ADDR_CYCL_NONE:
@@ -264,7 +264,7 @@ static u8 arasan_nand_get_addrcycle(struct mtd_info *mtd)
 
 static int arasan_nand_read_page(struct mtd_info *mtd, u8 *buf, u32 size)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	u32 reg_val, i, pktsize, pktnum;
 	u32 *bufptr = (u32 *)buf;
 	u32 timeout;
@@ -433,7 +433,8 @@ static void arasan_nand_fill_tx(const u8 *buf, int len)
 }
 
 static int arasan_nand_write_page_hwecc(struct mtd_info *mtd,
-		struct nand_chip *chip, const u8 *buf, int oob_required)
+		struct nand_chip *chip, const u8 *buf, int oob_required,
+		int page)
 {
 	u32 reg_val, i, pktsize, pktnum;
 	const u32 *bufptr = (const u32 *)buf;
@@ -441,7 +442,7 @@ static int arasan_nand_write_page_hwecc(struct mtd_info *mtd,
 	u32 size = mtd->writesize;
 	u32 rdcount = 0;
 	u8 column_addr_cycles;
-	struct arasan_nand_info *nand = chip->priv;
+	struct arasan_nand_info *nand = nand_get_controller_data(chip);
 
 	if (chip->ecc_step_ds >= ARASAN_NAND_PKTSIZE_1K)
 		pktsize = ARASAN_NAND_PKTSIZE_1K;
@@ -944,7 +945,7 @@ static void arasan_nand_read_buf(struct mtd_info *mtd, u8 *buf, int size)
 
 static u8 arasan_nand_read_byte(struct mtd_info *mtd)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	u32 size;
 	u8 val;
 	struct nand_onfi_params *p;
@@ -976,8 +977,8 @@ static void arasan_nand_cmd_function(struct mtd_info *mtd, unsigned int command,
 				     int column, int page_addr)
 {
 	u32 i, ret = 0;
-	struct nand_chip *chip = mtd->priv;
-	struct arasan_nand_info *nand = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct arasan_nand_info *nand = nand_get_controller_data(chip);
 
 	curr_cmd = NULL;
 	writel(ARASAN_NAND_INT_STS_XFR_CMPLT_MASK,
@@ -1033,7 +1034,7 @@ static int arasan_nand_ecc_init(struct mtd_info *mtd)
 {
 	int found = -1;
 	u32 regval, eccpos_start, i;
-	struct nand_chip *nand_chip = mtd->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
 
 	nand_chip->ecc.mode = NAND_ECC_HW;
 	nand_chip->ecc.hwctl = NULL;
@@ -1058,20 +1059,20 @@ static int arasan_nand_ecc_init(struct mtd_info *mtd)
 	if (found < 0)
 		return 1;
 
-	regval = ecc_matrix[i].eccaddr |
-		 (ecc_matrix[i].eccsize << ARASAN_NAND_ECC_SIZE_SHIFT) |
-		 (ecc_matrix[i].bch << ARASAN_NAND_ECC_BCH_SHIFT);
+	regval = ecc_matrix[found].eccaddr |
+		 (ecc_matrix[found].eccsize << ARASAN_NAND_ECC_SIZE_SHIFT) |
+		 (ecc_matrix[found].bch << ARASAN_NAND_ECC_BCH_SHIFT);
 	writel(regval, &arasan_nand_base->ecc_reg);
 
-	if (ecc_matrix[i].bch) {
+	if (ecc_matrix[found].bch) {
 		regval = readl(&arasan_nand_base->memadr_reg2);
 		regval &= ~ARASAN_NAND_MEM_ADDR2_BCH_MASK;
-		regval |= (ecc_matrix[i].bchval <<
+		regval |= (ecc_matrix[found].bchval <<
 			   ARASAN_NAND_MEM_ADDR2_BCH_SHIFT);
 		writel(regval, &arasan_nand_base->memadr_reg2);
 	}
 
-	nand_oob.eccbytes = ecc_matrix[i].eccsize;
+	nand_oob.eccbytes = ecc_matrix[found].eccsize;
 	eccpos_start = mtd->oobsize - nand_oob.eccbytes;
 
 	for (i = 0; i < nand_oob.eccbytes; i++)
@@ -1080,9 +1081,9 @@ static int arasan_nand_ecc_init(struct mtd_info *mtd)
 	nand_oob.oobfree[0].offset = 2;
 	nand_oob.oobfree[0].length = eccpos_start - 2;
 
-	nand_chip->ecc.size = ecc_matrix[i].ecc_codeword_size;
-	nand_chip->ecc.strength = ecc_matrix[i].eccbits;
-	nand_chip->ecc.bytes = ecc_matrix[i].eccsize;
+	nand_chip->ecc.size = ecc_matrix[found].ecc_codeword_size;
+	nand_chip->ecc.strength = ecc_matrix[found].eccbits;
+	nand_chip->ecc.bytes = ecc_matrix[found].eccsize;
 	nand_chip->ecc.layout = &nand_oob;
 
 	return 0;
@@ -1101,9 +1102,8 @@ static int arasan_nand_init(struct nand_chip *nand_chip, int devnum)
 	}
 
 	nand->nand_base = arasan_nand_base;
-	mtd = &nand_info[0];
-	nand_chip->priv = nand;
-	mtd->priv = nand_chip;
+	mtd = nand_to_mtd(nand_chip);
+	nand_set_controller_data(nand_chip, nand);
 
 	/* Set the driver entry points for MTD */
 	nand_chip->cmdfunc = arasan_nand_cmd_function;
@@ -1134,7 +1134,7 @@ static int arasan_nand_init(struct nand_chip *nand_chip, int devnum)
 		goto fail;
 	}
 
-	if (nand_register(devnum)) {
+	if (nand_register(devnum, mtd)) {
 		printf("Nand Register Fail\n");
 		goto fail;
 	}
diff --git a/drivers/mtd/nand/atmel_nand.c b/drivers/mtd/nand/atmel_nand.c
index 7cc1de0..75e8307 100644
--- a/drivers/mtd/nand/atmel_nand.c
+++ b/drivers/mtd/nand/atmel_nand.c
@@ -160,8 +160,8 @@ static int pmecc_data_alloc(struct atmel_nand_host *host)
 
 static void pmecc_gen_syndrome(struct mtd_info *mtd, int sector)
 {
-	struct nand_chip *nand_chip = mtd->priv;
-	struct atmel_nand_host *host = nand_chip->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct atmel_nand_host *host = nand_get_controller_data(nand_chip);
 	int i;
 	uint32_t value;
 
@@ -177,8 +177,8 @@ static void pmecc_gen_syndrome(struct mtd_info *mtd, int sector)
 
 static void pmecc_substitute(struct mtd_info *mtd)
 {
-	struct nand_chip *nand_chip = mtd->priv;
-	struct atmel_nand_host *host = nand_chip->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct atmel_nand_host *host = nand_get_controller_data(nand_chip);
 	int16_t __iomem *alpha_to = host->pmecc_alpha_to;
 	int16_t __iomem *index_of = host->pmecc_index_of;
 	int16_t *partial_syn = host->pmecc_partial_syn;
@@ -227,8 +227,8 @@ static void pmecc_substitute(struct mtd_info *mtd)
  */
 static void pmecc_get_sigma(struct mtd_info *mtd)
 {
-	struct nand_chip *nand_chip = mtd->priv;
-	struct atmel_nand_host *host = nand_chip->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct atmel_nand_host *host = nand_get_controller_data(nand_chip);
 
 	int16_t *lmu = host->pmecc_lmu;
 	int16_t *si = host->pmecc_si;
@@ -383,8 +383,8 @@ static void pmecc_get_sigma(struct mtd_info *mtd)
 
 static int pmecc_err_location(struct mtd_info *mtd)
 {
-	struct nand_chip *nand_chip = mtd->priv;
-	struct atmel_nand_host *host = nand_chip->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct atmel_nand_host *host = nand_get_controller_data(nand_chip);
 	const int cap = host->pmecc_corr_cap;
 	const int num = 2 * cap + 1;
 	int sector_size = host->pmecc_sector_size;
@@ -437,8 +437,8 @@ static int pmecc_err_location(struct mtd_info *mtd)
 static void pmecc_correct_data(struct mtd_info *mtd, uint8_t *buf, uint8_t *ecc,
 		int sector_num, int extra_bytes, int err_nbr)
 {
-	struct nand_chip *nand_chip = mtd->priv;
-	struct atmel_nand_host *host = nand_chip->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct atmel_nand_host *host = nand_get_controller_data(nand_chip);
 	int i = 0;
 	int byte_pos, bit_pos, sector_size, pos;
 	uint32_t tmp;
@@ -483,8 +483,8 @@ static void pmecc_correct_data(struct mtd_info *mtd, uint8_t *buf, uint8_t *ecc,
 static int pmecc_correction(struct mtd_info *mtd, u32 pmecc_stat, uint8_t *buf,
 	u8 *ecc)
 {
-	struct nand_chip *nand_chip = mtd->priv;
-	struct atmel_nand_host *host = nand_chip->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct atmel_nand_host *host = nand_get_controller_data(nand_chip);
 	int i, err_nbr, eccbytes;
 	uint8_t *buf_pos;
 
@@ -513,7 +513,7 @@ normal_check:
 			if (err_nbr == -1) {
 				dev_err(host->dev, "PMECC: Too many errors\n");
 				mtd->ecc_stats.failed++;
-				return -EIO;
+				return -EBADMSG;
 			} else {
 				pmecc_correct_data(mtd, buf_pos, ecc, i,
 					host->pmecc_bytes_per_sector, err_nbr);
@@ -529,7 +529,7 @@ normal_check:
 static int atmel_nand_pmecc_read_page(struct mtd_info *mtd,
 	struct nand_chip *chip, uint8_t *buf, int oob_required, int page)
 {
-	struct atmel_nand_host *host = chip->priv;
+	struct atmel_nand_host *host = nand_get_controller_data(chip);
 	int eccsize = chip->ecc.size;
 	uint8_t *oob = chip->oob_poi;
 	uint32_t *eccpos = chip->ecc.layout->eccpos;
@@ -562,16 +562,16 @@ static int atmel_nand_pmecc_read_page(struct mtd_info *mtd,
 	stat = pmecc_readl(host->pmecc, isr);
 	if (stat != 0)
 		if (pmecc_correction(mtd, stat, buf, &oob[eccpos[0]]) != 0)
-			return -EIO;
+			return -EBADMSG;
 
 	return 0;
 }
 
 static int atmel_nand_pmecc_write_page(struct mtd_info *mtd,
 		struct nand_chip *chip, const uint8_t *buf,
-		int oob_required)
+		int oob_required, int page)
 {
-	struct atmel_nand_host *host = chip->priv;
+	struct atmel_nand_host *host = nand_get_controller_data(chip);
 	uint32_t *eccpos = chip->ecc.layout->eccpos;
 	int i, j;
 	int timeout = PMECC_MAX_TIMEOUT_US;
@@ -615,8 +615,8 @@ out:
 
 static void atmel_pmecc_core_init(struct mtd_info *mtd)
 {
-	struct nand_chip *nand_chip = mtd->priv;
-	struct atmel_nand_host *host = nand_chip->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct atmel_nand_host *host = nand_get_controller_data(nand_chip);
 	uint32_t val = 0;
 	struct nand_ecclayout *ecc_layout;
 
@@ -808,7 +808,8 @@ static int atmel_pmecc_nand_init_params(struct nand_chip *nand,
 	struct atmel_nand_host *host;
 	int cap, sector_size;
 
-	host = nand->priv = &pmecc_host;
+	host = &pmecc_host;
+	nand_set_controller_data(nand, host);
 
 	nand->ecc.mode = NAND_ECC_HW;
 	nand->ecc.calculate = NULL;
@@ -1080,7 +1081,7 @@ static int atmel_nand_read_page(struct mtd_info *mtd, struct nand_chip *chip,
 static int atmel_nand_correct(struct mtd_info *mtd, u_char *dat,
 		u_char *read_ecc, u_char *isnull)
 {
-	struct nand_chip *nand_chip = mtd->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
 	unsigned int ecc_status;
 	unsigned int ecc_word, ecc_bit;
 
@@ -1111,7 +1112,7 @@ static int atmel_nand_correct(struct mtd_info *mtd, u_char *dat,
 		 * We can't correct so many errors */
 		dev_warn(host->dev, "atmel_nand : multiple errors detected."
 				" Unable to correct.\n");
-		return -EIO;
+		return -EBADMSG;
 	}
 
 	/* if there's a single bit error : we can correct it */
@@ -1207,7 +1208,7 @@ int atmel_hwecc_nand_init_param(struct nand_chip *nand, struct mtd_info *mtd)
 static void at91_nand_hwcontrol(struct mtd_info *mtd,
 					 int cmd, unsigned int ctrl)
 {
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 
 	if (ctrl & NAND_CTRL_CHANGE) {
 		ulong IO_ADDR_W = (ulong) this->IO_ADDR_W;
@@ -1238,17 +1239,17 @@ static int at91_nand_ready(struct mtd_info *mtd)
 
 #ifdef CONFIG_SPL_BUILD
 /* The following code is for SPL */
-static nand_info_t mtd;
+static struct mtd_info *mtd;
 static struct nand_chip nand_chip;
 
 static int nand_command(int block, int page, uint32_t offs, u8 cmd)
 {
-	struct nand_chip *this = mtd.priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 	int page_addr = page + block * CONFIG_SYS_NAND_PAGE_COUNT;
 	void (*hwctrl)(struct mtd_info *mtd, int cmd,
 			unsigned int ctrl) = this->cmd_ctrl;
 
-	while (!this->dev_ready(&mtd))
+	while (!this->dev_ready(mtd))
 		;
 
 	if (cmd == NAND_CMD_READOOB) {
@@ -1256,24 +1257,24 @@ static int nand_command(int block, int page, uint32_t offs, u8 cmd)
 		cmd = NAND_CMD_READ0;
 	}
 
-	hwctrl(&mtd, cmd, NAND_CTRL_CLE | NAND_CTRL_CHANGE);
+	hwctrl(mtd, cmd, NAND_CTRL_CLE | NAND_CTRL_CHANGE);
 
 	if ((this->options & NAND_BUSWIDTH_16) && !nand_opcode_8bits(cmd))
 		offs >>= 1;
 
-	hwctrl(&mtd, offs & 0xff, NAND_CTRL_ALE | NAND_CTRL_CHANGE);
-	hwctrl(&mtd, (offs >> 8) & 0xff, NAND_CTRL_ALE);
-	hwctrl(&mtd, (page_addr & 0xff), NAND_CTRL_ALE);
-	hwctrl(&mtd, ((page_addr >> 8) & 0xff), NAND_CTRL_ALE);
+	hwctrl(mtd, offs & 0xff, NAND_CTRL_ALE | NAND_CTRL_CHANGE);
+	hwctrl(mtd, (offs >> 8) & 0xff, NAND_CTRL_ALE);
+	hwctrl(mtd, (page_addr & 0xff), NAND_CTRL_ALE);
+	hwctrl(mtd, ((page_addr >> 8) & 0xff), NAND_CTRL_ALE);
 #ifdef CONFIG_SYS_NAND_5_ADDR_CYCLE
-	hwctrl(&mtd, (page_addr >> 16) & 0x0f, NAND_CTRL_ALE);
+	hwctrl(mtd, (page_addr >> 16) & 0x0f, NAND_CTRL_ALE);
 #endif
-	hwctrl(&mtd, NAND_CMD_NONE, NAND_NCE | NAND_CTRL_CHANGE);
+	hwctrl(mtd, NAND_CMD_NONE, NAND_NCE | NAND_CTRL_CHANGE);
 
-	hwctrl(&mtd, NAND_CMD_READSTART, NAND_CTRL_CLE | NAND_CTRL_CHANGE);
-	hwctrl(&mtd, NAND_CMD_NONE, NAND_NCE | NAND_CTRL_CHANGE);
+	hwctrl(mtd, NAND_CMD_READSTART, NAND_CTRL_CLE | NAND_CTRL_CHANGE);
+	hwctrl(mtd, NAND_CMD_NONE, NAND_NCE | NAND_CTRL_CHANGE);
 
-	while (!this->dev_ready(&mtd))
+	while (!this->dev_ready(mtd))
 		;
 
 	return 0;
@@ -1281,7 +1282,7 @@ static int nand_command(int block, int page, uint32_t offs, u8 cmd)
 
 static int nand_is_bad_block(int block)
 {
-	struct nand_chip *this = mtd.priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 
 	nand_command(block, 0, CONFIG_SYS_NAND_BAD_BLOCK_POS, NAND_CMD_READOOB);
 
@@ -1304,7 +1305,7 @@ static int nand_ecc_pos[] = CONFIG_SYS_NAND_ECCPOS;
 
 static int nand_read_page(int block, int page, void *dst)
 {
-	struct nand_chip *this = mtd.priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 	u_char ecc_calc[ECCTOTAL];
 	u_char ecc_code[ECCTOTAL];
 	u_char oob_data[CONFIG_SYS_NAND_OOBSIZE];
@@ -1317,11 +1318,11 @@ static int nand_read_page(int block, int page, void *dst)
 
 	for (i = 0; eccsteps; eccsteps--, i += eccbytes, p += eccsize) {
 		if (this->ecc.mode != NAND_ECC_SOFT)
-			this->ecc.hwctl(&mtd, NAND_ECC_READ);
-		this->read_buf(&mtd, p, eccsize);
-		this->ecc.calculate(&mtd, p, &ecc_calc[i]);
+			this->ecc.hwctl(mtd, NAND_ECC_READ);
+		this->read_buf(mtd, p, eccsize);
+		this->ecc.calculate(mtd, p, &ecc_calc[i]);
 	}
-	this->read_buf(&mtd, oob_data, CONFIG_SYS_NAND_OOBSIZE);
+	this->read_buf(mtd, oob_data, CONFIG_SYS_NAND_OOBSIZE);
 
 	for (i = 0; i < ECCTOTAL; i++)
 		ecc_code[i] = oob_data[nand_ecc_pos[i]];
@@ -1330,35 +1331,35 @@ static int nand_read_page(int block, int page, void *dst)
 	p = dst;
 
 	for (i = 0; eccsteps; eccsteps--, i += eccbytes, p += eccsize)
-		this->ecc.correct(&mtd, p, &ecc_code[i], &ecc_calc[i]);
+		this->ecc.correct(mtd, p, &ecc_code[i], &ecc_calc[i]);
 
 	return 0;
 }
 
 int spl_nand_erase_one(int block, int page)
 {
-	struct nand_chip *this = mtd.priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 	void (*hwctrl)(struct mtd_info *mtd, int cmd,
 			unsigned int ctrl) = this->cmd_ctrl;
 	int page_addr;
 
 	if (nand_chip.select_chip)
-		nand_chip.select_chip(&mtd, 0);
+		nand_chip.select_chip(mtd, 0);
 
 	page_addr = page + block * CONFIG_SYS_NAND_PAGE_COUNT;
-	hwctrl(&mtd, NAND_CMD_ERASE1, NAND_CTRL_CLE | NAND_CTRL_CHANGE);
+	hwctrl(mtd, NAND_CMD_ERASE1, NAND_CTRL_CLE | NAND_CTRL_CHANGE);
 	/* Row address */
-	hwctrl(&mtd, (page_addr & 0xff), NAND_CTRL_ALE | NAND_CTRL_CHANGE);
-	hwctrl(&mtd, ((page_addr >> 8) & 0xff),
+	hwctrl(mtd, (page_addr & 0xff), NAND_CTRL_ALE | NAND_CTRL_CHANGE);
+	hwctrl(mtd, ((page_addr >> 8) & 0xff),
 	       NAND_CTRL_ALE | NAND_CTRL_CHANGE);
 #ifdef CONFIG_SYS_NAND_5_ADDR_CYCLE
 	/* One more address cycle for devices > 128MiB */
-	hwctrl(&mtd, (page_addr >> 16) & 0x0f,
+	hwctrl(mtd, (page_addr >> 16) & 0x0f,
 	       NAND_CTRL_ALE | NAND_CTRL_CHANGE);
 #endif
-	hwctrl(&mtd, NAND_CMD_ERASE2, NAND_CTRL_CLE | NAND_CTRL_CHANGE);
+	hwctrl(mtd, NAND_CMD_ERASE2, NAND_CTRL_CLE | NAND_CTRL_CHANGE);
 
-	while (!this->dev_ready(&mtd))
+	while (!this->dev_ready(mtd))
 		;
 
 	nand_deselect();
@@ -1368,10 +1369,10 @@ int spl_nand_erase_one(int block, int page)
 #else
 static int nand_read_page(int block, int page, void *dst)
 {
-	struct nand_chip *this = mtd.priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 
 	nand_command(block, page, 0, NAND_CMD_READ0);
-	atmel_nand_pmecc_read_page(&mtd, this, dst, 0, page);
+	atmel_nand_pmecc_read_page(mtd, this, dst, 0, page);
 
 	return 0;
 }
@@ -1407,7 +1408,7 @@ int nand_spl_load_image(uint32_t offs, unsigned int size, void *dst)
 
 int at91_nand_wait_ready(struct mtd_info *mtd)
 {
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 
 	udelay(this->chip_delay);
 
@@ -1438,7 +1439,7 @@ int board_nand_init(struct nand_chip *nand)
 
 #ifdef CONFIG_ATMEL_NAND_HWECC
 #ifdef CONFIG_ATMEL_NAND_HW_PMECC
-	ret = atmel_pmecc_nand_init_params(nand, &mtd);
+	ret = atmel_pmecc_nand_init_params(nand, mtd);
 #endif
 #endif
 
@@ -1447,9 +1448,9 @@ int board_nand_init(struct nand_chip *nand)
 
 void nand_init(void)
 {
-	mtd.writesize = CONFIG_SYS_NAND_PAGE_SIZE;
-	mtd.oobsize = CONFIG_SYS_NAND_OOBSIZE;
-	mtd.priv = &nand_chip;
+	mtd = &nand_chip.mtd;
+	mtd->writesize = CONFIG_SYS_NAND_PAGE_SIZE;
+	mtd->oobsize = CONFIG_SYS_NAND_OOBSIZE;
 	nand_chip.IO_ADDR_R = (void __iomem *)CONFIG_SYS_NAND_BASE;
 	nand_chip.IO_ADDR_W = (void __iomem *)CONFIG_SYS_NAND_BASE;
 	board_nand_init(&nand_chip);
@@ -1462,13 +1463,13 @@ void nand_init(void)
 #endif
 
 	if (nand_chip.select_chip)
-		nand_chip.select_chip(&mtd, 0);
+		nand_chip.select_chip(mtd, 0);
 }
 
 void nand_deselect(void)
 {
 	if (nand_chip.select_chip)
-		nand_chip.select_chip(&mtd, -1);
+		nand_chip.select_chip(mtd, -1);
 }
 
 #else
@@ -1482,10 +1483,9 @@ static ulong base_addr[CONFIG_SYS_MAX_NAND_DEVICE] = CONFIG_SYS_NAND_BASE_LIST;
 int atmel_nand_chip_init(int devnum, ulong base_addr)
 {
 	int ret;
-	struct mtd_info *mtd = &nand_info[devnum];
 	struct nand_chip *nand = &nand_chip[devnum];
+	struct mtd_info *mtd = nand_to_mtd(nand);
 
-	mtd->priv = nand;
 	nand->IO_ADDR_R = nand->IO_ADDR_W = (void  __iomem *)base_addr;
 
 #ifdef CONFIG_NAND_ECC_BCH
@@ -1521,7 +1521,7 @@ int atmel_nand_chip_init(int devnum, ulong base_addr)
 
 	ret = nand_scan_tail(mtd);
 	if (!ret)
-		nand_register(devnum);
+		nand_register(devnum, mtd);
 
 	return ret;
 }
diff --git a/drivers/mtd/nand/davinci_nand.c b/drivers/mtd/nand/davinci_nand.c
index a397074..48a8ca7 100644
--- a/drivers/mtd/nand/davinci_nand.c
+++ b/drivers/mtd/nand/davinci_nand.c
@@ -54,7 +54,7 @@
  */
 static void nand_davinci_read_buf(struct mtd_info *mtd, uint8_t *buf, int len)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	const u32 *nand = chip->IO_ADDR_R;
 
 	/* Make sure that buf is 32 bit aligned */
@@ -99,7 +99,7 @@ static void nand_davinci_read_buf(struct mtd_info *mtd, uint8_t *buf, int len)
 static void nand_davinci_write_buf(struct mtd_info *mtd, const uint8_t *buf,
 				   int len)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	const u32 *nand = chip->IO_ADDR_W;
 
 	/* Make sure that buf is 32 bit aligned */
@@ -144,7 +144,7 @@ static void nand_davinci_write_buf(struct mtd_info *mtd, const uint8_t *buf,
 static void nand_davinci_hwcontrol(struct mtd_info *mtd, int cmd,
 		unsigned int ctrl)
 {
-	struct		nand_chip *this = mtd->priv;
+	struct		nand_chip *this = mtd_to_nand(mtd);
 	u_int32_t	IO_ADDR_W = (u_int32_t)this->IO_ADDR_W;
 
 	if (ctrl & NAND_CTRL_CHANGE) {
@@ -223,7 +223,7 @@ static int nand_davinci_calculate_ecc(struct mtd_info *mtd, const u_char *dat,
 static int nand_davinci_correct_data(struct mtd_info *mtd, u_char *dat,
 		u_char *read_ecc, u_char *calc_ecc)
 {
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 	u_int32_t ecc_nand = read_ecc[0] | (read_ecc[1] << 8) |
 					  (read_ecc[2] << 16);
 	u_int32_t ecc_calc = calc_ecc[0] | (calc_ecc[1] << 8) |
@@ -243,7 +243,7 @@ static int nand_davinci_correct_data(struct mtd_info *mtd, u_char *dat,
 					 "%d\n", find_byte, find_bit);
 				return 1;
 			} else {
-				return -1;
+				return -EBADMSG;
 			}
 		} else if (!(diff & (diff - 1))) {
 			/* Single bit ECC error in the ECC itself,
@@ -254,7 +254,7 @@ static int nand_davinci_correct_data(struct mtd_info *mtd, u_char *dat,
 		} else {
 			/* Uncorrectable error */
 			MTDDEBUG(MTD_DEBUG_LEVEL0, "ECC UNCORRECTED_ERROR 1\n");
-			return -1;
+			return -EBADMSG;
 		}
 	}
 	return 0;
@@ -380,10 +380,13 @@ static int nand_davinci_write_page(struct mtd_info *mtd, struct nand_chip *chip,
 
 	chip->cmdfunc(mtd, NAND_CMD_SEQIN, 0x00, page);
 
-	if (unlikely(raw))
-		status = chip->ecc.write_page_raw(mtd, chip, buf, oob_required);
-	else
-		status = chip->ecc.write_page(mtd, chip, buf, oob_required);
+	if (unlikely(raw)) {
+		status = chip->ecc.write_page_raw(mtd, chip, buf,
+						  oob_required, page);
+	} else {
+		status = chip->ecc.write_page(mtd, chip, buf,
+					      oob_required, page);
+	}
 
 	if (status < 0) {
 		ret = status;
@@ -698,7 +701,7 @@ static int nand_davinci_4bit_correct_data(struct mtd_info *mtd, uint8_t *dat,
 		return 0;
 	} else if (iserror == ECC_STATE_TOO_MANY_ERRS) {
 		val = __raw_readl(&davinci_emif_regs->nanderrval1);
-		return -1;
+		return -EBADMSG;
 	}
 
 	numerrors = ((__raw_readl(&davinci_emif_regs->nandfsr) >> 16)
diff --git a/drivers/mtd/nand/denali.c b/drivers/mtd/nand/denali.c
index 5894fcc..601e744 100644
--- a/drivers/mtd/nand/denali.c
+++ b/drivers/mtd/nand/denali.c
@@ -48,7 +48,10 @@ static int onfi_timing_mode = NAND_DEFAULT_TIMINGS;
  * this macro allows us to convert from an MTD structure to our own
  * device context (denali) structure.
  */
-#define mtd_to_denali(m) container_of(m->priv, struct denali_nand_info, nand)
+static inline struct denali_nand_info *mtd_to_denali(struct mtd_info *mtd)
+{
+	return container_of(mtd_to_nand(mtd), struct denali_nand_info, nand);
+}
 
 /*
  * These constants are defined by the driver to enable common driver
@@ -865,7 +868,7 @@ static int write_page(struct mtd_info *mtd, struct nand_chip *chip,
  * by write_page above.
  */
 static int denali_write_page(struct mtd_info *mtd, struct nand_chip *chip,
-				const uint8_t *buf, int oob_required)
+				const uint8_t *buf, int oob_required, int page)
 {
 	struct denali_nand_info *denali = mtd_to_denali(mtd);
 
@@ -889,7 +892,8 @@ static int denali_write_page(struct mtd_info *mtd, struct nand_chip *chip,
  * write_page() function above.
  */
 static int denali_write_page_raw(struct mtd_info *mtd, struct nand_chip *chip,
-					const uint8_t *buf, int oob_required)
+				 const uint8_t *buf, int oob_required,
+				 int page)
 {
 	struct denali_nand_info *denali = mtd_to_denali(mtd);
 
@@ -988,7 +992,7 @@ static int denali_read_page(struct mtd_info *mtd, struct nand_chip *chip,
 			debug("  ECC error cause by erased block\n");
 			/* false alarm, return the 0xFF */
 		} else {
-			return -EIO;
+			return -EBADMSG;
 		}
 	}
 	memcpy(buf, denali->buf.dma_buf, mtd->writesize);
@@ -1173,13 +1177,13 @@ static struct nand_ecclayout nand_oob;
 
 static int denali_init(struct denali_nand_info *denali)
 {
+	struct mtd_info *mtd = nand_to_mtd(&denali->nand);
 	int ret;
 
 	denali_hw_init(denali);
 
-	denali->mtd->name = "denali-nand";
-	denali->mtd->owner = THIS_MODULE;
-	denali->mtd->priv = &denali->nand;
+	mtd->name = "denali-nand";
+	mtd->owner = THIS_MODULE;
 
 	/* register the driver with the NAND core subsystem */
 	denali->nand.select_chip = denali_select_chip;
@@ -1193,7 +1197,7 @@ static int denali_init(struct denali_nand_info *denali)
 	 * this is the first stage in a two step process to register
 	 * with the nand subsystem
 	 */
-	if (nand_scan_ident(denali->mtd, denali->max_banks, NULL)) {
+	if (nand_scan_ident(mtd, denali->max_banks, NULL)) {
 		ret = -ENXIO;
 		goto fail;
 	}
@@ -1239,13 +1243,13 @@ static int denali_init(struct denali_nand_info *denali)
 	nand_oob.eccbytes = denali->nand.ecc.bytes;
 	denali->nand.ecc.layout = &nand_oob;
 
-	writel(denali->mtd->erasesize / denali->mtd->writesize,
+	writel(mtd->erasesize / mtd->writesize,
 	       denali->flash_reg + PAGES_PER_BLOCK);
 	writel(denali->nand.options & NAND_BUSWIDTH_16 ? 1 : 0,
 	       denali->flash_reg + DEVICE_WIDTH);
-	writel(denali->mtd->writesize,
+	writel(mtd->writesize,
 	       denali->flash_reg + DEVICE_MAIN_AREA_SIZE);
-	writel(denali->mtd->oobsize,
+	writel(mtd->oobsize,
 	       denali->flash_reg + DEVICE_SPARE_AREA_SIZE);
 	if (readl(denali->flash_reg + DEVICES_CONNECTED) == 0)
 		writel(1, denali->flash_reg + DEVICES_CONNECTED);
@@ -1258,12 +1262,12 @@ static int denali_init(struct denali_nand_info *denali)
 	denali->nand.ecc.read_oob = denali_read_oob;
 	denali->nand.ecc.write_oob = denali_write_oob;
 
-	if (nand_scan_tail(denali->mtd)) {
+	if (nand_scan_tail(mtd)) {
 		ret = -ENXIO;
 		goto fail;
 	}
 
-	ret = nand_register(0);
+	ret = nand_register(0, mtd);
 
 fail:
 	return ret;
@@ -1278,13 +1282,6 @@ static int __board_nand_init(void)
 		return -ENOMEM;
 
 	/*
-	 * If CONFIG_SYS_NAND_SELF_INIT is defined, each driver is responsible
-	 * for instantiating struct nand_chip, while drivers/mtd/nand/nand.c
-	 * still provides a "struct mtd_info nand_info" instance.
-	 */
-	denali->mtd = &nand_info[0];
-
-	/*
 	 * In the future, these base addresses should be taken from
 	 * Device Tree or platform data.
 	 */
diff --git a/drivers/mtd/nand/denali.h b/drivers/mtd/nand/denali.h
index db1457a..0e098bd 100644
--- a/drivers/mtd/nand/denali.h
+++ b/drivers/mtd/nand/denali.h
@@ -436,7 +436,6 @@ struct nand_buf {
 #define DT		3
 
 struct denali_nand_info {
-	struct mtd_info *mtd;
 	struct nand_chip nand;
 	int flash_bank; /* currently selected chip */
 	int status;
diff --git a/drivers/mtd/nand/denali_spl.c b/drivers/mtd/nand/denali_spl.c
index 1587413..c693032 100644
--- a/drivers/mtd/nand/denali_spl.c
+++ b/drivers/mtd/nand/denali_spl.c
@@ -41,7 +41,7 @@ static int wait_for_irq(uint32_t irq_mask)
 
 		if (intr_status & INTR_STATUS__ECC_UNCOR_ERR) {
 			debug("Uncorrected ECC detected\n");
-			return -EIO;
+			return -EBADMSG;
 		}
 
 		if (intr_status & irq_mask)
diff --git a/drivers/mtd/nand/docg4.c b/drivers/mtd/nand/docg4.c
deleted file mode 100644
index c1c1ff8..0000000
--- a/drivers/mtd/nand/docg4.c
+++ /dev/null
@@ -1,1030 +0,0 @@
-/*
- * drivers/mtd/nand/docg4.c
- *
- * Copyright (C) 2013 Mike Dunn <mikedunn@newsguy.com>
- *
- * SPDX-License-Identifier:	GPL-2.0+
- *
- * mtd nand driver for M-Systems DiskOnChip G4
- *
- * Tested on the Palm Treo 680.  The G4 is also present on Toshiba Portege, Asus
- * P526, some HTC smartphones (Wizard, Prophet, ...), O2 XDA Zinc, maybe others.
- * Should work on these as well.  Let me know!
- *
- * TODO:
- *
- *  Mechanism for management of password-protected areas
- *
- *  Hamming ecc when reading oob only
- *
- *  According to the M-Sys documentation, this device is also available in a
- *  "dual-die" configuration having a 256MB capacity, but no mechanism for
- *  detecting this variant is documented.  Currently this driver assumes 128MB
- *  capacity.
- *
- *  Support for multiple cascaded devices ("floors").  Not sure which gadgets
- *  contain multiple G4s in a cascaded configuration, if any.
- */
-
-
-#include <common.h>
-#include <asm/arch/hardware.h>
-#include <asm/io.h>
-#include <asm/bitops.h>
-#include <asm/errno.h>
-#include <malloc.h>
-#include <nand.h>
-#include <linux/bch.h>
-#include <linux/bitrev.h>
-#include <linux/mtd/docg4.h>
-
-/*
- * The device has a nop register which M-Sys claims is for the purpose of
- * inserting precise delays.  But beware; at least some operations fail if the
- * nop writes are replaced with a generic delay!
- */
-static inline void write_nop(void __iomem *docptr)
-{
-	writew(0, docptr + DOC_NOP);
-}
-
-
-static int poll_status(void __iomem *docptr)
-{
-	/*
-	 * Busy-wait for the FLASHREADY bit to be set in the FLASHCONTROL
-	 * register.  Operations known to take a long time (e.g., block erase)
-	 * should sleep for a while before calling this.
-	 */
-
-	uint8_t flash_status;
-
-	/* hardware quirk requires reading twice initially */
-	flash_status = readb(docptr + DOC_FLASHCONTROL);
-
-	do {
-		flash_status = readb(docptr + DOC_FLASHCONTROL);
-	} while (!(flash_status & DOC_CTRL_FLASHREADY));
-
-	return 0;
-}
-
-static void write_addr(void __iomem *docptr, uint32_t docg4_addr)
-{
-	/* write the four address bytes packed in docg4_addr to the device */
-
-	writeb(docg4_addr & 0xff, docptr + DOC_FLASHADDRESS);
-	docg4_addr >>= 8;
-	writeb(docg4_addr & 0xff, docptr + DOC_FLASHADDRESS);
-	docg4_addr >>= 8;
-	writeb(docg4_addr & 0xff, docptr + DOC_FLASHADDRESS);
-	docg4_addr >>= 8;
-	writeb(docg4_addr & 0xff, docptr + DOC_FLASHADDRESS);
-}
-
-/*
- * This is a module parameter in the linux kernel version of this driver.  It is
- * hard-coded to 'off' for u-boot.  This driver uses oob to mark bad blocks.
- * This can be problematic when dealing with data not intended for the mtd/nand
- * subsystem.  For example, on boards that boot from the docg4 and use the IPL
- * to load an spl + u-boot image, the blocks containing the image will be
- * reported as "bad" because the oob of the first page of each block contains a
- * magic number that the IPL looks for, which causes the badblock scan to
- * erroneously add them to the bad block table.  To erase such a block, use
- * u-boot's 'nand scrub'.  scrub is safe for the docg4.  The device does have a
- * factory bad block table, but it is read-only, and is used in conjunction with
- * oob bad block markers that are written by mtd/nand when a block is deemed to
- * be bad.  To read data from "bad" blocks, use 'read.raw'.  Unfortunately,
- * read.raw does not use ecc, which would still work fine on such misidentified
- * bad blocks.  TODO: u-boot nand utilities need the ability to ignore bad
- * blocks.
- */
-static const int ignore_badblocks; /* remains false */
-
-struct docg4_priv {
-	int status;
-	struct {
-		unsigned int command;
-		int column;
-		int page;
-	} last_command;
-	uint8_t oob_buf[16];
-	uint8_t ecc_buf[7];
-	int oob_page;
-	struct bch_control *bch;
-};
-/*
- * Oob bytes 0 - 6 are available to the user.
- * Byte 7 is hamming ecc for first 7 bytes.  Bytes 8 - 14 are hw-generated ecc.
- * Byte 15 (the last) is used by the driver as a "page written" flag.
- */
-static struct nand_ecclayout docg4_oobinfo = {
-	.eccbytes = 9,
-	.eccpos = {7, 8, 9, 10, 11, 12, 13, 14, 15},
-	.oobavail = 7,
-	.oobfree = { {0, 7} }
-};
-
-static void reset(void __iomem *docptr)
-{
-	/* full device reset */
-
-	writew(DOC_ASICMODE_RESET | DOC_ASICMODE_MDWREN, docptr + DOC_ASICMODE);
-	writew(~(DOC_ASICMODE_RESET | DOC_ASICMODE_MDWREN),
-	       docptr + DOC_ASICMODECONFIRM);
-	write_nop(docptr);
-
-	writew(DOC_ASICMODE_NORMAL | DOC_ASICMODE_MDWREN,
-	       docptr + DOC_ASICMODE);
-	writew(~(DOC_ASICMODE_NORMAL | DOC_ASICMODE_MDWREN),
-	       docptr + DOC_ASICMODECONFIRM);
-
-	writew(DOC_ECCCONF1_ECC_ENABLE, docptr + DOC_ECCCONF1);
-
-	poll_status(docptr);
-}
-
-static void docg4_select_chip(struct mtd_info *mtd, int chip)
-{
-	/*
-	 * Select among multiple cascaded chips ("floors").  Multiple floors are
-	 * not yet supported, so the only valid non-negative value is 0.
-	 */
-	void __iomem *docptr = CONFIG_SYS_NAND_BASE;
-
-	if (chip < 0)
-		return;		/* deselected */
-
-	if (chip > 0)
-		printf("multiple floors currently unsupported\n");
-
-	writew(0, docptr + DOC_DEVICESELECT);
-}
-
-static void read_hw_ecc(void __iomem *docptr, uint8_t *ecc_buf)
-{
-	/* read the 7 hw-generated ecc bytes */
-
-	int i;
-	for (i = 0; i < 7; i++) { /* hw quirk; read twice */
-		ecc_buf[i] = readb(docptr + DOC_BCH_SYNDROM(i));
-		ecc_buf[i] = readb(docptr + DOC_BCH_SYNDROM(i));
-	}
-}
-
-static int correct_data(struct mtd_info *mtd, uint8_t *buf, int page)
-{
-	/*
-	 * Called after a page read when hardware reports bitflips.
-	 * Up to four bitflips can be corrected.
-	 */
-
-	struct nand_chip *nand = mtd->priv;
-	struct docg4_priv *doc = nand->priv;
-	void __iomem *docptr = CONFIG_SYS_NAND_BASE;
-	int i, numerrs;
-	unsigned int errpos[4];
-	const uint8_t blank_read_hwecc[8] = {
-		0xcf, 0x72, 0xfc, 0x1b, 0xa9, 0xc7, 0xb9, 0 };
-
-	read_hw_ecc(docptr, doc->ecc_buf); /* read 7 hw-generated ecc bytes */
-
-	/* check if read error is due to a blank page */
-	if (!memcmp(doc->ecc_buf, blank_read_hwecc, 7))
-		return 0;	/* yes */
-
-	/* skip additional check of "written flag" if ignore_badblocks */
-	if (!ignore_badblocks) {
-		/*
-		 * If the hw ecc bytes are not those of a blank page, there's
-		 * still a chance that the page is blank, but was read with
-		 * errors.  Check the "written flag" in last oob byte, which
-		 * is set to zero when a page is written.  If more than half
-		 * the bits are set, assume a blank page.  Unfortunately, the
-		 * bit flips(s) are not reported in stats.
-		 */
-
-		if (doc->oob_buf[15]) {
-			int bit, numsetbits = 0;
-			unsigned long written_flag = doc->oob_buf[15];
-
-			for (bit = 0; bit < 8; bit++) {
-				if (written_flag & 0x01)
-					numsetbits++;
-				written_flag >>= 1;
-			}
-			if (numsetbits > 4) { /* assume blank */
-				printf("errors in blank page at offset %08x\n",
-				       page * DOCG4_PAGE_SIZE);
-				return 0;
-			}
-		}
-	}
-
-	/*
-	 * The hardware ecc unit produces oob_ecc ^ calc_ecc.  The kernel's bch
-	 * algorithm is used to decode this.  However the hw operates on page
-	 * data in a bit order that is the reverse of that of the bch alg,
-	 * requiring that the bits be reversed on the result.  Thanks to Ivan
-	 * Djelic for his analysis!
-	 */
-	for (i = 0; i < 7; i++)
-		doc->ecc_buf[i] = bitrev8(doc->ecc_buf[i]);
-
-	numerrs = decode_bch(doc->bch, NULL, DOCG4_USERDATA_LEN, NULL,
-			     doc->ecc_buf, NULL, errpos);
-
-	if (numerrs == -EBADMSG) {
-		printf("uncorrectable errors at offset %08x\n",
-		       page * DOCG4_PAGE_SIZE);
-		return -EBADMSG;
-	}
-
-	BUG_ON(numerrs < 0);	/* -EINVAL, or anything other than -EBADMSG */
-
-	/* undo last step in BCH alg (modulo mirroring not needed) */
-	for (i = 0; i < numerrs; i++)
-		errpos[i] = (errpos[i] & ~7)|(7-(errpos[i] & 7));
-
-	/* fix the errors */
-	for (i = 0; i < numerrs; i++) {
-		/* ignore if error within oob ecc bytes */
-		if (errpos[i] > DOCG4_USERDATA_LEN * 8)
-			continue;
-
-		/* if error within oob area preceeding ecc bytes... */
-		if (errpos[i] > DOCG4_PAGE_SIZE * 8)
-			__change_bit(errpos[i] - DOCG4_PAGE_SIZE * 8,
-				     (unsigned long *)doc->oob_buf);
-
-		else    /* error in page data */
-			__change_bit(errpos[i], (unsigned long *)buf);
-	}
-
-	printf("%d error(s) corrected at offset %08x\n",
-	       numerrs, page * DOCG4_PAGE_SIZE);
-
-	return numerrs;
-}
-
-static int read_progstatus(struct docg4_priv *doc, void __iomem *docptr)
-{
-	/*
-	 * This apparently checks the status of programming.  Done after an
-	 * erasure, and after page data is written.  On error, the status is
-	 * saved, to be later retrieved by the nand infrastructure code.
-	 */
-
-	/* status is read from the I/O reg */
-	uint16_t status1 = readw(docptr + DOC_IOSPACE_DATA);
-	uint16_t status2 = readw(docptr + DOC_IOSPACE_DATA);
-	uint16_t status3 = readw(docptr + DOCG4_MYSTERY_REG);
-
-	MTDDEBUG(MTD_DEBUG_LEVEL3, "docg4: %s: %02x %02x %02x\n",
-	    __func__, status1, status2, status3);
-
-	if (status1 != DOCG4_PROGSTATUS_GOOD ||
-	    status2 != DOCG4_PROGSTATUS_GOOD_2 ||
-	    status3 != DOCG4_PROGSTATUS_GOOD_2) {
-		doc->status = NAND_STATUS_FAIL;
-		printf("read_progstatus failed: %02x, %02x, %02x\n",
-		       status1, status2, status3);
-		return -EIO;
-	}
-	return 0;
-}
-
-static int pageprog(struct mtd_info *mtd)
-{
-	/*
-	 * Final step in writing a page.  Writes the contents of its
-	 * internal buffer out to the flash array, or some such.
-	 */
-
-	struct nand_chip *nand = mtd->priv;
-	struct docg4_priv *doc = nand->priv;
-	void __iomem *docptr = CONFIG_SYS_NAND_BASE;
-	int retval = 0;
-
-	MTDDEBUG(MTD_DEBUG_LEVEL3, "docg4: %s\n", __func__);
-
-	writew(DOCG4_SEQ_PAGEPROG, docptr + DOC_FLASHSEQUENCE);
-	writew(DOC_CMD_PROG_CYCLE2, docptr + DOC_FLASHCOMMAND);
-	write_nop(docptr);
-	write_nop(docptr);
-
-	/* Just busy-wait; usleep_range() slows things down noticeably. */
-	poll_status(docptr);
-
-	writew(DOCG4_SEQ_FLUSH, docptr + DOC_FLASHSEQUENCE);
-	writew(DOCG4_CMD_FLUSH, docptr + DOC_FLASHCOMMAND);
-	writew(DOC_ECCCONF0_READ_MODE | 4, docptr + DOC_ECCCONF0);
-	write_nop(docptr);
-	write_nop(docptr);
-	write_nop(docptr);
-	write_nop(docptr);
-	write_nop(docptr);
-
-	retval = read_progstatus(doc, docptr);
-	writew(0, docptr + DOC_DATAEND);
-	write_nop(docptr);
-	poll_status(docptr);
-	write_nop(docptr);
-
-	return retval;
-}
-
-static void sequence_reset(void __iomem *docptr)
-{
-	/* common starting sequence for all operations */
-
-	writew(DOC_CTRL_UNKNOWN | DOC_CTRL_CE, docptr + DOC_FLASHCONTROL);
-	writew(DOC_SEQ_RESET, docptr + DOC_FLASHSEQUENCE);
-	writew(DOC_CMD_RESET, docptr + DOC_FLASHCOMMAND);
-	write_nop(docptr);
-	write_nop(docptr);
-	poll_status(docptr);
-	write_nop(docptr);
-}
-
-static void read_page_prologue(void __iomem *docptr, uint32_t docg4_addr)
-{
-	/* first step in reading a page */
-
-	sequence_reset(docptr);
-
-	writew(DOCG4_SEQ_PAGE_READ, docptr + DOC_FLASHSEQUENCE);
-	writew(DOCG4_CMD_PAGE_READ, docptr + DOC_FLASHCOMMAND);
-	write_nop(docptr);
-
-	write_addr(docptr, docg4_addr);
-
-	write_nop(docptr);
-	writew(DOCG4_CMD_READ2, docptr + DOC_FLASHCOMMAND);
-	write_nop(docptr);
-	write_nop(docptr);
-
-	poll_status(docptr);
-}
-
-static void write_page_prologue(void __iomem *docptr, uint32_t docg4_addr)
-{
-	/* first step in writing a page */
-
-	sequence_reset(docptr);
-	writew(DOCG4_SEQ_PAGEWRITE, docptr + DOC_FLASHSEQUENCE);
-	writew(DOCG4_CMD_PAGEWRITE, docptr + DOC_FLASHCOMMAND);
-	write_nop(docptr);
-	write_addr(docptr, docg4_addr);
-	write_nop(docptr);
-	write_nop(docptr);
-	poll_status(docptr);
-}
-
-static uint32_t mtd_to_docg4_address(int page, int column)
-{
-	/*
-	 * Convert mtd address to format used by the device, 32 bit packed.
-	 *
-	 * Some notes on G4 addressing... The M-Sys documentation on this device
-	 * claims that pages are 2K in length, and indeed, the format of the
-	 * address used by the device reflects that.  But within each page are
-	 * four 512 byte "sub-pages", each with its own oob data that is
-	 * read/written immediately after the 512 bytes of page data.  This oob
-	 * data contains the ecc bytes for the preceeding 512 bytes.
-	 *
-	 * Rather than tell the mtd nand infrastructure that page size is 2k,
-	 * with four sub-pages each, we engage in a little subterfuge and tell
-	 * the infrastructure code that pages are 512 bytes in size.  This is
-	 * done because during the course of reverse-engineering the device, I
-	 * never observed an instance where an entire 2K "page" was read or
-	 * written as a unit.  Each "sub-page" is always addressed individually,
-	 * its data read/written, and ecc handled before the next "sub-page" is
-	 * addressed.
-	 *
-	 * This requires us to convert addresses passed by the mtd nand
-	 * infrastructure code to those used by the device.
-	 *
-	 * The address that is written to the device consists of four bytes: the
-	 * first two are the 2k page number, and the second is the index into
-	 * the page.  The index is in terms of 16-bit half-words and includes
-	 * the preceeding oob data, so e.g., the index into the second
-	 * "sub-page" is 0x108, and the full device address of the start of mtd
-	 * page 0x201 is 0x00800108.
-	 */
-	int g4_page = page / 4;	                      /* device's 2K page */
-	int g4_index = (page % 4) * 0x108 + column/2; /* offset into page */
-	return (g4_page << 16) | g4_index;	      /* pack */
-}
-
-static void docg4_command(struct mtd_info *mtd, unsigned command, int column,
-			  int page_addr)
-{
-	/* handle standard nand commands */
-
-	struct nand_chip *nand = mtd->priv;
-	struct docg4_priv *doc = nand->priv;
-	uint32_t g4_addr = mtd_to_docg4_address(page_addr, column);
-
-	MTDDEBUG(MTD_DEBUG_LEVEL3, "%s %x, page_addr=%x, column=%x\n",
-	    __func__, command, page_addr, column);
-
-	/*
-	 * Save the command and its arguments.  This enables emulation of
-	 * standard flash devices, and also some optimizations.
-	 */
-	doc->last_command.command = command;
-	doc->last_command.column = column;
-	doc->last_command.page = page_addr;
-
-	switch (command) {
-	case NAND_CMD_RESET:
-		reset(CONFIG_SYS_NAND_BASE);
-		break;
-
-	case NAND_CMD_READ0:
-		read_page_prologue(CONFIG_SYS_NAND_BASE, g4_addr);
-		break;
-
-	case NAND_CMD_STATUS:
-		/* next call to read_byte() will expect a status */
-		break;
-
-	case NAND_CMD_SEQIN:
-		write_page_prologue(CONFIG_SYS_NAND_BASE, g4_addr);
-
-		/* hack for deferred write of oob bytes */
-		if (doc->oob_page == page_addr)
-			memcpy(nand->oob_poi, doc->oob_buf, 16);
-		break;
-
-	case NAND_CMD_PAGEPROG:
-		pageprog(mtd);
-		break;
-
-	/* we don't expect these, based on review of nand_base.c */
-	case NAND_CMD_READOOB:
-	case NAND_CMD_READID:
-	case NAND_CMD_ERASE1:
-	case NAND_CMD_ERASE2:
-		printf("docg4_command: unexpected nand command 0x%x\n",
-		       command);
-		break;
-	}
-}
-
-static void docg4_read_buf(struct mtd_info *mtd, uint8_t *buf, int len)
-{
-	int i;
-	struct nand_chip *nand = mtd->priv;
-	uint16_t *p = (uint16_t *)buf;
-	len >>= 1;
-
-	for (i = 0; i < len; i++)
-		p[i] = readw(nand->IO_ADDR_R);
-}
-
-static int docg4_read_oob(struct mtd_info *mtd, struct nand_chip *nand,
-			  int page)
-{
-	struct docg4_priv *doc = nand->priv;
-	void __iomem *docptr = CONFIG_SYS_NAND_BASE;
-	uint16_t status;
-
-	MTDDEBUG(MTD_DEBUG_LEVEL3, "%s: page %x\n", __func__, page);
-
-	/*
-	 * Oob bytes are read as part of a normal page read.  If the previous
-	 * nand command was a read of the page whose oob is now being read, just
-	 * copy the oob bytes that we saved in a local buffer and avoid a
-	 * separate oob read.
-	 */
-	if (doc->last_command.command == NAND_CMD_READ0 &&
-	    doc->last_command.page == page) {
-		memcpy(nand->oob_poi, doc->oob_buf, 16);
-		return 0;
-	}
-
-	/*
-	 * Separate read of oob data only.
-	 */
-	docg4_command(mtd, NAND_CMD_READ0, nand->ecc.size, page);
-
-	writew(DOC_ECCCONF0_READ_MODE | DOCG4_OOB_SIZE, docptr + DOC_ECCCONF0);
-	write_nop(docptr);
-	write_nop(docptr);
-	write_nop(docptr);
-	write_nop(docptr);
-	write_nop(docptr);
-
-	/* the 1st byte from the I/O reg is a status; the rest is oob data */
-	status = readw(docptr + DOC_IOSPACE_DATA);
-	if (status & DOCG4_READ_ERROR) {
-		printf("docg4_read_oob failed: status = 0x%02x\n", status);
-		return -EIO;
-	}
-
-	MTDDEBUG(MTD_DEBUG_LEVEL3, "%s: status = 0x%x\n", __func__, status);
-
-	docg4_read_buf(mtd, nand->oob_poi, 16);
-
-	write_nop(docptr);
-	write_nop(docptr);
-	write_nop(docptr);
-	writew(0, docptr + DOC_DATAEND);
-	write_nop(docptr);
-
-	return 0;
-}
-
-static int docg4_write_oob(struct mtd_info *mtd, struct nand_chip *nand,
-			   int page)
-{
-	/*
-	 * Writing oob-only is not really supported, because MLC nand must write
-	 * oob bytes at the same time as page data.  Nonetheless, we save the
-	 * oob buffer contents here, and then write it along with the page data
-	 * if the same page is subsequently written.  This allows user space
-	 * utilities that write the oob data prior to the page data to work
-	 * (e.g., nandwrite).  The disdvantage is that, if the intention was to
-	 * write oob only, the operation is quietly ignored.  Also, oob can get
-	 * corrupted if two concurrent processes are running nandwrite.
-	 */
-
-	/* note that bytes 7..14 are hw generated hamming/ecc and overwritten */
-	struct docg4_priv *doc = nand->priv;
-	doc->oob_page = page;
-	memcpy(doc->oob_buf, nand->oob_poi, 16);
-	return 0;
-}
-
-static int docg4_block_neverbad(struct mtd_info *mtd, loff_t ofs, int getchip)
-{
-	/* only called when module_param ignore_badblocks is set */
-	return 0;
-}
-
-static void docg4_write_buf16(struct mtd_info *mtd, const uint8_t *buf, int len)
-{
-	int i;
-	struct nand_chip *nand = mtd->priv;
-	uint16_t *p = (uint16_t *)buf;
-	len >>= 1;
-
-	for (i = 0; i < len; i++)
-		writew(p[i], nand->IO_ADDR_W);
-}
-
-static int write_page(struct mtd_info *mtd, struct nand_chip *nand,
-		       const uint8_t *buf, int use_ecc)
-{
-	void __iomem *docptr = CONFIG_SYS_NAND_BASE;
-	uint8_t ecc_buf[8];
-
-	writew(DOC_ECCCONF0_ECC_ENABLE |
-	       DOC_ECCCONF0_UNKNOWN |
-	       DOCG4_BCH_SIZE,
-	       docptr + DOC_ECCCONF0);
-	write_nop(docptr);
-
-	/* write the page data */
-	docg4_write_buf16(mtd, buf, DOCG4_PAGE_SIZE);
-
-	/* oob bytes 0 through 5 are written to I/O reg */
-	docg4_write_buf16(mtd, nand->oob_poi, 6);
-
-	/* oob byte 6 written to a separate reg */
-	writew(nand->oob_poi[6], docptr + DOCG4_OOB_6_7);
-
-	write_nop(docptr);
-	write_nop(docptr);
-
-	/* write hw-generated ecc bytes to oob */
-	if (likely(use_ecc)) {
-		/* oob byte 7 is hamming code */
-		uint8_t hamming = readb(docptr + DOC_HAMMINGPARITY);
-		hamming = readb(docptr + DOC_HAMMINGPARITY); /* 2nd read */
-		writew(hamming, docptr + DOCG4_OOB_6_7);
-		write_nop(docptr);
-
-		/* read the 7 bch bytes from ecc regs */
-		read_hw_ecc(docptr, ecc_buf);
-		ecc_buf[7] = 0;         /* clear the "page written" flag */
-	}
-
-	/* write user-supplied bytes to oob */
-	else {
-		writew(nand->oob_poi[7], docptr + DOCG4_OOB_6_7);
-		write_nop(docptr);
-		memcpy(ecc_buf, &nand->oob_poi[8], 8);
-	}
-
-	docg4_write_buf16(mtd, ecc_buf, 8);
-	write_nop(docptr);
-	write_nop(docptr);
-	writew(0, docptr + DOC_DATAEND);
-	write_nop(docptr);
-
-	return 0;
-}
-
-static int docg4_write_page_raw(struct mtd_info *mtd, struct nand_chip *nand,
-				 const uint8_t *buf, int oob_required)
-{
-	return write_page(mtd, nand, buf, 0);
-}
-
-static int docg4_write_page(struct mtd_info *mtd, struct nand_chip *nand,
-			     const uint8_t *buf, int oob_required)
-{
-	return write_page(mtd, nand, buf, 1);
-}
-
-static int read_page(struct mtd_info *mtd, struct nand_chip *nand,
-		     uint8_t *buf, int page, int use_ecc)
-{
-	struct docg4_priv *doc = nand->priv;
-	void __iomem *docptr = CONFIG_SYS_NAND_BASE;
-	uint16_t status, edc_err, *buf16;
-
-	writew(DOC_ECCCONF0_READ_MODE |
-	       DOC_ECCCONF0_ECC_ENABLE |
-	       DOC_ECCCONF0_UNKNOWN |
-	       DOCG4_BCH_SIZE,
-	       docptr + DOC_ECCCONF0);
-	write_nop(docptr);
-	write_nop(docptr);
-	write_nop(docptr);
-	write_nop(docptr);
-	write_nop(docptr);
-
-	/* the 1st byte from the I/O reg is a status; the rest is page data */
-	status = readw(docptr + DOC_IOSPACE_DATA);
-	if (status & DOCG4_READ_ERROR) {
-		printf("docg4_read_page: bad status: 0x%02x\n", status);
-		writew(0, docptr + DOC_DATAEND);
-		return -EIO;
-	}
-
-	docg4_read_buf(mtd, buf, DOCG4_PAGE_SIZE); /* read the page data */
-
-	/* first 14 oob bytes read from I/O reg */
-	docg4_read_buf(mtd, nand->oob_poi, 14);
-
-	/* last 2 read from another reg */
-	buf16 = (uint16_t *)(nand->oob_poi + 14);
-	*buf16 = readw(docptr + DOCG4_MYSTERY_REG);
-
-	/*
-	 * Diskonchips read oob immediately after a page read.  Mtd
-	 * infrastructure issues a separate command for reading oob after the
-	 * page is read.  So we save the oob bytes in a local buffer and just
-	 * copy it if the next command reads oob from the same page.
-	 */
-	memcpy(doc->oob_buf, nand->oob_poi, 16);
-
-	write_nop(docptr);
-
-	if (likely(use_ecc)) {
-		/* read the register that tells us if bitflip(s) detected  */
-		edc_err = readw(docptr + DOC_ECCCONF1);
-		edc_err = readw(docptr + DOC_ECCCONF1);
-
-		/* If bitflips are reported, attempt to correct with ecc */
-		if (edc_err & DOC_ECCCONF1_BCH_SYNDROM_ERR) {
-			int bits_corrected = correct_data(mtd, buf, page);
-			if (bits_corrected == -EBADMSG)
-				mtd->ecc_stats.failed++;
-			else
-				mtd->ecc_stats.corrected += bits_corrected;
-		}
-	}
-
-	writew(0, docptr + DOC_DATAEND);
-	return 0;
-}
-
-
-static int docg4_read_page_raw(struct mtd_info *mtd, struct nand_chip *nand,
-			       uint8_t *buf, int oob_required, int page)
-{
-	return read_page(mtd, nand, buf, page, 0);
-}
-
-static int docg4_read_page(struct mtd_info *mtd, struct nand_chip *nand,
-			   uint8_t *buf, int oob_required, int page)
-{
-	return read_page(mtd, nand, buf, page, 1);
-}
-
-static int docg4_erase_block(struct mtd_info *mtd, int page)
-{
-	struct nand_chip *nand = mtd->priv;
-	struct docg4_priv *doc = nand->priv;
-	void __iomem *docptr = CONFIG_SYS_NAND_BASE;
-	uint16_t g4_page;
-
-	MTDDEBUG(MTD_DEBUG_LEVEL3, "%s: page %04x\n", __func__, page);
-
-	sequence_reset(docptr);
-
-	writew(DOCG4_SEQ_BLOCKERASE, docptr + DOC_FLASHSEQUENCE);
-	writew(DOC_CMD_PROG_BLOCK_ADDR, docptr + DOC_FLASHCOMMAND);
-	write_nop(docptr);
-
-	/* only 2 bytes of address are written to specify erase block */
-	g4_page = (uint16_t)(page / 4);  /* to g4's 2k page addressing */
-	writeb(g4_page & 0xff, docptr + DOC_FLASHADDRESS);
-	g4_page >>= 8;
-	writeb(g4_page & 0xff, docptr + DOC_FLASHADDRESS);
-	write_nop(docptr);
-
-	/* start the erasure */
-	writew(DOC_CMD_ERASECYCLE2, docptr + DOC_FLASHCOMMAND);
-	write_nop(docptr);
-	write_nop(docptr);
-
-	poll_status(docptr);
-	writew(DOCG4_SEQ_FLUSH, docptr + DOC_FLASHSEQUENCE);
-	writew(DOCG4_CMD_FLUSH, docptr + DOC_FLASHCOMMAND);
-	writew(DOC_ECCCONF0_READ_MODE | 4, docptr + DOC_ECCCONF0);
-	write_nop(docptr);
-	write_nop(docptr);
-	write_nop(docptr);
-	write_nop(docptr);
-	write_nop(docptr);
-
-	read_progstatus(doc, docptr);
-
-	writew(0, docptr + DOC_DATAEND);
-	write_nop(docptr);
-	poll_status(docptr);
-	write_nop(docptr);
-
-	return nand->waitfunc(mtd, nand);
-}
-
-static int read_factory_bbt(struct mtd_info *mtd)
-{
-	/*
-	 * The device contains a read-only factory bad block table.  Read it and
-	 * update the memory-based bbt accordingly.
-	 */
-
-	struct nand_chip *nand = mtd->priv;
-	uint32_t g4_addr = mtd_to_docg4_address(DOCG4_FACTORY_BBT_PAGE, 0);
-	uint8_t *buf;
-	int i, block, status;
-
-	buf = kzalloc(DOCG4_PAGE_SIZE, GFP_KERNEL);
-	if (buf == NULL)
-		return -ENOMEM;
-
-	read_page_prologue(CONFIG_SYS_NAND_BASE, g4_addr);
-	status = docg4_read_page(mtd, nand, buf, 0, DOCG4_FACTORY_BBT_PAGE);
-	if (status)
-		goto exit;
-
-	/*
-	 * If no memory-based bbt was created, exit.  This will happen if module
-	 * parameter ignore_badblocks is set.  Then why even call this function?
-	 * For an unknown reason, block erase always fails if it's the first
-	 * operation after device power-up.  The above read ensures it never is.
-	 * Ugly, I know.
-	 */
-	if (nand->bbt == NULL)  /* no memory-based bbt */
-		goto exit;
-
-	/*
-	 * Parse factory bbt and update memory-based bbt.  Factory bbt format is
-	 * simple: one bit per block, block numbers increase left to right (msb
-	 * to lsb).  Bit clear means bad block.
-	 */
-	for (i = block = 0; block < DOCG4_NUMBLOCKS; block += 8, i++) {
-		int bitnum;
-		uint8_t mask;
-		for (bitnum = 0, mask = 0x80;
-		     bitnum < 8; bitnum++, mask >>= 1) {
-			if (!(buf[i] & mask)) {
-				int badblock = block + bitnum;
-				nand->bbt[badblock / 4] |=
-					0x03 << ((badblock % 4) * 2);
-				mtd->ecc_stats.badblocks++;
-				printf("factory-marked bad block: %d\n",
-				       badblock);
-			}
-		}
-	}
- exit:
-	kfree(buf);
-	return status;
-}
-
-static int docg4_block_markbad(struct mtd_info *mtd, loff_t ofs)
-{
-	/*
-	 * Mark a block as bad.  Bad blocks are marked in the oob area of the
-	 * first page of the block.  The default scan_bbt() in the nand
-	 * infrastructure code works fine for building the memory-based bbt
-	 * during initialization, as does the nand infrastructure function that
-	 * checks if a block is bad by reading the bbt.  This function replaces
-	 * the nand default because writes to oob-only are not supported.
-	 */
-
-	int ret, i;
-	uint8_t *buf;
-	struct nand_chip *nand = mtd->priv;
-	struct nand_bbt_descr *bbtd = nand->badblock_pattern;
-	int block = (int)(ofs >> nand->bbt_erase_shift);
-	int page = (int)(ofs >> nand->page_shift);
-	uint32_t g4_addr = mtd_to_docg4_address(page, 0);
-
-	MTDDEBUG(MTD_DEBUG_LEVEL3, "%s: %08llx\n", __func__, ofs);
-
-	if (unlikely(ofs & (DOCG4_BLOCK_SIZE - 1)))
-		printf("%s: ofs %llx not start of block!\n",
-		       __func__, ofs);
-
-	/* allocate blank buffer for page data */
-	buf = kzalloc(DOCG4_PAGE_SIZE, GFP_KERNEL);
-	if (buf == NULL)
-		return -ENOMEM;
-
-	/* update bbt in memory */
-	nand->bbt[block / 4] |= 0x01 << ((block & 0x03) * 2);
-
-	/* write bit-wise negation of pattern to oob buffer */
-	memset(nand->oob_poi, 0xff, mtd->oobsize);
-	for (i = 0; i < bbtd->len; i++)
-		nand->oob_poi[bbtd->offs + i] = ~bbtd->pattern[i];
-
-	/* write first page of block */
-	write_page_prologue(CONFIG_SYS_NAND_BASE, g4_addr);
-	docg4_write_page(mtd, nand, buf, 1);
-	ret = pageprog(mtd);
-	if (!ret)
-		mtd->ecc_stats.badblocks++;
-
-	kfree(buf);
-
-	return ret;
-}
-
-static uint8_t docg4_read_byte(struct mtd_info *mtd)
-{
-	struct nand_chip *nand = mtd->priv;
-	struct docg4_priv *doc = nand->priv;
-
-	MTDDEBUG(MTD_DEBUG_LEVEL3, "%s\n", __func__);
-
-	if (doc->last_command.command == NAND_CMD_STATUS) {
-		int status;
-
-		/*
-		 * Previous nand command was status request, so nand
-		 * infrastructure code expects to read the status here.  If an
-		 * error occurred in a previous operation, report it.
-		 */
-		doc->last_command.command = 0;
-
-		if (doc->status) {
-			status = doc->status;
-			doc->status = 0;
-		}
-
-		/* why is NAND_STATUS_WP inverse logic?? */
-		else
-			status = NAND_STATUS_WP | NAND_STATUS_READY;
-
-		return status;
-	}
-
-	printf("unexpectd call to read_byte()\n");
-
-	return 0;
-}
-
-static int docg4_wait(struct mtd_info *mtd, struct nand_chip *nand)
-{
-	struct docg4_priv *doc = nand->priv;
-	int status = NAND_STATUS_WP;       /* inverse logic?? */
-	MTDDEBUG(MTD_DEBUG_LEVEL3, "%s...\n", __func__);
-
-	/* report any previously unreported error */
-	if (doc->status) {
-		status |= doc->status;
-		doc->status = 0;
-		return status;
-	}
-
-	status |= poll_status(CONFIG_SYS_NAND_BASE);
-	return status;
-}
-
-int docg4_nand_init(struct mtd_info *mtd, struct nand_chip *nand, int devnum)
-{
-	uint16_t id1, id2;
-	struct docg4_priv *docg4;
-	int retval;
-
-	docg4 = kzalloc(sizeof(*docg4), GFP_KERNEL);
-	if (!docg4)
-		return -1;
-
-	mtd->priv = nand;
-	nand->priv = docg4;
-
-	/* These must be initialized here because the docg4 is non-standard
-	 * and doesn't produce an id that the nand code can use to look up
-	 * these values (nand_scan_ident() not called).
-	 */
-	mtd->size = DOCG4_CHIP_SIZE;
-	mtd->name = "Msys_Diskonchip_G4";
-	mtd->writesize = DOCG4_PAGE_SIZE;
-	mtd->erasesize = DOCG4_BLOCK_SIZE;
-	mtd->oobsize = DOCG4_OOB_SIZE;
-
-	nand->IO_ADDR_R =
-		(void __iomem *)CONFIG_SYS_NAND_BASE + DOC_IOSPACE_DATA;
-	nand->IO_ADDR_W = nand->IO_ADDR_R;
-	nand->chipsize = DOCG4_CHIP_SIZE;
-	nand->chip_shift = DOCG4_CHIP_SHIFT;
-	nand->bbt_erase_shift = DOCG4_ERASE_SHIFT;
-	nand->phys_erase_shift = DOCG4_ERASE_SHIFT;
-	nand->chip_delay = 20;
-	nand->page_shift = DOCG4_PAGE_SHIFT;
-	nand->pagemask = 0x3ffff;
-	nand->badblockpos = NAND_LARGE_BADBLOCK_POS;
-	nand->badblockbits = 8;
-	nand->ecc.layout = &docg4_oobinfo;
-	nand->ecc.mode = NAND_ECC_HW_SYNDROME;
-	nand->ecc.size = DOCG4_PAGE_SIZE;
-	nand->ecc.prepad = 8;
-	nand->ecc.bytes	= 8;
-	nand->ecc.strength = DOCG4_T;
-	nand->options = NAND_BUSWIDTH_16 | NAND_NO_SUBPAGE_WRITE;
-	nand->controller = &nand->hwcontrol;
-
-	/* methods */
-	nand->cmdfunc = docg4_command;
-	nand->waitfunc = docg4_wait;
-	nand->select_chip = docg4_select_chip;
-	nand->read_byte = docg4_read_byte;
-	nand->block_markbad = docg4_block_markbad;
-	nand->read_buf = docg4_read_buf;
-	nand->write_buf = docg4_write_buf16;
-	nand->scan_bbt = nand_default_bbt;
-	nand->erase = docg4_erase_block;
-	nand->ecc.read_page = docg4_read_page;
-	nand->ecc.write_page = docg4_write_page;
-	nand->ecc.read_page_raw = docg4_read_page_raw;
-	nand->ecc.write_page_raw = docg4_write_page_raw;
-	nand->ecc.read_oob = docg4_read_oob;
-	nand->ecc.write_oob = docg4_write_oob;
-
-	/*
-	 * The way the nand infrastructure code is written, a memory-based bbt
-	 * is not created if NAND_SKIP_BBTSCAN is set.  With no memory bbt,
-	 * nand->block_bad() is used.  So when ignoring bad blocks, we skip the
-	 * scan and define a dummy block_bad() which always returns 0.
-	 */
-	if (ignore_badblocks) {
-		nand->options |= NAND_SKIP_BBTSCAN;
-		nand->block_bad	= docg4_block_neverbad;
-	}
-
-	reset(CONFIG_SYS_NAND_BASE);
-
-	/* check for presence of g4 chip by reading id registers */
-	id1 = readw(CONFIG_SYS_NAND_BASE + DOC_CHIPID);
-	id1 = readw(CONFIG_SYS_NAND_BASE + DOCG4_MYSTERY_REG);
-	id2 = readw(CONFIG_SYS_NAND_BASE + DOC_CHIPID_INV);
-	id2 = readw(CONFIG_SYS_NAND_BASE + DOCG4_MYSTERY_REG);
-	if (id1 != DOCG4_IDREG1_VALUE || id2 != DOCG4_IDREG2_VALUE)
-		return -1;
-
-	/* initialize bch algorithm */
-	docg4->bch = init_bch(DOCG4_M, DOCG4_T, DOCG4_PRIMITIVE_POLY);
-	if (docg4->bch == NULL)
-		return -1;
-
-	retval = nand_scan_tail(mtd);
-	if (retval)
-		return -1;
-
-	/*
-	 * Scan for bad blocks and create bbt here, then add the factory-marked
-	 * bad blocks to the bbt.
-	 */
-	nand->scan_bbt(mtd);
-	nand->options |= NAND_BBT_SCANNED;
-	retval = read_factory_bbt(mtd);
-	if (retval)
-		return -1;
-
-	retval = nand_register(devnum);
-	if (retval)
-		return -1;
-
-	return 0;
-}
diff --git a/drivers/mtd/nand/docg4_spl.c b/drivers/mtd/nand/docg4_spl.c
deleted file mode 100644
index 351b75a..0000000
--- a/drivers/mtd/nand/docg4_spl.c
+++ /dev/null
@@ -1,219 +0,0 @@
-/*
- * SPL driver for Diskonchip G4 nand flash
- *
- * Copyright (C) 2013 Mike Dunn <mikedunn@newsguy.com>
- *
- * SPDX-License-Identifier:	GPL-2.0+
- *
- * This driver basically mimics the load functionality of a typical IPL (initial
- * program loader) resident in the 2k NOR-like region of the docg4 that is
- * mapped to the reset vector.  It allows the u-boot SPL to continue loading if
- * the IPL loads a fixed number of flash blocks that is insufficient to contain
- * the entire u-boot image.  In this case, a concatenated spl + u-boot image is
- * written at the flash offset from which the IPL loads an image, and when the
- * IPL jumps to the SPL, the SPL resumes loading where the IPL left off.  See
- * the palmtreo680 for an example.
- *
- * This driver assumes that the data was written to the flash using the device's
- * "reliable" mode, and also assumes that each 512 byte page is stored
- * redundantly in the subsequent page.  This storage format is likely to be used
- * by all boards that boot from the docg4.  The format compensates for the lack
- * of ecc in the IPL.
- *
- * Reliable mode reduces the capacity of a block by half, and the redundant
- * pages reduce it by half again.  As a result, the normal 256k capacity of a
- * block is reduced to 64k for the purposes of the IPL/SPL.
- */
-
-#include <asm/io.h>
-#include <linux/mtd/docg4.h>
-
-/* forward declarations */
-static inline void write_nop(void __iomem *docptr);
-static int poll_status(void __iomem *docptr);
-static void write_addr(void __iomem *docptr, uint32_t docg4_addr);
-static void address_sequence(unsigned int g4_page, unsigned int g4_index,
-			     void __iomem *docptr);
-static int docg4_load_block_reliable(uint32_t flash_offset, void *dest_addr);
-
-int nand_spl_load_image(uint32_t offs, unsigned int size, void *dst)
-{
-	void *load_addr = dst;
-	uint32_t flash_offset = offs;
-	const unsigned int block_count =
-		(size + DOCG4_BLOCK_CAPACITY_SPL - 1)
-		/ DOCG4_BLOCK_CAPACITY_SPL;
-	int i;
-
-	for (i = 0; i < block_count; i++) {
-		int ret = docg4_load_block_reliable(flash_offset, load_addr);
-		if (ret)
-			return ret;
-		load_addr += DOCG4_BLOCK_CAPACITY_SPL;
-		flash_offset += DOCG4_BLOCK_SIZE;
-	}
-	return 0;
-}
-
-static inline void write_nop(void __iomem *docptr)
-{
-	writew(0, docptr + DOC_NOP);
-}
-
-static int poll_status(void __iomem *docptr)
-{
-	/*
-	 * Busy-wait for the FLASHREADY bit to be set in the FLASHCONTROL
-	 * register.  Operations known to take a long time (e.g., block erase)
-	 * should sleep for a while before calling this.
-	 */
-
-	uint8_t flash_status;
-
-	/* hardware quirk requires reading twice initially */
-	flash_status = readb(docptr + DOC_FLASHCONTROL);
-
-	do {
-		flash_status = readb(docptr + DOC_FLASHCONTROL);
-	} while (!(flash_status & DOC_CTRL_FLASHREADY));
-
-	return 0;
-}
-
-static void write_addr(void __iomem *docptr, uint32_t docg4_addr)
-{
-	/* write the four address bytes packed in docg4_addr to the device */
-
-	writeb(docg4_addr & 0xff, docptr + DOC_FLASHADDRESS);
-	docg4_addr >>= 8;
-	writeb(docg4_addr & 0xff, docptr + DOC_FLASHADDRESS);
-	docg4_addr >>= 8;
-	writeb(docg4_addr & 0xff, docptr + DOC_FLASHADDRESS);
-	docg4_addr >>= 8;
-	writeb(docg4_addr & 0xff, docptr + DOC_FLASHADDRESS);
-}
-
-static void address_sequence(unsigned int g4_page, unsigned int g4_index,
-			     void __iomem *docptr)
-{
-	writew(DOCG4_SEQ_PAGE_READ, docptr + DOC_FLASHSEQUENCE);
-	writew(DOCG4_CMD_PAGE_READ, docptr + DOC_FLASHCOMMAND);
-	write_nop(docptr);
-	write_addr(docptr, ((uint32_t)g4_page << 16) | g4_index);
-	write_nop(docptr);
-}
-
-static int docg4_load_block_reliable(uint32_t flash_offset, void *dest_addr)
-{
-	void __iomem *docptr = (void *)CONFIG_SYS_NAND_BASE;
-	unsigned int g4_page = flash_offset >> 11; /* 2k page */
-	const unsigned int last_g4_page = g4_page + 0x80; /* last in block */
-	int g4_index = 0;
-	uint16_t flash_status;
-	uint16_t *buf;
-
-	/* flash_offset must be aligned to the start of a block */
-	if (flash_offset & 0x3ffff)
-		return -1;
-
-	writew(DOC_SEQ_RESET, docptr + DOC_FLASHSEQUENCE);
-	writew(DOC_CMD_RESET, docptr + DOC_FLASHCOMMAND);
-	write_nop(docptr);
-	write_nop(docptr);
-	poll_status(docptr);
-	write_nop(docptr);
-	writew(0x45, docptr + DOC_FLASHSEQUENCE);
-	writew(0xa3, docptr + DOC_FLASHCOMMAND);
-	write_nop(docptr);
-	writew(0x22, docptr + DOC_FLASHCOMMAND);
-	write_nop(docptr);
-
-	/* read 1st 4 oob bytes of first subpage of block */
-	address_sequence(g4_page, 0x0100, docptr); /* index at oob */
-	write_nop(docptr);
-	flash_status = readw(docptr + DOC_FLASHCONTROL);
-	flash_status = readw(docptr + DOC_FLASHCONTROL);
-	if (flash_status & 0x06) /* sequence or protection errors */
-		return -1;
-	writew(DOCG4_CMD_READ2, docptr + DOC_FLASHCOMMAND);
-	write_nop(docptr);
-	write_nop(docptr);
-	poll_status(docptr);
-	writew(DOC_ECCCONF0_READ_MODE | 4, docptr + DOC_ECCCONF0);
-	write_nop(docptr);
-	write_nop(docptr);
-	write_nop(docptr);
-	write_nop(docptr);
-	write_nop(docptr);
-
-	/*
-	 * Here we read the first four oob bytes of the first page of the block.
-	 * The IPL on the palmtreo680 requires that this contain a 32 bit magic
-	 * number, or the load aborts.  We'll ignore it.
-	 */
-	readw(docptr + 0x103c); /* hw quirk; 1st read discarded */
-	readw(docptr + 0x103c);	/* lower 16 bits of magic number */
-	readw(docptr + DOCG4_MYSTERY_REG); /* upper 16 bits of magic number */
-	writew(0, docptr + DOC_DATAEND);
-	write_nop(docptr);
-	write_nop(docptr);
-
-	/* load contents of block to memory */
-	buf = (uint16_t *)dest_addr;
-	do {
-		int i;
-
-		address_sequence(g4_page, g4_index, docptr);
-		writew(DOCG4_CMD_READ2,
-		       docptr + DOC_FLASHCOMMAND);
-		write_nop(docptr);
-		write_nop(docptr);
-		poll_status(docptr);
-		writew(DOC_ECCCONF0_READ_MODE |
-		       DOC_ECCCONF0_ECC_ENABLE |
-		       DOCG4_BCH_SIZE,
-		       docptr + DOC_ECCCONF0);
-		write_nop(docptr);
-		write_nop(docptr);
-		write_nop(docptr);
-		write_nop(docptr);
-		write_nop(docptr);
-
-		/* read the 512 bytes of page data, 2 bytes at a time */
-		readw(docptr + 0x103c); /* hw quirk */
-		for (i = 0; i < 256; i++)
-			*buf++ = readw(docptr + 0x103c);
-
-		/* read oob, but discard it */
-		for (i = 0; i < 7; i++)
-			readw(docptr + 0x103c);
-		readw(docptr + DOCG4_OOB_6_7);
-		readw(docptr + DOCG4_OOB_6_7);
-
-		writew(0, docptr + DOC_DATAEND);
-		write_nop(docptr);
-		write_nop(docptr);
-
-		if (!(g4_index & 0x100)) {
-			/* not redundant subpage read; check for ecc error */
-			write_nop(docptr);
-			flash_status = readw(docptr + DOC_ECCCONF1);
-			flash_status = readw(docptr + DOC_ECCCONF1);
-			if (flash_status & 0x80) { /* ecc error */
-				g4_index += 0x108; /* read redundant subpage */
-				buf -= 256;        /* back up ram ptr */
-				continue;
-			} else                       /* no ecc error */
-				g4_index += 0x210; /* skip redundant subpage */
-		} else  /* redundant page was just read; skip ecc error check */
-			g4_index += 0x108;
-
-		if (g4_index == 0x420) { /* finished with 2k page */
-			g4_index = 0;
-			g4_page += 2; /* odd-numbered 2k pages skipped */
-		}
-
-	} while (g4_page != last_g4_page); /* while still on same block */
-
-	return 0;
-}
diff --git a/drivers/mtd/nand/fsl_elbc_nand.c b/drivers/mtd/nand/fsl_elbc_nand.c
index d457d53..f621f14 100644
--- a/drivers/mtd/nand/fsl_elbc_nand.c
+++ b/drivers/mtd/nand/fsl_elbc_nand.c
@@ -154,8 +154,8 @@ static struct nand_bbt_descr bbt_mirror_descr = {
  */
 static void set_addr(struct mtd_info *mtd, int column, int page_addr, int oob)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct fsl_elbc_mtd *priv = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct fsl_elbc_mtd *priv = nand_get_controller_data(chip);
 	struct fsl_elbc_ctrl *ctrl = priv->ctrl;
 	fsl_lbc_t *lbc = ctrl->regs;
 	int buf_num;
@@ -194,8 +194,8 @@ static void set_addr(struct mtd_info *mtd, int column, int page_addr, int oob)
  */
 static int fsl_elbc_run_command(struct mtd_info *mtd)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct fsl_elbc_mtd *priv = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct fsl_elbc_mtd *priv = nand_get_controller_data(chip);
 	struct fsl_elbc_ctrl *ctrl = priv->ctrl;
 	fsl_lbc_t *lbc = ctrl->regs;
 	u32 timeo = (CONFIG_SYS_HZ * 10) / 1000;
@@ -246,7 +246,7 @@ static int fsl_elbc_run_command(struct mtd_info *mtd)
 
 static void fsl_elbc_do_read(struct nand_chip *chip, int oob)
 {
-	struct fsl_elbc_mtd *priv = chip->priv;
+	struct fsl_elbc_mtd *priv = nand_get_controller_data(chip);
 	struct fsl_elbc_ctrl *ctrl = priv->ctrl;
 	fsl_lbc_t *lbc = ctrl->regs;
 
@@ -279,8 +279,8 @@ static void fsl_elbc_do_read(struct nand_chip *chip, int oob)
 static void fsl_elbc_cmdfunc(struct mtd_info *mtd, unsigned int command,
 			     int column, int page_addr)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct fsl_elbc_mtd *priv = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct fsl_elbc_mtd *priv = nand_get_controller_data(chip);
 	struct fsl_elbc_ctrl *ctrl = priv->ctrl;
 	fsl_lbc_t *lbc = ctrl->regs;
 
@@ -489,8 +489,8 @@ static void fsl_elbc_select_chip(struct mtd_info *mtd, int chip)
  */
 static void fsl_elbc_write_buf(struct mtd_info *mtd, const u8 *buf, int len)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct fsl_elbc_mtd *priv = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct fsl_elbc_mtd *priv = nand_get_controller_data(chip);
 	struct fsl_elbc_ctrl *ctrl = priv->ctrl;
 	unsigned int bufsize = mtd->writesize + mtd->oobsize;
 
@@ -526,8 +526,8 @@ static void fsl_elbc_write_buf(struct mtd_info *mtd, const u8 *buf, int len)
  */
 static u8 fsl_elbc_read_byte(struct mtd_info *mtd)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct fsl_elbc_mtd *priv = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct fsl_elbc_mtd *priv = nand_get_controller_data(chip);
 	struct fsl_elbc_ctrl *ctrl = priv->ctrl;
 
 	/* If there are still bytes in the FCM, then use the next byte. */
@@ -543,8 +543,8 @@ static u8 fsl_elbc_read_byte(struct mtd_info *mtd)
  */
 static void fsl_elbc_read_buf(struct mtd_info *mtd, u8 *buf, int len)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct fsl_elbc_mtd *priv = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct fsl_elbc_mtd *priv = nand_get_controller_data(chip);
 	struct fsl_elbc_ctrl *ctrl = priv->ctrl;
 	int avail;
 
@@ -566,7 +566,7 @@ static void fsl_elbc_read_buf(struct mtd_info *mtd, u8 *buf, int len)
  */
 static int fsl_elbc_wait(struct mtd_info *mtd, struct nand_chip *chip)
 {
-	struct fsl_elbc_mtd *priv = chip->priv;
+	struct fsl_elbc_mtd *priv = nand_get_controller_data(chip);
 	struct fsl_elbc_ctrl *ctrl = priv->ctrl;
 	fsl_lbc_t *lbc = ctrl->regs;
 
@@ -611,7 +611,8 @@ static int fsl_elbc_read_page(struct mtd_info *mtd, struct nand_chip *chip,
  * waitfunc.
  */
 static int fsl_elbc_write_page(struct mtd_info *mtd, struct nand_chip *chip,
-				const uint8_t *buf, int oob_required)
+				const uint8_t *buf, int oob_required,
+				int page)
 {
 	fsl_elbc_write_buf(mtd, buf, mtd->writesize);
 	fsl_elbc_write_buf(mtd, chip->oob_poi, mtd->oobsize);
@@ -626,7 +627,7 @@ static struct fsl_elbc_ctrl *elbc_ctrl;
  */
 static int fsl_elbc_write_subpage(struct mtd_info *mtd, struct nand_chip *chip,
 				uint32_t offset, uint32_t data_len,
-				const uint8_t *buf, int oob_required)
+				const uint8_t *buf, int oob_required, int page)
 {
 	fsl_elbc_write_buf(mtd, buf, mtd->writesize);
 	fsl_elbc_write_buf(mtd, chip->oob_poi, mtd->oobsize);
@@ -656,7 +657,7 @@ static void fsl_elbc_ctrl_init(void)
 
 static int fsl_elbc_chip_init(int devnum, u8 *addr)
 {
-	struct mtd_info *mtd = &nand_info[devnum];
+	struct mtd_info *mtd;
 	struct nand_chip *nand;
 	struct fsl_elbc_mtd *priv;
 	uint32_t br = 0, or = 0;
@@ -697,7 +698,7 @@ static int fsl_elbc_chip_init(int devnum, u8 *addr)
 	}
 
 	nand = &priv->chip;
-	mtd->priv = nand;
+	mtd = nand_to_mtd(nand);
 
 	elbc_ctrl->chips[priv->bank] = priv;
 
@@ -719,7 +720,7 @@ static int fsl_elbc_chip_init(int devnum, u8 *addr)
 	nand->bbt_options = NAND_BBT_USE_FLASH;
 
 	nand->controller = &elbc_ctrl->controller;
-	nand->priv = priv;
+	nand_set_controller_data(nand, priv);
 
 	nand->ecc.read_page = fsl_elbc_read_page;
 	nand->ecc.write_page = fsl_elbc_write_page;
@@ -787,7 +788,7 @@ static int fsl_elbc_chip_init(int devnum, u8 *addr)
 	if (ret)
 		return ret;
 
-	ret = nand_register(devnum);
+	ret = nand_register(devnum, mtd);
 	if (ret)
 		return ret;
 
diff --git a/drivers/mtd/nand/fsl_ifc_nand.c b/drivers/mtd/nand/fsl_ifc_nand.c
index 975b0d4..7001cbd 100644
--- a/drivers/mtd/nand/fsl_ifc_nand.c
+++ b/drivers/mtd/nand/fsl_ifc_nand.c
@@ -222,8 +222,8 @@ static struct nand_bbt_descr bbt_mirror_descr = {
  */
 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 nand_chip *chip = mtd_to_nand(mtd);
+	struct fsl_ifc_mtd *priv = nand_get_controller_data(chip);
 	struct fsl_ifc_ctrl *ctrl = priv->ctrl;
 	struct fsl_ifc_runtime *ifc = ctrl->regs.rregs;
 	int buf_num;
@@ -247,8 +247,8 @@ static void set_addr(struct mtd_info *mtd, int column, int page_addr, int oob)
 static int is_blank(struct mtd_info *mtd, struct fsl_ifc_ctrl *ctrl,
 		    unsigned int bufnum)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct fsl_ifc_mtd *priv = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct fsl_ifc_mtd *priv = nand_get_controller_data(chip);
 	u8 __iomem *addr = priv->vbase + bufnum * (mtd->writesize * 2);
 	u32 __iomem *main = (u32 *)addr;
 	u8 __iomem *oob = addr + mtd->writesize;
@@ -286,8 +286,8 @@ static int check_read_ecc(struct mtd_info *mtd, struct fsl_ifc_ctrl *ctrl,
  */
 static int fsl_ifc_run_command(struct mtd_info *mtd)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct fsl_ifc_mtd *priv = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct fsl_ifc_mtd *priv = nand_get_controller_data(chip);
 	struct fsl_ifc_ctrl *ctrl = priv->ctrl;
 	struct fsl_ifc_runtime *ifc = ctrl->regs.rregs;
 	u32 timeo = (CONFIG_SYS_HZ * 10) / 1000;
@@ -367,7 +367,7 @@ static void fsl_ifc_do_read(struct nand_chip *chip,
 			    int oob,
 			    struct mtd_info *mtd)
 {
-	struct fsl_ifc_mtd *priv = chip->priv;
+	struct fsl_ifc_mtd *priv = nand_get_controller_data(chip);
 	struct fsl_ifc_ctrl *ctrl = priv->ctrl;
 	struct fsl_ifc_runtime *ifc = ctrl->regs.rregs;
 
@@ -404,8 +404,8 @@ static void fsl_ifc_do_read(struct nand_chip *chip,
 static void fsl_ifc_cmdfunc(struct mtd_info *mtd, unsigned int command,
 			     int column, int page_addr)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct fsl_ifc_mtd *priv = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct fsl_ifc_mtd *priv = nand_get_controller_data(chip);
 	struct fsl_ifc_ctrl *ctrl = priv->ctrl;
 	struct fsl_ifc_runtime *ifc = ctrl->regs.rregs;
 
@@ -607,8 +607,8 @@ static void fsl_ifc_cmdfunc(struct mtd_info *mtd, unsigned int command,
  */
 static void fsl_ifc_write_buf(struct mtd_info *mtd, const u8 *buf, int len)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct fsl_ifc_mtd *priv = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct fsl_ifc_mtd *priv = nand_get_controller_data(chip);
 	struct fsl_ifc_ctrl *ctrl = priv->ctrl;
 	unsigned int bufsize = mtd->writesize + mtd->oobsize;
 
@@ -635,8 +635,8 @@ static void fsl_ifc_write_buf(struct mtd_info *mtd, const u8 *buf, int len)
  */
 static u8 fsl_ifc_read_byte(struct mtd_info *mtd)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct fsl_ifc_mtd *priv = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct fsl_ifc_mtd *priv = nand_get_controller_data(chip);
 	struct fsl_ifc_ctrl *ctrl = priv->ctrl;
 	unsigned int offset;
 
@@ -659,8 +659,8 @@ static u8 fsl_ifc_read_byte(struct mtd_info *mtd)
  */
 static uint8_t fsl_ifc_read_byte16(struct mtd_info *mtd)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct fsl_ifc_mtd *priv = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct fsl_ifc_mtd *priv = nand_get_controller_data(chip);
 	struct fsl_ifc_ctrl *ctrl = priv->ctrl;
 	uint16_t data;
 
@@ -683,8 +683,8 @@ static uint8_t fsl_ifc_read_byte16(struct mtd_info *mtd)
  */
 static void fsl_ifc_read_buf(struct mtd_info *mtd, u8 *buf, int len)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct fsl_ifc_mtd *priv = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct fsl_ifc_mtd *priv = nand_get_controller_data(chip);
 	struct fsl_ifc_ctrl *ctrl = priv->ctrl;
 	int avail;
 
@@ -706,7 +706,7 @@ static void fsl_ifc_read_buf(struct mtd_info *mtd, u8 *buf, int len)
  */
 static int fsl_ifc_wait(struct mtd_info *mtd, struct nand_chip *chip)
 {
-	struct fsl_ifc_mtd *priv = chip->priv;
+	struct fsl_ifc_mtd *priv = nand_get_controller_data(chip);
 	struct fsl_ifc_ctrl *ctrl = priv->ctrl;
 	struct fsl_ifc_runtime *ifc = ctrl->regs.rregs;
 	u32 nand_fsr;
@@ -739,7 +739,7 @@ static int fsl_ifc_wait(struct mtd_info *mtd, struct nand_chip *chip)
 static int fsl_ifc_read_page(struct mtd_info *mtd, struct nand_chip *chip,
 			     uint8_t *buf, int oob_required, int page)
 {
-	struct fsl_ifc_mtd *priv = chip->priv;
+	struct fsl_ifc_mtd *priv = nand_get_controller_data(chip);
 	struct fsl_ifc_ctrl *ctrl = priv->ctrl;
 
 	fsl_ifc_read_buf(mtd, buf, mtd->writesize);
@@ -755,7 +755,7 @@ static int fsl_ifc_read_page(struct mtd_info *mtd, struct nand_chip *chip,
  * waitfunc.
  */
 static int fsl_ifc_write_page(struct mtd_info *mtd, struct nand_chip *chip,
-			       const uint8_t *buf, int oob_required)
+			       const uint8_t *buf, int oob_required, int page)
 {
 	fsl_ifc_write_buf(mtd, buf, mtd->writesize);
 	fsl_ifc_write_buf(mtd, chip->oob_poi, mtd->oobsize);
@@ -880,7 +880,7 @@ static int fsl_ifc_sram_init(uint32_t ver)
 
 static int fsl_ifc_chip_init(int devnum, u8 *addr)
 {
-	struct mtd_info *mtd = &nand_info[devnum];
+	struct mtd_info *mtd;
 	struct nand_chip *nand;
 	struct fsl_ifc_mtd *priv;
 	struct nand_ecclayout *layout;
@@ -925,7 +925,7 @@ static int fsl_ifc_chip_init(int devnum, u8 *addr)
 	}
 
 	nand = &priv->chip;
-	mtd->priv = nand;
+	mtd = nand_to_mtd(nand);
 
 	ifc_ctrl->chips[priv->bank] = priv;
 
@@ -954,7 +954,7 @@ static int fsl_ifc_chip_init(int devnum, u8 *addr)
 	}
 
 	nand->controller = &ifc_ctrl->controller;
-	nand->priv = priv;
+	nand_set_controller_data(nand, priv);
 
 	nand->ecc.read_page = fsl_ifc_read_page;
 	nand->ecc.write_page = fsl_ifc_write_page;
@@ -1044,7 +1044,7 @@ static int fsl_ifc_chip_init(int devnum, u8 *addr)
 	if (ret)
 		return ret;
 
-	ret = nand_register(devnum);
+	ret = nand_register(devnum, mtd);
 	if (ret)
 		return ret;
 	return 0;
diff --git a/drivers/mtd/nand/fsl_upm.c b/drivers/mtd/nand/fsl_upm.c
index 5426c32..d2b3881 100644
--- a/drivers/mtd/nand/fsl_upm.c
+++ b/drivers/mtd/nand/fsl_upm.c
@@ -64,8 +64,8 @@ static void fun_wait(struct fsl_upm_nand *fun)
 #if CONFIG_SYS_NAND_MAX_CHIPS > 1
 static void fun_select_chip(struct mtd_info *mtd, int chip_nr)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct fsl_upm_nand *fun = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct fsl_upm_nand *fun = nand_get_controller_data(chip);
 
 	if (chip_nr >= 0) {
 		fun->chip_nr = chip_nr;
@@ -79,8 +79,8 @@ static void fun_select_chip(struct mtd_info *mtd, int chip_nr)
 
 static void fun_cmd_ctrl(struct mtd_info *mtd, int cmd, unsigned int ctrl)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct fsl_upm_nand *fun = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct fsl_upm_nand *fun = nand_get_controller_data(chip);
 	void __iomem *io_addr;
 	u32 mar;
 
@@ -123,7 +123,7 @@ static void fun_cmd_ctrl(struct mtd_info *mtd, int cmd, unsigned int ctrl)
 
 static u8 upm_nand_read_byte(struct mtd_info *mtd)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 
 	return in_8(chip->IO_ADDR_R);
 }
@@ -131,8 +131,8 @@ static u8 upm_nand_read_byte(struct mtd_info *mtd)
 static void upm_nand_write_buf(struct mtd_info *mtd, const u_char *buf, int len)
 {
 	int i;
-	struct nand_chip *chip = mtd->priv;
-	struct fsl_upm_nand *fun = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct fsl_upm_nand *fun = nand_get_controller_data(chip);
 
 	for (i = 0; i < len; i++) {
 		out_8(chip->IO_ADDR_W, buf[i]);
@@ -147,7 +147,7 @@ static void upm_nand_write_buf(struct mtd_info *mtd, const u_char *buf, int len)
 static void upm_nand_read_buf(struct mtd_info *mtd, u_char *buf, int len)
 {
 	int i;
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 
 	for (i = 0; i < len; i++)
 		buf[i] = in_8(chip->IO_ADDR_R);
@@ -155,8 +155,8 @@ static void upm_nand_read_buf(struct mtd_info *mtd, u_char *buf, int len)
 
 static int nand_dev_ready(struct mtd_info *mtd)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct fsl_upm_nand *fun = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct fsl_upm_nand *fun = nand_get_controller_data(chip);
 
 	return fun->dev_ready(fun->chip_nr);
 }
@@ -168,7 +168,7 @@ int fsl_upm_nand_init(struct nand_chip *chip, struct fsl_upm_nand *fun)
 
 	fun->last_ctrl = NAND_CLE;
 
-	chip->priv = fun;
+	nand_set_controller_data(chip, fun);
 	chip->chip_delay = fun->chip_delay;
 	chip->ecc.mode = NAND_ECC_SOFT;
 	chip->cmd_ctrl = fun_cmd_ctrl;
diff --git a/drivers/mtd/nand/fsmc_nand.c b/drivers/mtd/nand/fsmc_nand.c
index e0e9e1e..a1f2cba 100644
--- a/drivers/mtd/nand/fsmc_nand.c
+++ b/drivers/mtd/nand/fsmc_nand.c
@@ -165,7 +165,7 @@ static int count_written_bits(uint8_t *buff, int size, int max_bits)
 
 static void fsmc_nand_hwcontrol(struct mtd_info *mtd, int cmd, uint ctrl)
 {
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 	ulong IO_ADDR_W;
 
 	if (ctrl & NAND_CTRL_CHANGE) {
@@ -409,8 +409,8 @@ int fsmc_nand_switch_ecc(uint32_t eccstrength)
 	 * Nomadik SoC is currently supporting this fsmc_nand_switch_ecc()
 	 * function, as it doesn't need to switch to a different ECC layout.
 	 */
-	mtd = &nand_info[nand_curr_device];
-	nand = mtd->priv;
+	mtd = nand_info[nand_curr_device];
+	nand = mtd_to_nand(mtd);
 
 	/* Setup the ecc configurations again */
 	if (eccstrength == 1) {
@@ -443,7 +443,6 @@ int fsmc_nand_init(struct nand_chip *nand)
 {
 	static int chip_nr;
 	struct mtd_info *mtd;
-	int i;
 	u32 peripid2 = readl(&fsmc_regs_p->peripid2);
 
 	fsmc_version = (peripid2 >> FSMC_REVISION_SHFT) &
@@ -480,8 +479,7 @@ int fsmc_nand_init(struct nand_chip *nand)
 		(void  __iomem *)CONFIG_SYS_NAND_BASE;
 	nand->badblockbits = 7;
 
-	mtd = &nand_info[chip_nr++];
-	mtd->priv = nand;
+	mtd = nand_to_mtd(nand);
 
 	switch (fsmc_version) {
 	case FSMC_VER8:
@@ -514,9 +512,8 @@ int fsmc_nand_init(struct nand_chip *nand)
 	if (nand_scan_tail(mtd))
 		return -ENXIO;
 
-	for (i = 0; i < CONFIG_SYS_MAX_NAND_DEVICE; i++)
-		if (nand_register(i))
-			return -ENXIO;
+	if (nand_register(chip_nr++, mtd))
+		return -ENXIO;
 
 	return 0;
 }
diff --git a/drivers/mtd/nand/jz4740_nand.c b/drivers/mtd/nand/jz4740_nand.c
deleted file mode 100644
index abcedc2..0000000
--- a/drivers/mtd/nand/jz4740_nand.c
+++ /dev/null
@@ -1,258 +0,0 @@
-/*
- * Platform independend driver for JZ4740.
- *
- * Copyright (c) 2007 Ingenic Semiconductor Inc.
- * Author: <jlwei@ingenic.cn>
- *
- * SPDX-License-Identifier:	GPL-2.0+
- */
-#include <common.h>
-
-#include <nand.h>
-#include <asm/io.h>
-#include <asm/jz4740.h>
-
-#define JZ_NAND_DATA_ADDR ((void __iomem *)0xB8000000)
-#define JZ_NAND_CMD_ADDR (JZ_NAND_DATA_ADDR + 0x8000)
-#define JZ_NAND_ADDR_ADDR (JZ_NAND_DATA_ADDR + 0x10000)
-
-#define JZ_NAND_ECC_CTRL_ENCODING	BIT(3)
-#define JZ_NAND_ECC_CTRL_RS		BIT(2)
-#define JZ_NAND_ECC_CTRL_RESET		BIT(1)
-#define JZ_NAND_ECC_CTRL_ENABLE		BIT(0)
-
-#define EMC_SMCR1_OPT_NAND	0x094c4400
-/* Optimize the timing of nand */
-
-static struct jz4740_emc * emc = (struct jz4740_emc *)JZ4740_EMC_BASE;
-
-static struct nand_ecclayout qi_lb60_ecclayout_2gb = {
-	.eccbytes = 72,
-	.eccpos = {
-		12, 13, 14, 15, 16, 17, 18, 19,
-		20, 21, 22, 23, 24, 25, 26, 27,
-		28, 29, 30, 31, 32, 33, 34, 35,
-		36, 37, 38, 39, 40, 41, 42, 43,
-		44, 45, 46, 47, 48, 49, 50, 51,
-		52, 53, 54, 55, 56, 57, 58, 59,
-		60, 61, 62, 63, 64, 65, 66, 67,
-		68, 69, 70, 71, 72, 73, 74, 75,
-		76, 77, 78, 79, 80, 81, 82, 83 },
-	.oobfree = {
-		{.offset = 2,
-		 .length = 10 },
-		{.offset = 84,
-		 .length = 44 } }
-};
-
-static int is_reading;
-
-static void jz_nand_cmd_ctrl(struct mtd_info *mtd, int cmd, unsigned int ctrl)
-{
-	struct nand_chip *this = mtd->priv;
-	uint32_t reg;
-
-	if (ctrl & NAND_CTRL_CHANGE) {
-		if (ctrl & NAND_ALE)
-			this->IO_ADDR_W = JZ_NAND_ADDR_ADDR;
-		else if (ctrl & NAND_CLE)
-			this->IO_ADDR_W = JZ_NAND_CMD_ADDR;
-		else
-			this->IO_ADDR_W = JZ_NAND_DATA_ADDR;
-
-		reg = readl(&emc->nfcsr);
-		if (ctrl & NAND_NCE)
-			reg |= EMC_NFCSR_NFCE1;
-		else
-			reg &= ~EMC_NFCSR_NFCE1;
-		writel(reg, &emc->nfcsr);
-	}
-
-	if (cmd != NAND_CMD_NONE)
-		writeb(cmd, this->IO_ADDR_W);
-}
-
-static int jz_nand_device_ready(struct mtd_info *mtd)
-{
-	return (readl(GPIO_PXPIN(2)) & 0x40000000) ? 1 : 0;
-}
-
-void board_nand_select_device(struct nand_chip *nand, int chip)
-{
-	/*
-	 * Don't use "chip" to address the NAND device,
-	 * generate the cs from the address where it is encoded.
-	 */
-}
-
-static int jz_nand_rs_calculate_ecc(struct mtd_info *mtd, const u_char *dat,
-				u_char *ecc_code)
-{
-	uint32_t status;
-	int i;
-
-	if (is_reading)
-		return 0;
-
-	do {
-		status = readl(&emc->nfints);
-	} while (!(status & EMC_NFINTS_ENCF));
-
-	/* disable ecc */
-	writel(readl(&emc->nfecr) & ~EMC_NFECR_ECCE, &emc->nfecr);
-
-	for (i = 0; i < 9; i++)
-		ecc_code[i] = readb(&emc->nfpar[i]);
-
-	return 0;
-}
-
-static void jz_nand_hwctl(struct mtd_info *mtd, int mode)
-{
-	uint32_t reg;
-
-	writel(0, &emc->nfints);
-	reg = readl(&emc->nfecr);
-	reg |= JZ_NAND_ECC_CTRL_RESET;
-	reg |= JZ_NAND_ECC_CTRL_ENABLE;
-	reg |= JZ_NAND_ECC_CTRL_RS;
-
-	switch (mode) {
-	case NAND_ECC_READ:
-		reg &= ~JZ_NAND_ECC_CTRL_ENCODING;
-		is_reading = 1;
-		break;
-	case NAND_ECC_WRITE:
-		reg |= JZ_NAND_ECC_CTRL_ENCODING;
-		is_reading = 0;
-		break;
-	default:
-		break;
-	}
-
-	writel(reg, &emc->nfecr);
-}
-
-/* Correct 1~9-bit errors in 512-bytes data */
-static void jz_rs_correct(unsigned char *dat, int idx, int mask)
-{
-	int i;
-
-	idx--;
-
-	i = idx + (idx >> 3);
-	if (i >= 512)
-		return;
-
-	mask <<= (idx & 0x7);
-
-	dat[i] ^= mask & 0xff;
-	if (i < 511)
-		dat[i + 1] ^= (mask >> 8) & 0xff;
-}
-
-static int jz_nand_rs_correct_data(struct mtd_info *mtd, u_char *dat,
-				   u_char *read_ecc, u_char *calc_ecc)
-{
-	int k;
-	uint32_t errcnt, index, mask, status;
-
-	/* Set PAR values */
-	const uint8_t all_ff_ecc[] = {
-		0xcd, 0x9d, 0x90, 0x58, 0xf4, 0x8b, 0xff, 0xb7, 0x6f };
-
-	if (read_ecc[0] == 0xff && read_ecc[1] == 0xff &&
-	    read_ecc[2] == 0xff && read_ecc[3] == 0xff &&
-	    read_ecc[4] == 0xff && read_ecc[5] == 0xff &&
-	    read_ecc[6] == 0xff && read_ecc[7] == 0xff &&
-	    read_ecc[8] == 0xff) {
-		for (k = 0; k < 9; k++)
-			writeb(all_ff_ecc[k], &emc->nfpar[k]);
-	} else {
-		for (k = 0; k < 9; k++)
-			writeb(read_ecc[k], &emc->nfpar[k]);
-	}
-	/* Set PRDY */
-	writel(readl(&emc->nfecr) | EMC_NFECR_PRDY, &emc->nfecr);
-
-	/* Wait for completion */
-	do {
-		status = readl(&emc->nfints);
-	} while (!(status & EMC_NFINTS_DECF));
-
-	/* disable ecc */
-	writel(readl(&emc->nfecr) & ~EMC_NFECR_ECCE, &emc->nfecr);
-
-	/* Check decoding */
-	if (!(status & EMC_NFINTS_ERR))
-		return 0;
-
-	if (status & EMC_NFINTS_UNCOR) {
-		printf("uncorrectable ecc\n");
-		return -1;
-	}
-
-	errcnt = (status & EMC_NFINTS_ERRCNT_MASK) >> EMC_NFINTS_ERRCNT_BIT;
-
-	switch (errcnt) {
-	case 4:
-		index = (readl(&emc->nferr[3]) & EMC_NFERR_INDEX_MASK) >>
-			EMC_NFERR_INDEX_BIT;
-		mask = (readl(&emc->nferr[3]) & EMC_NFERR_MASK_MASK) >>
-			EMC_NFERR_MASK_BIT;
-		jz_rs_correct(dat, index, mask);
-	case 3:
-		index = (readl(&emc->nferr[2]) & EMC_NFERR_INDEX_MASK) >>
-			EMC_NFERR_INDEX_BIT;
-		mask = (readl(&emc->nferr[2]) & EMC_NFERR_MASK_MASK) >>
-			EMC_NFERR_MASK_BIT;
-		jz_rs_correct(dat, index, mask);
-	case 2:
-		index = (readl(&emc->nferr[1]) & EMC_NFERR_INDEX_MASK) >>
-			EMC_NFERR_INDEX_BIT;
-		mask = (readl(&emc->nferr[1]) & EMC_NFERR_MASK_MASK) >>
-			EMC_NFERR_MASK_BIT;
-		jz_rs_correct(dat, index, mask);
-	case 1:
-		index = (readl(&emc->nferr[0]) & EMC_NFERR_INDEX_MASK) >>
-			EMC_NFERR_INDEX_BIT;
-		mask = (readl(&emc->nferr[0]) & EMC_NFERR_MASK_MASK) >>
-			EMC_NFERR_MASK_BIT;
-		jz_rs_correct(dat, index, mask);
-	default:
-		break;
-	}
-
-	return errcnt;
-}
-
-/*
- * Main initialization routine
- */
-int board_nand_init(struct nand_chip *nand)
-{
-	uint32_t reg;
-
-	reg = readl(&emc->nfcsr);
-	reg |= EMC_NFCSR_NFE1;	/* EMC setup, Set NFE bit */
-	writel(reg, &emc->nfcsr);
-
-	writel(EMC_SMCR1_OPT_NAND, &emc->smcr[1]);
-
-	nand->IO_ADDR_R		= JZ_NAND_DATA_ADDR;
-	nand->IO_ADDR_W		= JZ_NAND_DATA_ADDR;
-	nand->cmd_ctrl		= jz_nand_cmd_ctrl;
-	nand->dev_ready		= jz_nand_device_ready;
-	nand->ecc.hwctl		= jz_nand_hwctl;
-	nand->ecc.correct	= jz_nand_rs_correct_data;
-	nand->ecc.calculate	= jz_nand_rs_calculate_ecc;
-	nand->ecc.mode		= NAND_ECC_HW_OOB_FIRST;
-	nand->ecc.size		= CONFIG_SYS_NAND_ECCSIZE;
-	nand->ecc.bytes		= CONFIG_SYS_NAND_ECCBYTES;
-	nand->ecc.strength	= 4;
-	nand->ecc.layout	= &qi_lb60_ecclayout_2gb;
-	nand->chip_delay	= 50;
-	nand->bbt_options	|= NAND_BBT_USE_FLASH;
-
-	return 0;
-}
diff --git a/drivers/mtd/nand/kb9202_nand.c b/drivers/mtd/nand/kb9202_nand.c
index 22c5625..e978cf8 100644
--- a/drivers/mtd/nand/kb9202_nand.c
+++ b/drivers/mtd/nand/kb9202_nand.c
@@ -35,7 +35,7 @@
  */
 static void kb9202_nand_hwcontrol(struct mtd_info *mtd, int cmd, unsigned int ctrl)
 {
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 
 	if (ctrl & NAND_CTRL_CHANGE) {
 		ulong IO_ADDR_W = (ulong) this->IO_ADDR_W;
diff --git a/drivers/mtd/nand/kirkwood_nand.c b/drivers/mtd/nand/kirkwood_nand.c
index d734113..d0a68bd 100644
--- a/drivers/mtd/nand/kirkwood_nand.c
+++ b/drivers/mtd/nand/kirkwood_nand.c
@@ -33,7 +33,7 @@ static u32 nand_mpp_backup[9] = { 0 };
 static void kw_nand_hwcontrol(struct mtd_info *mtd, int cmd,
 			      unsigned int ctrl)
 {
-	struct nand_chip *nc = mtd->priv;
+	struct nand_chip *nc = mtd_to_nand(mtd);
 	u32 offs;
 
 	if (cmd == NAND_CMD_NONE)
diff --git a/drivers/mtd/nand/lpc32xx_nand_mlc.c b/drivers/mtd/nand/lpc32xx_nand_mlc.c
index 8156fe9..4262029 100644
--- a/drivers/mtd/nand/lpc32xx_nand_mlc.c
+++ b/drivers/mtd/nand/lpc32xx_nand_mlc.c
@@ -378,7 +378,8 @@ static int lpc32xx_read_oob(struct mtd_info *mtd, struct nand_chip *chip,
  */
 
 static int lpc32xx_write_page_hwecc(struct mtd_info *mtd,
-	struct nand_chip *chip, const uint8_t *buf, int oob_required)
+	struct nand_chip *chip, const uint8_t *buf, int oob_required,
+	int page)
 {
 	unsigned int i, status, timeout;
 	struct lpc32xx_oob *oob = (struct lpc32xx_oob *)chip->oob_poi;
@@ -435,7 +436,8 @@ static int lpc32xx_write_page_hwecc(struct mtd_info *mtd,
  */
 
 static int lpc32xx_write_page_raw(struct mtd_info *mtd,
-	struct nand_chip *chip, const uint8_t *buf, int oob_required)
+	struct nand_chip *chip, const uint8_t *buf, int oob_required,
+	int page)
 {
 	unsigned int i;
 	struct lpc32xx_oob *oob = (struct lpc32xx_oob *)chip->oob_poi;
@@ -539,11 +541,7 @@ static struct nand_chip lpc32xx_chip;
 
 void board_nand_init(void)
 {
-	/* we have only one device anyway */
-	struct mtd_info *mtd = &nand_info[0];
-	/* chip is struct nand_chip, and is now provided by the driver. */
-	mtd->priv = &lpc32xx_chip;
-	/* to store return status in case we need to print it */
+	struct mtd_info *mtd = &lpc32xx_chip.mtd;
 	int ret;
 
 	/* Set all BOARDSPECIFIC (actually core-specific) fields  */
@@ -597,7 +595,7 @@ void board_nand_init(void)
 	}
 
 	/* chip is good, register it */
-	ret = nand_register(0);
+	ret = nand_register(0, mtd);
 	if (ret)
 		error("nand_register returned %i", ret);
 }
diff --git a/drivers/mtd/nand/lpc32xx_nand_slc.c b/drivers/mtd/nand/lpc32xx_nand_slc.c
index 4e1be36..daa1e7a 100644
--- a/drivers/mtd/nand/lpc32xx_nand_slc.c
+++ b/drivers/mtd/nand/lpc32xx_nand_slc.c
@@ -291,7 +291,7 @@ static void lpc32xx_nand_dma_configure(struct nand_chip *chip,
 static void lpc32xx_nand_xfer(struct mtd_info *mtd, const u8 *buf,
 			      int len, int read)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	u32 config;
 	int ret;
 
@@ -486,7 +486,8 @@ static int lpc32xx_read_page_hwecc(struct mtd_info *mtd, struct nand_chip *chip,
 /* Reuse the logic from "nand_write_page_hwecc()" */
 static int lpc32xx_write_page_hwecc(struct mtd_info *mtd,
 				    struct nand_chip *chip,
-				    const uint8_t *buf, int oob_required)
+				    const uint8_t *buf, int oob_required,
+				    int page)
 {
 	int i;
 	uint8_t *ecc_calc = chip->buffers->ecccalc;
diff --git a/drivers/mtd/nand/mpc5121_nfc.c b/drivers/mtd/nand/mpc5121_nfc.c
index e621c36..8a8775c 100644
--- a/drivers/mtd/nand/mpc5121_nfc.c
+++ b/drivers/mtd/nand/mpc5121_nfc.c
@@ -100,7 +100,6 @@
 #define NFC_WPC_UNLOCK		(1 << 2)
 
 struct mpc5121_nfc_prv {
-	struct mtd_info mtd;
 	struct nand_chip chip;
 	int irq;
 	void __iomem *regs;
@@ -117,8 +116,8 @@ static void mpc5121_nfc_done(struct mtd_info *mtd);
 /* Read NFC register */
 static inline u16 nfc_read(struct mtd_info *mtd, uint reg)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct mpc5121_nfc_prv *prv = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct mpc5121_nfc_prv *prv = nand_get_controller_data(chip);
 
 	return in_be16(prv->regs + reg);
 }
@@ -126,8 +125,8 @@ static inline u16 nfc_read(struct mtd_info *mtd, uint reg)
 /* Write NFC register */
 static inline void nfc_write(struct mtd_info *mtd, uint reg, u16 val)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct mpc5121_nfc_prv *prv = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct mpc5121_nfc_prv *prv = nand_get_controller_data(chip);
 
 	out_be16(prv->regs + reg, val);
 }
@@ -211,7 +210,7 @@ static void mpc5121_nfc_done(struct mtd_info *mtd)
 /* Do address cycle(s) */
 static void mpc5121_nfc_addr_cycle(struct mtd_info *mtd, int column, int page)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	u32 pagemask = chip->pagemask;
 
 	if (column != -1) {
@@ -283,8 +282,8 @@ static int mpc5121_nfc_dev_ready(struct mtd_info *mtd)
 static void mpc5121_nfc_command(struct mtd_info *mtd, unsigned command,
 				int column, int page)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct mpc5121_nfc_prv *prv = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct mpc5121_nfc_prv *prv = nand_get_controller_data(chip);
 
 	prv->column = (column >= 0) ? column : 0;
 	prv->spareonly = 0;
@@ -357,8 +356,8 @@ static void mpc5121_nfc_command(struct mtd_info *mtd, unsigned command,
 static void mpc5121_nfc_copy_spare(struct mtd_info *mtd, uint offset,
 				   u8 * buffer, uint size, int wr)
 {
-	struct nand_chip *nand = mtd->priv;
-	struct mpc5121_nfc_prv *prv = nand->priv;
+	struct nand_chip *nand = mtd_to_nand(mtd);
+	struct mpc5121_nfc_prv *prv = nand_get_controller_data(nand);
 	uint o, s, sbsize, blksize;
 
 	/*
@@ -410,8 +409,8 @@ static void mpc5121_nfc_copy_spare(struct mtd_info *mtd, uint offset,
 static void mpc5121_nfc_buf_copy(struct mtd_info *mtd, u_char * buf, int len,
 				 int wr)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct mpc5121_nfc_prv *prv = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct mpc5121_nfc_prv *prv = nand_get_controller_data(chip);
 	uint c = prv->column;
 	uint l;
 
@@ -489,7 +488,7 @@ static u16 mpc5121_nfc_read_word(struct mtd_info *mtd)
 static int mpc5121_nfc_read_hw_config(struct mtd_info *mtd)
 {
 	immap_t *im = (immap_t *)CONFIG_SYS_IMMR;
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	uint rcw_pagesize = 0;
 	uint rcw_sparesize = 0;
 	uint rcw_width;
@@ -549,7 +548,6 @@ int board_nand_init(struct nand_chip *chip)
 	int resettime = 0;
 	int retval = 0;
 	int rev;
-	static int chip_nr = 0;
 
 	/*
 	 * Check SoC revision. This driver supports only NFC
@@ -568,9 +566,8 @@ int board_nand_init(struct nand_chip *chip)
 		return -ENOMEM;
 	}
 
-	mtd = &nand_info[chip_nr++];
-	mtd->priv = chip;
-	chip->priv = prv;
+	mtd = &chip->mtd;
+	nand_set_controller_data(chip, prv);
 
 	/* Read NFC configuration from Reset Config Word */
 	retval = mpc5121_nfc_read_hw_config(mtd);
diff --git a/drivers/mtd/nand/mxc_nand.c b/drivers/mtd/nand/mxc_nand.c
index f12b07e..7221d0b 100644
--- a/drivers/mtd/nand/mxc_nand.c
+++ b/drivers/mtd/nand/mxc_nand.c
@@ -19,7 +19,6 @@
 #define DRIVER_NAME "mxc_nand"
 
 struct mxc_nand_host {
-	struct mtd_info			mtd;
 	struct nand_chip		*nand;
 
 	struct mxc_nand_regs __iomem	*regs;
@@ -351,8 +350,8 @@ static int mxc_nand_dev_ready(struct mtd_info *mtd)
 
 static void _mxc_nand_enable_hwecc(struct mtd_info *mtd, int on)
 {
-	struct nand_chip *nand_chip = mtd->priv;
-	struct mxc_nand_host *host = nand_chip->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct mxc_nand_host *host = nand_get_controller_data(nand_chip);
 #if defined(MXC_NFC_V1) || defined(MXC_NFC_V2_1)
 	uint16_t tmp = readnfc(&host->regs->config1);
 
@@ -386,7 +385,7 @@ static int mxc_nand_read_oob_syndrome(struct mtd_info *mtd,
 				      struct nand_chip *chip,
 				      int page)
 {
-	struct mxc_nand_host *host = chip->priv;
+	struct mxc_nand_host *host = nand_get_controller_data(chip);
 	uint8_t *buf = chip->oob_poi;
 	int length = mtd->oobsize;
 	int eccpitch = chip->ecc.bytes + chip->ecc.prepad + chip->ecc.postpad;
@@ -441,7 +440,7 @@ static int mxc_nand_read_page_raw_syndrome(struct mtd_info *mtd,
 					   int oob_required,
 					   int page)
 {
-	struct mxc_nand_host *host = chip->priv;
+	struct mxc_nand_host *host = nand_get_controller_data(chip);
 	int eccsize = chip->ecc.size;
 	int eccbytes = chip->ecc.bytes;
 	int eccpitch = eccbytes + chip->ecc.prepad + chip->ecc.postpad;
@@ -486,7 +485,7 @@ static int mxc_nand_read_page_syndrome(struct mtd_info *mtd,
 				       int oob_required,
 				       int page)
 {
-	struct mxc_nand_host *host = chip->priv;
+	struct mxc_nand_host *host = nand_get_controller_data(chip);
 	int n, eccsize = chip->ecc.size;
 	int eccbytes = chip->ecc.bytes;
 	int eccpitch = eccbytes + chip->ecc.prepad + chip->ecc.postpad;
@@ -550,7 +549,7 @@ static int mxc_nand_read_page_syndrome(struct mtd_info *mtd,
 static int mxc_nand_write_oob_syndrome(struct mtd_info *mtd,
 				       struct nand_chip *chip, int page)
 {
-	struct mxc_nand_host *host = chip->priv;
+	struct mxc_nand_host *host = nand_get_controller_data(chip);
 	int eccpitch = chip->ecc.bytes + chip->ecc.prepad + chip->ecc.postpad;
 	int length = mtd->oobsize;
 	int i, len, status, steps = chip->ecc.steps;
@@ -576,9 +575,9 @@ static int mxc_nand_write_oob_syndrome(struct mtd_info *mtd,
 static int mxc_nand_write_page_raw_syndrome(struct mtd_info *mtd,
 					     struct nand_chip *chip,
 					     const uint8_t *buf,
-					     int oob_required)
+					     int oob_required, int page)
 {
-	struct mxc_nand_host *host = chip->priv;
+	struct mxc_nand_host *host = nand_get_controller_data(chip);
 	int eccsize = chip->ecc.size;
 	int eccbytes = chip->ecc.bytes;
 	int eccpitch = eccbytes + chip->ecc.prepad + chip->ecc.postpad;
@@ -616,9 +615,9 @@ static int mxc_nand_write_page_raw_syndrome(struct mtd_info *mtd,
 static int mxc_nand_write_page_syndrome(struct mtd_info *mtd,
 					 struct nand_chip *chip,
 					 const uint8_t *buf,
-					 int oob_required)
+					 int oob_required, int page)
 {
-	struct mxc_nand_host *host = chip->priv;
+	struct mxc_nand_host *host = nand_get_controller_data(chip);
 	int i, n, eccsize = chip->ecc.size;
 	int eccbytes = chip->ecc.bytes;
 	int eccpitch = eccbytes + chip->ecc.prepad + chip->ecc.postpad;
@@ -661,8 +660,8 @@ static int mxc_nand_write_page_syndrome(struct mtd_info *mtd,
 static int mxc_nand_correct_data(struct mtd_info *mtd, u_char *dat,
 				 u_char *read_ecc, u_char *calc_ecc)
 {
-	struct nand_chip *nand_chip = mtd->priv;
-	struct mxc_nand_host *host = nand_chip->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct mxc_nand_host *host = nand_get_controller_data(nand_chip);
 	uint32_t ecc_status = readl(&host->regs->ecc_status_result);
 	int subpages = mtd->writesize / nand_chip->subpagesize;
 	int pg2blk_shift = nand_chip->phys_erase_shift -
@@ -681,7 +680,7 @@ static int mxc_nand_correct_data(struct mtd_info *mtd, u_char *dat,
 				       mtd->writesize / nand_chip->subpagesize
 					    - subpages);
 			}
-			return -1;
+			return -EBADMSG;
 		}
 		ecc_status >>= 4;
 		subpages--;
@@ -700,8 +699,8 @@ static int mxc_nand_correct_data(struct mtd_info *mtd, u_char *dat,
 static int mxc_nand_correct_data(struct mtd_info *mtd, u_char *dat,
 				 u_char *read_ecc, u_char *calc_ecc)
 {
-	struct nand_chip *nand_chip = mtd->priv;
-	struct mxc_nand_host *host = nand_chip->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct mxc_nand_host *host = nand_get_controller_data(nand_chip);
 
 	/*
 	 * 1-Bit errors are automatically corrected in HW.  No need for
@@ -713,7 +712,7 @@ static int mxc_nand_correct_data(struct mtd_info *mtd, u_char *dat,
 	if (((ecc_status & 0x3) == 2) || ((ecc_status >> 2) == 2)) {
 		MTDDEBUG(MTD_DEBUG_LEVEL0,
 		      "MXC_NAND: HWECC uncorrectable 2-bit ECC error\n");
-		return -1;
+		return -EBADMSG;
 	}
 
 	return 0;
@@ -729,8 +728,8 @@ static int mxc_nand_calculate_ecc(struct mtd_info *mtd, const u_char *dat,
 
 static u_char mxc_nand_read_byte(struct mtd_info *mtd)
 {
-	struct nand_chip *nand_chip = mtd->priv;
-	struct mxc_nand_host *host = nand_chip->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct mxc_nand_host *host = nand_get_controller_data(nand_chip);
 	uint8_t ret = 0;
 	uint16_t col;
 	uint16_t __iomem *main_buf =
@@ -769,8 +768,8 @@ static u_char mxc_nand_read_byte(struct mtd_info *mtd)
 
 static uint16_t mxc_nand_read_word(struct mtd_info *mtd)
 {
-	struct nand_chip *nand_chip = mtd->priv;
-	struct mxc_nand_host *host = nand_chip->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct mxc_nand_host *host = nand_get_controller_data(nand_chip);
 	uint16_t col, ret;
 	uint16_t __iomem *p;
 
@@ -821,8 +820,8 @@ static uint16_t mxc_nand_read_word(struct mtd_info *mtd)
 static void mxc_nand_write_buf(struct mtd_info *mtd,
 				const u_char *buf, int len)
 {
-	struct nand_chip *nand_chip = mtd->priv;
-	struct mxc_nand_host *host = nand_chip->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct mxc_nand_host *host = nand_get_controller_data(nand_chip);
 	int n, col, i = 0;
 
 	MTDDEBUG(MTD_DEBUG_LEVEL3,
@@ -895,8 +894,8 @@ static void mxc_nand_write_buf(struct mtd_info *mtd,
  */
 static void mxc_nand_read_buf(struct mtd_info *mtd, u_char *buf, int len)
 {
-	struct nand_chip *nand_chip = mtd->priv;
-	struct mxc_nand_host *host = nand_chip->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct mxc_nand_host *host = nand_get_controller_data(nand_chip);
 	int n, col, i = 0;
 
 	MTDDEBUG(MTD_DEBUG_LEVEL3,
@@ -955,8 +954,8 @@ static void mxc_nand_read_buf(struct mtd_info *mtd, u_char *buf, int len)
  */
 static void mxc_nand_select_chip(struct mtd_info *mtd, int chip)
 {
-	struct nand_chip *nand_chip = mtd->priv;
-	struct mxc_nand_host *host = nand_chip->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct mxc_nand_host *host = nand_get_controller_data(nand_chip);
 
 	switch (chip) {
 	case -1:
@@ -982,8 +981,8 @@ static void mxc_nand_select_chip(struct mtd_info *mtd, int chip)
 void mxc_nand_command(struct mtd_info *mtd, unsigned command,
 				int column, int page_addr)
 {
-	struct nand_chip *nand_chip = mtd->priv;
-	struct mxc_nand_host *host = nand_chip->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct mxc_nand_host *host = nand_get_controller_data(nand_chip);
 
 	MTDDEBUG(MTD_DEBUG_LEVEL3,
 	      "mxc_nand_command (cmd = 0x%x, col = 0x%x, page = 0x%x)\n",
@@ -1164,14 +1163,13 @@ int board_nand_init(struct nand_chip *this)
 #endif
 
 	/* structures must be linked */
-	mtd = &host->mtd;
-	mtd->priv = this;
+	mtd = &this->mtd;
 	host->nand = this;
 
 	/* 5 us command delay time */
 	this->chip_delay = 5;
 
-	this->priv = host;
+	nand_set_controller_data(this, host);
 	this->dev_ready = mxc_nand_dev_ready;
 	this->cmdfunc = mxc_nand_command;
 	this->select_chip = mxc_nand_select_chip;
diff --git a/drivers/mtd/nand/mxc_nand_spl.c b/drivers/mtd/nand/mxc_nand_spl.c
index 6ac2c96..841fb5b 100644
--- a/drivers/mtd/nand/mxc_nand_spl.c
+++ b/drivers/mtd/nand/mxc_nand_spl.c
@@ -232,7 +232,7 @@ static int nfc_read_page(unsigned int page_address, unsigned char *buf)
 	nfc_nand_read_page(page_address);
 
 	if (nfc_nand_check_ecc())
-		return -1;
+		return -EBADMSG;
 
 	src = (u32 *)&nfc->main_area[0][0];
 	dst = (u32 *)buf;
diff --git a/drivers/mtd/nand/mxs_nand.c b/drivers/mtd/nand/mxs_nand.c
index 5528d4b..c90a3a7 100644
--- a/drivers/mtd/nand/mxs_nand.c
+++ b/drivers/mtd/nand/mxs_nand.c
@@ -264,8 +264,8 @@ static int mxs_nand_wait_for_bch_complete(void)
  */
 static void mxs_nand_cmd_ctrl(struct mtd_info *mtd, int data, unsigned int ctrl)
 {
-	struct nand_chip *nand = mtd->priv;
-	struct mxs_nand_info *nand_info = nand->priv;
+	struct nand_chip *nand = mtd_to_nand(mtd);
+	struct mxs_nand_info *nand_info = nand_get_controller_data(nand);
 	struct mxs_dma_desc *d;
 	uint32_t channel = MXS_DMA_CHANNEL_AHB_APBH_GPMI0 + nand_info->cur_chip;
 	int ret;
@@ -343,8 +343,8 @@ static void mxs_nand_cmd_ctrl(struct mtd_info *mtd, int data, unsigned int ctrl)
  */
 static int mxs_nand_device_ready(struct mtd_info *mtd)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct mxs_nand_info *nand_info = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct mxs_nand_info *nand_info = nand_get_controller_data(chip);
 	struct mxs_gpmi_regs *gpmi_regs =
 		(struct mxs_gpmi_regs *)MXS_GPMI_BASE;
 	uint32_t tmp;
@@ -360,8 +360,8 @@ static int mxs_nand_device_ready(struct mtd_info *mtd)
  */
 static void mxs_nand_select_chip(struct mtd_info *mtd, int chip)
 {
-	struct nand_chip *nand = mtd->priv;
-	struct mxs_nand_info *nand_info = nand->priv;
+	struct nand_chip *nand = mtd_to_nand(mtd);
+	struct mxs_nand_info *nand_info = nand_get_controller_data(nand);
 
 	nand_info->cur_chip = chip;
 }
@@ -410,8 +410,8 @@ static void mxs_nand_swap_block_mark(struct mtd_info *mtd,
  */
 static void mxs_nand_read_buf(struct mtd_info *mtd, uint8_t *buf, int length)
 {
-	struct nand_chip *nand = mtd->priv;
-	struct mxs_nand_info *nand_info = nand->priv;
+	struct nand_chip *nand = mtd_to_nand(mtd);
+	struct mxs_nand_info *nand_info = nand_get_controller_data(nand);
 	struct mxs_dma_desc *d;
 	uint32_t channel = MXS_DMA_CHANNEL_AHB_APBH_GPMI0 + nand_info->cur_chip;
 	int ret;
@@ -494,8 +494,8 @@ rtn:
 static void mxs_nand_write_buf(struct mtd_info *mtd, const uint8_t *buf,
 				int length)
 {
-	struct nand_chip *nand = mtd->priv;
-	struct mxs_nand_info *nand_info = nand->priv;
+	struct nand_chip *nand = mtd_to_nand(mtd);
+	struct mxs_nand_info *nand_info = nand_get_controller_data(nand);
 	struct mxs_dma_desc *d;
 	uint32_t channel = MXS_DMA_CHANNEL_AHB_APBH_GPMI0 + nand_info->cur_chip;
 	int ret;
@@ -559,7 +559,7 @@ static int mxs_nand_ecc_read_page(struct mtd_info *mtd, struct nand_chip *nand,
 					uint8_t *buf, int oob_required,
 					int page)
 {
-	struct mxs_nand_info *nand_info = nand->priv;
+	struct mxs_nand_info *nand_info = nand_get_controller_data(nand);
 	struct mxs_dma_desc *d;
 	uint32_t channel = MXS_DMA_CHANNEL_AHB_APBH_GPMI0 + nand_info->cur_chip;
 	uint32_t corrected = 0, failed = 0;
@@ -707,9 +707,9 @@ rtn:
  */
 static int mxs_nand_ecc_write_page(struct mtd_info *mtd,
 				struct nand_chip *nand, const uint8_t *buf,
-				int oob_required)
+				int oob_required, int page)
 {
-	struct mxs_nand_info *nand_info = nand->priv;
+	struct mxs_nand_info *nand_info = nand_get_controller_data(nand);
 	struct mxs_dma_desc *d;
 	uint32_t channel = MXS_DMA_CHANNEL_AHB_APBH_GPMI0 + nand_info->cur_chip;
 	int ret;
@@ -775,8 +775,8 @@ rtn:
 static int mxs_nand_hook_read_oob(struct mtd_info *mtd, loff_t from,
 					struct mtd_oob_ops *ops)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct mxs_nand_info *nand_info = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct mxs_nand_info *nand_info = nand_get_controller_data(chip);
 	int ret;
 
 	if (ops->mode == MTD_OPS_RAW)
@@ -800,8 +800,8 @@ static int mxs_nand_hook_read_oob(struct mtd_info *mtd, loff_t from,
 static int mxs_nand_hook_write_oob(struct mtd_info *mtd, loff_t to,
 					struct mtd_oob_ops *ops)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct mxs_nand_info *nand_info = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct mxs_nand_info *nand_info = nand_get_controller_data(chip);
 	int ret;
 
 	if (ops->mode == MTD_OPS_RAW)
@@ -824,8 +824,8 @@ static int mxs_nand_hook_write_oob(struct mtd_info *mtd, loff_t to,
  */
 static int mxs_nand_hook_block_markbad(struct mtd_info *mtd, loff_t ofs)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct mxs_nand_info *nand_info = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct mxs_nand_info *nand_info = nand_get_controller_data(chip);
 	int ret;
 
 	nand_info->marking_block_bad = 1;
@@ -884,7 +884,7 @@ static int mxs_nand_hook_block_markbad(struct mtd_info *mtd, loff_t ofs)
 static int mxs_nand_ecc_read_oob(struct mtd_info *mtd, struct nand_chip *nand,
 				int page)
 {
-	struct mxs_nand_info *nand_info = nand->priv;
+	struct mxs_nand_info *nand_info = nand_get_controller_data(nand);
 
 	/*
 	 * First, fill in the OOB buffer. If we're doing a raw read, we need to
@@ -919,7 +919,7 @@ static int mxs_nand_ecc_read_oob(struct mtd_info *mtd, struct nand_chip *nand,
 static int mxs_nand_ecc_write_oob(struct mtd_info *mtd, struct nand_chip *nand,
 					int page)
 {
-	struct mxs_nand_info *nand_info = nand->priv;
+	struct mxs_nand_info *nand_info = nand_get_controller_data(nand);
 	uint8_t block_mark = 0;
 
 	/*
@@ -961,7 +961,7 @@ static int mxs_nand_ecc_write_oob(struct mtd_info *mtd, struct nand_chip *nand,
  * Thus, this function is only called when we want *all* blocks to look good,
  * so it *always* return success.
  */
-static int mxs_nand_block_bad(struct mtd_info *mtd, loff_t ofs, int getchip)
+static int mxs_nand_block_bad(struct mtd_info *mtd, loff_t ofs)
 {
 	return 0;
 }
@@ -982,8 +982,8 @@ static int mxs_nand_block_bad(struct mtd_info *mtd, loff_t ofs, int getchip)
  */
 static int mxs_nand_scan_bbt(struct mtd_info *mtd)
 {
-	struct nand_chip *nand = mtd->priv;
-	struct mxs_nand_info *nand_info = nand->priv;
+	struct nand_chip *nand = mtd_to_nand(mtd);
+	struct mxs_nand_info *nand_info = nand_get_controller_data(nand);
 	struct mxs_bch_regs *bch_regs = (struct mxs_bch_regs *)MXS_BCH_BASE;
 	uint32_t tmp;
 
@@ -1175,7 +1175,7 @@ int board_nand_init(struct nand_chip *nand)
 
 	memset(&fake_ecc_layout, 0, sizeof(fake_ecc_layout));
 
-	nand->priv = nand_info;
+	nand_set_controller_data(nand, nand_info);
 	nand->options |= NAND_NO_SUBPAGE_WRITE;
 
 	nand->cmd_ctrl		= mxs_nand_cmd_ctrl;
diff --git a/drivers/mtd/nand/mxs_nand_spl.c b/drivers/mtd/nand/mxs_nand_spl.c
index 0e7c364..a8a3084 100644
--- a/drivers/mtd/nand/mxs_nand_spl.c
+++ b/drivers/mtd/nand/mxs_nand_spl.c
@@ -8,13 +8,13 @@
 #include <nand.h>
 #include <malloc.h>
 
-static nand_info_t mtd;
+static struct mtd_info *mtd;
 static struct nand_chip nand_chip;
 
 static void mxs_nand_command(struct mtd_info *mtd, unsigned int command,
 			     int column, int page_addr)
 {
-	register struct nand_chip *chip = mtd->priv;
+	register struct nand_chip *chip = mtd_to_nand(mtd);
 	u32 timeo, time_start;
 
 	/* write out the command to the device */
@@ -51,7 +51,7 @@ static void mxs_nand_command(struct mtd_info *mtd, unsigned int command,
 
 static int mxs_flash_ident(struct mtd_info *mtd)
 {
-	register struct nand_chip *chip = mtd->priv;
+	register struct nand_chip *chip = mtd_to_nand(mtd);
 	int i;
 	u8 mfg_id, dev_id;
 	u8 id_data[8];
@@ -111,7 +111,7 @@ static int mxs_flash_ident(struct mtd_info *mtd)
 
 static int mxs_read_page_ecc(struct mtd_info *mtd, void *buf, unsigned int page)
 {
-	register struct nand_chip *chip = mtd->priv;
+	register struct nand_chip *chip = mtd_to_nand(mtd);
 	int ret;
 
 	chip->cmdfunc(mtd, NAND_CMD_READ0, 0x0, page);
@@ -125,7 +125,7 @@ static int mxs_read_page_ecc(struct mtd_info *mtd, void *buf, unsigned int page)
 
 static int is_badblock(struct mtd_info *mtd, loff_t offs, int allowbbt)
 {
-	register struct nand_chip *chip = mtd->priv;
+	register struct nand_chip *chip = mtd_to_nand(mtd);
 	unsigned int block = offs >> chip->phys_erase_shift;
 	unsigned int page = offs >> chip->page_shift;
 
@@ -147,14 +147,14 @@ static int mxs_nand_init(void)
 
 	/* init mxs nand driver */
 	board_nand_init(&nand_chip);
-	mtd.priv = &nand_chip;
+	mtd = &nand_chip.mtd;
 	/* set mtd functions */
 	nand_chip.cmdfunc = mxs_nand_command;
 	nand_chip.numchips = 1;
 
 	/* identify flash device */
 	puts("NAND : ");
-	if (mxs_flash_ident(&mtd)) {
+	if (mxs_flash_ident(mtd)) {
 		printf("Failed to identify\n");
 		return -1;
 	}
@@ -162,12 +162,12 @@ static int mxs_nand_init(void)
 	/* allocate and initialize buffers */
 	nand_chip.buffers = memalign(ARCH_DMA_MINALIGN,
 				     sizeof(*nand_chip.buffers));
-	nand_chip.oob_poi = nand_chip.buffers->databuf + mtd.writesize;
+	nand_chip.oob_poi = nand_chip.buffers->databuf + mtd->writesize;
 	/* setup flash layout (does not scan as we override that) */
-	mtd.size = nand_chip.chipsize;
-	nand_chip.scan_bbt(&mtd);
+	mtd->size = nand_chip.chipsize;
+	nand_chip.scan_bbt(mtd);
 
-	printf("%llu MiB\n", (mtd.size / (1024 * 1024)));
+	printf("%llu MiB\n", (mtd->size / (1024 * 1024)));
 	return 0;
 }
 
@@ -180,20 +180,20 @@ int nand_spl_load_image(uint32_t offs, unsigned int size, void *buf)
 
 	if (mxs_nand_init())
 		return -ENODEV;
-	chip = mtd.priv;
+	chip = mtd_to_nand(mtd);
 	page = offs >> chip->page_shift;
-	nand_page_per_block = mtd.erasesize / mtd.writesize;
+	nand_page_per_block = mtd->erasesize / mtd->writesize;
 
 	debug("%s offset:0x%08x len:%d page:%d\n", __func__, offs, size, page);
 
-	size = roundup(size, mtd.writesize);
+	size = roundup(size, mtd->writesize);
 	while (sz < size) {
-		if (mxs_read_page_ecc(&mtd, buf, page) < 0)
+		if (mxs_read_page_ecc(mtd, buf, page) < 0)
 			return -1;
-		sz += mtd.writesize;
-		offs += mtd.writesize;
+		sz += mtd->writesize;
+		offs += mtd->writesize;
 		page++;
-		buf += mtd.writesize;
+		buf += mtd->writesize;
 
 		/*
 		 * Check if we have crossed a block boundary, and if so
@@ -204,10 +204,10 @@ int nand_spl_load_image(uint32_t offs, unsigned int size, void *buf)
 			 * Yes, new block. See if this block is good. If not,
 			 * loop until we find a good block.
 			 */
-			while (is_badblock(&mtd, offs, 1)) {
+			while (is_badblock(mtd, offs, 1)) {
 				page = page + nand_page_per_block;
 				/* Check i we've reached the end of flash. */
-				if (page >= mtd.size >> chip->page_shift)
+				if (page >= mtd->size >> chip->page_shift)
 					return -ENOMEM;
 			}
 		}
diff --git a/drivers/mtd/nand/nand.c b/drivers/mtd/nand/nand.c
index 8f0a921..0551241 100644
--- a/drivers/mtd/nand/nand.c
+++ b/drivers/mtd/nand/nand.c
@@ -9,6 +9,7 @@
 #include <common.h>
 #include <nand.h>
 #include <errno.h>
+#include <linux/mtd/concat.h>
 
 #ifndef CONFIG_SYS_NAND_BASE_LIST
 #define CONFIG_SYS_NAND_BASE_LIST { CONFIG_SYS_NAND_BASE }
@@ -19,7 +20,7 @@ DECLARE_GLOBAL_DATA_PTR;
 int nand_curr_device = -1;
 
 
-nand_info_t nand_info[CONFIG_SYS_MAX_NAND_DEVICE];
+struct mtd_info *nand_info[CONFIG_SYS_MAX_NAND_DEVICE];
 
 #ifndef CONFIG_SYS_NAND_SELF_INIT
 static struct nand_chip nand_chip[CONFIG_SYS_MAX_NAND_DEVICE];
@@ -30,15 +31,25 @@ static char dev_name[CONFIG_SYS_MAX_NAND_DEVICE][8];
 
 static unsigned long total_nand_size; /* in kiB */
 
-/* Register an initialized NAND mtd device with the U-Boot NAND command. */
-int nand_register(int devnum)
+int nand_mtd_to_devnum(struct mtd_info *mtd)
 {
-	struct mtd_info *mtd;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(nand_info); i++) {
+		if (mtd && nand_info[i] == mtd)
+			return i;
+	}
+
+	return -ENODEV;
+}
 
+/* Register an initialized NAND mtd device with the U-Boot NAND command. */
+int nand_register(int devnum, struct mtd_info *mtd)
+{
 	if (devnum >= CONFIG_SYS_MAX_NAND_DEVICE)
 		return -EINVAL;
 
-	mtd = &nand_info[devnum];
+	nand_info[devnum] = mtd;
 
 	sprintf(dev_name[devnum], "nand%d", devnum);
 	mtd->name = dev_name[devnum];
@@ -62,15 +73,14 @@ int nand_register(int devnum)
 #ifndef CONFIG_SYS_NAND_SELF_INIT
 static void nand_init_chip(int i)
 {
-	struct mtd_info *mtd = &nand_info[i];
 	struct nand_chip *nand = &nand_chip[i];
+	struct mtd_info *mtd = nand_to_mtd(nand);
 	ulong base_addr = base_address[i];
 	int maxchips = CONFIG_SYS_NAND_MAX_CHIPS;
 
 	if (maxchips < 1)
 		maxchips = 1;
 
-	mtd->priv = nand;
 	nand->IO_ADDR_R = nand->IO_ADDR_W = (void  __iomem *)base_addr;
 
 	if (board_nand_init(nand))
@@ -79,7 +89,45 @@ static void nand_init_chip(int i)
 	if (nand_scan(mtd, maxchips))
 		return;
 
-	nand_register(i);
+	nand_register(i, mtd);
+}
+#endif
+
+#ifdef CONFIG_MTD_CONCAT
+static void create_mtd_concat(void)
+{
+	struct mtd_info *nand_info_list[CONFIG_SYS_MAX_NAND_DEVICE];
+	int nand_devices_found = 0;
+	int i;
+
+	for (i = 0; i < CONFIG_SYS_MAX_NAND_DEVICE; i++) {
+		if (nand_info[i] != NULL) {
+			nand_info_list[nand_devices_found] = nand_info[i];
+			nand_devices_found++;
+		}
+	}
+	if (nand_devices_found > 1) {
+		struct mtd_info *mtd;
+		char c_mtd_name[16];
+
+		/*
+		 * We detected multiple devices. Concatenate them together.
+		 */
+		sprintf(c_mtd_name, "nand%d", nand_devices_found);
+		mtd = mtd_concat_create(nand_info_list, nand_devices_found,
+					c_mtd_name);
+
+		if (mtd == NULL)
+			return;
+
+		nand_register(nand_devices_found, mtd);
+	}
+
+	return;
+}
+#else
+static void create_mtd_concat(void)
+{
 }
 #endif
 
@@ -100,6 +148,9 @@ void nand_init(void)
 	/*
 	 * Select the chip in the board/cpu specific driver
 	 */
-	board_nand_select_device(nand_info[nand_curr_device].priv, nand_curr_device);
+	board_nand_select_device(mtd_to_nand(nand_info[nand_curr_device]),
+				 nand_curr_device);
 #endif
+
+	create_mtd_concat();
 }
diff --git a/drivers/mtd/nand/nand_base.c b/drivers/mtd/nand/nand_base.c
index 9e8fc1f..74c563c 100644
--- a/drivers/mtd/nand/nand_base.c
+++ b/drivers/mtd/nand/nand_base.c
@@ -1,6 +1,4 @@
 /*
- *  drivers/mtd/nand.c
- *
  *  Overview:
  *   This is the generic MTD driver for NAND flash devices. It should be
  *   capable of working with almost all NAND chips currently available.
@@ -45,8 +43,6 @@
 #include <asm/io.h>
 #include <asm/errno.h>
 
-static bool is_module_text_address(unsigned long addr) {return 0;}
-
 /* Define default oob placement schemes for large and small page devices */
 static struct nand_ecclayout nand_oob_8 = {
 	.eccbytes = 3,
@@ -105,7 +101,7 @@ DEFINE_LED_TRIGGER(nand_led_trigger);
 static int check_offs_len(struct mtd_info *mtd,
 					loff_t ofs, uint64_t len)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	int ret = 0;
 
 	/* Start address must align on block boundary */
@@ -131,7 +127,7 @@ static int check_offs_len(struct mtd_info *mtd,
  */
 static void nand_release_device(struct mtd_info *mtd)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 
 	/* De-select the NAND device */
 	chip->select_chip(mtd, -1);
@@ -145,7 +141,7 @@ static void nand_release_device(struct mtd_info *mtd)
  */
 uint8_t nand_read_byte(struct mtd_info *mtd)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	return readb(chip->IO_ADDR_R);
 }
 
@@ -158,7 +154,7 @@ uint8_t nand_read_byte(struct mtd_info *mtd)
  */
 static uint8_t nand_read_byte16(struct mtd_info *mtd)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	return (uint8_t) cpu_to_le16(readw(chip->IO_ADDR_R));
 }
 
@@ -170,7 +166,7 @@ static uint8_t nand_read_byte16(struct mtd_info *mtd)
  */
 static u16 nand_read_word(struct mtd_info *mtd)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	return readw(chip->IO_ADDR_R);
 }
 
@@ -183,7 +179,7 @@ static u16 nand_read_word(struct mtd_info *mtd)
  */
 static void nand_select_chip(struct mtd_info *mtd, int chipnr)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 
 	switch (chipnr) {
 	case -1:
@@ -206,7 +202,7 @@ static void nand_select_chip(struct mtd_info *mtd, int chipnr)
  */
 static void nand_write_byte(struct mtd_info *mtd, uint8_t byte)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 
 	chip->write_buf(mtd, &byte, 1);
 }
@@ -220,7 +216,7 @@ static void nand_write_byte(struct mtd_info *mtd, uint8_t byte)
  */
 static void nand_write_byte16(struct mtd_info *mtd, uint8_t byte)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	uint16_t word = byte;
 
 	/*
@@ -287,7 +283,7 @@ static void iowrite16_rep(void *addr, void *buf, int len)
  */
 void nand_write_buf(struct mtd_info *mtd, const uint8_t *buf, int len)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 
 	iowrite8_rep(chip->IO_ADDR_W, buf, len);
 }
@@ -302,7 +298,7 @@ void nand_write_buf(struct mtd_info *mtd, const uint8_t *buf, int len)
  */
 void nand_read_buf(struct mtd_info *mtd, uint8_t *buf, int len)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 
 	ioread8_rep(chip->IO_ADDR_R, buf, len);
 }
@@ -317,7 +313,7 @@ void nand_read_buf(struct mtd_info *mtd, uint8_t *buf, int len)
  */
 void nand_write_buf16(struct mtd_info *mtd, const uint8_t *buf, int len)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	u16 *p = (u16 *) buf;
 
 	iowrite16_rep(chip->IO_ADDR_W, p, len >> 1);
@@ -333,7 +329,7 @@ void nand_write_buf16(struct mtd_info *mtd, const uint8_t *buf, int len)
  */
 void nand_read_buf16(struct mtd_info *mtd, uint8_t *buf, int len)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	u16 *p = (u16 *) buf;
 
 	ioread16_rep(chip->IO_ADDR_R, p, len >> 1);
@@ -343,14 +339,13 @@ void nand_read_buf16(struct mtd_info *mtd, uint8_t *buf, int len)
  * nand_block_bad - [DEFAULT] Read bad block marker from the chip
  * @mtd: MTD device structure
  * @ofs: offset from device start
- * @getchip: 0, if the chip is already selected
  *
  * Check, if the block is bad.
  */
-static int nand_block_bad(struct mtd_info *mtd, loff_t ofs, int getchip)
+static int nand_block_bad(struct mtd_info *mtd, loff_t ofs)
 {
-	int page, chipnr, res = 0, i = 0;
-	struct nand_chip *chip = mtd->priv;
+	int page, res = 0, i = 0;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	u16 bad;
 
 	if (chip->bbt_options & NAND_BBT_SCANLASTPAGE)
@@ -358,15 +353,6 @@ static int nand_block_bad(struct mtd_info *mtd, loff_t ofs, int getchip)
 
 	page = (int)(ofs >> chip->page_shift) & chip->pagemask;
 
-	if (getchip) {
-		chipnr = (int)(ofs >> chip->chip_shift);
-
-		nand_get_device(mtd, FL_READING);
-
-		/* Select the NAND device */
-		chip->select_chip(mtd, chipnr);
-	}
-
 	do {
 		if (chip->options & NAND_BUSWIDTH_16) {
 			chip->cmdfunc(mtd, NAND_CMD_READOOB,
@@ -391,11 +377,6 @@ static int nand_block_bad(struct mtd_info *mtd, loff_t ofs, int getchip)
 		i++;
 	} while (!res && i < 2 && (chip->bbt_options & NAND_BBT_SCAN2NDPAGE));
 
-	if (getchip) {
-		chip->select_chip(mtd, -1);
-		nand_release_device(mtd);
-	}
-
 	return res;
 }
 
@@ -410,7 +391,7 @@ static int nand_block_bad(struct mtd_info *mtd, loff_t ofs, int getchip)
  */
 static int nand_default_block_markbad(struct mtd_info *mtd, loff_t ofs)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	struct mtd_oob_ops ops;
 	uint8_t buf[2] = { 0, 0 };
 	int ret = 0, res, i = 0;
@@ -460,7 +441,7 @@ static int nand_default_block_markbad(struct mtd_info *mtd, loff_t ofs)
 */
 static int nand_block_markbad_lowlevel(struct mtd_info *mtd, loff_t ofs)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	int res, ret = 0;
 
 	if (!(chip->bbt_options & NAND_BBT_NO_OOB_BBM)) {
@@ -501,7 +482,7 @@ static int nand_block_markbad_lowlevel(struct mtd_info *mtd, loff_t ofs)
  */
 static int nand_check_wp(struct mtd_info *mtd)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 
 	/* Broken xD cards report WP despite being writable */
 	if (chip->options & NAND_BROKEN_XD)
@@ -521,7 +502,7 @@ static int nand_check_wp(struct mtd_info *mtd)
  */
 static int nand_block_isreserved(struct mtd_info *mtd, loff_t ofs)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 
 	if (!chip->bbt)
 		return 0;
@@ -533,16 +514,14 @@ static int nand_block_isreserved(struct mtd_info *mtd, loff_t ofs)
  * nand_block_checkbad - [GENERIC] Check if a block is marked bad
  * @mtd: MTD device structure
  * @ofs: offset from device start
- * @getchip: 0, if the chip is already selected
  * @allowbbt: 1, if its allowed to access the bbt area
  *
  * Check, if the block is bad. Either by reading the bad block table or
  * calling of the scan function.
  */
-static int nand_block_checkbad(struct mtd_info *mtd, loff_t ofs, int getchip,
-			       int allowbbt)
+static int nand_block_checkbad(struct mtd_info *mtd, loff_t ofs, int allowbbt)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 
 	if (!(chip->options & NAND_SKIP_BBTSCAN) &&
 	    !(chip->options & NAND_BBT_SCANNED)) {
@@ -551,17 +530,22 @@ static int nand_block_checkbad(struct mtd_info *mtd, loff_t ofs, int getchip,
 	}
 
 	if (!chip->bbt)
-		return chip->block_bad(mtd, ofs, getchip);
+		return chip->block_bad(mtd, ofs);
 
 	/* Return info from the table */
 	return nand_isbad_bbt(mtd, ofs, allowbbt);
 }
 
-/* Wait for the ready pin, after a command. The timeout is caught later. */
+/**
+ * nand_wait_ready - [GENERIC] Wait for the ready pin after commands.
+ * @mtd: MTD device structure
+ *
+ * Wait for the ready pin after a command, and warn if a timeout occurs.
+ */
 void nand_wait_ready(struct mtd_info *mtd)
 {
-	struct nand_chip *chip = mtd->priv;
-	u32 timeo = (CONFIG_SYS_HZ * 20) / 1000;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	u32 timeo = (CONFIG_SYS_HZ * 400) / 1000;
 	u32 time_start;
 
 	time_start = get_timer(0);
@@ -571,6 +555,9 @@ void nand_wait_ready(struct mtd_info *mtd)
 			if (chip->dev_ready(mtd))
 				break;
 	}
+
+	if (!chip->dev_ready(mtd))
+		pr_warn("timeout while waiting for chip to become ready\n");
 }
 EXPORT_SYMBOL_GPL(nand_wait_ready);
 
@@ -583,7 +570,7 @@ EXPORT_SYMBOL_GPL(nand_wait_ready);
  */
 static void nand_wait_status_ready(struct mtd_info *mtd, unsigned long timeo)
 {
-	register struct nand_chip *chip = mtd->priv;
+	register struct nand_chip *chip = mtd_to_nand(mtd);
 	u32 time_start;
 
 	timeo = (CONFIG_SYS_HZ * timeo) / 1000;
@@ -608,7 +595,7 @@ static void nand_wait_status_ready(struct mtd_info *mtd, unsigned long timeo)
 static void nand_command(struct mtd_info *mtd, unsigned int command,
 			 int column, int page_addr)
 {
-	register struct nand_chip *chip = mtd->priv;
+	register struct nand_chip *chip = mtd_to_nand(mtd);
 	int ctrl = NAND_CTRL_CLE | NAND_CTRL_CHANGE;
 
 	/* Write out the command to the device */
@@ -711,7 +698,7 @@ static void nand_command(struct mtd_info *mtd, unsigned int command,
 static void nand_command_lp(struct mtd_info *mtd, unsigned int command,
 			    int column, int page_addr)
 {
-	register struct nand_chip *chip = mtd->priv;
+	register struct nand_chip *chip = mtd_to_nand(mtd);
 
 	/* Emulate NAND_CMD_READOOB */
 	if (command == NAND_CMD_READOOB) {
@@ -835,7 +822,7 @@ static void panic_nand_get_device(struct nand_chip *chip,
 static int
 nand_get_device(struct mtd_info *mtd, int new_state)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	chip->state = new_state;
 	return 0;
 }
@@ -871,15 +858,13 @@ static void panic_nand_wait(struct mtd_info *mtd, struct nand_chip *chip,
  * @mtd: MTD device structure
  * @chip: NAND chip structure
  *
- * Wait for command done. This applies to erase and program only. Erase can
- * take up to 400ms and program up to 20ms according to general NAND and
- * SmartMedia specs.
+ * Wait for command done. This applies to erase and program only.
  */
 static int nand_wait(struct mtd_info *mtd, struct nand_chip *chip)
 {
 
-	int status, state = chip->state;
-	unsigned long timeo = (state == FL_ERASING ? 400 : 20);
+	int status;
+	unsigned long timeo = 400;
 
 	led_trigger_event(nand_led_trigger, LED_FULL);
 
@@ -912,6 +897,135 @@ static int nand_wait(struct mtd_info *mtd, struct nand_chip *chip)
 	return status;
 }
 
+#define BITS_PER_BYTE 8
+
+/**
+ * nand_check_erased_buf - check if a buffer contains (almost) only 0xff data
+ * @buf: buffer to test
+ * @len: buffer length
+ * @bitflips_threshold: maximum number of bitflips
+ *
+ * Check if a buffer contains only 0xff, which means the underlying region
+ * has been erased and is ready to be programmed.
+ * The bitflips_threshold specify the maximum number of bitflips before
+ * considering the region is not erased.
+ * Note: The logic of this function has been extracted from the memweight
+ * implementation, except that nand_check_erased_buf function exit before
+ * testing the whole buffer if the number of bitflips exceed the
+ * bitflips_threshold value.
+ *
+ * Returns a positive number of bitflips less than or equal to
+ * bitflips_threshold, or -ERROR_CODE for bitflips in excess of the
+ * threshold.
+ */
+static int nand_check_erased_buf(void *buf, int len, int bitflips_threshold)
+{
+	const unsigned char *bitmap = buf;
+	int bitflips = 0;
+	int weight;
+
+	for (; len && ((uintptr_t)bitmap) % sizeof(long);
+	     len--, bitmap++) {
+		weight = hweight8(*bitmap);
+		bitflips += BITS_PER_BYTE - weight;
+		if (unlikely(bitflips > bitflips_threshold))
+			return -EBADMSG;
+	}
+
+	for (; len >= 4; len -= 4, bitmap += 4) {
+		weight = hweight32(*((u32 *)bitmap));
+		bitflips += 32 - weight;
+		if (unlikely(bitflips > bitflips_threshold))
+			return -EBADMSG;
+	}
+
+	for (; len > 0; len--, bitmap++) {
+		weight = hweight8(*bitmap);
+		bitflips += BITS_PER_BYTE - weight;
+		if (unlikely(bitflips > bitflips_threshold))
+			return -EBADMSG;
+	}
+
+	return bitflips;
+}
+
+/**
+ * nand_check_erased_ecc_chunk - check if an ECC chunk contains (almost) only
+ *				 0xff data
+ * @data: data buffer to test
+ * @datalen: data length
+ * @ecc: ECC buffer
+ * @ecclen: ECC length
+ * @extraoob: extra OOB buffer
+ * @extraooblen: extra OOB length
+ * @bitflips_threshold: maximum number of bitflips
+ *
+ * Check if a data buffer and its associated ECC and OOB data contains only
+ * 0xff pattern, which means the underlying region has been erased and is
+ * ready to be programmed.
+ * The bitflips_threshold specify the maximum number of bitflips before
+ * considering the region as not erased.
+ *
+ * Note:
+ * 1/ ECC algorithms are working on pre-defined block sizes which are usually
+ *    different from the NAND page size. When fixing bitflips, ECC engines will
+ *    report the number of errors per chunk, and the NAND core infrastructure
+ *    expect you to return the maximum number of bitflips for the whole page.
+ *    This is why you should always use this function on a single chunk and
+ *    not on the whole page. After checking each chunk you should update your
+ *    max_bitflips value accordingly.
+ * 2/ When checking for bitflips in erased pages you should not only check
+ *    the payload data but also their associated ECC data, because a user might
+ *    have programmed almost all bits to 1 but a few. In this case, we
+ *    shouldn't consider the chunk as erased, and checking ECC bytes prevent
+ *    this case.
+ * 3/ The extraoob argument is optional, and should be used if some of your OOB
+ *    data are protected by the ECC engine.
+ *    It could also be used if you support subpages and want to attach some
+ *    extra OOB data to an ECC chunk.
+ *
+ * Returns a positive number of bitflips less than or equal to
+ * bitflips_threshold, or -ERROR_CODE for bitflips in excess of the
+ * threshold. In case of success, the passed buffers are filled with 0xff.
+ */
+int nand_check_erased_ecc_chunk(void *data, int datalen,
+				void *ecc, int ecclen,
+				void *extraoob, int extraooblen,
+				int bitflips_threshold)
+{
+	int data_bitflips = 0, ecc_bitflips = 0, extraoob_bitflips = 0;
+
+	data_bitflips = nand_check_erased_buf(data, datalen,
+					      bitflips_threshold);
+	if (data_bitflips < 0)
+		return data_bitflips;
+
+	bitflips_threshold -= data_bitflips;
+
+	ecc_bitflips = nand_check_erased_buf(ecc, ecclen, bitflips_threshold);
+	if (ecc_bitflips < 0)
+		return ecc_bitflips;
+
+	bitflips_threshold -= ecc_bitflips;
+
+	extraoob_bitflips = nand_check_erased_buf(extraoob, extraooblen,
+						  bitflips_threshold);
+	if (extraoob_bitflips < 0)
+		return extraoob_bitflips;
+
+	if (data_bitflips)
+		memset(data, 0xff, datalen);
+
+	if (ecc_bitflips)
+		memset(ecc, 0xff, ecclen);
+
+	if (extraoob_bitflips)
+		memset(extraoob, 0xff, extraooblen);
+
+	return data_bitflips + ecc_bitflips + extraoob_bitflips;
+}
+EXPORT_SYMBOL(nand_check_erased_ecc_chunk);
+
 /**
  * nand_read_page_raw - [INTERN] read raw page data without ecc
  * @mtd: mtd info structure
@@ -1103,6 +1217,16 @@ static int nand_read_subpage(struct mtd_info *mtd, struct nand_chip *chip,
 
 		stat = chip->ecc.correct(mtd, p,
 			&chip->buffers->ecccode[i], &chip->buffers->ecccalc[i]);
+		if (stat == -EBADMSG &&
+		    (chip->ecc.options & NAND_ECC_GENERIC_ERASED_CHECK)) {
+			/* check for empty pages with bitflips */
+			stat = nand_check_erased_ecc_chunk(p, chip->ecc.size,
+						&chip->buffers->ecccode[i],
+						chip->ecc.bytes,
+						NULL, 0,
+						chip->ecc.strength);
+		}
+
 		if (stat < 0) {
 			mtd->ecc_stats.failed++;
 		} else {
@@ -1152,6 +1276,15 @@ static int nand_read_page_hwecc(struct mtd_info *mtd, struct nand_chip *chip,
 		int stat;
 
 		stat = chip->ecc.correct(mtd, p, &ecc_code[i], &ecc_calc[i]);
+		if (stat == -EBADMSG &&
+		    (chip->ecc.options & NAND_ECC_GENERIC_ERASED_CHECK)) {
+			/* check for empty pages with bitflips */
+			stat = nand_check_erased_ecc_chunk(p, eccsize,
+						&ecc_code[i], eccbytes,
+						NULL, 0,
+						chip->ecc.strength);
+		}
+
 		if (stat < 0) {
 			mtd->ecc_stats.failed++;
 		} else {
@@ -1204,6 +1337,15 @@ static int nand_read_page_hwecc_oob_first(struct mtd_info *mtd,
 		chip->ecc.calculate(mtd, p, &ecc_calc[i]);
 
 		stat = chip->ecc.correct(mtd, p, &ecc_code[i], NULL);
+		if (stat == -EBADMSG &&
+		    (chip->ecc.options & NAND_ECC_GENERIC_ERASED_CHECK)) {
+			/* check for empty pages with bitflips */
+			stat = nand_check_erased_ecc_chunk(p, eccsize,
+						&ecc_code[i], eccbytes,
+						NULL, 0,
+						chip->ecc.strength);
+		}
+
 		if (stat < 0) {
 			mtd->ecc_stats.failed++;
 		} else {
@@ -1231,6 +1373,7 @@ static int nand_read_page_syndrome(struct mtd_info *mtd, struct nand_chip *chip,
 	int i, eccsize = chip->ecc.size;
 	int eccbytes = chip->ecc.bytes;
 	int eccsteps = chip->ecc.steps;
+	int eccpadbytes = eccbytes + chip->ecc.prepad + chip->ecc.postpad;
 	uint8_t *p = buf;
 	uint8_t *oob = chip->oob_poi;
 	unsigned int max_bitflips = 0;
@@ -1250,19 +1393,29 @@ static int nand_read_page_syndrome(struct mtd_info *mtd, struct nand_chip *chip,
 		chip->read_buf(mtd, oob, eccbytes);
 		stat = chip->ecc.correct(mtd, p, oob, NULL);
 
-		if (stat < 0) {
-			mtd->ecc_stats.failed++;
-		} else {
-			mtd->ecc_stats.corrected += stat;
-			max_bitflips = max_t(unsigned int, max_bitflips, stat);
-		}
-
 		oob += eccbytes;
 
 		if (chip->ecc.postpad) {
 			chip->read_buf(mtd, oob, chip->ecc.postpad);
 			oob += chip->ecc.postpad;
 		}
+
+		if (stat == -EBADMSG &&
+		    (chip->ecc.options & NAND_ECC_GENERIC_ERASED_CHECK)) {
+			/* check for empty pages with bitflips */
+			stat = nand_check_erased_ecc_chunk(p, chip->ecc.size,
+							   oob - eccpadbytes,
+							   eccpadbytes,
+							   NULL, 0,
+							   chip->ecc.strength);
+		}
+
+		if (stat < 0) {
+			mtd->ecc_stats.failed++;
+		} else {
+			mtd->ecc_stats.corrected += stat;
+			max_bitflips = max_t(unsigned int, max_bitflips, stat);
+		}
 	}
 
 	/* Calculate remaining oob bytes */
@@ -1332,7 +1485,7 @@ static uint8_t *nand_transfer_oob(struct nand_chip *chip, uint8_t *oob,
  */
 static int nand_setup_read_retry(struct mtd_info *mtd, int retry_mode)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 
 	pr_debug("setting READ RETRY mode %d\n", retry_mode);
 
@@ -1357,12 +1510,11 @@ static int nand_do_read_ops(struct mtd_info *mtd, loff_t from,
 			    struct mtd_oob_ops *ops)
 {
 	int chipnr, page, realpage, col, bytes, aligned, oob_required;
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	int ret = 0;
 	uint32_t readlen = ops->len;
 	uint32_t oobreadlen = ops->ooblen;
-	uint32_t max_oobsize = ops->mode == MTD_OPS_AUTO_OOB ?
-		mtd->oobavail : mtd->oobsize;
+	uint32_t max_oobsize = mtd_oobavail(mtd, ops);
 
 	uint8_t *bufpoi, *oob, *buf;
 	int use_bufpoi;
@@ -1700,7 +1852,7 @@ static int nand_do_read_oob(struct mtd_info *mtd, loff_t from,
 			    struct mtd_oob_ops *ops)
 {
 	int page, realpage, chipnr;
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	struct mtd_ecc_stats stats;
 	int readlen = ops->ooblen;
 	int len;
@@ -1712,10 +1864,7 @@ static int nand_do_read_oob(struct mtd_info *mtd, loff_t from,
 
 	stats = mtd->ecc_stats;
 
-	if (ops->mode == MTD_OPS_AUTO_OOB)
-		len = chip->ecc.layout->oobavail;
-	else
-		len = mtd->oobsize;
+	len = mtd_oobavail(mtd, ops);
 
 	if (unlikely(ops->ooboffs >= len)) {
 		pr_debug("%s: attempt to start read outside oob\n",
@@ -1840,11 +1989,12 @@ out:
  * @chip: nand chip info structure
  * @buf: data buffer
  * @oob_required: must write chip->oob_poi to OOB
+ * @page: page number to write
  *
  * Not for syndrome calculating ECC controllers, which use a special oob layout.
  */
 static int nand_write_page_raw(struct mtd_info *mtd, struct nand_chip *chip,
-				const uint8_t *buf, int oob_required)
+			       const uint8_t *buf, int oob_required, int page)
 {
 	chip->write_buf(mtd, buf, mtd->writesize);
 	if (oob_required)
@@ -1859,12 +2009,14 @@ static int nand_write_page_raw(struct mtd_info *mtd, struct nand_chip *chip,
  * @chip: nand chip info structure
  * @buf: data buffer
  * @oob_required: must write chip->oob_poi to OOB
+ * @page: page number to write
  *
  * We need a special oob layout and handling even when ECC isn't checked.
  */
 static int nand_write_page_raw_syndrome(struct mtd_info *mtd,
 					struct nand_chip *chip,
-					const uint8_t *buf, int oob_required)
+					const uint8_t *buf, int oob_required,
+					int page)
 {
 	int eccsize = chip->ecc.size;
 	int eccbytes = chip->ecc.bytes;
@@ -1901,9 +2053,11 @@ static int nand_write_page_raw_syndrome(struct mtd_info *mtd,
  * @chip: nand chip info structure
  * @buf: data buffer
  * @oob_required: must write chip->oob_poi to OOB
+ * @page: page number to write
  */
 static int nand_write_page_swecc(struct mtd_info *mtd, struct nand_chip *chip,
-				  const uint8_t *buf, int oob_required)
+				 const uint8_t *buf, int oob_required,
+				 int page)
 {
 	int i, eccsize = chip->ecc.size;
 	int eccbytes = chip->ecc.bytes;
@@ -1919,7 +2073,7 @@ static int nand_write_page_swecc(struct mtd_info *mtd, struct nand_chip *chip,
 	for (i = 0; i < chip->ecc.total; i++)
 		chip->oob_poi[eccpos[i]] = ecc_calc[i];
 
-	return chip->ecc.write_page_raw(mtd, chip, buf, 1);
+	return chip->ecc.write_page_raw(mtd, chip, buf, 1, page);
 }
 
 /**
@@ -1928,9 +2082,11 @@ static int nand_write_page_swecc(struct mtd_info *mtd, struct nand_chip *chip,
  * @chip: nand chip info structure
  * @buf: data buffer
  * @oob_required: must write chip->oob_poi to OOB
+ * @page: page number to write
  */
 static int nand_write_page_hwecc(struct mtd_info *mtd, struct nand_chip *chip,
-				  const uint8_t *buf, int oob_required)
+				  const uint8_t *buf, int oob_required,
+				  int page)
 {
 	int i, eccsize = chip->ecc.size;
 	int eccbytes = chip->ecc.bytes;
@@ -1962,11 +2118,12 @@ static int nand_write_page_hwecc(struct mtd_info *mtd, struct nand_chip *chip,
  * @data_len:	data length
  * @buf:	data buffer
  * @oob_required: must write chip->oob_poi to OOB
+ * @page: page number to write
  */
 static int nand_write_subpage_hwecc(struct mtd_info *mtd,
 				struct nand_chip *chip, uint32_t offset,
 				uint32_t data_len, const uint8_t *buf,
-				int oob_required)
+				int oob_required, int page)
 {
 	uint8_t *oob_buf  = chip->oob_poi;
 	uint8_t *ecc_calc = chip->buffers->ecccalc;
@@ -2021,13 +2178,15 @@ static int nand_write_subpage_hwecc(struct mtd_info *mtd,
  * @chip: nand chip info structure
  * @buf: data buffer
  * @oob_required: must write chip->oob_poi to OOB
+ * @page: page number to write
  *
  * The hw generator calculates the error syndrome automatically. Therefore we
  * need a special oob layout and handling.
  */
 static int nand_write_page_syndrome(struct mtd_info *mtd,
 				    struct nand_chip *chip,
-				    const uint8_t *buf, int oob_required)
+				    const uint8_t *buf, int oob_required,
+				    int page)
 {
 	int i, eccsize = chip->ecc.size;
 	int eccbytes = chip->ecc.bytes;
@@ -2091,12 +2250,13 @@ static int nand_write_page(struct mtd_info *mtd, struct nand_chip *chip,
 
 	if (unlikely(raw))
 		status = chip->ecc.write_page_raw(mtd, chip, buf,
-							oob_required);
+						  oob_required, page);
 	else if (subpage)
 		status = chip->ecc.write_subpage(mtd, chip, offset, data_len,
-							 buf, oob_required);
+						 buf, oob_required, page);
 	else
-		status = chip->ecc.write_page(mtd, chip, buf, oob_required);
+		status = chip->ecc.write_page(mtd, chip, buf, oob_required,
+					      page);
 
 	if (status < 0)
 		return status;
@@ -2139,7 +2299,7 @@ static int nand_write_page(struct mtd_info *mtd, struct nand_chip *chip,
 static uint8_t *nand_fill_oob(struct mtd_info *mtd, uint8_t *oob, size_t len,
 			      struct mtd_oob_ops *ops)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 
 	/*
 	 * Initialise to all 0xFF, to avoid the possibility of left over OOB
@@ -2199,12 +2359,11 @@ static int nand_do_write_ops(struct mtd_info *mtd, loff_t to,
 			     struct mtd_oob_ops *ops)
 {
 	int chipnr, realpage, page, blockmask, column;
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	uint32_t writelen = ops->len;
 
 	uint32_t oobwritelen = ops->ooblen;
-	uint32_t oobmaxlen = ops->mode == MTD_OPS_AUTO_OOB ?
-				mtd->oobavail : mtd->oobsize;
+	uint32_t oobmaxlen = mtd_oobavail(mtd, ops);
 
 	uint8_t *oob = ops->oobbuf;
 	uint8_t *buf = ops->datbuf;
@@ -2328,7 +2487,7 @@ err_out:
 static int panic_nand_write(struct mtd_info *mtd, loff_t to, size_t len,
 			    size_t *retlen, const uint8_t *buf)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	struct mtd_oob_ops ops;
 	int ret;
 
@@ -2388,15 +2547,12 @@ static int nand_do_write_oob(struct mtd_info *mtd, loff_t to,
 			     struct mtd_oob_ops *ops)
 {
 	int chipnr, page, status, len;
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 
 	pr_debug("%s: to = 0x%08x, len = %i\n",
 			 __func__, (unsigned int)to, (int)ops->ooblen);
 
-	if (ops->mode == MTD_OPS_AUTO_OOB)
-		len = chip->ecc.layout->oobavail;
-	else
-		len = mtd->oobsize;
+	len = mtd_oobavail(mtd, ops);
 
 	/* Do not allow write past end of page */
 	if ((ops->ooboffs + ops->ooblen) > len) {
@@ -2513,7 +2669,7 @@ out:
  */
 static int single_erase(struct mtd_info *mtd, int page)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	/* Send commands to erase a block */
 	chip->cmdfunc(mtd, NAND_CMD_ERASE1, -1, page);
 	chip->cmdfunc(mtd, NAND_CMD_ERASE2, -1, -1);
@@ -2545,7 +2701,7 @@ int nand_erase_nand(struct mtd_info *mtd, struct erase_info *instr,
 		    int allowbbt)
 {
 	int page, status, pages_per_block, ret, chipnr;
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	loff_t len;
 
 	pr_debug("%s: start = 0x%012llx, len = %llu\n",
@@ -2586,7 +2742,7 @@ int nand_erase_nand(struct mtd_info *mtd, struct erase_info *instr,
 
 		/* Check if we have a bad block, we do not erase bad blocks! */
 		if (!instr->scrub && nand_block_checkbad(mtd, ((loff_t) page) <<
-					chip->page_shift, 0, allowbbt)) {
+					chip->page_shift, allowbbt)) {
 			pr_warn("%s: attempt to erase a bad block at page 0x%08x\n",
 				    __func__, page);
 			instr->state = MTD_ERASE_FAILED;
@@ -2673,7 +2829,20 @@ static void nand_sync(struct mtd_info *mtd)
  */
 static int nand_block_isbad(struct mtd_info *mtd, loff_t offs)
 {
-	return nand_block_checkbad(mtd, offs, 1, 0);
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	int chipnr = (int)(offs >> chip->chip_shift);
+	int ret;
+
+	/* Select the NAND device */
+	nand_get_device(mtd, FL_READING);
+	chip->select_chip(mtd, chipnr);
+
+	ret = nand_block_checkbad(mtd, offs, 0);
+
+	chip->select_chip(mtd, -1);
+	nand_release_device(mtd);
+
+	return ret;
 }
 
 /**
@@ -2745,9 +2914,6 @@ static int nand_onfi_get_features(struct mtd_info *mtd, struct nand_chip *chip,
 		return -EINVAL;
 #endif
 
-	/* clear the sub feature parameters */
-	memset(subfeature_param, 0, ONFI_SUBFEATURE_PARAM_LEN);
-
 	chip->cmdfunc(mtd, NAND_CMD_GET_FEATURES, addr, -1);
 	for (i = 0; i < ONFI_SUBFEATURE_PARAM_LEN; ++i)
 		*subfeature_param++ = chip->read_byte(mtd);
@@ -2908,7 +3074,7 @@ ext_out:
 
 static int nand_setup_read_retry_micron(struct mtd_info *mtd, int retry_mode)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	uint8_t feature[ONFI_SUBFEATURE_PARAM_LEN] = {retry_mode};
 
 	return chip->onfi_set_features(mtd, chip, ONFI_FEATURE_ADDR_READ_RETRY,
@@ -3480,7 +3646,7 @@ static struct nand_flash_dev *nand_get_flash_type(struct mtd_info *mtd,
 			if (find_full_id_nand(mtd, chip, type, id_data, &busw))
 				goto ident_done;
 		} else if (*dev_id == type->dev_id) {
-				break;
+			break;
 		}
 	}
 
@@ -3503,10 +3669,7 @@ static struct nand_flash_dev *nand_get_flash_type(struct mtd_info *mtd,
 
 	chip->chipsize = (uint64_t)type->chipsize << 20;
 
-	if (!type->pagesize && chip->init_size) {
-		/* Set the pagesize, oobsize, erasesize by the driver */
-		busw = chip->init_size(mtd, chip, id_data);
-	} else if (!type->pagesize) {
+	if (!type->pagesize) {
 		/* Decode parameters from extended ID */
 		nand_decode_ext_id(mtd, chip, id_data, &busw);
 	} else {
@@ -3610,13 +3773,12 @@ ident_done:
  * This is the first phase of the normal nand_scan() function. It reads the
  * flash ID and sets up MTD fields accordingly.
  *
- * The mtd->owner field must be set to the module of the caller.
  */
 int nand_scan_ident(struct mtd_info *mtd, int maxchips,
 		    struct nand_flash_dev *table)
 {
 	int i, nand_maf_id, nand_dev_id;
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	struct nand_flash_dev *type;
 
 	/* Set the default functions */
@@ -3680,7 +3842,7 @@ EXPORT_SYMBOL(nand_scan_ident);
  */
 static bool nand_ecc_strength_good(struct mtd_info *mtd)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	struct nand_ecc_ctrl *ecc = &chip->ecc;
 	int corr, ds_corr;
 
@@ -3709,7 +3871,7 @@ static bool nand_ecc_strength_good(struct mtd_info *mtd)
 int nand_scan_tail(struct mtd_info *mtd)
 {
 	int i;
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	struct nand_ecc_ctrl *ecc = &chip->ecc;
 	struct nand_buffers *nbuf;
 
@@ -3786,7 +3948,7 @@ int nand_scan_tail(struct mtd_info *mtd)
 			ecc->write_oob = nand_write_oob_std;
 		if (!ecc->read_subpage)
 			ecc->read_subpage = nand_read_subpage;
-		if (!ecc->write_subpage)
+		if (!ecc->write_subpage && ecc->hwctl && ecc->calculate)
 			ecc->write_subpage = nand_write_subpage_hwecc;
 
 	case NAND_ECC_HW_SYNDROME:
@@ -3864,10 +4026,8 @@ int nand_scan_tail(struct mtd_info *mtd)
 		}
 
 		/* See nand_bch_init() for details. */
-		ecc->bytes = DIV_ROUND_UP(
-				ecc->strength * fls(8 * ecc->size), 8);
-		ecc->priv = nand_bch_init(mtd, ecc->size, ecc->bytes,
-					       &ecc->layout);
+		ecc->bytes = 0;
+		ecc->priv = nand_bch_init(mtd);
 		if (!ecc->priv) {
 			pr_warn("BCH ECC initialization failed!\n");
 			BUG();
@@ -3902,11 +4062,11 @@ int nand_scan_tail(struct mtd_info *mtd)
 	 * The number of bytes available for a client to place data into
 	 * the out of band area.
 	 */
-	ecc->layout->oobavail = 0;
-	for (i = 0; ecc->layout->oobfree[i].length
-			&& i < ARRAY_SIZE(ecc->layout->oobfree); i++)
-		ecc->layout->oobavail += ecc->layout->oobfree[i].length;
-	mtd->oobavail = ecc->layout->oobavail;
+	mtd->oobavail = 0;
+	if (ecc->layout) {
+		for (i = 0; ecc->layout->oobfree[i].length; i++)
+			mtd->oobavail += ecc->layout->oobfree[i].length;
+	}
 
 	/* ECC sanity check: warn if it's too weak */
 	if (!nand_ecc_strength_good(mtd))
@@ -3991,18 +4151,6 @@ int nand_scan_tail(struct mtd_info *mtd)
 }
 EXPORT_SYMBOL(nand_scan_tail);
 
-/*
- * is_module_text_address() isn't exported, and it's mostly a pointless
- * test if this is a module _anyway_ -- they'd have to try _really_ hard
- * to call us from in-kernel code if the core NAND support is modular.
- */
-#ifdef MODULE
-#define caller_is_module() (1)
-#else
-#define caller_is_module() \
-	is_module_text_address((unsigned long)__builtin_return_address(0))
-#endif
-
 /**
  * nand_scan - [NAND Interface] Scan for the NAND device
  * @mtd: MTD device structure
@@ -4010,19 +4158,12 @@ EXPORT_SYMBOL(nand_scan_tail);
  *
  * This fills out all the uninitialized function pointers with the defaults.
  * The flash ID is read and the mtd/chip structures are filled with the
- * appropriate values. The mtd->owner field must be set to the module of the
- * caller.
+ * appropriate values.
  */
 int nand_scan(struct mtd_info *mtd, int maxchips)
 {
 	int ret;
 
-	/* Many callers got this wrong, so check for it for a while... */
-	if (!mtd->owner && caller_is_module()) {
-		pr_crit("%s called with NULL mtd->owner!\n", __func__);
-		BUG();
-	}
-
 	ret = nand_scan_ident(mtd, maxchips, NULL);
 	if (!ret)
 		ret = nand_scan_tail(mtd);
@@ -4030,9 +4171,6 @@ int nand_scan(struct mtd_info *mtd, int maxchips)
 }
 EXPORT_SYMBOL(nand_scan);
 
-module_init(nand_base_init);
-module_exit(nand_base_exit);
-
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Steven J. Hill <sjhill@realitydiluted.com>");
 MODULE_AUTHOR("Thomas Gleixner <tglx@linutronix.de>");
diff --git a/drivers/mtd/nand/nand_bbt.c b/drivers/mtd/nand/nand_bbt.c
index 00f28a4..74c4c9a 100644
--- a/drivers/mtd/nand/nand_bbt.c
+++ b/drivers/mtd/nand/nand_bbt.c
@@ -1,6 +1,4 @@
 /*
- *  drivers/mtd/nand_bbt.c
- *
  *  Overview:
  *   Bad block table support for the NAND driver
  *
@@ -65,7 +63,6 @@
 #include <linux/mtd/mtd.h>
 #include <linux/mtd/bbm.h>
 #include <linux/mtd/nand.h>
-#include <linux/mtd/nand_ecc.h>
 #include <linux/bitops.h>
 #include <linux/string.h>
 
@@ -173,7 +170,7 @@ static int read_bbt(struct mtd_info *mtd, uint8_t *buf, int page, int num,
 		struct nand_bbt_descr *td, int offs)
 {
 	int res, ret = 0, i, j, act = 0;
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 	size_t retlen, len, totlen;
 	loff_t from;
 	int bits = td->options & NAND_BBT_NRBITS_MSK;
@@ -264,7 +261,7 @@ static int read_bbt(struct mtd_info *mtd, uint8_t *buf, int page, int num,
  */
 static int read_abs_bbt(struct mtd_info *mtd, uint8_t *buf, struct nand_bbt_descr *td, int chip)
 {
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 	int res = 0, i;
 
 	if (td->options & NAND_BBT_PERCHIP) {
@@ -389,7 +386,7 @@ static u32 bbt_get_ver_offs(struct mtd_info *mtd, struct nand_bbt_descr *td)
 static void read_abs_bbts(struct mtd_info *mtd, uint8_t *buf,
 			  struct nand_bbt_descr *td, struct nand_bbt_descr *md)
 {
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 
 	/* Read the primary version, if available */
 	if (td->options & NAND_BBT_VERSION) {
@@ -455,7 +452,7 @@ static int scan_block_fast(struct mtd_info *mtd, struct nand_bbt_descr *bd,
 static int create_bbt(struct mtd_info *mtd, uint8_t *buf,
 	struct nand_bbt_descr *bd, int chip)
 {
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 	int i, numblocks, numpages;
 	int startblock;
 	loff_t from;
@@ -524,7 +521,7 @@ static int create_bbt(struct mtd_info *mtd, uint8_t *buf,
  */
 static int search_bbt(struct mtd_info *mtd, uint8_t *buf, struct nand_bbt_descr *td)
 {
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 	int i, chips;
 	int startblock, block, dir;
 	int scanlen = mtd->writesize + mtd->oobsize;
@@ -619,7 +616,7 @@ static int write_bbt(struct mtd_info *mtd, uint8_t *buf,
 		     struct nand_bbt_descr *td, struct nand_bbt_descr *md,
 		     int chipsel)
 {
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 	struct erase_info einfo;
 	int i, res, chip = 0;
 	int bits, startblock, dir, page, offs, numblocks, sft, sftmsk;
@@ -718,7 +715,7 @@ static int write_bbt(struct mtd_info *mtd, uint8_t *buf,
 		/* Must we save the block contents? */
 		if (td->options & NAND_BBT_SAVECONTENT) {
 			/* Make it block aligned */
-			to &= ~((loff_t)((1 << this->bbt_erase_shift) - 1));
+			to &= ~(((loff_t)1 << this->bbt_erase_shift) - 1);
 			len = 1 << this->bbt_erase_shift;
 			res = mtd_read(mtd, to, len, &retlen, buf);
 			if (res < 0) {
@@ -820,7 +817,7 @@ static int write_bbt(struct mtd_info *mtd, uint8_t *buf,
  */
 static inline int nand_memory_bbt(struct mtd_info *mtd, struct nand_bbt_descr *bd)
 {
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 
 	return create_bbt(mtd, this->buffers->databuf, bd, -1);
 }
@@ -839,7 +836,7 @@ static inline int nand_memory_bbt(struct mtd_info *mtd, struct nand_bbt_descr *b
 static int check_create(struct mtd_info *mtd, uint8_t *buf, struct nand_bbt_descr *bd)
 {
 	int i, chips, writeops, create, chipsel, res, res2;
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 	struct nand_bbt_descr *td = this->bbt_td;
 	struct nand_bbt_descr *md = this->bbt_md;
 	struct nand_bbt_descr *rd, *rd2;
@@ -963,7 +960,7 @@ static int check_create(struct mtd_info *mtd, uint8_t *buf, struct nand_bbt_desc
  */
 static void mark_bbt_region(struct mtd_info *mtd, struct nand_bbt_descr *td)
 {
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 	int i, j, chips, block, nrblocks, update;
 	uint8_t oldval;
 
@@ -1023,7 +1020,7 @@ static void mark_bbt_region(struct mtd_info *mtd, struct nand_bbt_descr *td)
  */
 static void verify_bbt_descr(struct mtd_info *mtd, struct nand_bbt_descr *bd)
 {
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 	u32 pattern_len;
 	u32 bits;
 	u32 table_size;
@@ -1073,15 +1070,15 @@ static void verify_bbt_descr(struct mtd_info *mtd, struct nand_bbt_descr *bd)
  * The bad block table memory is allocated here. It must be freed by calling
  * the nand_free_bbt function.
  */
-int nand_scan_bbt(struct mtd_info *mtd, struct nand_bbt_descr *bd)
+static int nand_scan_bbt(struct mtd_info *mtd, struct nand_bbt_descr *bd)
 {
-	struct nand_chip *this = mtd->priv;
-	int len, res = 0;
+	struct nand_chip *this = mtd_to_nand(mtd);
+	int len, res;
 	uint8_t *buf;
 	struct nand_bbt_descr *td = this->bbt_td;
 	struct nand_bbt_descr *md = this->bbt_md;
 
-	len = mtd->size >> (this->bbt_erase_shift + 2);
+	len = (mtd->size >> (this->bbt_erase_shift + 2)) ? : 1;
 	/*
 	 * Allocate memory (2bit per block) and clear the memory bad block
 	 * table.
@@ -1097,10 +1094,9 @@ int nand_scan_bbt(struct mtd_info *mtd, struct nand_bbt_descr *bd)
 	if (!td) {
 		if ((res = nand_memory_bbt(mtd, bd))) {
 			pr_err("nand_bbt: can't scan flash and build the RAM-based BBT\n");
-			kfree(this->bbt);
-			this->bbt = NULL;
+			goto err;
 		}
-		return res;
+		return 0;
 	}
 	verify_bbt_descr(mtd, td);
 	verify_bbt_descr(mtd, md);
@@ -1110,9 +1106,8 @@ int nand_scan_bbt(struct mtd_info *mtd, struct nand_bbt_descr *bd)
 	len += (len >> this->page_shift) * mtd->oobsize;
 	buf = vmalloc(len);
 	if (!buf) {
-		kfree(this->bbt);
-		this->bbt = NULL;
-		return -ENOMEM;
+		res = -ENOMEM;
+		goto err;
 	}
 
 	/* Is the bbt at a given page? */
@@ -1124,6 +1119,8 @@ int nand_scan_bbt(struct mtd_info *mtd, struct nand_bbt_descr *bd)
 	}
 
 	res = check_create(mtd, buf, bd);
+	if (res)
+		goto err;
 
 	/* Prevent the bbt regions from erasing / writing */
 	mark_bbt_region(mtd, td);
@@ -1131,6 +1128,11 @@ int nand_scan_bbt(struct mtd_info *mtd, struct nand_bbt_descr *bd)
 		mark_bbt_region(mtd, md);
 
 	vfree(buf);
+	return 0;
+
+err:
+	kfree(this->bbt);
+	this->bbt = NULL;
 	return res;
 }
 
@@ -1143,7 +1145,7 @@ int nand_scan_bbt(struct mtd_info *mtd, struct nand_bbt_descr *bd)
  */
 static int nand_update_bbt(struct mtd_info *mtd, loff_t offs)
 {
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 	int len, res = 0;
 	int chip, chipsel;
 	uint8_t *buf;
@@ -1277,7 +1279,7 @@ static int nand_create_badblock_pattern(struct nand_chip *this)
  */
 int nand_default_bbt(struct mtd_info *mtd)
 {
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 	int ret;
 
 	/* Is a flash based bad block table requested? */
@@ -1313,7 +1315,7 @@ int nand_default_bbt(struct mtd_info *mtd)
  */
 int nand_isreserved_bbt(struct mtd_info *mtd, loff_t offs)
 {
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 	int block;
 
 	block = (int)(offs >> this->bbt_erase_shift);
@@ -1328,7 +1330,7 @@ int nand_isreserved_bbt(struct mtd_info *mtd, loff_t offs)
  */
 int nand_isbad_bbt(struct mtd_info *mtd, loff_t offs, int allowbbt)
 {
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 	int block, res;
 
 	block = (int)(offs >> this->bbt_erase_shift);
@@ -1355,7 +1357,7 @@ int nand_isbad_bbt(struct mtd_info *mtd, loff_t offs, int allowbbt)
  */
 int nand_markbad_bbt(struct mtd_info *mtd, loff_t offs)
 {
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 	int block, ret = 0;
 
 	block = (int)(offs >> this->bbt_erase_shift);
@@ -1369,5 +1371,3 @@ int nand_markbad_bbt(struct mtd_info *mtd, loff_t offs)
 
 	return ret;
 }
-
-EXPORT_SYMBOL(nand_scan_bbt);
diff --git a/drivers/mtd/nand/nand_bch.c b/drivers/mtd/nand/nand_bch.c
index 35d2140..c145203 100644
--- a/drivers/mtd/nand/nand_bch.c
+++ b/drivers/mtd/nand/nand_bch.c
@@ -41,7 +41,7 @@ struct nand_bch_control {
 int nand_bch_calculate_ecc(struct mtd_info *mtd, const unsigned char *buf,
 			   unsigned char *code)
 {
-	const struct nand_chip *chip = mtd->priv;
+	const struct nand_chip *chip = mtd_to_nand(mtd);
 	struct nand_bch_control *nbc = chip->ecc.priv;
 	unsigned int i;
 
@@ -67,7 +67,7 @@ int nand_bch_calculate_ecc(struct mtd_info *mtd, const unsigned char *buf,
 int nand_bch_correct_data(struct mtd_info *mtd, unsigned char *buf,
 			  unsigned char *read_ecc, unsigned char *calc_ecc)
 {
-	const struct nand_chip *chip = mtd->priv;
+	const struct nand_chip *chip = mtd_to_nand(mtd);
 	struct nand_bch_control *nbc = chip->ecc.priv;
 	unsigned int *errloc = nbc->errloc;
 	int i, count;
@@ -86,7 +86,7 @@ int nand_bch_correct_data(struct mtd_info *mtd, unsigned char *buf,
 		}
 	} else if (count < 0) {
 		printk(KERN_ERR "ecc unrecoverable error\n");
-		count = -1;
+		count = -EBADMSG;
 	}
 	return count;
 }
@@ -94,9 +94,6 @@ int nand_bch_correct_data(struct mtd_info *mtd, unsigned char *buf,
 /**
  * nand_bch_init - [NAND Interface] Initialize NAND BCH error correction
  * @mtd:	MTD block structure
- * @eccsize:	ecc block size in bytes
- * @eccbytes:	ecc length in bytes
- * @ecclayout:	output default layout
  *
  * Returns:
  *  a pointer to a new NAND BCH control structure, or NULL upon failure
@@ -110,14 +107,21 @@ int nand_bch_correct_data(struct mtd_info *mtd, unsigned char *buf,
  * @eccsize = 512  (thus, m=13 is the smallest integer such that 2^m-1 > 512*8)
  * @eccbytes = 7   (7 bytes are required to store m*t = 13*4 = 52 bits)
  */
-struct nand_bch_control *
-nand_bch_init(struct mtd_info *mtd, unsigned int eccsize, unsigned int eccbytes,
-	      struct nand_ecclayout **ecclayout)
+struct nand_bch_control *nand_bch_init(struct mtd_info *mtd)
 {
+	struct nand_chip *nand = mtd_to_nand(mtd);
 	unsigned int m, t, eccsteps, i;
-	struct nand_ecclayout *layout;
+	struct nand_ecclayout *layout = nand->ecc.layout;
 	struct nand_bch_control *nbc = NULL;
 	unsigned char *erased_page;
+	unsigned int eccsize = nand->ecc.size;
+	unsigned int eccbytes = nand->ecc.bytes;
+	unsigned int eccstrength = nand->ecc.strength;
+
+	if (!eccbytes && eccstrength) {
+		eccbytes = DIV_ROUND_UP(eccstrength * fls(8 * eccsize), 8);
+		nand->ecc.bytes = eccbytes;
+	}
 
 	if (!eccsize || !eccbytes) {
 		printk(KERN_WARNING "ecc parameters not supplied\n");
@@ -145,7 +149,7 @@ nand_bch_init(struct mtd_info *mtd, unsigned int eccsize, unsigned int eccbytes,
 	eccsteps = mtd->writesize/eccsize;
 
 	/* if no ecc placement scheme was provided, build one */
-	if (!*ecclayout) {
+	if (!layout) {
 
 		/* handle large page devices only */
 		if (mtd->oobsize < 64) {
@@ -171,7 +175,7 @@ nand_bch_init(struct mtd_info *mtd, unsigned int eccsize, unsigned int eccbytes,
 		layout->oobfree[0].offset = 2;
 		layout->oobfree[0].length = mtd->oobsize-2-layout->eccbytes;
 
-		*ecclayout = layout;
+		nand->ecc.layout = layout;
 	}
 
 	/* sanity checks */
@@ -179,7 +183,7 @@ nand_bch_init(struct mtd_info *mtd, unsigned int eccsize, unsigned int eccbytes,
 		printk(KERN_WARNING "eccsize %u is too large\n", eccsize);
 		goto fail;
 	}
-	if ((*ecclayout)->eccbytes != (eccsteps*eccbytes)) {
+	if (layout->eccbytes != (eccsteps*eccbytes)) {
 		printk(KERN_WARNING "invalid ecc layout\n");
 		goto fail;
 	}
@@ -203,6 +207,9 @@ nand_bch_init(struct mtd_info *mtd, unsigned int eccsize, unsigned int eccbytes,
 	for (i = 0; i < eccbytes; i++)
 		nbc->eccmask[i] ^= 0xff;
 
+	if (!eccstrength)
+		nand->ecc.strength = (eccbytes * 8) / fls(8 * eccsize);
+
 	return nbc;
 fail:
 	nand_bch_free(nbc);
diff --git a/drivers/mtd/nand/nand_ids.c b/drivers/mtd/nand/nand_ids.c
index fdd0074..561d2cd 100644
--- a/drivers/mtd/nand/nand_ids.c
+++ b/drivers/mtd/nand/nand_ids.c
@@ -1,6 +1,4 @@
 /*
- *  drivers/mtd/nandids.c
- *
  *  Copyright (C) 2002 Thomas Gleixner (tglx@linutronix.de)
  *
  * This program is free software; you can redistribute it and/or modify
@@ -41,6 +39,10 @@ struct nand_flash_dev nand_flash_ids[] = {
 	 * listed by full ID. We list them first so that we can easily identify
 	 * the most specific match.
 	 */
+	{"TC58NVG0S3E 1G 3.3V 8-bit",
+		{ .id = {0x98, 0xd1, 0x90, 0x15, 0x76, 0x14, 0x01, 0x00} },
+		  SZ_2K, SZ_128, SZ_128K, 0, 8, 64, NAND_ECC_INFO(1, SZ_512),
+		  2 },
 	{"TC58NVG2S0F 4G 3.3V 8-bit",
 		{ .id = {0x98, 0xdc, 0x90, 0x26, 0x76, 0x15, 0x01, 0x08} },
 		  SZ_4K, SZ_512, SZ_256K, 0, 8, 224, NAND_ECC_INFO(4, SZ_512) },
@@ -58,8 +60,8 @@ struct nand_flash_dev nand_flash_ids[] = {
 		  SZ_16K, SZ_8K, SZ_4M, 0, 6, 1280, NAND_ECC_INFO(40, SZ_1K) },
 	{"H27UCG8T2ATR-BC 64G 3.3V 8-bit",
 		{ .id = {0xad, 0xde, 0x94, 0xda, 0x74, 0xc4} },
-		  SZ_8K, SZ_8K, SZ_2M, 0, 6, 640, NAND_ECC_INFO(40, SZ_1K),
-		  4 },
+		  SZ_8K, SZ_8K, SZ_2M, NAND_NEED_SCRAMBLING, 6, 640,
+		  NAND_ECC_INFO(40, SZ_1K), 4 },
 
 	LEGACY_ID_NAND("NAND 4MiB 5V 8-bit",   0x6B, 4, SZ_8K, SP_OPTIONS),
 	LEGACY_ID_NAND("NAND 4MiB 3,3V 8-bit", 0xE3, 4, SZ_8K, SP_OPTIONS),
diff --git a/drivers/mtd/nand/nand_plat.c b/drivers/mtd/nand/nand_plat.c
index 37a0206..335c3e3 100644
--- a/drivers/mtd/nand/nand_plat.c
+++ b/drivers/mtd/nand/nand_plat.c
@@ -25,7 +25,7 @@
 
 static void plat_cmd_ctrl(struct mtd_info *mtd, int cmd, unsigned int ctrl)
 {
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 
 	if (cmd == NAND_CMD_NONE)
 		return;
@@ -39,7 +39,7 @@ static void plat_cmd_ctrl(struct mtd_info *mtd, int cmd, unsigned int ctrl)
 #ifdef NAND_PLAT_DEV_READY
 static int plat_dev_ready(struct mtd_info *mtd)
 {
-	return NAND_PLAT_DEV_READY((struct nand_chip *)mtd->priv);
+	return NAND_PLAT_DEV_READY((struct nand_chip *)mtd_to_nand(mtd));
 }
 #else
 # define plat_dev_ready NULL
diff --git a/drivers/mtd/nand/nand_spl_simple.c b/drivers/mtd/nand/nand_spl_simple.c
index e69f662..b023e00 100644
--- a/drivers/mtd/nand/nand_spl_simple.c
+++ b/drivers/mtd/nand/nand_spl_simple.c
@@ -11,7 +11,7 @@
 #include <linux/mtd/nand_ecc.h>
 
 static int nand_ecc_pos[] = CONFIG_SYS_NAND_ECCPOS;
-static nand_info_t mtd;
+static struct mtd_info *mtd;
 static struct nand_chip nand_chip;
 
 #define ECCSTEPS	(CONFIG_SYS_NAND_PAGE_SIZE / \
@@ -26,32 +26,32 @@ static struct nand_chip nand_chip;
 static int nand_command(int block, int page, uint32_t offs,
 	u8 cmd)
 {
-	struct nand_chip *this = mtd.priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 	int page_addr = page + block * CONFIG_SYS_NAND_PAGE_COUNT;
 
-	while (!this->dev_ready(&mtd))
+	while (!this->dev_ready(mtd))
 		;
 
 	/* Begin command latch cycle */
-	this->cmd_ctrl(&mtd, cmd, NAND_CTRL_CLE | NAND_CTRL_CHANGE);
+	this->cmd_ctrl(mtd, cmd, NAND_CTRL_CLE | NAND_CTRL_CHANGE);
 	/* Set ALE and clear CLE to start address cycle */
 	/* Column address */
-	this->cmd_ctrl(&mtd, offs, NAND_CTRL_ALE | NAND_CTRL_CHANGE);
-	this->cmd_ctrl(&mtd, page_addr & 0xff, NAND_CTRL_ALE); /* A[16:9] */
-	this->cmd_ctrl(&mtd, (page_addr >> 8) & 0xff,
+	this->cmd_ctrl(mtd, offs, NAND_CTRL_ALE | NAND_CTRL_CHANGE);
+	this->cmd_ctrl(mtd, page_addr & 0xff, NAND_CTRL_ALE); /* A[16:9] */
+	this->cmd_ctrl(mtd, (page_addr >> 8) & 0xff,
 		       NAND_CTRL_ALE); /* A[24:17] */
 #ifdef CONFIG_SYS_NAND_4_ADDR_CYCLE
 	/* One more address cycle for devices > 32MiB */
-	this->cmd_ctrl(&mtd, (page_addr >> 16) & 0x0f,
+	this->cmd_ctrl(mtd, (page_addr >> 16) & 0x0f,
 		       NAND_CTRL_ALE); /* A[28:25] */
 #endif
 	/* Latch in address */
-	this->cmd_ctrl(&mtd, NAND_CMD_NONE, NAND_NCE | NAND_CTRL_CHANGE);
+	this->cmd_ctrl(mtd, NAND_CMD_NONE, NAND_NCE | NAND_CTRL_CHANGE);
 
 	/*
 	 * Wait a while for the data to be ready
 	 */
-	while (!this->dev_ready(&mtd))
+	while (!this->dev_ready(mtd))
 		;
 
 	return 0;
@@ -63,12 +63,12 @@ static int nand_command(int block, int page, uint32_t offs,
 static int nand_command(int block, int page, uint32_t offs,
 	u8 cmd)
 {
-	struct nand_chip *this = mtd.priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 	int page_addr = page + block * CONFIG_SYS_NAND_PAGE_COUNT;
 	void (*hwctrl)(struct mtd_info *mtd, int cmd,
 			unsigned int ctrl) = this->cmd_ctrl;
 
-	while (!this->dev_ready(&mtd))
+	while (!this->dev_ready(mtd))
 		;
 
 	/* Emulate NAND_CMD_READOOB */
@@ -82,30 +82,30 @@ static int nand_command(int block, int page, uint32_t offs,
 		offs >>= 1;
 
 	/* Begin command latch cycle */
-	hwctrl(&mtd, cmd, NAND_CTRL_CLE | NAND_CTRL_CHANGE);
+	hwctrl(mtd, cmd, NAND_CTRL_CLE | NAND_CTRL_CHANGE);
 	/* Set ALE and clear CLE to start address cycle */
 	/* Column address */
-	hwctrl(&mtd, offs & 0xff,
-		       NAND_CTRL_ALE | NAND_CTRL_CHANGE); /* A[7:0] */
-	hwctrl(&mtd, (offs >> 8) & 0xff, NAND_CTRL_ALE); /* A[11:9] */
+	hwctrl(mtd, offs & 0xff,
+		    NAND_CTRL_ALE | NAND_CTRL_CHANGE); /* A[7:0] */
+	hwctrl(mtd, (offs >> 8) & 0xff, NAND_CTRL_ALE); /* A[11:9] */
 	/* Row address */
-	hwctrl(&mtd, (page_addr & 0xff), NAND_CTRL_ALE); /* A[19:12] */
-	hwctrl(&mtd, ((page_addr >> 8) & 0xff),
-		       NAND_CTRL_ALE); /* A[27:20] */
+	hwctrl(mtd, (page_addr & 0xff), NAND_CTRL_ALE); /* A[19:12] */
+	hwctrl(mtd, ((page_addr >> 8) & 0xff),
+		    NAND_CTRL_ALE); /* A[27:20] */
 #ifdef CONFIG_SYS_NAND_5_ADDR_CYCLE
 	/* One more address cycle for devices > 128MiB */
-	hwctrl(&mtd, (page_addr >> 16) & 0x0f,
+	hwctrl(mtd, (page_addr >> 16) & 0x0f,
 		       NAND_CTRL_ALE); /* A[31:28] */
 #endif
 	/* Latch in address */
-	hwctrl(&mtd, NAND_CMD_READSTART,
-		       NAND_CTRL_CLE | NAND_CTRL_CHANGE);
-	hwctrl(&mtd, NAND_CMD_NONE, NAND_NCE | NAND_CTRL_CHANGE);
+	hwctrl(mtd, NAND_CMD_READSTART,
+		    NAND_CTRL_CLE | NAND_CTRL_CHANGE);
+	hwctrl(mtd, NAND_CMD_NONE, NAND_NCE | NAND_CTRL_CHANGE);
 
 	/*
 	 * Wait a while for the data to be ready
 	 */
-	while (!this->dev_ready(&mtd))
+	while (!this->dev_ready(mtd))
 		;
 
 	return 0;
@@ -114,7 +114,7 @@ static int nand_command(int block, int page, uint32_t offs,
 
 static int nand_is_bad_block(int block)
 {
-	struct nand_chip *this = mtd.priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 	u_char bb_data[2];
 
 	nand_command(block, 0, CONFIG_SYS_NAND_BAD_BLOCK_POS,
@@ -124,11 +124,11 @@ static int nand_is_bad_block(int block)
 	 * Read one byte (or two if it's a 16 bit chip).
 	 */
 	if (this->options & NAND_BUSWIDTH_16) {
-		this->read_buf(&mtd, bb_data, 2);
+		this->read_buf(mtd, bb_data, 2);
 		if (bb_data[0] != 0xff || bb_data[1] != 0xff)
 			return 1;
 	} else {
-		this->read_buf(&mtd, bb_data, 1);
+		this->read_buf(mtd, bb_data, 1);
 		if (bb_data[0] != 0xff)
 			return 1;
 	}
@@ -139,7 +139,7 @@ static int nand_is_bad_block(int block)
 #if defined(CONFIG_SYS_NAND_HW_ECC_OOBFIRST)
 static int nand_read_page(int block, int page, uchar *dst)
 {
-	struct nand_chip *this = mtd.priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 	u_char ecc_calc[ECCTOTAL];
 	u_char ecc_code[ECCTOTAL];
 	u_char oob_data[CONFIG_SYS_NAND_OOBSIZE];
@@ -150,7 +150,7 @@ static int nand_read_page(int block, int page, uchar *dst)
 	uint8_t *p = dst;
 
 	nand_command(block, page, 0, NAND_CMD_READOOB);
-	this->read_buf(&mtd, oob_data, CONFIG_SYS_NAND_OOBSIZE);
+	this->read_buf(mtd, oob_data, CONFIG_SYS_NAND_OOBSIZE);
 	nand_command(block, page, 0, NAND_CMD_READ0);
 
 	/* Pick the ECC bytes out of the oob data */
@@ -159,10 +159,10 @@ static int nand_read_page(int block, int page, uchar *dst)
 
 
 	for (i = 0; eccsteps; eccsteps--, i += eccbytes, p += eccsize) {
-		this->ecc.hwctl(&mtd, NAND_ECC_READ);
-		this->read_buf(&mtd, p, eccsize);
-		this->ecc.calculate(&mtd, p, &ecc_calc[i]);
-		this->ecc.correct(&mtd, p, &ecc_code[i], &ecc_calc[i]);
+		this->ecc.hwctl(mtd, NAND_ECC_READ);
+		this->read_buf(mtd, p, eccsize);
+		this->ecc.calculate(mtd, p, &ecc_calc[i]);
+		this->ecc.correct(mtd, p, &ecc_code[i], &ecc_calc[i]);
 	}
 
 	return 0;
@@ -170,7 +170,7 @@ static int nand_read_page(int block, int page, uchar *dst)
 #else
 static int nand_read_page(int block, int page, void *dst)
 {
-	struct nand_chip *this = mtd.priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 	u_char ecc_calc[ECCTOTAL];
 	u_char ecc_code[ECCTOTAL];
 	u_char oob_data[CONFIG_SYS_NAND_OOBSIZE];
@@ -184,11 +184,11 @@ static int nand_read_page(int block, int page, void *dst)
 
 	for (i = 0; eccsteps; eccsteps--, i += eccbytes, p += eccsize) {
 		if (this->ecc.mode != NAND_ECC_SOFT)
-			this->ecc.hwctl(&mtd, NAND_ECC_READ);
-		this->read_buf(&mtd, p, eccsize);
-		this->ecc.calculate(&mtd, p, &ecc_calc[i]);
+			this->ecc.hwctl(mtd, NAND_ECC_READ);
+		this->read_buf(mtd, p, eccsize);
+		this->ecc.calculate(mtd, p, &ecc_calc[i]);
 	}
-	this->read_buf(&mtd, oob_data, CONFIG_SYS_NAND_OOBSIZE);
+	this->read_buf(mtd, oob_data, CONFIG_SYS_NAND_OOBSIZE);
 
 	/* Pick the ECC bytes out of the oob data */
 	for (i = 0; i < ECCTOTAL; i++)
@@ -202,7 +202,7 @@ static int nand_read_page(int block, int page, void *dst)
 		 * from correct_data(). We just hope that all possible errors
 		 * are corrected by this routine.
 		 */
-		this->ecc.correct(&mtd, p, &ecc_code[i], &ecc_calc[i]);
+		this->ecc.correct(mtd, p, &ecc_code[i], &ecc_calc[i]);
 	}
 
 	return 0;
@@ -249,7 +249,7 @@ void nand_init(void)
 	/*
 	 * Init board specific nand support
 	 */
-	mtd.priv = &nand_chip;
+	mtd = &nand_chip.mtd;
 	nand_chip.IO_ADDR_R = nand_chip.IO_ADDR_W =
 		(void  __iomem *)CONFIG_SYS_NAND_BASE;
 	board_nand_init(&nand_chip);
@@ -262,12 +262,12 @@ void nand_init(void)
 #endif
 
 	if (nand_chip.select_chip)
-		nand_chip.select_chip(&mtd, 0);
+		nand_chip.select_chip(mtd, 0);
 }
 
 /* Unselect after operation */
 void nand_deselect(void)
 {
 	if (nand_chip.select_chip)
-		nand_chip.select_chip(&mtd, -1);
+		nand_chip.select_chip(mtd, -1);
 }
diff --git a/drivers/mtd/nand/nand_util.c b/drivers/mtd/nand/nand_util.c
index 71285b6..5bba66a 100644
--- a/drivers/mtd/nand/nand_util.c
+++ b/drivers/mtd/nand/nand_util.c
@@ -42,25 +42,26 @@ typedef struct mtd_info		mtd_info_t;
  * nand_erase_opts: - erase NAND flash with support for various options
  *		      (jffs2 formatting)
  *
- * @param meminfo	NAND device to erase
+ * @param mtd		nand mtd instance to erase
  * @param opts		options,  @see struct nand_erase_options
  * @return		0 in case of success
  *
  * This code is ported from flash_eraseall.c from Linux mtd utils by
  * Arcom Control System Ltd.
  */
-int nand_erase_opts(nand_info_t *meminfo, const nand_erase_options_t *opts)
+int nand_erase_opts(struct mtd_info *mtd,
+		    const nand_erase_options_t *opts)
 {
 	struct jffs2_unknown_node cleanmarker;
 	erase_info_t erase;
 	unsigned long erase_length, erased_length; /* in blocks */
 	int result;
 	int percent_complete = -1;
-	const char *mtd_device = meminfo->name;
+	const char *mtd_device = mtd->name;
 	struct mtd_oob_ops oob_opts;
-	struct nand_chip *chip = meminfo->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 
-	if ((opts->offset & (meminfo->erasesize - 1)) != 0) {
+	if ((opts->offset & (mtd->erasesize - 1)) != 0) {
 		printf("Attempt to erase non block-aligned data\n");
 		return -1;
 	}
@@ -68,11 +69,11 @@ int nand_erase_opts(nand_info_t *meminfo, const nand_erase_options_t *opts)
 	memset(&erase, 0, sizeof(erase));
 	memset(&oob_opts, 0, sizeof(oob_opts));
 
-	erase.mtd = meminfo;
-	erase.len  = meminfo->erasesize;
+	erase.mtd = mtd;
+	erase.len = mtd->erasesize;
 	erase.addr = opts->offset;
-	erase_length = lldiv(opts->length + meminfo->erasesize - 1,
-			     meminfo->erasesize);
+	erase_length = lldiv(opts->length + mtd->erasesize - 1,
+			     mtd->erasesize);
 
 	cleanmarker.magic = cpu_to_je16(JFFS2_MAGIC_BITMASK);
 	cleanmarker.nodetype = cpu_to_je16(JFFS2_NODETYPE_CLEANMARKER);
@@ -97,7 +98,7 @@ int nand_erase_opts(nand_info_t *meminfo, const nand_erase_options_t *opts)
 
 	for (erased_length = 0;
 	     erased_length < erase_length;
-	     erase.addr += meminfo->erasesize) {
+	     erase.addr += mtd->erasesize) {
 
 		WATCHDOG_RESET();
 
@@ -106,7 +107,7 @@ int nand_erase_opts(nand_info_t *meminfo, const nand_erase_options_t *opts)
 			return -EFBIG;
 		}
 		if (!opts->scrub) {
-			int ret = mtd_block_isbad(meminfo, erase.addr);
+			int ret = mtd_block_isbad(mtd, erase.addr);
 			if (ret > 0) {
 				if (!opts->quiet)
 					printf("\rSkipping bad block at  "
@@ -129,7 +130,7 @@ int nand_erase_opts(nand_info_t *meminfo, const nand_erase_options_t *opts)
 
 		erased_length++;
 
-		result = mtd_erase(meminfo, &erase);
+		result = mtd_erase(mtd, &erase);
 		if (result != 0) {
 			printf("\n%s: MTD Erase failure: %d\n",
 			       mtd_device, result);
@@ -145,9 +146,7 @@ int nand_erase_opts(nand_info_t *meminfo, const nand_erase_options_t *opts)
 			ops.ooboffs = 0;
 			ops.mode = MTD_OPS_AUTO_OOB;
 
-			result = mtd_write_oob(meminfo,
-						    erase.addr,
-						    &ops);
+			result = mtd_write_oob(mtd, erase.addr, &ops);
 			if (result != 0) {
 				printf("\n%s: MTD writeoob failure: %d\n",
 				       mtd_device, result);
@@ -218,7 +217,7 @@ int nand_lock(struct mtd_info *mtd, int tight)
 {
 	int ret = 0;
 	int status;
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 
 	/* select the NAND device */
 	chip->select_chip(mtd, 0);
@@ -268,7 +267,7 @@ int nand_get_lock_status(struct mtd_info *mtd, loff_t offset)
 	int ret = 0;
 	int chipnr;
 	int page;
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 
 	/* select the NAND device */
 	chipnr = (int)(offset >> chip->chip_shift);
@@ -303,7 +302,7 @@ int nand_get_lock_status(struct mtd_info *mtd, loff_t offset)
  * @param mtd		nand mtd instance
  * @param start		start byte address
  * @param length	number of bytes to unlock (must be a multiple of
- *			page size nand->writesize)
+ *			page size mtd->writesize)
  * @param allexcept	if set, unlock everything not selected
  *
  * @return		0 on success, -1 in case of error
@@ -315,7 +314,7 @@ int nand_unlock(struct mtd_info *mtd, loff_t start, size_t length,
 	int chipnr;
 	int status;
 	int page;
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 
 	debug("nand_unlock%s: start: %08llx, length: %zd!\n",
 		allexcept ? " (allexcept)" : "", start, length);
@@ -399,7 +398,7 @@ int nand_unlock(struct mtd_info *mtd, loff_t start, size_t length,
  * Check if there are any bad blocks, and whether length including bad
  * blocks fits into device
  *
- * @param nand NAND device
+ * @param mtd nand mtd instance
  * @param offset offset in flash
  * @param length image length
  * @param used length of flash needed for the requested length
@@ -407,8 +406,8 @@ int nand_unlock(struct mtd_info *mtd, loff_t start, size_t length,
  *         1 if the image fits, but there are bad blocks
  *        -1 if the image does not fit
  */
-static int check_skip_len(nand_info_t *nand, loff_t offset, size_t length,
-		size_t *used)
+static int check_skip_len(struct mtd_info *mtd, loff_t offset, size_t length,
+			  size_t *used)
 {
 	size_t len_excl_bad = 0;
 	int ret = 0;
@@ -417,14 +416,14 @@ static int check_skip_len(nand_info_t *nand, loff_t offset, size_t length,
 		size_t block_len, block_off;
 		loff_t block_start;
 
-		if (offset >= nand->size)
+		if (offset >= mtd->size)
 			return -1;
 
-		block_start = offset & ~(loff_t)(nand->erasesize - 1);
-		block_off = offset & (nand->erasesize - 1);
-		block_len = nand->erasesize - block_off;
+		block_start = offset & ~(loff_t)(mtd->erasesize - 1);
+		block_off = offset & (mtd->erasesize - 1);
+		block_len = mtd->erasesize - block_off;
 
-		if (!nand_block_isbad(nand, block_start))
+		if (!nand_block_isbad(mtd, block_start))
 			len_excl_bad += block_len;
 		else
 			ret = 1;
@@ -441,7 +440,7 @@ static int check_skip_len(nand_info_t *nand, loff_t offset, size_t length,
 }
 
 #ifdef CONFIG_CMD_NAND_TRIMFFS
-static size_t drop_ffs(const nand_info_t *nand, const u_char *buf,
+static size_t drop_ffs(const struct mtd_info *mtd, const u_char *buf,
 			const size_t *len)
 {
 	size_t l = *len;
@@ -453,8 +452,8 @@ static size_t drop_ffs(const nand_info_t *nand, const u_char *buf,
 
 	/* The resulting length must be aligned to the minimum flash I/O size */
 	l = i + 1;
-	l = (l + nand->writesize - 1) / nand->writesize;
-	l *=  nand->writesize;
+	l = (l + mtd->writesize - 1) / mtd->writesize;
+	l *=  mtd->writesize;
 
 	/*
 	 * since the input length may be unaligned, prevent access past the end
@@ -471,16 +470,17 @@ static size_t drop_ffs(const nand_info_t *nand, const u_char *buf,
  * Reads page of NAND and verifies the contents and OOB against the
  * values in ops.
  *
- * @param nand		NAND device
+ * @param mtd		nand mtd instance
  * @param ops		MTD operations, including data to verify
  * @param ofs		offset in flash
  * @return		0 in case of success
  */
-int nand_verify_page_oob(nand_info_t *nand, struct mtd_oob_ops *ops, loff_t ofs)
+int nand_verify_page_oob(struct mtd_info *mtd, struct mtd_oob_ops *ops,
+			 loff_t ofs)
 {
 	int rval;
 	struct mtd_oob_ops vops;
-	size_t verlen = nand->writesize + nand->oobsize;
+	size_t verlen = mtd->writesize + mtd->oobsize;
 
 	memcpy(&vops, ops, sizeof(vops));
 
@@ -489,9 +489,9 @@ int nand_verify_page_oob(nand_info_t *nand, struct mtd_oob_ops *ops, loff_t ofs)
 	if (!vops.datbuf)
 		return -ENOMEM;
 
-	vops.oobbuf = vops.datbuf + nand->writesize;
+	vops.oobbuf = vops.datbuf + mtd->writesize;
 
-	rval = mtd_read_oob(nand, ofs, &vops);
+	rval = mtd_read_oob(mtd, ofs, &vops);
 	if (!rval)
 		rval = memcmp(ops->datbuf, vops.datbuf, vops.len);
 	if (!rval)
@@ -510,17 +510,17 @@ int nand_verify_page_oob(nand_info_t *nand, struct mtd_oob_ops *ops, loff_t ofs)
  * the contents of a buffer.  The offset into the NAND must be
  * page-aligned, and the function doesn't handle skipping bad blocks.
  *
- * @param nand		NAND device
+ * @param mtd		nand mtd instance
  * @param ofs		offset in flash
  * @param len		buffer length
  * @param buf		buffer to read from
  * @return		0 in case of success
  */
-int nand_verify(nand_info_t *nand, loff_t ofs, size_t len, u_char *buf)
+int nand_verify(struct mtd_info *mtd, loff_t ofs, size_t len, u_char *buf)
 {
 	int rval = 0;
 	size_t verofs;
-	size_t verlen = nand->writesize;
+	size_t verlen = mtd->writesize;
 	uint8_t *verbuf = memalign(ARCH_DMA_MINALIGN, verlen);
 
 	if (!verbuf)
@@ -529,8 +529,8 @@ int nand_verify(nand_info_t *nand, loff_t ofs, size_t len, u_char *buf)
 	/* Read the NAND back in page-size groups to limit malloc size */
 	for (verofs = ofs; verofs < ofs + len;
 	     verofs += verlen, buf += verlen) {
-		verlen = min(nand->writesize, (uint32_t)(ofs + len - verofs));
-		rval = nand_read(nand, verofs, &verlen, verbuf);
+		verlen = min(mtd->writesize, (uint32_t)(ofs + len - verofs));
+		rval = nand_read(mtd, verofs, &verlen, verbuf);
 		if (!rval || (rval == -EUCLEAN))
 			rval = memcmp(buf, verbuf, verlen);
 
@@ -558,7 +558,7 @@ int nand_verify(nand_info_t *nand, loff_t ofs, size_t len, u_char *buf)
  * beyond the limit we are passed, length is set to 0 and actual is set
  * to the required length.
  *
- * @param nand  	NAND device
+ * @param mtd		nand mtd instance
  * @param offset	offset in flash
  * @param length	buffer length
  * @param actual	set to size required to write length worth of
@@ -569,8 +569,8 @@ int nand_verify(nand_info_t *nand, loff_t ofs, size_t len, u_char *buf)
  * @param flags		flags modifying the behaviour of the write to NAND
  * @return		0 in case of success
  */
-int nand_write_skip_bad(nand_info_t *nand, loff_t offset, size_t *length,
-		size_t *actual, loff_t lim, u_char *buffer, int flags)
+int nand_write_skip_bad(struct mtd_info *mtd, loff_t offset, size_t *length,
+			size_t *actual, loff_t lim, u_char *buffer, int flags)
 {
 	int rval = 0, blocksize;
 	size_t left_to_write = *length;
@@ -581,7 +581,7 @@ int nand_write_skip_bad(nand_info_t *nand, loff_t offset, size_t *length,
 	if (actual)
 		*actual = 0;
 
-	blocksize = nand->erasesize;
+	blocksize = mtd->erasesize;
 
 	/*
 	 * nand_write() handles unaligned, partial page writes.
@@ -594,13 +594,13 @@ int nand_write_skip_bad(nand_info_t *nand, loff_t offset, size_t *length,
 	 * you should only start a block skipping access at a
 	 * partition boundary).  So don't try to handle that.
 	 */
-	if ((offset & (nand->writesize - 1)) != 0) {
+	if ((offset & (mtd->writesize - 1)) != 0) {
 		printf("Attempt to write non page-aligned data\n");
 		*length = 0;
 		return -EINVAL;
 	}
 
-	need_skip = check_skip_len(nand, offset, *length, &used_for_write);
+	need_skip = check_skip_len(mtd, offset, *length, &used_for_write);
 
 	if (actual)
 		*actual = used_for_write;
@@ -618,10 +618,10 @@ int nand_write_skip_bad(nand_info_t *nand, loff_t offset, size_t *length,
 	}
 
 	if (!need_skip && !(flags & WITH_DROP_FFS)) {
-		rval = nand_write(nand, offset, length, buffer);
+		rval = nand_write(mtd, offset, length, buffer);
 
 		if ((flags & WITH_WR_VERIFY) && !rval)
-			rval = nand_verify(nand, offset, *length, buffer);
+			rval = nand_verify(mtd, offset, *length, buffer);
 
 		if (rval == 0)
 			return 0;
@@ -633,15 +633,15 @@ int nand_write_skip_bad(nand_info_t *nand, loff_t offset, size_t *length,
 	}
 
 	while (left_to_write > 0) {
-		size_t block_offset = offset & (nand->erasesize - 1);
+		size_t block_offset = offset & (mtd->erasesize - 1);
 		size_t write_size, truncated_write_size;
 
 		WATCHDOG_RESET();
 
-		if (nand_block_isbad(nand, offset & ~(nand->erasesize - 1))) {
+		if (nand_block_isbad(mtd, offset & ~(mtd->erasesize - 1))) {
 			printf("Skip bad block 0x%08llx\n",
-				offset & ~(nand->erasesize - 1));
-			offset += nand->erasesize - block_offset;
+				offset & ~(mtd->erasesize - 1));
+			offset += mtd->erasesize - block_offset;
 			continue;
 		}
 
@@ -653,15 +653,15 @@ int nand_write_skip_bad(nand_info_t *nand, loff_t offset, size_t *length,
 		truncated_write_size = write_size;
 #ifdef CONFIG_CMD_NAND_TRIMFFS
 		if (flags & WITH_DROP_FFS)
-			truncated_write_size = drop_ffs(nand, p_buffer,
+			truncated_write_size = drop_ffs(mtd, p_buffer,
 					&write_size);
 #endif
 
-		rval = nand_write(nand, offset, &truncated_write_size,
+		rval = nand_write(mtd, offset, &truncated_write_size,
 				p_buffer);
 
 		if ((flags & WITH_WR_VERIFY) && !rval)
-			rval = nand_verify(nand, offset,
+			rval = nand_verify(mtd, offset,
 				truncated_write_size, p_buffer);
 
 		offset += write_size;
@@ -693,7 +693,7 @@ int nand_write_skip_bad(nand_info_t *nand, loff_t offset, size_t *length,
  * the limit we are passed, length is set to 0 and actual is set to the
  * required length.
  *
- * @param nand NAND device
+ * @param mtd nand mtd instance
  * @param offset offset in flash
  * @param length buffer length, on return holds number of read bytes
  * @param actual set to size required to read length worth of buffer or 0
@@ -703,8 +703,8 @@ int nand_write_skip_bad(nand_info_t *nand, loff_t offset, size_t *length,
  * @param buffer buffer to write to
  * @return 0 in case of success
  */
-int nand_read_skip_bad(nand_info_t *nand, loff_t offset, size_t *length,
-		size_t *actual, loff_t lim, u_char *buffer)
+int nand_read_skip_bad(struct mtd_info *mtd, loff_t offset, size_t *length,
+		       size_t *actual, loff_t lim, u_char *buffer)
 {
 	int rval;
 	size_t left_to_read = *length;
@@ -712,7 +712,7 @@ int nand_read_skip_bad(nand_info_t *nand, loff_t offset, size_t *length,
 	u_char *p_buffer = buffer;
 	int need_skip;
 
-	if ((offset & (nand->writesize - 1)) != 0) {
+	if ((offset & (mtd->writesize - 1)) != 0) {
 		printf("Attempt to read non page-aligned data\n");
 		*length = 0;
 		if (actual)
@@ -720,7 +720,7 @@ int nand_read_skip_bad(nand_info_t *nand, loff_t offset, size_t *length,
 		return -EINVAL;
 	}
 
-	need_skip = check_skip_len(nand, offset, *length, &used_for_read);
+	need_skip = check_skip_len(mtd, offset, *length, &used_for_read);
 
 	if (actual)
 		*actual = used_for_read;
@@ -738,7 +738,7 @@ int nand_read_skip_bad(nand_info_t *nand, loff_t offset, size_t *length,
 	}
 
 	if (!need_skip) {
-		rval = nand_read(nand, offset, length, buffer);
+		rval = nand_read(mtd, offset, length, buffer);
 		if (!rval || rval == -EUCLEAN)
 			return 0;
 
@@ -749,24 +749,24 @@ int nand_read_skip_bad(nand_info_t *nand, loff_t offset, size_t *length,
 	}
 
 	while (left_to_read > 0) {
-		size_t block_offset = offset & (nand->erasesize - 1);
+		size_t block_offset = offset & (mtd->erasesize - 1);
 		size_t read_length;
 
 		WATCHDOG_RESET();
 
-		if (nand_block_isbad(nand, offset & ~(nand->erasesize - 1))) {
+		if (nand_block_isbad(mtd, offset & ~(mtd->erasesize - 1))) {
 			printf("Skipping bad block 0x%08llx\n",
-				offset & ~(nand->erasesize - 1));
-			offset += nand->erasesize - block_offset;
+				offset & ~(mtd->erasesize - 1));
+			offset += mtd->erasesize - block_offset;
 			continue;
 		}
 
-		if (left_to_read < (nand->erasesize - block_offset))
+		if (left_to_read < (mtd->erasesize - block_offset))
 			read_length = left_to_read;
 		else
-			read_length = nand->erasesize - block_offset;
+			read_length = mtd->erasesize - block_offset;
 
-		rval = nand_read(nand, offset, &read_length, p_buffer);
+		rval = nand_read(mtd, offset, &read_length, p_buffer);
 		if (rval && rval != -EUCLEAN) {
 			printf("NAND read from offset %llx failed %d\n",
 				offset, rval);
@@ -812,57 +812,57 @@ static int check_pattern(const u_char *buf, u_char patt, int size)
  * This is useful to determine if a block that caused a write error is still
  * good or should be marked as bad.
  *
- * @param nand NAND device
+ * @param mtd nand mtd instance
  * @param offset offset in flash
  * @return 0 if the block is still good
  */
-int nand_torture(nand_info_t *nand, loff_t offset)
+int nand_torture(struct mtd_info *mtd, loff_t offset)
 {
 	u_char patterns[] = {0xa5, 0x5a, 0x00};
 	struct erase_info instr = {
 		.mtd = nand,
 		.addr = offset,
-		.len = nand->erasesize,
+		.len = mtd->erasesize,
 	};
 	size_t retlen;
 	int err, ret = -1, i, patt_count;
 	u_char *buf;
 
-	if ((offset & (nand->erasesize - 1)) != 0) {
+	if ((offset & (mtd->erasesize - 1)) != 0) {
 		puts("Attempt to torture a block at a non block-aligned offset\n");
 		return -EINVAL;
 	}
 
-	if (offset + nand->erasesize > nand->size) {
+	if (offset + mtd->erasesize > mtd->size) {
 		puts("Attempt to torture a block outside the flash area\n");
 		return -EINVAL;
 	}
 
 	patt_count = ARRAY_SIZE(patterns);
 
-	buf = malloc_cache_aligned(nand->erasesize);
+	buf = malloc_cache_aligned(mtd->erasesize);
 	if (buf == NULL) {
 		puts("Out of memory for erase block buffer\n");
 		return -ENOMEM;
 	}
 
 	for (i = 0; i < patt_count; i++) {
-		err = nand->erase(nand, &instr);
+		err = mtd_erase(mtd, &instr);
 		if (err) {
 			printf("%s: erase() failed for block at 0x%llx: %d\n",
-				nand->name, instr.addr, err);
+				mtd->name, instr.addr, err);
 			goto out;
 		}
 
 		/* Make sure the block contains only 0xff bytes */
-		err = nand->read(nand, offset, nand->erasesize, &retlen, buf);
-		if ((err && err != -EUCLEAN) || retlen != nand->erasesize) {
+		err = mtd_read(mtd, offset, mtd->erasesize, &retlen, buf);
+		if ((err && err != -EUCLEAN) || retlen != mtd->erasesize) {
 			printf("%s: read() failed for block at 0x%llx: %d\n",
-				nand->name, instr.addr, err);
+				mtd->name, instr.addr, err);
 			goto out;
 		}
 
-		err = check_pattern(buf, 0xff, nand->erasesize);
+		err = check_pattern(buf, 0xff, mtd->erasesize);
 		if (!err) {
 			printf("Erased block at 0x%llx, but a non-0xff byte was found\n",
 				offset);
@@ -871,22 +871,22 @@ int nand_torture(nand_info_t *nand, loff_t offset)
 		}
 
 		/* Write a pattern and check it */
-		memset(buf, patterns[i], nand->erasesize);
-		err = nand->write(nand, offset, nand->erasesize, &retlen, buf);
-		if (err || retlen != nand->erasesize) {
+		memset(buf, patterns[i], mtd->erasesize);
+		err = mtd_write(mtd, offset, mtd->erasesize, &retlen, buf);
+		if (err || retlen != mtd->erasesize) {
 			printf("%s: write() failed for block at 0x%llx: %d\n",
-				nand->name, instr.addr, err);
+				mtd->name, instr.addr, err);
 			goto out;
 		}
 
-		err = nand->read(nand, offset, nand->erasesize, &retlen, buf);
-		if ((err && err != -EUCLEAN) || retlen != nand->erasesize) {
+		err = mtd_read(mtd, offset, mtd->erasesize, &retlen, buf);
+		if ((err && err != -EUCLEAN) || retlen != mtd->erasesize) {
 			printf("%s: read() failed for block at 0x%llx: %d\n",
-				nand->name, instr.addr, err);
+				mtd->name, instr.addr, err);
 			goto out;
 		}
 
-		err = check_pattern(buf, patterns[i], nand->erasesize);
+		err = check_pattern(buf, patterns[i], mtd->erasesize);
 		if (!err) {
 			printf("Pattern 0x%.2x checking failed for block at "
 					"0x%llx\n", patterns[i], offset);
diff --git a/drivers/mtd/nand/ndfc.c b/drivers/mtd/nand/ndfc.c
index 8a68cb0..0a9849e 100644
--- a/drivers/mtd/nand/ndfc.c
+++ b/drivers/mtd/nand/ndfc.c
@@ -1,6 +1,6 @@
 /*
  * Overview:
- *   Platform independend driver for NDFC (NanD Flash Controller)
+ *   Platform independent driver for NDFC (NanD Flash Controller)
  *   integrated into IBM/AMCC PPC4xx cores
  *
  * (C) Copyright 2006-2009
@@ -37,7 +37,7 @@ static int ndfc_cs[NDFC_MAX_BANKS];
 
 static void ndfc_hwcontrol(struct mtd_info *mtd, int cmd, unsigned int ctrl)
 {
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 	ulong base = (ulong) this->IO_ADDR_W & 0xffffff00;
 
 	if (cmd == NAND_CMD_NONE)
@@ -51,7 +51,7 @@ static void ndfc_hwcontrol(struct mtd_info *mtd, int cmd, unsigned int ctrl)
 
 static int ndfc_dev_ready(struct mtd_info *mtdinfo)
 {
-	struct nand_chip *this = mtdinfo->priv;
+	struct nand_chip *this = mtd_to_nand(mtdinfo);
 	ulong base = (ulong) this->IO_ADDR_W & 0xffffff00;
 
 	return (in_be32((u32 *)(base + NDFC_STAT)) & NDFC_STAT_IS_READY);
@@ -59,7 +59,7 @@ static int ndfc_dev_ready(struct mtd_info *mtdinfo)
 
 static void ndfc_enable_hwecc(struct mtd_info *mtdinfo, int mode)
 {
-	struct nand_chip *this = mtdinfo->priv;
+	struct nand_chip *this = mtd_to_nand(mtdinfo);
 	ulong base = (ulong) this->IO_ADDR_W & 0xffffff00;
 	u32 ccr;
 
@@ -71,7 +71,7 @@ static void ndfc_enable_hwecc(struct mtd_info *mtdinfo, int mode)
 static int ndfc_calculate_ecc(struct mtd_info *mtdinfo,
 			      const u_char *dat, u_char *ecc_code)
 {
-	struct nand_chip *this = mtdinfo->priv;
+	struct nand_chip *this = mtd_to_nand(mtdinfo);
 	ulong base = (ulong) this->IO_ADDR_W & 0xffffff00;
 	u32 ecc;
 	u8 *p = (u8 *)&ecc;
@@ -96,7 +96,7 @@ static int ndfc_calculate_ecc(struct mtd_info *mtdinfo,
  */
 static void ndfc_read_buf(struct mtd_info *mtdinfo, uint8_t *buf, int len)
 {
-	struct nand_chip *this = mtdinfo->priv;
+	struct nand_chip *this = mtd_to_nand(mtdinfo);
 	ulong base = (ulong) this->IO_ADDR_W & 0xffffff00;
 	uint32_t *p = (uint32_t *) buf;
 
@@ -110,7 +110,7 @@ static void ndfc_read_buf(struct mtd_info *mtdinfo, uint8_t *buf, int len)
  */
 static void ndfc_write_buf(struct mtd_info *mtdinfo, const uint8_t *buf, int len)
 {
-	struct nand_chip *this = mtdinfo->priv;
+	struct nand_chip *this = mtd_to_nand(mtdinfo);
 	ulong base = (ulong) this->IO_ADDR_W & 0xffffff00;
 	uint32_t *p = (uint32_t *) buf;
 
@@ -124,7 +124,7 @@ static void ndfc_write_buf(struct mtd_info *mtdinfo, const uint8_t *buf, int len
 static uint8_t ndfc_read_byte(struct mtd_info *mtd)
 {
 
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 
 #ifdef CONFIG_SYS_NAND_BUSWIDTH_16BIT
 	return (uint8_t) readw(chip->IO_ADDR_R);
diff --git a/drivers/mtd/nand/omap_gpmc.c b/drivers/mtd/nand/omap_gpmc.c
index 6a45d28..67f293d 100644
--- a/drivers/mtd/nand/omap_gpmc.c
+++ b/drivers/mtd/nand/omap_gpmc.c
@@ -58,8 +58,8 @@ static struct omap_nand_info omap_nand_info[GPMC_MAX_CS];
 static void omap_nand_hwcontrol(struct mtd_info *mtd, int32_t cmd,
 				uint32_t ctrl)
 {
-	register struct nand_chip *this = mtd->priv;
-	struct omap_nand_info *info = this->priv;
+	register struct nand_chip *this = mtd_to_nand(mtd);
+	struct omap_nand_info *info = nand_get_controller_data(this);
 	int cs = info->cs;
 
 	/*
@@ -85,8 +85,8 @@ static void omap_nand_hwcontrol(struct mtd_info *mtd, int32_t cmd,
 /* Check wait pin as dev ready indicator */
 static int omap_dev_ready(struct mtd_info *mtd)
 {
-	register struct nand_chip *this = mtd->priv;
-	struct omap_nand_info *info = this->priv;
+	register struct nand_chip *this = mtd_to_nand(mtd);
+	struct omap_nand_info *info = nand_get_controller_data(this);
 	return gpmc_cfg->status & (1 << (8 + info->ws));
 }
 
@@ -163,7 +163,7 @@ static int __maybe_unused omap_correct_data(struct mtd_info *mtd, uint8_t *dat,
 				return 0;
 			printf("Error: Bad compare! failed\n");
 			/* detected 2 bit error */
-			return -1;
+			return -EBADMSG;
 		}
 	}
 	return 0;
@@ -177,8 +177,8 @@ static int __maybe_unused omap_correct_data(struct mtd_info *mtd, uint8_t *dat,
 __maybe_unused
 static void omap_enable_hwecc(struct mtd_info *mtd, int32_t mode)
 {
-	struct nand_chip	*nand	= mtd->priv;
-	struct omap_nand_info	*info	= nand->priv;
+	struct nand_chip	*nand	= mtd_to_nand(mtd);
+	struct omap_nand_info	*info	= nand_get_controller_data(nand);
 	unsigned int dev_width = (nand->options & NAND_BUSWIDTH_16) ? 1 : 0;
 	unsigned int ecc_algo = 0;
 	unsigned int bch_type = 0;
@@ -262,8 +262,8 @@ static void omap_enable_hwecc(struct mtd_info *mtd, int32_t mode)
 static int omap_calculate_ecc(struct mtd_info *mtd, const uint8_t *dat,
 				uint8_t *ecc_code)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct omap_nand_info *info = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct omap_nand_info *info = nand_get_controller_data(chip);
 	uint32_t *ptr, val = 0;
 	int8_t i = 0, j;
 
@@ -392,7 +392,7 @@ static int __read_prefetch_aligned(struct nand_chip *chip, uint32_t *buf, int le
 {
 	int ret;
 	uint32_t cnt;
-	struct omap_nand_info *info = chip->priv;
+	struct omap_nand_info *info = nand_get_controller_data(chip);
 
 	ret = omap_prefetch_enable(PREFETCH_FIFOTHRESHOLD_MAX, len, 0, info->cs);
 	if (ret < 0)
@@ -417,7 +417,7 @@ static int __read_prefetch_aligned(struct nand_chip *chip, uint32_t *buf, int le
 
 static inline void omap_nand_read(struct mtd_info *mtd, uint8_t *buf, int len)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 
 	if (chip->options & NAND_BUSWIDTH_16)
 		nand_read_buf16(mtd, buf, len);
@@ -429,7 +429,7 @@ static void omap_nand_read_prefetch(struct mtd_info *mtd, uint8_t *buf, int len)
 {
 	int ret;
 	uint32_t head, tail;
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 
 	/*
 	 * If the destination buffer is unaligned, start with reading
@@ -491,8 +491,8 @@ static void omap_reverse_list(u8 *list, unsigned int length)
 static int omap_correct_data_bch(struct mtd_info *mtd, uint8_t *dat,
 				uint8_t *read_ecc, uint8_t *calc_ecc)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct omap_nand_info *info = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct omap_nand_info *info = nand_get_controller_data(chip);
 	struct nand_ecc_ctrl *ecc = &chip->ecc;
 	uint32_t error_count = 0, error_max;
 	uint32_t error_loc[ELM_MAX_ERROR_COUNT];
@@ -652,8 +652,8 @@ static int omap_correct_data_bch_sw(struct mtd_info *mtd, u_char *data,
 	int i, count;
 	/* cannot correct more than 8 errors */
 	unsigned int errloc[8];
-	struct nand_chip *chip = mtd->priv;
-	struct omap_nand_info *info = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct omap_nand_info *info = nand_get_controller_data(chip);
 
 	count = decode_bch(info->control, NULL, 512, read_ecc, calc_ecc,
 							NULL, errloc);
@@ -691,8 +691,8 @@ static int omap_correct_data_bch_sw(struct mtd_info *mtd, u_char *data,
  */
 static void __maybe_unused omap_free_bch(struct mtd_info *mtd)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct omap_nand_info *info = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct omap_nand_info *info = nand_get_controller_data(chip);
 
 	if (info->control) {
 		free_bch(info->control);
@@ -710,7 +710,7 @@ static void __maybe_unused omap_free_bch(struct mtd_info *mtd)
  */
 static int omap_select_ecc_scheme(struct nand_chip *nand,
 	enum omap_ecc ecc_scheme, unsigned int pagesize, unsigned int oobsize) {
-	struct omap_nand_info	*info		= nand->priv;
+	struct omap_nand_info	*info		= nand_get_controller_data(nand);
 	struct nand_ecclayout	*ecclayout	= &omap_ecclayout;
 	int eccsteps = pagesize / SECTOR_BYTES;
 	int i;
@@ -898,13 +898,13 @@ int __maybe_unused omap_nand_switch_ecc(uint32_t hardware, uint32_t eccstrength)
 
 	if (nand_curr_device < 0 ||
 	    nand_curr_device >= CONFIG_SYS_MAX_NAND_DEVICE ||
-	    !nand_info[nand_curr_device].name) {
+	    !nand_info[nand_curr_device]->name) {
 		printf("nand: error: no NAND devices found\n");
 		return -ENODEV;
 	}
 
-	mtd = &nand_info[nand_curr_device];
-	nand = mtd->priv;
+	mtd = nand_info[nand_curr_device];
+	nand = mtd_to_nand(mtd);
 	nand->options |= NAND_OWN_BUFFERS;
 	nand->options &= ~NAND_SUBPAGE_READ;
 	/* Setup the ecc configurations again */
@@ -917,6 +917,10 @@ int __maybe_unused omap_nand_switch_ecc(uint32_t hardware, uint32_t eccstrength)
 			err = omap_select_ecc_scheme(nand,
 					OMAP_ECC_BCH8_CODE_HW,
 					mtd->writesize, mtd->oobsize);
+		} else if (eccstrength == 16) {
+			err = omap_select_ecc_scheme(nand,
+					OMAP_ECC_BCH16_CODE_HW,
+					mtd->writesize, mtd->oobsize);
 		} else {
 			printf("nand: error: unsupported ECC scheme\n");
 			return -EINVAL;
@@ -994,7 +998,7 @@ int board_nand_init(struct nand_chip *nand)
 	omap_nand_info[cs].control = NULL;
 	omap_nand_info[cs].cs = cs;
 	omap_nand_info[cs].ws = wscfg[cs];
-	nand->priv	= &omap_nand_info[cs];
+	nand_set_controller_data(nand, &omap_nand_info[cs]);
 	nand->cmd_ctrl	= omap_nand_hwcontrol;
 	nand->options	|= NAND_NO_PADDING | NAND_CACHEPRG;
 	nand->chip_delay = 100;
diff --git a/drivers/mtd/nand/pxa3xx_nand.c b/drivers/mtd/nand/pxa3xx_nand.c
index d529467..d3ac539 100644
--- a/drivers/mtd/nand/pxa3xx_nand.c
+++ b/drivers/mtd/nand/pxa3xx_nand.c
@@ -900,7 +900,8 @@ static int prepare_set_command(struct pxa3xx_nand_info *info, int command,
 static void nand_cmdfunc(struct mtd_info *mtd, unsigned command,
 			 int column, int page_addr)
 {
-	struct pxa3xx_nand_host *host = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct pxa3xx_nand_host *host = nand_get_controller_data(chip);
 	struct pxa3xx_nand_info *info = host->info_data;
 	int exec_cmd;
 
@@ -960,7 +961,8 @@ static void nand_cmdfunc_extended(struct mtd_info *mtd,
 				  const unsigned command,
 				  int column, int page_addr)
 {
-	struct pxa3xx_nand_host *host = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct pxa3xx_nand_host *host = nand_get_controller_data(chip);
 	struct pxa3xx_nand_info *info = host->info_data;
 	int exec_cmd, ext_cmd_type;
 
@@ -1079,7 +1081,8 @@ static void nand_cmdfunc_extended(struct mtd_info *mtd,
 }
 
 static int pxa3xx_nand_write_page_hwecc(struct mtd_info *mtd,
-		struct nand_chip *chip, const uint8_t *buf, int oob_required)
+		struct nand_chip *chip, const uint8_t *buf, int oob_required,
+		int page)
 {
 	chip->write_buf(mtd, buf, mtd->writesize);
 	chip->write_buf(mtd, chip->oob_poi, mtd->oobsize);
@@ -1091,7 +1094,7 @@ static int pxa3xx_nand_read_page_hwecc(struct mtd_info *mtd,
 		struct nand_chip *chip, uint8_t *buf, int oob_required,
 		int page)
 {
-	struct pxa3xx_nand_host *host = mtd->priv;
+	struct pxa3xx_nand_host *host = nand_get_controller_data(chip);
 	struct pxa3xx_nand_info *info = host->info_data;
 
 	chip->read_buf(mtd, buf, mtd->writesize);
@@ -1117,7 +1120,8 @@ static int pxa3xx_nand_read_page_hwecc(struct mtd_info *mtd,
 
 static uint8_t pxa3xx_nand_read_byte(struct mtd_info *mtd)
 {
-	struct pxa3xx_nand_host *host = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct pxa3xx_nand_host *host = nand_get_controller_data(chip);
 	struct pxa3xx_nand_info *info = host->info_data;
 	char retval = 0xFF;
 
@@ -1130,7 +1134,8 @@ static uint8_t pxa3xx_nand_read_byte(struct mtd_info *mtd)
 
 static u16 pxa3xx_nand_read_word(struct mtd_info *mtd)
 {
-	struct pxa3xx_nand_host *host = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct pxa3xx_nand_host *host = nand_get_controller_data(chip);
 	struct pxa3xx_nand_info *info = host->info_data;
 	u16 retval = 0xFFFF;
 
@@ -1143,7 +1148,8 @@ static u16 pxa3xx_nand_read_word(struct mtd_info *mtd)
 
 static void pxa3xx_nand_read_buf(struct mtd_info *mtd, uint8_t *buf, int len)
 {
-	struct pxa3xx_nand_host *host = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct pxa3xx_nand_host *host = nand_get_controller_data(chip);
 	struct pxa3xx_nand_info *info = host->info_data;
 	int real_len = min_t(size_t, len, info->buf_count - info->buf_start);
 
@@ -1154,7 +1160,8 @@ static void pxa3xx_nand_read_buf(struct mtd_info *mtd, uint8_t *buf, int len)
 static void pxa3xx_nand_write_buf(struct mtd_info *mtd,
 		const uint8_t *buf, int len)
 {
-	struct pxa3xx_nand_host *host = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct pxa3xx_nand_host *host = nand_get_controller_data(chip);
 	struct pxa3xx_nand_info *info = host->info_data;
 	int real_len = min_t(size_t, len, info->buf_count - info->buf_start);
 
@@ -1169,7 +1176,8 @@ static void pxa3xx_nand_select_chip(struct mtd_info *mtd, int chip)
 
 static int pxa3xx_nand_waitfunc(struct mtd_info *mtd, struct nand_chip *this)
 {
-	struct pxa3xx_nand_host *host = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct pxa3xx_nand_host *host = nand_get_controller_data(chip);
 	struct pxa3xx_nand_info *info = host->info_data;
 
 	if (info->need_wait) {
@@ -1210,7 +1218,7 @@ static int pxa3xx_nand_config_flash(struct pxa3xx_nand_info *info)
 {
 	struct pxa3xx_nand_host *host = info->host[info->cs];
 	struct mtd_info *mtd = host->mtd;
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 
 	info->reg_ndcr |= (host->col_addr_cycles == 2) ? NDCR_RA_START : 0;
 	info->reg_ndcr |= (chip->page_shift == 6) ? NDCR_PG_PER_BLK : 0;
@@ -1262,7 +1270,7 @@ static int pxa3xx_nand_sensing(struct pxa3xx_nand_host *host)
 	int ret;
 
 	mtd = info->host[info->cs]->mtd;
-	chip = mtd->priv;
+	chip = mtd_to_nand(mtd);
 
 	/* configure default flash values */
 	info->reg_ndcr = 0x0; /* enable all interrupts */
@@ -1354,10 +1362,10 @@ static int pxa_ecc_init(struct pxa3xx_nand_info *info,
 
 static int pxa3xx_nand_scan(struct mtd_info *mtd)
 {
-	struct pxa3xx_nand_host *host = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct pxa3xx_nand_host *host = nand_get_controller_data(chip);
 	struct pxa3xx_nand_info *info = host->info_data;
 	struct pxa3xx_nand_platform_data *pdata = info->pdata;
-	struct nand_chip *chip = mtd->priv;
 	int ret;
 	uint16_t ecc_strength, ecc_step;
 
@@ -1477,16 +1485,15 @@ static int alloc_nand_resource(struct pxa3xx_nand_info *info)
 
 	info->variant = pxa3xx_nand_get_variant();
 	for (cs = 0; cs < pdata->num_cs; cs++) {
-		mtd = &nand_info[cs];
 		chip = (struct nand_chip *)
 			((u8 *)&info[1] + sizeof(*host) * cs);
+		mtd = nand_to_mtd(chip);
 		host = (struct pxa3xx_nand_host *)chip;
 		info->host[cs] = host;
 		host->mtd = mtd;
 		host->cs = cs;
 		host->info_data = info;
 		host->read_id_bytes = 4;
-		mtd->priv = host;
 		mtd->owner = THIS_MODULE;
 
 		chip->ecc.read_page	= pxa3xx_nand_read_page_hwecc;
@@ -1573,8 +1580,10 @@ static int pxa3xx_nand_probe(struct pxa3xx_nand_info *info)
 			continue;
 		}
 
-		if (!ret)
-			probe_success = 1;
+		if (nand_register(cs, mtd))
+			continue;
+
+		probe_success = 1;
 	}
 
 	if (!probe_success)
@@ -1601,6 +1610,4 @@ void board_nand_init(void)
 	ret = pxa3xx_nand_probe(info);
 	if (ret)
 		return;
-
-	nand_register(0);
 }
diff --git a/drivers/mtd/nand/s3c2410_nand.c b/drivers/mtd/nand/s3c2410_nand.c
index b3a2a60..dd742a6 100644
--- a/drivers/mtd/nand/s3c2410_nand.c
+++ b/drivers/mtd/nand/s3c2410_nand.c
@@ -31,7 +31,7 @@
 static void nand_read_buf(struct mtd_info *mtd, u_char *buf, int len)
 {
 	int i;
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 
 	for (i = 0; i < len; i++)
 		buf[i] = readb(this->IO_ADDR_R);
@@ -40,7 +40,7 @@ static void nand_read_buf(struct mtd_info *mtd, u_char *buf, int len)
 
 static void s3c24x0_hwcontrol(struct mtd_info *mtd, int cmd, unsigned int ctrl)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	struct s3c24x0_nand *nand = s3c24x0_get_base_nand();
 
 	debug("hwcontrol(): 0x%02x 0x%02x\n", cmd, ctrl);
@@ -104,7 +104,7 @@ static int s3c24x0_nand_correct_data(struct mtd_info *mtd, u_char *dat,
 		return 0;
 
 	printf("s3c24x0_nand_correct_data: not implemented\n");
-	return -1;
+	return -EBADMSG;
 }
 #endif
 
diff --git a/drivers/mtd/nand/tegra_nand.c b/drivers/mtd/nand/tegra_nand.c
index a77db7b..2032f65 100644
--- a/drivers/mtd/nand/tegra_nand.c
+++ b/drivers/mtd/nand/tegra_nand.c
@@ -143,10 +143,10 @@ static int nand_waitfor_cmd_completion(struct nand_ctlr *reg)
  */
 static uint8_t read_byte(struct mtd_info *mtd)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	struct nand_drv *info;
 
-	info = (struct nand_drv *)chip->priv;
+	info = (struct nand_drv *)nand_get_controller_data(chip);
 
 	writel(CMD_GO | CMD_PIO | CMD_RX | CMD_CE0 | CMD_A_VALID,
 	       &info->reg->command);
@@ -169,8 +169,8 @@ static void read_buf(struct mtd_info *mtd, uint8_t *buf, int len)
 {
 	int i, s;
 	unsigned int reg;
-	struct nand_chip *chip = mtd->priv;
-	struct nand_drv *info = (struct nand_drv *)chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct nand_drv *info = (struct nand_drv *)nand_get_controller_data(chip);
 
 	for (i = 0; i < len; i += 4) {
 		s = (len - i) > 4 ? 4 : len - i;
@@ -194,11 +194,11 @@ static void read_buf(struct mtd_info *mtd, uint8_t *buf, int len)
  */
 static int nand_dev_ready(struct mtd_info *mtd)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	int reg_val;
 	struct nand_drv *info;
 
-	info = (struct nand_drv *)chip->priv;
+	info = (struct nand_drv *)nand_get_controller_data(chip);
 
 	reg_val = readl(&info->reg->status);
 	if (reg_val & STATUS_RBSY0)
@@ -245,10 +245,10 @@ static void nand_clear_interrupt_status(struct nand_ctlr *reg)
 static void nand_command(struct mtd_info *mtd, unsigned int command,
 	int column, int page_addr)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	struct nand_drv *info;
 
-	info = (struct nand_drv *)chip->priv;
+	info = (struct nand_drv *)nand_get_controller_data(chip);
 
 	/*
 	 * Write out the command to the device.
@@ -512,7 +512,7 @@ static int nand_rw_page(struct mtd_info *mtd, struct nand_chip *chip,
 		return -EINVAL;
 	}
 
-	info = (struct nand_drv *)chip->priv;
+	info = (struct nand_drv *)nand_get_controller_data(chip);
 	config = &info->config;
 	if (set_bus_width_page_size(config, &reg_val))
 		return -EINVAL;
@@ -657,16 +657,9 @@ static int nand_read_page_hwecc(struct mtd_info *mtd,
  * @param buf	data buffer
  */
 static int nand_write_page_hwecc(struct mtd_info *mtd,
-	struct nand_chip *chip, const uint8_t *buf, int oob_required)
+	struct nand_chip *chip, const uint8_t *buf, int oob_required,
+	int page)
 {
-	int page;
-	struct nand_drv *info;
-
-	info = (struct nand_drv *)chip->priv;
-
-	page = (readl(&info->reg->addr_reg1) >> 16) |
-		(readl(&info->reg->addr_reg2) << 16);
-
 	nand_rw_page(mtd, chip, (uint8_t *)buf, page, 1, 1);
 	return 0;
 }
@@ -697,15 +690,9 @@ static int nand_read_page_raw(struct mtd_info *mtd,
  * @param buf	data buffer
  */
 static int nand_write_page_raw(struct mtd_info *mtd,
-		struct nand_chip *chip,	const uint8_t *buf, int oob_required)
+		struct nand_chip *chip,	const uint8_t *buf,
+		int oob_required, int page)
 {
-	int page;
-	struct nand_drv *info;
-
-	info = (struct nand_drv *)chip->priv;
-	page = (readl(&info->reg->addr_reg1) >> 16) |
-		(readl(&info->reg->addr_reg2) << 16);
-
 	nand_rw_page(mtd, chip, (uint8_t *)buf, page, 0, 1);
 	return 0;
 }
@@ -734,7 +721,7 @@ static int nand_rw_oob(struct mtd_info *mtd, struct nand_chip *chip,
 
 	if (((int)chip->oob_poi) & 0x03)
 		return -EINVAL;
-	info = (struct nand_drv *)chip->priv;
+	info = (struct nand_drv *)nand_get_controller_data(chip);
 	if (set_bus_width_page_size(&info->config, &reg_val))
 		return -EINVAL;
 
@@ -963,7 +950,7 @@ int tegra_nand_init(struct nand_chip *nand, int devnum)
 	nand->ecc.strength = 1;
 	nand->select_chip = nand_select_chip;
 	nand->dev_ready  = nand_dev_ready;
-	nand->priv = &nand_ctrl;
+	nand_set_controller_data(nand, &nand_ctrl);
 
 	/* Disable subpage writes as we do not provide ecc->hwctl */
 	nand->options |= NAND_NO_SUBPAGE_WRITE;
@@ -976,8 +963,7 @@ int tegra_nand_init(struct nand_chip *nand, int devnum)
 
 	dm_gpio_set_value(&config->wp_gpio, 1);
 
-	our_mtd = &nand_info[devnum];
-	our_mtd->priv = nand;
+	our_mtd = nand_to_mtd(nand);
 	ret = nand_scan_ident(our_mtd, CONFIG_SYS_NAND_MAX_CHIPS, NULL);
 	if (ret)
 		return ret;
@@ -989,7 +975,7 @@ int tegra_nand_init(struct nand_chip *nand, int devnum)
 	if (ret)
 		return ret;
 
-	ret = nand_register(devnum);
+	ret = nand_register(devnum, our_mtd);
 	if (ret)
 		return ret;
 
diff --git a/drivers/mtd/nand/vf610_nfc.c b/drivers/mtd/nand/vf610_nfc.c
index 1faec5e..f99bdaf 100644
--- a/drivers/mtd/nand/vf610_nfc.c
+++ b/drivers/mtd/nand/vf610_nfc.c
@@ -146,7 +146,6 @@ enum vf610_nfc_alt_buf {
 };
 
 struct vf610_nfc {
-	struct mtd_info *mtd;
 	struct nand_chip chip;
 	void __iomem *regs;
 	uint buf_offset;
@@ -155,8 +154,7 @@ struct vf610_nfc {
 	enum vf610_nfc_alt_buf alt_buf;
 };
 
-#define mtd_to_nfc(_mtd) \
-	(struct vf610_nfc *)((struct nand_chip *)_mtd->priv)->priv
+#define mtd_to_nfc(_mtd) nand_get_controller_data(mtd_to_nand(_mtd))
 
 #if defined(CONFIG_SYS_NAND_VF610_NFC_45_ECC_BYTES)
 #define ECC_HW_MODE ECC_45_BYTE
@@ -608,7 +606,7 @@ static int vf610_nfc_read_page(struct mtd_info *mtd, struct nand_chip *chip,
  * ECC will be calculated automatically
  */
 static int vf610_nfc_write_page(struct mtd_info *mtd, struct nand_chip *chip,
-			       const uint8_t *buf, int oob_required)
+			       const uint8_t *buf, int oob_required, int page)
 {
 	struct vf610_nfc *nfc = mtd_to_nfc(mtd);
 
@@ -630,7 +628,7 @@ struct vf610_nfc_config {
 
 static int vf610_nfc_nand_init(int devnum, void __iomem *addr)
 {
-	struct mtd_info *mtd = &nand_info[devnum];
+	struct mtd_info *mtd;
 	struct nand_chip *chip;
 	struct vf610_nfc *nfc;
 	int err = 0;
@@ -653,8 +651,8 @@ static int vf610_nfc_nand_init(int devnum, void __iomem *addr)
 	chip = &nfc->chip;
 	nfc->regs = addr;
 
-	mtd->priv = chip;
-	chip->priv = nfc;
+	mtd = nand_to_mtd(chip);
+	nand_set_controller_data(chip, nfc);
 
 	if (cfg.width == 16)
 		chip->options |= NAND_BUSWIDTH_16;
@@ -753,7 +751,7 @@ static int vf610_nfc_nand_init(int devnum, void __iomem *addr)
 	if (err)
 		return err;
 
-	err = nand_register(devnum);
+	err = nand_register(devnum, mtd);
 	if (err)
 		return err;
 
diff --git a/drivers/mtd/spi/sf_internal.h b/drivers/mtd/spi/sf_internal.h
index 007a5a0..da2bb7b 100644
--- a/drivers/mtd/spi/sf_internal.h
+++ b/drivers/mtd/spi/sf_internal.h
@@ -127,6 +127,11 @@ int sst_write_bp(struct spi_flash *flash, u32 offset, size_t len,
 		const void *buf);
 #endif
 
+#ifdef CONFIG_SPI_FLASH_SPANSION
+/* Used for Spansion S25FS-S family flash only. */
+#define CMD_SPANSION_RDAR	0x65 /* Read any device register */
+#define CMD_SPANSION_WRAR	0x71 /* Write any device register */
+#endif
 /**
  * struct spi_flash_params - SPI/QSPI flash device params structure
  *
diff --git a/drivers/mtd/spi/sf_params.c b/drivers/mtd/spi/sf_params.c
index 4f37e33..c577d9e 100644
--- a/drivers/mtd/spi/sf_params.c
+++ b/drivers/mtd/spi/sf_params.c
@@ -67,6 +67,7 @@ const struct spi_flash_params spi_flash_params_table[] = {
 	{"S25FL128S_64K",  0x012018, 0x4d01,    64 * 1024,   256, RD_FULL,		     WR_QPP},
 	{"S25FL256S_256K", 0x010219, 0x4d00,   256 * 1024,   128, RD_FULL,		     WR_QPP},
 	{"S25FL256S_64K",  0x010219, 0x4d01,	64 * 1024,   512, RD_FULL,		     WR_QPP},
+	{"S25FS512S",      0x010220, 0x4D00,   128 * 1024,   512, RD_FULL,                   WR_QPP},
 	{"S25FL512S_256K", 0x010220, 0x4d00,   256 * 1024,   256, RD_FULL,		     WR_QPP},
 	{"S25FL512S_64K",  0x010220, 0x4d01,    64 * 1024,  1024, RD_FULL,		     WR_QPP},
 	{"S25FL512S_512K", 0x010220, 0x4f00,   256 * 1024,   256, RD_FULL,		     WR_QPP},
diff --git a/drivers/mtd/spi/spi_flash.c b/drivers/mtd/spi/spi_flash.c
index 5451725..64d4e0f 100644
--- a/drivers/mtd/spi/spi_flash.c
+++ b/drivers/mtd/spi/spi_flash.c
@@ -971,6 +971,43 @@ int spi_flash_decode_fdt(const void *blob, struct spi_flash *flash)
 }
 #endif /* CONFIG_IS_ENABLED(OF_CONTROL) */
 
+#ifdef CONFIG_SPI_FLASH_SPANSION
+static int spansion_s25fss_disable_4KB_erase(struct spi_slave *spi)
+{
+	u8 cmd[4];
+	u32 offset = 0x800004; /* CR3V register offset */
+	u8 cr3v;
+	int ret;
+
+	cmd[0] = CMD_SPANSION_RDAR;
+	cmd[1] = offset >> 16;
+	cmd[2] = offset >> 8;
+	cmd[3] = offset >> 0;
+
+	ret = spi_flash_cmd_read(spi, cmd, 4, &cr3v, 1);
+	if (ret)
+		return -EIO;
+	/* CR3V bit3: 4-KB Erase */
+	if (cr3v & 0x8)
+		return 0;
+
+	cmd[0] = CMD_SPANSION_WRAR;
+	cr3v |= 0x8;
+	ret = spi_flash_cmd_write(spi, cmd, 4, &cr3v, 1);
+	if (ret)
+		return -EIO;
+
+	cmd[0] = CMD_SPANSION_RDAR;
+	ret = spi_flash_cmd_read(spi, cmd, 4, &cr3v, 1);
+	if (ret)
+		return -EIO;
+	if (!(cr3v & 0x8))
+		return -EFAULT;
+
+	return 0;
+}
+#endif
+
 int spi_flash_scan(struct spi_flash *flash)
 {
 	struct spi_slave *spi = flash->spi;
@@ -1021,6 +1058,42 @@ int spi_flash_scan(struct spi_flash *flash)
 		return -EPROTONOSUPPORT;
 	}
 
+#ifdef CONFIG_SPI_FLASH_SPANSION
+	/*
+	 * The S25FS-S family physical sectors may be configured as a
+	 * hybrid combination of eight 4-kB parameter sectors
+	 * at the top or bottom of the address space with all
+	 * but one of the remaining sectors being uniform size.
+	 * The Parameter Sector Erase commands (20h or 21h) must
+	 * be used to erase the 4-kB parameter sectors individually.
+	 * The Sector (uniform sector) Erase commands (D8h or DCh)
+	 * must be used to erase any of the remaining
+	 * sectors, including the portion of highest or lowest address
+	 * sector that is not overlaid by the parameter sectors.
+	 * The uniform sector erase command has no effect on parameter sectors.
+	 */
+	if ((jedec == 0x0219 || (jedec == 0x0220)) &&
+	    (ext_jedec & 0xff00) == 0x4d00) {
+		int ret;
+		u8 id[6];
+
+		/* Read the ID codes again, 6 bytes */
+		ret = spi_flash_cmd(flash->spi, CMD_READ_ID, id, sizeof(id));
+		if (ret)
+			return -EIO;
+
+		ret = memcmp(id, idcode, 5);
+		if (ret)
+			return -EIO;
+
+		/* 0x81: S25FS-S family 0x80: S25FL-S family */
+		if (id[5] == 0x81) {
+			ret = spansion_s25fss_disable_4KB_erase(spi);
+			if (ret)
+				return ret;
+		}
+	}
+#endif
 	/* Flash powers up read-only, so clear BP# bits */
 	if (idcode[0] == SPI_FLASH_CFI_MFR_ATMEL ||
 	    idcode[0] == SPI_FLASH_CFI_MFR_MACRONIX ||
@@ -1074,7 +1147,7 @@ int spi_flash_scan(struct spi_flash *flash)
 	 * have 256b pages.
 	 */
 	if (ext_jedec == 0x4d00) {
-		if ((jedec == 0x0215) || (jedec == 0x216))
+		if ((jedec == 0x0215) || (jedec == 0x216) || (jedec == 0x220))
 			flash->page_size = 256;
 		else
 			flash->page_size = 512;
diff --git a/drivers/mtd/spi/spi_spl_load.c b/drivers/mtd/spi/spi_spl_load.c
index 46c98a9..bac1e85 100644
--- a/drivers/mtd/spi/spi_spl_load.c
+++ b/drivers/mtd/spi/spi_spl_load.c
@@ -48,6 +48,18 @@ static int spi_load_image_os(struct spi_flash *flash,
 }
 #endif
 
+static ulong spl_spi_fit_read(struct spl_load_info *load, ulong sector,
+			      ulong count, void *buf)
+{
+	struct spi_flash *flash = load->dev;
+	ulong ret;
+
+	ret = spi_flash_read(flash, sector, count, buf);
+	if (!ret)
+		return count;
+	else
+		return 0;
+}
 /*
  * The main entry for SPI booting. It's necessary that SDRAM is already
  * configured and available since this code loads the main U-Boot image
@@ -85,11 +97,26 @@ int spl_spi_load_image(void)
 		if (err)
 			return err;
 
-		err = spl_parse_image_header(header);
-		if (err)
-			return err;
-		err = spi_flash_read(flash, CONFIG_SYS_SPI_U_BOOT_OFFS,
-			       spl_image.size, (void *)spl_image.load_addr);
+		if (IS_ENABLED(CONFIG_SPL_LOAD_FIT)) {
+			struct spl_load_info load;
+
+			debug("Found FIT\n");
+			load.dev = flash;
+			load.priv = NULL;
+			load.filename = NULL;
+			load.bl_len = 1;
+			load.read = spl_spi_fit_read;
+			err = spl_load_simple_fit(&load,
+						  CONFIG_SYS_SPI_U_BOOT_OFFS,
+						  header);
+		} else {
+			err = spl_parse_image_header(header);
+			if (err)
+				return err;
+			err = spi_flash_read(flash, CONFIG_SYS_SPI_U_BOOT_OFFS,
+					     spl_image.size,
+					     (void *)spl_image.load_addr);
+		}
 	}
 
 	return err;
diff --git a/drivers/net/Kconfig b/drivers/net/Kconfig
index 91b7690..c1cb689 100644
--- a/drivers/net/Kconfig
+++ b/drivers/net/Kconfig
@@ -46,6 +46,15 @@ menuconfig NETDEVICES
 
 if NETDEVICES
 
+config AG7XXX
+	bool "Atheros AG7xxx Ethernet MAC support"
+	depends on DM_ETH && ARCH_ATH79
+	select PHYLIB
+	help
+	  This driver supports the Atheros AG7xxx Ethernet MAC. This MAC is
+	  present in the Atheros AR7xxx, AR9xxx and QCA9xxx MIPS chips.
+
+
 config ALTERA_TSE
 	bool "Altera Triple-Speed Ethernet MAC support"
 	depends on DM_ETH
diff --git a/drivers/net/Makefile b/drivers/net/Makefile
index fbedd04..5702592 100644
--- a/drivers/net/Makefile
+++ b/drivers/net/Makefile
@@ -7,6 +7,7 @@
 
 obj-$(CONFIG_PPC4xx_EMAC) += 4xx_enet.o
 obj-$(CONFIG_ALTERA_TSE) += altera_tse.o
+obj-$(CONFIG_AG7XXX) += ag7xxx.o
 obj-$(CONFIG_ARMADA100_FEC) += armada100_fec.o
 obj-$(CONFIG_DRIVER_AT91EMAC) += at91_emac.o
 obj-$(CONFIG_DRIVER_AX88180) += ax88180.o
@@ -59,7 +60,7 @@ obj-$(CONFIG_SMC91111) += smc91111.o
 obj-$(CONFIG_SMC911X) += smc911x.o
 obj-$(CONFIG_DRIVER_TI_EMAC) += davinci_emac.o
 obj-$(CONFIG_TSEC_ENET) += tsec.o fsl_mdio.o
-obj-$(CONFIG_DRIVER_TI_CPSW) += cpsw.o
+obj-$(CONFIG_DRIVER_TI_CPSW) += cpsw.o cpsw-common.o
 obj-$(CONFIG_FMAN_ENET) += fsl_mdio.o
 obj-$(CONFIG_TSI108_ETH) += tsi108_eth.o
 obj-$(CONFIG_ULI526X) += uli526x.o
diff --git a/drivers/net/ag7xxx.c b/drivers/net/ag7xxx.c
new file mode 100644
index 0000000..346f138
--- /dev/null
+++ b/drivers/net/ag7xxx.c
@@ -0,0 +1,980 @@
+/*
+ * Atheros AR71xx / AR9xxx GMAC driver
+ *
+ * Copyright (C) 2016 Marek Vasut <marex@denx.de>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <dm.h>
+#include <errno.h>
+#include <miiphy.h>
+#include <malloc.h>
+#include <linux/compiler.h>
+#include <linux/err.h>
+#include <linux/mii.h>
+#include <wait_bit.h>
+#include <asm/io.h>
+
+#include <mach/ath79.h>
+
+DECLARE_GLOBAL_DATA_PTR;
+
+enum ag7xxx_model {
+	AG7XXX_MODEL_AG933X,
+	AG7XXX_MODEL_AG934X,
+};
+
+#define AG7XXX_ETH_CFG1				0x00
+#define AG7XXX_ETH_CFG1_SOFT_RST		BIT(31)
+#define AG7XXX_ETH_CFG1_RX_RST			BIT(19)
+#define AG7XXX_ETH_CFG1_TX_RST			BIT(18)
+#define AG7XXX_ETH_CFG1_LOOPBACK		BIT(8)
+#define AG7XXX_ETH_CFG1_RX_EN			BIT(2)
+#define AG7XXX_ETH_CFG1_TX_EN			BIT(0)
+
+#define AG7XXX_ETH_CFG2				0x04
+#define AG7XXX_ETH_CFG2_IF_1000			BIT(9)
+#define AG7XXX_ETH_CFG2_IF_10_100		BIT(8)
+#define AG7XXX_ETH_CFG2_IF_SPEED_MASK		(3 << 8)
+#define AG7XXX_ETH_CFG2_HUGE_FRAME_EN		BIT(5)
+#define AG7XXX_ETH_CFG2_LEN_CHECK		BIT(4)
+#define AG7XXX_ETH_CFG2_PAD_CRC_EN		BIT(2)
+#define AG7XXX_ETH_CFG2_FDX			BIT(0)
+
+#define AG7XXX_ETH_MII_MGMT_CFG			0x20
+#define AG7XXX_ETH_MII_MGMT_CFG_RESET		BIT(31)
+
+#define AG7XXX_ETH_MII_MGMT_CMD			0x24
+#define AG7XXX_ETH_MII_MGMT_CMD_READ		0x1
+
+#define AG7XXX_ETH_MII_MGMT_ADDRESS		0x28
+#define AG7XXX_ETH_MII_MGMT_ADDRESS_SHIFT	8
+
+#define AG7XXX_ETH_MII_MGMT_CTRL		0x2c
+
+#define AG7XXX_ETH_MII_MGMT_STATUS		0x30
+
+#define AG7XXX_ETH_MII_MGMT_IND			0x34
+#define AG7XXX_ETH_MII_MGMT_IND_INVALID		BIT(2)
+#define AG7XXX_ETH_MII_MGMT_IND_BUSY		BIT(0)
+
+#define AG7XXX_ETH_ADDR1			0x40
+#define AG7XXX_ETH_ADDR2			0x44
+
+#define AG7XXX_ETH_FIFO_CFG_0			0x48
+#define AG7XXX_ETH_FIFO_CFG_1			0x4c
+#define AG7XXX_ETH_FIFO_CFG_2			0x50
+#define AG7XXX_ETH_FIFO_CFG_3			0x54
+#define AG7XXX_ETH_FIFO_CFG_4			0x58
+#define AG7XXX_ETH_FIFO_CFG_5			0x5c
+
+#define AG7XXX_ETH_DMA_TX_CTRL			0x180
+#define AG7XXX_ETH_DMA_TX_CTRL_TXE		BIT(0)
+
+#define AG7XXX_ETH_DMA_TX_DESC			0x184
+
+#define AG7XXX_ETH_DMA_TX_STATUS		0x188
+
+#define AG7XXX_ETH_DMA_RX_CTRL			0x18c
+#define AG7XXX_ETH_DMA_RX_CTRL_RXE		BIT(0)
+
+#define AG7XXX_ETH_DMA_RX_DESC			0x190
+
+#define AG7XXX_ETH_DMA_RX_STATUS		0x194
+
+/* Custom register at 0x18070000 */
+#define AG7XXX_GMAC_ETH_CFG			0x00
+#define AG7XXX_ETH_CFG_SW_PHY_ADDR_SWAP		BIT(8)
+#define AG7XXX_ETH_CFG_SW_PHY_SWAP		BIT(7)
+#define AG7XXX_ETH_CFG_SW_ONLY_MODE		BIT(6)
+#define AG7XXX_ETH_CFG_GE0_ERR_EN		BIT(5)
+#define AG7XXX_ETH_CFG_MII_GE0_SLAVE		BIT(4)
+#define AG7XXX_ETH_CFG_MII_GE0_MASTER		BIT(3)
+#define AG7XXX_ETH_CFG_GMII_GE0			BIT(2)
+#define AG7XXX_ETH_CFG_MII_GE0			BIT(1)
+#define AG7XXX_ETH_CFG_RGMII_GE0		BIT(0)
+
+#define CONFIG_TX_DESCR_NUM	8
+#define CONFIG_RX_DESCR_NUM	8
+#define CONFIG_ETH_BUFSIZE	2048
+#define TX_TOTAL_BUFSIZE	(CONFIG_ETH_BUFSIZE * CONFIG_TX_DESCR_NUM)
+#define RX_TOTAL_BUFSIZE	(CONFIG_ETH_BUFSIZE * CONFIG_RX_DESCR_NUM)
+
+/* DMA descriptor. */
+struct ag7xxx_dma_desc {
+	u32	data_addr;
+#define AG7XXX_DMADESC_IS_EMPTY			BIT(31)
+#define AG7XXX_DMADESC_FTPP_OVERRIDE_OFFSET	16
+#define AG7XXX_DMADESC_PKT_SIZE_OFFSET		0
+#define AG7XXX_DMADESC_PKT_SIZE_MASK		0xfff
+	u32	config;
+	u32	next_desc;
+	u32	_pad[5];
+};
+
+struct ar7xxx_eth_priv {
+	struct ag7xxx_dma_desc	tx_mac_descrtable[CONFIG_TX_DESCR_NUM];
+	struct ag7xxx_dma_desc	rx_mac_descrtable[CONFIG_RX_DESCR_NUM];
+	char		txbuffs[TX_TOTAL_BUFSIZE] __aligned(ARCH_DMA_MINALIGN);
+	char		rxbuffs[RX_TOTAL_BUFSIZE] __aligned(ARCH_DMA_MINALIGN);
+
+	void __iomem		*regs;
+	void __iomem		*phyregs;
+
+	struct eth_device	*dev;
+	struct phy_device	*phydev;
+	struct mii_dev		*bus;
+
+	u32			interface;
+	u32			tx_currdescnum;
+	u32			rx_currdescnum;
+	enum ag7xxx_model	model;
+};
+
+/*
+ * Switch and MDIO access
+ */
+static int ag7xxx_switch_read(struct mii_dev *bus, int addr, int reg, u16 *val)
+{
+	struct ar7xxx_eth_priv *priv = bus->priv;
+	void __iomem *regs = priv->phyregs;
+	int ret;
+
+	writel(0x0, regs + AG7XXX_ETH_MII_MGMT_CMD);
+	writel((addr << AG7XXX_ETH_MII_MGMT_ADDRESS_SHIFT) | reg,
+	       regs + AG7XXX_ETH_MII_MGMT_ADDRESS);
+	writel(AG7XXX_ETH_MII_MGMT_CMD_READ,
+	       regs + AG7XXX_ETH_MII_MGMT_CMD);
+
+	ret = wait_for_bit("ag7xxx", regs + AG7XXX_ETH_MII_MGMT_IND,
+			   AG7XXX_ETH_MII_MGMT_IND_BUSY, 0, 1000, 0);
+	if (ret)
+		return ret;
+
+	*val = readl(regs + AG7XXX_ETH_MII_MGMT_STATUS) & 0xffff;
+	writel(0x0, regs + AG7XXX_ETH_MII_MGMT_CMD);
+
+	return 0;
+}
+
+static int ag7xxx_switch_write(struct mii_dev *bus, int addr, int reg, u16 val)
+{
+	struct ar7xxx_eth_priv *priv = bus->priv;
+	void __iomem *regs = priv->phyregs;
+	int ret;
+
+	writel((addr << AG7XXX_ETH_MII_MGMT_ADDRESS_SHIFT) | reg,
+	       regs + AG7XXX_ETH_MII_MGMT_ADDRESS);
+	writel(val, regs + AG7XXX_ETH_MII_MGMT_CTRL);
+
+	ret = wait_for_bit("ag7xxx", regs + AG7XXX_ETH_MII_MGMT_IND,
+			   AG7XXX_ETH_MII_MGMT_IND_BUSY, 0, 1000, 0);
+
+	return ret;
+}
+
+static int ag7xxx_switch_reg_read(struct mii_dev *bus, int reg, u32 *val)
+{
+	struct ar7xxx_eth_priv *priv = bus->priv;
+	u32 phy_addr;
+	u32 reg_addr;
+	u32 phy_temp;
+	u32 reg_temp;
+	u16 rv = 0;
+	int ret;
+
+	if (priv->model == AG7XXX_MODEL_AG933X) {
+		phy_addr = 0x1f;
+		reg_addr = 0x10;
+	} else if (priv->model == AG7XXX_MODEL_AG934X) {
+		phy_addr = 0x18;
+		reg_addr = 0x00;
+	} else
+		return -EINVAL;
+
+	ret = ag7xxx_switch_write(bus, phy_addr, reg_addr, reg >> 9);
+	if (ret)
+		return ret;
+
+	phy_temp = ((reg >> 6) & 0x7) | 0x10;
+	reg_temp = (reg >> 1) & 0x1e;
+	*val = 0;
+
+	ret = ag7xxx_switch_read(bus, phy_temp, reg_temp | 0, &rv);
+	if (ret < 0)
+		return ret;
+	*val |= rv;
+
+	ret = ag7xxx_switch_read(bus, phy_temp, reg_temp | 1, &rv);
+	if (ret < 0)
+		return ret;
+	*val |= (rv << 16);
+
+	return 0;
+}
+
+static int ag7xxx_switch_reg_write(struct mii_dev *bus, int reg, u32 val)
+{
+	struct ar7xxx_eth_priv *priv = bus->priv;
+	u32 phy_addr;
+	u32 reg_addr;
+	u32 phy_temp;
+	u32 reg_temp;
+	int ret;
+
+	if (priv->model == AG7XXX_MODEL_AG933X) {
+		phy_addr = 0x1f;
+		reg_addr = 0x10;
+	} else if (priv->model == AG7XXX_MODEL_AG934X) {
+		phy_addr = 0x18;
+		reg_addr = 0x00;
+	} else
+		return -EINVAL;
+
+	ret = ag7xxx_switch_write(bus, phy_addr, reg_addr, reg >> 9);
+	if (ret)
+		return ret;
+
+	phy_temp = ((reg >> 6) & 0x7) | 0x10;
+	reg_temp = (reg >> 1) & 0x1e;
+
+	/*
+	 * The switch on AR933x has some special register behavior, which
+	 * expects particular write order of their nibbles:
+	 *   0x40 ..... MSB first, LSB second
+	 *   0x50 ..... MSB first, LSB second
+	 *   0x98 ..... LSB first, MSB second
+	 *   others ... don't care
+	 */
+	if ((priv->model == AG7XXX_MODEL_AG933X) && (reg == 0x98)) {
+		ret = ag7xxx_switch_write(bus, phy_temp, reg_temp | 0, val & 0xffff);
+		if (ret < 0)
+			return ret;
+
+		ret = ag7xxx_switch_write(bus, phy_temp, reg_temp | 1, val >> 16);
+		if (ret < 0)
+			return ret;
+	} else {
+		ret = ag7xxx_switch_write(bus, phy_temp, reg_temp | 1, val >> 16);
+		if (ret < 0)
+			return ret;
+
+		ret = ag7xxx_switch_write(bus, phy_temp, reg_temp | 0, val & 0xffff);
+		if (ret < 0)
+			return ret;
+	}
+
+	return 0;
+}
+
+static u16 ag7xxx_mdio_rw(struct mii_dev *bus, int addr, int reg, u32 val)
+{
+	u32 data;
+
+	/* Dummy read followed by PHY read/write command. */
+	ag7xxx_switch_reg_read(bus, 0x98, &data);
+	data = val | (reg << 16) | (addr << 21) | BIT(30) | BIT(31);
+	ag7xxx_switch_reg_write(bus, 0x98, data);
+
+	/* Wait for operation to finish */
+	do {
+		ag7xxx_switch_reg_read(bus, 0x98, &data);
+	} while (data & BIT(31));
+
+	return data & 0xffff;
+}
+
+static int ag7xxx_mdio_read(struct mii_dev *bus, int addr, int devad, int reg)
+{
+	return ag7xxx_mdio_rw(bus, addr, reg, BIT(27));
+}
+
+static int ag7xxx_mdio_write(struct mii_dev *bus, int addr, int devad, int reg,
+			     u16 val)
+{
+	ag7xxx_mdio_rw(bus, addr, reg, val);
+	return 0;
+}
+
+/*
+ * DMA ring handlers
+ */
+static void ag7xxx_dma_clean_tx(struct udevice *dev)
+{
+	struct ar7xxx_eth_priv *priv = dev_get_priv(dev);
+	struct ag7xxx_dma_desc *curr, *next;
+	u32 start, end;
+	int i;
+
+	for (i = 0; i < CONFIG_TX_DESCR_NUM; i++) {
+		curr = &priv->tx_mac_descrtable[i];
+		next = &priv->tx_mac_descrtable[(i + 1) % CONFIG_TX_DESCR_NUM];
+
+		curr->data_addr = virt_to_phys(&priv->txbuffs[i * CONFIG_ETH_BUFSIZE]);
+		curr->config = AG7XXX_DMADESC_IS_EMPTY;
+		curr->next_desc = virt_to_phys(next);
+	}
+
+	priv->tx_currdescnum = 0;
+
+	/* Cache: Flush descriptors, don't care about buffers. */
+	start = (u32)(&priv->tx_mac_descrtable[0]);
+	end = start + sizeof(priv->tx_mac_descrtable);
+	flush_dcache_range(start, end);
+}
+
+static void ag7xxx_dma_clean_rx(struct udevice *dev)
+{
+	struct ar7xxx_eth_priv *priv = dev_get_priv(dev);
+	struct ag7xxx_dma_desc *curr, *next;
+	u32 start, end;
+	int i;
+
+	for (i = 0; i < CONFIG_RX_DESCR_NUM; i++) {
+		curr = &priv->rx_mac_descrtable[i];
+		next = &priv->rx_mac_descrtable[(i + 1) % CONFIG_RX_DESCR_NUM];
+
+		curr->data_addr = virt_to_phys(&priv->rxbuffs[i * CONFIG_ETH_BUFSIZE]);
+		curr->config = AG7XXX_DMADESC_IS_EMPTY;
+		curr->next_desc = virt_to_phys(next);
+	}
+
+	priv->rx_currdescnum = 0;
+
+	/* Cache: Flush+Invalidate descriptors, Invalidate buffers. */
+	start = (u32)(&priv->rx_mac_descrtable[0]);
+	end = start + sizeof(priv->rx_mac_descrtable);
+	flush_dcache_range(start, end);
+	invalidate_dcache_range(start, end);
+
+	start = (u32)&priv->rxbuffs;
+	end = start + sizeof(priv->rxbuffs);
+	invalidate_dcache_range(start, end);
+}
+
+/*
+ * Ethernet I/O
+ */
+static int ag7xxx_eth_send(struct udevice *dev, void *packet, int length)
+{
+	struct ar7xxx_eth_priv *priv = dev_get_priv(dev);
+	struct ag7xxx_dma_desc *curr;
+	u32 start, end;
+
+	curr = &priv->tx_mac_descrtable[priv->tx_currdescnum];
+
+	/* Cache: Invalidate descriptor. */
+	start = (u32)curr;
+	end = start + sizeof(*curr);
+	invalidate_dcache_range(start, end);
+
+	if (!(curr->config & AG7XXX_DMADESC_IS_EMPTY)) {
+		printf("ag7xxx: Out of TX DMA descriptors!\n");
+		return -EPERM;
+	}
+
+	/* Copy the packet into the data buffer. */
+	memcpy(phys_to_virt(curr->data_addr), packet, length);
+	curr->config = length & AG7XXX_DMADESC_PKT_SIZE_MASK;
+
+	/* Cache: Flush descriptor, Flush buffer. */
+	start = (u32)curr;
+	end = start + sizeof(*curr);
+	flush_dcache_range(start, end);
+	start = (u32)phys_to_virt(curr->data_addr);
+	end = start + length;
+	flush_dcache_range(start, end);
+
+	/* Load the DMA descriptor and start TX DMA. */
+	writel(AG7XXX_ETH_DMA_TX_CTRL_TXE,
+	       priv->regs + AG7XXX_ETH_DMA_TX_CTRL);
+
+	/* Switch to next TX descriptor. */
+	priv->tx_currdescnum = (priv->tx_currdescnum + 1) % CONFIG_TX_DESCR_NUM;
+
+	return 0;
+}
+
+static int ag7xxx_eth_recv(struct udevice *dev, int flags, uchar **packetp)
+{
+	struct ar7xxx_eth_priv *priv = dev_get_priv(dev);
+	struct ag7xxx_dma_desc *curr;
+	u32 start, end, length;
+
+	curr = &priv->rx_mac_descrtable[priv->rx_currdescnum];
+
+	/* Cache: Invalidate descriptor. */
+	start = (u32)curr;
+	end = start + sizeof(*curr);
+	invalidate_dcache_range(start, end);
+
+	/* No packets received. */
+	if (curr->config & AG7XXX_DMADESC_IS_EMPTY)
+		return -EAGAIN;
+
+	length = curr->config & AG7XXX_DMADESC_PKT_SIZE_MASK;
+
+	/* Cache: Invalidate buffer. */
+	start = (u32)phys_to_virt(curr->data_addr);
+	end = start + length;
+	invalidate_dcache_range(start, end);
+
+	/* Receive one packet and return length. */
+	*packetp = phys_to_virt(curr->data_addr);
+	return length;
+}
+
+static int ag7xxx_eth_free_pkt(struct udevice *dev, uchar *packet,
+				   int length)
+{
+	struct ar7xxx_eth_priv *priv = dev_get_priv(dev);
+	struct ag7xxx_dma_desc *curr;
+	u32 start, end;
+
+	curr = &priv->rx_mac_descrtable[priv->rx_currdescnum];
+
+	curr->config = AG7XXX_DMADESC_IS_EMPTY;
+
+	/* Cache: Flush descriptor. */
+	start = (u32)curr;
+	end = start + sizeof(*curr);
+	flush_dcache_range(start, end);
+
+	/* Switch to next RX descriptor. */
+	priv->rx_currdescnum = (priv->rx_currdescnum + 1) % CONFIG_RX_DESCR_NUM;
+
+	return 0;
+}
+
+static int ag7xxx_eth_start(struct udevice *dev)
+{
+	struct ar7xxx_eth_priv *priv = dev_get_priv(dev);
+
+	/* FIXME: Check if link up */
+
+	/* Clear the DMA rings. */
+	ag7xxx_dma_clean_tx(dev);
+	ag7xxx_dma_clean_rx(dev);
+
+	/* Load DMA descriptors and start the RX DMA. */
+	writel(virt_to_phys(&priv->tx_mac_descrtable[priv->tx_currdescnum]),
+	       priv->regs + AG7XXX_ETH_DMA_TX_DESC);
+	writel(virt_to_phys(&priv->rx_mac_descrtable[priv->rx_currdescnum]),
+	       priv->regs + AG7XXX_ETH_DMA_RX_DESC);
+	writel(AG7XXX_ETH_DMA_RX_CTRL_RXE,
+	       priv->regs + AG7XXX_ETH_DMA_RX_CTRL);
+
+	return 0;
+}
+
+static void ag7xxx_eth_stop(struct udevice *dev)
+{
+	struct ar7xxx_eth_priv *priv = dev_get_priv(dev);
+
+	/* Stop the TX DMA. */
+	writel(0, priv->regs + AG7XXX_ETH_DMA_TX_CTRL);
+	wait_for_bit("ag7xxx", priv->regs + AG7XXX_ETH_DMA_TX_CTRL, ~0, 0,
+		     1000, 0);
+
+	/* Stop the RX DMA. */
+	writel(0, priv->regs + AG7XXX_ETH_DMA_RX_CTRL);
+	wait_for_bit("ag7xxx", priv->regs + AG7XXX_ETH_DMA_RX_CTRL, ~0, 0,
+		     1000, 0);
+}
+
+/*
+ * Hardware setup
+ */
+static int ag7xxx_eth_write_hwaddr(struct udevice *dev)
+{
+	struct eth_pdata *pdata = dev_get_platdata(dev);
+	struct ar7xxx_eth_priv *priv = dev_get_priv(dev);
+	unsigned char *mac = pdata->enetaddr;
+	u32 macid_lo, macid_hi;
+
+	macid_hi = mac[3] | (mac[2] << 8) | (mac[1] << 16) | (mac[0] << 24);
+	macid_lo = (mac[5] << 16) | (mac[4] << 24);
+
+	writel(macid_lo, priv->regs + AG7XXX_ETH_ADDR1);
+	writel(macid_hi, priv->regs + AG7XXX_ETH_ADDR2);
+
+	return 0;
+}
+
+static void ag7xxx_hw_setup(struct udevice *dev)
+{
+	struct ar7xxx_eth_priv *priv = dev_get_priv(dev);
+	u32 speed;
+
+	setbits_be32(priv->regs + AG7XXX_ETH_CFG1,
+		     AG7XXX_ETH_CFG1_RX_RST | AG7XXX_ETH_CFG1_TX_RST |
+		     AG7XXX_ETH_CFG1_SOFT_RST);
+
+	mdelay(10);
+
+	writel(AG7XXX_ETH_CFG1_RX_EN | AG7XXX_ETH_CFG1_TX_EN,
+	       priv->regs + AG7XXX_ETH_CFG1);
+
+	if (priv->interface == PHY_INTERFACE_MODE_RMII)
+		speed = AG7XXX_ETH_CFG2_IF_10_100;
+	else
+		speed = AG7XXX_ETH_CFG2_IF_1000;
+
+	clrsetbits_be32(priv->regs + AG7XXX_ETH_CFG2,
+			AG7XXX_ETH_CFG2_IF_SPEED_MASK,
+			speed | AG7XXX_ETH_CFG2_PAD_CRC_EN |
+			AG7XXX_ETH_CFG2_LEN_CHECK);
+
+	writel(0xfff0000, priv->regs + AG7XXX_ETH_FIFO_CFG_1);
+	writel(0x1fff, priv->regs + AG7XXX_ETH_FIFO_CFG_2);
+
+	writel(0x1f00, priv->regs + AG7XXX_ETH_FIFO_CFG_0);
+	setbits_be32(priv->regs + AG7XXX_ETH_FIFO_CFG_4, 0x3ffff);
+	writel(0x10ffff, priv->regs + AG7XXX_ETH_FIFO_CFG_1);
+	writel(0xaaa0555, priv->regs + AG7XXX_ETH_FIFO_CFG_2);
+	writel(0x7eccf, priv->regs + AG7XXX_ETH_FIFO_CFG_5);
+	writel(0x1f00140, priv->regs + AG7XXX_ETH_FIFO_CFG_3);
+}
+
+static int ag7xxx_mii_get_div(void)
+{
+	ulong freq = get_bus_freq(0);
+
+	switch (freq / 1000000) {
+	case 150:	return 0x7;
+	case 175:	return 0x5;
+	case 200:	return 0x4;
+	case 210:	return 0x9;
+	case 220:	return 0x9;
+	default:	return 0x7;
+	}
+}
+
+static int ag7xxx_mii_setup(struct udevice *dev)
+{
+	struct ar7xxx_eth_priv *priv = dev_get_priv(dev);
+	int i, ret, div = ag7xxx_mii_get_div();
+	u32 reg;
+
+	if (priv->model == AG7XXX_MODEL_AG933X) {
+		/* Unit 0 is PHY-less on AR9331, see datasheet Figure 2-3 */
+		if (priv->interface == PHY_INTERFACE_MODE_RMII)
+			return 0;
+	}
+
+	if (priv->model == AG7XXX_MODEL_AG934X) {
+		writel(AG7XXX_ETH_MII_MGMT_CFG_RESET | 0x4,
+		       priv->regs + AG7XXX_ETH_MII_MGMT_CFG);
+		writel(0x4, priv->regs + AG7XXX_ETH_MII_MGMT_CFG);
+		return 0;
+	}
+
+	for (i = 0; i < 10; i++) {
+		writel(AG7XXX_ETH_MII_MGMT_CFG_RESET | div,
+		       priv->regs + AG7XXX_ETH_MII_MGMT_CFG);
+		writel(div, priv->regs + AG7XXX_ETH_MII_MGMT_CFG);
+
+		/* Check the switch */
+		ret = ag7xxx_switch_reg_read(priv->bus, 0x10c, &reg);
+		if (ret)
+			continue;
+
+		if (reg != 0x18007fff)
+			continue;
+
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+static int ag933x_phy_setup_wan(struct udevice *dev)
+{
+	struct ar7xxx_eth_priv *priv = dev_get_priv(dev);
+
+	/* Configure switch port 4 (GMAC0) */
+	return ag7xxx_mdio_write(priv->bus, 4, 0, MII_BMCR, 0x9000);
+}
+
+static int ag933x_phy_setup_lan(struct udevice *dev)
+{
+	struct ar7xxx_eth_priv *priv = dev_get_priv(dev);
+	int i, ret;
+	u32 reg;
+
+	/* Reset the switch */
+	ret = ag7xxx_switch_reg_read(priv->bus, 0, &reg);
+	if (ret)
+		return ret;
+	reg |= BIT(31);
+	ret = ag7xxx_switch_reg_write(priv->bus, 0, reg);
+	if (ret)
+		return ret;
+
+	do {
+		ret = ag7xxx_switch_reg_read(priv->bus, 0, &reg);
+		if (ret)
+			return ret;
+	} while (reg & BIT(31));
+
+	/* Configure switch ports 0...3 (GMAC1) */
+	for (i = 0; i < 4; i++) {
+		ret = ag7xxx_mdio_write(priv->bus, 0x4, 0, MII_BMCR, 0x9000);
+		if (ret)
+			return ret;
+	}
+
+	/* Enable CPU port */
+	ret = ag7xxx_switch_reg_write(priv->bus, 0x78, BIT(8));
+	if (ret)
+		return ret;
+
+	for (i = 0; i < 4; i++) {
+		ret = ag7xxx_switch_reg_write(priv->bus, i * 0x100, BIT(9));
+		if (ret)
+			return ret;
+	}
+
+	/* QM Control */
+	ret = ag7xxx_switch_reg_write(priv->bus, 0x38, 0xc000050e);
+	if (ret)
+		return ret;
+
+	/* Disable Atheros header */
+	ret = ag7xxx_switch_reg_write(priv->bus, 0x104, 0x4004);
+	if (ret)
+		return ret;
+
+	/* Tag priority mapping */
+	ret = ag7xxx_switch_reg_write(priv->bus, 0x70, 0xfa50);
+	if (ret)
+		return ret;
+
+	/* Enable ARP packets to the CPU */
+	ret = ag7xxx_switch_reg_read(priv->bus, 0x5c, &reg);
+	if (ret)
+		return ret;
+	reg |= 0x100000;
+	ret = ag7xxx_switch_reg_write(priv->bus, 0x5c, reg);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int ag933x_phy_setup_reset_set(struct udevice *dev, int port)
+{
+	struct ar7xxx_eth_priv *priv = dev_get_priv(dev);
+	int ret;
+
+	ret = ag7xxx_mdio_write(priv->bus, port, 0, MII_ADVERTISE,
+				ADVERTISE_ALL | ADVERTISE_PAUSE_CAP |
+				ADVERTISE_PAUSE_ASYM);
+	if (ret)
+		return ret;
+
+	if (priv->model == AG7XXX_MODEL_AG934X) {
+		ret = ag7xxx_mdio_write(priv->bus, port, 0, MII_CTRL1000,
+					ADVERTISE_1000FULL);
+		if (ret)
+			return ret;
+	}
+
+	return ag7xxx_mdio_write(priv->bus, port, 0, MII_BMCR,
+				 BMCR_ANENABLE | BMCR_RESET);
+}
+
+static int ag933x_phy_setup_reset_fin(struct udevice *dev, int port)
+{
+	struct ar7xxx_eth_priv *priv = dev_get_priv(dev);
+	int ret;
+
+	do {
+		ret = ag7xxx_mdio_read(priv->bus, port, 0, MII_BMCR);
+		if (ret < 0)
+			return ret;
+		mdelay(10);
+	} while (ret & BMCR_RESET);
+
+	return 0;
+}
+
+static int ag933x_phy_setup_common(struct udevice *dev)
+{
+	struct ar7xxx_eth_priv *priv = dev_get_priv(dev);
+	int i, ret, phymax;
+
+	if (priv->model == AG7XXX_MODEL_AG933X)
+		phymax = 4;
+	else if (priv->model == AG7XXX_MODEL_AG934X)
+		phymax = 5;
+	else
+		return -EINVAL;
+
+	if (priv->interface == PHY_INTERFACE_MODE_RMII) {
+		ret = ag933x_phy_setup_reset_set(dev, phymax);
+		if (ret)
+			return ret;
+
+		ret = ag933x_phy_setup_reset_fin(dev, phymax);
+		if (ret)
+			return ret;
+
+		/* Read out link status */
+		ret = ag7xxx_mdio_read(priv->bus, phymax, 0, MII_MIPSCR);
+		if (ret < 0)
+			return ret;
+
+		return 0;
+	}
+
+	/* Switch ports */
+	for (i = 0; i < phymax; i++) {
+		ret = ag933x_phy_setup_reset_set(dev, i);
+		if (ret)
+			return ret;
+	}
+
+	for (i = 0; i < phymax; i++) {
+		ret = ag933x_phy_setup_reset_fin(dev, i);
+		if (ret)
+			return ret;
+	}
+
+	for (i = 0; i < phymax; i++) {
+		/* Read out link status */
+		ret = ag7xxx_mdio_read(priv->bus, i, 0, MII_MIPSCR);
+		if (ret < 0)
+			return ret;
+	}
+
+	return 0;
+}
+
+static int ag934x_phy_setup(struct udevice *dev)
+{
+	struct ar7xxx_eth_priv *priv = dev_get_priv(dev);
+	int i, ret;
+	u32 reg;
+
+	ret = ag7xxx_switch_reg_write(priv->bus, 0x624, 0x7f7f7f7f);
+	if (ret)
+		return ret;
+	ret = ag7xxx_switch_reg_write(priv->bus, 0x10, 0x40000000);
+	if (ret)
+		return ret;
+	ret = ag7xxx_switch_reg_write(priv->bus, 0x4, 0x07600000);
+	if (ret)
+		return ret;
+	ret = ag7xxx_switch_reg_write(priv->bus, 0xc, 0x01000000);
+	if (ret)
+		return ret;
+	ret = ag7xxx_switch_reg_write(priv->bus, 0x7c, 0x0000007e);
+	if (ret)
+		return ret;
+
+	/* AR8327/AR8328 v1.0 fixup */
+	ret = ag7xxx_switch_reg_read(priv->bus, 0, &reg);
+	if (ret)
+		return ret;
+	if ((reg & 0xffff) == 0x1201) {
+		for (i = 0; i < 5; i++) {
+			ret = ag7xxx_mdio_write(priv->bus, i, 0, 0x1d, 0x0);
+			if (ret)
+				return ret;
+			ret = ag7xxx_mdio_write(priv->bus, i, 0, 0x1e, 0x02ea);
+			if (ret)
+				return ret;
+			ret = ag7xxx_mdio_write(priv->bus, i, 0, 0x1d, 0x3d);
+			if (ret)
+				return ret;
+			ret = ag7xxx_mdio_write(priv->bus, i, 0, 0x1e, 0x68a0);
+			if (ret)
+				return ret;
+		}
+	}
+
+	ret = ag7xxx_switch_reg_read(priv->bus, 0x66c, &reg);
+	if (ret)
+		return ret;
+	reg &= ~0x70000;
+	ret = ag7xxx_switch_reg_write(priv->bus, 0x66c, reg);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int ag7xxx_mac_probe(struct udevice *dev)
+{
+	struct ar7xxx_eth_priv *priv = dev_get_priv(dev);
+	int ret;
+
+	ag7xxx_hw_setup(dev);
+	ret = ag7xxx_mii_setup(dev);
+	if (ret)
+		return ret;
+
+	ag7xxx_eth_write_hwaddr(dev);
+
+	if (priv->model == AG7XXX_MODEL_AG933X) {
+		if (priv->interface == PHY_INTERFACE_MODE_RMII)
+			ret = ag933x_phy_setup_wan(dev);
+		else
+			ret = ag933x_phy_setup_lan(dev);
+	} else if (priv->model == AG7XXX_MODEL_AG934X) {
+		ret = ag934x_phy_setup(dev);
+	} else {
+		return -EINVAL;
+	}
+
+	if (ret)
+		return ret;
+
+	return ag933x_phy_setup_common(dev);
+}
+
+static int ag7xxx_mdio_probe(struct udevice *dev)
+{
+	struct ar7xxx_eth_priv *priv = dev_get_priv(dev);
+	struct mii_dev *bus = mdio_alloc();
+
+	if (!bus)
+		return -ENOMEM;
+
+	bus->read = ag7xxx_mdio_read;
+	bus->write = ag7xxx_mdio_write;
+	snprintf(bus->name, sizeof(bus->name), dev->name);
+
+	bus->priv = (void *)priv;
+
+	return mdio_register(bus);
+}
+
+static int ag7xxx_get_phy_iface_offset(struct udevice *dev)
+{
+	int offset;
+
+	offset = fdtdec_lookup_phandle(gd->fdt_blob, dev->of_offset, "phy");
+	if (offset <= 0) {
+		debug("%s: PHY OF node not found (ret=%i)\n", __func__, offset);
+		return -EINVAL;
+	}
+
+	offset = fdt_parent_offset(gd->fdt_blob, offset);
+	if (offset <= 0) {
+		debug("%s: PHY OF node parent MDIO bus not found (ret=%i)\n",
+		      __func__, offset);
+		return -EINVAL;
+	}
+
+	offset = fdt_parent_offset(gd->fdt_blob, offset);
+	if (offset <= 0) {
+		debug("%s: PHY MDIO OF node parent MAC not found (ret=%i)\n",
+		      __func__, offset);
+		return -EINVAL;
+	}
+
+	return offset;
+}
+
+static int ag7xxx_eth_probe(struct udevice *dev)
+{
+	struct eth_pdata *pdata = dev_get_platdata(dev);
+	struct ar7xxx_eth_priv *priv = dev_get_priv(dev);
+	void __iomem *iobase, *phyiobase;
+	int ret, phyreg;
+
+	/* Decoding of convoluted PHY wiring on Atheros MIPS. */
+	ret = ag7xxx_get_phy_iface_offset(dev);
+	if (ret <= 0)
+		return ret;
+	phyreg = fdtdec_get_int(gd->fdt_blob, ret, "reg", -1);
+
+	iobase = map_physmem(pdata->iobase, 0x200, MAP_NOCACHE);
+	phyiobase = map_physmem(phyreg, 0x200, MAP_NOCACHE);
+
+	debug("%s, iobase=%p, phyiobase=%p, priv=%p\n",
+	      __func__, iobase, phyiobase, priv);
+	priv->regs = iobase;
+	priv->phyregs = phyiobase;
+	priv->interface = pdata->phy_interface;
+	priv->model = dev_get_driver_data(dev);
+
+	ret = ag7xxx_mdio_probe(dev);
+	if (ret)
+		return ret;
+
+	priv->bus = miiphy_get_dev_by_name(dev->name);
+
+	ret = ag7xxx_mac_probe(dev);
+	debug("%s, ret=%d\n", __func__, ret);
+
+	return ret;
+}
+
+static int ag7xxx_eth_remove(struct udevice *dev)
+{
+	struct ar7xxx_eth_priv *priv = dev_get_priv(dev);
+
+	free(priv->phydev);
+	mdio_unregister(priv->bus);
+	mdio_free(priv->bus);
+
+	return 0;
+}
+
+static const struct eth_ops ag7xxx_eth_ops = {
+	.start			= ag7xxx_eth_start,
+	.send			= ag7xxx_eth_send,
+	.recv			= ag7xxx_eth_recv,
+	.free_pkt		= ag7xxx_eth_free_pkt,
+	.stop			= ag7xxx_eth_stop,
+	.write_hwaddr		= ag7xxx_eth_write_hwaddr,
+};
+
+static int ag7xxx_eth_ofdata_to_platdata(struct udevice *dev)
+{
+	struct eth_pdata *pdata = dev_get_platdata(dev);
+	const char *phy_mode;
+	int ret;
+
+	pdata->iobase = dev_get_addr(dev);
+	pdata->phy_interface = -1;
+
+	/* Decoding of convoluted PHY wiring on Atheros MIPS. */
+	ret = ag7xxx_get_phy_iface_offset(dev);
+	if (ret <= 0)
+		return ret;
+
+	phy_mode = fdt_getprop(gd->fdt_blob, ret, "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 ag7xxx_eth_ids[] = {
+	{ .compatible = "qca,ag933x-mac", .data = AG7XXX_MODEL_AG933X },
+	{ .compatible = "qca,ag934x-mac", .data = AG7XXX_MODEL_AG934X },
+	{ }
+};
+
+U_BOOT_DRIVER(eth_ag7xxx) = {
+	.name		= "eth_ag7xxx",
+	.id		= UCLASS_ETH,
+	.of_match	= ag7xxx_eth_ids,
+	.ofdata_to_platdata = ag7xxx_eth_ofdata_to_platdata,
+	.probe		= ag7xxx_eth_probe,
+	.remove		= ag7xxx_eth_remove,
+	.ops		= &ag7xxx_eth_ops,
+	.priv_auto_alloc_size = sizeof(struct ar7xxx_eth_priv),
+	.platdata_auto_alloc_size = sizeof(struct eth_pdata),
+	.flags		= DM_FLAG_ALLOC_PRIV_DMA,
+};
diff --git a/drivers/net/cpsw-common.c b/drivers/net/cpsw-common.c
new file mode 100644
index 0000000..e828e85
--- /dev/null
+++ b/drivers/net/cpsw-common.c
@@ -0,0 +1,121 @@
+/*
+ * CPSW common - libs used across TI ethernet devices.
+ *
+ * Copyright (C) 2016, Texas Instruments, Incorporated
+ *
+ * SPDX-License-Identifier: GPL-2.0+
+ */
+
+#include <common.h>
+#include <dm.h>
+#include <fdt_support.h>
+#include <asm/io.h>
+#include <cpsw.h>
+
+DECLARE_GLOBAL_DATA_PTR;
+
+#define CTRL_MAC_REG(offset, id) ((offset) + 0x8 * (id))
+
+static int davinci_emac_3517_get_macid(struct udevice *dev, u16 offset,
+				       int slave, u8 *mac_addr)
+{
+	void *fdt = (void *)gd->fdt_blob;
+	int node = dev->of_offset;
+	u32 macid_lsb;
+	u32 macid_msb;
+	fdt32_t gmii = 0;
+	int syscon;
+	u32 addr;
+
+	syscon = fdtdec_lookup_phandle(fdt, node, "syscon");
+	if (syscon < 0) {
+		error("Syscon offset not found\n");
+		return -ENOENT;
+	}
+
+	addr = (u32)map_physmem(fdt_translate_address(fdt, syscon, &gmii),
+				sizeof(u32), MAP_NOCACHE);
+	if (addr == FDT_ADDR_T_NONE) {
+		error("Not able to get syscon address to get mac efuse address\n");
+		return -ENOENT;
+	}
+
+	addr += CTRL_MAC_REG(offset, slave);
+
+	/* try reading mac address from efuse */
+	macid_lsb = readl(addr);
+	macid_msb = readl(addr + 4);
+
+	mac_addr[0] = (macid_msb >> 16) & 0xff;
+	mac_addr[1] = (macid_msb >> 8)  & 0xff;
+	mac_addr[2] = macid_msb & 0xff;
+	mac_addr[3] = (macid_lsb >> 16) & 0xff;
+	mac_addr[4] = (macid_lsb >> 8)  & 0xff;
+	mac_addr[5] = macid_lsb & 0xff;
+
+	return 0;
+}
+
+static int cpsw_am33xx_cm_get_macid(struct udevice *dev, u16 offset, int slave,
+				    u8 *mac_addr)
+{
+	void *fdt = (void *)gd->fdt_blob;
+	int node = dev->of_offset;
+	u32 macid_lo;
+	u32 macid_hi;
+	fdt32_t gmii = 0;
+	int syscon;
+	u32 addr;
+
+	syscon = fdtdec_lookup_phandle(fdt, node, "syscon");
+	if (syscon < 0) {
+		error("Syscon offset not found\n");
+		return -ENOENT;
+	}
+
+	addr = (u32)map_physmem(fdt_translate_address(fdt, syscon, &gmii),
+				sizeof(u32), MAP_NOCACHE);
+	if (addr == FDT_ADDR_T_NONE) {
+		error("Not able to get syscon address to get mac efuse address\n");
+		return -ENOENT;
+	}
+
+	addr += CTRL_MAC_REG(offset, slave);
+
+	/* try reading mac address from efuse */
+	macid_lo = readl(addr);
+	macid_hi = readl(addr + 4);
+
+	mac_addr[5] = (macid_lo >> 8) & 0xff;
+	mac_addr[4] = macid_lo & 0xff;
+	mac_addr[3] = (macid_hi >> 24) & 0xff;
+	mac_addr[2] = (macid_hi >> 16) & 0xff;
+	mac_addr[1] = (macid_hi >> 8) & 0xff;
+	mac_addr[0] = macid_hi & 0xff;
+
+	return 0;
+}
+
+int ti_cm_get_macid(struct udevice *dev, int slave, u8 *mac_addr)
+{
+	if (of_machine_is_compatible("ti,dm8148"))
+		return cpsw_am33xx_cm_get_macid(dev, 0x630, slave, mac_addr);
+
+	if (of_machine_is_compatible("ti,am33xx"))
+		return cpsw_am33xx_cm_get_macid(dev, 0x630, slave, mac_addr);
+
+	if (of_device_is_compatible(dev, "ti,am3517-emac"))
+		return davinci_emac_3517_get_macid(dev, 0x110, slave, mac_addr);
+
+	if (of_device_is_compatible(dev, "ti,dm816-emac"))
+		return cpsw_am33xx_cm_get_macid(dev, 0x30, slave, mac_addr);
+
+	if (of_machine_is_compatible("ti,am4372"))
+		return cpsw_am33xx_cm_get_macid(dev, 0x630, slave, mac_addr);
+
+	if (of_machine_is_compatible("ti,dra7"))
+		return davinci_emac_3517_get_macid(dev, 0x514, slave, mac_addr);
+
+	dev_err(dev, "incompatible machine/device type for reading mac address\n");
+	return -ENOENT;
+}
diff --git a/drivers/net/cpsw.c b/drivers/net/cpsw.c
index 7104754..2ce4ec6 100644
--- a/drivers/net/cpsw.c
+++ b/drivers/net/cpsw.c
@@ -26,6 +26,7 @@
 #include <phy.h>
 #include <asm/arch/cpu.h>
 #include <dm.h>
+#include <fdt_support.h>
 
 DECLARE_GLOBAL_DATA_PTR;
 
@@ -965,6 +966,11 @@ static int cpsw_phy_init(struct cpsw_priv *priv, struct cpsw_slave *slave)
 	phydev->supported &= supported;
 	phydev->advertising = phydev->supported;
 
+#ifdef CONFIG_DM_ETH
+	if (slave->data->phy_of_handle)
+		phydev->dev->of_offset = slave->data->phy_of_handle;
+#endif
+
 	priv->phydev = phydev;
 	phy_config(phydev);
 
@@ -1137,6 +1143,11 @@ static const struct eth_ops cpsw_eth_ops = {
 	.stop		= cpsw_eth_stop,
 };
 
+static inline fdt_addr_t cpsw_get_addr_by_node(const void *fdt, int node)
+{
+	return fdtdec_get_addr_size_auto_noparent(fdt, node, "reg", 0, NULL);
+}
+
 static int cpsw_eth_ofdata_to_platdata(struct udevice *dev)
 {
 	struct eth_pdata *pdata = dev_get_platdata(dev);
@@ -1146,9 +1157,8 @@ static int cpsw_eth_ofdata_to_platdata(struct udevice *dev)
 	int node = dev->of_offset;
 	int subnode;
 	int slave_index = 0;
-	uint32_t mac_hi, mac_lo;
-	fdt32_t gmii = 0;
 	int active_slave;
+	int ret;
 
 	pdata->iobase = dev_get_addr(dev);
 	priv->data.version = CPSW_CTRL_VERSION_2;
@@ -1202,29 +1212,52 @@ static int cpsw_eth_ofdata_to_platdata(struct udevice *dev)
 
 		name = fdt_get_name(fdt, subnode, &len);
 		if (!strncmp(name, "mdio", 4)) {
-			priv->data.mdio_base = fdtdec_get_addr(fdt, subnode,
-							       "reg");
+			u32 mdio_base;
+
+			mdio_base = cpsw_get_addr_by_node(fdt, subnode);
+			if (mdio_base == FDT_ADDR_T_NONE) {
+				error("Not able to get MDIO address space\n");
+				return -ENOENT;
+			}
+			priv->data.mdio_base = mdio_base;
 		}
 
 		if (!strncmp(name, "slave", 5)) {
 			u32 phy_id[2];
 
-			if (slave_index >= priv->data.slaves) {
-				printf("error: num slaves and slave nodes did not match\n");
-				return -EINVAL;
-			}
+			if (slave_index >= priv->data.slaves)
+				continue;
 			phy_mode = fdt_getprop(fdt, subnode, "phy-mode", NULL);
 			if (phy_mode)
 				priv->data.slave_data[slave_index].phy_if =
 					phy_get_interface_by_name(phy_mode);
-			fdtdec_get_int_array(fdt, subnode, "phy_id", phy_id, 2);
-			priv->data.slave_data[slave_index].phy_addr = phy_id[1];
+
+			priv->data.slave_data[slave_index].phy_of_handle =
+				fdtdec_lookup_phandle(fdt, subnode,
+						      "phy-handle");
+
+			if (priv->data.slave_data[slave_index].phy_of_handle >= 0) {
+				priv->data.slave_data[slave_index].phy_addr =
+						fdtdec_get_int(gd->fdt_blob,
+							       priv->data.slave_data[slave_index].phy_of_handle,
+							       "reg", -1);
+			} else {
+				fdtdec_get_int_array(fdt, subnode, "phy_id",
+						     phy_id, 2);
+				priv->data.slave_data[slave_index].phy_addr =
+						phy_id[1];
+			}
 			slave_index++;
 		}
 
 		if (!strncmp(name, "cpsw-phy-sel", 12)) {
-			priv->data.gmii_sel = fdtdec_get_addr(fdt, subnode,
-							      "reg");
+			priv->data.gmii_sel = cpsw_get_addr_by_node(fdt,
+								    subnode);
+
+			if (priv->data.gmii_sel == FDT_ADDR_T_NONE) {
+				error("Not able to get gmii_sel reg address\n");
+				return -ENOENT;
+			}
 		}
 	}
 
@@ -1236,20 +1269,11 @@ static int cpsw_eth_ofdata_to_platdata(struct udevice *dev)
 		priv->data.slave_data[1].sliver_reg_ofs = CPSW_SLIVER1_OFFSET;
 	}
 
-	subnode = fdtdec_lookup_phandle(fdt, node, "syscon");
-	priv->data.mac_id = fdt_translate_address((void *)fdt, subnode, &gmii);
-	priv->data.mac_id += AM335X_GMII_SEL_OFFSET;
-	priv->data.mac_id += active_slave * 8;
-
-	/* try reading mac address from efuse */
-	mac_lo = readl(priv->data.mac_id);
-	mac_hi = readl(priv->data.mac_id + 4);
-	pdata->enetaddr[0] = mac_hi & 0xFF;
-	pdata->enetaddr[1] = (mac_hi & 0xFF00) >> 8;
-	pdata->enetaddr[2] = (mac_hi & 0xFF0000) >> 16;
-	pdata->enetaddr[3] = (mac_hi & 0xFF000000) >> 24;
-	pdata->enetaddr[4] = mac_lo & 0xFF;
-	pdata->enetaddr[5] = (mac_lo & 0xFF00) >> 8;
+	ret = ti_cm_get_macid(dev, active_slave, pdata->enetaddr);
+	if (ret < 0) {
+		error("cpsw read efuse mac failed\n");
+		return ret;
+	}
 
 	pdata->phy_interface = priv->data.slave_data[active_slave].phy_if;
 	if (pdata->phy_interface == -1) {
@@ -1270,6 +1294,7 @@ static int cpsw_eth_ofdata_to_platdata(struct udevice *dev)
 		writel(RGMII_MODE_ENABLE, priv->data.gmii_sel);
 		break;
 	}
+
 	return 0;
 }
 
diff --git a/drivers/net/designware.c b/drivers/net/designware.c
index ca58f34..8858f07 100644
--- a/drivers/net/designware.c
+++ b/drivers/net/designware.c
@@ -24,7 +24,12 @@ DECLARE_GLOBAL_DATA_PTR;
 
 static int dw_mdio_read(struct mii_dev *bus, int addr, int devad, int reg)
 {
+#ifdef CONFIG_DM_ETH
+	struct dw_eth_dev *priv = dev_get_priv((struct udevice *)bus->priv);
+	struct eth_mac_regs *mac_p = priv->mac_regs_p;
+#else
 	struct eth_mac_regs *mac_p = bus->priv;
+#endif
 	ulong start;
 	u16 miiaddr;
 	int timeout = CONFIG_MDIO_TIMEOUT;
@@ -47,7 +52,12 @@ static int dw_mdio_read(struct mii_dev *bus, int addr, int devad, int reg)
 static int dw_mdio_write(struct mii_dev *bus, int addr, int devad, int reg,
 			u16 val)
 {
+#ifdef CONFIG_DM_ETH
+	struct dw_eth_dev *priv = dev_get_priv((struct udevice *)bus->priv);
+	struct eth_mac_regs *mac_p = priv->mac_regs_p;
+#else
 	struct eth_mac_regs *mac_p = bus->priv;
+#endif
 	ulong start;
 	u16 miiaddr;
 	int ret = -ETIMEDOUT, timeout = CONFIG_MDIO_TIMEOUT;
@@ -70,7 +80,41 @@ static int dw_mdio_write(struct mii_dev *bus, int addr, int devad, int reg,
 	return ret;
 }
 
-static int dw_mdio_init(const char *name, struct eth_mac_regs *mac_regs_p)
+#if CONFIG_DM_ETH
+static int dw_mdio_reset(struct mii_dev *bus)
+{
+	struct udevice *dev = bus->priv;
+	struct dw_eth_dev *priv = dev_get_priv(dev);
+	struct dw_eth_pdata *pdata = dev_get_platdata(dev);
+	int ret;
+
+	if (!dm_gpio_is_valid(&priv->reset_gpio))
+		return 0;
+
+	/* reset the phy */
+	ret = dm_gpio_set_value(&priv->reset_gpio, 0);
+	if (ret)
+		return ret;
+
+	udelay(pdata->reset_delays[0]);
+
+	ret = dm_gpio_set_value(&priv->reset_gpio, 1);
+	if (ret)
+		return ret;
+
+	udelay(pdata->reset_delays[1]);
+
+	ret = dm_gpio_set_value(&priv->reset_gpio, 0);
+	if (ret)
+		return ret;
+
+	udelay(pdata->reset_delays[2]);
+
+	return 0;
+}
+#endif
+
+static int dw_mdio_init(const char *name, void *priv)
 {
 	struct mii_dev *bus = mdio_alloc();
 
@@ -82,8 +126,11 @@ static int dw_mdio_init(const char *name, struct eth_mac_regs *mac_regs_p)
 	bus->read = dw_mdio_read;
 	bus->write = dw_mdio_write;
 	snprintf(bus->name, sizeof(bus->name), "%s", name);
+#ifdef CONFIG_DM_ETH
+	bus->reset = dw_mdio_reset;
+#endif
 
-	bus->priv = (void *)mac_regs_p;
+	bus->priv = priv;
 
 	return mdio_register(bus);
 }
@@ -98,8 +145,8 @@ static void tx_descs_init(struct dw_eth_dev *priv)
 
 	for (idx = 0; idx < CONFIG_TX_DESCR_NUM; idx++) {
 		desc_p = &desc_table_p[idx];
-		desc_p->dmamac_addr = &txbuffs[idx * CONFIG_ETH_BUFSIZE];
-		desc_p->dmamac_next = &desc_table_p[idx + 1];
+		desc_p->dmamac_addr = (ulong)&txbuffs[idx * CONFIG_ETH_BUFSIZE];
+		desc_p->dmamac_next = (ulong)&desc_table_p[idx + 1];
 
 #if defined(CONFIG_DW_ALTDESCRIPTOR)
 		desc_p->txrx_status &= ~(DESC_TXSTS_TXINT | DESC_TXSTS_TXLAST |
@@ -117,11 +164,11 @@ static void tx_descs_init(struct dw_eth_dev *priv)
 	}
 
 	/* Correcting the last pointer of the chain */
-	desc_p->dmamac_next = &desc_table_p[0];
+	desc_p->dmamac_next = (ulong)&desc_table_p[0];
 
 	/* Flush all Tx buffer descriptors at once */
-	flush_dcache_range((unsigned int)priv->tx_mac_descrtable,
-			   (unsigned int)priv->tx_mac_descrtable +
+	flush_dcache_range((ulong)priv->tx_mac_descrtable,
+			   (ulong)priv->tx_mac_descrtable +
 			   sizeof(priv->tx_mac_descrtable));
 
 	writel((ulong)&desc_table_p[0], &dma_p->txdesclistaddr);
@@ -142,13 +189,12 @@ static void rx_descs_init(struct dw_eth_dev *priv)
 	 * Otherwise there's a chance to get some of them flushed in RAM when
 	 * GMAC is already pushing data to RAM via DMA. This way incoming from
 	 * GMAC data will be corrupted. */
-	flush_dcache_range((unsigned int)rxbuffs, (unsigned int)rxbuffs +
-			   RX_TOTAL_BUFSIZE);
+	flush_dcache_range((ulong)rxbuffs, (ulong)rxbuffs + RX_TOTAL_BUFSIZE);
 
 	for (idx = 0; idx < CONFIG_RX_DESCR_NUM; idx++) {
 		desc_p = &desc_table_p[idx];
-		desc_p->dmamac_addr = &rxbuffs[idx * CONFIG_ETH_BUFSIZE];
-		desc_p->dmamac_next = &desc_table_p[idx + 1];
+		desc_p->dmamac_addr = (ulong)&rxbuffs[idx * CONFIG_ETH_BUFSIZE];
+		desc_p->dmamac_next = (ulong)&desc_table_p[idx + 1];
 
 		desc_p->dmamac_cntl =
 			(MAC_MAX_FRAME_SZ & DESC_RXCTRL_SIZE1MASK) |
@@ -158,11 +204,11 @@ static void rx_descs_init(struct dw_eth_dev *priv)
 	}
 
 	/* Correcting the last pointer of the chain */
-	desc_p->dmamac_next = &desc_table_p[0];
+	desc_p->dmamac_next = (ulong)&desc_table_p[0];
 
 	/* Flush all Rx buffer descriptors at once */
-	flush_dcache_range((unsigned int)priv->rx_mac_descrtable,
-			   (unsigned int)priv->rx_mac_descrtable +
+	flush_dcache_range((ulong)priv->rx_mac_descrtable,
+			   (ulong)priv->rx_mac_descrtable +
 			   sizeof(priv->rx_mac_descrtable));
 
 	writel((ulong)&desc_table_p[0], &dma_p->rxdesclistaddr);
@@ -290,12 +336,11 @@ static int _dw_eth_send(struct dw_eth_dev *priv, void *packet, int length)
 	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];
-	uint32_t desc_start = (uint32_t)desc_p;
-	uint32_t desc_end = desc_start +
+	ulong desc_start = (ulong)desc_p;
+	ulong desc_end = desc_start +
 		roundup(sizeof(*desc_p), ARCH_DMA_MINALIGN);
-	uint32_t data_start = (uint32_t)desc_p->dmamac_addr;
-	uint32_t data_end = data_start +
-		roundup(length, ARCH_DMA_MINALIGN);
+	ulong data_start = desc_p->dmamac_addr;
+	ulong data_end = data_start + roundup(length, ARCH_DMA_MINALIGN);
 	/*
 	 * Strictly we only need to invalidate the "txrx_status" field
 	 * for the following check, but on some platforms we cannot
@@ -312,7 +357,7 @@ static int _dw_eth_send(struct dw_eth_dev *priv, void *packet, int length)
 		return -EPERM;
 	}
 
-	memcpy(desc_p->dmamac_addr, packet, length);
+	memcpy((void *)data_start, packet, length);
 
 	/* Flush data to be sent */
 	flush_dcache_range(data_start, data_end);
@@ -352,11 +397,11 @@ static int _dw_eth_recv(struct dw_eth_dev *priv, uchar **packetp)
 	u32 status, desc_num = priv->rx_currdescnum;
 	struct dmamacdescr *desc_p = &priv->rx_mac_descrtable[desc_num];
 	int length = -EAGAIN;
-	uint32_t desc_start = (uint32_t)desc_p;
-	uint32_t desc_end = desc_start +
+	ulong desc_start = (ulong)desc_p;
+	ulong desc_end = desc_start +
 		roundup(sizeof(*desc_p), ARCH_DMA_MINALIGN);
-	uint32_t data_start = (uint32_t)desc_p->dmamac_addr;
-	uint32_t data_end;
+	ulong data_start = desc_p->dmamac_addr;
+	ulong data_end;
 
 	/* Invalidate entire buffer descriptor */
 	invalidate_dcache_range(desc_start, desc_end);
@@ -372,7 +417,7 @@ static int _dw_eth_recv(struct dw_eth_dev *priv, uchar **packetp)
 		/* Invalidate received data */
 		data_end = data_start + roundup(length, ARCH_DMA_MINALIGN);
 		invalidate_dcache_range(data_start, data_end);
-		*packetp = desc_p->dmamac_addr;
+		*packetp = (uchar *)(ulong)desc_p->dmamac_addr;
 	}
 
 	return length;
@@ -382,8 +427,8 @@ 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 +
+	ulong desc_start = (ulong)desc_p;
+	ulong desc_end = desc_start +
 		roundup(sizeof(*desc_p), ARCH_DMA_MINALIGN);
 
 	/*
@@ -488,6 +533,11 @@ int designware_initialize(ulong base_addr, u32 interface)
 		return -ENOMEM;
 	}
 
+	if ((phys_addr_t)priv + sizeof(*priv) > (1ULL << 32)) {
+		printf("designware: buffers are outside DMA memory\n");
+		return -EINVAL;
+	}
+
 	memset(dev, 0, sizeof(struct eth_device));
 	memset(priv, 0, sizeof(struct dw_eth_dev));
 
@@ -583,6 +633,7 @@ 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);
 	u32 iobase = pdata->iobase;
+	ulong ioaddr;
 	int ret;
 
 #ifdef CONFIG_DM_PCI
@@ -601,12 +652,13 @@ static int designware_eth_probe(struct udevice *dev)
 #endif
 
 	debug("%s, iobase=%x, priv=%p\n", __func__, iobase, priv);
-	priv->mac_regs_p = (struct eth_mac_regs *)iobase;
-	priv->dma_regs_p = (struct eth_dma_regs *)(iobase + DW_DMA_BASE_OFFSET);
+	ioaddr = iobase;
+	priv->mac_regs_p = (struct eth_mac_regs *)ioaddr;
+	priv->dma_regs_p = (struct eth_dma_regs *)(ioaddr + DW_DMA_BASE_OFFSET);
 	priv->interface = pdata->phy_interface;
 	priv->max_speed = pdata->max_speed;
 
-	dw_mdio_init(dev->name, priv->mac_regs_p);
+	dw_mdio_init(dev->name, dev);
 	priv->bus = miiphy_get_dev_by_name(dev->name);
 
 	ret = dw_phy_init(priv, dev);
@@ -637,9 +689,13 @@ static const struct eth_ops designware_eth_ops = {
 
 static int designware_eth_ofdata_to_platdata(struct udevice *dev)
 {
-	struct eth_pdata *pdata = dev_get_platdata(dev);
+	struct dw_eth_pdata *dw_pdata = dev_get_platdata(dev);
+	struct dw_eth_dev *priv = dev_get_priv(dev);
+	struct eth_pdata *pdata = &dw_pdata->eth_pdata;
 	const char *phy_mode;
 	const fdt32_t *cell;
+	int reset_flags = GPIOD_IS_OUT;
+	int ret = 0;
 
 	pdata->iobase = dev_get_addr(dev);
 	pdata->phy_interface = -1;
@@ -656,7 +712,20 @@ static int designware_eth_ofdata_to_platdata(struct udevice *dev)
 	if (cell)
 		pdata->max_speed = fdt32_to_cpu(*cell);
 
-	return 0;
+	if (fdtdec_get_bool(gd->fdt_blob, dev->of_offset,
+			    "snps,reset-active-low"))
+		reset_flags |= GPIOD_ACTIVE_LOW;
+
+	ret = gpio_request_by_name(dev, "snps,reset-gpio", 0,
+		&priv->reset_gpio, reset_flags);
+	if (ret == 0) {
+		ret = fdtdec_get_int_array(gd->fdt_blob, dev->of_offset,
+			"snps,reset-delays-us", dw_pdata->reset_delays, 3);
+	} else if (ret == -ENOENT) {
+		ret = 0;
+	}
+
+	return ret;
 }
 
 static const struct udevice_id designware_eth_ids[] = {
@@ -675,7 +744,7 @@ U_BOOT_DRIVER(eth_designware) = {
 	.remove	= designware_eth_remove,
 	.ops	= &designware_eth_ops,
 	.priv_auto_alloc_size = sizeof(struct dw_eth_dev),
-	.platdata_auto_alloc_size = sizeof(struct eth_pdata),
+	.platdata_auto_alloc_size = sizeof(struct dw_eth_pdata),
 	.flags = DM_FLAG_ALLOC_PRIV_DMA,
 };
 
diff --git a/drivers/net/designware.h b/drivers/net/designware.h
index ed6344c..51ba769 100644
--- a/drivers/net/designware.h
+++ b/drivers/net/designware.h
@@ -8,6 +8,8 @@
 #ifndef _DW_ETH_H
 #define _DW_ETH_H
 
+#include <asm/gpio.h>
+
 #define CONFIG_TX_DESCR_NUM	16
 #define CONFIG_RX_DESCR_NUM	16
 #define CONFIG_ETH_BUFSIZE	2048
@@ -110,8 +112,8 @@ struct eth_dma_regs {
 struct dmamacdescr {
 	u32 txrx_status;
 	u32 dmamac_cntl;
-	void *dmamac_addr;
-	struct dmamacdescr *dmamac_next;
+	u32 dmamac_addr;
+	u32 dmamac_next;
 } __aligned(ARCH_DMA_MINALIGN);
 
 /*
@@ -232,8 +234,16 @@ struct dw_eth_dev {
 #ifndef CONFIG_DM_ETH
 	struct eth_device *dev;
 #endif
+	struct gpio_desc reset_gpio;
 	struct phy_device *phydev;
 	struct mii_dev *bus;
 };
 
+#ifdef CONFIG_DM_ETH
+struct dw_eth_pdata {
+	struct eth_pdata eth_pdata;
+	u32 reset_delays[3];
+};
+#endif
+
 #endif
diff --git a/drivers/net/fm/fm.c b/drivers/net/fm/fm.c
index e2a8ed3..00cdfd4 100644
--- a/drivers/net/fm/fm.c
+++ b/drivers/net/fm/fm.c
@@ -360,7 +360,7 @@ int fm_init_common(int index, struct ccsr_fman *reg)
 	size_t fw_length = CONFIG_SYS_QE_FMAN_FW_LENGTH;
 	void *addr = malloc(CONFIG_SYS_QE_FMAN_FW_LENGTH);
 
-	rc = nand_read(&nand_info[0], (loff_t)CONFIG_SYS_FMAN_FW_ADDR,
+	rc = nand_read(nand_info[0], (loff_t)CONFIG_SYS_FMAN_FW_ADDR,
 		       &fw_length, (u_char *)addr);
 	if (rc == -EUCLEAN) {
 		printf("NAND read of FMAN firmware at offset 0x%x failed %d\n",
diff --git a/drivers/net/fm/t4240.c b/drivers/net/fm/t4240.c
index ae5aca4..70887fa 100644
--- a/drivers/net/fm/t4240.c
+++ b/drivers/net/fm/t4240.c
@@ -74,7 +74,7 @@ phy_interface_t fman_port_enet_if(enum fm_port port)
 	if ((port == FM1_DTSEC9 || port == FM1_DTSEC10) &&
 	    ((is_serdes_configured(XFI_FM1_MAC9)) ||
 	     (is_serdes_configured(XFI_FM1_MAC10))))
-		return PHY_INTERFACE_MODE_XGMII;
+		return PHY_INTERFACE_MODE_NONE;
 
 	if ((port == FM2_10GEC1 || port == FM2_10GEC2) &&
 	    ((is_serdes_configured(XAUI_FM2_MAC9))	||
diff --git a/drivers/net/pcnet.c b/drivers/net/pcnet.c
index 16a7512..1da9996 100644
--- a/drivers/net/pcnet.c
+++ b/drivers/net/pcnet.c
@@ -135,14 +135,11 @@ static void pcnet_halt (struct eth_device *dev);
 static int pcnet_probe (struct eth_device *dev, bd_t * bis, int dev_num);
 
 static inline pci_addr_t pcnet_virt_to_mem(const struct eth_device *dev,
-						void *addr, bool uncached)
+						void *addr)
 {
-	pci_dev_t devbusfn = (pci_dev_t)dev->priv;
+	pci_dev_t devbusfn = (pci_dev_t)(unsigned long)dev->priv;
 	void *virt_addr = addr;
 
-	if (uncached)
-		virt_addr = (void *)CKSEG0ADDR(addr);
-
 	return pci_virt_to_mem(devbusfn, virt_addr);
 }
 
@@ -158,6 +155,7 @@ int pcnet_initialize(bd_t *bis)
 	struct eth_device *dev;
 	u16 command, status;
 	int dev_nr = 0;
+	u32 bar;
 
 	PCNET_DEBUG1("\npcnet_initialize...\n");
 
@@ -179,19 +177,18 @@ int pcnet_initialize(bd_t *bis)
 			break;
 		}
 		memset(dev, 0, sizeof(*dev));
-		dev->priv = (void *)devbusfn;
+		dev->priv = (void *)(unsigned long)devbusfn;
 		sprintf(dev->name, "pcnet#%d", dev_nr);
 
 		/*
 		 * Setup the PCI device.
 		 */
-		pci_read_config_dword(devbusfn, PCI_BASE_ADDRESS_0,
-				      (unsigned int *)&dev->iobase);
-		dev->iobase = pci_io_to_phys(devbusfn, dev->iobase);
+		pci_read_config_dword(devbusfn, PCI_BASE_ADDRESS_0, &bar);
+		dev->iobase = pci_io_to_phys(devbusfn, bar);
 		dev->iobase &= ~0xf;
 
-		PCNET_DEBUG1("%s: devbusfn=0x%x iobase=0x%x: ",
-			     dev->name, devbusfn, dev->iobase);
+		PCNET_DEBUG1("%s: devbusfn=0x%x iobase=0x%lx: ",
+			     dev->name, devbusfn, (unsigned long)dev->iobase);
 
 		command = PCI_COMMAND_IO | PCI_COMMAND_MASTER;
 		pci_write_config_word(devbusfn, PCI_COMMAND, command);
@@ -298,7 +295,7 @@ static int pcnet_init(struct eth_device *dev, bd_t *bis)
 {
 	struct pcnet_uncached_priv *uc;
 	int i, val;
-	u32 addr;
+	unsigned long addr;
 
 	PCNET_DEBUG1("%s: pcnet_init...\n", dev->name);
 
@@ -336,16 +333,18 @@ static int pcnet_init(struct eth_device *dev, bd_t *bis)
 	 * must be aligned on 16-byte boundaries.
 	 */
 	if (lp == NULL) {
-		addr = (u32)malloc(sizeof(pcnet_priv_t) + 0x10);
+		addr = (unsigned long)malloc(sizeof(pcnet_priv_t) + 0x10);
 		addr = (addr + 0xf) & ~0xf;
 		lp = (pcnet_priv_t *)addr;
 
-		addr = (u32)memalign(ARCH_DMA_MINALIGN, sizeof(*lp->uc));
+		addr = (unsigned long)memalign(ARCH_DMA_MINALIGN,
+					       sizeof(*lp->uc));
 		flush_dcache_range(addr, addr + sizeof(*lp->uc));
 		addr = UNCACHED_SDRAM(addr);
 		lp->uc = (struct pcnet_uncached_priv *)addr;
 
-		addr = (u32)memalign(ARCH_DMA_MINALIGN, sizeof(*lp->rx_buf));
+		addr = (unsigned long)memalign(ARCH_DMA_MINALIGN,
+					       sizeof(*lp->rx_buf));
 		flush_dcache_range(addr, addr + sizeof(*lp->rx_buf));
 		lp->rx_buf = (void *)addr;
 	}
@@ -361,7 +360,7 @@ static int pcnet_init(struct eth_device *dev, bd_t *bis)
 	 */
 	lp->cur_rx = 0;
 	for (i = 0; i < RX_RING_SIZE; i++) {
-		addr = pcnet_virt_to_mem(dev, (*lp->rx_buf)[i], false);
+		addr = pcnet_virt_to_mem(dev, (*lp->rx_buf)[i]);
 		uc->rx_ring[i].base = cpu_to_le32(addr);
 		uc->rx_ring[i].buf_length = cpu_to_le16(-PKT_BUF_SZ);
 		uc->rx_ring[i].status = cpu_to_le16(0x8000);
@@ -393,9 +392,9 @@ static int pcnet_init(struct eth_device *dev, bd_t *bis)
 
 	uc->init_block.tlen_rlen = cpu_to_le16(TX_RING_LEN_BITS |
 					       RX_RING_LEN_BITS);
-	addr = pcnet_virt_to_mem(dev, uc->rx_ring, true);
+	addr = pcnet_virt_to_mem(dev, uc->rx_ring);
 	uc->init_block.rx_ring = cpu_to_le32(addr);
-	addr = pcnet_virt_to_mem(dev, uc->tx_ring, true);
+	addr = pcnet_virt_to_mem(dev, uc->tx_ring);
 	uc->init_block.tx_ring = cpu_to_le32(addr);
 
 	PCNET_DEBUG1("\ntlen_rlen=0x%x rx_ring=0x%x tx_ring=0x%x\n",
@@ -406,7 +405,7 @@ static int pcnet_init(struct eth_device *dev, bd_t *bis)
 	 * Tell the controller where the Init Block is located.
 	 */
 	barrier();
-	addr = pcnet_virt_to_mem(dev, &lp->uc->init_block, true);
+	addr = pcnet_virt_to_mem(dev, &lp->uc->init_block);
 	pcnet_write_csr(dev, 1, addr & 0xffff);
 	pcnet_write_csr(dev, 2, (addr >> 16) & 0xffff);
 
@@ -464,7 +463,7 @@ static int pcnet_send(struct eth_device *dev, void *packet, int pkt_len)
 	 * Setup Tx ring. Caution: the write order is important here,
 	 * set the status with the "ownership" bits last.
 	 */
-	addr = pcnet_virt_to_mem(dev, packet, false);
+	addr = pcnet_virt_to_mem(dev, packet);
 	writew(-pkt_len, &entry->length);
 	writel(0, &entry->misc);
 	writel(addr, &entry->base);
diff --git a/drivers/net/phy/broadcom.c b/drivers/net/phy/broadcom.c
index 4b2808e..9871cc3 100644
--- a/drivers/net/phy/broadcom.c
+++ b/drivers/net/phy/broadcom.c
@@ -84,11 +84,14 @@ static int bcm54xx_parse_status(struct phy_device *phydev)
 
 static int bcm54xx_startup(struct phy_device *phydev)
 {
+	int ret;
+
 	/* Read the Status (2x to make sure link is right) */
-	genphy_update_link(phydev);
-	bcm54xx_parse_status(phydev);
+	ret = genphy_update_link(phydev);
+	if (ret)
+		return ret;
 
-	return 0;
+	return bcm54xx_parse_status(phydev);
 }
 
 /* Broadcom BCM5482S */
@@ -139,11 +142,14 @@ static int bcm5482_config(struct phy_device *phydev)
 
 static int bcm_cygnus_startup(struct phy_device *phydev)
 {
+	int ret;
+
 	/* Read the Status (2x to make sure link is right) */
-	genphy_update_link(phydev);
-	genphy_parse_link(phydev);
+	ret = genphy_update_link(phydev);
+	if (ret)
+		return ret;
 
-	return 0;
+	return genphy_parse_link(phydev);
 }
 
 static int bcm_cygnus_config(struct phy_device *phydev)
@@ -239,17 +245,21 @@ static u32 bcm5482_parse_serdes_sr(struct phy_device *phydev)
  */
 static int bcm5482_startup(struct phy_device *phydev)
 {
+	int ret;
+
 	if (bcm5482_is_serdes(phydev)) {
 		bcm5482_parse_serdes_sr(phydev);
 		phydev->port = PORT_FIBRE;
-	} else {
-		/* Wait for auto-negotiation to complete or fail */
-		genphy_update_link(phydev);
-		/* Parse BCM54xx copper aux status register */
-		bcm54xx_parse_status(phydev);
+		return 0;
 	}
 
-	return 0;
+	/* Wait for auto-negotiation to complete or fail */
+	ret = genphy_update_link(phydev);
+	if (ret)
+		return ret;
+
+	/* Parse BCM54xx copper aux status register */
+	return bcm54xx_parse_status(phydev);
 }
 
 static struct phy_driver BCM5461S_driver = {
diff --git a/drivers/net/phy/cortina.c b/drivers/net/phy/cortina.c
index f975fd8..fd130d5 100644
--- a/drivers/net/phy/cortina.c
+++ b/drivers/net/phy/cortina.c
@@ -139,8 +139,8 @@ void cs4340_upload_firmware(struct phy_device *phydev)
 	size_t fw_length = CONFIG_CORTINA_FW_LENGTH;
 
 	addr = malloc(CONFIG_CORTINA_FW_LENGTH);
-	ret = nand_read(&nand_info[0], (loff_t)CONFIG_CORTINA_FW_ADDR,
-		       &fw_length, (u_char *)addr);
+	ret = nand_read(nand_info[0], (loff_t)CONFIG_CORTINA_FW_ADDR,
+			&fw_length, (u_char *)addr);
 	if (ret == -EUCLEAN) {
 		printf("NAND read of Cortina firmware at 0x%x failed %d\n",
 		       CONFIG_CORTINA_FW_ADDR, ret);
diff --git a/drivers/net/phy/davicom.c b/drivers/net/phy/davicom.c
index 0c039fe..0a6e410 100644
--- a/drivers/net/phy/davicom.c
+++ b/drivers/net/phy/davicom.c
@@ -60,10 +60,13 @@ static int dm9161_parse_status(struct phy_device *phydev)
 
 static int dm9161_startup(struct phy_device *phydev)
 {
-	genphy_update_link(phydev);
-	dm9161_parse_status(phydev);
+	int ret;
 
-	return 0;
+	ret = genphy_update_link(phydev);
+	if (ret)
+		return ret;
+
+	return dm9161_parse_status(phydev);
 }
 
 static struct phy_driver DM9161_driver = {
diff --git a/drivers/net/phy/et1011c.c b/drivers/net/phy/et1011c.c
index 70c15e2..2fe0132 100644
--- a/drivers/net/phy/et1011c.c
+++ b/drivers/net/phy/et1011c.c
@@ -79,9 +79,13 @@ static int et1011c_parse_status(struct phy_device *phydev)
 
 static int et1011c_startup(struct phy_device *phydev)
 {
-	genphy_update_link(phydev);
-	et1011c_parse_status(phydev);
-	return 0;
+	int ret;
+
+	ret = genphy_update_link(phydev);
+	if (ret)
+		return ret;
+
+	return et1011c_parse_status(phydev);
 }
 
 static struct phy_driver et1011c_driver = {
diff --git a/drivers/net/phy/lxt.c b/drivers/net/phy/lxt.c
index 91838ce..9abc2a8 100644
--- a/drivers/net/phy/lxt.c
+++ b/drivers/net/phy/lxt.c
@@ -49,10 +49,13 @@ static int lxt971_parse_status(struct phy_device *phydev)
 
 static int lxt971_startup(struct phy_device *phydev)
 {
-	genphy_update_link(phydev);
-	lxt971_parse_status(phydev);
+	int ret;
 
-	return 0;
+	ret = genphy_update_link(phydev);
+	if (ret)
+		return ret;
+
+	return lxt971_parse_status(phydev);
 }
 
 static struct phy_driver LXT971_driver = {
diff --git a/drivers/net/phy/marvell.c b/drivers/net/phy/marvell.c
index b8b1157..d2e68d4 100644
--- a/drivers/net/phy/marvell.c
+++ b/drivers/net/phy/marvell.c
@@ -103,7 +103,7 @@ static int m88e1011s_config(struct phy_device *phydev)
 /* Parse the 88E1011's status register for speed and duplex
  * information
  */
-static uint m88e1xxx_parse_status(struct phy_device *phydev)
+static int m88e1xxx_parse_status(struct phy_device *phydev)
 {
 	unsigned int speed;
 	unsigned int mii_reg;
@@ -120,7 +120,7 @@ static uint m88e1xxx_parse_status(struct phy_device *phydev)
 			if (i > PHY_AUTONEGOTIATE_TIMEOUT) {
 				puts(" TIMEOUT !\n");
 				phydev->link = 0;
-				break;
+				return -ETIMEDOUT;
 			}
 
 			if ((i++ % 1000) == 0)
@@ -162,10 +162,13 @@ static uint m88e1xxx_parse_status(struct phy_device *phydev)
 
 static int m88e1011s_startup(struct phy_device *phydev)
 {
-	genphy_update_link(phydev);
-	m88e1xxx_parse_status(phydev);
+	int ret;
 
-	return 0;
+	ret = genphy_update_link(phydev);
+	if (ret)
+		return ret;
+
+	return m88e1xxx_parse_status(phydev);
 }
 
 /* Marvell 88E1111S */
@@ -349,22 +352,21 @@ static int m88e1118_config(struct phy_device *phydev)
 	/* Change Page Number */
 	phy_write(phydev, MDIO_DEVAD_NONE, MIIM_88E1118_PHY_PAGE, 0x0000);
 
-	genphy_config_aneg(phydev);
-
-	phy_reset(phydev);
-
-	return 0;
+	return genphy_config_aneg(phydev);
 }
 
 static int m88e1118_startup(struct phy_device *phydev)
 {
+	int ret;
+
 	/* Change Page Number */
 	phy_write(phydev, MDIO_DEVAD_NONE, MIIM_88E1118_PHY_PAGE, 0x0000);
 
-	genphy_update_link(phydev);
-	m88e1xxx_parse_status(phydev);
+	ret = genphy_update_link(phydev);
+	if (ret)
+		return ret;
 
-	return 0;
+	return m88e1xxx_parse_status(phydev);
 }
 
 /* Marvell 88E1121R */
@@ -421,12 +423,15 @@ static int m88e1145_config(struct phy_device *phydev)
 
 static int m88e1145_startup(struct phy_device *phydev)
 {
-	genphy_update_link(phydev);
+	int ret;
+
+	ret = genphy_update_link(phydev);
+	if (ret)
+		return ret;
+
 	phy_write(phydev, MDIO_DEVAD_NONE, MIIM_88E1145_PHY_LED_CONTROL,
 			MIIM_88E1145_PHY_LED_DIRECT);
-	m88e1xxx_parse_status(phydev);
-
-	return 0;
+	return m88e1xxx_parse_status(phydev);
 }
 
 /* Marvell 88E1149S */
diff --git a/drivers/net/phy/micrel.c b/drivers/net/phy/micrel.c
index 8fcf737..b08788a 100644
--- a/drivers/net/phy/micrel.c
+++ b/drivers/net/phy/micrel.c
@@ -181,7 +181,12 @@ static struct phy_driver KS8721_driver = {
 static int ksz90xx_startup(struct phy_device *phydev)
 {
 	unsigned phy_ctl;
-	genphy_update_link(phydev);
+	int ret;
+
+	ret = genphy_update_link(phydev);
+	if (ret)
+		return ret;
+
 	phy_ctl = phy_read(phydev, MDIO_DEVAD_NONE, MII_KSZ90xx_PHY_CTL);
 
 	if (phy_ctl & MIIM_KSZ90xx_PHYCTL_DUPLEX)
diff --git a/drivers/net/phy/mv88e61xx.c b/drivers/net/phy/mv88e61xx.c
index 302abe8..74d5609 100644
--- a/drivers/net/phy/mv88e61xx.c
+++ b/drivers/net/phy/mv88e61xx.c
@@ -1,4 +1,9 @@
 /*
+ * (C) Copyright 2015
+ * Elecsys Corporation <www.elecsyscorp.com>
+ * Kevin Smith <kevin.smith@elecsyscorp.com>
+ *
+ * Original driver:
  * (C) Copyright 2009
  * Marvell Semiconductor <www.marvell.com>
  * Prafulla Wadaskar <prafulla@marvell.com>
@@ -6,532 +11,1007 @@
  * SPDX-License-Identifier:	GPL-2.0+
  */
 
+/*
+ * PHY driver for mv88e61xx ethernet switches.
+ *
+ * This driver configures the mv88e61xx for basic use as a PHY.  The switch
+ * supports a VLAN configuration that determines how traffic will be routed
+ * between the ports.  This driver uses a simple configuration that routes
+ * traffic from each PHY port only to the CPU port, and from the CPU port to
+ * any PHY port.
+ *
+ * The configuration determines which PHY ports to activate using the
+ * CONFIG_MV88E61XX_PHY_PORTS bitmask.  Setting bit 0 will activate port 0, bit
+ * 1 activates port 1, etc.  Do not set the bit for the port the CPU is
+ * connected to unless it is connected over a PHY interface (not MII).
+ *
+ * This driver was written for and tested on the mv88e6176 with an SGMII
+ * connection.  Other configurations should be supported, but some additions or
+ * changes may be required.
+ */
+
 #include <common.h>
+
+#include <bitfield.h>
+#include <errno.h>
+#include <malloc.h>
+#include <miiphy.h>
 #include <netdev.h>
-#include "mv88e61xx.h"
+
+#define PHY_AUTONEGOTIATE_TIMEOUT	5000
+
+#define PORT_COUNT			7
+#define PORT_MASK			((1 << PORT_COUNT) - 1)
+
+/* Device addresses */
+#define DEVADDR_PHY(p)			(p)
+#define DEVADDR_PORT(p)			(0x10 + (p))
+#define DEVADDR_SERDES			0x0F
+#define DEVADDR_GLOBAL_1		0x1B
+#define DEVADDR_GLOBAL_2		0x1C
+
+/* SMI indirection registers for multichip addressing mode */
+#define SMI_CMD_REG			0x00
+#define SMI_DATA_REG			0x01
+
+/* Global registers */
+#define GLOBAL1_STATUS			0x00
+#define GLOBAL1_CTRL			0x04
+#define GLOBAL1_MON_CTRL		0x1A
+
+/* Global 2 registers */
+#define GLOBAL2_REG_PHY_CMD		0x18
+#define GLOBAL2_REG_PHY_DATA		0x19
+
+/* Port registers */
+#define PORT_REG_STATUS			0x00
+#define PORT_REG_PHYS_CTRL		0x01
+#define PORT_REG_SWITCH_ID		0x03
+#define PORT_REG_CTRL			0x04
+#define PORT_REG_VLAN_MAP		0x06
+#define PORT_REG_VLAN_ID		0x07
+
+/* Phy registers */
+#define PHY_REG_CTRL1			0x10
+#define PHY_REG_STATUS1			0x11
+#define PHY_REG_PAGE			0x16
+
+/* Serdes registers */
+#define SERDES_REG_CTRL_1		0x10
+
+/* Phy page numbers */
+#define PHY_PAGE_COPPER			0
+#define PHY_PAGE_SERDES			1
+
+/* Register fields */
+#define GLOBAL1_CTRL_SWRESET		BIT(15)
+
+#define GLOBAL1_MON_CTRL_CPUDEST_SHIFT	4
+#define GLOBAL1_MON_CTRL_CPUDEST_WIDTH	4
+
+#define PORT_REG_STATUS_LINK		BIT(11)
+#define PORT_REG_STATUS_DUPLEX		BIT(10)
+
+#define PORT_REG_STATUS_SPEED_SHIFT	8
+#define PORT_REG_STATUS_SPEED_WIDTH	2
+#define PORT_REG_STATUS_SPEED_10	0
+#define PORT_REG_STATUS_SPEED_100	1
+#define PORT_REG_STATUS_SPEED_1000	2
+
+#define PORT_REG_STATUS_CMODE_MASK		0xF
+#define PORT_REG_STATUS_CMODE_100BASE_X		0x8
+#define PORT_REG_STATUS_CMODE_1000BASE_X	0x9
+#define PORT_REG_STATUS_CMODE_SGMII		0xa
+
+#define PORT_REG_PHYS_CTRL_LINK_VALUE	BIT(5)
+#define PORT_REG_PHYS_CTRL_LINK_FORCE	BIT(4)
+
+#define PORT_REG_CTRL_PSTATE_SHIFT	0
+#define PORT_REG_CTRL_PSTATE_WIDTH	2
+
+#define PORT_REG_VLAN_ID_DEF_VID_SHIFT	0
+#define PORT_REG_VLAN_ID_DEF_VID_WIDTH	12
+
+#define PORT_REG_VLAN_MAP_TABLE_SHIFT	0
+#define PORT_REG_VLAN_MAP_TABLE_WIDTH	11
+
+#define SERDES_REG_CTRL_1_FORCE_LINK	BIT(10)
+
+#define PHY_REG_CTRL1_ENERGY_DET_SHIFT	8
+#define PHY_REG_CTRL1_ENERGY_DET_WIDTH	2
+
+/* Field values */
+#define PORT_REG_CTRL_PSTATE_DISABLED	0
+#define PORT_REG_CTRL_PSTATE_FORWARD	3
+
+#define PHY_REG_CTRL1_ENERGY_DET_OFF	0
+#define PHY_REG_CTRL1_ENERGY_DET_SENSE_ONLY	2
+#define PHY_REG_CTRL1_ENERGY_DET_SENSE_XMIT	3
+
+/* PHY Status Register */
+#define PHY_REG_STATUS1_SPEED		0xc000
+#define PHY_REG_STATUS1_GBIT		0x8000
+#define PHY_REG_STATUS1_100		0x4000
+#define PHY_REG_STATUS1_DUPLEX		0x2000
+#define PHY_REG_STATUS1_SPDDONE		0x0800
+#define PHY_REG_STATUS1_LINK		0x0400
+#define PHY_REG_STATUS1_ENERGY		0x0010
 
 /*
- * Uncomment either of the following line for local debug control;
- * otherwise global debug control will apply.
+ * Macros for building commands for indirect addressing modes.  These are valid
+ * for both the indirect multichip addressing mode and the PHY indirection
+ * required for the writes to any PHY register.
  */
+#define SMI_BUSY			BIT(15)
+#define SMI_CMD_CLAUSE_22		BIT(12)
+#define SMI_CMD_CLAUSE_22_OP_READ	(2 << 10)
+#define SMI_CMD_CLAUSE_22_OP_WRITE	(1 << 10)
+
+#define SMI_CMD_READ			(SMI_BUSY | SMI_CMD_CLAUSE_22 | \
+					 SMI_CMD_CLAUSE_22_OP_READ)
+#define SMI_CMD_WRITE			(SMI_BUSY | SMI_CMD_CLAUSE_22 | \
+					 SMI_CMD_CLAUSE_22_OP_WRITE)
+
+#define SMI_CMD_ADDR_SHIFT		5
+#define SMI_CMD_ADDR_WIDTH		5
+#define SMI_CMD_REG_SHIFT		0
+#define SMI_CMD_REG_WIDTH		5
+
+/* Check for required macros */
+#ifndef CONFIG_MV88E61XX_PHY_PORTS
+#error Define CONFIG_MV88E61XX_PHY_PORTS to indicate which physical ports \
+	to activate
+#endif
+#ifndef CONFIG_MV88E61XX_CPU_PORT
+#error Define CONFIG_MV88E61XX_CPU_PORT to the port the CPU is attached to
+#endif
 
-/* #undef DEBUG */
-/* #define DEBUG */
+/* ID register values for different switch models */
+#define PORT_SWITCH_ID_6172		0x1720
+#define PORT_SWITCH_ID_6176		0x1760
+#define PORT_SWITCH_ID_6240		0x2400
+#define PORT_SWITCH_ID_6352		0x3520
 
-#ifdef CONFIG_MV88E61XX_MULTICHIP_ADRMODE
-/* Chip Address mode
- * The Switch support two modes of operation
- * 1. single chip mode and
- * 2. Multi-chip mode
- * Refer section 9.2 &9.3 in chip datasheet-02 for more details
- *
- * By default single chip mode is configured
- * multichip mode operation can be configured in board header
- */
-static int mv88e61xx_busychk_multic(char *name, u32 devaddr)
+struct mv88e61xx_phy_priv {
+	struct mii_dev *mdio_bus;
+	int smi_addr;
+	int id;
+};
+
+static inline int smi_cmd(int cmd, int addr, int reg)
 {
-	u16 reg = 0;
-	u32 timeout = MV88E61XX_PHY_TIMEOUT;
+	cmd = bitfield_replace(cmd, SMI_CMD_ADDR_SHIFT, SMI_CMD_ADDR_WIDTH,
+			       addr);
+	cmd = bitfield_replace(cmd, SMI_CMD_REG_SHIFT, SMI_CMD_REG_WIDTH, reg);
+	return cmd;
+}
 
-	/* Poll till SMIBusy bit is clear */
-	do {
-		miiphy_read(name, devaddr, 0x0, &reg);
-		if (timeout-- == 0) {
-			printf("SMI busy timeout\n");
-			return -1;
-		}
-	} while (reg & (1 << 15));
-	return 0;
+static inline int smi_cmd_read(int addr, int reg)
+{
+	return smi_cmd(SMI_CMD_READ, addr, reg);
 }
 
-static void mv88e61xx_switch_write(char *name, u32 phy_adr,
-	u32 reg_ofs, u16 data)
+static inline int smi_cmd_write(int addr, int reg)
 {
-	u16 mii_dev_addr;
+	return smi_cmd(SMI_CMD_WRITE, addr, reg);
+}
 
-	/* command to read PHY dev address */
-	if (miiphy_read(name, 0xEE, 0xEE, &mii_dev_addr)) {
-		printf("Error..could not read PHY dev address\n");
-		return;
-	}
-	mv88e61xx_busychk_multic(name, mii_dev_addr);
-	/* Write data to Switch indirect data register */
-	miiphy_write(name, mii_dev_addr, 0x1, data);
-	/* Write command to Switch indirect command register (write) */
-	miiphy_write(name, mii_dev_addr, 0x0,
-		     reg_ofs | (phy_adr << 5) | (1 << 10) | (1 << 12) | (1 <<
-									 15));
+__weak int mv88e61xx_hw_reset(struct phy_device *phydev)
+{
+	return 0;
 }
 
-static void mv88e61xx_switch_read(char *name, u32 phy_adr,
-	u32 reg_ofs, u16 *data)
+/* Wait for the current SMI indirect command to complete */
+static int mv88e61xx_smi_wait(struct mii_dev *bus, int smi_addr)
 {
-	u16 mii_dev_addr;
+	int val;
+	u32 timeout = 100;
 
-	/* command to read PHY dev address */
-	if (miiphy_read(name, 0xEE, 0xEE, &mii_dev_addr)) {
-		printf("Error..could not read PHY dev address\n");
-		return;
-	}
-	mv88e61xx_busychk_multic(name, mii_dev_addr);
-	/* Write command to Switch indirect command register (read) */
-	miiphy_write(name, mii_dev_addr, 0x0,
-		     reg_ofs | (phy_adr << 5) | (1 << 11) | (1 << 12) | (1 <<
-									 15));
-	mv88e61xx_busychk_multic(name, mii_dev_addr);
-	/* Read data from Switch indirect data register */
-	miiphy_read(name, mii_dev_addr, 0x1, data);
+	do {
+		val = bus->read(bus, smi_addr, MDIO_DEVAD_NONE, SMI_CMD_REG);
+		if (val >= 0 && (val & SMI_BUSY) == 0)
+			return 0;
+
+		mdelay(1);
+	} while (--timeout);
+
+	puts("SMI busy timeout\n");
+	return -ETIMEDOUT;
 }
-#endif /* CONFIG_MV88E61XX_MULTICHIP_ADRMODE */
 
 /*
- * Convenience macros for switch device/port reads/writes
- * These macros output valid 'mv88e61xx' U_BOOT_CMDs
+ * The mv88e61xx has three types of addresses: the smi bus address, the device
+ * address, and the register address.  The smi bus address distinguishes it on
+ * the smi bus from other PHYs or switches.  The device address determines
+ * which on-chip register set you are reading/writing (the various PHYs, their
+ * associated ports, or global configuration registers).  The register address
+ * is the offset of the register you are reading/writing.
+ *
+ * When the mv88e61xx is hardware configured to have address zero, it behaves in
+ * single-chip addressing mode, where it responds to all SMI addresses, using
+ * the smi address as its device address.  This obviously only works when this
+ * is the only chip on the SMI bus.  This allows the driver to access device
+ * registers without using indirection.  When the chip is configured to a
+ * non-zero address, it only responds to that SMI address and requires indirect
+ * writes to access the different device addresses.
  */
-
-#ifndef DEBUG
-#define WR_SWITCH_REG wr_switch_reg
-#define RD_SWITCH_REG rd_switch_reg
-#define WR_SWITCH_PORT_REG(n, p, r, d) \
-	WR_SWITCH_REG(n, (MV88E61XX_PRT_OFST+p), r, d)
-#define RD_SWITCH_PORT_REG(n, p, r, d) \
-	RD_SWITCH_REG(n, (MV88E61XX_PRT_OFST+p), r, d)
-#else
-static void WR_SWITCH_REG(char *name, u32 dev_adr, u32 reg_ofs, u16 data)
+static int mv88e61xx_reg_read(struct phy_device *phydev, int dev, int reg)
 {
-	printf("mv88e61xx %s dev %02x reg %02x write %04x\n",
-		name, dev_adr, reg_ofs, data);
-	wr_switch_reg(name, dev_adr, reg_ofs, data);
+	struct mv88e61xx_phy_priv *priv = phydev->priv;
+	struct mii_dev *mdio_bus = priv->mdio_bus;
+	int smi_addr = priv->smi_addr;
+	int res;
+
+	/* In single-chip mode, the device can be addressed directly */
+	if (smi_addr == 0)
+		return mdio_bus->read(mdio_bus, dev, MDIO_DEVAD_NONE, reg);
+
+	/* Wait for the bus to become free */
+	res = mv88e61xx_smi_wait(mdio_bus, smi_addr);
+	if (res < 0)
+		return res;
+
+	/* Issue the read command */
+	res = mdio_bus->write(mdio_bus, smi_addr, MDIO_DEVAD_NONE, SMI_CMD_REG,
+			 smi_cmd_read(dev, reg));
+	if (res < 0)
+		return res;
+
+	/* Wait for the read command to complete */
+	res = mv88e61xx_smi_wait(mdio_bus, smi_addr);
+	if (res < 0)
+		return res;
+
+	/* Read the data */
+	res = mdio_bus->read(mdio_bus, smi_addr, MDIO_DEVAD_NONE, SMI_DATA_REG);
+	if (res < 0)
+		return res;
+
+	return bitfield_extract(res, 0, 16);
 }
-static void RD_SWITCH_REG(char *name, u32 dev_adr, u32 reg_ofs, u16 *data)
+
+/* See the comment above mv88e61xx_reg_read */
+static int mv88e61xx_reg_write(struct phy_device *phydev, int dev, int reg,
+			       u16 val)
 {
-	rd_switch_reg(name, dev_adr, reg_ofs, data);
-	printf("mv88e61xx %s dev %02x reg %02x read %04x\n",
-		name, dev_adr, reg_ofs, *data);
+	struct mv88e61xx_phy_priv *priv = phydev->priv;
+	struct mii_dev *mdio_bus = priv->mdio_bus;
+	int smi_addr = priv->smi_addr;
+	int res;
+
+	/* In single-chip mode, the device can be addressed directly */
+	if (smi_addr == 0) {
+		return mdio_bus->write(mdio_bus, dev, MDIO_DEVAD_NONE, reg,
+				val);
+	}
+
+	/* Wait for the bus to become free */
+	res = mv88e61xx_smi_wait(mdio_bus, smi_addr);
+	if (res < 0)
+		return res;
+
+	/* Set the data to write */
+	res = mdio_bus->write(mdio_bus, smi_addr, MDIO_DEVAD_NONE,
+				SMI_DATA_REG, val);
+	if (res < 0)
+		return res;
+
+	/* Issue the write command */
+	res = mdio_bus->write(mdio_bus, smi_addr, MDIO_DEVAD_NONE, SMI_CMD_REG,
+				smi_cmd_write(dev, reg));
+	if (res < 0)
+		return res;
+
+	/* Wait for the write command to complete */
+	res = mv88e61xx_smi_wait(mdio_bus, smi_addr);
+	if (res < 0)
+		return res;
+
+	return 0;
 }
-static void WR_SWITCH_PORT_REG(char *name, u32 prt_adr, u32 reg_ofs,
-	u16 data)
+
+static int mv88e61xx_phy_wait(struct phy_device *phydev)
 {
-	printf("mv88e61xx %s port %02x reg %02x write %04x\n",
-		name, prt_adr, reg_ofs, data);
-	wr_switch_reg(name, (MV88E61XX_PRT_OFST+prt_adr), reg_ofs, data);
+	int val;
+	u32 timeout = 100;
+
+	do {
+		val = mv88e61xx_reg_read(phydev, DEVADDR_GLOBAL_2,
+					 GLOBAL2_REG_PHY_CMD);
+		if (val >= 0 && (val & SMI_BUSY) == 0)
+			return 0;
+
+		mdelay(1);
+	} while (--timeout);
+
+	return -ETIMEDOUT;
 }
-static void RD_SWITCH_PORT_REG(char *name, u32 prt_adr, u32 reg_ofs,
-	u16 *data)
+
+static int mv88e61xx_phy_read_indirect(struct mii_dev *smi_wrapper, int dev,
+		int devad, int reg)
 {
-	rd_switch_reg(name, (MV88E61XX_PRT_OFST+prt_adr), reg_ofs, data);
-	printf("mv88e61xx %s port %02x reg %02x read %04x\n",
-		name, prt_adr, reg_ofs, *data);
+	struct phy_device *phydev;
+	int res;
+
+	phydev = (struct phy_device *)smi_wrapper->priv;
+
+	/* Issue command to read */
+	res = mv88e61xx_reg_write(phydev, DEVADDR_GLOBAL_2,
+				  GLOBAL2_REG_PHY_CMD,
+				  smi_cmd_read(dev, reg));
+
+	/* Wait for data to be read */
+	res = mv88e61xx_phy_wait(phydev);
+	if (res < 0)
+		return res;
+
+	/* Read retrieved data */
+	return mv88e61xx_reg_read(phydev, DEVADDR_GLOBAL_2,
+				  GLOBAL2_REG_PHY_DATA);
 }
-#endif
 
-/*
- * Local functions to read/write registers on the switch PHYs.
- * NOTE! This goes through switch, not direct miiphy, writes and reads!
- */
+static int mv88e61xx_phy_write_indirect(struct mii_dev *smi_wrapper, int dev,
+		int devad, int reg, u16 data)
+{
+	struct phy_device *phydev;
+	int res;
+
+	phydev = (struct phy_device *)smi_wrapper->priv;
+
+	/* Set the data to write */
+	res = mv88e61xx_reg_write(phydev, DEVADDR_GLOBAL_2,
+				  GLOBAL2_REG_PHY_DATA, data);
+	if (res < 0)
+		return res;
+	/* Issue the write command */
+	res = mv88e61xx_reg_write(phydev, DEVADDR_GLOBAL_2,
+				  GLOBAL2_REG_PHY_CMD,
+				  smi_cmd_write(dev, reg));
+	if (res < 0)
+		return res;
+
+	/* Wait for command to complete */
+	return mv88e61xx_phy_wait(phydev);
+}
 
-/*
- * Make sure SMIBusy bit cleared before another
- * SMI operation can take place
- */
-static int mv88e61xx_busychk(char *name)
+/* Wrapper function to make calls to phy_read_indirect simpler */
+static int mv88e61xx_phy_read(struct phy_device *phydev, int phy, int reg)
 {
-	u16 reg = 0;
-	u32 timeout = MV88E61XX_PHY_TIMEOUT;
-	do {
-		rd_switch_reg(name, MV88E61XX_GLB2REG_DEVADR,
-		       MV88E61XX_PHY_CMD, &reg);
-		if (timeout-- == 0) {
-			printf("SMI busy timeout\n");
-			return -1;
-		}
-	} while (reg & 1 << 15);	/* busy mask */
-	return 0;
+	return mv88e61xx_phy_read_indirect(phydev->bus, DEVADDR_PHY(phy),
+					   MDIO_DEVAD_NONE, reg);
 }
 
-static inline int mv88e61xx_switch_miiphy_write(char *name, u32 phy,
-	u32 reg, u16 data)
+/* Wrapper function to make calls to phy_read_indirect simpler */
+static int mv88e61xx_phy_write(struct phy_device *phydev, int phy,
+		int reg, u16 val)
 {
-	/* write switch data reg then cmd reg then check completion */
-	wr_switch_reg(name, MV88E61XX_GLB2REG_DEVADR, MV88E61XX_PHY_DATA,
-		data);
-	wr_switch_reg(name, MV88E61XX_GLB2REG_DEVADR, MV88E61XX_PHY_CMD,
-		(MV88E61XX_PHY_WRITE_CMD | (phy << 5)  | reg));
-	return mv88e61xx_busychk(name);
+	return mv88e61xx_phy_write_indirect(phydev->bus, DEVADDR_PHY(phy),
+					    MDIO_DEVAD_NONE, reg, val);
 }
 
-static inline int mv88e61xx_switch_miiphy_read(char *name, u32 phy,
-	u32 reg, u16 *data)
+static int mv88e61xx_port_read(struct phy_device *phydev, u8 port, u8 reg)
 {
-	/* write switch cmd reg, check for completion */
-	wr_switch_reg(name, MV88E61XX_GLB2REG_DEVADR, MV88E61XX_PHY_CMD,
-		(MV88E61XX_PHY_READ_CMD | (phy << 5)  | reg));
-	if (mv88e61xx_busychk(name))
-		return -1;
-	/* read switch data reg and return success */
-	rd_switch_reg(name, MV88E61XX_GLB2REG_DEVADR, MV88E61XX_PHY_DATA, data);
-	return 0;
+	return mv88e61xx_reg_read(phydev, DEVADDR_PORT(port), reg);
 }
 
-/*
- * Convenience macros for switch PHY reads/writes
- */
+static int mv88e61xx_port_write(struct phy_device *phydev, u8 port, u8 reg,
+								u16 val)
+{
+	return mv88e61xx_reg_write(phydev, DEVADDR_PORT(port), reg, val);
+}
 
-#ifndef DEBUG
-#define WR_SWITCH_PHY_REG mv88e61xx_switch_miiphy_write
-#define RD_SWITCH_PHY_REG mv88e61xx_switch_miiphy_read
-#else
-static inline int WR_SWITCH_PHY_REG(char *name, u32 phy_adr,
-	u32 reg_ofs, u16 data)
-{
-	int r = mv88e61xx_switch_miiphy_write(name, phy_adr, reg_ofs, data);
-	if (r)
-		printf("** ERROR writing mv88e61xx %s phy %02x reg %02x\n",
-			name, phy_adr, reg_ofs);
-	else
-		printf("mv88e61xx %s phy %02x reg %02x write %04x\n",
-			name, phy_adr, reg_ofs, data);
-	return r;
+static int mv88e61xx_set_page(struct phy_device *phydev, u8 phy, u8 page)
+{
+	return mv88e61xx_phy_write(phydev, phy, PHY_REG_PAGE, page);
 }
-static inline int RD_SWITCH_PHY_REG(char *name, u32 phy_adr,
-	u32 reg_ofs, u16 *data)
+
+static int mv88e61xx_get_switch_id(struct phy_device *phydev)
 {
-	int r = mv88e61xx_switch_miiphy_read(name, phy_adr, reg_ofs, data);
-	if (r)
-		printf("** ERROR reading mv88e61xx %s phy %02x reg %02x\n",
-			name, phy_adr, reg_ofs);
-	else
-		printf("mv88e61xx %s phy %02x reg %02x read %04x\n",
-			name, phy_adr, reg_ofs, *data);
-	return r;
+	int res;
+
+	res = mv88e61xx_port_read(phydev, 0, PORT_REG_SWITCH_ID);
+	if (res < 0)
+		return res;
+	return res & 0xfff0;
 }
-#endif
 
-static void mv88e61xx_port_vlan_config(struct mv88e61xx_config *swconfig)
-{
-	u32 prt;
-	u16 reg;
-	char *name = swconfig->name;
-	u32 port_mask = swconfig->ports_enabled;
-
-	/* apply internal vlan config */
-	for (prt = 0; prt < MV88E61XX_MAX_PORTS_NUM; prt++) {
-		/* only for enabled ports */
-		if ((1 << prt) & port_mask) {
-			/* take vlan map from swconfig */
-			u8 vlanmap = swconfig->vlancfg[prt];
-			/* remove disabled ports from vlan map */
-			vlanmap &= swconfig->ports_enabled;
-			/* apply vlan map to port */
-			RD_SWITCH_PORT_REG(name, prt,
-				MV88E61XX_PRT_VMAP_REG, &reg);
-			reg &= ~((1 << MV88E61XX_MAX_PORTS_NUM) - 1);
-			reg |= vlanmap;
-			WR_SWITCH_PORT_REG(name, prt,
-				MV88E61XX_PRT_VMAP_REG, reg);
-		}
+static bool mv88e61xx_6352_family(struct phy_device *phydev)
+{
+	struct mv88e61xx_phy_priv *priv = phydev->priv;
+
+	switch (priv->id) {
+	case PORT_SWITCH_ID_6172:
+	case PORT_SWITCH_ID_6176:
+	case PORT_SWITCH_ID_6240:
+	case PORT_SWITCH_ID_6352:
+		return true;
 	}
+	return false;
 }
 
-/*
- * Power up the specified port and reset PHY
- */
-static int mv88361xx_powerup(struct mv88e61xx_config *swconfig, u32 phy)
+static int mv88e61xx_get_cmode(struct phy_device *phydev, u8 port)
 {
-	char *name = swconfig->name;
+	int res;
 
-	/* Write Copper Specific control reg1 (0x10) for-
-	 * Enable Phy power up
-	 * Energy Detect on (sense&Xmit NLP Periodically
-	 * reset other settings default
-	 */
-	if (WR_SWITCH_PHY_REG(name, phy, 0x10, 0x3360))
-		return -1;
+	res = mv88e61xx_port_read(phydev, port, PORT_REG_STATUS);
+	if (res < 0)
+		return res;
+	return res & PORT_REG_STATUS_CMODE_MASK;
+}
 
-	/* Write PHY ctrl reg (0x0) to apply
-	 * Phy reset (set bit 15 low)
-	 * reset other default values
-	 */
-	if (WR_SWITCH_PHY_REG(name, phy, 0x00, 0x9140))
-		return -1;
+static int mv88e61xx_parse_status(struct phy_device *phydev)
+{
+	unsigned int speed;
+	unsigned int mii_reg;
+
+	mii_reg = phy_read(phydev, MDIO_DEVAD_NONE, PHY_REG_STATUS1);
+
+	if ((mii_reg & PHY_REG_STATUS1_LINK) &&
+	    !(mii_reg & PHY_REG_STATUS1_SPDDONE)) {
+		int i = 0;
+
+		puts("Waiting for PHY realtime link");
+		while (!(mii_reg & PHY_REG_STATUS1_SPDDONE)) {
+			/* Timeout reached ? */
+			if (i > PHY_AUTONEGOTIATE_TIMEOUT) {
+				puts(" TIMEOUT !\n");
+				phydev->link = 0;
+				break;
+			}
+
+			if ((i++ % 1000) == 0)
+				putc('.');
+			udelay(1000);
+			mii_reg = phy_read(phydev, MDIO_DEVAD_NONE,
+					PHY_REG_STATUS1);
+		}
+		puts(" done\n");
+		udelay(500000);	/* another 500 ms (results in faster booting) */
+	} else {
+		if (mii_reg & PHY_REG_STATUS1_LINK)
+			phydev->link = 1;
+		else
+			phydev->link = 0;
+	}
+
+	if (mii_reg & PHY_REG_STATUS1_DUPLEX)
+		phydev->duplex = DUPLEX_FULL;
+	else
+		phydev->duplex = DUPLEX_HALF;
+
+	speed = mii_reg & PHY_REG_STATUS1_SPEED;
+
+	switch (speed) {
+	case PHY_REG_STATUS1_GBIT:
+		phydev->speed = SPEED_1000;
+		break;
+	case PHY_REG_STATUS1_100:
+		phydev->speed = SPEED_100;
+		break;
+	default:
+		phydev->speed = SPEED_10;
+		break;
+	}
 
 	return 0;
 }
 
-/*
- * Default Setup for LED[0]_Control (ref: Table 46 Datasheet-3)
- * is set to "On-1000Mb/s Link, Off Else"
- * This function sets it to "On-Link, Blink-Activity, Off-NoLink"
- *
- * This is optional settings may be needed on some boards
- * to setup PHY LEDs default configuration to detect 10/100/1000Mb/s
- * Link status
- */
-static int mv88361xx_led_init(struct mv88e61xx_config *swconfig, u32 phy)
+static int mv88e61xx_switch_reset(struct phy_device *phydev)
 {
-	char *name = swconfig->name;
+	int time;
+	int val;
+	u8 port;
+
+	/* Disable all ports */
+	for (port = 0; port < PORT_COUNT; port++) {
+		val = mv88e61xx_port_read(phydev, port, PORT_REG_CTRL);
+		if (val < 0)
+			return val;
+		val = bitfield_replace(val, PORT_REG_CTRL_PSTATE_SHIFT,
+				       PORT_REG_CTRL_PSTATE_WIDTH,
+				       PORT_REG_CTRL_PSTATE_DISABLED);
+		val = mv88e61xx_port_write(phydev, port, PORT_REG_CTRL, val);
+		if (val < 0)
+			return val;
+	}
 
-	if (swconfig->led_init != MV88E61XX_LED_INIT_EN)
-		return 0;
+	/* Wait 2 ms for queues to drain */
+	udelay(2000);
+
+	/* Reset switch */
+	val = mv88e61xx_reg_read(phydev, DEVADDR_GLOBAL_1, GLOBAL1_CTRL);
+	if (val < 0)
+		return val;
+	val |= GLOBAL1_CTRL_SWRESET;
+	val = mv88e61xx_reg_write(phydev, DEVADDR_GLOBAL_1,
+				     GLOBAL1_CTRL, val);
+	if (val < 0)
+		return val;
+
+	/* Wait up to 1 second for switch reset complete */
+	for (time = 1000; time; time--) {
+		val = mv88e61xx_reg_read(phydev, DEVADDR_GLOBAL_1,
+					    GLOBAL1_CTRL);
+		if (val >= 0 && ((val & GLOBAL1_CTRL_SWRESET) == 0))
+			break;
+		udelay(1000);
+	}
+	if (!time)
+		return -ETIMEDOUT;
 
-	/* set page address to 3 */
-	if (WR_SWITCH_PHY_REG(name, phy, 0x16, 0x0003))
-		return -1;
+	return 0;
+}
 
-	/*
-	 * set LED Func Ctrl reg
-	 * value 0x0001 = LED[0] On-Link, Blink-Activity, Off-NoLink
-	 */
-	if (WR_SWITCH_PHY_REG(name, phy, 0x10, 0x0001))
-		return -1;
+static int mv88e61xx_serdes_init(struct phy_device *phydev)
+{
+	int val;
 
-	/* set page address to 0 */
-	if (WR_SWITCH_PHY_REG(name, phy, 0x16, 0x0000))
-		return -1;
+	val = mv88e61xx_set_page(phydev, DEVADDR_SERDES, PHY_PAGE_SERDES);
+	if (val < 0)
+		return val;
+
+	/* Power up serdes module */
+	val = mv88e61xx_phy_read(phydev, DEVADDR_SERDES, MII_BMCR);
+	if (val < 0)
+		return val;
+	val &= ~(BMCR_PDOWN);
+	val = mv88e61xx_phy_write(phydev, DEVADDR_SERDES, MII_BMCR, val);
+	if (val < 0)
+		return val;
 
 	return 0;
 }
 
-/*
- * Reverse Transmit polarity for Media Dependent Interface
- * Pins (MDIP) bits in Copper Specific Control Register 3
- * (Page 0, Reg 20 for each phy (except cpu port)
- * Reference: Section 1.1 Switch datasheet-3
- *
- * This is optional settings may be needed on some boards
- * for PHY<->magnetics h/w tuning
- */
-static int mv88361xx_reverse_mdipn(struct mv88e61xx_config *swconfig, u32 phy)
+static int mv88e61xx_port_enable(struct phy_device *phydev, u8 port)
 {
-	char *name = swconfig->name;
+	int val;
+
+	val = mv88e61xx_port_read(phydev, port, PORT_REG_CTRL);
+	if (val < 0)
+		return val;
+	val = bitfield_replace(val, PORT_REG_CTRL_PSTATE_SHIFT,
+			       PORT_REG_CTRL_PSTATE_WIDTH,
+			       PORT_REG_CTRL_PSTATE_FORWARD);
+	val = mv88e61xx_port_write(phydev, port, PORT_REG_CTRL, val);
+	if (val < 0)
+		return val;
 
-	if (swconfig->mdip != MV88E61XX_MDIP_REVERSE)
-		return 0;
+	return 0;
+}
 
-	/*Reverse MDIP/N[3:0] bits */
-	if (WR_SWITCH_PHY_REG(name, phy, 0x14, 0x000f))
-		return -1;
+static int mv88e61xx_port_set_vlan(struct phy_device *phydev, u8 port,
+							u8 mask)
+{
+	int val;
+
+	/* Set VID to port number plus one */
+	val = mv88e61xx_port_read(phydev, port, PORT_REG_VLAN_ID);
+	if (val < 0)
+		return val;
+	val = bitfield_replace(val, PORT_REG_VLAN_ID_DEF_VID_SHIFT,
+			       PORT_REG_VLAN_ID_DEF_VID_WIDTH,
+			       port + 1);
+	val = mv88e61xx_port_write(phydev, port, PORT_REG_VLAN_ID, val);
+	if (val < 0)
+		return val;
+
+	/* Set VID mask */
+	val = mv88e61xx_port_read(phydev, port, PORT_REG_VLAN_MAP);
+	if (val < 0)
+		return val;
+	val = bitfield_replace(val, PORT_REG_VLAN_MAP_TABLE_SHIFT,
+			       PORT_REG_VLAN_MAP_TABLE_WIDTH,
+			       mask);
+	val = mv88e61xx_port_write(phydev, port, PORT_REG_VLAN_MAP, val);
+	if (val < 0)
+		return val;
 
 	return 0;
 }
 
-/*
- * Marvell 88E61XX Switch initialization
- */
-int mv88e61xx_switch_initialize(struct mv88e61xx_config *swconfig)
+static int mv88e61xx_read_port_config(struct phy_device *phydev, u8 port)
 {
-	u32 prt;
-	u16 reg;
-	char *idstr;
-	char *name = swconfig->name;
-	int time;
-
-	if (miiphy_set_current_dev(name)) {
-		printf("%s failed\n", __FUNCTION__);
-		return -1;
+	int res;
+	int val;
+	bool forced = false;
+
+	val = mv88e61xx_port_read(phydev, port, PORT_REG_STATUS);
+	if (val < 0)
+		return val;
+	if (!(val & PORT_REG_STATUS_LINK)) {
+		/* Temporarily force link to read port configuration */
+		u32 timeout = 100;
+		forced = true;
+
+		val = mv88e61xx_port_read(phydev, port, PORT_REG_PHYS_CTRL);
+		if (val < 0)
+			return val;
+		val |= (PORT_REG_PHYS_CTRL_LINK_FORCE |
+				PORT_REG_PHYS_CTRL_LINK_VALUE);
+		val = mv88e61xx_port_write(phydev, port, PORT_REG_PHYS_CTRL,
+					   val);
+		if (val < 0)
+			return val;
+
+		/* Wait for status register to reflect forced link */
+		do {
+			val = mv88e61xx_port_read(phydev, port,
+						  PORT_REG_STATUS);
+			if (val < 0)
+				goto unforce;
+			if (val & PORT_REG_STATUS_LINK)
+				break;
+		} while (--timeout);
+
+		if (timeout == 0) {
+			res = -ETIMEDOUT;
+			goto unforce;
+		}
 	}
 
-	if (!(swconfig->cpuport & ((1 << 4) | (1 << 5)))) {
-		swconfig->cpuport = (1 << 5);
-		printf("Invalid cpu port config, using default port5\n");
-	}
+	if (val & PORT_REG_STATUS_DUPLEX)
+		phydev->duplex = DUPLEX_FULL;
+	else
+		phydev->duplex = DUPLEX_HALF;
 
-	RD_SWITCH_PORT_REG(name, 0, MII_PHYSID2, &reg);
-	switch (reg &= 0xfff0) {
-	case 0x1610:
-		idstr = "88E6161";
-		break;
-	case 0x1650:
-		idstr = "88E6165";
+	val = bitfield_extract(val, PORT_REG_STATUS_SPEED_SHIFT,
+			       PORT_REG_STATUS_SPEED_WIDTH);
+	switch (val) {
+	case PORT_REG_STATUS_SPEED_1000:
+		phydev->speed = SPEED_1000;
 		break;
-	case 0x1210:
-		idstr = "88E6123";
-		/* ports 2,3,4 not available */
-		swconfig->ports_enabled &= 0x023;
+	case PORT_REG_STATUS_SPEED_100:
+		phydev->speed = SPEED_100;
 		break;
 	default:
-		/* Could not detect switch id */
-		idstr = "88E61??";
+		phydev->speed = SPEED_10;
 		break;
 	}
 
-	/* be sure all ports are disabled */
-	for (prt = 0; prt < MV88E61XX_MAX_PORTS_NUM; prt++) {
-		RD_SWITCH_PORT_REG(name, prt, MV88E61XX_PRT_CTRL_REG, &reg);
-		reg &= ~0x3;
-		WR_SWITCH_PORT_REG(name, prt, MV88E61XX_PRT_CTRL_REG, reg);
+	res = 0;
+
+unforce:
+	if (forced) {
+		val = mv88e61xx_port_read(phydev, port, PORT_REG_PHYS_CTRL);
+		if (val < 0)
+			return val;
+		val &= ~(PORT_REG_PHYS_CTRL_LINK_FORCE |
+				PORT_REG_PHYS_CTRL_LINK_VALUE);
+		val = mv88e61xx_port_write(phydev, port, PORT_REG_PHYS_CTRL,
+					   val);
+		if (val < 0)
+			return val;
 	}
 
-	/* wait 2 ms for queues to drain */
-	udelay(2000);
-
-	/* reset switch */
-	RD_SWITCH_REG(name, MV88E61XX_GLBREG_DEVADR, MV88E61XX_SGCR, &reg);
-	reg |= 0x8000;
-	WR_SWITCH_REG(name, MV88E61XX_GLBREG_DEVADR, MV88E61XX_SGCR, reg);
+	return res;
+}
 
-	/* wait up to 1 second for switch reset complete */
-	for (time = 1000; time; time--) {
-		RD_SWITCH_REG(name, MV88E61XX_GLBREG_DEVADR, MV88E61XX_SGSR,
-			&reg);
-		if ((reg & 0xc800) == 0xc800)
-			break;
-		udelay(1000);
-	}
-	if (!time)
-		return -1;
-
-	/* Port based VLANs configuration */
-	mv88e61xx_port_vlan_config(swconfig);
-
-	if (swconfig->rgmii_delay == MV88E61XX_RGMII_DELAY_EN) {
-		/*
-		 * Enable RGMII delay on Tx and Rx for CPU port
-		 * Ref: sec 9.5 of chip datasheet-02
-		 */
-		/*Force port link down */
-		WR_SWITCH_PORT_REG(name, 5, MV88E61XX_PCS_CTRL_REG, 0x10);
-		/* configure port RGMII delay */
-		WR_SWITCH_PORT_REG(name, 4,
-			MV88E61XX_RGMII_TIMECTRL_REG, 0x81e7);
-		RD_SWITCH_PORT_REG(name, 5,
-			MV88E61XX_RGMII_TIMECTRL_REG, &reg);
-		WR_SWITCH_PORT_REG(name, 5,
-			MV88E61XX_RGMII_TIMECTRL_REG, reg | 0x18);
-		WR_SWITCH_PORT_REG(name, 4,
-			MV88E61XX_RGMII_TIMECTRL_REG, 0xc1e7);
-		/* Force port to RGMII FDX 1000Base then up */
-		WR_SWITCH_PORT_REG(name, 5, MV88E61XX_PCS_CTRL_REG, 0x1e);
-		WR_SWITCH_PORT_REG(name, 5, MV88E61XX_PCS_CTRL_REG, 0x3e);
+static int mv88e61xx_set_cpu_port(struct phy_device *phydev)
+{
+	int val;
+
+	/* Set CPUDest */
+	val = mv88e61xx_reg_read(phydev, DEVADDR_GLOBAL_1, GLOBAL1_MON_CTRL);
+	if (val < 0)
+		return val;
+	val = bitfield_replace(val, GLOBAL1_MON_CTRL_CPUDEST_SHIFT,
+			       GLOBAL1_MON_CTRL_CPUDEST_WIDTH,
+			       CONFIG_MV88E61XX_CPU_PORT);
+	val = mv88e61xx_reg_write(phydev, DEVADDR_GLOBAL_1,
+				     GLOBAL1_MON_CTRL, val);
+	if (val < 0)
+		return val;
+
+	/* Allow CPU to route to any port */
+	val = PORT_MASK & ~(1 << CONFIG_MV88E61XX_CPU_PORT);
+	val = mv88e61xx_port_set_vlan(phydev, CONFIG_MV88E61XX_CPU_PORT, val);
+	if (val < 0)
+		return val;
+
+	/* Enable CPU port */
+	val = mv88e61xx_port_enable(phydev, CONFIG_MV88E61XX_CPU_PORT);
+	if (val < 0)
+		return val;
+
+	val = mv88e61xx_read_port_config(phydev, CONFIG_MV88E61XX_CPU_PORT);
+	if (val < 0)
+		return val;
+
+	/* If CPU is connected to serdes, initialize serdes */
+	if (mv88e61xx_6352_family(phydev)) {
+		val = mv88e61xx_get_cmode(phydev, CONFIG_MV88E61XX_CPU_PORT);
+		if (val < 0)
+			return val;
+		if (val == PORT_REG_STATUS_CMODE_100BASE_X ||
+		    val == PORT_REG_STATUS_CMODE_1000BASE_X ||
+		    val == PORT_REG_STATUS_CMODE_SGMII) {
+			val = mv88e61xx_serdes_init(phydev);
+			if (val < 0)
+				return val;
+		}
 	}
 
-	for (prt = 0; prt < MV88E61XX_MAX_PORTS_NUM; prt++) {
-
-		/* configure port's PHY */
-		if (!((1 << prt) & swconfig->cpuport)) {
-			/* port 4 has phy 6, not 4 */
-			int phy = (prt == 4) ? 6 : prt;
-			if (mv88361xx_powerup(swconfig, phy))
-				return -1;
-			if (mv88361xx_reverse_mdipn(swconfig, phy))
-				return -1;
-			if (mv88361xx_led_init(swconfig, phy))
-				return -1;
-		}
+	return 0;
+}
 
-		/* set port VID to port+1 except for cpu port */
-		if (!((1 << prt) & swconfig->cpuport)) {
-			RD_SWITCH_PORT_REG(name, prt,
-				MV88E61XX_PRT_VID_REG, &reg);
-			WR_SWITCH_PORT_REG(name, prt,
-				MV88E61XX_PRT_VID_REG,
-				(reg & ~1023) | (prt+1));
-		}
+static int mv88e61xx_switch_init(struct phy_device *phydev)
+{
+	static int init;
+	int res;
 
-		/*Program port state */
-		RD_SWITCH_PORT_REG(name, prt,
-			MV88E61XX_PRT_CTRL_REG, &reg);
-		WR_SWITCH_PORT_REG(name, prt,
-			MV88E61XX_PRT_CTRL_REG,
-			reg | (swconfig->portstate & 0x03));
+	if (init)
+		return 0;
 
-	}
+	res = mv88e61xx_switch_reset(phydev);
+	if (res < 0)
+		return res;
+
+	res = mv88e61xx_set_cpu_port(phydev);
+	if (res < 0)
+		return res;
+
+	init = 1;
 
-	printf("%s Initialized on %s\n", idstr, name);
 	return 0;
 }
 
-#ifdef CONFIG_MV88E61XX_CMD
-static int
-do_switch(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
+static int mv88e61xx_phy_enable(struct phy_device *phydev, u8 phy)
 {
-	char *name, *endp;
-	int write = 0;
-	enum { dev, prt, phy } target = dev;
-	u32 addrlo, addrhi, addr;
-	u32 reglo, reghi, reg;
-	u16 data, rdata;
+	int val;
 
-	if (argc < 7)
-		return -1;
+	val = mv88e61xx_phy_read(phydev, phy, MII_BMCR);
+	if (val < 0)
+		return val;
+	val &= ~(BMCR_PDOWN);
+	val = mv88e61xx_phy_write(phydev, phy, MII_BMCR, val);
+	if (val < 0)
+		return val;
 
-	name = argv[1];
+	return 0;
+}
 
-	if (strcmp(argv[2], "phy") == 0)
-		target = phy;
-	else if (strcmp(argv[2], "port") == 0)
-		target = prt;
-	else if (strcmp(argv[2], "dev") != 0)
-		return 1;
+static int mv88e61xx_phy_setup(struct phy_device *phydev, u8 phy)
+{
+	int val;
 
-	addrlo = simple_strtoul(argv[3], &endp, 16);
+	/*
+	 * Enable energy-detect sensing on PHY, used to determine when a PHY
+	 * port is physically connected
+	 */
+	val = mv88e61xx_phy_read(phydev, phy, PHY_REG_CTRL1);
+	if (val < 0)
+		return val;
+	val = bitfield_replace(val, PHY_REG_CTRL1_ENERGY_DET_SHIFT,
+			       PHY_REG_CTRL1_ENERGY_DET_WIDTH,
+			       PHY_REG_CTRL1_ENERGY_DET_SENSE_XMIT);
+	val = mv88e61xx_phy_write(phydev, phy, PHY_REG_CTRL1, val);
+	if (val < 0)
+		return val;
 
-	if (!*endp) {
-		addrhi = addrlo;
-	} else {
-		while (*endp < '0' || *endp > '9')
-			endp++;
-		addrhi = simple_strtoul(endp, NULL, 16);
-	}
+	return 0;
+}
 
-	reglo = simple_strtoul(argv[5], &endp, 16);
-	if (!*endp) {
-		reghi = reglo;
-	} else {
-		while (*endp < '0' || *endp > '9')
-			endp++;
-		reghi = simple_strtoul(endp, NULL, 16);
+static int mv88e61xx_phy_config_port(struct phy_device *phydev, u8 phy)
+{
+	int val;
+
+	val = mv88e61xx_port_enable(phydev, phy);
+	if (val < 0)
+		return val;
+
+	val = mv88e61xx_port_set_vlan(phydev, phy,
+			1 << CONFIG_MV88E61XX_CPU_PORT);
+	if (val < 0)
+		return val;
+
+	return 0;
+}
+
+static int mv88e61xx_probe(struct phy_device *phydev)
+{
+	struct mii_dev *smi_wrapper;
+	struct mv88e61xx_phy_priv *priv;
+	int res;
+
+	res = mv88e61xx_hw_reset(phydev);
+	if (res < 0)
+		return res;
+
+	priv = malloc(sizeof(*priv));
+	if (!priv)
+		return -ENOMEM;
+
+	memset(priv, 0, sizeof(*priv));
+
+	/*
+	 * This device requires indirect reads/writes to the PHY registers
+	 * which the generic PHY code can't handle.  Make a wrapper MII device
+	 * to handle reads/writes
+	 */
+	smi_wrapper = mdio_alloc();
+	if (!smi_wrapper) {
+		free(priv);
+		return -ENOMEM;
 	}
 
-	if (strcmp(argv[6], "write") == 0)
-		write = 1;
-	else if (strcmp(argv[6], "read") != 0)
-		return 1;
-
-	data = simple_strtoul(argv[7], NULL, 16);
-
-	for (addr = addrlo; addr <= addrhi; addr++) {
-		for (reg = reglo; reg <= reghi; reg++) {
-			if (write) {
-				if (target == phy)
-					mv88e61xx_switch_miiphy_write(
-						name, addr, reg, data);
-				else if (target == prt)
-					wr_switch_reg(name,
-						addr+MV88E61XX_PRT_OFST,
-						reg, data);
-				else
-					wr_switch_reg(name, addr, reg, data);
-			} else {
-				if (target == phy)
-					mv88e61xx_switch_miiphy_read(
-						name, addr, reg, &rdata);
-				else if (target == prt)
-					rd_switch_reg(name,
-						addr+MV88E61XX_PRT_OFST,
-						reg, &rdata);
-				else
-					rd_switch_reg(name, addr, reg, &rdata);
-				printf("%s %s %s %02x %s %02x %s %04x\n",
-					argv[0], argv[1], argv[2], addr,
-					argv[4], reg, argv[6], rdata);
-				if (write && argc == 7 && rdata != data)
-					return 1;
+	/*
+	 * Store the mdio bus in the private data, as we are going to replace
+	 * the bus with the wrapper bus
+	 */
+	priv->mdio_bus = phydev->bus;
+
+	/*
+	 * Store the smi bus address in private data.  This lets us use the
+	 * phydev addr field for device address instead, as the genphy code
+	 * expects.
+	 */
+	priv->smi_addr = phydev->addr;
+
+	/*
+	 * Store the phy_device in the wrapper mii device. This lets us get it
+	 * back when genphy functions call phy_read/phy_write.
+	 */
+	smi_wrapper->priv = phydev;
+	strncpy(smi_wrapper->name, "indirect mii", sizeof(smi_wrapper->name));
+	smi_wrapper->read = mv88e61xx_phy_read_indirect;
+	smi_wrapper->write = mv88e61xx_phy_write_indirect;
+
+	/* Replace the bus with the wrapper device */
+	phydev->bus = smi_wrapper;
+
+	phydev->priv = priv;
+
+	priv->id = mv88e61xx_get_switch_id(phydev);
+
+	return 0;
+}
+
+static int mv88e61xx_phy_config(struct phy_device *phydev)
+{
+	int res;
+	int i;
+	int ret = -1;
+
+	res = mv88e61xx_switch_init(phydev);
+	if (res < 0)
+		return res;
+
+	for (i = 0; i < PORT_COUNT; i++) {
+		if ((1 << i) & CONFIG_MV88E61XX_PHY_PORTS) {
+			phydev->addr = i;
+
+			res = mv88e61xx_phy_enable(phydev, i);
+			if (res < 0) {
+				printf("Error enabling PHY %i\n", i);
+				continue;
+			}
+			res = mv88e61xx_phy_setup(phydev, i);
+			if (res < 0) {
+				printf("Error setting up PHY %i\n", i);
+				continue;
 			}
+			res = mv88e61xx_phy_config_port(phydev, i);
+			if (res < 0) {
+				printf("Error configuring PHY %i\n", i);
+				continue;
+			}
+
+			res = genphy_config_aneg(phydev);
+			if (res < 0) {
+				printf("Error setting PHY %i autoneg\n", i);
+				continue;
+			}
+			res = phy_reset(phydev);
+			if (res < 0) {
+				printf("Error resetting PHY %i\n", i);
+				continue;
+			}
+
+			/* Return success if any PHY succeeds */
+			ret = 0;
 		}
 	}
+
+	return ret;
+}
+
+static int mv88e61xx_phy_is_connected(struct phy_device *phydev)
+{
+	int val;
+
+	val = mv88e61xx_phy_read(phydev, phydev->addr, PHY_REG_STATUS1);
+	if (val < 0)
+		return 0;
+
+	/*
+	 * After reset, the energy detect signal remains high for a few seconds
+	 * regardless of whether a cable is connected.  This function will
+	 * return false positives during this time.
+	 */
+	return (val & PHY_REG_STATUS1_ENERGY) == 0;
+}
+
+static int mv88e61xx_phy_startup(struct phy_device *phydev)
+{
+	int i;
+	int link = 0;
+	int res;
+	int speed = phydev->speed;
+	int duplex = phydev->duplex;
+
+	for (i = 0; i < PORT_COUNT; i++) {
+		if ((1 << i) & CONFIG_MV88E61XX_PHY_PORTS) {
+			phydev->addr = i;
+			if (!mv88e61xx_phy_is_connected(phydev))
+				continue;
+			res = genphy_update_link(phydev);
+			if (res < 0)
+				continue;
+			res = mv88e61xx_parse_status(phydev);
+			if (res < 0)
+				continue;
+			link = (link || phydev->link);
+		}
+	}
+	phydev->link = link;
+
+	/* Restore CPU interface speed and duplex after it was changed for
+	 * other ports */
+	phydev->speed = speed;
+	phydev->duplex = duplex;
+
+	return 0;
+}
+
+static struct phy_driver mv88e61xx_driver = {
+	.name = "Marvell MV88E61xx",
+	.uid = 0x01410eb1,
+	.mask = 0xfffffff0,
+	.features = PHY_GBIT_FEATURES,
+	.probe = mv88e61xx_probe,
+	.config = mv88e61xx_phy_config,
+	.startup = mv88e61xx_phy_startup,
+	.shutdown = &genphy_shutdown,
+};
+
+int phy_mv88e61xx_init(void)
+{
+	phy_register(&mv88e61xx_driver);
+
 	return 0;
 }
 
-U_BOOT_CMD(mv88e61xx, 8, 0, do_switch,
-	"Read or write mv88e61xx switch registers",
-	"<ethdevice> dev|port|phy <addr> reg <reg> write <data>\n"
-	"<ethdevice> dev|port|phy <addr> reg <reg> read [<data>]\n"
-	"    - read/write switch device, port or phy at (addr,reg)\n"
-	"      addr=0..0x1C for dev, 0..5 for port or phy.\n"
-	"      reg=0..0x1F.\n"
-	"      data=0..0xFFFF (tested if present against actual read).\n"
-	"      All numeric parameters are assumed to be hex.\n"
-	"      <addr> and <<reg> arguments can be ranges (x..y)"
-);
-#endif /* CONFIG_MV88E61XX_CMD */
+/*
+ * Overload weak get_phy_id definition since we need non-standard functions
+ * to read PHY registers
+ */
+int get_phy_id(struct mii_dev *bus, int smi_addr, int devad, u32 *phy_id)
+{
+	struct phy_device temp_phy;
+	struct mv88e61xx_phy_priv temp_priv;
+	struct mii_dev temp_mii;
+	int val;
+
+	/*
+	 * Buid temporary data structures that the chip reading code needs to
+	 * read the ID
+	 */
+	temp_priv.mdio_bus = bus;
+	temp_priv.smi_addr = smi_addr;
+	temp_phy.priv = &temp_priv;
+	temp_mii.priv = &temp_phy;
+
+	val = mv88e61xx_phy_read_indirect(&temp_mii, 0, devad, MII_PHYSID1);
+	if (val < 0)
+		return -EIO;
+
+	*phy_id = val << 16;
+
+	val = mv88e61xx_phy_read_indirect(&temp_mii, 0, devad, MII_PHYSID2);
+	if (val < 0)
+		return -EIO;
+
+	*phy_id |= (val & 0xffff);
+
+	return 0;
+}
diff --git a/drivers/net/phy/mv88e61xx.h b/drivers/net/phy/mv88e61xx.h
deleted file mode 100644
index 9c62e4a..0000000
--- a/drivers/net/phy/mv88e61xx.h
+++ /dev/null
@@ -1,61 +0,0 @@
-/*
- * (C) Copyright 2009
- * Marvell Semiconductor <www.marvell.com>
- * Prafulla Wadaskar <prafulla@marvell.com>
- *
- * SPDX-License-Identifier:	GPL-2.0+
- */
-
-#ifndef _MV88E61XX_H
-#define _MV88E61XX_H
-
-#include <miiphy.h>
-
-#define MV88E61XX_CPU_PORT		0x5
-
-#define MV88E61XX_PHY_TIMEOUT		100000
-
-/* port dev-addr (= port + 0x10) */
-#define MV88E61XX_PRT_OFST		0x10
-/* port registers */
-#define MV88E61XX_PCS_CTRL_REG		0x1
-#define MV88E61XX_PRT_CTRL_REG		0x4
-#define MV88E61XX_PRT_VMAP_REG		0x6
-#define MV88E61XX_PRT_VID_REG		0x7
-#define MV88E61XX_RGMII_TIMECTRL_REG	0x1A
-
-/* global registers dev-addr */
-#define MV88E61XX_GLBREG_DEVADR	0x1B
-/* global registers */
-#define MV88E61XX_SGSR			0x00
-#define MV88E61XX_SGCR			0x04
-
-/* global 2 registers dev-addr */
-#define MV88E61XX_GLB2REG_DEVADR	0x1C
-/* global 2 registers */
-#define MV88E61XX_PHY_CMD		0x18
-#define MV88E61XX_PHY_DATA		0x19
-/* global 2 phy commands */
-#define MV88E61XX_PHY_WRITE_CMD		0x9400
-#define MV88E61XX_PHY_READ_CMD		0x9800
-
-#define MV88E61XX_BUSY_OFST		15
-#define MV88E61XX_MODE_OFST		12
-#define MV88E61XX_OP_OFST		10
-#define MV88E61XX_ADDR_OFST		5
-
-#ifdef CONFIG_MV88E61XX_MULTICHIP_ADRMODE
-static int mv88e61xx_busychk_multic(char *name, u32 devaddr);
-static void mv88e61xx_switch_write(char *name, u32 phy_adr,
-	u32 reg_ofs, u16 data);
-static void mv88e61xx_switch_read(char *name, u32 phy_adr,
-	u32 reg_ofs, u16 *data);
-#define wr_switch_reg mv88e61xx_switch_write
-#define rd_switch_reg mv88e61xx_switch_read
-#else
-/* switch appears a s simple PHY and can thus use miiphy */
-#define wr_switch_reg miiphy_write
-#define rd_switch_reg miiphy_read
-#endif /* CONFIG_MV88E61XX_MULTICHIP_ADRMODE */
-
-#endif /* _MV88E61XX_H */
diff --git a/drivers/net/phy/natsemi.c b/drivers/net/phy/natsemi.c
index d2e4c3c..1592e9b 100644
--- a/drivers/net/phy/natsemi.c
+++ b/drivers/net/phy/natsemi.c
@@ -93,10 +93,13 @@ static int dp83865_parse_status(struct phy_device *phydev)
 
 static int dp83865_startup(struct phy_device *phydev)
 {
-	genphy_update_link(phydev);
-	dp83865_parse_status(phydev);
+	int ret;
 
-	return 0;
+	ret = genphy_update_link(phydev);
+	if (ret)
+		return ret;
+
+	return dp83865_parse_status(phydev);
 }
 
 
@@ -134,10 +137,13 @@ static int dp83848_parse_status(struct phy_device *phydev)
 
 static int dp83848_startup(struct phy_device *phydev)
 {
-	genphy_update_link(phydev);
-	dp83848_parse_status(phydev);
+	int ret;
 
-	return 0;
+	ret = genphy_update_link(phydev);
+	if (ret)
+		return ret;
+
+	return dp83848_parse_status(phydev);
 }
 
 static struct phy_driver DP83848_driver = {
diff --git a/drivers/net/phy/phy.c b/drivers/net/phy/phy.c
index 23c82bb..80bdfb6 100644
--- a/drivers/net/phy/phy.c
+++ b/drivers/net/phy/phy.c
@@ -248,7 +248,7 @@ int genphy_update_link(struct phy_device *phydev)
 			if (i > PHY_ANEG_TIMEOUT) {
 				printf(" TIMEOUT !\n");
 				phydev->link = 0;
-				return 0;
+				return -ETIMEDOUT;
 			}
 
 			if (ctrlc()) {
@@ -431,10 +431,13 @@ int genphy_config(struct phy_device *phydev)
 
 int genphy_startup(struct phy_device *phydev)
 {
-	genphy_update_link(phydev);
-	genphy_parse_link(phydev);
+	int ret;
 
-	return 0;
+	ret = genphy_update_link(phydev);
+	if (ret)
+		return ret;
+
+	return genphy_parse_link(phydev);
 }
 
 int genphy_shutdown(struct phy_device *phydev)
@@ -458,6 +461,9 @@ static LIST_HEAD(phy_drivers);
 
 int phy_init(void)
 {
+#ifdef CONFIG_MV88E61XX_SWITCH
+	phy_mv88e61xx_init();
+#endif
 #ifdef CONFIG_PHY_AQUANTIA
 	phy_aquantia_init();
 #endif
@@ -876,9 +882,7 @@ __weak int board_phy_config(struct phy_device *phydev)
 int phy_config(struct phy_device *phydev)
 {
 	/* Invoke an optional board-specific helper */
-	board_phy_config(phydev);
-
-	return 0;
+	return board_phy_config(phydev);
 }
 
 int phy_shutdown(struct phy_device *phydev)
diff --git a/drivers/net/phy/realtek.c b/drivers/net/phy/realtek.c
index 9d7f55b..7a99cb0 100644
--- a/drivers/net/phy/realtek.c
+++ b/drivers/net/phy/realtek.c
@@ -208,28 +208,38 @@ static int rtl8211f_parse_status(struct phy_device *phydev)
 
 static int rtl8211x_startup(struct phy_device *phydev)
 {
+	int ret;
+
 	/* Read the Status (2x to make sure link is right) */
-	genphy_update_link(phydev);
-	rtl8211x_parse_status(phydev);
+	ret = genphy_update_link(phydev);
+	if (ret)
+		return ret;
 
-	return 0;
+	return rtl8211x_parse_status(phydev);
 }
 
 static int rtl8211e_startup(struct phy_device *phydev)
 {
-	genphy_update_link(phydev);
-	genphy_parse_link(phydev);
+	int ret;
 
-	return 0;
+	ret = genphy_update_link(phydev);
+	if (ret)
+		return ret;
+
+	return genphy_parse_link(phydev);
 }
 
 static int rtl8211f_startup(struct phy_device *phydev)
 {
+	int ret;
+
+	/* Read the Status (2x to make sure link is right) */
+	ret = genphy_update_link(phydev);
+	if (ret)
+		return ret;
 	/* Read the Status (2x to make sure link is right) */
-	genphy_update_link(phydev);
-	rtl8211f_parse_status(phydev);
 
-	return 0;
+	return rtl8211f_parse_status(phydev);
 }
 
 /* Support for RTL8211B PHY */
diff --git a/drivers/net/phy/smsc.c b/drivers/net/phy/smsc.c
index 34986a2..313fcdf 100644
--- a/drivers/net/phy/smsc.c
+++ b/drivers/net/phy/smsc.c
@@ -34,9 +34,13 @@ static int smsc_parse_status(struct phy_device *phydev)
 
 static int smsc_startup(struct phy_device *phydev)
 {
-	genphy_update_link(phydev);
-	smsc_parse_status(phydev);
-	return 0;
+	int ret;
+
+	ret = genphy_update_link(phydev);
+	if (ret)
+		return ret;
+
+	return smsc_parse_status(phydev);
 }
 
 static struct phy_driver lan8700_driver = {
diff --git a/drivers/net/phy/ti.c b/drivers/net/phy/ti.c
index 937426b..c55dd97 100644
--- a/drivers/net/phy/ti.c
+++ b/drivers/net/phy/ti.c
@@ -6,6 +6,14 @@
  */
 #include <common.h>
 #include <phy.h>
+#include <linux/compat.h>
+#include <malloc.h>
+
+#include <fdtdec.h>
+#include <dm.h>
+#include <dt-bindings/net/ti-dp83867.h>
+
+DECLARE_GLOBAL_DATA_PTR;
 
 /* TI DP83867 */
 #define DP83867_DEVADDR		0x1f
@@ -71,6 +79,17 @@
 #define MII_MMD_CTRL_INCR_RDWT	0x8000 /* post increment on reads & writes */
 #define MII_MMD_CTRL_INCR_ON_WT	0xC000 /* post increment on writes only */
 
+/* User setting - can be taken from DTS */
+#define DEFAULT_RX_ID_DELAY	DP83867_RGMIIDCTL_2_25_NS
+#define DEFAULT_TX_ID_DELAY	DP83867_RGMIIDCTL_2_75_NS
+#define DEFAULT_FIFO_DEPTH	DP83867_PHYCR_FIFO_DEPTH_4_B_NIB
+
+struct dp83867_private {
+	int rx_id_delay;
+	int tx_id_delay;
+	int fifo_depth;
+};
+
 /**
  * phy_read_mmd_indirect - reads data from the MMD registers
  * @phydev: The PHY device bus
@@ -137,27 +156,60 @@ void phy_write_mmd_indirect(struct phy_device *phydev, int prtad,
 	phy_write(phydev, addr, MII_MMD_DATA, data);
 }
 
+#if defined(CONFIG_DM_ETH)
 /**
- * phy_interface_is_rgmii - Convenience function for testing if a PHY interface
- * is RGMII (all variants)
+ * dp83867_data_init - Convenience function for setting PHY specific data
+ *
  * @phydev: the phy_device struct
  */
-static inline bool phy_interface_is_rgmii(struct phy_device *phydev)
+static int dp83867_of_init(struct phy_device *phydev)
 {
-	return phydev->interface >= PHY_INTERFACE_MODE_RGMII &&
-		phydev->interface <= PHY_INTERFACE_MODE_RGMII_TXID;
+	struct dp83867_private *dp83867 = phydev->priv;
+	struct udevice *dev = phydev->dev;
+
+	dp83867->rx_id_delay = fdtdec_get_uint(gd->fdt_blob, dev->of_offset,
+				 "ti,rx-internal-delay", -1);
+
+	dp83867->tx_id_delay = fdtdec_get_uint(gd->fdt_blob, dev->of_offset,
+				 "ti,tx-internal-delay", -1);
+
+	dp83867->fifo_depth = fdtdec_get_uint(gd->fdt_blob, dev->of_offset,
+				 "ti,fifo-depth", -1);
+
+	return 0;
 }
+#else
+static int dp83867_of_init(struct phy_device *phydev)
+{
+	struct dp83867_private *dp83867 = phydev->priv;
 
-/* User setting - can be taken from DTS */
-#define RX_ID_DELAY	8
-#define TX_ID_DELAY	0xa
-#define FIFO_DEPTH	1
+	dp83867->rx_id_delay = DEFAULT_RX_ID_DELAY;
+	dp83867->tx_id_delay = DEFAULT_TX_ID_DELAY;
+	dp83867->fifo_depth = DEFAULT_FIFO_DEPTH;
+
+	return 0;
+}
+#endif
 
 static int dp83867_config(struct phy_device *phydev)
 {
+	struct dp83867_private *dp83867;
 	unsigned int val, delay, cfg2;
 	int ret;
 
+	if (!phydev->priv) {
+		dp83867 = kzalloc(sizeof(*dp83867), GFP_KERNEL);
+		if (!dp83867)
+			return -ENOMEM;
+
+		phydev->priv = dp83867;
+		ret = dp83867_of_init(phydev);
+		if (ret)
+			goto err_out;
+	} else {
+		dp83867 = (struct dp83867_private *)phydev->priv;
+	}
+
 	/* Restart the PHY.  */
 	val = phy_read(phydev, MDIO_DEVAD_NONE, DP83867_CTRL);
 	phy_write(phydev, MDIO_DEVAD_NONE, DP83867_CTRL,
@@ -166,10 +218,10 @@ static int dp83867_config(struct phy_device *phydev)
 	if (phy_interface_is_rgmii(phydev)) {
 		ret = phy_write(phydev, MDIO_DEVAD_NONE, MII_DP83867_PHYCTRL,
 			(DP83867_MDI_CROSSOVER_AUTO << DP83867_MDI_CROSSOVER) |
-			(FIFO_DEPTH << DP83867_PHYCR_FIFO_DEPTH_SHIFT));
+			(dp83867->fifo_depth << DP83867_PHYCR_FIFO_DEPTH_SHIFT));
 		if (ret)
-			return ret;
-	} else {
+			goto err_out;
+	} else if (phy_interface_is_sgmii(phydev)) {
 		phy_write(phydev, MDIO_DEVAD_NONE, MII_BMCR,
 			  (BMCR_ANENABLE | BMCR_FULLDPLX | BMCR_SPEED1000));
 
@@ -189,8 +241,8 @@ static int dp83867_config(struct phy_device *phydev)
 			  DP83867_PHYCTRL_SGMIIEN |
 			  (DP83867_MDI_CROSSOVER_MDIX <<
 			  DP83867_MDI_CROSSOVER) |
-			  (FIFO_DEPTH << DP83867_PHYCTRL_RXFIFO_SHIFT) |
-			  (FIFO_DEPTH  << DP83867_PHYCTRL_TXFIFO_SHIFT));
+			  (dp83867->fifo_depth << DP83867_PHYCTRL_RXFIFO_SHIFT) |
+			  (dp83867->fifo_depth << DP83867_PHYCTRL_TXFIFO_SHIFT));
 		phy_write(phydev, MDIO_DEVAD_NONE, MII_DP83867_BISCR, 0x0);
 	}
 
@@ -212,8 +264,8 @@ static int dp83867_config(struct phy_device *phydev)
 		phy_write_mmd_indirect(phydev, DP83867_RGMIICTL,
 				       DP83867_DEVADDR, phydev->addr, val);
 
-		delay = (RX_ID_DELAY |
-			 (TX_ID_DELAY << DP83867_RGMII_TX_CLK_DELAY_SHIFT));
+		delay = (dp83867->rx_id_delay |
+			 (dp83867->tx_id_delay << DP83867_RGMII_TX_CLK_DELAY_SHIFT));
 
 		phy_write_mmd_indirect(phydev, DP83867_RGMIIDCTL,
 				       DP83867_DEVADDR, phydev->addr, delay);
@@ -221,6 +273,10 @@ static int dp83867_config(struct phy_device *phydev)
 
 	genphy_config_aneg(phydev);
 	return 0;
+
+err_out:
+	kfree(dp83867);
+	return ret;
 }
 
 static struct phy_driver DP83867_driver = {
diff --git a/drivers/net/phy/vitesse.c b/drivers/net/phy/vitesse.c
index 941d076..2635b82 100644
--- a/drivers/net/phy/vitesse.c
+++ b/drivers/net/phy/vitesse.c
@@ -112,10 +112,12 @@ static int vitesse_parse_status(struct phy_device *phydev)
 
 static int vitesse_startup(struct phy_device *phydev)
 {
-	genphy_update_link(phydev);
-	vitesse_parse_status(phydev);
+	int ret;
 
-	return 0;
+	ret = genphy_update_link(phydev);
+	if (ret)
+		return ret;
+	return vitesse_parse_status(phydev);
 }
 
 static int cis8204_config(struct phy_device *phydev)
diff --git a/drivers/net/xilinx_emaclite.c b/drivers/net/xilinx_emaclite.c
index 5862bf0..7b85aa0 100644
--- a/drivers/net/xilinx_emaclite.c
+++ b/drivers/net/xilinx_emaclite.c
@@ -250,7 +250,7 @@ static void emaclite_stop(struct udevice *dev)
 
 static int setup_phy(struct udevice *dev)
 {
-	int i;
+	int i, ret;
 	u16 phyreg;
 	struct xemaclite *emaclite = dev_get_priv(dev);
 	struct phy_device *phydev;
@@ -302,7 +302,9 @@ static int setup_phy(struct udevice *dev)
 	phydev->advertising = supported;
 	emaclite->phydev = phydev;
 	phy_config(phydev);
-	phy_startup(phydev);
+	ret = phy_startup(phydev);
+	if (ret)
+		return ret;
 
 	if (!phydev->link) {
 		printf("%s: No link.\n", phydev->dev->name);
diff --git a/drivers/net/zynq_gem.c b/drivers/net/zynq_gem.c
index aec8077..519699d 100644
--- a/drivers/net/zynq_gem.c
+++ b/drivers/net/zynq_gem.c
@@ -179,6 +179,7 @@ struct zynq_gem_priv {
 	struct zynq_gem_regs *iobase;
 	phy_interface_t interface;
 	struct phy_device *phydev;
+	int phy_of_handle;
 	struct mii_dev *bus;
 };
 
@@ -352,14 +353,17 @@ static int zynq_phy_init(struct udevice *dev)
 	priv->phydev->supported = supported | ADVERTISED_Pause |
 				  ADVERTISED_Asym_Pause;
 	priv->phydev->advertising = priv->phydev->supported;
-	phy_config(priv->phydev);
 
-	return 0;
+	if (priv->phy_of_handle > 0)
+		priv->phydev->dev->of_offset = priv->phy_of_handle;
+
+	return phy_config(priv->phydev);
 }
 
 static int zynq_gem_init(struct udevice *dev)
 {
 	u32 i, nwconfig;
+	int ret;
 	unsigned long clk_rate = 0;
 	struct zynq_gem_priv *priv = dev_get_priv(dev);
 	struct zynq_gem_regs *regs = priv->iobase;
@@ -427,7 +431,9 @@ static int zynq_gem_init(struct udevice *dev)
 		priv->init++;
 	}
 
-	phy_startup(priv->phydev);
+	ret = phy_startup(priv->phydev);
+	if (ret)
+		return ret;
 
 	if (!priv->phydev->link) {
 		printf("%s: No link.\n", priv->phydev->dev->name);
@@ -675,7 +681,6 @@ static int zynq_gem_ofdata_to_platdata(struct udevice *dev)
 {
 	struct eth_pdata *pdata = dev_get_platdata(dev);
 	struct zynq_gem_priv *priv = dev_get_priv(dev);
-	int offset = 0;
 	const char *phy_mode;
 
 	pdata->iobase = (phys_addr_t)dev_get_addr(dev);
@@ -684,10 +689,11 @@ static int zynq_gem_ofdata_to_platdata(struct udevice *dev)
 	priv->emio = 0;
 	priv->phyaddr = -1;
 
-	offset = fdtdec_lookup_phandle(gd->fdt_blob, dev->of_offset,
-				       "phy-handle");
-	if (offset > 0)
-		priv->phyaddr = fdtdec_get_int(gd->fdt_blob, offset, "reg", -1);
+	priv->phy_of_handle = fdtdec_lookup_phandle(gd->fdt_blob,
+					dev->of_offset, "phy-handle");
+	if (priv->phy_of_handle > 0)
+		priv->phyaddr = fdtdec_get_int(gd->fdt_blob,
+					priv->phy_of_handle, "reg", -1);
 
 	phy_mode = fdt_getprop(gd->fdt_blob, dev->of_offset, "phy-mode", NULL);
 	if (phy_mode)
diff --git a/drivers/pci/pcie_layerscape.c b/drivers/pci/pcie_layerscape.c
index 0ba960e..2e6b986 100644
--- a/drivers/pci/pcie_layerscape.c
+++ b/drivers/pci/pcie_layerscape.c
@@ -569,7 +569,7 @@ static void fdt_fixup_pcie(void *blob)
 	unsigned char header_type;
 	int index;
 	u32 streamid;
-	pci_dev_t dev;
+	pci_dev_t dev, bdf;
 	int bus;
 	unsigned short id;
 	struct pci_controller *hose;
@@ -611,12 +611,15 @@ static void fdt_fixup_pcie(void *blob)
 					continue;
 				}
 
+				/* the DT fixup must be relative to the hose first_busno */
+				bdf = dev - PCI_BDF(hose->first_busno, 0, 0);
+
 				/* map PCI b.d.f to streamID in LUT */
-				ls_pcie_lut_set_mapping(pcie, index, dev >> 8,
+				ls_pcie_lut_set_mapping(pcie, index, bdf >> 8,
 							streamid);
 
 				/* update msi-map in device tree */
-				fdt_pcie_set_msi_map_entry(blob, pcie, dev >> 8,
+				fdt_pcie_set_msi_map_entry(blob, pcie, bdf >> 8,
 							   streamid);
 			}
 		}
diff --git a/drivers/pinctrl/Kconfig b/drivers/pinctrl/Kconfig
index 567b766..1785e3b 100644
--- a/drivers/pinctrl/Kconfig
+++ b/drivers/pinctrl/Kconfig
@@ -163,5 +163,6 @@ endif
 
 source "drivers/pinctrl/nxp/Kconfig"
 source "drivers/pinctrl/uniphier/Kconfig"
+source "drivers/pinctrl/exynos/Kconfig"
 
 endmenu
diff --git a/drivers/pinctrl/Makefile b/drivers/pinctrl/Makefile
index b99ed2f..7f94681 100644
--- a/drivers/pinctrl/Makefile
+++ b/drivers/pinctrl/Makefile
@@ -12,3 +12,4 @@ obj-$(CONFIG_PINCTRL_SANDBOX)	+= pinctrl-sandbox.o
 
 obj-$(CONFIG_PINCTRL_UNIPHIER)	+= uniphier/
 obj-$(CONFIG_PIC32_PINCTRL)	+= pinctrl_pic32.o
+obj-$(CONFIG_PINCTRL_EXYNOS)	+= exynos/
diff --git a/drivers/pinctrl/exynos/Kconfig b/drivers/pinctrl/exynos/Kconfig
new file mode 100644
index 0000000..84b6aaa
--- /dev/null
+++ b/drivers/pinctrl/exynos/Kconfig
@@ -0,0 +1,10 @@
+config PINCTRL_EXYNOS
+	bool
+
+config PINCTRL_EXYNOS7420
+	bool "Samsung Exynos7420 pinctrl driver"
+	depends on ARCH_EXYNOS && PINCTRL_FULL
+	select PINCTRL_EXYNOS
+	help
+	  Support pin multiplexing and pin configuration control on
+	  Samsung's Exynos7420 SoC.
diff --git a/drivers/pinctrl/exynos/Makefile b/drivers/pinctrl/exynos/Makefile
new file mode 100644
index 0000000..d9b941a
--- /dev/null
+++ b/drivers/pinctrl/exynos/Makefile
@@ -0,0 +1,9 @@
+#
+# Copyright (C) 2016 Samsung Electronics
+# Thomas Abraham <thomas.ab@samsung.com>
+#
+# SPDX-License-Identifier:	GPL-2.0+
+#
+
+obj-$(CONFIG_PINCTRL_EXYNOS)		+= pinctrl-exynos.o
+obj-$(CONFIG_PINCTRL_EXYNOS7420)	+= pinctrl-exynos7420.o
diff --git a/drivers/pinctrl/exynos/pinctrl-exynos.c b/drivers/pinctrl/exynos/pinctrl-exynos.c
new file mode 100644
index 0000000..a28405f
--- /dev/null
+++ b/drivers/pinctrl/exynos/pinctrl-exynos.c
@@ -0,0 +1,141 @@
+/*
+ * Exynos pinctrl driver common code.
+ * Copyright (C) 2016 Samsung Electronics
+ * Thomas Abraham <thomas.ab@samsung.com>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <dm.h>
+#include <errno.h>
+#include <asm/io.h>
+#include "pinctrl-exynos.h"
+
+DECLARE_GLOBAL_DATA_PTR;
+
+/**
+ * exynos_pinctrl_setup_peri: setup pinctrl for a peripheral.
+ * conf: soc specific pin configuration data array
+ * num_conf: number of configurations in the conf array.
+ * base: base address of the pin controller.
+ */
+void exynos_pinctrl_setup_peri(struct exynos_pinctrl_config_data *conf,
+		unsigned int num_conf, unsigned long base)
+{
+	unsigned int idx, val;
+
+	for (idx = 0; idx < num_conf; idx++) {
+		val = readl(base + conf[idx].offset);
+		val &= ~(conf[idx].mask);
+		val |= conf[idx].value;
+		writel(val, base + conf[idx].offset);
+	}
+}
+
+/* given a pin-name, return the address of pin config registers */
+static unsigned long pin_to_bank_base(struct udevice *dev, const char *pin_name,
+						u32 *pin)
+{
+	struct exynos_pinctrl_priv *priv = dev_get_priv(dev);
+	const struct samsung_pin_ctrl *pin_ctrl = priv->pin_ctrl;
+	const struct samsung_pin_bank_data *bank_data = pin_ctrl->pin_banks;
+	u32 nr_banks = pin_ctrl->nr_banks, idx = 0;
+	char bank[10];
+
+	/*
+	 * The format of the pin name is <bank name>-<pin_number>.
+	 * Example: gpa0-4 (gpa0 is the bank name and 4 is the pin number.
+	 */
+	while (pin_name[idx] != '-') {
+		bank[idx] = pin_name[idx];
+		idx++;
+	}
+	bank[idx] = '\0';
+	*pin = pin_name[++idx] - '0';
+
+	/* lookup the pin bank data using the pin bank name */
+	for (idx = 0; idx < nr_banks; idx++)
+		if (!strcmp(bank, bank_data[idx].name))
+			break;
+
+	return priv->base + bank_data[idx].offset;
+}
+
+/**
+ * exynos_pinctrl_set_state: configure a pin state.
+ * dev: the pinctrl device to be configured.
+ * config: the state to be configured.
+ */
+int exynos_pinctrl_set_state(struct udevice *dev, struct udevice *config)
+{
+	const void *fdt = gd->fdt_blob;
+	int node = config->of_offset;
+	unsigned int count, idx, pin_num, ret;
+	unsigned int pinfunc, pinpud, pindrv;
+	unsigned long reg, value;
+	const char *name;
+
+	/*
+	 * refer to the following document for the pinctrl bindings
+	 * linux/Documentation/devicetree/bindings/pinctrl/samsung-pinctrl.txt
+	 */
+	count = fdt_count_strings(fdt, node, "samsung,pins");
+	if (count <= 0)
+		return -EINVAL;
+
+	pinfunc = fdtdec_get_int(fdt, node, "samsung,pin-function", -1);
+	pinpud = fdtdec_get_int(fdt, node, "samsung,pin-pud", -1);
+	pindrv = fdtdec_get_int(fdt, node, "samsung,pin-drv", -1);
+
+	for (idx = 0; idx < count; idx++) {
+		ret = fdt_get_string_index(fdt, node, "samsung,pins",
+						idx, &name);
+		if (ret < 0)
+			continue;
+		reg = pin_to_bank_base(dev, name, &pin_num);
+
+		if (pinfunc != -1) {
+			value = readl(reg + PIN_CON);
+			value &= ~(0xf << (pin_num << 2));
+			value |= (pinfunc << (pin_num << 2));
+			writel(value, reg + PIN_CON);
+		}
+
+		if (pinpud != -1) {
+			value = readl(reg + PIN_PUD);
+			value &= ~(0x3 << (pin_num << 1));
+			value |= (pinpud << (pin_num << 1));
+			writel(value, reg + PIN_PUD);
+		}
+
+		if (pindrv != -1) {
+			value = readl(reg + PIN_DRV);
+			value &= ~(0x3 << (pin_num << 1));
+			value |= (pindrv << (pin_num << 1));
+			writel(value, reg + PIN_DRV);
+		}
+	}
+
+	return 0;
+}
+
+int exynos_pinctrl_probe(struct udevice *dev)
+{
+	struct exynos_pinctrl_priv *priv;
+	fdt_addr_t base;
+
+	priv = dev_get_priv(dev);
+	if (!priv)
+		return -EINVAL;
+
+	base = dev_get_addr(dev);
+	if (base == FDT_ADDR_T_NONE)
+		return -EINVAL;
+
+	priv->base = base;
+	priv->pin_ctrl = (struct samsung_pin_ctrl *)dev_get_driver_data(dev) +
+				dev->req_seq;
+
+	return 0;
+}
diff --git a/drivers/pinctrl/exynos/pinctrl-exynos.h b/drivers/pinctrl/exynos/pinctrl-exynos.h
new file mode 100644
index 0000000..abd582d
--- /dev/null
+++ b/drivers/pinctrl/exynos/pinctrl-exynos.h
@@ -0,0 +1,77 @@
+/*
+ * Exynos pinctrl driver header.
+ * Copyright (C) 2016 Samsung Electronics
+ * Thomas Abraham <thomas.ab@samsung.com>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#ifndef __PINCTRL_EXYNOS_H_
+#define __PINCTRL_EXYNOS__H_
+
+#define PIN_CON		0x00	/* Offset of pin function register */
+#define PIN_DAT		0x04	/* Offset of pin data register */
+#define PIN_PUD		0x08	/* Offset of pin pull up/down config register */
+#define PIN_DRV		0x0C	/* Offset of pin drive strength register */
+
+/**
+ * struct samsung_pin_bank_data: represent a controller pin-bank data.
+ * @offset: starting offset of the pin-bank registers.
+ * @nr_pins: number of pins included in this bank.
+ * @name: name to be prefixed for each pin in this pin bank.
+ */
+struct samsung_pin_bank_data {
+	u32		offset;
+	u8		nr_pins;
+	const char	*name;
+};
+
+#define EXYNOS_PIN_BANK(pins, reg, id)			\
+	{						\
+		.offset	= reg,				\
+		.nr_pins	= pins,			\
+		.name		= id			\
+	}
+
+/**
+ * struct samsung_pin_ctrl: represent a pin controller.
+ * @pin_banks: list of pin banks included in this controller.
+ * @nr_banks: number of pin banks.
+ */
+struct samsung_pin_ctrl {
+	const struct samsung_pin_bank_data *pin_banks;
+	u32 nr_banks;
+};
+
+/**
+ * struct exynos_pinctrl_priv: exynos pin controller driver private data
+ * @pin_ctrl: pin controller bank information.
+ * @base: base address of the pin controller instance.
+ * @num_banks: number of pin banks included in the pin controller.
+ */
+struct exynos_pinctrl_priv {
+	const struct samsung_pin_ctrl *pin_ctrl;
+	unsigned long base;
+	int num_banks;
+};
+
+/**
+ * struct exynos_pinctrl_config_data: configuration for a peripheral.
+ * @offset: offset of the config registers in the controller.
+ * @mask: value of the register to be masked with.
+ * @value: new value to be programmed.
+ */
+struct exynos_pinctrl_config_data {
+	const unsigned int	offset;
+	const unsigned int	mask;
+	const unsigned int	value;
+};
+
+
+void exynos_pinctrl_setup_peri(struct exynos_pinctrl_config_data *conf,
+		unsigned int num_conf, unsigned long base);
+int exynos_pinctrl_set_state(struct udevice *dev,
+		struct udevice *config);
+int exynos_pinctrl_probe(struct udevice *dev);
+
+#endif /* __PINCTRL_EXYNOS_H_ */
diff --git a/drivers/pinctrl/exynos/pinctrl-exynos7420.c b/drivers/pinctrl/exynos/pinctrl-exynos7420.c
new file mode 100644
index 0000000..8ae5ce7
--- /dev/null
+++ b/drivers/pinctrl/exynos/pinctrl-exynos7420.c
@@ -0,0 +1,120 @@
+/*
+ * Exynos7420 pinctrl driver.
+ * Copyright (C) 2016 Samsung Electronics
+ * Thomas Abraham <thomas.ab@samsung.com>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <dm.h>
+#include <errno.h>
+#include <asm/io.h>
+#include <dm/pinctrl.h>
+#include <dm/root.h>
+#include <fdtdec.h>
+#include <asm/arch/pinmux.h>
+#include "pinctrl-exynos.h"
+
+DECLARE_GLOBAL_DATA_PTR;
+
+#define	GPD1_OFFSET	0xc0
+
+static struct exynos_pinctrl_config_data serial2_conf[] = {
+	{
+		.offset	= GPD1_OFFSET + PIN_CON,
+		.mask	= 0x00ff0000,
+		.value	= 0x00220000,
+	}, {
+		.offset	= GPD1_OFFSET + PIN_PUD,
+		.mask	= 0x00000f00,
+		.value	= 0x00000f00,
+	},
+};
+
+static int exynos7420_pinctrl_request(struct udevice *dev, int peripheral,
+						int flags)
+{
+	struct exynos_pinctrl_priv *priv = dev_get_priv(dev);
+	unsigned long base = priv->base;
+
+	switch (PERIPH_ID_UART2) {
+	case PERIPH_ID_UART2:
+		exynos_pinctrl_setup_peri(serial2_conf,
+					  ARRAY_SIZE(serial2_conf), base);
+		break;
+	default:
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+static struct pinctrl_ops exynos7420_pinctrl_ops = {
+	.set_state	= exynos_pinctrl_set_state,
+	.request	= exynos7420_pinctrl_request,
+};
+
+/* pin banks of Exynos7420 pin-controller - BUS0 */
+static const struct samsung_pin_bank_data exynos7420_pin_banks0[] = {
+	EXYNOS_PIN_BANK(5, 0x000, "gpb0"),
+	EXYNOS_PIN_BANK(8, 0x020, "gpc0"),
+	EXYNOS_PIN_BANK(2, 0x040, "gpc1"),
+	EXYNOS_PIN_BANK(6, 0x060, "gpc2"),
+	EXYNOS_PIN_BANK(8, 0x080, "gpc3"),
+	EXYNOS_PIN_BANK(4, 0x0a0, "gpd0"),
+	EXYNOS_PIN_BANK(6, 0x0c0, "gpd1"),
+	EXYNOS_PIN_BANK(8, 0x0e0, "gpd2"),
+	EXYNOS_PIN_BANK(5, 0x100, "gpd4"),
+	EXYNOS_PIN_BANK(4, 0x120, "gpd5"),
+	EXYNOS_PIN_BANK(6, 0x140, "gpd6"),
+	EXYNOS_PIN_BANK(3, 0x160, "gpd7"),
+	EXYNOS_PIN_BANK(2, 0x180, "gpd8"),
+	EXYNOS_PIN_BANK(2, 0x1a0, "gpg0"),
+	EXYNOS_PIN_BANK(4, 0x1c0, "gpg3"),
+};
+
+/* pin banks of Exynos7420 pin-controller - FSYS0 */
+static const struct samsung_pin_bank_data exynos7420_pin_banks1[] = {
+	EXYNOS_PIN_BANK(7, 0x000, "gpr4"),
+};
+
+/* pin banks of Exynos7420 pin-controller - FSYS1 */
+static const struct samsung_pin_bank_data exynos7420_pin_banks2[] = {
+	EXYNOS_PIN_BANK(4, 0x000, "gpr0"),
+	EXYNOS_PIN_BANK(8, 0x020, "gpr1"),
+	EXYNOS_PIN_BANK(5, 0x040, "gpr2"),
+	EXYNOS_PIN_BANK(8, 0x060, "gpr3"),
+};
+
+const struct samsung_pin_ctrl exynos7420_pin_ctrl[] = {
+	{
+		/* pin-controller instance BUS0 data */
+		.pin_banks	= exynos7420_pin_banks0,
+		.nr_banks	= ARRAY_SIZE(exynos7420_pin_banks0),
+	}, {
+		/* pin-controller instance FSYS0 data */
+		.pin_banks	= exynos7420_pin_banks1,
+		.nr_banks	= ARRAY_SIZE(exynos7420_pin_banks1),
+	}, {
+		/* pin-controller instance FSYS1 data */
+		.pin_banks	= exynos7420_pin_banks2,
+		.nr_banks	= ARRAY_SIZE(exynos7420_pin_banks2),
+	},
+};
+
+static const struct udevice_id exynos7420_pinctrl_ids[] = {
+	{ .compatible = "samsung,exynos7420-pinctrl",
+		.data = (ulong)exynos7420_pin_ctrl },
+	{ }
+};
+
+U_BOOT_DRIVER(pinctrl_exynos7420) = {
+	.name		= "pinctrl_exynos7420",
+	.id		= UCLASS_PINCTRL,
+	.of_match	= exynos7420_pinctrl_ids,
+	.priv_auto_alloc_size = sizeof(struct exynos_pinctrl_priv),
+	.ops		= &exynos7420_pinctrl_ops,
+	.probe		= exynos_pinctrl_probe,
+	.flags		= DM_FLAG_PRE_RELOC
+};
diff --git a/drivers/pinctrl/pinctrl-uclass.c b/drivers/pinctrl/pinctrl-uclass.c
index ccc5d30..fd04b26 100644
--- a/drivers/pinctrl/pinctrl-uclass.c
+++ b/drivers/pinctrl/pinctrl-uclass.c
@@ -287,5 +287,6 @@ static int pinctrl_post_bind(struct udevice *dev)
 UCLASS_DRIVER(pinctrl) = {
 	.id = UCLASS_PINCTRL,
 	.post_bind = pinctrl_post_bind,
+	.flags = DM_UC_FLAG_SEQ_ALIAS,
 	.name = "pinctrl",
 };
diff --git a/drivers/pinctrl/rockchip/pinctrl_rk3288.c b/drivers/pinctrl/rockchip/pinctrl_rk3288.c
index 7c769bd..1fa1daa 100644
--- a/drivers/pinctrl/rockchip/pinctrl_rk3288.c
+++ b/drivers/pinctrl/rockchip/pinctrl_rk3288.c
@@ -623,7 +623,7 @@ static int rk3288_pinctrl_set_state(struct udevice *dev, struct udevice *config)
 {
 	const void *blob = gd->fdt_blob;
 	int pcfg_node, ret, flags, count, i;
-	u32 cell[40], *ptr;
+	u32 cell[60], *ptr;
 
 	debug("%s: %s %s\n", __func__, dev->name, config->name);
 	ret = fdtdec_get_int_array_count(blob, config->of_offset,
diff --git a/drivers/power/Kconfig b/drivers/power/Kconfig
index 3c41bca..3c44167 100644
--- a/drivers/power/Kconfig
+++ b/drivers/power/Kconfig
@@ -41,9 +41,17 @@ config AXP221_POWER
 	Select this to enable support for the axp221/axp223 pmic found on most
 	A23 and A31 boards.
 
+config AXP809_POWER
+	boolean "axp809 pmic support"
+	depends on MACH_SUN9I
+	select CMD_POWEROFF
+	---help---
+	Say y here to enable support for the axp809 pmic found on A80 boards.
+
 config AXP818_POWER
 	boolean "axp818 pmic support"
 	depends on MACH_SUN8I_A83T
+	select CMD_POWEROFF
 	---help---
 	Say y here to enable support for the axp818 pmic found on
 	A83T dev board.
@@ -59,36 +67,39 @@ endchoice
 
 config AXP_DCDC1_VOLT
 	int "axp pmic dcdc1 voltage"
-	depends on AXP221_POWER || AXP818_POWER
+	depends on AXP221_POWER || AXP809_POWER || AXP818_POWER
 	default 3300 if AXP818_POWER
-	default 3000 if MACH_SUN6I || MACH_SUN8I
+	default 3000 if MACH_SUN6I || MACH_SUN8I || MACH_SUN9I
 	---help---
 	Set the voltage (mV) to program the axp pmic dcdc1 at, set to 0 to
 	disable dcdc1. On A23 / A31 / A33 (axp221) boards dcdc1 is used for
 	generic 3.3V IO voltage for external devices like the lcd-panal and
 	sdcard interfaces, etc. On most boards dcdc1 is undervolted to 3.0V to
-	safe battery. On A31 devices dcdc1 is also used for VCC-IO. On A83T
-	dcdc1 is used for VCC-IO, nand, usb0, sd , etc.
+	save battery. On A31 devices dcdc1 is also used for VCC-IO. On A83T
+	dcdc1 is used for VCC-IO, nand, usb0, sd , etc. On A80 dcdc1 normally
+	powers some of the pingroups, NAND/eMMC, SD/MMC, and USB OTG.
 
 config AXP_DCDC2_VOLT
 	int "axp pmic dcdc2 voltage"
-	depends on AXP152_POWER || AXP209_POWER || AXP221_POWER || AXP818_POWER
+	depends on AXP152_POWER || AXP209_POWER || AXP221_POWER || AXP809_POWER || AXP818_POWER
 	default 900 if AXP818_POWER
 	default 1400 if AXP152_POWER || AXP209_POWER
 	default 1200 if MACH_SUN6I
 	default 1100 if MACH_SUN8I
+	default 0 if MACH_SUN9I
 	---help---
 	Set the voltage (mV) to program the axp pmic dcdc2 at, set to 0 to
 	disable dcdc2.
 	On A10(s) / A13 / A20 boards dcdc2 is VDD-CPU and should be 1.4V.
 	On A31 boards dcdc2 is used for VDD-GPU and should be 1.2V.
 	On A23/A33 boards dcdc2 is used for VDD-SYS and should be 1.1V.
+	On A80 boards dcdc2 powers the GPU and can be left off.
 	On A83T boards dcdc2 is used for VDD-CPUA(cluster 0) and should be 0.9V.
 
 config AXP_DCDC3_VOLT
 	int "axp pmic dcdc3 voltage"
-	depends on AXP152_POWER || AXP209_POWER || AXP221_POWER || AXP818_POWER
-	default 900 if AXP818_POWER
+	depends on AXP152_POWER || AXP209_POWER || AXP221_POWER || AXP809_POWER || AXP818_POWER
+	default 900 if AXP809_POWER || AXP818_POWER
 	default 1500 if AXP152_POWER
 	default 1250 if AXP209_POWER
 	default 1200 if MACH_SUN6I || MACH_SUN8I
@@ -99,51 +110,55 @@ config AXP_DCDC3_VOLT
 	should be 1.25V.
 	On A10s boards with an axp152 dcdc3 is VCC-DRAM and should be 1.5V.
 	On A23 / A31 / A33 boards dcdc3 is VDD-CPU and should be 1.2V.
+	On A80 boards dcdc3 is used for VDD-CPUA(cluster 0) and should be 0.9V.
 	On A83T boards dcdc3 is used for VDD-CPUB(cluster 1) and should be 0.9V.
 
 config AXP_DCDC4_VOLT
 	int "axp pmic dcdc4 voltage"
-	depends on AXP152_POWER || AXP221_POWER || AXP818_POWER
+	depends on AXP152_POWER || AXP221_POWER || AXP809_POWER || AXP818_POWER
 	default 1250 if AXP152_POWER
 	default 1200 if MACH_SUN6I
 	default 0 if MACH_SUN8I
+	default 900 if MACH_SUN9I
 	---help---
 	Set the voltage (mV) to program the axp pmic dcdc4 at, set to 0 to
 	disable dcdc4.
 	On A10s boards with an axp152 dcdc4 is VDD-INT-DLL and should be 1.25V.
 	On A31 boards dcdc4 is used for VDD-SYS and should be 1.2V.
 	On A23 / A33 boards dcdc4 is unused and should be disabled.
+	On A80 boards dcdc4 powers VDD-SYS, HDMI, USB OTG and should be 0.9V.
 	On A83T boards dcdc4 is used for VDD-GPU.
 
 config AXP_DCDC5_VOLT
 	int "axp pmic dcdc5 voltage"
-	depends on AXP221_POWER || AXP818_POWER
-	default 1500 if MACH_SUN6I || MACH_SUN8I
+	depends on AXP221_POWER || AXP809_POWER || AXP818_POWER
+	default 1500 if MACH_SUN6I || MACH_SUN8I || MACH_SUN9I
 	---help---
 	Set the voltage (mV) to program the axp pmic dcdc5 at, set to 0 to
 	disable dcdc5.
-	On A23 / A31 / A33 / A83T boards dcdc5 is VCC-DRAM and should be 1.5V,
-	1.35V if DDR3L is used.
+	On A23 / A31 / A33 / A80 / A83T boards dcdc5 is VCC-DRAM and
+	should be 1.5V, 1.35V if DDR3L is used.
 
 config AXP_ALDO1_VOLT
 	int "axp pmic (a)ldo1 voltage"
-	depends on AXP221_POWER || AXP818_POWER
+	depends on AXP221_POWER || AXP809_POWER || AXP818_POWER
 	default 0 if MACH_SUN6I
 	default 1800 if MACH_SUN8I_A83T
-	default 3000 if MACH_SUN8I
+	default 3000 if MACH_SUN8I || MACH_SUN9I
 	---help---
 	Set the voltage (mV) to program the axp pmic aldo1 at, set to 0 to
 	disable aldo1.
 	On A31 boards aldo1 is often used to power the wifi module.
 	On A23 / A33 boards aldo1 is used for VCC-IO and should be 3.0V.
+	On A80 boards aldo1 powers the USB hosts and should be 3.0V.
 	On A83T / H8 boards aldo1 is used for MIPI CSI, DSI, HDMI, EFUSE, and
 	should be 1.8V.
 
 config AXP_ALDO2_VOLT
 	int "axp pmic (a)ldo2 voltage"
-	depends on AXP152_POWER || AXP209_POWER || AXP221_POWER || AXP818_POWER
+	depends on AXP152_POWER || AXP209_POWER || AXP221_POWER || AXP809_POWER || AXP818_POWER
 	default 3000 if AXP152_POWER || AXP209_POWER
-	default 0 if MACH_SUN6I
+	default 0 if MACH_SUN6I || MACH_SUN9I
 	default 1800 if MACH_SUN8I_A83T
 	default 2500 if MACH_SUN8I
 	---help---
@@ -153,19 +168,21 @@ config AXP_ALDO2_VOLT
 	On A31 boards aldo2 is typically unused and should be disabled.
 	On A31 boards aldo2 may be used for LPDDR2 then it should be 1.8V.
 	On A23 / A33 boards aldo2 is used for VDD-DLL and should be 2.5V.
+	On A80 boards aldo2 powers PB pingroup and camera IO and can be left off.
 	On A83T / H8 boards aldo2 powers VDD-DLL, VCC18-PLL, CPVDD, VDD18-ADC,
 	LPDDR2, and the codec. It should be 1.8V.
 
 config AXP_ALDO3_VOLT
 	int "axp pmic (a)ldo3 voltage"
-	depends on AXP209_POWER || AXP221_POWER || AXP818_POWER
-	default 0 if AXP209_POWER
+	depends on AXP209_POWER || AXP221_POWER || AXP809_POWER || AXP818_POWER
+	default 0 if AXP209_POWER || MACH_SUN9I
 	default 3000 if MACH_SUN6I || MACH_SUN8I
 	---help---
 	Set the voltage (mV) to program the axp pmic aldo3 at, set to 0 to
 	disable aldo3.
 	On A10(s) / A13 / A20 boards aldo3 should be 2.8V.
 	On A23 / A31 / A33 boards aldo3 is VCC-PLL and AVCC and should be 3.0V.
+	On A80 boards aldo3 is normally not used.
 	On A83T / H8 boards aldo3 is AVCC, VCC-PL, and VCC-LED, and should be
 	3.0V.
 
@@ -180,21 +197,23 @@ config AXP_ALDO4_VOLT
 
 config AXP_DLDO1_VOLT
 	int "axp pmic dldo1 voltage"
-	depends on AXP221_POWER || AXP818_POWER
+	depends on AXP221_POWER || AXP809_POWER || AXP818_POWER
 	default 0
 	---help---
 	Set the voltage (mV) to program the axp pmic dldo1 at, set to 0 to
 	disable dldo1. On sun6i (A31) boards with ethernet dldo1 is often used
-	to power the ethernet phy. On sun8i (A23) boards this is often used to
-	power the wifi.
+	to power the ethernet phy. On A23, A33 and A80 boards this is often
+	used to power the wifi.
 
 config AXP_DLDO2_VOLT
 	int "axp pmic dldo2 voltage"
-	depends on AXP221_POWER || AXP818_POWER
+	depends on AXP221_POWER || AXP809_POWER || AXP818_POWER
+	default 3000 if MACH_SUN9I
 	default 0
 	---help---
 	Set the voltage (mV) to program the axp pmic dldo2 at, set to 0 to
 	disable dldo2.
+	On A80 boards dldo2 normally powers the PL pins and should be 3.0V.
 
 config AXP_DLDO3_VOLT
 	int "axp pmic dldo3 voltage"
@@ -214,7 +233,7 @@ config AXP_DLDO4_VOLT
 
 config AXP_ELDO1_VOLT
 	int "axp pmic eldo1 voltage"
-	depends on AXP221_POWER || AXP818_POWER
+	depends on AXP221_POWER || AXP809_POWER || AXP818_POWER
 	default 0
 	---help---
 	Set the voltage (mV) to program the axp pmic eldo1 at, set to 0 to
@@ -222,7 +241,7 @@ config AXP_ELDO1_VOLT
 
 config AXP_ELDO2_VOLT
 	int "axp pmic eldo2 voltage"
-	depends on AXP221_POWER || AXP818_POWER
+	depends on AXP221_POWER || AXP809_POWER || AXP818_POWER
 	default 0
 	---help---
 	Set the voltage (mV) to program the axp pmic eldo2 at, set to 0 to
@@ -230,13 +249,15 @@ config AXP_ELDO2_VOLT
 
 config AXP_ELDO3_VOLT
 	int "axp pmic eldo3 voltage"
-	depends on AXP221_POWER || AXP818_POWER
+	depends on AXP221_POWER || AXP809_POWER || AXP818_POWER
+	default 3000 if MACH_SUN9I
 	default 0
 	---help---
 	Set the voltage (mV) to program the axp pmic eldo3 at, set to 0 to
 	disable eldo3. On some A31(s) tablets it might be used to supply
 	1.2V for the SSD2828 chip (converter of parallel LCD interface
 	into MIPI DSI).
+	On A80 boards it powers the PM pingroup and should be 3.0V.
 
 config AXP_FLDO1_VOLT
 	int "axp pmic fldo1 voltage"
@@ -249,7 +270,7 @@ config AXP_FLDO1_VOLT
 	used.
 
 config AXP_FLDO2_VOLT
-	int "axp pmic eldo2 voltage"
+	int "axp pmic fldo2 voltage"
 	depends on AXP818_POWER
 	default 900 if MACH_SUN8I_A83T
 	---help---
@@ -265,6 +286,13 @@ config AXP_FLDO3_VOLT
 	Set the voltage (mV) to program the axp pmic fldo3 at, set to 0 to
 	disable fldo3.
 
+config AXP_SW_ON
+	bool "axp pmic sw on"
+	depends on AXP809_POWER || AXP818_POWER
+	default n
+	---help---
+	Enable to turn on axp pmic sw.
+
 config SY8106A_VOUT1_VOLT
 	int "SY8106A pmic VOUT1 voltage"
 	depends on SY8106A_POWER
diff --git a/drivers/power/Makefile b/drivers/power/Makefile
index 690faa0..b43523e 100644
--- a/drivers/power/Makefile
+++ b/drivers/power/Makefile
@@ -9,6 +9,7 @@ obj-$(CONFIG_AS3722_POWER)	+= as3722.o
 obj-$(CONFIG_AXP152_POWER)	+= axp152.o
 obj-$(CONFIG_AXP209_POWER)	+= axp209.o
 obj-$(CONFIG_AXP221_POWER)	+= axp221.o
+obj-$(CONFIG_AXP809_POWER)	+= axp809.o
 obj-$(CONFIG_AXP818_POWER)	+= axp818.o
 obj-$(CONFIG_EXYNOS_TMU)	+= exynos-tmu.o
 obj-$(CONFIG_FTPMU010_POWER)	+= ftpmu010.o
diff --git a/drivers/power/axp221.c b/drivers/power/axp221.c
index cb1f88b..727ab09 100644
--- a/drivers/power/axp221.c
+++ b/drivers/power/axp221.c
@@ -191,33 +191,20 @@ int axp_set_eldo(int eldo_num, unsigned int mvolt)
 {
 	int ret;
 	u8 cfg = axp221_mvolt_to_cfg(mvolt, 700, 3300, 100);
-	u8 addr, bits;
-
-	switch (eldo_num) {
-	case 3:
-		addr = AXP221_ELDO3_CTRL;
-		bits = AXP221_OUTPUT_CTRL2_ELDO3_EN;
-		break;
-	case 2:
-		addr = AXP221_ELDO2_CTRL;
-		bits = AXP221_OUTPUT_CTRL2_ELDO2_EN;
-		break;
-	case 1:
-		addr = AXP221_ELDO1_CTRL;
-		bits = AXP221_OUTPUT_CTRL2_ELDO1_EN;
-		break;
-	default:
+
+	if (eldo_num < 1 || eldo_num > 3)
 		return -EINVAL;
-	}
 
 	if (mvolt == 0)
-		return pmic_bus_clrbits(AXP221_OUTPUT_CTRL2, bits);
+		return pmic_bus_clrbits(AXP221_OUTPUT_CTRL2,
+				AXP221_OUTPUT_CTRL2_ELDO1_EN << (eldo_num - 1));
 
-	ret = pmic_bus_write(addr, cfg);
+	ret = pmic_bus_write(AXP221_ELDO1_CTRL + (eldo_num - 1), cfg);
 	if (ret)
 		return ret;
 
-	return pmic_bus_setbits(AXP221_OUTPUT_CTRL2, bits);
+	return pmic_bus_setbits(AXP221_OUTPUT_CTRL2,
+				AXP221_OUTPUT_CTRL2_ELDO1_EN << (eldo_num - 1));
 }
 
 int axp_init(void)
diff --git a/drivers/power/axp809.c b/drivers/power/axp809.c
new file mode 100644
index 0000000..c8b76cf
--- /dev/null
+++ b/drivers/power/axp809.c
@@ -0,0 +1,238 @@
+/*
+ * AXP809 driver based on AXP221 driver
+ *
+ *
+ * (C) Copyright 2016 Chen-Yu Tsai <wens@csie.org>
+ *
+ * Based on axp221.c
+ * (C) Copyright 2014 Hans de Goede <hdegoede@redhat.com>
+ * (C) Copyright 2013 Oliver Schinagl <oliver@schinagl.nl>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <errno.h>
+#include <asm/arch/gpio.h>
+#include <asm/arch/pmic_bus.h>
+#include <axp_pmic.h>
+
+static u8 axp809_mvolt_to_cfg(int mvolt, int min, int max, int div)
+{
+	if (mvolt < min)
+		mvolt = min;
+	else if (mvolt > max)
+		mvolt = max;
+
+	return (mvolt - min) / div;
+}
+
+int axp_set_dcdc1(unsigned int mvolt)
+{
+	int ret;
+	u8 cfg = axp809_mvolt_to_cfg(mvolt, 1600, 3400, 100);
+
+	if (mvolt == 0)
+		return pmic_bus_clrbits(AXP809_OUTPUT_CTRL1,
+					AXP809_OUTPUT_CTRL1_DCDC1_EN);
+
+	ret = pmic_bus_write(AXP809_DCDC1_CTRL, cfg);
+	if (ret)
+		return ret;
+
+	ret = pmic_bus_setbits(AXP809_OUTPUT_CTRL2,
+			       AXP809_OUTPUT_CTRL2_DC1SW_EN);
+	if (ret)
+		return ret;
+
+	return pmic_bus_setbits(AXP809_OUTPUT_CTRL1,
+				AXP809_OUTPUT_CTRL1_DCDC1_EN);
+}
+
+int axp_set_dcdc2(unsigned int mvolt)
+{
+	int ret;
+	u8 cfg = axp809_mvolt_to_cfg(mvolt, 600, 1540, 20);
+
+	if (mvolt == 0)
+		return pmic_bus_clrbits(AXP809_OUTPUT_CTRL1,
+					AXP809_OUTPUT_CTRL1_DCDC2_EN);
+
+	ret = pmic_bus_write(AXP809_DCDC2_CTRL, cfg);
+	if (ret)
+		return ret;
+
+	return pmic_bus_setbits(AXP809_OUTPUT_CTRL1,
+				AXP809_OUTPUT_CTRL1_DCDC2_EN);
+}
+
+int axp_set_dcdc3(unsigned int mvolt)
+{
+	int ret;
+	u8 cfg = axp809_mvolt_to_cfg(mvolt, 600, 1860, 20);
+
+	if (mvolt == 0)
+		return pmic_bus_clrbits(AXP809_OUTPUT_CTRL1,
+					AXP809_OUTPUT_CTRL1_DCDC3_EN);
+
+	ret = pmic_bus_write(AXP809_DCDC3_CTRL, cfg);
+	if (ret)
+		return ret;
+
+	return pmic_bus_setbits(AXP809_OUTPUT_CTRL1,
+				AXP809_OUTPUT_CTRL1_DCDC3_EN);
+}
+
+int axp_set_dcdc4(unsigned int mvolt)
+{
+	int ret;
+	u8 cfg = axp809_mvolt_to_cfg(mvolt, 600, 1540, 20);
+
+	if (mvolt >= 1540)
+		cfg = 0x30 + axp809_mvolt_to_cfg(mvolt, 1800, 2600, 100);
+
+	if (mvolt == 0)
+		return pmic_bus_clrbits(AXP809_OUTPUT_CTRL1,
+					AXP809_OUTPUT_CTRL1_DCDC4_EN);
+
+	ret = pmic_bus_write(AXP809_DCDC5_CTRL, cfg);
+	if (ret)
+		return ret;
+
+	return pmic_bus_setbits(AXP809_OUTPUT_CTRL1,
+				AXP809_OUTPUT_CTRL1_DCDC4_EN);
+}
+
+int axp_set_dcdc5(unsigned int mvolt)
+{
+	int ret;
+	u8 cfg = axp809_mvolt_to_cfg(mvolt, 1000, 2550, 50);
+
+	if (mvolt == 0)
+		return pmic_bus_clrbits(AXP809_OUTPUT_CTRL1,
+					AXP809_OUTPUT_CTRL1_DCDC5_EN);
+
+	ret = pmic_bus_write(AXP809_DCDC5_CTRL, cfg);
+	if (ret)
+		return ret;
+
+	return pmic_bus_setbits(AXP809_OUTPUT_CTRL1,
+				AXP809_OUTPUT_CTRL1_DCDC5_EN);
+}
+
+int axp_set_aldo(int aldo_num, unsigned int mvolt)
+{
+	int ret;
+	u8 cfg;
+
+	if (aldo_num < 1 || aldo_num > 3)
+		return -EINVAL;
+
+	if (mvolt == 0 && aldo_num == 3)
+		return pmic_bus_clrbits(AXP809_OUTPUT_CTRL2,
+					AXP809_OUTPUT_CTRL2_ALDO3_EN);
+	if (mvolt == 0)
+		return pmic_bus_clrbits(AXP809_OUTPUT_CTRL1,
+				AXP809_OUTPUT_CTRL1_ALDO1_EN << (aldo_num - 1));
+
+	cfg = axp809_mvolt_to_cfg(mvolt, 700, 3300, 100);
+	ret = pmic_bus_write(AXP809_ALDO1_CTRL + (aldo_num - 1), cfg);
+	if (ret)
+		return ret;
+
+	if (aldo_num == 3)
+		return pmic_bus_setbits(AXP809_OUTPUT_CTRL2,
+					AXP809_OUTPUT_CTRL2_ALDO3_EN);
+	return pmic_bus_clrbits(AXP809_OUTPUT_CTRL1,
+				AXP809_OUTPUT_CTRL1_ALDO1_EN << (aldo_num - 1));
+}
+
+/* TODO: re-work other AXP drivers to consolidate ALDO functions. */
+int axp_set_aldo1(unsigned int mvolt)
+{
+	return axp_set_aldo(1, mvolt);
+}
+
+int axp_set_aldo2(unsigned int mvolt)
+{
+	return axp_set_aldo(2, mvolt);
+}
+
+int axp_set_aldo3(unsigned int mvolt)
+{
+	return axp_set_aldo(3, mvolt);
+}
+
+int axp_set_dldo(int dldo_num, unsigned int mvolt)
+{
+	u8 cfg = axp809_mvolt_to_cfg(mvolt, 700, 3300, 100);
+	int ret;
+
+	if (dldo_num < 1 || dldo_num > 2)
+		return -EINVAL;
+
+	if (mvolt == 0)
+		return pmic_bus_clrbits(AXP809_OUTPUT_CTRL2,
+				AXP809_OUTPUT_CTRL2_DLDO1_EN << (dldo_num - 1));
+
+	if (dldo_num == 1 && mvolt > 3300)
+		cfg += 1 + axp809_mvolt_to_cfg(mvolt, 3400, 4200, 200);
+	ret = pmic_bus_write(AXP809_DLDO1_CTRL + (dldo_num - 1), cfg);
+	if (ret)
+		return ret;
+
+	return pmic_bus_setbits(AXP809_OUTPUT_CTRL2,
+				AXP809_OUTPUT_CTRL2_DLDO1_EN << (dldo_num - 1));
+}
+
+int axp_set_eldo(int eldo_num, unsigned int mvolt)
+{
+	int ret;
+	u8 cfg = axp809_mvolt_to_cfg(mvolt, 700, 3300, 100);
+
+	if (eldo_num < 1 || eldo_num > 3)
+		return -EINVAL;
+
+	if (mvolt == 0)
+		return pmic_bus_clrbits(AXP809_OUTPUT_CTRL2,
+				AXP809_OUTPUT_CTRL2_ELDO1_EN << (eldo_num - 1));
+
+	ret = pmic_bus_write(AXP809_ELDO1_CTRL + (eldo_num - 1), cfg);
+	if (ret)
+		return ret;
+
+	return pmic_bus_setbits(AXP809_OUTPUT_CTRL2,
+				AXP809_OUTPUT_CTRL2_ELDO1_EN << (eldo_num - 1));
+}
+
+int axp_set_sw(bool on)
+{
+	if (on)
+		return pmic_bus_setbits(AXP809_OUTPUT_CTRL2,
+					AXP809_OUTPUT_CTRL2_SWOUT_EN);
+
+	return pmic_bus_clrbits(AXP809_OUTPUT_CTRL2,
+				AXP809_OUTPUT_CTRL2_SWOUT_EN);
+}
+
+int axp_init(void)
+{
+	int ret;
+
+	ret = pmic_bus_init();
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+int do_poweroff(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
+{
+	pmic_bus_write(AXP809_SHUTDOWN, AXP809_SHUTDOWN_POWEROFF);
+
+	/* infinite loop during shutdown */
+	while (1) {}
+
+	/* not reached */
+	return 0;
+}
diff --git a/drivers/power/axp818.c b/drivers/power/axp818.c
index 3ac05ff..af4d7a6 100644
--- a/drivers/power/axp818.c
+++ b/drivers/power/axp818.c
@@ -225,6 +225,16 @@ int axp_set_fldo(int fldo_num, unsigned int mvolt)
 				AXP818_OUTPUT_CTRL3_FLDO1_EN << (fldo_num - 1));
 }
 
+int axp_set_sw(bool on)
+{
+	if (on)
+		return pmic_bus_setbits(AXP818_OUTPUT_CTRL2,
+					AXP818_OUTPUT_CTRL2_SW_EN);
+
+	return pmic_bus_clrbits(AXP818_OUTPUT_CTRL2,
+				AXP818_OUTPUT_CTRL2_SW_EN);
+}
+
 int axp_init(void)
 {
 	u8 axp_chip_id;
@@ -245,3 +255,14 @@ int axp_init(void)
 
 	return 0;
 }
+
+int do_poweroff(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
+{
+	pmic_bus_write(AXP818_SHUTDOWN, AXP818_SHUTDOWN_POWEROFF);
+
+	/* infinite loop during shutdown */
+	while (1) {}
+
+	/* not reached */
+	return 0;
+}
diff --git a/drivers/pwm/Kconfig b/drivers/pwm/Kconfig
index 6f0d61e..37ea2b8 100644
--- a/drivers/pwm/Kconfig
+++ b/drivers/pwm/Kconfig
@@ -9,6 +9,15 @@ config DM_PWM
 	  frequency/period can be controlled along with the proportion of that
 	  time that the signal is high.
 
+config PWM_EXYNOS
+	bool "Enable support for the Exynos PWM"
+	depends on DM_PWM
+	help
+	  This PWM is found on Samsung Exynos 5250 and other Samsung SoCs. It
+	  supports a programmable period and duty cycle. A 32-bit counter is
+	  used. It provides 5 channels which can be independently
+	  programmed. Channel 4 (the last) is normally used as a timer.
+
 config PWM_ROCKCHIP
 	bool "Enable support for the Rockchip PWM"
 	depends on DM_PWM
diff --git a/drivers/pwm/Makefile b/drivers/pwm/Makefile
index fd414b1..af39347 100644
--- a/drivers/pwm/Makefile
+++ b/drivers/pwm/Makefile
@@ -15,4 +15,5 @@ obj-$(CONFIG_PWM_ROCKCHIP) += rk_pwm.o
 obj-$(CONFIG_PWM_IMX) += pwm-imx.o pwm-imx-util.o
 ifdef CONFIG_DM_PWM
 obj-$(CONFIG_PWM_TEGRA) += tegra_pwm.o
+obj-$(CONFIG_PWM_EXYNOS) += exynos_pwm.o
 endif
diff --git a/drivers/pwm/exynos_pwm.c b/drivers/pwm/exynos_pwm.c
new file mode 100644
index 0000000..a0edafc
--- /dev/null
+++ b/drivers/pwm/exynos_pwm.c
@@ -0,0 +1,120 @@
+/*
+ * Copyright 2016 Google Inc.
+ *
+ * SPDX-License-Identifier:     GPL-2.0+
+ */
+
+#include <common.h>
+#include <dm.h>
+#include <pwm.h>
+#include <asm/io.h>
+#include <asm/arch/clk.h>
+#include <asm/arch/clock.h>
+#include <asm/arch/pwm.h>
+
+DECLARE_GLOBAL_DATA_PTR;
+
+struct exynos_pwm_priv {
+	struct s5p_timer *regs;
+};
+
+static int exynos_pwm_set_config(struct udevice *dev, uint channel,
+				uint period_ns, uint duty_ns)
+{
+	struct exynos_pwm_priv *priv = dev_get_priv(dev);
+	struct s5p_timer *regs = priv->regs;
+	unsigned int offset, prescaler;
+	uint div = 4, rate, rate_ns;
+	u32 val;
+	u32 tcnt, tcmp, tcon;
+
+	if (channel >= 5)
+		return -EINVAL;
+	debug("%s: Configure '%s' channel %u, period_ns %u, duty_ns %u\n",
+	      __func__, dev->name, channel, period_ns, duty_ns);
+
+	val = readl(&regs->tcfg0);
+	prescaler = (channel < 2 ? val : (val >> 8)) & 0xff;
+	div = (readl(&regs->tcfg1) >> MUX_DIV_SHIFT(channel)) & 0xf;
+
+	rate = get_pwm_clk() / ((prescaler + 1) * (1 << div));
+	debug("%s: pwm_clk %lu, rate %u\n", __func__, get_pwm_clk(), rate);
+
+	if (channel < 4) {
+		rate_ns = 1000000000 / rate;
+		tcnt = period_ns / rate_ns;
+		tcmp = duty_ns / rate_ns;
+		debug("%s: tcnt %u, tcmp %u\n", __func__, tcnt, tcmp);
+		offset = channel * 3;
+		writel(tcnt, &regs->tcntb0 + offset);
+		writel(tcmp, &regs->tcmpb0 + offset);
+	}
+
+	tcon = readl(&regs->tcon);
+	tcon |= TCON_UPDATE(channel);
+	if (channel < 4)
+		tcon |= TCON_AUTO_RELOAD(channel);
+	else
+		tcon |= TCON4_AUTO_RELOAD;
+	writel(tcon, &regs->tcon);
+
+	tcon &= ~TCON_UPDATE(channel);
+	writel(tcon, &regs->tcon);
+
+	return 0;
+}
+
+static int exynos_pwm_set_enable(struct udevice *dev, uint channel,
+				 bool enable)
+{
+	struct exynos_pwm_priv *priv = dev_get_priv(dev);
+	struct s5p_timer *regs = priv->regs;
+	u32 mask;
+
+	if (channel >= 4)
+		return -EINVAL;
+	debug("%s: Enable '%s' channel %u\n", __func__, dev->name, channel);
+	mask = TCON_START(channel);
+	clrsetbits_le32(&regs->tcon, mask, enable ? mask : 0);
+
+	return 0;
+}
+
+static int exynos_pwm_probe(struct udevice *dev)
+{
+	struct exynos_pwm_priv *priv = dev_get_priv(dev);
+	struct s5p_timer *regs = priv->regs;
+
+	writel(PRESCALER_0 | PRESCALER_1 << 8, &regs->tcfg0);
+
+	return 0;
+}
+
+static int exynos_pwm_ofdata_to_platdata(struct udevice *dev)
+{
+	struct exynos_pwm_priv *priv = dev_get_priv(dev);
+
+	priv->regs = (struct s5p_timer *)dev_get_addr(dev);
+
+	return 0;
+}
+
+static const struct pwm_ops exynos_pwm_ops = {
+	.set_config	= exynos_pwm_set_config,
+	.set_enable	= exynos_pwm_set_enable,
+};
+
+static const struct udevice_id exynos_channels[] = {
+	{ .compatible = "samsung,exynos4210-pwm" },
+	{ }
+};
+
+U_BOOT_DRIVER(exynos_pwm) = {
+	.name	= "exynos_pwm",
+	.id	= UCLASS_PWM,
+	.of_match = exynos_channels,
+	.ops	= &exynos_pwm_ops,
+	.probe	= exynos_pwm_probe,
+	.ofdata_to_platdata	= exynos_pwm_ofdata_to_platdata,
+	.priv_auto_alloc_size	= sizeof(struct exynos_pwm_priv),
+};
diff --git a/drivers/serial/Kconfig b/drivers/serial/Kconfig
index 2497ae9..0e38903 100644
--- a/drivers/serial/Kconfig
+++ b/drivers/serial/Kconfig
@@ -121,6 +121,14 @@ config DEBUG_UART_S5P
 	  will need to provide parameters to make this work. The driver will
 	  be available until the real driver-model serial is running.
 
+config DEBUG_UART_MESON
+	bool "Amlogic Meson"
+	depends on MESON_SERIAL
+	help
+	  Select this to enable a debug UART using the serial_meson driver. You
+	  will need to provide parameters to make this work. The driver will
+	  be available until the real driver-model serial is running.
+
 config DEBUG_UART_UARTLITE
 	bool "Xilinx Uartlite"
 	help
@@ -338,6 +346,13 @@ config XILINX_UARTLITE
 	  If you have a Xilinx based board and want to use the uartlite
 	  serial ports, say Y to this option. If unsure, say N.
 
+config MESON_SERIAL
+	bool "Support for Amlogic Meson UART"
+	depends on DM_SERIAL && ARCH_MESON
+	help
+	  If you have an Amlogic Meson based board and want to use the on-chip
+	  serial ports, say Y to this option. If unsure, say N.
+
 config MSM_SERIAL
 	bool "Qualcomm on-chip UART"
 	depends on DM_SERIAL
diff --git a/drivers/serial/Makefile b/drivers/serial/Makefile
index 9def128..e1e28de 100644
--- a/drivers/serial/Makefile
+++ b/drivers/serial/Makefile
@@ -27,6 +27,7 @@ obj-$(CONFIG_SYS_NS16550) += ns16550.o
 obj-$(CONFIG_S5P) += serial_s5p.o
 obj-$(CONFIG_MXC_UART) += serial_mxc.o
 obj-$(CONFIG_PXA_SERIAL) += serial_pxa.o
+obj-$(CONFIG_MESON_SERIAL) += serial_meson.o
 obj-$(CONFIG_S3C24X0_SERIAL) += serial_s3c24x0.o
 obj-$(CONFIG_XILINX_UARTLITE) += serial_xuartlite.o
 obj-$(CONFIG_SANDBOX_SERIAL) += sandbox.o
diff --git a/drivers/serial/ns16550.c b/drivers/serial/ns16550.c
index 28da9dd..c6cb3eb 100644
--- a/drivers/serial/ns16550.c
+++ b/drivers/serial/ns16550.c
@@ -54,12 +54,6 @@ DECLARE_GLOBAL_DATA_PTR;
 #define CONFIG_SYS_NS16550_IER  0x00
 #endif /* CONFIG_SYS_NS16550_IER */
 
-#ifdef CONFIG_DM_SERIAL
-
-#ifndef CONFIG_SYS_NS16550_CLK
-#define CONFIG_SYS_NS16550_CLK  0
-#endif
-
 static inline void serial_out_shift(void *addr, int shift, int value)
 {
 #ifdef CONFIG_SYS_NS16550_PORT_MAPPED
@@ -94,13 +88,20 @@ static inline int serial_in_shift(void *addr, int shift)
 #endif
 }
 
+#ifdef CONFIG_DM_SERIAL
+
+#ifndef CONFIG_SYS_NS16550_CLK
+#define CONFIG_SYS_NS16550_CLK  0
+#endif
+
 static void ns16550_writeb(NS16550_t port, int offset, int value)
 {
 	struct ns16550_platdata *plat = port->plat;
 	unsigned char *addr;
 
 	offset *= 1 << plat->reg_shift;
-	addr = map_physmem(plat->base, 0, MAP_NOCACHE) + offset;
+	addr = (unsigned char *)plat->base + offset;
+
 	/*
 	 * As far as we know it doesn't make sense to support selection of
 	 * these options at run-time, so use the existing CONFIG options.
@@ -114,7 +115,7 @@ static int ns16550_readb(NS16550_t port, int offset)
 	unsigned char *addr;
 
 	offset *= 1 << plat->reg_shift;
-	addr = map_physmem(plat->base, 0, MAP_NOCACHE) + offset;
+	addr = (unsigned char *)plat->base + offset;
 
 	return serial_in_shift(addr + plat->reg_offset, plat->reg_shift);
 }
@@ -128,27 +129,13 @@ static int ns16550_readb(NS16550_t port, int offset)
 		(unsigned char *)addr - (unsigned char *)com_port)
 #endif
 
-static inline int calc_divisor(NS16550_t port, int clock, int baudrate)
+int ns16550_calc_divisor(NS16550_t port, int clock, int baudrate)
 {
 	const unsigned int mode_x_div = 16;
 
 	return DIV_ROUND_CLOSEST(clock, mode_x_div * baudrate);
 }
 
-int ns16550_calc_divisor(NS16550_t port, int clock, int baudrate)
-{
-#ifdef CONFIG_OMAP1510
-	/* If can't cleanly clock 115200 set div to 1 */
-	if ((clock == 12000000) && (baudrate == 115200)) {
-		port->osc_12m_sel = OSC_12M_SEL;  /* enable 6.5 * divisor */
-		return 1;			/* return 1 for base divisor */
-	}
-	port->osc_12m_sel = 0;			/* clear if previsouly set */
-#endif
-
-	return calc_divisor(port, clock, baudrate);
-}
-
 static void NS16550_setbrg(NS16550_t com_port, int baud_divisor)
 {
 	serial_out(UART_LCR_BKSE | UART_LCRVAL, &com_port->lcr);
@@ -271,8 +258,8 @@ static inline void _debug_uart_init(void)
 	 * feasible. The better fix is to move all users of this driver to
 	 * driver model.
 	 */
-	baud_divisor = calc_divisor(com_port, CONFIG_DEBUG_UART_CLOCK,
-				    CONFIG_BAUDRATE);
+	baud_divisor = ns16550_calc_divisor(com_port, CONFIG_DEBUG_UART_CLOCK,
+					    CONFIG_BAUDRATE);
 	serial_dout(&com_port->ier, CONFIG_SYS_NS16550_IER);
 	serial_dout(&com_port->mcr, UART_MCRVAL);
 	serial_dout(&com_port->fcr, UART_FCRVAL);
@@ -400,7 +387,12 @@ int ns16550_serial_ofdata_to_platdata(struct udevice *dev)
 	if (addr == FDT_ADDR_T_NONE)
 		return -EINVAL;
 
+#ifdef CONFIG_SYS_NS16550_PORT_MAPPED
 	plat->base = addr;
+#else
+	plat->base = (unsigned long)map_physmem(addr, 0, MAP_NOCACHE);
+#endif
+
 	plat->reg_offset = fdtdec_get_int(gd->fdt_blob, dev->of_offset,
 				     "reg-offset", 0);
 	plat->reg_shift = fdtdec_get_int(gd->fdt_blob, dev->of_offset,
diff --git a/drivers/serial/serial_meson.c b/drivers/serial/serial_meson.c
new file mode 100644
index 0000000..1b49426
--- /dev/null
+++ b/drivers/serial/serial_meson.c
@@ -0,0 +1,162 @@
+/*
+ * (C) Copyright 2016 Beniamino Galvani <b.galvani@gmail.com>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <dm.h>
+#include <errno.h>
+#include <fdtdec.h>
+#include <linux/compiler.h>
+#include <serial.h>
+
+DECLARE_GLOBAL_DATA_PTR;
+
+struct meson_uart {
+	u32 wfifo;
+	u32 rfifo;
+	u32 control;
+	u32 status;
+	u32 misc;
+};
+
+struct meson_serial_platdata {
+	struct meson_uart *reg;
+};
+
+/* AML_UART_STATUS bits */
+#define AML_UART_PARITY_ERR		BIT(16)
+#define AML_UART_FRAME_ERR		BIT(17)
+#define AML_UART_TX_FIFO_WERR		BIT(18)
+#define AML_UART_RX_EMPTY		BIT(20)
+#define AML_UART_TX_FULL		BIT(21)
+#define AML_UART_TX_EMPTY		BIT(22)
+#define AML_UART_XMIT_BUSY		BIT(25)
+#define AML_UART_ERR			(AML_UART_PARITY_ERR | \
+					 AML_UART_FRAME_ERR  | \
+					 AML_UART_TX_FIFO_WERR)
+
+/* AML_UART_CONTROL bits */
+#define AML_UART_TX_EN			BIT(12)
+#define AML_UART_RX_EN			BIT(13)
+#define AML_UART_TX_RST			BIT(22)
+#define AML_UART_RX_RST			BIT(23)
+#define AML_UART_CLR_ERR		BIT(24)
+
+static void meson_serial_init(struct meson_uart *uart)
+{
+	u32 val;
+
+	val = readl(&uart->control);
+	val |= (AML_UART_RX_RST | AML_UART_TX_RST | AML_UART_CLR_ERR);
+	writel(val, &uart->control);
+	val &= ~(AML_UART_RX_RST | AML_UART_TX_RST | AML_UART_CLR_ERR);
+	writel(val, &uart->control);
+	val |= (AML_UART_RX_EN | AML_UART_TX_EN);
+	writel(val, &uart->control);
+}
+
+static int meson_serial_probe(struct udevice *dev)
+{
+	struct meson_serial_platdata *plat = dev->platdata;
+	struct meson_uart *const uart = plat->reg;
+
+	meson_serial_init(uart);
+
+	return 0;
+}
+
+static int meson_serial_getc(struct udevice *dev)
+{
+	struct meson_serial_platdata *plat = dev->platdata;
+	struct meson_uart *const uart = plat->reg;
+
+	if (readl(&uart->status) & AML_UART_RX_EMPTY)
+		return -EAGAIN;
+
+	return readl(&uart->rfifo) & 0xff;
+}
+
+static int meson_serial_putc(struct udevice *dev, const char ch)
+{
+	struct meson_serial_platdata *plat = dev->platdata;
+	struct meson_uart *const uart = plat->reg;
+
+	if (readl(&uart->status) & AML_UART_TX_FULL)
+		return -EAGAIN;
+
+	writel(ch, &uart->wfifo);
+
+	return 0;
+}
+
+static int meson_serial_pending(struct udevice *dev, bool input)
+{
+	struct meson_serial_platdata *plat = dev->platdata;
+	struct meson_uart *const uart = plat->reg;
+	uint32_t status = readl(&uart->status);
+
+	if (input)
+		return !(status & AML_UART_RX_EMPTY);
+	else
+		return !(status & AML_UART_TX_FULL);
+}
+
+static int meson_serial_ofdata_to_platdata(struct udevice *dev)
+{
+	struct meson_serial_platdata *plat = dev->platdata;
+	fdt_addr_t addr;
+
+	addr = dev_get_addr(dev);
+	if (addr == FDT_ADDR_T_NONE)
+		return -EINVAL;
+
+	plat->reg = (struct meson_uart *)addr;
+
+	return 0;
+}
+
+static const struct dm_serial_ops meson_serial_ops = {
+	.putc = meson_serial_putc,
+	.pending = meson_serial_pending,
+	.getc = meson_serial_getc,
+};
+
+static const struct udevice_id meson_serial_ids[] = {
+	{ .compatible = "amlogic,meson-uart" },
+	{ }
+};
+
+U_BOOT_DRIVER(serial_meson) = {
+	.name		= "serial_meson",
+	.id		= UCLASS_SERIAL,
+	.of_match	= meson_serial_ids,
+	.probe		= meson_serial_probe,
+	.ops		= &meson_serial_ops,
+	.flags		= DM_FLAG_PRE_RELOC,
+	.ofdata_to_platdata = meson_serial_ofdata_to_platdata,
+	.platdata_auto_alloc_size = sizeof(struct meson_serial_platdata),
+};
+
+#ifdef CONFIG_DEBUG_UART_MESON
+
+#include <debug_uart.h>
+
+static inline void _debug_uart_init(void)
+{
+}
+
+static inline void _debug_uart_putc(int ch)
+{
+	struct meson_uart *regs = (struct meson_uart *)CONFIG_DEBUG_UART_BASE;
+
+	while (readl(&regs->status) & AML_UART_TX_FULL)
+		;
+
+	writel(ch, &regs->wfifo);
+}
+
+DEBUG_UART_FUNCS
+
+#endif
diff --git a/drivers/serial/serial_s5p.c b/drivers/serial/serial_s5p.c
index feba467..cb55c5a 100644
--- a/drivers/serial/serial_s5p.c
+++ b/drivers/serial/serial_s5p.c
@@ -17,6 +17,7 @@
 #include <asm/arch/clk.h>
 #include <asm/arch/uart.h>
 #include <serial.h>
+#include <clk.h>
 
 DECLARE_GLOBAL_DATA_PTR;
 
@@ -90,7 +91,19 @@ int s5p_serial_setbrg(struct udevice *dev, int baudrate)
 {
 	struct s5p_serial_platdata *plat = dev->platdata;
 	struct s5p_uart *const uart = plat->reg;
-	u32 uclk = get_uart_clk(plat->port_id);
+	u32 uclk;
+
+#ifdef CONFIG_CLK_EXYNOS
+	struct udevice *clk_dev;
+	u32 ret;
+
+	ret = clk_get_by_index(dev, 1, &clk_dev);
+	if (ret < 0)
+		return ret;
+	uclk = clk_get_periph_rate(clk_dev, ret);
+#else
+	uclk = get_uart_clk(plat->port_id);
+#endif
 
 	s5p_serial_baud(uart, uclk, baudrate);
 
@@ -174,8 +187,8 @@ static int s5p_serial_ofdata_to_platdata(struct udevice *dev)
 		return -EINVAL;
 
 	plat->reg = (struct s5p_uart *)addr;
-	plat->port_id = fdtdec_get_int(gd->fdt_blob, dev->of_offset, "id", -1);
-
+	plat->port_id = fdtdec_get_int(gd->fdt_blob, dev->of_offset,
+					"id", dev->seq);
 	return 0;
 }
 
diff --git a/drivers/spi/fsl_qspi.c b/drivers/spi/fsl_qspi.c
index cb8d929..75cbab2 100644
--- a/drivers/spi/fsl_qspi.c
+++ b/drivers/spi/fsl_qspi.c
@@ -44,6 +44,8 @@ DECLARE_GLOBAL_DATA_PTR;
 #define SEQID_RDEAR		11
 #define SEQID_WREAR		12
 #endif
+#define SEQID_WRAR		13
+#define SEQID_RDAR		14
 
 /* QSPI CMD */
 #define QSPI_CMD_PP		0x02	/* Page program (up to 256 bytes) */
@@ -63,6 +65,10 @@ DECLARE_GLOBAL_DATA_PTR;
 #define	QSPI_CMD_BRRD		0x16	/* Bank register read */
 #define	QSPI_CMD_BRWR		0x17	/* Bank register write */
 
+/* Used for Spansion S25FS-S family flash only. */
+#define QSPI_CMD_RDAR		0x65	/* Read any device register */
+#define QSPI_CMD_WRAR		0x71	/* Write any device register */
+
 /* 4-byte address QSPI CMD - used on Spansion and some Macronix flashes */
 #define QSPI_CMD_FAST_READ_4B	0x0c    /* Read data bytes (high frequency) */
 #define QSPI_CMD_PP_4B		0x12    /* Page program (up to 256 bytes) */
@@ -92,9 +98,9 @@ DECLARE_GLOBAL_DATA_PTR;
 struct fsl_qspi_platdata {
 	u32 flags;
 	u32 speed_hz;
-	u32 reg_base;
-	u32 amba_base;
-	u32 amba_total_size;
+	fdt_addr_t reg_base;
+	fdt_addr_t amba_base;
+	fdt_size_t amba_total_size;
 	u32 flash_num;
 	u32 num_chipselect;
 };
@@ -317,6 +323,33 @@ static void qspi_set_lut(struct fsl_qspi_priv *priv)
 		     PAD0(LUT_PAD1) | INSTR0(LUT_CMD) | OPRND1(1) |
 		     PAD1(LUT_PAD1) | INSTR1(LUT_WRITE));
 #endif
+
+	/*
+	 * Read any device register.
+	 * Used for Spansion S25FS-S family flash only.
+	 */
+	lut_base = SEQID_RDAR * 4;
+	qspi_write32(priv->flags, &regs->lut[lut_base],
+		     OPRND0(QSPI_CMD_RDAR) | PAD0(LUT_PAD1) |
+		     INSTR0(LUT_CMD) | OPRND1(ADDR24BIT) |
+		     PAD1(LUT_PAD1) | INSTR1(LUT_ADDR));
+	qspi_write32(priv->flags, &regs->lut[lut_base + 1],
+		     OPRND0(8) | PAD0(LUT_PAD1) | INSTR0(LUT_DUMMY) |
+		     OPRND1(1) | PAD1(LUT_PAD1) |
+		     INSTR1(LUT_READ));
+
+	/*
+	 * Write any device register.
+	 * Used for Spansion S25FS-S family flash only.
+	 */
+	lut_base = SEQID_WRAR * 4;
+	qspi_write32(priv->flags, &regs->lut[lut_base],
+		     OPRND0(QSPI_CMD_WRAR) | PAD0(LUT_PAD1) |
+		     INSTR0(LUT_CMD) | OPRND1(ADDR24BIT) |
+		     PAD1(LUT_PAD1) | INSTR1(LUT_ADDR));
+	qspi_write32(priv->flags, &regs->lut[lut_base + 1],
+		     OPRND0(1) | PAD0(LUT_PAD1) | INSTR0(LUT_WRITE));
+
 	/* Lock the LUT */
 	qspi_write32(priv->flags, &regs->lutkey, LUT_KEY_VALUE);
 	qspi_write32(priv->flags, &regs->lckcr, QSPI_LCKCR_LOCK);
@@ -510,7 +543,6 @@ static void qspi_op_rdid(struct fsl_qspi_priv *priv, u32 *rxbuf, u32 len)
 	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_priv *priv, u32 *rxbuf, u32 len)
 {
@@ -518,6 +550,12 @@ static void qspi_op_read(struct fsl_qspi_priv *priv, u32 *rxbuf, u32 len)
 	u32 mcr_reg, data;
 	int i, size;
 	u32 to_or_from;
+	u32 seqid;
+
+	if (priv->cur_seqid == QSPI_CMD_RDAR)
+		seqid = SEQID_RDAR;
+	else
+		seqid = SEQID_FAST_READ;
 
 	mcr_reg = qspi_read32(priv->flags, &regs->mcr);
 	qspi_write32(priv->flags, &regs->mcr,
@@ -536,7 +574,7 @@ static void qspi_op_read(struct fsl_qspi_priv *priv, u32 *rxbuf, u32 len)
 			RX_BUFFER_SIZE : len;
 
 		qspi_write32(priv->flags, &regs->ipcr,
-			     (SEQID_FAST_READ << QSPI_IPCR_SEQID_SHIFT) |
+			     (seqid << QSPI_IPCR_SEQID_SHIFT) |
 			     size);
 		while (qspi_read32(priv->flags, &regs->sr) & QSPI_SR_BUSY_MASK)
 			;
@@ -548,7 +586,10 @@ static void qspi_op_read(struct fsl_qspi_priv *priv, u32 *rxbuf, u32 len)
 		while ((RX_BUFFER_SIZE >= size) && (size > 0)) {
 			data = qspi_read32(priv->flags, &regs->rbdr[i]);
 			data = qspi_endian_xchg(data);
-			memcpy(rxbuf, &data, 4);
+			if (size < 4)
+				memcpy(rxbuf, &data, size);
+			else
+				memcpy(rxbuf, &data, 4);
 			rxbuf++;
 			size -= 4;
 			i++;
@@ -560,7 +601,6 @@ static void qspi_op_read(struct fsl_qspi_priv *priv, u32 *rxbuf, u32 len)
 
 	qspi_write32(priv->flags, &regs->mcr, mcr_reg);
 }
-#endif
 
 static void qspi_op_write(struct fsl_qspi_priv *priv, u8 *txbuf, u32 len)
 {
@@ -601,6 +641,8 @@ static void qspi_op_write(struct fsl_qspi_priv *priv, u8 *txbuf, u32 len)
 
 	/* Default is page programming */
 	seqid = SEQID_PP;
+	if (priv->cur_seqid == QSPI_CMD_WRAR)
+		seqid = SEQID_WRAR;
 #ifdef CONFIG_SPI_FLASH_BAR
 	if (priv->cur_seqid == QSPI_CMD_BRWR)
 		seqid = SEQID_BRWR;
@@ -725,13 +767,15 @@ int qspi_xfer(struct fsl_qspi_priv *priv, unsigned int bitlen,
 			return 0;
 		}
 
-		if (priv->cur_seqid == QSPI_CMD_FAST_READ) {
+		if (priv->cur_seqid == QSPI_CMD_FAST_READ ||
+		    priv->cur_seqid == QSPI_CMD_RDAR) {
 			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) {
+		} else if (priv->cur_seqid == QSPI_CMD_PP ||
+			   priv->cur_seqid == QSPI_CMD_WRAR) {
 			wr_sfaddr = swab32(txbuf) & OFFSET_BITS_MASK;
 		} else if ((priv->cur_seqid == QSPI_CMD_BRWR) ||
 			 (priv->cur_seqid == QSPI_CMD_WREAR)) {
@@ -748,6 +792,8 @@ int qspi_xfer(struct fsl_qspi_priv *priv, unsigned int bitlen,
 #else
 			qspi_op_read(priv, din, bytes);
 #endif
+		} else if (priv->cur_seqid == QSPI_CMD_RDAR) {
+			qspi_op_read(priv, din, bytes);
 		} else if (priv->cur_seqid == QSPI_CMD_RDID)
 			qspi_op_rdid(priv, din, bytes);
 		else if (priv->cur_seqid == QSPI_CMD_RDSR)
@@ -927,10 +973,11 @@ static int fsl_qspi_child_pre_probe(struct udevice *dev)
 
 static int fsl_qspi_probe(struct udevice *bus)
 {
-	u32 total_size;
+	u32 amba_size_per_chip;
 	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;
+	int i;
 
 	dm_spi_bus = bus->uclass_priv;
 
@@ -940,8 +987,13 @@ static int fsl_qspi_probe(struct udevice *bus)
 	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;
+	/*
+	 * QSPI SFADR width is 32bits, the max dest addr is 4GB-1.
+	 * AMBA memory zone should be located on the 0~4GB space
+	 * even on a 64bits cpu.
+	 */
+	priv->amba_base[0] = (u32)plat->amba_base;
+	priv->amba_total_size = (u32)plat->amba_total_size;
 	priv->flash_num = plat->flash_num;
 	priv->num_chipselect = plat->num_chipselect;
 
@@ -951,7 +1003,22 @@ static int fsl_qspi_probe(struct udevice *bus)
 	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;
+	/*
+	 * Assign AMBA memory zone for every chipselect
+	 * QuadSPI has two channels, every channel has two chipselects.
+	 * If the property 'num-cs' in dts is 2, the AMBA memory will be divided
+	 * into two parts and assign to every channel. This indicate that every
+	 * channel only has one valid chipselect.
+	 * If the property 'num-cs' in dts is 4, the AMBA memory will be divided
+	 * into four parts and assign to every chipselect.
+	 * Every channel will has two valid chipselects.
+	 */
+	amba_size_per_chip = priv->amba_total_size >>
+			     (priv->num_chipselect >> 1);
+	for (i = 1 ; i < priv->num_chipselect ; i++)
+		priv->amba_base[i] =
+			amba_size_per_chip + priv->amba_base[i - 1];
+
 	/*
 	 * Any read access to non-implemented addresses will provide
 	 * undefined results.
@@ -962,14 +1029,30 @@ static int fsl_qspi_probe(struct udevice *bus)
 	 * 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_write32(priv->flags, &priv->regs->sfa1ad, priv->amba_base[1]);
+	switch (priv->num_chipselect) {
+	case 2:
+		qspi_write32(priv->flags, &priv->regs->sfa2ad,
+			     priv->amba_base[1]);
+		qspi_write32(priv->flags, &priv->regs->sfb1ad,
+			     priv->amba_base[1] + amba_size_per_chip);
+		qspi_write32(priv->flags, &priv->regs->sfb2ad,
+			     priv->amba_base[1] + amba_size_per_chip);
+		break;
+	case 4:
+		qspi_write32(priv->flags, &priv->regs->sfa2ad,
+			     priv->amba_base[2]);
+		qspi_write32(priv->flags, &priv->regs->sfb1ad,
+			     priv->amba_base[3]);
+		qspi_write32(priv->flags, &priv->regs->sfb2ad,
+			     priv->amba_base[3] + amba_size_per_chip);
+		break;
+	default:
+		debug("Error: Unsupported chipselect number %u!\n",
+		      priv->num_chipselect);
+		qspi_module_disable(priv, 1);
+		return -EINVAL;
+	}
 
 	qspi_set_lut(priv);
 
@@ -984,10 +1067,7 @@ static int fsl_qspi_probe(struct udevice *bus)
 
 static int fsl_qspi_ofdata_to_platdata(struct udevice *bus)
 {
-	struct reg_data {
-		u32 addr;
-		u32 size;
-	} regs_data[2];
+	struct fdt_resource res_regs, res_mem;
 	struct fsl_qspi_platdata *plat = bus->platdata;
 	const void *blob = gd->fdt_blob;
 	int node = bus->of_offset;
@@ -996,10 +1076,16 @@ static int fsl_qspi_ofdata_to_platdata(struct udevice *bus)
 	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));
+	ret = fdt_get_named_resource(blob, node, "reg", "reg-names",
+				     "QuadSPI", &res_regs);
+	if (ret) {
+		debug("Error: can't get regs base addresses(ret = %d)!\n", ret);
+		return -ENOMEM;
+	}
+	ret = fdt_get_named_resource(blob, node, "reg", "reg-names",
+				     "QuadSPI-memory", &res_mem);
 	if (ret) {
-		debug("Error: can't get base addresses (ret = %d)!\n", ret);
+		debug("Error: can't get AMBA base addresses(ret = %d)!\n", ret);
 		return -ENOMEM;
 	}
 
@@ -1017,16 +1103,16 @@ static int fsl_qspi_ofdata_to_platdata(struct udevice *bus)
 	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->reg_base = res_regs.start;
+	plat->amba_base = res_mem.start;
+	plat->amba_total_size = res_mem.end - res_mem.start + 1;
 	plat->flash_num = flash_num;
 
-	debug("%s: regs=<0x%x> <0x%x, 0x%x>, max-frequency=%d, endianess=%s\n",
+	debug("%s: regs=<0x%llx> <0x%llx, 0x%llx>, max-frequency=%d, endianess=%s\n",
 	      __func__,
-	      plat->reg_base,
-	      plat->amba_base,
-	      plat->amba_total_size,
+	      (u64)plat->reg_base,
+	      (u64)plat->amba_base,
+	      (u64)plat->amba_total_size,
 	      plat->speed_hz,
 	      plat->flags & QSPI_FLAG_REGMAP_ENDIAN_BIG ? "be" : "le"
 	      );
@@ -1055,8 +1141,7 @@ static int fsl_qspi_claim_bus(struct udevice *dev)
 	bus = dev->parent;
 	priv = dev_get_priv(bus);
 
-	priv->cur_amba_base =
-		priv->amba_base[0] + FSL_QSPI_FLASH_SIZE * slave_plat->cs;
+	priv->cur_amba_base = priv->amba_base[slave_plat->cs];
 
 	qspi_module_disable(priv, 0);
 
diff --git a/drivers/usb/dwc3/Makefile b/drivers/usb/dwc3/Makefile
index 0cd7302..2964bae 100644
--- a/drivers/usb/dwc3/Makefile
+++ b/drivers/usb/dwc3/Makefile
@@ -6,7 +6,7 @@ obj-$(CONFIG_USB_DWC3)			+= dwc3.o
 
 dwc3-y					:= core.o
 
-dwc3-y					+= gadget.o ep0.o
+obj-$(CONFIG_USB_DWC3_GADGET)		+= gadget.o ep0.o
 
 obj-$(CONFIG_USB_DWC3_OMAP)		+= dwc3-omap.o
 obj-$(CONFIG_USB_DWC3_PHY_OMAP)		+= ti_usb_phy.o
diff --git a/drivers/usb/host/Kconfig b/drivers/usb/host/Kconfig
index d2363c8..89580cc 100644
--- a/drivers/usb/host/Kconfig
+++ b/drivers/usb/host/Kconfig
@@ -9,12 +9,6 @@ config USB_XHCI_HCD
 	  The eXtensible Host Controller Interface (xHCI) is standard for USB 3.0
 	  "SuperSpeed" host controller hardware.
 
-config USB_XHCI
-	bool
-	default USB_XHCI_HCD
-	---help---
-	  TODO: rename after most boards switch to Kconfig
-
 if USB_XHCI_HCD
 
 config USB_XHCI_UNIPHIER
@@ -24,6 +18,12 @@ config USB_XHCI_UNIPHIER
 	---help---
 	  Enables support for the on-chip xHCI controller on UniPhier SoCs.
 
+config USB_XHCI_DWC3
+	bool "DesignWare USB3 DRD Core Support"
+	help
+	  Say Y or if your system has a Dual Role SuperSpeed
+	  USB controller based on the DesignWare USB3 IP Core.
+
 endif
 
 config USB_OHCI_GENERIC
diff --git a/drivers/usb/host/Makefile b/drivers/usb/host/Makefile
index 507519e..620d114 100644
--- a/drivers/usb/host/Makefile
+++ b/drivers/usb/host/Makefile
@@ -54,7 +54,7 @@ obj-$(CONFIG_USB_EHCI_RMOBILE) += ehci-rmobile.o
 obj-$(CONFIG_USB_EHCI_ZYNQ) += ehci-zynq.o
 
 # xhci
-obj-$(CONFIG_USB_XHCI) += xhci.o xhci-mem.o xhci-ring.o
+obj-$(CONFIG_USB_XHCI_HCD) += xhci.o xhci-mem.o xhci-ring.o
 obj-$(CONFIG_USB_XHCI_DWC3) += xhci-dwc3.o
 obj-$(CONFIG_USB_XHCI_ZYNQMP) += xhci-zynqmp.o
 obj-$(CONFIG_USB_XHCI_KEYSTONE) += xhci-keystone.o
diff --git a/drivers/usb/phy/omap_usb_phy.c b/drivers/usb/phy/omap_usb_phy.c
index f9069c7..1993da1 100644
--- a/drivers/usb/phy/omap_usb_phy.c
+++ b/drivers/usb/phy/omap_usb_phy.c
@@ -23,7 +23,7 @@
 #include "../host/xhci.h"
 
 #ifdef CONFIG_OMAP_USB3PHY1_HOST
-struct usb_dpll_params {
+struct usb3_dpll_params {
 	u16	m;
 	u8	n;
 	u8	freq:3;
@@ -31,17 +31,39 @@ struct usb_dpll_params {
 	u32	mf;
 };
 
-#define	NUM_USB_CLKS		6
+struct usb3_dpll_map {
+	unsigned long rate;
+	struct usb3_dpll_params params;
+	struct usb3_dpll_map *dpll_map;
+};
 
-static struct usb_dpll_params omap_usb3_dpll_params[NUM_USB_CLKS] = {
-	{1250, 5, 4, 20, 0},		/* 12 MHz */
-	{3125, 20, 4, 20, 0},		/* 16.8 MHz */
-	{1172, 8, 4, 20, 65537},	/* 19.2 MHz */
-	{1250, 12, 4, 20, 0},		/* 26 MHz */
-	{3125, 47, 4, 20, 92843},	/* 38.4 MHz */
-	{1000, 7, 4, 10, 0},        /* 20 MHz */
+static struct usb3_dpll_map dpll_map_usb[] = {
+	{12000000, {1250, 5, 4, 20, 0} },	/* 12 MHz */
+	{16800000, {3125, 20, 4, 20, 0} },	/* 16.8 MHz */
+	{19200000, {1172, 8, 4, 20, 65537} },	/* 19.2 MHz */
+	{20000000, {1000, 7, 4, 10, 0} },	/* 20 MHz */
+	{26000000, {1250, 12, 4, 20, 0} },	/* 26 MHz */
+	{38400000, {3125, 47, 4, 20, 92843} },	/* 38.4 MHz */
+	{ },					/* Terminator */
 };
 
+static struct usb3_dpll_params *omap_usb3_get_dpll_params(void)
+{
+	unsigned long rate;
+	struct usb3_dpll_map *dpll_map = dpll_map_usb;
+
+	rate = get_sys_clk_freq();
+
+	for (; dpll_map->rate; dpll_map++) {
+		if (rate == dpll_map->rate)
+			return &dpll_map->params;
+	}
+
+	dev_err(phy->dev, "No DPLL configuration for %lu Hz SYS CLK\n", rate);
+
+	return NULL;
+}
+
 static void omap_usb_dpll_relock(struct omap_usb3_phy *phy_regs)
 {
 	u32 val;
@@ -56,32 +78,36 @@ static void omap_usb_dpll_relock(struct omap_usb3_phy *phy_regs)
 
 static void omap_usb_dpll_lock(struct omap_usb3_phy *phy_regs)
 {
-	u32 clk_index = get_sys_clk_index();
+	struct usb3_dpll_params	*dpll_params;
 	u32 val;
 
+	dpll_params = omap_usb3_get_dpll_params();
+	if (!dpll_params)
+		return;
+
 	val = readl(&phy_regs->pll_config_1);
 	val &= ~PLL_REGN_MASK;
-	val |= omap_usb3_dpll_params[clk_index].n << PLL_REGN_SHIFT;
+	val |= dpll_params->n << PLL_REGN_SHIFT;
 	writel(val, &phy_regs->pll_config_1);
 
 	val = readl(&phy_regs->pll_config_2);
 	val &= ~PLL_SELFREQDCO_MASK;
-	val |= omap_usb3_dpll_params[clk_index].freq << PLL_SELFREQDCO_SHIFT;
+	val |= dpll_params->freq << PLL_SELFREQDCO_SHIFT;
 	writel(val, &phy_regs->pll_config_2);
 
 	val = readl(&phy_regs->pll_config_1);
 	val &= ~PLL_REGM_MASK;
-	val |= omap_usb3_dpll_params[clk_index].m << PLL_REGM_SHIFT;
+	val |= dpll_params->m << PLL_REGM_SHIFT;
 	writel(val, &phy_regs->pll_config_1);
 
 	val = readl(&phy_regs->pll_config_4);
 	val &= ~PLL_REGM_F_MASK;
-	val |= omap_usb3_dpll_params[clk_index].mf << PLL_REGM_F_SHIFT;
+	val |= dpll_params->mf << PLL_REGM_F_SHIFT;
 	writel(val, &phy_regs->pll_config_4);
 
 	val = readl(&phy_regs->pll_config_3);
 	val &= ~PLL_SD_MASK;
-	val |= omap_usb3_dpll_params[clk_index].sd << PLL_SD_SHIFT;
+	val |= dpll_params->sd << PLL_SD_SHIFT;
 	writel(val, &phy_regs->pll_config_3);
 
 	omap_usb_dpll_relock(phy_regs);
diff --git a/drivers/video/Makefile b/drivers/video/Makefile
index 2fd0891..3f045fe 100644
--- a/drivers/video/Makefile
+++ b/drivers/video/Makefile
@@ -25,11 +25,6 @@ obj-$(CONFIG_ATI_RADEON_FB) += ati_radeon_fb.o videomodes.o
 obj-$(CONFIG_ATMEL_HLCD) += atmel_hlcdfb.o
 obj-$(CONFIG_ATMEL_LCD) += atmel_lcdfb.o
 obj-$(CONFIG_CFB_CONSOLE) += cfb_console.o
-obj-$(CONFIG_EXYNOS_DP) += exynos_dp.o exynos_dp_lowlevel.o
-obj-$(CONFIG_EXYNOS_FB) += exynos_fb.o exynos_fimd.o
-obj-$(CONFIG_EXYNOS_MIPI_DSIM) += exynos_mipi_dsi.o exynos_mipi_dsi_common.o \
-				exynos_mipi_dsi_lowlevel.o
-obj-$(CONFIG_EXYNOS_PWM_BL) += exynos_pwm_bl.o
 obj-$(CONFIG_FSL_DIU_FB) += fsl_diu_fb.o videomodes.o
 obj-$(CONFIG_FSL_DCU_FB) += fsl_dcu_fb.o videomodes.o
 obj-$(CONFIG_L5F31188) += l5f31188.o
@@ -68,6 +63,7 @@ obj-$(CONFIG_LG4573) += lg4573.o
 obj-$(CONFIG_AM335X_LCD) += am335x-fb.o
 
 obj-${CONFIG_VIDEO_TEGRA124} += tegra124/
+obj-${CONFIG_EXYNOS_FB} += exynos/
 obj-${CONFIG_VIDEO_ROCKCHIP} += rockchip/
 
 obj-y += bridge/
diff --git a/drivers/video/exynos/Makefile b/drivers/video/exynos/Makefile
new file mode 100644
index 0000000..001a80f
--- /dev/null
+++ b/drivers/video/exynos/Makefile
@@ -0,0 +1,12 @@
+#
+# (C) Copyright 2000-2007
+# Wolfgang Denk, DENX Software Engineering, wd@denx.de.
+#
+# SPDX-License-Identifier:	GPL-2.0+
+#
+
+obj-$(CONFIG_EXYNOS_DP) += exynos_dp.o exynos_dp_lowlevel.o
+obj-$(CONFIG_EXYNOS_FB) += exynos_fb.o
+obj-$(CONFIG_EXYNOS_MIPI_DSIM) += exynos_mipi_dsi.o exynos_mipi_dsi_common.o \
+				exynos_mipi_dsi_lowlevel.o
+obj-$(CONFIG_EXYNOS_PWM_BL) += exynos_pwm_bl.o
diff --git a/drivers/video/exynos_dp.c b/drivers/video/exynos/exynos_dp.c
index 0d5d090..fc39f2c 100644
--- a/drivers/video/exynos_dp.c
+++ b/drivers/video/exynos/exynos_dp.c
@@ -7,27 +7,26 @@
  */
 
 #include <config.h>
+#include <dm.h>
 #include <common.h>
+#include <display.h>
+#include <fdtdec.h>
+#include <libfdt.h>
 #include <malloc.h>
+#include <video_bridge.h>
 #include <linux/compat.h>
 #include <linux/err.h>
 #include <asm/arch/clk.h>
 #include <asm/arch/cpu.h>
 #include <asm/arch/dp_info.h>
 #include <asm/arch/dp.h>
-#include <fdtdec.h>
-#include <libfdt.h>
+#include <asm/arch/pinmux.h>
+#include <asm/arch/power.h>
 
 #include "exynos_dp_lowlevel.h"
 
 DECLARE_GLOBAL_DATA_PTR;
 
-void __exynos_set_dp_phy(unsigned int onoff)
-{
-}
-void exynos_set_dp_phy(unsigned int onoff)
-	__attribute__((weak, alias("__exynos_set_dp_phy")));
-
 static void exynos_dp_disp_info(struct edp_disp_info *disp_info)
 {
 	disp_info->h_total = disp_info->h_res + disp_info->h_sync_width +
@@ -38,20 +37,20 @@ static void exynos_dp_disp_info(struct edp_disp_info *disp_info)
 	return;
 }
 
-static int exynos_dp_init_dp(void)
+static int exynos_dp_init_dp(struct exynos_dp *regs)
 {
 	int ret;
-	exynos_dp_reset();
+	exynos_dp_reset(regs);
 
 	/* SW defined function Normal operation */
-	exynos_dp_enable_sw_func(DP_ENABLE);
+	exynos_dp_enable_sw_func(regs, DP_ENABLE);
 
-	ret = exynos_dp_init_analog_func();
+	ret = exynos_dp_init_analog_func(regs);
 	if (ret != EXYNOS_DP_SUCCESS)
 		return ret;
 
-	exynos_dp_init_hpd();
-	exynos_dp_init_aux();
+	exynos_dp_init_hpd(regs);
+	exynos_dp_init_aux(regs);
 
 	return ret;
 }
@@ -67,7 +66,7 @@ static unsigned char exynos_dp_calc_edid_check_sum(unsigned char *edid_data)
 	return sum;
 }
 
-static unsigned int exynos_dp_read_edid(void)
+static unsigned int exynos_dp_read_edid(struct exynos_dp *regs)
 {
 	unsigned char edid[EDID_BLOCK_LENGTH * 2];
 	unsigned int extend_block = 0;
@@ -82,14 +81,15 @@ static unsigned int exynos_dp_read_edid(void)
 	 */
 
 	/* Read Extension Flag, Number of 128-byte EDID extension blocks */
-	exynos_dp_read_byte_from_i2c(I2C_EDID_DEVICE_ADDR, EDID_EXTENSION_FLAG,
-			&extend_block);
+	exynos_dp_read_byte_from_i2c(regs, I2C_EDID_DEVICE_ADDR,
+				     EDID_EXTENSION_FLAG, &extend_block);
 
 	if (extend_block > 0) {
 		printf("DP EDID data includes a single extension!\n");
 
 		/* Read EDID data */
-		retval = exynos_dp_read_bytes_from_i2c(I2C_EDID_DEVICE_ADDR,
+		retval = exynos_dp_read_bytes_from_i2c(regs,
+						I2C_EDID_DEVICE_ADDR,
 						EDID_HEADER_PATTERN,
 						EDID_BLOCK_LENGTH,
 						&edid[EDID_HEADER_PATTERN]);
@@ -104,7 +104,8 @@ static unsigned int exynos_dp_read_edid(void)
 		}
 
 		/* Read additional EDID data */
-		retval = exynos_dp_read_bytes_from_i2c(I2C_EDID_DEVICE_ADDR,
+		retval = exynos_dp_read_bytes_from_i2c(regs,
+				I2C_EDID_DEVICE_ADDR,
 				EDID_BLOCK_LENGTH,
 				EDID_BLOCK_LENGTH,
 				&edid[EDID_BLOCK_LENGTH]);
@@ -118,19 +119,22 @@ static unsigned int exynos_dp_read_edid(void)
 			return -1;
 		}
 
-		exynos_dp_read_byte_from_dpcd(DPCD_TEST_REQUEST,
-					&test_vector);
+		exynos_dp_read_byte_from_dpcd(regs, DPCD_TEST_REQUEST,
+					      &test_vector);
 		if (test_vector & DPCD_TEST_EDID_READ) {
-			exynos_dp_write_byte_to_dpcd(DPCD_TEST_EDID_CHECKSUM,
+			exynos_dp_write_byte_to_dpcd(regs,
+				DPCD_TEST_EDID_CHECKSUM,
 				edid[EDID_BLOCK_LENGTH + EDID_CHECKSUM]);
-			exynos_dp_write_byte_to_dpcd(DPCD_TEST_RESPONSE,
+			exynos_dp_write_byte_to_dpcd(regs,
+				DPCD_TEST_RESPONSE,
 				DPCD_TEST_EDID_CHECKSUM_WRITE);
 		}
 	} else {
 		debug("DP EDID data does not include any extensions.\n");
 
 		/* Read EDID data */
-		retval = exynos_dp_read_bytes_from_i2c(I2C_EDID_DEVICE_ADDR,
+		retval = exynos_dp_read_bytes_from_i2c(regs,
+				I2C_EDID_DEVICE_ADDR,
 				EDID_HEADER_PATTERN,
 				EDID_BLOCK_LENGTH,
 				&edid[EDID_HEADER_PATTERN]);
@@ -145,12 +149,13 @@ static unsigned int exynos_dp_read_edid(void)
 			return -1;
 		}
 
-		exynos_dp_read_byte_from_dpcd(DPCD_TEST_REQUEST,
+		exynos_dp_read_byte_from_dpcd(regs, DPCD_TEST_REQUEST,
 			&test_vector);
 		if (test_vector & DPCD_TEST_EDID_READ) {
-			exynos_dp_write_byte_to_dpcd(DPCD_TEST_EDID_CHECKSUM,
-				edid[EDID_CHECKSUM]);
-			exynos_dp_write_byte_to_dpcd(DPCD_TEST_RESPONSE,
+			exynos_dp_write_byte_to_dpcd(regs,
+				DPCD_TEST_EDID_CHECKSUM, edid[EDID_CHECKSUM]);
+			exynos_dp_write_byte_to_dpcd(regs,
+				DPCD_TEST_RESPONSE,
 				DPCD_TEST_EDID_CHECKSUM_WRITE);
 		}
 	}
@@ -160,7 +165,8 @@ static unsigned int exynos_dp_read_edid(void)
 	return 0;
 }
 
-static unsigned int exynos_dp_handle_edid(struct edp_device_info *edp_info)
+static unsigned int exynos_dp_handle_edid(struct exynos_dp *regs,
+					  struct exynos_dp_priv *priv)
 {
 	unsigned char buf[12];
 	unsigned int ret;
@@ -178,8 +184,8 @@ static unsigned int exynos_dp_handle_edid(struct edp_device_info *edp_info)
 	retry_cnt = 5;
 	while (retry_cnt) {
 		/* Read DPCD 0x0000-0x000b */
-		ret = exynos_dp_read_bytes_from_dpcd(DPCD_DPCD_REV, 12,
-				buf);
+		ret = exynos_dp_read_bytes_from_dpcd(regs, DPCD_DPCD_REV, 12,
+						     buf);
 		if (ret != EXYNOS_DP_SUCCESS) {
 			if (retry_cnt == 0) {
 				printf("DP read_byte_from_dpcd() failed\n");
@@ -193,7 +199,7 @@ static unsigned int exynos_dp_handle_edid(struct edp_device_info *edp_info)
 	/* */
 	temp = buf[DPCD_DPCD_REV];
 	if (temp == DP_DPCD_REV_10 || temp == DP_DPCD_REV_11)
-		edp_info->dpcd_rev = temp;
+		priv->dpcd_rev = temp;
 	else {
 		printf("DP Wrong DPCD Rev : %x\n", temp);
 		return -ENODEV;
@@ -201,33 +207,33 @@ static unsigned int exynos_dp_handle_edid(struct edp_device_info *edp_info)
 
 	temp = buf[DPCD_MAX_LINK_RATE];
 	if (temp == DP_LANE_BW_1_62 || temp == DP_LANE_BW_2_70)
-		edp_info->lane_bw = temp;
+		priv->lane_bw = temp;
 	else {
 		printf("DP Wrong MAX LINK RATE : %x\n", temp);
 		return -EINVAL;
 	}
 
 	/* Refer VESA Display Port Standard Ver1.1a Page 120 */
-	if (edp_info->dpcd_rev == DP_DPCD_REV_11) {
+	if (priv->dpcd_rev == DP_DPCD_REV_11) {
 		temp = buf[DPCD_MAX_LANE_COUNT] & 0x1f;
 		if (buf[DPCD_MAX_LANE_COUNT] & 0x80)
-			edp_info->dpcd_efc = 1;
+			priv->dpcd_efc = 1;
 		else
-			edp_info->dpcd_efc = 0;
+			priv->dpcd_efc = 0;
 	} else {
 		temp = buf[DPCD_MAX_LANE_COUNT];
-		edp_info->dpcd_efc = 0;
+		priv->dpcd_efc = 0;
 	}
 
 	if (temp == DP_LANE_CNT_1 || temp == DP_LANE_CNT_2 ||
 			temp == DP_LANE_CNT_4) {
-		edp_info->lane_cnt = temp;
+		priv->lane_cnt = temp;
 	} else {
 		printf("DP Wrong MAX LANE COUNT : %x\n", temp);
 		return -EINVAL;
 	}
 
-	ret = exynos_dp_read_edid();
+	ret = exynos_dp_read_edid(regs);
 	if (ret != EXYNOS_DP_SUCCESS) {
 		printf("DP exynos_dp_read_edid() failed\n");
 		return -EINVAL;
@@ -236,60 +242,60 @@ static unsigned int exynos_dp_handle_edid(struct edp_device_info *edp_info)
 	return ret;
 }
 
-static void exynos_dp_init_training(void)
+static void exynos_dp_init_training(struct exynos_dp *regs)
 {
 	/*
 	 * MACRO_RST must be applied after the PLL_LOCK to avoid
 	 * the DP inter pair skew issue for at least 10 us
 	 */
-	exynos_dp_reset_macro();
+	exynos_dp_reset_macro(regs);
 
 	/* All DP analog module power up */
-	exynos_dp_set_analog_power_down(POWER_ALL, 0);
+	exynos_dp_set_analog_power_down(regs, POWER_ALL, 0);
 }
 
-static unsigned int exynos_dp_link_start(struct edp_device_info *edp_info)
+static unsigned int exynos_dp_link_start(struct exynos_dp *regs,
+					 struct exynos_dp_priv *priv)
 {
 	unsigned char buf[5];
 	unsigned int ret = 0;
 
 	debug("DP: %s was called\n", __func__);
 
-	edp_info->lt_info.lt_status = DP_LT_CR;
-	edp_info->lt_info.ep_loop = 0;
-	edp_info->lt_info.cr_loop[0] = 0;
-	edp_info->lt_info.cr_loop[1] = 0;
-	edp_info->lt_info.cr_loop[2] = 0;
-	edp_info->lt_info.cr_loop[3] = 0;
+	priv->lt_info.lt_status = DP_LT_CR;
+	priv->lt_info.ep_loop = 0;
+	priv->lt_info.cr_loop[0] = 0;
+	priv->lt_info.cr_loop[1] = 0;
+	priv->lt_info.cr_loop[2] = 0;
+	priv->lt_info.cr_loop[3] = 0;
 
 		/* Set sink to D0 (Sink Not Ready) mode. */
-		ret = exynos_dp_write_byte_to_dpcd(DPCD_SINK_POWER_STATE,
-				DPCD_SET_POWER_STATE_D0);
+	ret = exynos_dp_write_byte_to_dpcd(regs, DPCD_SINK_POWER_STATE,
+					   DPCD_SET_POWER_STATE_D0);
 	if (ret != EXYNOS_DP_SUCCESS) {
 		printf("DP write_dpcd_byte failed\n");
 		return ret;
 	}
 
 	/* Set link rate and count as you want to establish */
-	exynos_dp_set_link_bandwidth(edp_info->lane_bw);
-	exynos_dp_set_lane_count(edp_info->lane_cnt);
+	exynos_dp_set_link_bandwidth(regs, priv->lane_bw);
+	exynos_dp_set_lane_count(regs, priv->lane_cnt);
 
 	/* Setup RX configuration */
-	buf[0] = edp_info->lane_bw;
-	buf[1] = edp_info->lane_cnt;
+	buf[0] = priv->lane_bw;
+	buf[1] = priv->lane_cnt;
 
-	ret = exynos_dp_write_bytes_to_dpcd(DPCD_LINK_BW_SET, 2,
-			buf);
+	ret = exynos_dp_write_bytes_to_dpcd(regs, DPCD_LINK_BW_SET, 2, buf);
 	if (ret != EXYNOS_DP_SUCCESS) {
 		printf("DP write_dpcd_byte failed\n");
 		return ret;
 	}
 
-	exynos_dp_set_lane_pre_emphasis(PRE_EMPHASIS_LEVEL_0,
-			edp_info->lane_cnt);
+	exynos_dp_set_lane_pre_emphasis(regs, PRE_EMPHASIS_LEVEL_0,
+			priv->lane_cnt);
 
 	/* Set training pattern 1 */
-	exynos_dp_set_training_pattern(TRAINING_PTN1);
+	exynos_dp_set_training_pattern(regs, TRAINING_PTN1);
 
 	/* Set RX training pattern */
 	buf[0] = DPCD_SCRAMBLING_DISABLED | DPCD_TRAINING_PATTERN_1;
@@ -303,8 +309,8 @@ static unsigned int exynos_dp_link_start(struct edp_device_info *edp_info)
 	buf[4] = DPCD_PRE_EMPHASIS_SET_PATTERN_2_LEVEL_0 |
 		DPCD_VOLTAGE_SWING_SET_PATTERN_1_LEVEL_0;
 
-	ret = exynos_dp_write_bytes_to_dpcd(DPCD_TRAINING_PATTERN_SET,
-			5, buf);
+	ret = exynos_dp_write_bytes_to_dpcd(regs, DPCD_TRAINING_PATTERN_SET,
+					    5, buf);
 	if (ret != EXYNOS_DP_SUCCESS) {
 		printf("DP write_dpcd_byte failed\n");
 		return ret;
@@ -313,14 +319,14 @@ static unsigned int exynos_dp_link_start(struct edp_device_info *edp_info)
 	return ret;
 }
 
-static unsigned int exynos_dp_training_pattern_dis(void)
+static unsigned int exynos_dp_training_pattern_dis(struct exynos_dp *regs)
 {
 	unsigned int ret = EXYNOS_DP_SUCCESS;
 
-	exynos_dp_set_training_pattern(DP_NONE);
+	exynos_dp_set_training_pattern(regs, DP_NONE);
 
-	ret = exynos_dp_write_byte_to_dpcd(DPCD_TRAINING_PATTERN_SET,
-			DPCD_TRAINING_PATTERN_DISABLED);
+	ret = exynos_dp_write_byte_to_dpcd(regs, DPCD_TRAINING_PATTERN_SET,
+					   DPCD_TRAINING_PATTERN_DISABLED);
 	if (ret != EXYNOS_DP_SUCCESS) {
 		printf("DP request_link_training_req failed\n");
 		return -EAGAIN;
@@ -329,13 +335,14 @@ static unsigned int exynos_dp_training_pattern_dis(void)
 	return ret;
 }
 
-static unsigned int exynos_dp_enable_rx_to_enhanced_mode(unsigned char enable)
+static unsigned int exynos_dp_enable_rx_to_enhanced_mode(
+		struct exynos_dp *regs, unsigned char enable)
 {
 	unsigned char data;
 	unsigned int ret = EXYNOS_DP_SUCCESS;
 
-	ret = exynos_dp_read_byte_from_dpcd(DPCD_LANE_COUNT_SET,
-			&data);
+	ret = exynos_dp_read_byte_from_dpcd(regs, DPCD_LANE_COUNT_SET,
+					    &data);
 	if (ret != EXYNOS_DP_SUCCESS) {
 		printf("DP read_from_dpcd failed\n");
 		return -EAGAIN;
@@ -346,8 +353,7 @@ static unsigned int exynos_dp_enable_rx_to_enhanced_mode(unsigned char enable)
 	else
 		data = DPCD_LN_COUNT_SET(data);
 
-	ret = exynos_dp_write_byte_to_dpcd(DPCD_LANE_COUNT_SET,
-			data);
+	ret = exynos_dp_write_byte_to_dpcd(regs, DPCD_LANE_COUNT_SET, data);
 	if (ret != EXYNOS_DP_SUCCESS) {
 			printf("DP write_to_dpcd failed\n");
 			return -EAGAIN;
@@ -357,23 +363,25 @@ static unsigned int exynos_dp_enable_rx_to_enhanced_mode(unsigned char enable)
 	return ret;
 }
 
-static unsigned int exynos_dp_set_enhanced_mode(unsigned char enhance_mode)
+static unsigned int exynos_dp_set_enhanced_mode(struct exynos_dp *regs,
+						unsigned char enhance_mode)
 {
 	unsigned int ret = EXYNOS_DP_SUCCESS;
 
-	ret = exynos_dp_enable_rx_to_enhanced_mode(enhance_mode);
+	ret = exynos_dp_enable_rx_to_enhanced_mode(regs, enhance_mode);
 	if (ret != EXYNOS_DP_SUCCESS) {
 		printf("DP rx_enhance_mode failed\n");
 		return -EAGAIN;
 	}
 
-	exynos_dp_enable_enhanced_mode(enhance_mode);
+	exynos_dp_enable_enhanced_mode(regs, enhance_mode);
 
 	return ret;
 }
 
-static int exynos_dp_read_dpcd_lane_stat(struct edp_device_info *edp_info,
-		unsigned char *status)
+static int exynos_dp_read_dpcd_lane_stat(struct exynos_dp *regs,
+					 struct exynos_dp_priv *priv,
+					 unsigned char *status)
 {
 	unsigned int ret, i;
 	unsigned char buf[2];
@@ -385,13 +393,14 @@ static int exynos_dp_read_dpcd_lane_stat(struct edp_device_info *edp_info,
 	shift_val[2] = 0;
 	shift_val[3] = 4;
 
-	ret = exynos_dp_read_bytes_from_dpcd(DPCD_LANE0_1_STATUS, 2, buf);
+	ret = exynos_dp_read_bytes_from_dpcd(regs, DPCD_LANE0_1_STATUS, 2,
+					     buf);
 	if (ret != EXYNOS_DP_SUCCESS) {
 		printf("DP read lane status failed\n");
 		return ret;
 	}
 
-	for (i = 0; i < edp_info->lane_cnt; i++) {
+	for (i = 0; i < priv->lane_cnt; i++) {
 		lane_stat[i] = (buf[(i / 2)] >> shift_val[i]) & 0x0f;
 		if (lane_stat[0] != lane_stat[i]) {
 			printf("Wrong lane status\n");
@@ -404,8 +413,8 @@ static int exynos_dp_read_dpcd_lane_stat(struct edp_device_info *edp_info,
 	return ret;
 }
 
-static unsigned int exynos_dp_read_dpcd_adj_req(unsigned char lane_num,
-		unsigned char *sw, unsigned char *em)
+static unsigned int exynos_dp_read_dpcd_adj_req(struct exynos_dp *regs,
+		unsigned char lane_num, unsigned char *sw, unsigned char *em)
 {
 	unsigned int ret = EXYNOS_DP_SUCCESS;
 	unsigned char buf;
@@ -415,7 +424,7 @@ static unsigned int exynos_dp_read_dpcd_adj_req(unsigned char lane_num,
 	/* lane_num value is used as array index, so this range 0 ~ 3 */
 	dpcd_addr = DPCD_ADJUST_REQUEST_LANE0_1 + (lane_num / 2);
 
-	ret = exynos_dp_read_byte_from_dpcd(dpcd_addr, &buf);
+	ret = exynos_dp_read_byte_from_dpcd(regs, dpcd_addr, &buf);
 	if (ret != EXYNOS_DP_SUCCESS) {
 		printf("DP read adjust request failed\n");
 		return -EAGAIN;
@@ -427,51 +436,53 @@ static unsigned int exynos_dp_read_dpcd_adj_req(unsigned char lane_num,
 	return ret;
 }
 
-static int exynos_dp_equalizer_err_link(struct edp_device_info *edp_info)
+static int exynos_dp_equalizer_err_link(struct exynos_dp *regs,
+					struct exynos_dp_priv *priv)
 {
 	int ret;
 
-	ret = exynos_dp_training_pattern_dis();
+	ret = exynos_dp_training_pattern_dis(regs);
 	if (ret != EXYNOS_DP_SUCCESS) {
 		printf("DP training_pattern_disable() failed\n");
-		edp_info->lt_info.lt_status = DP_LT_FAIL;
+		priv->lt_info.lt_status = DP_LT_FAIL;
 	}
 
-	ret = exynos_dp_set_enhanced_mode(edp_info->dpcd_efc);
+	ret = exynos_dp_set_enhanced_mode(regs, priv->dpcd_efc);
 	if (ret != EXYNOS_DP_SUCCESS) {
 		printf("DP set_enhanced_mode() failed\n");
-		edp_info->lt_info.lt_status = DP_LT_FAIL;
+		priv->lt_info.lt_status = DP_LT_FAIL;
 	}
 
 	return ret;
 }
 
-static int exynos_dp_reduce_link_rate(struct edp_device_info *edp_info)
+static int exynos_dp_reduce_link_rate(struct exynos_dp *regs,
+				      struct exynos_dp_priv *priv)
 {
 	int ret;
 
-	if (edp_info->lane_bw == DP_LANE_BW_2_70) {
-		edp_info->lane_bw = DP_LANE_BW_1_62;
+	if (priv->lane_bw == DP_LANE_BW_2_70) {
+		priv->lane_bw = DP_LANE_BW_1_62;
 		printf("DP Change lane bw to 1.62Gbps\n");
-		edp_info->lt_info.lt_status = DP_LT_START;
+		priv->lt_info.lt_status = DP_LT_START;
 		ret = EXYNOS_DP_SUCCESS;
 	} else {
-		ret = exynos_dp_training_pattern_dis();
+		ret = exynos_dp_training_pattern_dis(regs);
 		if (ret != EXYNOS_DP_SUCCESS)
 			printf("DP training_patter_disable() failed\n");
 
-		ret = exynos_dp_set_enhanced_mode(edp_info->dpcd_efc);
+		ret = exynos_dp_set_enhanced_mode(regs, priv->dpcd_efc);
 		if (ret != EXYNOS_DP_SUCCESS)
 			printf("DP set_enhanced_mode() failed\n");
 
-		edp_info->lt_info.lt_status = DP_LT_FAIL;
+		priv->lt_info.lt_status = DP_LT_FAIL;
 	}
 
 	return ret;
 }
 
-static unsigned int exynos_dp_process_clock_recovery(struct edp_device_info
-							*edp_info)
+static unsigned int exynos_dp_process_clock_recovery(struct exynos_dp *regs,
+					struct exynos_dp_priv *priv)
 {
 	unsigned int ret = EXYNOS_DP_SUCCESS;
 	unsigned char lane_stat;
@@ -484,22 +495,22 @@ static unsigned int exynos_dp_process_clock_recovery(struct edp_device_info
 	debug("DP: %s was called\n", __func__);
 	mdelay(1);
 
-	ret = exynos_dp_read_dpcd_lane_stat(edp_info, &lane_stat);
+	ret = exynos_dp_read_dpcd_lane_stat(regs, priv, &lane_stat);
 	if (ret != EXYNOS_DP_SUCCESS) {
 			printf("DP read lane status failed\n");
-			edp_info->lt_info.lt_status = DP_LT_FAIL;
+			priv->lt_info.lt_status = DP_LT_FAIL;
 			return ret;
 	}
 
 	if (lane_stat & DP_LANE_STAT_CR_DONE) {
 		debug("DP clock Recovery training succeed\n");
-		exynos_dp_set_training_pattern(TRAINING_PTN2);
+		exynos_dp_set_training_pattern(regs, TRAINING_PTN2);
 
-		for (i = 0; i < edp_info->lane_cnt; i++) {
-			ret = exynos_dp_read_dpcd_adj_req(i, &adj_req_sw,
-					&adj_req_em);
+		for (i = 0; i < priv->lane_cnt; i++) {
+			ret = exynos_dp_read_dpcd_adj_req(regs, i,
+						&adj_req_sw, &adj_req_em);
 			if (ret != EXYNOS_DP_SUCCESS) {
-				edp_info->lt_info.lt_status = DP_LT_FAIL;
+				priv->lt_info.lt_status = DP_LT_FAIL;
 				return ret;
 			}
 
@@ -511,7 +522,8 @@ static unsigned int exynos_dp_process_clock_recovery(struct edp_device_info
 				lt_ctl_val[i] |= MAX_DRIVE_CURRENT_REACH_3 |
 					MAX_PRE_EMPHASIS_REACH_3;
 			}
-			exynos_dp_set_lanex_pre_emphasis(lt_ctl_val[i], i);
+			exynos_dp_set_lanex_pre_emphasis(regs,
+							 lt_ctl_val[i], i);
 		}
 
 		buf[0] =  DPCD_SCRAMBLING_DISABLED | DPCD_TRAINING_PATTERN_2;
@@ -520,37 +532,39 @@ static unsigned int exynos_dp_process_clock_recovery(struct edp_device_info
 		buf[3] = lt_ctl_val[2];
 		buf[4] = lt_ctl_val[3];
 
-		ret = exynos_dp_write_bytes_to_dpcd(
+		ret = exynos_dp_write_bytes_to_dpcd(regs,
 				DPCD_TRAINING_PATTERN_SET, 5, buf);
 		if (ret != EXYNOS_DP_SUCCESS) {
 			printf("DP write training pattern1 failed\n");
-			edp_info->lt_info.lt_status = DP_LT_FAIL;
+			priv->lt_info.lt_status = DP_LT_FAIL;
 			return ret;
 		} else
-			edp_info->lt_info.lt_status = DP_LT_ET;
+			priv->lt_info.lt_status = DP_LT_ET;
 	} else {
-		for (i = 0; i < edp_info->lane_cnt; i++) {
-			lt_ctl_val[i] = exynos_dp_get_lanex_pre_emphasis(i);
-				ret = exynos_dp_read_dpcd_adj_req(i,
+		for (i = 0; i < priv->lane_cnt; i++) {
+			lt_ctl_val[i] = exynos_dp_get_lanex_pre_emphasis(
+						regs, i);
+				ret = exynos_dp_read_dpcd_adj_req(regs, i,
 						&adj_req_sw, &adj_req_em);
 			if (ret != EXYNOS_DP_SUCCESS) {
 				printf("DP read adj req failed\n");
-				edp_info->lt_info.lt_status = DP_LT_FAIL;
+				priv->lt_info.lt_status = DP_LT_FAIL;
 				return ret;
 			}
 
 			if ((adj_req_sw == VOLTAGE_LEVEL_3) ||
 					(adj_req_em == PRE_EMPHASIS_LEVEL_3))
-				ret = exynos_dp_reduce_link_rate(edp_info);
+				ret = exynos_dp_reduce_link_rate(regs,
+								 priv);
 
 			if ((DRIVE_CURRENT_SET_0_GET(lt_ctl_val[i]) ==
 						adj_req_sw) &&
 				(PRE_EMPHASIS_SET_0_GET(lt_ctl_val[i]) ==
 						adj_req_em)) {
-				edp_info->lt_info.cr_loop[i]++;
-				if (edp_info->lt_info.cr_loop[i] == MAX_CR_LOOP)
+				priv->lt_info.cr_loop[i]++;
+				if (priv->lt_info.cr_loop[i] == MAX_CR_LOOP)
 					ret = exynos_dp_reduce_link_rate(
-							edp_info);
+							regs, priv);
 			}
 
 			lt_ctl_val[i] = 0;
@@ -561,14 +575,15 @@ static unsigned int exynos_dp_process_clock_recovery(struct edp_device_info
 				lt_ctl_val[i] |= MAX_DRIVE_CURRENT_REACH_3 |
 					MAX_PRE_EMPHASIS_REACH_3;
 			}
-			exynos_dp_set_lanex_pre_emphasis(lt_ctl_val[i], i);
+			exynos_dp_set_lanex_pre_emphasis(regs,
+							 lt_ctl_val[i], i);
 		}
 
-		ret = exynos_dp_write_bytes_to_dpcd(
+		ret = exynos_dp_write_bytes_to_dpcd(regs,
 				DPCD_TRAINING_LANE0_SET, 4, lt_ctl_val);
 		if (ret != EXYNOS_DP_SUCCESS) {
 			printf("DP write training pattern2 failed\n");
-			edp_info->lt_info.lt_status = DP_LT_FAIL;
+			priv->lt_info.lt_status = DP_LT_FAIL;
 			return ret;
 		}
 	}
@@ -576,8 +591,8 @@ static unsigned int exynos_dp_process_clock_recovery(struct edp_device_info
 	return ret;
 }
 
-static unsigned int exynos_dp_process_equalizer_training(struct edp_device_info
-		*edp_info)
+static unsigned int exynos_dp_process_equalizer_training(
+		struct exynos_dp *regs, struct exynos_dp_priv *priv)
 {
 	unsigned int ret = EXYNOS_DP_SUCCESS;
 	unsigned char lane_stat, adj_req_sw, adj_req_em, i;
@@ -589,32 +604,33 @@ static unsigned int exynos_dp_process_equalizer_training(struct edp_device_info
 
 	mdelay(1);
 
-	ret = exynos_dp_read_dpcd_lane_stat(edp_info, &lane_stat);
+	ret = exynos_dp_read_dpcd_lane_stat(regs, priv, &lane_stat);
 	if (ret != EXYNOS_DP_SUCCESS) {
 		printf("DP read lane status failed\n");
-		edp_info->lt_info.lt_status = DP_LT_FAIL;
+		priv->lt_info.lt_status = DP_LT_FAIL;
 		return ret;
 	}
 
 	debug("DP lane stat : %x\n", lane_stat);
 
 	if (lane_stat & DP_LANE_STAT_CR_DONE) {
-		ret = exynos_dp_read_byte_from_dpcd(DPCD_LN_ALIGN_UPDATED,
-				&sink_stat);
+		ret = exynos_dp_read_byte_from_dpcd(regs,
+						    DPCD_LN_ALIGN_UPDATED,
+						    &sink_stat);
 		if (ret != EXYNOS_DP_SUCCESS) {
-			edp_info->lt_info.lt_status = DP_LT_FAIL;
+			priv->lt_info.lt_status = DP_LT_FAIL;
 
 			return ret;
 		}
 
 		interlane_aligned = (sink_stat & DPCD_INTERLANE_ALIGN_DONE);
 
-		for (i = 0; i < edp_info->lane_cnt; i++) {
-			ret = exynos_dp_read_dpcd_adj_req(i,
+		for (i = 0; i < priv->lane_cnt; i++) {
+			ret = exynos_dp_read_dpcd_adj_req(regs, i,
 					&adj_req_sw, &adj_req_em);
 			if (ret != EXYNOS_DP_SUCCESS) {
 				printf("DP read adj req 1 failed\n");
-				edp_info->lt_info.lt_status = DP_LT_FAIL;
+				priv->lt_info.lt_status = DP_LT_FAIL;
 
 				return ret;
 			}
@@ -634,86 +650,91 @@ static unsigned int exynos_dp_process_equalizer_training(struct edp_device_info
 			&& (interlane_aligned == DPCD_INTERLANE_ALIGN_DONE)) {
 			debug("DP Equalizer training succeed\n");
 
-			f_bw = exynos_dp_get_link_bandwidth();
-			f_lane_cnt = exynos_dp_get_lane_count();
+			f_bw = exynos_dp_get_link_bandwidth(regs);
+			f_lane_cnt = exynos_dp_get_lane_count(regs);
 
 			debug("DP final BandWidth : %x\n", f_bw);
 			debug("DP final Lane Count : %x\n", f_lane_cnt);
 
-			edp_info->lt_info.lt_status = DP_LT_FINISHED;
+			priv->lt_info.lt_status = DP_LT_FINISHED;
 
-			exynos_dp_equalizer_err_link(edp_info);
+			exynos_dp_equalizer_err_link(regs, priv);
 
 		} else {
-			edp_info->lt_info.ep_loop++;
+			priv->lt_info.ep_loop++;
 
-			if (edp_info->lt_info.ep_loop > MAX_EQ_LOOP) {
-				if (edp_info->lane_bw == DP_LANE_BW_2_70) {
+			if (priv->lt_info.ep_loop > MAX_EQ_LOOP) {
+				if (priv->lane_bw == DP_LANE_BW_2_70) {
 					ret = exynos_dp_reduce_link_rate(
-							edp_info);
+							regs, priv);
 				} else {
-					edp_info->lt_info.lt_status =
+					priv->lt_info.lt_status =
 								DP_LT_FAIL;
-					exynos_dp_equalizer_err_link(edp_info);
+					exynos_dp_equalizer_err_link(regs,
+								     priv);
 				}
 			} else {
-				for (i = 0; i < edp_info->lane_cnt; i++)
+				for (i = 0; i < priv->lane_cnt; i++)
 					exynos_dp_set_lanex_pre_emphasis(
-							lt_ctl_val[i], i);
+						regs, lt_ctl_val[i], i);
 
-				ret = exynos_dp_write_bytes_to_dpcd(
-					DPCD_TRAINING_LANE0_SET,
-					4, lt_ctl_val);
+				ret = exynos_dp_write_bytes_to_dpcd(regs,
+						DPCD_TRAINING_LANE0_SET,
+						4, lt_ctl_val);
 				if (ret != EXYNOS_DP_SUCCESS) {
 					printf("DP set lt pattern failed\n");
-					edp_info->lt_info.lt_status =
+					priv->lt_info.lt_status =
 								DP_LT_FAIL;
-					exynos_dp_equalizer_err_link(edp_info);
+					exynos_dp_equalizer_err_link(regs,
+								     priv);
 				}
 			}
 		}
-	} else if (edp_info->lane_bw == DP_LANE_BW_2_70) {
-		ret = exynos_dp_reduce_link_rate(edp_info);
+	} else if (priv->lane_bw == DP_LANE_BW_2_70) {
+		ret = exynos_dp_reduce_link_rate(regs, priv);
 	} else {
-		edp_info->lt_info.lt_status = DP_LT_FAIL;
-		exynos_dp_equalizer_err_link(edp_info);
+		priv->lt_info.lt_status = DP_LT_FAIL;
+		exynos_dp_equalizer_err_link(regs, priv);
 	}
 
 	return ret;
 }
 
-static unsigned int exynos_dp_sw_link_training(struct edp_device_info *edp_info)
+static unsigned int exynos_dp_sw_link_training(struct exynos_dp *regs,
+					       struct exynos_dp_priv *priv)
 {
 	unsigned int ret = 0;
 	int training_finished;
 
 	/* Turn off unnecessary lane */
-	if (edp_info->lane_cnt == 1)
-		exynos_dp_set_analog_power_down(CH1_BLOCK, 1);
+	if (priv->lane_cnt == 1)
+		exynos_dp_set_analog_power_down(regs, CH1_BLOCK, 1);
 
 	training_finished = 0;
 
-	edp_info->lt_info.lt_status = DP_LT_START;
+	priv->lt_info.lt_status = DP_LT_START;
 
 	/* Process here */
 	while (!training_finished) {
-		switch (edp_info->lt_info.lt_status) {
+		switch (priv->lt_info.lt_status) {
 		case DP_LT_START:
-			ret = exynos_dp_link_start(edp_info);
+			ret = exynos_dp_link_start(regs, priv);
 			if (ret != EXYNOS_DP_SUCCESS) {
 				printf("DP LT:link start failed\n");
 				return ret;
 			}
 			break;
 		case DP_LT_CR:
-			ret = exynos_dp_process_clock_recovery(edp_info);
+			ret = exynos_dp_process_clock_recovery(regs,
+							       priv);
 			if (ret != EXYNOS_DP_SUCCESS) {
 				printf("DP LT:clock recovery failed\n");
 				return ret;
 			}
 			break;
 		case DP_LT_ET:
-			ret = exynos_dp_process_equalizer_training(edp_info);
+			ret = exynos_dp_process_equalizer_training(regs,
+								   priv);
 			if (ret != EXYNOS_DP_SUCCESS) {
 				printf("DP LT:equalizer training failed\n");
 				return ret;
@@ -730,71 +751,75 @@ static unsigned int exynos_dp_sw_link_training(struct edp_device_info *edp_info)
 	return ret;
 }
 
-static unsigned int exynos_dp_set_link_train(struct edp_device_info *edp_info)
+static unsigned int exynos_dp_set_link_train(struct exynos_dp *regs,
+					     struct exynos_dp_priv *priv)
 {
 	unsigned int ret;
 
-	exynos_dp_init_training();
+	exynos_dp_init_training(regs);
 
-	ret = exynos_dp_sw_link_training(edp_info);
+	ret = exynos_dp_sw_link_training(regs, priv);
 	if (ret != EXYNOS_DP_SUCCESS)
 		printf("DP dp_sw_link_training() failed\n");
 
 	return ret;
 }
 
-static void exynos_dp_enable_scramble(unsigned int enable)
+static void exynos_dp_enable_scramble(struct exynos_dp *regs,
+				      unsigned int enable)
 {
 	unsigned char data;
 
 	if (enable) {
-		exynos_dp_enable_scrambling(DP_ENABLE);
+		exynos_dp_enable_scrambling(regs, DP_ENABLE);
 
-		exynos_dp_read_byte_from_dpcd(DPCD_TRAINING_PATTERN_SET,
-				&data);
-		exynos_dp_write_byte_to_dpcd(DPCD_TRAINING_PATTERN_SET,
+		exynos_dp_read_byte_from_dpcd(regs,
+					      DPCD_TRAINING_PATTERN_SET, &data);
+		exynos_dp_write_byte_to_dpcd(regs, DPCD_TRAINING_PATTERN_SET,
 			(u8)(data & ~DPCD_SCRAMBLING_DISABLED));
 	} else {
-		exynos_dp_enable_scrambling(DP_DISABLE);
-		exynos_dp_read_byte_from_dpcd(DPCD_TRAINING_PATTERN_SET,
-				&data);
-		exynos_dp_write_byte_to_dpcd(DPCD_TRAINING_PATTERN_SET,
+		exynos_dp_enable_scrambling(regs, DP_DISABLE);
+		exynos_dp_read_byte_from_dpcd(regs,
+					      DPCD_TRAINING_PATTERN_SET, &data);
+		exynos_dp_write_byte_to_dpcd(regs, DPCD_TRAINING_PATTERN_SET,
 			(u8)(data | DPCD_SCRAMBLING_DISABLED));
 	}
 }
 
-static unsigned int exynos_dp_config_video(struct edp_device_info *edp_info)
+static unsigned int exynos_dp_config_video(struct exynos_dp *regs,
+					   struct exynos_dp_priv *priv)
 {
 	unsigned int ret = 0;
 	unsigned int retry_cnt;
 
 	mdelay(1);
 
-	if (edp_info->video_info.master_mode) {
+	if (priv->video_info.master_mode) {
 		printf("DP does not support master mode\n");
 		return -ENODEV;
 	} else {
 		/* debug slave */
-		exynos_dp_config_video_slave_mode(&edp_info->video_info);
+		exynos_dp_config_video_slave_mode(regs,
+						  &priv->video_info);
 	}
 
-	exynos_dp_set_video_color_format(&edp_info->video_info);
+	exynos_dp_set_video_color_format(regs, &priv->video_info);
 
-	if (edp_info->video_info.bist_mode) {
-		if (exynos_dp_config_video_bist(edp_info) != 0)
+	if (priv->video_info.bist_mode) {
+		if (exynos_dp_config_video_bist(regs, priv) != 0)
 			return -1;
 	}
 
-	ret = exynos_dp_get_pll_lock_status();
+	ret = exynos_dp_get_pll_lock_status(regs);
 	if (ret != PLL_LOCKED) {
 		printf("DP PLL is not locked yet\n");
 		return -EIO;
 	}
 
-	if (edp_info->video_info.master_mode == 0) {
+	if (priv->video_info.master_mode == 0) {
 		retry_cnt = 10;
 		while (retry_cnt) {
-			ret = exynos_dp_is_slave_video_stream_clock_on();
+			ret = exynos_dp_is_slave_video_stream_clock_on(regs);
 			if (ret != EXYNOS_DP_SUCCESS) {
 				if (retry_cnt == 0) {
 					printf("DP stream_clock_on failed\n");
@@ -808,32 +833,34 @@ static unsigned int exynos_dp_config_video(struct edp_device_info *edp_info)
 	}
 
 	/* Set to use the register calculated M/N video */
-	exynos_dp_set_video_cr_mn(CALCULATED_M, 0, 0);
+	exynos_dp_set_video_cr_mn(regs, CALCULATED_M, 0, 0);
 
 	/* For video bist, Video timing must be generated by register */
-	exynos_dp_set_video_timing_mode(VIDEO_TIMING_FROM_CAPTURE);
+	exynos_dp_set_video_timing_mode(regs, VIDEO_TIMING_FROM_CAPTURE);
 
 	/* Enable video bist */
-	if (edp_info->video_info.bist_pattern != COLOR_RAMP &&
-		edp_info->video_info.bist_pattern != BALCK_WHITE_V_LINES &&
-		edp_info->video_info.bist_pattern != COLOR_SQUARE)
-		exynos_dp_enable_video_bist(edp_info->video_info.bist_mode);
+	if (priv->video_info.bist_pattern != COLOR_RAMP &&
+		priv->video_info.bist_pattern != BALCK_WHITE_V_LINES &&
+		priv->video_info.bist_pattern != COLOR_SQUARE)
+		exynos_dp_enable_video_bist(regs,
+					    priv->video_info.bist_mode);
 	else
-		exynos_dp_enable_video_bist(DP_DISABLE);
+		exynos_dp_enable_video_bist(regs, DP_DISABLE);
 
 	/* Disable video mute */
-	exynos_dp_enable_video_mute(DP_DISABLE);
+	exynos_dp_enable_video_mute(regs, DP_DISABLE);
 
 	/* Configure video Master or Slave mode */
-	exynos_dp_enable_video_master(edp_info->video_info.master_mode);
+	exynos_dp_enable_video_master(regs,
+				      priv->video_info.master_mode);
 
 	/* Enable video */
-	exynos_dp_start_video();
+	exynos_dp_start_video(regs);
 
-	if (edp_info->video_info.master_mode == 0) {
+	if (priv->video_info.master_mode == 0) {
 		retry_cnt = 100;
 		while (retry_cnt) {
-			ret = exynos_dp_is_video_stream_on();
+			ret = exynos_dp_is_video_stream_on(regs);
 			if (ret != EXYNOS_DP_SUCCESS) {
 				if (retry_cnt == 0) {
 					printf("DP Timeout of video stream\n");
@@ -849,107 +876,184 @@ static unsigned int exynos_dp_config_video(struct edp_device_info *edp_info)
 	return ret;
 }
 
-int exynos_dp_parse_dt(const void *blob, struct edp_device_info *edp_info)
+static int exynos_dp_ofdata_to_platdata(struct udevice *dev)
 {
-	unsigned int node = fdtdec_next_compatible(blob, 0,
-						COMPAT_SAMSUNG_EXYNOS5_DP);
-	if (node <= 0) {
-		debug("exynos_dp: Can't get device node for dp\n");
-		return -ENODEV;
+	struct exynos_dp_priv *priv = dev_get_priv(dev);
+	const void *blob = gd->fdt_blob;
+	unsigned int node = dev->of_offset;
+	fdt_addr_t addr;
+
+	addr = dev_get_addr(dev);
+	if (addr == FDT_ADDR_T_NONE) {
+		debug("Can't get the DP base address\n");
+		return -EINVAL;
 	}
-
-	edp_info->disp_info.h_res = fdtdec_get_int(blob, node,
+	priv->regs = (struct exynos_dp *)addr;
+	priv->disp_info.h_res = fdtdec_get_int(blob, node,
 							"samsung,h-res", 0);
-	edp_info->disp_info.h_sync_width = fdtdec_get_int(blob, node,
+	priv->disp_info.h_sync_width = fdtdec_get_int(blob, node,
 						"samsung,h-sync-width", 0);
-	edp_info->disp_info.h_back_porch = fdtdec_get_int(blob, node,
+	priv->disp_info.h_back_porch = fdtdec_get_int(blob, node,
 						"samsung,h-back-porch", 0);
-	edp_info->disp_info.h_front_porch = fdtdec_get_int(blob, node,
+	priv->disp_info.h_front_porch = fdtdec_get_int(blob, node,
 						"samsung,h-front-porch", 0);
-	edp_info->disp_info.v_res = fdtdec_get_int(blob, node,
+	priv->disp_info.v_res = fdtdec_get_int(blob, node,
 						"samsung,v-res", 0);
-	edp_info->disp_info.v_sync_width = fdtdec_get_int(blob, node,
+	priv->disp_info.v_sync_width = fdtdec_get_int(blob, node,
 						"samsung,v-sync-width", 0);
-	edp_info->disp_info.v_back_porch = fdtdec_get_int(blob, node,
+	priv->disp_info.v_back_porch = fdtdec_get_int(blob, node,
 						"samsung,v-back-porch", 0);
-	edp_info->disp_info.v_front_porch = fdtdec_get_int(blob, node,
+	priv->disp_info.v_front_porch = fdtdec_get_int(blob, node,
 						"samsung,v-front-porch", 0);
-	edp_info->disp_info.v_sync_rate = fdtdec_get_int(blob, node,
+	priv->disp_info.v_sync_rate = fdtdec_get_int(blob, node,
 						"samsung,v-sync-rate", 0);
 
-	edp_info->lt_info.lt_status = fdtdec_get_int(blob, node,
+	priv->lt_info.lt_status = fdtdec_get_int(blob, node,
 						"samsung,lt-status", 0);
 
-	edp_info->video_info.master_mode = fdtdec_get_int(blob, node,
+	priv->video_info.master_mode = fdtdec_get_int(blob, node,
 						"samsung,master-mode", 0);
-	edp_info->video_info.bist_mode = fdtdec_get_int(blob, node,
+	priv->video_info.bist_mode = fdtdec_get_int(blob, node,
 						"samsung,bist-mode", 0);
-	edp_info->video_info.bist_pattern = fdtdec_get_int(blob, node,
+	priv->video_info.bist_pattern = fdtdec_get_int(blob, node,
 						"samsung,bist-pattern", 0);
-	edp_info->video_info.h_sync_polarity = fdtdec_get_int(blob, node,
+	priv->video_info.h_sync_polarity = fdtdec_get_int(blob, node,
 						"samsung,h-sync-polarity", 0);
-	edp_info->video_info.v_sync_polarity = fdtdec_get_int(blob, node,
+	priv->video_info.v_sync_polarity = fdtdec_get_int(blob, node,
 						"samsung,v-sync-polarity", 0);
-	edp_info->video_info.interlaced = fdtdec_get_int(blob, node,
+	priv->video_info.interlaced = fdtdec_get_int(blob, node,
 						"samsung,interlaced", 0);
-	edp_info->video_info.color_space = fdtdec_get_int(blob, node,
+	priv->video_info.color_space = fdtdec_get_int(blob, node,
 						"samsung,color-space", 0);
-	edp_info->video_info.dynamic_range = fdtdec_get_int(blob, node,
+	priv->video_info.dynamic_range = fdtdec_get_int(blob, node,
 						"samsung,dynamic-range", 0);
-	edp_info->video_info.ycbcr_coeff = fdtdec_get_int(blob, node,
+	priv->video_info.ycbcr_coeff = fdtdec_get_int(blob, node,
 						"samsung,ycbcr-coeff", 0);
-	edp_info->video_info.color_depth = fdtdec_get_int(blob, node,
+	priv->video_info.color_depth = fdtdec_get_int(blob, node,
 						"samsung,color-depth", 0);
 	return 0;
 }
 
-unsigned int exynos_init_dp(void)
+static int exynos_dp_bridge_init(struct udevice *dev)
 {
-	unsigned int ret;
-	struct edp_device_info *edp_info;
+	const int max_tries = 10;
+	int num_tries;
+	int ret;
+
+	debug("%s\n", __func__);
+	ret = video_bridge_attach(dev);
+	if (ret) {
+		debug("video bridge init failed: %d\n", ret);
+		return ret;
+	}
+
+	/*
+	 * We need to wait for 90ms after bringing up the bridge since there
+	 * is a phantom "high" on the HPD chip during its bootup.  The phantom
+	 * high comes within 7ms of de-asserting PD and persists for at least
+	 * 15ms.  The real high comes roughly 50ms after PD is de-asserted. The
+	 * phantom high makes it hard for us to know when the NXP chip is up.
+	 */
+	mdelay(90);
+
+	for (num_tries = 0; num_tries < max_tries; num_tries++) {
+		/* Check HPD. If it's high, or we don't have it, all is well */
+		ret = video_bridge_check_attached(dev);
+		if (!ret || ret == -ENOENT)
+			return 0;
+
+		debug("%s: eDP bridge failed to come up; try %d of %d\n",
+		      __func__, num_tries, max_tries);
+	}
+
+	/* Immediately go into bridge reset if the hp line is not high */
+	return -EIO;
+}
+
+static int exynos_dp_bridge_setup(const void *blob)
+{
+	const int max_tries = 2;
+	int num_tries;
+	struct udevice *dev;
+	int ret;
 
-	edp_info = kzalloc(sizeof(struct edp_device_info), GFP_KERNEL);
-	if (!edp_info) {
-		debug("failed to allocate edp device object.\n");
-		return -EFAULT;
+	/* Configure I2C registers for Parade bridge */
+	ret = uclass_get_device(UCLASS_VIDEO_BRIDGE, 0, &dev);
+	if (ret) {
+		debug("video bridge init failed: %d\n", ret);
+		return ret;
 	}
 
-	if (exynos_dp_parse_dt(gd->fdt_blob, edp_info))
-		debug("unable to parse DP DT node\n");
+	if (strncmp(dev->driver->name, "parade", 6)) {
+		/* Mux HPHPD to the special hotplug detect mode */
+		exynos_pinmux_config(PERIPH_ID_DPHPD, 0);
+	}
 
-	exynos_dp_set_base_addr();
+	for (num_tries = 0; num_tries < max_tries; num_tries++) {
+		ret = exynos_dp_bridge_init(dev);
+		if (!ret)
+			return 0;
+		if (num_tries == max_tries - 1)
+			break;
 
-	exynos_dp_disp_info(&edp_info->disp_info);
+		/*
+		* If we're here, the bridge chip failed to initialise.
+		* Power down the bridge in an attempt to reset.
+		*/
+		video_bridge_set_active(dev, false);
+
+		/*
+		* Arbitrarily wait 300ms here with DP_N low.  Don't know for
+		* sure how long we should wait, but we're being paranoid.
+		*/
+		mdelay(300);
+	}
 
-	exynos_set_dp_phy(1);
+	return ret;
+}
+int exynos_dp_enable(struct udevice *dev, int panel_bpp,
+		     const struct display_timing *timing)
+{
+	struct exynos_dp_priv *priv = dev_get_priv(dev);
+	struct exynos_dp *regs = priv->regs;
+	unsigned int ret;
+
+	debug("%s: start\n", __func__);
+	exynos_dp_disp_info(&priv->disp_info);
+
+	ret = exynos_dp_bridge_setup(gd->fdt_blob);
+	if (ret && ret != -ENODEV)
+		printf("LCD bridge failed to enable: %d\n", ret);
 
-	ret = exynos_dp_init_dp();
+	exynos_dp_phy_ctrl(1);
+
+	ret = exynos_dp_init_dp(regs);
 	if (ret != EXYNOS_DP_SUCCESS) {
 		printf("DP exynos_dp_init_dp() failed\n");
 		return ret;
 	}
 
-	ret = exynos_dp_handle_edid(edp_info);
+	ret = exynos_dp_handle_edid(regs, priv);
 	if (ret != EXYNOS_DP_SUCCESS) {
 		printf("EDP handle_edid fail\n");
 		return ret;
 	}
 
-	ret = exynos_dp_set_link_train(edp_info);
+	ret = exynos_dp_set_link_train(regs, priv);
 	if (ret != EXYNOS_DP_SUCCESS) {
 		printf("DP link training fail\n");
 		return ret;
 	}
 
-	exynos_dp_enable_scramble(DP_ENABLE);
-	exynos_dp_enable_rx_to_enhanced_mode(DP_ENABLE);
-	exynos_dp_enable_enhanced_mode(DP_ENABLE);
+	exynos_dp_enable_scramble(regs, DP_ENABLE);
+	exynos_dp_enable_rx_to_enhanced_mode(regs, DP_ENABLE);
+	exynos_dp_enable_enhanced_mode(regs, DP_ENABLE);
 
-	exynos_dp_set_link_bandwidth(edp_info->lane_bw);
-	exynos_dp_set_lane_count(edp_info->lane_cnt);
+	exynos_dp_set_link_bandwidth(regs, priv->lane_bw);
+	exynos_dp_set_lane_count(regs, priv->lane_cnt);
 
-	exynos_dp_init_video();
-	ret = exynos_dp_config_video(edp_info);
+	exynos_dp_init_video(regs);
+	ret = exynos_dp_config_video(regs, priv);
 	if (ret != EXYNOS_DP_SUCCESS) {
 		printf("Exynos DP init failed\n");
 		return ret;
@@ -959,3 +1063,22 @@ unsigned int exynos_init_dp(void)
 
 	return ret;
 }
+
+
+static const struct dm_display_ops exynos_dp_ops = {
+	.enable = exynos_dp_enable,
+};
+
+static const struct udevice_id exynos_dp_ids[] = {
+	{ .compatible = "samsung,exynos5-dp" },
+	{ }
+};
+
+U_BOOT_DRIVER(exynos_dp) = {
+	.name	= "eexynos_dp",
+	.id	= UCLASS_DISPLAY,
+	.of_match = exynos_dp_ids,
+	.ops	= &exynos_dp_ops,
+	.ofdata_to_platdata	= exynos_dp_ofdata_to_platdata,
+	.priv_auto_alloc_size	= sizeof(struct exynos_dp_priv),
+};
diff --git a/drivers/video/exynos_dp_lowlevel.c b/drivers/video/exynos/exynos_dp_lowlevel.c
index acb5bc8..f978473 100644
--- a/drivers/video/exynos_dp_lowlevel.c
+++ b/drivers/video/exynos/exynos_dp_lowlevel.c
@@ -14,30 +14,13 @@
 #include <asm/arch/dp.h>
 #include <fdtdec.h>
 #include <libfdt.h>
+#include "exynos_dp_lowlevel.h"
 
 /* Declare global data pointer */
 DECLARE_GLOBAL_DATA_PTR;
 
-struct exynos_dp *dp_regs;
-
-void exynos_dp_set_base_addr(void)
-{
-#if CONFIG_IS_ENABLED(OF_CONTROL)
-	unsigned int node = fdtdec_next_compatible(gd->fdt_blob,
-					0, COMPAT_SAMSUNG_EXYNOS5_DP);
-	if (node <= 0)
-		debug("exynos_dp: Can't get device node for dp\n");
-
-	dp_regs = (struct exynos_dp *)fdtdec_get_addr(gd->fdt_blob,
-								node, "reg");
-	if (dp_regs == NULL)
-		debug("Can't get the DP base address\n");
-#else
-	dp_regs = (struct exynos_dp *)samsung_get_base_dp();
-#endif
-}
-
-static void exynos_dp_enable_video_input(unsigned int enable)
+static void exynos_dp_enable_video_input(struct exynos_dp *dp_regs,
+					 unsigned int enable)
 {
 	unsigned int reg;
 
@@ -53,7 +36,7 @@ static void exynos_dp_enable_video_input(unsigned int enable)
 	return;
 }
 
-void exynos_dp_enable_video_bist(unsigned int enable)
+void exynos_dp_enable_video_bist(struct exynos_dp *dp_regs, unsigned int enable)
 {
 	/* enable video bist */
 	unsigned int reg;
@@ -70,7 +53,7 @@ void exynos_dp_enable_video_bist(unsigned int enable)
 	return;
 }
 
-void exynos_dp_enable_video_mute(unsigned int enable)
+void exynos_dp_enable_video_mute(struct exynos_dp *dp_regs, unsigned int enable)
 {
 	unsigned int reg;
 
@@ -85,7 +68,7 @@ void exynos_dp_enable_video_mute(unsigned int enable)
 }
 
 
-static void exynos_dp_init_analog_param(void)
+static void exynos_dp_init_analog_param(struct exynos_dp *dp_regs)
 {
 	unsigned int reg;
 
@@ -134,7 +117,7 @@ static void exynos_dp_init_analog_param(void)
 	writel(reg, &dp_regs->pll_ctl);
 }
 
-static void exynos_dp_init_interrupt(void)
+static void exynos_dp_init_interrupt(struct exynos_dp *dp_regs)
 {
 	/* Set interrupt registers to initial states */
 
@@ -161,16 +144,16 @@ static void exynos_dp_init_interrupt(void)
 	writel(0x00, &dp_regs->int_sta_mask);
 }
 
-void exynos_dp_reset(void)
+void exynos_dp_reset(struct exynos_dp *dp_regs)
 {
 	unsigned int reg_func_1;
 
 	/* dp tx sw reset */
 	writel(RESET_DP_TX, &dp_regs->tx_sw_reset);
 
-	exynos_dp_enable_video_input(DP_DISABLE);
-	exynos_dp_enable_video_bist(DP_DISABLE);
-	exynos_dp_enable_video_mute(DP_DISABLE);
+	exynos_dp_enable_video_input(dp_regs, DP_DISABLE);
+	exynos_dp_enable_video_bist(dp_regs, DP_DISABLE);
+	exynos_dp_enable_video_mute(dp_regs, DP_DISABLE);
 
 	/* software reset */
 	reg_func_1 = MASTER_VID_FUNC_EN_N | SLAVE_VID_FUNC_EN_N |
@@ -182,13 +165,13 @@ void exynos_dp_reset(void)
 
 	mdelay(1);
 
-	exynos_dp_init_analog_param();
-	exynos_dp_init_interrupt();
+	exynos_dp_init_analog_param(dp_regs);
+	exynos_dp_init_interrupt(dp_regs);
 
 	return;
 }
 
-void exynos_dp_enable_sw_func(unsigned int enable)
+void exynos_dp_enable_sw_func(struct exynos_dp *dp_regs, unsigned int enable)
 {
 	unsigned int reg;
 
@@ -203,7 +186,8 @@ void exynos_dp_enable_sw_func(unsigned int enable)
 	return;
 }
 
-unsigned int exynos_dp_set_analog_power_down(unsigned int block, u32 enable)
+unsigned int exynos_dp_set_analog_power_down(struct exynos_dp *dp_regs,
+					     unsigned int block, u32 enable)
 {
 	unsigned int reg;
 
@@ -256,7 +240,7 @@ unsigned int exynos_dp_set_analog_power_down(unsigned int block, u32 enable)
 	return 0;
 }
 
-unsigned int exynos_dp_get_pll_lock_status(void)
+unsigned int exynos_dp_get_pll_lock_status(struct exynos_dp *dp_regs)
 {
 	unsigned int reg;
 
@@ -268,7 +252,8 @@ unsigned int exynos_dp_get_pll_lock_status(void)
 		return PLL_UNLOCKED;
 }
 
-static void exynos_dp_set_pll_power(unsigned int enable)
+static void exynos_dp_set_pll_power(struct exynos_dp *dp_regs,
+				    unsigned int enable)
 {
 	unsigned int reg;
 
@@ -281,14 +266,14 @@ static void exynos_dp_set_pll_power(unsigned int enable)
 	writel(reg, &dp_regs->pll_ctl);
 }
 
-int exynos_dp_init_analog_func(void)
+int exynos_dp_init_analog_func(struct exynos_dp *dp_regs)
 {
 	int ret = EXYNOS_DP_SUCCESS;
 	unsigned int retry_cnt = 10;
 	unsigned int reg;
 
 	/* Power On All Analog block */
-	exynos_dp_set_analog_power_down(POWER_ALL, DP_DISABLE);
+	exynos_dp_set_analog_power_down(dp_regs, POWER_ALL, DP_DISABLE);
 
 	reg = PLL_LOCK_CHG;
 	writel(reg, &dp_regs->common_int_sta1);
@@ -309,9 +294,9 @@ int exynos_dp_init_analog_func(void)
 	reg &= ~(DP_PLL_RESET);
 	writel(reg, &dp_regs->pll_ctl);
 
-	exynos_dp_set_pll_power(DP_ENABLE);
+	exynos_dp_set_pll_power(dp_regs, DP_ENABLE);
 
-	while (exynos_dp_get_pll_lock_status() == PLL_UNLOCKED) {
+	while (exynos_dp_get_pll_lock_status(dp_regs) == PLL_UNLOCKED) {
 		mdelay(1);
 		retry_cnt--;
 		if (retry_cnt == 0) {
@@ -332,7 +317,7 @@ int exynos_dp_init_analog_func(void)
 	return ret;
 }
 
-void exynos_dp_init_hpd(void)
+void exynos_dp_init_hpd(struct exynos_dp *dp_regs)
 {
 	unsigned int reg;
 
@@ -350,7 +335,7 @@ void exynos_dp_init_hpd(void)
 	return;
 }
 
-static inline void exynos_dp_reset_aux(void)
+static inline void exynos_dp_reset_aux(struct exynos_dp *dp_regs)
 {
 	unsigned int reg;
 
@@ -362,7 +347,7 @@ static inline void exynos_dp_reset_aux(void)
 	return;
 }
 
-void exynos_dp_init_aux(void)
+void exynos_dp_init_aux(struct exynos_dp *dp_regs)
 {
 	unsigned int reg;
 
@@ -370,7 +355,7 @@ void exynos_dp_init_aux(void)
 	reg = RPLY_RECEIV | AUX_ERR;
 	writel(reg, &dp_regs->int_sta);
 
-	exynos_dp_reset_aux();
+	exynos_dp_reset_aux(dp_regs);
 
 	/* Disable AUX transaction H/W retry */
 	reg = AUX_BIT_PERIOD_EXPECTED_DELAY(3) | AUX_HW_RETRY_COUNT_SEL(3)|
@@ -389,7 +374,7 @@ void exynos_dp_init_aux(void)
 	return;
 }
 
-void exynos_dp_config_interrupt(void)
+void exynos_dp_config_interrupt(struct exynos_dp *dp_regs)
 {
 	unsigned int reg;
 
@@ -412,7 +397,7 @@ void exynos_dp_config_interrupt(void)
 	return;
 }
 
-unsigned int exynos_dp_get_plug_in_status(void)
+unsigned int exynos_dp_get_plug_in_status(struct exynos_dp *dp_regs)
 {
 	unsigned int reg;
 
@@ -423,13 +408,13 @@ unsigned int exynos_dp_get_plug_in_status(void)
 	return -1;
 }
 
-unsigned int exynos_dp_detect_hpd(void)
+unsigned int exynos_dp_detect_hpd(struct exynos_dp *dp_regs)
 {
 	int timeout_loop = DP_TIMEOUT_LOOP_COUNT;
 
 	mdelay(2);
 
-	while (exynos_dp_get_plug_in_status() != 0) {
+	while (exynos_dp_get_plug_in_status(dp_regs) != 0) {
 		if (timeout_loop == 0)
 			return -EINVAL;
 		mdelay(10);
@@ -439,7 +424,7 @@ unsigned int exynos_dp_detect_hpd(void)
 	return EXYNOS_DP_SUCCESS;
 }
 
-unsigned int exynos_dp_start_aux_transaction(void)
+unsigned int exynos_dp_start_aux_transaction(struct exynos_dp *dp_regs)
 {
 	unsigned int reg;
 	unsigned int ret = 0;
@@ -488,8 +473,9 @@ unsigned int exynos_dp_start_aux_transaction(void)
 	return EXYNOS_DP_SUCCESS;
 }
 
-unsigned int exynos_dp_write_byte_to_dpcd(unsigned int reg_addr,
-				unsigned char data)
+unsigned int exynos_dp_write_byte_to_dpcd(struct exynos_dp *dp_regs,
+					  unsigned int reg_addr,
+					  unsigned char data)
 {
 	unsigned int reg, ret;
 
@@ -518,7 +504,7 @@ unsigned int exynos_dp_write_byte_to_dpcd(unsigned int reg_addr,
 	writel(reg, &dp_regs->aux_ch_ctl1);
 
 	/* Start AUX transaction */
-	ret = exynos_dp_start_aux_transaction();
+	ret = exynos_dp_start_aux_transaction(dp_regs);
 	if (ret != EXYNOS_DP_SUCCESS) {
 		printf("DP Aux transaction failed\n");
 		return ret;
@@ -527,8 +513,9 @@ unsigned int exynos_dp_write_byte_to_dpcd(unsigned int reg_addr,
 	return ret;
 }
 
-unsigned int exynos_dp_read_byte_from_dpcd(unsigned int reg_addr,
-		unsigned char *data)
+unsigned int exynos_dp_read_byte_from_dpcd(struct exynos_dp *dp_regs,
+					   unsigned int reg_addr,
+					   unsigned char *data)
 {
 	unsigned int reg;
 	int retval;
@@ -554,7 +541,7 @@ unsigned int exynos_dp_read_byte_from_dpcd(unsigned int reg_addr,
 	writel(reg, &dp_regs->aux_ch_ctl1);
 
 	/* Start AUX transaction */
-	retval = exynos_dp_start_aux_transaction();
+	retval = exynos_dp_start_aux_transaction(dp_regs);
 	if (!retval)
 		debug("DP Aux Transaction fail!\n");
 
@@ -565,9 +552,10 @@ unsigned int exynos_dp_read_byte_from_dpcd(unsigned int reg_addr,
 	return retval;
 }
 
-unsigned int exynos_dp_write_bytes_to_dpcd(unsigned int reg_addr,
-				unsigned int count,
-				unsigned char data[])
+unsigned int exynos_dp_write_bytes_to_dpcd(struct exynos_dp *dp_regs,
+					   unsigned int reg_addr,
+					   unsigned int count,
+					   unsigned char data[])
 {
 	unsigned int reg;
 	unsigned int start_offset;
@@ -614,7 +602,7 @@ unsigned int exynos_dp_write_bytes_to_dpcd(unsigned int reg_addr,
 			writel(reg, &dp_regs->aux_ch_ctl1);
 
 			/* Start AUX transaction */
-			ret = exynos_dp_start_aux_transaction();
+			ret = exynos_dp_start_aux_transaction(dp_regs);
 			if (ret != EXYNOS_DP_SUCCESS) {
 				if (retry_cnt == 0) {
 					printf("DP Aux Transaction failed\n");
@@ -630,9 +618,10 @@ unsigned int exynos_dp_write_bytes_to_dpcd(unsigned int reg_addr,
 	return ret;
 }
 
-unsigned int exynos_dp_read_bytes_from_dpcd(unsigned int reg_addr,
-				unsigned int count,
-				unsigned char data[])
+unsigned int exynos_dp_read_bytes_from_dpcd(struct exynos_dp *dp_regs,
+					    unsigned int reg_addr,
+					    unsigned int count,
+					    unsigned char data[])
 {
 	unsigned int reg;
 	unsigned int start_offset;
@@ -672,7 +661,7 @@ unsigned int exynos_dp_read_bytes_from_dpcd(unsigned int reg_addr,
 			writel(reg, &dp_regs->aux_ch_ctl1);
 
 			/* Start AUX transaction */
-			ret = exynos_dp_start_aux_transaction();
+			ret = exynos_dp_start_aux_transaction(dp_regs);
 			if (ret != EXYNOS_DP_SUCCESS) {
 				if (retry_cnt == 0) {
 					printf("DP Aux Transaction failed\n");
@@ -696,8 +685,8 @@ unsigned int exynos_dp_read_bytes_from_dpcd(unsigned int reg_addr,
 	return ret;
 }
 
-int exynos_dp_select_i2c_device(unsigned int device_addr,
-				unsigned int reg_addr)
+int exynos_dp_select_i2c_device(struct exynos_dp *dp_regs,
+				unsigned int device_addr, unsigned int reg_addr)
 {
 	unsigned int reg;
 	int retval;
@@ -721,16 +710,16 @@ int exynos_dp_select_i2c_device(unsigned int device_addr,
 	writel(reg, &dp_regs->aux_ch_ctl1);
 
 	/* Start AUX transaction */
-	retval = exynos_dp_start_aux_transaction();
+	retval = exynos_dp_start_aux_transaction(dp_regs);
 	if (retval != 0)
 		printf("%s: DP Aux Transaction fail!\n", __func__);
 
 	return retval;
 }
 
-int exynos_dp_read_byte_from_i2c(unsigned int device_addr,
-				unsigned int reg_addr,
-				unsigned int *data)
+int exynos_dp_read_byte_from_i2c(struct exynos_dp *dp_regs,
+				 unsigned int device_addr,
+				 unsigned int reg_addr, unsigned int *data)
 {
 	unsigned int reg;
 	int i;
@@ -742,7 +731,8 @@ int exynos_dp_read_byte_from_i2c(unsigned int device_addr,
 		writel(reg, &dp_regs->buffer_data_ctl);
 
 		/* Select EDID device */
-		retval = exynos_dp_select_i2c_device(device_addr, reg_addr);
+		retval = exynos_dp_select_i2c_device(dp_regs, device_addr,
+						     reg_addr);
 		if (retval != 0) {
 			printf("DP Select EDID device fail. retry !\n");
 			continue;
@@ -758,7 +748,7 @@ int exynos_dp_read_byte_from_i2c(unsigned int device_addr,
 		writel(reg, &dp_regs->aux_ch_ctl1);
 
 		/* Start AUX transaction */
-		retval = exynos_dp_start_aux_transaction();
+		retval = exynos_dp_start_aux_transaction(dp_regs);
 		if (retval != EXYNOS_DP_SUCCESS)
 			printf("%s: DP Aux Transaction fail!\n", __func__);
 	}
@@ -770,8 +760,10 @@ int exynos_dp_read_byte_from_i2c(unsigned int device_addr,
 	return retval;
 }
 
-int exynos_dp_read_bytes_from_i2c(unsigned int device_addr,
-		unsigned int reg_addr, unsigned int count, unsigned char edid[])
+int exynos_dp_read_bytes_from_i2c(struct exynos_dp *dp_regs,
+				  unsigned int device_addr,
+				  unsigned int reg_addr, unsigned int count,
+				  unsigned char edid[])
 {
 	unsigned int reg;
 	unsigned int i, j;
@@ -795,9 +787,8 @@ int exynos_dp_read_bytes_from_i2c(unsigned int device_addr,
 			 * request without sending addres
 			 */
 			if (!defer)
-				retval =
-					exynos_dp_select_i2c_device(device_addr,
-							reg_addr + i);
+				retval = exynos_dp_select_i2c_device(
+					dp_regs, device_addr, reg_addr + i);
 			else
 				defer = 0;
 
@@ -813,7 +804,8 @@ int exynos_dp_read_bytes_from_i2c(unsigned int device_addr,
 				writel(reg, &dp_regs->aux_ch_ctl1);
 
 				/* Start AUX transaction */
-				retval = exynos_dp_start_aux_transaction();
+				retval = exynos_dp_start_aux_transaction(
+							dp_regs);
 				if (retval == 0)
 					break;
 				else
@@ -838,7 +830,7 @@ int exynos_dp_read_bytes_from_i2c(unsigned int device_addr,
 	return retval;
 }
 
-void exynos_dp_reset_macro(void)
+void exynos_dp_reset_macro(struct exynos_dp *dp_regs)
 {
 	unsigned int reg;
 
@@ -853,7 +845,8 @@ void exynos_dp_reset_macro(void)
 	writel(reg, &dp_regs->phy_test);
 }
 
-void exynos_dp_set_link_bandwidth(unsigned char bwtype)
+void exynos_dp_set_link_bandwidth(struct exynos_dp *dp_regs,
+				  unsigned char bwtype)
 {
 	unsigned int reg;
 
@@ -864,7 +857,7 @@ void exynos_dp_set_link_bandwidth(unsigned char bwtype)
 		writel(reg, &dp_regs->link_bw_set);
 }
 
-unsigned char exynos_dp_get_link_bandwidth(void)
+unsigned char exynos_dp_get_link_bandwidth(struct exynos_dp *dp_regs)
 {
 	unsigned char ret;
 	unsigned int reg;
@@ -875,7 +868,7 @@ unsigned char exynos_dp_get_link_bandwidth(void)
 	return ret;
 }
 
-void exynos_dp_set_lane_count(unsigned char count)
+void exynos_dp_set_lane_count(struct exynos_dp *dp_regs, unsigned char count)
 {
 	unsigned int reg;
 
@@ -886,7 +879,7 @@ void exynos_dp_set_lane_count(unsigned char count)
 		writel(reg, &dp_regs->lane_count_set);
 }
 
-unsigned int exynos_dp_get_lane_count(void)
+unsigned int exynos_dp_get_lane_count(struct exynos_dp *dp_regs)
 {
 	unsigned int reg;
 
@@ -895,7 +888,8 @@ unsigned int exynos_dp_get_lane_count(void)
 	return reg;
 }
 
-unsigned char exynos_dp_get_lanex_pre_emphasis(unsigned char lanecnt)
+unsigned char exynos_dp_get_lanex_pre_emphasis(struct exynos_dp *dp_regs,
+					       unsigned char lanecnt)
 {
 	unsigned int reg_list[DP_LANE_CNT_4] = {
 		(unsigned int)&dp_regs->ln0_link_training_ctl,
@@ -907,8 +901,9 @@ unsigned char exynos_dp_get_lanex_pre_emphasis(unsigned char lanecnt)
 	return readl(reg_list[lanecnt]);
 }
 
-void exynos_dp_set_lanex_pre_emphasis(unsigned char request_val,
-		unsigned char lanecnt)
+void exynos_dp_set_lanex_pre_emphasis(struct exynos_dp *dp_regs,
+				      unsigned char request_val,
+				      unsigned char lanecnt)
 {
 	unsigned int reg_list[DP_LANE_CNT_4] = {
 		(unsigned int)&dp_regs->ln0_link_training_ctl,
@@ -920,7 +915,8 @@ void exynos_dp_set_lanex_pre_emphasis(unsigned char request_val,
 	writel(request_val, reg_list[lanecnt]);
 }
 
-void exynos_dp_set_lane_pre_emphasis(unsigned int level, unsigned char lanecnt)
+void exynos_dp_set_lane_pre_emphasis(struct exynos_dp *dp_regs,
+				     unsigned int level, unsigned char lanecnt)
 {
 	unsigned char i;
 	unsigned int reg;
@@ -943,7 +939,8 @@ void exynos_dp_set_lane_pre_emphasis(unsigned int level, unsigned char lanecnt)
 	}
 }
 
-void exynos_dp_set_training_pattern(unsigned int pattern)
+void exynos_dp_set_training_pattern(struct exynos_dp *dp_regs,
+				    unsigned int pattern)
 {
 	unsigned int reg = 0;
 
@@ -971,7 +968,8 @@ void exynos_dp_set_training_pattern(unsigned int pattern)
 	writel(reg, &dp_regs->training_ptn_set);
 }
 
-void exynos_dp_enable_enhanced_mode(unsigned char enable)
+void exynos_dp_enable_enhanced_mode(struct exynos_dp *dp_regs,
+				    unsigned char enable)
 {
 	unsigned int reg;
 
@@ -984,7 +982,7 @@ void exynos_dp_enable_enhanced_mode(unsigned char enable)
 	writel(reg, &dp_regs->sys_ctl4);
 }
 
-void exynos_dp_enable_scrambling(unsigned int enable)
+void exynos_dp_enable_scrambling(struct exynos_dp *dp_regs, unsigned int enable)
 {
 	unsigned int reg;
 
@@ -997,7 +995,7 @@ void exynos_dp_enable_scrambling(unsigned int enable)
 	writel(reg, &dp_regs->training_ptn_set);
 }
 
-int exynos_dp_init_video(void)
+int exynos_dp_init_video(struct exynos_dp *dp_regs)
 {
 	unsigned int reg;
 
@@ -1012,7 +1010,8 @@ int exynos_dp_init_video(void)
 	return 0;
 }
 
-void exynos_dp_config_video_slave_mode(struct edp_video_info *video_info)
+void exynos_dp_config_video_slave_mode(struct exynos_dp *dp_regs,
+				       struct edp_video_info *video_info)
 {
 	unsigned int reg;
 
@@ -1045,7 +1044,8 @@ void exynos_dp_config_video_slave_mode(struct edp_video_info *video_info)
 	writel(reg, &dp_regs->soc_general_ctl);
 }
 
-void exynos_dp_set_video_color_format(struct edp_video_info *video_info)
+void exynos_dp_set_video_color_format(struct exynos_dp *dp_regs,
+				      struct edp_video_info *video_info)
 {
 	unsigned int reg;
 
@@ -1065,49 +1065,47 @@ void exynos_dp_set_video_color_format(struct edp_video_info *video_info)
 	writel(reg, &dp_regs->video_ctl3);
 }
 
-int exynos_dp_config_video_bist(struct edp_device_info *edp_info)
+int exynos_dp_config_video_bist(struct exynos_dp *dp_regs,
+				struct exynos_dp_priv *priv)
 {
 	unsigned int reg;
 	unsigned int bist_type = 0;
-	struct edp_video_info video_info = edp_info->video_info;
+	struct edp_video_info video_info = priv->video_info;
 
 	/* For master mode, you don't need to set the video format */
 	if (video_info.master_mode == 0) {
-		writel(TOTAL_LINE_CFG_L(edp_info->disp_info.v_total),
-				&dp_regs->total_ln_cfg_l);
-		writel(TOTAL_LINE_CFG_H(edp_info->disp_info.v_total),
-				&dp_regs->total_ln_cfg_h);
-		writel(ACTIVE_LINE_CFG_L(edp_info->disp_info.v_res),
-				&dp_regs->active_ln_cfg_l);
-		writel(ACTIVE_LINE_CFG_H(edp_info->disp_info.v_res),
-				&dp_regs->active_ln_cfg_h);
-		writel(edp_info->disp_info.v_sync_width,
-				&dp_regs->vsw_cfg);
-		writel(edp_info->disp_info.v_back_porch,
-				&dp_regs->vbp_cfg);
-		writel(edp_info->disp_info.v_front_porch,
-				&dp_regs->vfp_cfg);
-
-		writel(TOTAL_PIXEL_CFG_L(edp_info->disp_info.h_total),
-				&dp_regs->total_pix_cfg_l);
-		writel(TOTAL_PIXEL_CFG_H(edp_info->disp_info.h_total),
-				&dp_regs->total_pix_cfg_h);
-		writel(ACTIVE_PIXEL_CFG_L(edp_info->disp_info.h_res),
-				&dp_regs->active_pix_cfg_l);
-		writel(ACTIVE_PIXEL_CFG_H(edp_info->disp_info.h_res),
-				&dp_regs->active_pix_cfg_h);
-		writel(H_F_PORCH_CFG_L(edp_info->disp_info.h_front_porch),
-				&dp_regs->hfp_cfg_l);
-		writel(H_F_PORCH_CFG_H(edp_info->disp_info.h_front_porch),
-				&dp_regs->hfp_cfg_h);
-		writel(H_SYNC_PORCH_CFG_L(edp_info->disp_info.h_sync_width),
-				&dp_regs->hsw_cfg_l);
-		writel(H_SYNC_PORCH_CFG_H(edp_info->disp_info.h_sync_width),
-				&dp_regs->hsw_cfg_h);
-		writel(H_B_PORCH_CFG_L(edp_info->disp_info.h_back_porch),
-				&dp_regs->hbp_cfg_l);
-		writel(H_B_PORCH_CFG_H(edp_info->disp_info.h_back_porch),
-				&dp_regs->hbp_cfg_h);
+		writel(TOTAL_LINE_CFG_L(priv->disp_info.v_total),
+		       &dp_regs->total_ln_cfg_l);
+		writel(TOTAL_LINE_CFG_H(priv->disp_info.v_total),
+		       &dp_regs->total_ln_cfg_h);
+		writel(ACTIVE_LINE_CFG_L(priv->disp_info.v_res),
+		       &dp_regs->active_ln_cfg_l);
+		writel(ACTIVE_LINE_CFG_H(priv->disp_info.v_res),
+		       &dp_regs->active_ln_cfg_h);
+		writel(priv->disp_info.v_sync_width, &dp_regs->vsw_cfg);
+		writel(priv->disp_info.v_back_porch, &dp_regs->vbp_cfg);
+		writel(priv->disp_info.v_front_porch, &dp_regs->vfp_cfg);
+
+		writel(TOTAL_PIXEL_CFG_L(priv->disp_info.h_total),
+		       &dp_regs->total_pix_cfg_l);
+		writel(TOTAL_PIXEL_CFG_H(priv->disp_info.h_total),
+		       &dp_regs->total_pix_cfg_h);
+		writel(ACTIVE_PIXEL_CFG_L(priv->disp_info.h_res),
+		       &dp_regs->active_pix_cfg_l);
+		writel(ACTIVE_PIXEL_CFG_H(priv->disp_info.h_res),
+		       &dp_regs->active_pix_cfg_h);
+		writel(H_F_PORCH_CFG_L(priv->disp_info.h_front_porch),
+		       &dp_regs->hfp_cfg_l);
+		writel(H_F_PORCH_CFG_H(priv->disp_info.h_front_porch),
+		       &dp_regs->hfp_cfg_h);
+		writel(H_SYNC_PORCH_CFG_L(priv->disp_info.h_sync_width),
+		       &dp_regs->hsw_cfg_l);
+		writel(H_SYNC_PORCH_CFG_H(priv->disp_info.h_sync_width),
+		       &dp_regs->hsw_cfg_h);
+		writel(H_B_PORCH_CFG_L(priv->disp_info.h_back_porch),
+		       &dp_regs->hbp_cfg_l);
+		writel(H_B_PORCH_CFG_H(priv->disp_info.h_back_porch),
+		       &dp_regs->hbp_cfg_h);
 
 		/*
 		 * Set SLAVE_I_SCAN_CFG[2], VSYNC_P_CFG[1],
@@ -1155,7 +1153,7 @@ int exynos_dp_config_video_bist(struct edp_device_info *edp_info)
 	return 0;
 }
 
-unsigned int exynos_dp_is_slave_video_stream_clock_on(void)
+unsigned int exynos_dp_is_slave_video_stream_clock_on(struct exynos_dp *dp_regs)
 {
 	unsigned int reg;
 
@@ -1173,8 +1171,8 @@ unsigned int exynos_dp_is_slave_video_stream_clock_on(void)
 	return EXYNOS_DP_SUCCESS;
 }
 
-void exynos_dp_set_video_cr_mn(unsigned int type, unsigned int m_value,
-		unsigned int n_value)
+void exynos_dp_set_video_cr_mn(struct exynos_dp *dp_regs, unsigned int type,
+			       unsigned int m_value, unsigned int n_value)
 {
 	unsigned int reg;
 
@@ -1202,7 +1200,8 @@ void exynos_dp_set_video_cr_mn(unsigned int type, unsigned int m_value,
 	}
 }
 
-void exynos_dp_set_video_timing_mode(unsigned int type)
+void exynos_dp_set_video_timing_mode(struct exynos_dp *dp_regs,
+				     unsigned int type)
 {
 	unsigned int reg;
 
@@ -1215,7 +1214,8 @@ void exynos_dp_set_video_timing_mode(unsigned int type)
 	writel(reg, &dp_regs->video_ctl10);
 }
 
-void exynos_dp_enable_video_master(unsigned int enable)
+void exynos_dp_enable_video_master(struct exynos_dp *dp_regs,
+				   unsigned int enable)
 {
 	unsigned int reg;
 
@@ -1231,7 +1231,7 @@ void exynos_dp_enable_video_master(unsigned int enable)
 	writel(reg, &dp_regs->soc_general_ctl);
 }
 
-void exynos_dp_start_video(void)
+void exynos_dp_start_video(struct exynos_dp *dp_regs)
 {
 	unsigned int reg;
 
@@ -1241,7 +1241,7 @@ void exynos_dp_start_video(void)
 	writel(reg, &dp_regs->video_ctl1);
 }
 
-unsigned int exynos_dp_is_video_stream_on(void)
+unsigned int exynos_dp_is_video_stream_on(struct exynos_dp *dp_regs)
 {
 	unsigned int reg;
 
diff --git a/drivers/video/exynos/exynos_dp_lowlevel.h b/drivers/video/exynos/exynos_dp_lowlevel.h
new file mode 100644
index 0000000..e4c867e
--- /dev/null
+++ b/drivers/video/exynos/exynos_dp_lowlevel.h
@@ -0,0 +1,89 @@
+/*
+ * Copyright (C) 2012 Samsung Electronics
+ *
+ * Author: Donghwa Lee <dh09.lee@samsung.com>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#ifndef _EXYNOS_EDP_LOWLEVEL_H
+#define _EXYNOS_EDP_LOWLEVEL_H
+
+void exynos_dp_enable_video_bist(struct exynos_dp *dp_regs,
+				 unsigned int enable);
+void exynos_dp_enable_video_mute(struct exynos_dp *dp_regs,
+				 unsigned int enable);
+void exynos_dp_reset(struct exynos_dp *dp_regs);
+void exynos_dp_enable_sw_func(struct exynos_dp *dp_regs, unsigned int enable);
+unsigned int exynos_dp_set_analog_power_down(struct exynos_dp *dp_regs,
+					     unsigned int block, u32 enable);
+unsigned int exynos_dp_get_pll_lock_status(struct exynos_dp *dp_regs);
+int exynos_dp_init_analog_func(struct exynos_dp *dp_regs);
+void exynos_dp_init_hpd(struct exynos_dp *dp_regs);
+void exynos_dp_init_aux(struct exynos_dp *dp_regs);
+void exynos_dp_config_interrupt(struct exynos_dp *dp_regs);
+unsigned int exynos_dp_get_plug_in_status(struct exynos_dp *dp_regs);
+unsigned int exynos_dp_detect_hpd(struct exynos_dp *dp_regs);
+unsigned int exynos_dp_start_aux_transaction(struct exynos_dp *dp_regs);
+unsigned int exynos_dp_write_byte_to_dpcd(struct exynos_dp *dp_regs,
+					  unsigned int reg_addr,
+					  unsigned char data);
+unsigned int exynos_dp_read_byte_from_dpcd(struct exynos_dp *dp_regs,
+					   unsigned int reg_addr,
+					   unsigned char *data);
+unsigned int exynos_dp_write_bytes_to_dpcd(struct exynos_dp *dp_regs,
+					   unsigned int reg_addr,
+					   unsigned int count,
+					   unsigned char data[]);
+unsigned int exynos_dp_read_bytes_from_dpcd(struct exynos_dp *dp_regs,
+					    unsigned int reg_addr,
+					    unsigned int count,
+					    unsigned char data[]);
+int exynos_dp_select_i2c_device(struct exynos_dp *dp_regs,
+				unsigned int device_addr,
+				unsigned int reg_addr);
+int exynos_dp_read_byte_from_i2c(struct exynos_dp *dp_regs,
+				 unsigned int device_addr,
+				 unsigned int reg_addr, unsigned int *data);
+int exynos_dp_read_bytes_from_i2c(struct exynos_dp *dp_regs,
+				  unsigned int device_addr,
+				  unsigned int reg_addr, unsigned int count,
+				  unsigned char edid[]);
+void exynos_dp_reset_macro(struct exynos_dp *dp_regs);
+void exynos_dp_set_link_bandwidth(struct exynos_dp *dp_regs,
+				  unsigned char bwtype);
+unsigned char exynos_dp_get_link_bandwidth(struct exynos_dp *dp_regs);
+void exynos_dp_set_lane_count(struct exynos_dp *dp_regs, unsigned char count);
+unsigned int exynos_dp_get_lane_count(struct exynos_dp *dp_regs);
+unsigned char exynos_dp_get_lanex_pre_emphasis(struct exynos_dp *dp_regs,
+					       unsigned char lanecnt);
+void exynos_dp_set_lane_pre_emphasis(struct exynos_dp *dp_regs,
+				     unsigned int level, unsigned char lanecnt);
+void exynos_dp_set_lanex_pre_emphasis(struct exynos_dp *dp_regs,
+				      unsigned char request_val,
+				      unsigned char lanecnt);
+void exynos_dp_set_training_pattern(struct exynos_dp *dp_regs,
+				    unsigned int pattern);
+void exynos_dp_enable_enhanced_mode(struct exynos_dp *dp_regs,
+				    unsigned char enable);
+void exynos_dp_enable_scrambling(struct exynos_dp *dp_regs,
+				 unsigned int enable);
+int exynos_dp_init_video(struct exynos_dp *dp_regs);
+void exynos_dp_config_video_slave_mode(struct exynos_dp *dp_regs,
+				       struct edp_video_info *video_info);
+void exynos_dp_set_video_color_format(struct exynos_dp *dp_regs,
+				      struct edp_video_info *video_info);
+int exynos_dp_config_video_bist(struct exynos_dp *dp_regs,
+				struct exynos_dp_priv *priv);
+unsigned int exynos_dp_is_slave_video_stream_clock_on(
+					struct exynos_dp *dp_regs);
+void exynos_dp_set_video_cr_mn(struct exynos_dp *dp_regs, unsigned int type,
+			       unsigned int m_value, unsigned int n_value);
+void exynos_dp_set_video_timing_mode(struct exynos_dp *dp_regs,
+				     unsigned int type);
+void exynos_dp_enable_video_master(struct exynos_dp *dp_regs,
+				   unsigned int enable);
+void exynos_dp_start_video(struct exynos_dp *dp_regs);
+unsigned int exynos_dp_is_video_stream_on(struct exynos_dp *dp_regs);
+
+#endif /* _EXYNOS_DP_LOWLEVEL_H */
diff --git a/drivers/video/exynos/exynos_fb.c b/drivers/video/exynos/exynos_fb.c
new file mode 100644
index 0000000..97228cd
--- /dev/null
+++ b/drivers/video/exynos/exynos_fb.c
@@ -0,0 +1,720 @@
+/*
+ * Copyright (C) 2012 Samsung Electronics
+ *
+ * Author: InKi Dae <inki.dae@samsung.com>
+ * Author: Donghwa Lee <dh09.lee@samsung.com>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <config.h>
+#include <common.h>
+#include <display.h>
+#include <div64.h>
+#include <dm.h>
+#include <fdtdec.h>
+#include <libfdt.h>
+#include <panel.h>
+#include <video.h>
+#include <video_bridge.h>
+#include <asm/io.h>
+#include <asm/arch/cpu.h>
+#include <asm/arch/clock.h>
+#include <asm/arch/clk.h>
+#include <asm/arch/mipi_dsim.h>
+#include <asm/arch/dp_info.h>
+#include <asm/arch/fb.h>
+#include <asm/arch/pinmux.h>
+#include <asm/arch/system.h>
+#include <asm/gpio.h>
+#include <asm-generic/errno.h>
+
+DECLARE_GLOBAL_DATA_PTR;
+
+enum {
+	FIMD_RGB_INTERFACE = 1,
+	FIMD_CPU_INTERFACE = 2,
+};
+
+enum exynos_fb_rgb_mode_t {
+	MODE_RGB_P = 0,
+	MODE_BGR_P = 1,
+	MODE_RGB_S = 2,
+	MODE_BGR_S = 3,
+};
+
+struct exynos_fb_priv {
+	ushort vl_col;		/* Number of columns (i.e. 640) */
+	ushort vl_row;		/* Number of rows (i.e. 480) */
+	ushort vl_rot;		/* Rotation of Display (0, 1, 2, 3) */
+	ushort vl_width;	/* Width of display area in millimeters */
+	ushort vl_height;	/* Height of display area in millimeters */
+
+	/* LCD configuration register */
+	u_char vl_freq;		/* Frequency */
+	u_char vl_clkp;		/* Clock polarity */
+	u_char vl_oep;		/* Output Enable polarity */
+	u_char vl_hsp;		/* Horizontal Sync polarity */
+	u_char vl_vsp;		/* Vertical Sync polarity */
+	u_char vl_dp;		/* Data polarity */
+	u_char vl_bpix;		/* Bits per pixel */
+
+	/* Horizontal control register. Timing from data sheet */
+	u_char vl_hspw;		/* Horz sync pulse width */
+	u_char vl_hfpd;		/* Wait before of line */
+	u_char vl_hbpd;		/* Wait end of line */
+
+	/* Vertical control register. */
+	u_char	vl_vspw;	/* Vertical sync pulse width */
+	u_char	vl_vfpd;	/* Wait before of frame */
+	u_char	vl_vbpd;	/* Wait end of frame */
+	u_char  vl_cmd_allow_len; /* Wait end of frame */
+
+	unsigned int win_id;
+	unsigned int init_delay;
+	unsigned int power_on_delay;
+	unsigned int reset_delay;
+	unsigned int interface_mode;
+	unsigned int mipi_enabled;
+	unsigned int dp_enabled;
+	unsigned int cs_setup;
+	unsigned int wr_setup;
+	unsigned int wr_act;
+	unsigned int wr_hold;
+	unsigned int logo_on;
+	unsigned int logo_width;
+	unsigned int logo_height;
+	int logo_x_offset;
+	int logo_y_offset;
+	unsigned long logo_addr;
+	unsigned int rgb_mode;
+	unsigned int resolution;
+
+	/* parent clock name(MPLL, EPLL or VPLL) */
+	unsigned int pclk_name;
+	/* ratio value for source clock from parent clock. */
+	unsigned int sclk_div;
+
+	unsigned int dual_lcd_enabled;
+	struct exynos_fb *reg;
+	struct exynos_platform_mipi_dsim *dsim_platform_data_dt;
+};
+
+static void exynos_fimd_set_dualrgb(struct exynos_fb_priv *priv, bool enabled)
+{
+	struct exynos_fb *reg = priv->reg;
+	unsigned int cfg = 0;
+
+	if (enabled) {
+		cfg = EXYNOS_DUALRGB_BYPASS_DUAL | EXYNOS_DUALRGB_LINESPLIT |
+			EXYNOS_DUALRGB_VDEN_EN_ENABLE;
+
+		/* in case of Line Split mode, MAIN_CNT doesn't neet to set. */
+		cfg |= EXYNOS_DUALRGB_SUB_CNT(priv->vl_col / 2) |
+			EXYNOS_DUALRGB_MAIN_CNT(0);
+	}
+
+	writel(cfg, &reg->dualrgb);
+}
+
+static void exynos_fimd_set_dp_clkcon(struct exynos_fb_priv *priv,
+				      unsigned int enabled)
+{
+	struct exynos_fb *reg = priv->reg;
+	unsigned int cfg = 0;
+
+	if (enabled)
+		cfg = EXYNOS_DP_CLK_ENABLE;
+
+	writel(cfg, &reg->dp_mie_clkcon);
+}
+
+static void exynos_fimd_set_par(struct exynos_fb_priv *priv,
+				unsigned int win_id)
+{
+	struct exynos_fb *reg = priv->reg;
+	unsigned int cfg = 0;
+
+	/* set window control */
+	cfg = readl((unsigned int)&reg->wincon0 +
+			EXYNOS_WINCON(win_id));
+
+	cfg &= ~(EXYNOS_WINCON_BITSWP_ENABLE | EXYNOS_WINCON_BYTESWP_ENABLE |
+		EXYNOS_WINCON_HAWSWP_ENABLE | EXYNOS_WINCON_WSWP_ENABLE |
+		EXYNOS_WINCON_BURSTLEN_MASK | EXYNOS_WINCON_BPPMODE_MASK |
+		EXYNOS_WINCON_INRGB_MASK | EXYNOS_WINCON_DATAPATH_MASK);
+
+	/* DATAPATH is DMA */
+	cfg |= EXYNOS_WINCON_DATAPATH_DMA;
+
+	cfg |= EXYNOS_WINCON_HAWSWP_ENABLE;
+
+	/* dma burst is 16 */
+	cfg |= EXYNOS_WINCON_BURSTLEN_16WORD;
+
+	switch (priv->vl_bpix) {
+	case 4:
+		cfg |= EXYNOS_WINCON_BPPMODE_16BPP_565;
+		break;
+	default:
+		cfg |= EXYNOS_WINCON_BPPMODE_24BPP_888;
+		break;
+	}
+
+	writel(cfg, (unsigned int)&reg->wincon0 +
+			EXYNOS_WINCON(win_id));
+
+	/* set window position to x=0, y=0*/
+	cfg = EXYNOS_VIDOSD_LEFT_X(0) | EXYNOS_VIDOSD_TOP_Y(0);
+	writel(cfg, (unsigned int)&reg->vidosd0a +
+			EXYNOS_VIDOSD(win_id));
+
+	cfg = EXYNOS_VIDOSD_RIGHT_X(priv->vl_col - 1) |
+		EXYNOS_VIDOSD_BOTTOM_Y(priv->vl_row - 1) |
+		EXYNOS_VIDOSD_RIGHT_X_E(1) |
+		EXYNOS_VIDOSD_BOTTOM_Y_E(0);
+
+	writel(cfg, (unsigned int)&reg->vidosd0b +
+			EXYNOS_VIDOSD(win_id));
+
+	/* set window size for window0*/
+	cfg = EXYNOS_VIDOSD_SIZE(priv->vl_col * priv->vl_row);
+	writel(cfg, (unsigned int)&reg->vidosd0c +
+			EXYNOS_VIDOSD(win_id));
+}
+
+static void exynos_fimd_set_buffer_address(struct exynos_fb_priv *priv,
+					   unsigned int win_id,
+					   ulong lcd_base_addr)
+{
+	struct exynos_fb *reg = priv->reg;
+	unsigned long start_addr, end_addr;
+
+	start_addr = lcd_base_addr;
+	end_addr = start_addr + ((priv->vl_col * (VNBITS(priv->vl_bpix) / 8)) *
+				priv->vl_row);
+
+	writel(start_addr, (unsigned int)&reg->vidw00add0b0 +
+			EXYNOS_BUFFER_OFFSET(win_id));
+	writel(end_addr, (unsigned int)&reg->vidw00add1b0 +
+			EXYNOS_BUFFER_OFFSET(win_id));
+}
+
+static void exynos_fimd_set_clock(struct exynos_fb_priv *priv)
+{
+	struct exynos_fb *reg = priv->reg;
+	unsigned int cfg = 0, div = 0, remainder, remainder_div;
+	unsigned long pixel_clock;
+	unsigned long long src_clock;
+
+	if (priv->dual_lcd_enabled) {
+		pixel_clock = priv->vl_freq *
+				(priv->vl_hspw + priv->vl_hfpd +
+				 priv->vl_hbpd + priv->vl_col / 2) *
+				(priv->vl_vspw + priv->vl_vfpd +
+				 priv->vl_vbpd + priv->vl_row);
+	} else if (priv->interface_mode == FIMD_CPU_INTERFACE) {
+		pixel_clock = priv->vl_freq *
+				priv->vl_width * priv->vl_height *
+				(priv->cs_setup + priv->wr_setup +
+				 priv->wr_act + priv->wr_hold + 1);
+	} else {
+		pixel_clock = priv->vl_freq *
+				(priv->vl_hspw + priv->vl_hfpd +
+				 priv->vl_hbpd + priv->vl_col) *
+				(priv->vl_vspw + priv->vl_vfpd +
+				 priv->vl_vbpd + priv->vl_row);
+	}
+
+	cfg = readl(&reg->vidcon0);
+	cfg &= ~(EXYNOS_VIDCON0_CLKSEL_MASK | EXYNOS_VIDCON0_CLKVALUP_MASK |
+		EXYNOS_VIDCON0_CLKVAL_F(0xFF) | EXYNOS_VIDCON0_VCLKEN_MASK |
+		EXYNOS_VIDCON0_CLKDIR_MASK);
+	cfg |= (EXYNOS_VIDCON0_CLKSEL_SCLK | EXYNOS_VIDCON0_CLKVALUP_ALWAYS |
+		EXYNOS_VIDCON0_VCLKEN_NORMAL | EXYNOS_VIDCON0_CLKDIR_DIVIDED);
+
+	src_clock = (unsigned long long) get_lcd_clk();
+
+	/* get quotient and remainder. */
+	remainder = do_div(src_clock, pixel_clock);
+	div = src_clock;
+
+	remainder *= 10;
+	remainder_div = remainder / pixel_clock;
+
+	/* round about one places of decimals. */
+	if (remainder_div >= 5)
+		div++;
+
+	/* in case of dual lcd mode. */
+	if (priv->dual_lcd_enabled)
+		div--;
+
+	cfg |= EXYNOS_VIDCON0_CLKVAL_F(div - 1);
+	writel(cfg, &reg->vidcon0);
+}
+
+void exynos_set_trigger(struct exynos_fb_priv *priv)
+{
+	struct exynos_fb *reg = priv->reg;
+	unsigned int cfg = 0;
+
+	cfg = readl(&reg->trigcon);
+
+	cfg |= (EXYNOS_I80SOFT_TRIG_EN | EXYNOS_I80START_TRIG);
+
+	writel(cfg, &reg->trigcon);
+}
+
+int exynos_is_i80_frame_done(struct exynos_fb_priv *priv)
+{
+	struct exynos_fb *reg = priv->reg;
+	unsigned int cfg = 0;
+	int status;
+
+	cfg = readl(&reg->trigcon);
+
+	/* frame done func is valid only when TRIMODE[0] is set to 1. */
+	status = (cfg & EXYNOS_I80STATUS_TRIG_DONE) ==
+			EXYNOS_I80STATUS_TRIG_DONE;
+
+	return status;
+}
+
+static void exynos_fimd_lcd_on(struct exynos_fb_priv *priv)
+{
+	struct exynos_fb *reg = priv->reg;
+	unsigned int cfg = 0;
+
+	/* display on */
+	cfg = readl(&reg->vidcon0);
+	cfg |= (EXYNOS_VIDCON0_ENVID_ENABLE | EXYNOS_VIDCON0_ENVID_F_ENABLE);
+	writel(cfg, &reg->vidcon0);
+}
+
+static void exynos_fimd_window_on(struct exynos_fb_priv *priv,
+				  unsigned int win_id)
+{
+	struct exynos_fb *reg = priv->reg;
+	unsigned int cfg = 0;
+
+	/* enable window */
+	cfg = readl((unsigned int)&reg->wincon0 +
+			EXYNOS_WINCON(win_id));
+	cfg |= EXYNOS_WINCON_ENWIN_ENABLE;
+	writel(cfg, (unsigned int)&reg->wincon0 +
+			EXYNOS_WINCON(win_id));
+
+	cfg = readl(&reg->winshmap);
+	cfg |= EXYNOS_WINSHMAP_CH_ENABLE(win_id);
+	writel(cfg, &reg->winshmap);
+}
+
+void exynos_fimd_lcd_off(struct exynos_fb_priv *priv)
+{
+	struct exynos_fb *reg = priv->reg;
+	unsigned int cfg = 0;
+
+	cfg = readl(&reg->vidcon0);
+	cfg &= (EXYNOS_VIDCON0_ENVID_DISABLE | EXYNOS_VIDCON0_ENVID_F_DISABLE);
+	writel(cfg, &reg->vidcon0);
+}
+
+void exynos_fimd_window_off(struct exynos_fb_priv *priv, unsigned int win_id)
+{
+	struct exynos_fb *reg = priv->reg;
+	unsigned int cfg = 0;
+
+	cfg = readl((unsigned int)&reg->wincon0 +
+			EXYNOS_WINCON(win_id));
+	cfg &= EXYNOS_WINCON_ENWIN_DISABLE;
+	writel(cfg, (unsigned int)&reg->wincon0 +
+			EXYNOS_WINCON(win_id));
+
+	cfg = readl(&reg->winshmap);
+	cfg &= ~EXYNOS_WINSHMAP_CH_DISABLE(win_id);
+	writel(cfg, &reg->winshmap);
+}
+
+/*
+* The reset value for FIMD SYSMMU register MMU_CTRL is 3
+* on Exynos5420 and newer versions.
+* This means FIMD SYSMMU is on by default on Exynos5420
+* and newer versions.
+* Since in u-boot we don't use SYSMMU, we should disable
+* those FIMD SYSMMU.
+* Note that there are 2 SYSMMU for FIMD: m0 and m1.
+* m0 handles windows 0 and 4, and m1 handles windows 1, 2 and 3.
+* We disable both of them here.
+*/
+void exynos_fimd_disable_sysmmu(void)
+{
+	u32 *sysmmufimd;
+	unsigned int node;
+	int node_list[2];
+	int count;
+	int i;
+
+	count = fdtdec_find_aliases_for_id(gd->fdt_blob, "fimd",
+				COMPAT_SAMSUNG_EXYNOS_SYSMMU, node_list, 2);
+	for (i = 0; i < count; i++) {
+		node = node_list[i];
+		if (node <= 0) {
+			debug("Can't get device node for fimd sysmmu\n");
+			return;
+		}
+
+		sysmmufimd = (u32 *)fdtdec_get_addr(gd->fdt_blob, node, "reg");
+		if (!sysmmufimd) {
+			debug("Can't get base address for sysmmu fimdm0");
+			return;
+		}
+
+		writel(0x0, sysmmufimd);
+	}
+}
+
+void exynos_fimd_lcd_init(struct udevice *dev)
+{
+	struct exynos_fb_priv *priv = dev_get_priv(dev);
+	struct video_uc_platdata *plat = dev_get_uclass_platdata(dev);
+	struct exynos_fb *reg = priv->reg;
+	unsigned int cfg = 0, rgb_mode;
+	unsigned int offset;
+	unsigned int node;
+
+	node = dev->of_offset;
+	if (fdtdec_get_bool(gd->fdt_blob, node, "samsung,disable-sysmmu"))
+		exynos_fimd_disable_sysmmu();
+
+	offset = exynos_fimd_get_base_offset();
+
+	rgb_mode = priv->rgb_mode;
+
+	if (priv->interface_mode == FIMD_RGB_INTERFACE) {
+		cfg |= EXYNOS_VIDCON0_VIDOUT_RGB;
+		writel(cfg, &reg->vidcon0);
+
+		cfg = readl(&reg->vidcon2);
+		cfg &= ~(EXYNOS_VIDCON2_WB_MASK |
+			EXYNOS_VIDCON2_TVFORMATSEL_MASK |
+			EXYNOS_VIDCON2_TVFORMATSEL_YUV_MASK);
+		cfg |= EXYNOS_VIDCON2_WB_DISABLE;
+		writel(cfg, &reg->vidcon2);
+
+		/* set polarity */
+		cfg = 0;
+		if (!priv->vl_clkp)
+			cfg |= EXYNOS_VIDCON1_IVCLK_RISING_EDGE;
+		if (!priv->vl_hsp)
+			cfg |= EXYNOS_VIDCON1_IHSYNC_INVERT;
+		if (!priv->vl_vsp)
+			cfg |= EXYNOS_VIDCON1_IVSYNC_INVERT;
+		if (!priv->vl_dp)
+			cfg |= EXYNOS_VIDCON1_IVDEN_INVERT;
+
+		writel(cfg, (unsigned int)&reg->vidcon1 + offset);
+
+		/* set timing */
+		cfg = EXYNOS_VIDTCON0_VFPD(priv->vl_vfpd - 1);
+		cfg |= EXYNOS_VIDTCON0_VBPD(priv->vl_vbpd - 1);
+		cfg |= EXYNOS_VIDTCON0_VSPW(priv->vl_vspw - 1);
+		writel(cfg, (unsigned int)&reg->vidtcon0 + offset);
+
+		cfg = EXYNOS_VIDTCON1_HFPD(priv->vl_hfpd - 1);
+		cfg |= EXYNOS_VIDTCON1_HBPD(priv->vl_hbpd - 1);
+		cfg |= EXYNOS_VIDTCON1_HSPW(priv->vl_hspw - 1);
+
+		writel(cfg, (unsigned int)&reg->vidtcon1 + offset);
+
+		/* set lcd size */
+		cfg = EXYNOS_VIDTCON2_HOZVAL(priv->vl_col - 1) |
+			EXYNOS_VIDTCON2_LINEVAL(priv->vl_row - 1) |
+			EXYNOS_VIDTCON2_HOZVAL_E(priv->vl_col - 1) |
+			EXYNOS_VIDTCON2_LINEVAL_E(priv->vl_row - 1);
+
+		writel(cfg, (unsigned int)&reg->vidtcon2 + offset);
+	}
+
+	/* set display mode */
+	cfg = readl(&reg->vidcon0);
+	cfg &= ~EXYNOS_VIDCON0_PNRMODE_MASK;
+	cfg |= (rgb_mode << EXYNOS_VIDCON0_PNRMODE_SHIFT);
+	writel(cfg, &reg->vidcon0);
+
+	/* set par */
+	exynos_fimd_set_par(priv, priv->win_id);
+
+	/* set memory address */
+	exynos_fimd_set_buffer_address(priv, priv->win_id, plat->base);
+
+	/* set buffer size */
+	cfg = EXYNOS_VIDADDR_PAGEWIDTH(priv->vl_col *
+			VNBITS(priv->vl_bpix) / 8) |
+		EXYNOS_VIDADDR_PAGEWIDTH_E(priv->vl_col *
+			VNBITS(priv->vl_bpix) / 8) |
+		EXYNOS_VIDADDR_OFFSIZE(0) |
+		EXYNOS_VIDADDR_OFFSIZE_E(0);
+
+	writel(cfg, (unsigned int)&reg->vidw00add2 +
+					EXYNOS_BUFFER_SIZE(priv->win_id));
+
+	/* set clock */
+	exynos_fimd_set_clock(priv);
+
+	/* set rgb mode to dual lcd. */
+	exynos_fimd_set_dualrgb(priv, priv->dual_lcd_enabled);
+
+	/* display on */
+	exynos_fimd_lcd_on(priv);
+
+	/* window on */
+	exynos_fimd_window_on(priv, priv->win_id);
+
+	exynos_fimd_set_dp_clkcon(priv, priv->dp_enabled);
+}
+
+unsigned long exynos_fimd_calc_fbsize(struct exynos_fb_priv *priv)
+{
+	return priv->vl_col * priv->vl_row * (VNBITS(priv->vl_bpix) / 8);
+}
+
+int exynos_fb_ofdata_to_platdata(struct udevice *dev)
+{
+	struct exynos_fb_priv *priv = dev_get_priv(dev);
+	unsigned int node = dev->of_offset;
+	const void *blob = gd->fdt_blob;
+	fdt_addr_t addr;
+
+	addr = dev_get_addr(dev);
+	if (addr == FDT_ADDR_T_NONE) {
+		debug("Can't get the FIMD base address\n");
+		return -EINVAL;
+	}
+	priv->reg = (struct exynos_fb *)addr;
+
+	priv->vl_col = fdtdec_get_int(blob, node, "samsung,vl-col", 0);
+	if (priv->vl_col == 0) {
+		debug("Can't get XRES\n");
+		return -ENXIO;
+	}
+
+	priv->vl_row = fdtdec_get_int(blob, node, "samsung,vl-row", 0);
+	if (priv->vl_row == 0) {
+		debug("Can't get YRES\n");
+		return -ENXIO;
+	}
+
+	priv->vl_width = fdtdec_get_int(blob, node,
+						"samsung,vl-width", 0);
+
+	priv->vl_height = fdtdec_get_int(blob, node,
+						"samsung,vl-height", 0);
+
+	priv->vl_freq = fdtdec_get_int(blob, node, "samsung,vl-freq", 0);
+	if (priv->vl_freq == 0) {
+		debug("Can't get refresh rate\n");
+		return -ENXIO;
+	}
+
+	if (fdtdec_get_bool(blob, node, "samsung,vl-clkp"))
+		priv->vl_clkp = VIDEO_ACTIVE_LOW;
+
+	if (fdtdec_get_bool(blob, node, "samsung,vl-oep"))
+		priv->vl_oep = VIDEO_ACTIVE_LOW;
+
+	if (fdtdec_get_bool(blob, node, "samsung,vl-hsp"))
+		priv->vl_hsp = VIDEO_ACTIVE_LOW;
+
+	if (fdtdec_get_bool(blob, node, "samsung,vl-vsp"))
+		priv->vl_vsp = VIDEO_ACTIVE_LOW;
+
+	if (fdtdec_get_bool(blob, node, "samsung,vl-dp"))
+		priv->vl_dp = VIDEO_ACTIVE_LOW;
+
+	priv->vl_bpix = fdtdec_get_int(blob, node, "samsung,vl-bpix", 0);
+	if (priv->vl_bpix == 0) {
+		debug("Can't get bits per pixel\n");
+		return -ENXIO;
+	}
+
+	priv->vl_hspw = fdtdec_get_int(blob, node, "samsung,vl-hspw", 0);
+	if (priv->vl_hspw == 0) {
+		debug("Can't get hsync width\n");
+		return -ENXIO;
+	}
+
+	priv->vl_hfpd = fdtdec_get_int(blob, node, "samsung,vl-hfpd", 0);
+	if (priv->vl_hfpd == 0) {
+		debug("Can't get right margin\n");
+		return -ENXIO;
+	}
+
+	priv->vl_hbpd = (u_char)fdtdec_get_int(blob, node,
+							"samsung,vl-hbpd", 0);
+	if (priv->vl_hbpd == 0) {
+		debug("Can't get left margin\n");
+		return -ENXIO;
+	}
+
+	priv->vl_vspw = (u_char)fdtdec_get_int(blob, node,
+							"samsung,vl-vspw", 0);
+	if (priv->vl_vspw == 0) {
+		debug("Can't get vsync width\n");
+		return -ENXIO;
+	}
+
+	priv->vl_vfpd = fdtdec_get_int(blob, node,
+							"samsung,vl-vfpd", 0);
+	if (priv->vl_vfpd == 0) {
+		debug("Can't get lower margin\n");
+		return -ENXIO;
+	}
+
+	priv->vl_vbpd = fdtdec_get_int(blob, node, "samsung,vl-vbpd", 0);
+	if (priv->vl_vbpd == 0) {
+		debug("Can't get upper margin\n");
+		return -ENXIO;
+	}
+
+	priv->vl_cmd_allow_len = fdtdec_get_int(blob, node,
+						"samsung,vl-cmd-allow-len", 0);
+
+	priv->win_id = fdtdec_get_int(blob, node, "samsung,winid", 0);
+	priv->init_delay = fdtdec_get_int(blob, node,
+						"samsung,init-delay", 0);
+	priv->power_on_delay = fdtdec_get_int(blob, node,
+						"samsung,power-on-delay", 0);
+	priv->reset_delay = fdtdec_get_int(blob, node,
+						"samsung,reset-delay", 0);
+	priv->interface_mode = fdtdec_get_int(blob, node,
+						"samsung,interface-mode", 0);
+	priv->mipi_enabled = fdtdec_get_int(blob, node,
+						"samsung,mipi-enabled", 0);
+	priv->dp_enabled = fdtdec_get_int(blob, node,
+						"samsung,dp-enabled", 0);
+	priv->cs_setup = fdtdec_get_int(blob, node,
+						"samsung,cs-setup", 0);
+	priv->wr_setup = fdtdec_get_int(blob, node,
+						"samsung,wr-setup", 0);
+	priv->wr_act = fdtdec_get_int(blob, node, "samsung,wr-act", 0);
+	priv->wr_hold = fdtdec_get_int(blob, node, "samsung,wr-hold", 0);
+
+	priv->logo_on = fdtdec_get_int(blob, node, "samsung,logo-on", 0);
+	if (priv->logo_on) {
+		priv->logo_width = fdtdec_get_int(blob, node,
+						"samsung,logo-width", 0);
+		priv->logo_height = fdtdec_get_int(blob, node,
+						"samsung,logo-height", 0);
+		priv->logo_addr = fdtdec_get_int(blob, node,
+						"samsung,logo-addr", 0);
+	}
+
+	priv->rgb_mode = fdtdec_get_int(blob, node,
+						"samsung,rgb-mode", 0);
+	priv->pclk_name = fdtdec_get_int(blob, node,
+						"samsung,pclk-name", 0);
+	priv->sclk_div = fdtdec_get_int(blob, node,
+						"samsung,sclk-div", 0);
+	priv->dual_lcd_enabled = fdtdec_get_int(blob, node,
+						"samsung,dual-lcd-enabled", 0);
+
+	return 0;
+}
+
+static int exynos_fb_probe(struct udevice *dev)
+{
+	struct video_priv *uc_priv = dev_get_uclass_priv(dev);
+	struct exynos_fb_priv *priv = dev_get_priv(dev);
+	struct udevice *panel, *bridge;
+	struct udevice *dp;
+	int ret;
+
+	debug("%s: start\n", __func__);
+	set_system_display_ctrl();
+	set_lcd_clk();
+
+#ifdef CONFIG_EXYNOS_MIPI_DSIM
+	exynos_init_dsim_platform_data(&panel_info);
+#endif
+	exynos_fimd_lcd_init(dev);
+
+	ret = uclass_first_device(UCLASS_PANEL, &panel);
+	if (ret) {
+		printf("LCD panel failed to probe\n");
+		return ret;
+	}
+	if (!panel) {
+		printf("LCD panel not found\n");
+		return -ENODEV;
+	}
+
+	ret = uclass_first_device(UCLASS_DISPLAY, &dp);
+	if (ret) {
+		debug("%s: Display device error %d\n", __func__, ret);
+		return ret;
+	}
+	if (!dev) {
+		debug("%s: Display device missing\n", __func__);
+		return -ENODEV;
+	}
+	ret = display_enable(dp, 18, NULL);
+	if (ret) {
+		debug("%s: Display enable error %d\n", __func__, ret);
+		return ret;
+	}
+
+	/* backlight / pwm */
+	ret = panel_enable_backlight(panel);
+	if (ret) {
+		debug("%s: backlight error: %d\n", __func__, ret);
+		return ret;
+	}
+
+	ret = uclass_get_device(UCLASS_VIDEO_BRIDGE, 0, &bridge);
+	if (!ret)
+		ret = video_bridge_set_backlight(bridge, 80);
+	if (ret) {
+		debug("%s: No video bridge, or no backlight on bridge\n",
+		      __func__);
+		exynos_pinmux_config(PERIPH_ID_PWM0, 0);
+	}
+
+	uc_priv->xsize = priv->vl_col;
+	uc_priv->ysize = priv->vl_row;
+	uc_priv->bpix = priv->vl_bpix;
+
+	/* Enable flushing after LCD writes if requested */
+	video_set_flush_dcache(dev, true);
+
+	return 0;
+}
+
+static int exynos_fb_bind(struct udevice *dev)
+{
+	struct video_uc_platdata *plat = dev_get_uclass_platdata(dev);
+
+	/* This is the maximum panel size we expect to see */
+	plat->size = 1920 * 1080 * 2;
+
+	return 0;
+}
+
+static const struct video_ops exynos_fb_ops = {
+};
+
+static const struct udevice_id exynos_fb_ids[] = {
+	{ .compatible = "samsung,exynos-fimd" },
+	{ }
+};
+
+U_BOOT_DRIVER(exynos_fb) = {
+	.name	= "exynos_fb",
+	.id	= UCLASS_VIDEO,
+	.of_match = exynos_fb_ids,
+	.ops	= &exynos_fb_ops,
+	.bind	= exynos_fb_bind,
+	.probe	= exynos_fb_probe,
+	.ofdata_to_platdata	= exynos_fb_ofdata_to_platdata,
+	.priv_auto_alloc_size	= sizeof(struct exynos_fb_priv),
+};
diff --git a/drivers/video/exynos_mipi_dsi.c b/drivers/video/exynos/exynos_mipi_dsi.c
index b597acc..a5d9b59 100644
--- a/drivers/video/exynos_mipi_dsi.c
+++ b/drivers/video/exynos/exynos_mipi_dsi.c
@@ -27,13 +27,6 @@
 
 DECLARE_GLOBAL_DATA_PTR;
 
-static struct exynos_platform_mipi_dsim *dsim_pd;
-#if CONFIG_IS_ENABLED(OF_CONTROL)
-static struct mipi_dsim_config dsim_config_dt;
-static struct exynos_platform_mipi_dsim dsim_platform_data_dt;
-static struct mipi_dsim_lcd_device mipi_lcd_device_dt;
-#endif
-
 struct mipi_dsim_ddi {
 	int				bus_id;
 	struct list_head		list;
@@ -178,7 +171,7 @@ static struct mipi_dsim_master_ops master_ops = {
 	.clear_dsim_frame_done		= exynos_mipi_dsi_clear_frame_done,
 };
 
-int exynos_mipi_dsi_init(void)
+int exynos_mipi_dsi_init(struct exynos_platform_mipi_dsim *dsim_pd)
 {
 	struct mipi_dsim_device *dsim;
 	struct mipi_dsim_config *dsim_config;
@@ -239,18 +232,8 @@ int exynos_mipi_dsi_init(void)
 	return 0;
 }
 
-void exynos_set_dsim_platform_data(struct exynos_platform_mipi_dsim *pd)
-{
-	if (pd == NULL) {
-		debug("pd is NULL\n");
-		return;
-	}
-
-	dsim_pd = pd;
-}
-
-#if CONFIG_IS_ENABLED(OF_CONTROL)
-int exynos_dsim_config_parse_dt(const void *blob)
+int exynos_dsim_config_parse_dt(const void *blob, struct mipi_dsim_config *dt,
+				struct mipi_dsim_lcd_device *lcd_dt)
 {
 	int node;
 
@@ -260,59 +243,59 @@ int exynos_dsim_config_parse_dt(const void *blob)
 		return -ENODEV;
 	}
 
-	dsim_config_dt.e_interface = fdtdec_get_int(blob, node,
+	dt->e_interface = fdtdec_get_int(blob, node,
 				"samsung,dsim-config-e-interface", 0);
 
-	dsim_config_dt.e_virtual_ch = fdtdec_get_int(blob, node,
+	dt->e_virtual_ch = fdtdec_get_int(blob, node,
 				"samsung,dsim-config-e-virtual-ch", 0);
 
-	dsim_config_dt.e_pixel_format = fdtdec_get_int(blob, node,
+	dt->e_pixel_format = fdtdec_get_int(blob, node,
 				"samsung,dsim-config-e-pixel-format", 0);
 
-	dsim_config_dt.e_burst_mode = fdtdec_get_int(blob, node,
+	dt->e_burst_mode = fdtdec_get_int(blob, node,
 				"samsung,dsim-config-e-burst-mode", 0);
 
-	dsim_config_dt.e_no_data_lane = fdtdec_get_int(blob, node,
+	dt->e_no_data_lane = fdtdec_get_int(blob, node,
 				"samsung,dsim-config-e-no-data-lane", 0);
 
-	dsim_config_dt.e_byte_clk = fdtdec_get_int(blob, node,
+	dt->e_byte_clk = fdtdec_get_int(blob, node,
 				"samsung,dsim-config-e-byte-clk", 0);
 
-	dsim_config_dt.hfp = fdtdec_get_int(blob, node,
+	dt->hfp = fdtdec_get_int(blob, node,
 				"samsung,dsim-config-hfp", 0);
 
-	dsim_config_dt.p = fdtdec_get_int(blob, node,
+	dt->p = fdtdec_get_int(blob, node,
 					  "samsung,dsim-config-p", 0);
-	dsim_config_dt.m = fdtdec_get_int(blob, node,
+	dt->m = fdtdec_get_int(blob, node,
 					  "samsung,dsim-config-m", 0);
-	dsim_config_dt.s = fdtdec_get_int(blob, node,
+	dt->s = fdtdec_get_int(blob, node,
 					  "samsung,dsim-config-s", 0);
 
-	dsim_config_dt.pll_stable_time = fdtdec_get_int(blob, node,
+	dt->pll_stable_time = fdtdec_get_int(blob, node,
 				"samsung,dsim-config-pll-stable-time", 0);
 
-	dsim_config_dt.esc_clk = fdtdec_get_int(blob, node,
+	dt->esc_clk = fdtdec_get_int(blob, node,
 				"samsung,dsim-config-esc-clk", 0);
 
-	dsim_config_dt.stop_holding_cnt = fdtdec_get_int(blob, node,
+	dt->stop_holding_cnt = fdtdec_get_int(blob, node,
 				"samsung,dsim-config-stop-holding-cnt", 0);
 
-	dsim_config_dt.bta_timeout = fdtdec_get_int(blob, node,
+	dt->bta_timeout = fdtdec_get_int(blob, node,
 				"samsung,dsim-config-bta-timeout", 0);
 
-	dsim_config_dt.rx_timeout = fdtdec_get_int(blob, node,
+	dt->rx_timeout = fdtdec_get_int(blob, node,
 				"samsung,dsim-config-rx-timeout", 0);
 
-	mipi_lcd_device_dt.name = fdtdec_get_config_string(blob,
+	lcd_dt->name = fdtdec_get_config_string(blob,
 				"samsung,dsim-device-name");
 
-	mipi_lcd_device_dt.id = fdtdec_get_int(blob, node,
+	lcd_dt->id = fdtdec_get_int(blob, node,
 				"samsung,dsim-device-id", 0);
 
-	mipi_lcd_device_dt.bus_id = fdtdec_get_int(blob, node,
+	lcd_dt->bus_id = fdtdec_get_int(blob, node,
 				"samsung,dsim-device-bus_id", 0);
 
-	mipi_lcd_device_dt.reverse_panel = fdtdec_get_int(blob, node,
+	lcd_dt->reverse_panel = fdtdec_get_int(blob, node,
 				"samsung,dsim-device-reverse-panel", 0);
 
 	return 0;
@@ -320,7 +303,12 @@ int exynos_dsim_config_parse_dt(const void *blob)
 
 void exynos_init_dsim_platform_data(vidinfo_t *vid)
 {
-	if (exynos_dsim_config_parse_dt(gd->fdt_blob))
+	static struct mipi_dsim_config dsim_config_dt;
+	static struct exynos_platform_mipi_dsim dsim_platform_data_dt;
+	static struct mipi_dsim_lcd_device mipi_lcd_device_dt;
+
+	if (exynos_dsim_config_parse_dt(gd->fdt_blob, &dsim_config_dt,
+					&mipi_lcd_device_dt))
 		debug("Can't get proper dsim config.\n");
 
 	strcpy(dsim_platform_data_dt.lcd_panel_name, mipi_lcd_device_dt.name);
@@ -332,6 +320,5 @@ void exynos_init_dsim_platform_data(vidinfo_t *vid)
 	mipi_lcd_device_dt.platform_data = (void *)&dsim_platform_data_dt;
 	exynos_mipi_dsi_register_lcd_device(&mipi_lcd_device_dt);
 
-	dsim_pd = &dsim_platform_data_dt;
+	vid->dsim_platform_data_dt = &dsim_platform_data_dt;
 }
-#endif
diff --git a/drivers/video/exynos_mipi_dsi_common.c b/drivers/video/exynos/exynos_mipi_dsi_common.c
index 925d515..85c5e0d 100644
--- a/drivers/video/exynos_mipi_dsi_common.c
+++ b/drivers/video/exynos/exynos_mipi_dsi_common.c
@@ -465,7 +465,7 @@ int exynos_mipi_dsi_enable_frame_done_int(struct mipi_dsim_device *dsim,
 }
 
 static void convert_to_fb_videomode(struct fb_videomode *mode1,
-				vidinfo_t *mode2)
+				    struct vidinfo *mode2)
 {
 	mode1->xres = mode2->vl_width;
 	mode1->yres = mode2->vl_height;
@@ -482,10 +482,10 @@ int exynos_mipi_dsi_set_display_mode(struct mipi_dsim_device *dsim,
 {
 	struct exynos_platform_mipi_dsim *dsim_pd;
 	struct fb_videomode lcd_video;
-	vidinfo_t *vid;
+	struct vidinfo *vid;
 
 	dsim_pd = (struct exynos_platform_mipi_dsim *)dsim->pd;
-	vid = (vidinfo_t *)dsim_pd->lcd_panel_info;
+	vid = (struct vidinfo *)dsim_pd->lcd_panel_info;
 
 	convert_to_fb_videomode(&lcd_video, vid);
 
diff --git a/drivers/video/exynos_mipi_dsi_common.h b/drivers/video/exynos/exynos_mipi_dsi_common.h
index 98eb78e..98eb78e 100644
--- a/drivers/video/exynos_mipi_dsi_common.h
+++ b/drivers/video/exynos/exynos_mipi_dsi_common.h
diff --git a/drivers/video/exynos_mipi_dsi_lowlevel.c b/drivers/video/exynos/exynos_mipi_dsi_lowlevel.c
index fcfdc8d..fcfdc8d 100644
--- a/drivers/video/exynos_mipi_dsi_lowlevel.c
+++ b/drivers/video/exynos/exynos_mipi_dsi_lowlevel.c
diff --git a/drivers/video/exynos_mipi_dsi_lowlevel.h b/drivers/video/exynos/exynos_mipi_dsi_lowlevel.h
index 0bede25..0bede25 100644
--- a/drivers/video/exynos_mipi_dsi_lowlevel.h
+++ b/drivers/video/exynos/exynos_mipi_dsi_lowlevel.h
diff --git a/drivers/video/exynos_pwm_bl.c b/drivers/video/exynos/exynos_pwm_bl.c
index a6890da..a6890da 100644
--- a/drivers/video/exynos_pwm_bl.c
+++ b/drivers/video/exynos/exynos_pwm_bl.c
diff --git a/drivers/video/exynos_dp_lowlevel.h b/drivers/video/exynos_dp_lowlevel.h
deleted file mode 100644
index 8651681..0000000
--- a/drivers/video/exynos_dp_lowlevel.h
+++ /dev/null
@@ -1,68 +0,0 @@
-/*
- * Copyright (C) 2012 Samsung Electronics
- *
- * Author: Donghwa Lee <dh09.lee@samsung.com>
- *
- * SPDX-License-Identifier:	GPL-2.0+
- */
-
-#ifndef _EXYNOS_EDP_LOWLEVEL_H
-#define _EXYNOS_EDP_LOWLEVEL_H
-
-void exynos_dp_enable_video_bist(unsigned int enable);
-void exynos_dp_enable_video_mute(unsigned int enable);
-void exynos_dp_reset(void);
-void exynos_dp_enable_sw_func(unsigned int enable);
-unsigned int exynos_dp_set_analog_power_down(unsigned int block, u32 enable);
-unsigned int exynos_dp_get_pll_lock_status(void);
-int exynos_dp_init_analog_func(void);
-void exynos_dp_init_hpd(void);
-void exynos_dp_init_aux(void);
-void exynos_dp_config_interrupt(void);
-unsigned int exynos_dp_get_plug_in_status(void);
-unsigned int exynos_dp_detect_hpd(void);
-unsigned int exynos_dp_start_aux_transaction(void);
-unsigned int exynos_dp_write_byte_to_dpcd(unsigned int reg_addr,
-				unsigned char data);
-unsigned int exynos_dp_read_byte_from_dpcd(unsigned int reg_addr,
-		unsigned char *data);
-unsigned int exynos_dp_write_bytes_to_dpcd(unsigned int reg_addr,
-		unsigned int count,
-		unsigned char data[]);
-unsigned int exynos_dp_read_bytes_from_dpcd( unsigned int reg_addr,
-		unsigned int count,
-		unsigned char data[]);
-int exynos_dp_select_i2c_device( unsigned int device_addr,
-		unsigned int reg_addr);
-int exynos_dp_read_byte_from_i2c(unsigned int device_addr,
-		unsigned int reg_addr, unsigned int *data);
-int exynos_dp_read_bytes_from_i2c(unsigned int device_addr,
-		unsigned int reg_addr, unsigned int count,
-		unsigned char edid[]);
-void exynos_dp_reset_macro(void);
-void exynos_dp_set_link_bandwidth(unsigned char bwtype);
-unsigned char exynos_dp_get_link_bandwidth(void);
-void exynos_dp_set_lane_count(unsigned char count);
-unsigned int exynos_dp_get_lane_count(void);
-unsigned char exynos_dp_get_lanex_pre_emphasis(unsigned char lanecnt);
-void exynos_dp_set_lane_pre_emphasis(unsigned int level,
-		unsigned char lanecnt);
-void exynos_dp_set_lanex_pre_emphasis(unsigned char request_val,
-		unsigned char lanecnt);
-void exynos_dp_set_training_pattern(unsigned int pattern);
-void exynos_dp_enable_enhanced_mode(unsigned char enable);
-void exynos_dp_enable_scrambling(unsigned int enable);
-int exynos_dp_init_video(void);
-void exynos_dp_config_video_slave_mode(struct edp_video_info *video_info);
-void exynos_dp_set_video_color_format(struct edp_video_info *video_info);
-int exynos_dp_config_video_bist(struct edp_device_info *edp_info);
-unsigned int exynos_dp_is_slave_video_stream_clock_on(void);
-void exynos_dp_set_video_cr_mn(unsigned int type, unsigned int m_value,
-		unsigned int n_value);
-void exynos_dp_set_video_timing_mode(unsigned int type);
-void exynos_dp_enable_video_master(unsigned int enable);
-void exynos_dp_start_video(void);
-unsigned int exynos_dp_is_video_stream_on(void);
-void exynos_dp_set_base_addr(void);
-
-#endif /* _EXYNOS_DP_LOWLEVEL_H */
diff --git a/drivers/video/exynos_fb.c b/drivers/video/exynos_fb.c
deleted file mode 100644
index 69edc3a..0000000
--- a/drivers/video/exynos_fb.c
+++ /dev/null
@@ -1,330 +0,0 @@
-/*
- * Copyright (C) 2012 Samsung Electronics
- *
- * Author: InKi Dae <inki.dae@samsung.com>
- * Author: Donghwa Lee <dh09.lee@samsung.com>
- *
- * SPDX-License-Identifier:	GPL-2.0+
- */
-
-#include <config.h>
-#include <common.h>
-#include <lcd.h>
-#include <fdtdec.h>
-#include <libfdt.h>
-#include <asm/io.h>
-#include <asm/arch/cpu.h>
-#include <asm/arch/clock.h>
-#include <asm/arch/clk.h>
-#include <asm/arch/mipi_dsim.h>
-#include <asm/arch/dp_info.h>
-#include <asm/arch/system.h>
-#include <asm/gpio.h>
-#include <asm-generic/errno.h>
-
-#include "exynos_fb.h"
-
-DECLARE_GLOBAL_DATA_PTR;
-
-static unsigned int panel_width, panel_height;
-
-#if CONFIG_IS_ENABLED(OF_CONTROL)
-vidinfo_t panel_info  = {
-	/*
-	 * Insert a value here so that we don't end up in the BSS
-	 * Reference: drivers/video/tegra.c
-	 */
-	.vl_col = -1,
-};
-#endif
-
-ushort *configuration_get_cmap(void)
-{
-#if defined(CONFIG_LCD_LOGO)
-	return bmp_logo_palette;
-#else
-	return NULL;
-#endif
-}
-
-static void exynos_lcd_init_mem(void *lcdbase, vidinfo_t *vid)
-{
-	unsigned long palette_size;
-	unsigned int fb_size;
-
-	fb_size = vid->vl_row * vid->vl_col * (NBITS(vid->vl_bpix) >> 3);
-
-	palette_size = NBITS(vid->vl_bpix) == 8 ? 256 : 16;
-
-	exynos_fimd_lcd_init_mem((unsigned long)lcdbase,
-			(unsigned long)fb_size, palette_size);
-}
-
-static void exynos_lcd_init(vidinfo_t *vid)
-{
-	exynos_fimd_lcd_init(vid);
-
-	/* Enable flushing after LCD writes if requested */
-	lcd_set_flush_dcache(1);
-}
-
-__weak void exynos_cfg_lcd_gpio(void)
-{
-}
-
-__weak void exynos_backlight_on(unsigned int onoff)
-{
-}
-
-__weak void exynos_reset_lcd(void)
-{
-}
-
-__weak void exynos_lcd_power_on(void)
-{
-}
-
-__weak void exynos_cfg_ldo(void)
-{
-}
-
-__weak void exynos_enable_ldo(unsigned int onoff)
-{
-}
-
-__weak void exynos_backlight_reset(void)
-{
-}
-
-__weak int exynos_lcd_misc_init(vidinfo_t *vid)
-{
-	return 0;
-}
-
-static void lcd_panel_on(vidinfo_t *vid)
-{
-	struct gpio_desc pwm_out_gpio;
-	struct gpio_desc bl_en_gpio;
-	unsigned int node;
-
-	udelay(vid->init_delay);
-
-	exynos_backlight_reset();
-
-	exynos_cfg_lcd_gpio();
-
-	exynos_lcd_power_on();
-
-	udelay(vid->power_on_delay);
-
-	if (vid->dp_enabled)
-		exynos_init_dp();
-
-	exynos_reset_lcd();
-
-	udelay(vid->reset_delay);
-
-	exynos_backlight_on(1);
-
-#if CONFIG_IS_ENABLED(OF_CONTROL)
-	node = fdtdec_next_compatible(gd->fdt_blob, 0,
-						COMPAT_SAMSUNG_EXYNOS_FIMD);
-	if (node <= 0) {
-		debug("FIMD: Can't get device node for FIMD\n");
-		return;
-	}
-	gpio_request_by_name_nodev(gd->fdt_blob, node, "samsung,pwm-out-gpio",
-				   0, &pwm_out_gpio,
-				   GPIOD_IS_OUT | GPIOD_IS_OUT_ACTIVE);
-
-	gpio_request_by_name_nodev(gd->fdt_blob, node, "samsung,bl-en-gpio", 0,
-				   &bl_en_gpio,
-				   GPIOD_IS_OUT | GPIOD_IS_OUT_ACTIVE);
-
-#endif
-	exynos_cfg_ldo();
-
-	exynos_enable_ldo(1);
-
-	if (vid->mipi_enabled)
-		exynos_mipi_dsi_init();
-}
-
-#if CONFIG_IS_ENABLED(OF_CONTROL)
-int exynos_lcd_early_init(const void *blob)
-{
-	unsigned int node;
-	node = fdtdec_next_compatible(blob, 0, COMPAT_SAMSUNG_EXYNOS_FIMD);
-	if (node <= 0) {
-		debug("exynos_fb: Can't get device node for fimd\n");
-		return -ENODEV;
-	}
-
-	panel_info.vl_col = fdtdec_get_int(blob, node, "samsung,vl-col", 0);
-	if (panel_info.vl_col == 0) {
-		debug("Can't get XRES\n");
-		return -ENXIO;
-	}
-
-	panel_info.vl_row = fdtdec_get_int(blob, node, "samsung,vl-row", 0);
-	if (panel_info.vl_row == 0) {
-		debug("Can't get YRES\n");
-		return -ENXIO;
-	}
-
-	panel_info.vl_width = fdtdec_get_int(blob, node,
-						"samsung,vl-width", 0);
-
-	panel_info.vl_height = fdtdec_get_int(blob, node,
-						"samsung,vl-height", 0);
-
-	panel_info.vl_freq = fdtdec_get_int(blob, node, "samsung,vl-freq", 0);
-	if (panel_info.vl_freq == 0) {
-		debug("Can't get refresh rate\n");
-		return -ENXIO;
-	}
-
-	if (fdtdec_get_bool(blob, node, "samsung,vl-clkp"))
-		panel_info.vl_clkp = CONFIG_SYS_LOW;
-
-	if (fdtdec_get_bool(blob, node, "samsung,vl-oep"))
-		panel_info.vl_oep = CONFIG_SYS_LOW;
-
-	if (fdtdec_get_bool(blob, node, "samsung,vl-hsp"))
-		panel_info.vl_hsp = CONFIG_SYS_LOW;
-
-	if (fdtdec_get_bool(blob, node, "samsung,vl-vsp"))
-		panel_info.vl_vsp = CONFIG_SYS_LOW;
-
-	if (fdtdec_get_bool(blob, node, "samsung,vl-dp"))
-		panel_info.vl_dp = CONFIG_SYS_LOW;
-
-	panel_info.vl_bpix = fdtdec_get_int(blob, node, "samsung,vl-bpix", 0);
-	if (panel_info.vl_bpix == 0) {
-		debug("Can't get bits per pixel\n");
-		return -ENXIO;
-	}
-
-	panel_info.vl_hspw = fdtdec_get_int(blob, node, "samsung,vl-hspw", 0);
-	if (panel_info.vl_hspw == 0) {
-		debug("Can't get hsync width\n");
-		return -ENXIO;
-	}
-
-	panel_info.vl_hfpd = fdtdec_get_int(blob, node, "samsung,vl-hfpd", 0);
-	if (panel_info.vl_hfpd == 0) {
-		debug("Can't get right margin\n");
-		return -ENXIO;
-	}
-
-	panel_info.vl_hbpd = (u_char)fdtdec_get_int(blob, node,
-							"samsung,vl-hbpd", 0);
-	if (panel_info.vl_hbpd == 0) {
-		debug("Can't get left margin\n");
-		return -ENXIO;
-	}
-
-	panel_info.vl_vspw = (u_char)fdtdec_get_int(blob, node,
-							"samsung,vl-vspw", 0);
-	if (panel_info.vl_vspw == 0) {
-		debug("Can't get vsync width\n");
-		return -ENXIO;
-	}
-
-	panel_info.vl_vfpd = fdtdec_get_int(blob, node,
-							"samsung,vl-vfpd", 0);
-	if (panel_info.vl_vfpd == 0) {
-		debug("Can't get lower margin\n");
-		return -ENXIO;
-	}
-
-	panel_info.vl_vbpd = fdtdec_get_int(blob, node, "samsung,vl-vbpd", 0);
-	if (panel_info.vl_vbpd == 0) {
-		debug("Can't get upper margin\n");
-		return -ENXIO;
-	}
-
-	panel_info.vl_cmd_allow_len = fdtdec_get_int(blob, node,
-						"samsung,vl-cmd-allow-len", 0);
-
-	panel_info.win_id = fdtdec_get_int(blob, node, "samsung,winid", 0);
-	panel_info.init_delay = fdtdec_get_int(blob, node,
-						"samsung,init-delay", 0);
-	panel_info.power_on_delay = fdtdec_get_int(blob, node,
-						"samsung,power-on-delay", 0);
-	panel_info.reset_delay = fdtdec_get_int(blob, node,
-						"samsung,reset-delay", 0);
-	panel_info.interface_mode = fdtdec_get_int(blob, node,
-						"samsung,interface-mode", 0);
-	panel_info.mipi_enabled = fdtdec_get_int(blob, node,
-						"samsung,mipi-enabled", 0);
-	panel_info.dp_enabled = fdtdec_get_int(blob, node,
-						"samsung,dp-enabled", 0);
-	panel_info.cs_setup = fdtdec_get_int(blob, node,
-						"samsung,cs-setup", 0);
-	panel_info.wr_setup = fdtdec_get_int(blob, node,
-						"samsung,wr-setup", 0);
-	panel_info.wr_act = fdtdec_get_int(blob, node, "samsung,wr-act", 0);
-	panel_info.wr_hold = fdtdec_get_int(blob, node, "samsung,wr-hold", 0);
-
-	panel_info.logo_on = fdtdec_get_int(blob, node, "samsung,logo-on", 0);
-	if (panel_info.logo_on) {
-		panel_info.logo_width = fdtdec_get_int(blob, node,
-						"samsung,logo-width", 0);
-		panel_info.logo_height = fdtdec_get_int(blob, node,
-						"samsung,logo-height", 0);
-		panel_info.logo_addr = fdtdec_get_int(blob, node,
-						"samsung,logo-addr", 0);
-	}
-
-	panel_info.rgb_mode = fdtdec_get_int(blob, node,
-						"samsung,rgb-mode", 0);
-	panel_info.pclk_name = fdtdec_get_int(blob, node,
-						"samsung,pclk-name", 0);
-	panel_info.sclk_div = fdtdec_get_int(blob, node,
-						"samsung,sclk-div", 0);
-	panel_info.dual_lcd_enabled = fdtdec_get_int(blob, node,
-						"samsung,dual-lcd-enabled", 0);
-
-	return 0;
-}
-#endif
-
-void lcd_ctrl_init(void *lcdbase)
-{
-	set_system_display_ctrl();
-	set_lcd_clk();
-
-#if CONFIG_IS_ENABLED(OF_CONTROL)
-#ifdef CONFIG_EXYNOS_MIPI_DSIM
-	exynos_init_dsim_platform_data(&panel_info);
-#endif
-	exynos_lcd_misc_init(&panel_info);
-#else
-	/* initialize parameters which is specific to panel. */
-	init_panel_info(&panel_info);
-#endif
-
-	panel_width = panel_info.vl_width;
-	panel_height = panel_info.vl_height;
-
-	exynos_lcd_init_mem(lcdbase, &panel_info);
-
-	exynos_lcd_init(&panel_info);
-}
-
-void lcd_enable(void)
-{
-	if (panel_info.logo_on) {
-		memset((void *) gd->fb_base, 0, panel_width * panel_height *
-				(NBITS(panel_info.vl_bpix) >> 3));
-	}
-
-	lcd_panel_on(&panel_info);
-}
-
-/* dummy function */
-void lcd_setcolreg(ushort regno, ushort red, ushort green, ushort blue)
-{
-	return;
-}
diff --git a/drivers/video/exynos_fb.h b/drivers/video/exynos_fb.h
deleted file mode 100644
index 2c2f94b..0000000
--- a/drivers/video/exynos_fb.h
+++ /dev/null
@@ -1,41 +0,0 @@
-/*
- * Copyright (C) 2012 Samsung Electronics
- *
- * Author: InKi Dae <inki.dae@samsung.com>
- * Author: Donghwa Lee <dh09.lee@samsung.com>
- *
- * SPDX-License-Identifier:	GPL-2.0+
- */
-
-#ifndef _EXYNOS_FB_H_
-#define _EXYNOS_FB_H_
-
-#include <asm/arch/fb.h>
-
-#define MAX_CLOCK	(86 * 1000000)
-
-enum exynos_cpu_auto_cmd_rate {
-	DISABLE_AUTO_FRM,
-	PER_TWO_FRM,
-	PER_FOUR_FRM,
-	PER_SIX_FRM,
-	PER_EIGHT_FRM,
-	PER_TEN_FRM,
-	PER_TWELVE_FRM,
-	PER_FOURTEEN_FRM,
-	PER_SIXTEEN_FRM,
-	PER_EIGHTEEN_FRM,
-	PER_TWENTY_FRM,
-	PER_TWENTY_TWO_FRM,
-	PER_TWENTY_FOUR_FRM,
-	PER_TWENTY_SIX_FRM,
-	PER_TWENTY_EIGHT_FRM,
-	PER_THIRTY_FRM,
-};
-
-void exynos_fimd_lcd_init_mem(unsigned long screen_base, unsigned long fb_size,
-	unsigned long palette_size);
-void exynos_fimd_lcd_init(vidinfo_t *vid);
-unsigned long exynos_fimd_calc_fbsize(void);
-
-#endif
diff --git a/drivers/video/exynos_fimd.c b/drivers/video/exynos_fimd.c
deleted file mode 100644
index ac001a8..0000000
--- a/drivers/video/exynos_fimd.c
+++ /dev/null
@@ -1,409 +0,0 @@
-/*
- * Copyright (C) 2012 Samsung Electronics
- *
- * Author: InKi Dae <inki.dae@samsung.com>
- * Author: Donghwa Lee <dh09.lee@samsung.com>
- *
- * SPDX-License-Identifier:	GPL-2.0+
- */
-
-#include <config.h>
-#include <common.h>
-#include <asm/io.h>
-#include <lcd.h>
-#include <div64.h>
-#include <fdtdec.h>
-#include <libfdt.h>
-#include <asm/arch/clk.h>
-#include <asm/arch/clock.h>
-#include <asm/arch/cpu.h>
-#include "exynos_fb.h"
-
-DECLARE_GLOBAL_DATA_PTR;
-
-static unsigned long *lcd_base_addr;
-static vidinfo_t *pvid;
-static struct exynos_fb *fimd_ctrl;
-
-void exynos_fimd_lcd_init_mem(u_long screen_base, u_long fb_size,
-		u_long palette_size)
-{
-	lcd_base_addr = (unsigned long *)screen_base;
-}
-
-static void exynos_fimd_set_dualrgb(unsigned int enabled)
-{
-	unsigned int cfg = 0;
-
-	if (enabled) {
-		cfg = EXYNOS_DUALRGB_BYPASS_DUAL | EXYNOS_DUALRGB_LINESPLIT |
-			EXYNOS_DUALRGB_VDEN_EN_ENABLE;
-
-		/* in case of Line Split mode, MAIN_CNT doesn't neet to set. */
-		cfg |= EXYNOS_DUALRGB_SUB_CNT(pvid->vl_col / 2) |
-			EXYNOS_DUALRGB_MAIN_CNT(0);
-	}
-
-	writel(cfg, &fimd_ctrl->dualrgb);
-}
-
-static void exynos_fimd_set_dp_clkcon(unsigned int enabled)
-{
-	unsigned int cfg = 0;
-
-	if (enabled)
-		cfg = EXYNOS_DP_CLK_ENABLE;
-
-	writel(cfg, &fimd_ctrl->dp_mie_clkcon);
-}
-
-static void exynos_fimd_set_par(unsigned int win_id)
-{
-	unsigned int cfg = 0;
-
-	/* set window control */
-	cfg = readl((unsigned int)&fimd_ctrl->wincon0 +
-			EXYNOS_WINCON(win_id));
-
-	cfg &= ~(EXYNOS_WINCON_BITSWP_ENABLE | EXYNOS_WINCON_BYTESWP_ENABLE |
-		EXYNOS_WINCON_HAWSWP_ENABLE | EXYNOS_WINCON_WSWP_ENABLE |
-		EXYNOS_WINCON_BURSTLEN_MASK | EXYNOS_WINCON_BPPMODE_MASK |
-		EXYNOS_WINCON_INRGB_MASK | EXYNOS_WINCON_DATAPATH_MASK);
-
-	/* DATAPATH is DMA */
-	cfg |= EXYNOS_WINCON_DATAPATH_DMA;
-
-	cfg |= EXYNOS_WINCON_HAWSWP_ENABLE;
-
-	/* dma burst is 16 */
-	cfg |= EXYNOS_WINCON_BURSTLEN_16WORD;
-
-	switch (pvid->vl_bpix) {
-	case 4:
-		cfg |= EXYNOS_WINCON_BPPMODE_16BPP_565;
-		break;
-	default:
-		cfg |= EXYNOS_WINCON_BPPMODE_24BPP_888;
-		break;
-	}
-
-	writel(cfg, (unsigned int)&fimd_ctrl->wincon0 +
-			EXYNOS_WINCON(win_id));
-
-	/* set window position to x=0, y=0*/
-	cfg = EXYNOS_VIDOSD_LEFT_X(0) | EXYNOS_VIDOSD_TOP_Y(0);
-	writel(cfg, (unsigned int)&fimd_ctrl->vidosd0a +
-			EXYNOS_VIDOSD(win_id));
-
-	cfg = EXYNOS_VIDOSD_RIGHT_X(pvid->vl_col - 1) |
-		EXYNOS_VIDOSD_BOTTOM_Y(pvid->vl_row - 1) |
-		EXYNOS_VIDOSD_RIGHT_X_E(1) |
-		EXYNOS_VIDOSD_BOTTOM_Y_E(0);
-
-	writel(cfg, (unsigned int)&fimd_ctrl->vidosd0b +
-			EXYNOS_VIDOSD(win_id));
-
-	/* set window size for window0*/
-	cfg = EXYNOS_VIDOSD_SIZE(pvid->vl_col * pvid->vl_row);
-	writel(cfg, (unsigned int)&fimd_ctrl->vidosd0c +
-			EXYNOS_VIDOSD(win_id));
-}
-
-static void exynos_fimd_set_buffer_address(unsigned int win_id)
-{
-	unsigned long start_addr, end_addr;
-
-	start_addr = (unsigned long)lcd_base_addr;
-	end_addr = start_addr + ((pvid->vl_col * (NBITS(pvid->vl_bpix) / 8)) *
-				pvid->vl_row);
-
-	writel(start_addr, (unsigned int)&fimd_ctrl->vidw00add0b0 +
-			EXYNOS_BUFFER_OFFSET(win_id));
-	writel(end_addr, (unsigned int)&fimd_ctrl->vidw00add1b0 +
-			EXYNOS_BUFFER_OFFSET(win_id));
-}
-
-static void exynos_fimd_set_clock(vidinfo_t *pvid)
-{
-	unsigned int cfg = 0, div = 0, remainder, remainder_div;
-	unsigned long pixel_clock;
-	unsigned long long src_clock;
-
-	if (pvid->dual_lcd_enabled) {
-		pixel_clock = pvid->vl_freq *
-				(pvid->vl_hspw + pvid->vl_hfpd +
-				 pvid->vl_hbpd + pvid->vl_col / 2) *
-				(pvid->vl_vspw + pvid->vl_vfpd +
-				 pvid->vl_vbpd + pvid->vl_row);
-	} else if (pvid->interface_mode == FIMD_CPU_INTERFACE) {
-		pixel_clock = pvid->vl_freq *
-				pvid->vl_width * pvid->vl_height *
-				(pvid->cs_setup + pvid->wr_setup +
-				 pvid->wr_act + pvid->wr_hold + 1);
-	} else {
-		pixel_clock = pvid->vl_freq *
-				(pvid->vl_hspw + pvid->vl_hfpd +
-				 pvid->vl_hbpd + pvid->vl_col) *
-				(pvid->vl_vspw + pvid->vl_vfpd +
-				 pvid->vl_vbpd + pvid->vl_row);
-	}
-
-	cfg = readl(&fimd_ctrl->vidcon0);
-	cfg &= ~(EXYNOS_VIDCON0_CLKSEL_MASK | EXYNOS_VIDCON0_CLKVALUP_MASK |
-		EXYNOS_VIDCON0_CLKVAL_F(0xFF) | EXYNOS_VIDCON0_VCLKEN_MASK |
-		EXYNOS_VIDCON0_CLKDIR_MASK);
-	cfg |= (EXYNOS_VIDCON0_CLKSEL_SCLK | EXYNOS_VIDCON0_CLKVALUP_ALWAYS |
-		EXYNOS_VIDCON0_VCLKEN_NORMAL | EXYNOS_VIDCON0_CLKDIR_DIVIDED);
-
-	src_clock = (unsigned long long) get_lcd_clk();
-
-	/* get quotient and remainder. */
-	remainder = do_div(src_clock, pixel_clock);
-	div = src_clock;
-
-	remainder *= 10;
-	remainder_div = remainder / pixel_clock;
-
-	/* round about one places of decimals. */
-	if (remainder_div >= 5)
-		div++;
-
-	/* in case of dual lcd mode. */
-	if (pvid->dual_lcd_enabled)
-		div--;
-
-	cfg |= EXYNOS_VIDCON0_CLKVAL_F(div - 1);
-	writel(cfg, &fimd_ctrl->vidcon0);
-}
-
-void exynos_set_trigger(void)
-{
-	unsigned int cfg = 0;
-
-	cfg = readl(&fimd_ctrl->trigcon);
-
-	cfg |= (EXYNOS_I80SOFT_TRIG_EN | EXYNOS_I80START_TRIG);
-
-	writel(cfg, &fimd_ctrl->trigcon);
-}
-
-int exynos_is_i80_frame_done(void)
-{
-	unsigned int cfg = 0;
-	int status;
-
-	cfg = readl(&fimd_ctrl->trigcon);
-
-	/* frame done func is valid only when TRIMODE[0] is set to 1. */
-	status = (cfg & EXYNOS_I80STATUS_TRIG_DONE) ==
-			EXYNOS_I80STATUS_TRIG_DONE;
-
-	return status;
-}
-
-static void exynos_fimd_lcd_on(void)
-{
-	unsigned int cfg = 0;
-
-	/* display on */
-	cfg = readl(&fimd_ctrl->vidcon0);
-	cfg |= (EXYNOS_VIDCON0_ENVID_ENABLE | EXYNOS_VIDCON0_ENVID_F_ENABLE);
-	writel(cfg, &fimd_ctrl->vidcon0);
-}
-
-static void exynos_fimd_window_on(unsigned int win_id)
-{
-	unsigned int cfg = 0;
-
-	/* enable window */
-	cfg = readl((unsigned int)&fimd_ctrl->wincon0 +
-			EXYNOS_WINCON(win_id));
-	cfg |= EXYNOS_WINCON_ENWIN_ENABLE;
-	writel(cfg, (unsigned int)&fimd_ctrl->wincon0 +
-			EXYNOS_WINCON(win_id));
-
-	cfg = readl(&fimd_ctrl->winshmap);
-	cfg |= EXYNOS_WINSHMAP_CH_ENABLE(win_id);
-	writel(cfg, &fimd_ctrl->winshmap);
-}
-
-void exynos_fimd_lcd_off(void)
-{
-	unsigned int cfg = 0;
-
-	cfg = readl(&fimd_ctrl->vidcon0);
-	cfg &= (EXYNOS_VIDCON0_ENVID_DISABLE | EXYNOS_VIDCON0_ENVID_F_DISABLE);
-	writel(cfg, &fimd_ctrl->vidcon0);
-}
-
-void exynos_fimd_window_off(unsigned int win_id)
-{
-	unsigned int cfg = 0;
-
-	cfg = readl((unsigned int)&fimd_ctrl->wincon0 +
-			EXYNOS_WINCON(win_id));
-	cfg &= EXYNOS_WINCON_ENWIN_DISABLE;
-	writel(cfg, (unsigned int)&fimd_ctrl->wincon0 +
-			EXYNOS_WINCON(win_id));
-
-	cfg = readl(&fimd_ctrl->winshmap);
-	cfg &= ~EXYNOS_WINSHMAP_CH_DISABLE(win_id);
-	writel(cfg, &fimd_ctrl->winshmap);
-}
-
-#if CONFIG_IS_ENABLED(OF_CONTROL)
-/*
-* The reset value for FIMD SYSMMU register MMU_CTRL is 3
-* on Exynos5420 and newer versions.
-* This means FIMD SYSMMU is on by default on Exynos5420
-* and newer versions.
-* Since in u-boot we don't use SYSMMU, we should disable
-* those FIMD SYSMMU.
-* Note that there are 2 SYSMMU for FIMD: m0 and m1.
-* m0 handles windows 0 and 4, and m1 handles windows 1, 2 and 3.
-* We disable both of them here.
-*/
-void exynos_fimd_disable_sysmmu(void)
-{
-	u32 *sysmmufimd;
-	unsigned int node;
-	int node_list[2];
-	int count;
-	int i;
-
-	count = fdtdec_find_aliases_for_id(gd->fdt_blob, "fimd",
-				COMPAT_SAMSUNG_EXYNOS_SYSMMU, node_list, 2);
-	for (i = 0; i < count; i++) {
-		node = node_list[i];
-		if (node <= 0) {
-			debug("Can't get device node for fimd sysmmu\n");
-			return;
-		}
-
-		sysmmufimd = (u32 *)fdtdec_get_addr(gd->fdt_blob, node, "reg");
-		if (!sysmmufimd) {
-			debug("Can't get base address for sysmmu fimdm0");
-			return;
-		}
-
-		writel(0x0, sysmmufimd);
-	}
-}
-#endif
-
-void exynos_fimd_lcd_init(vidinfo_t *vid)
-{
-	unsigned int cfg = 0, rgb_mode;
-	unsigned int offset;
-#if CONFIG_IS_ENABLED(OF_CONTROL)
-	unsigned int node;
-
-	node = fdtdec_next_compatible(gd->fdt_blob,
-					0, COMPAT_SAMSUNG_EXYNOS_FIMD);
-	if (node <= 0)
-		debug("exynos_fb: Can't get device node for fimd\n");
-
-	fimd_ctrl = (struct exynos_fb *)fdtdec_get_addr(gd->fdt_blob,
-								node, "reg");
-	if (fimd_ctrl == NULL)
-		debug("Can't get the FIMD base address\n");
-
-	if (fdtdec_get_bool(gd->fdt_blob, node, "samsung,disable-sysmmu"))
-		exynos_fimd_disable_sysmmu();
-
-#else
-	fimd_ctrl = (struct exynos_fb *)samsung_get_base_fimd();
-#endif
-
-	offset = exynos_fimd_get_base_offset();
-
-	/* store panel info to global variable */
-	pvid = vid;
-
-	rgb_mode = vid->rgb_mode;
-
-	if (vid->interface_mode == FIMD_RGB_INTERFACE) {
-		cfg |= EXYNOS_VIDCON0_VIDOUT_RGB;
-		writel(cfg, &fimd_ctrl->vidcon0);
-
-		cfg = readl(&fimd_ctrl->vidcon2);
-		cfg &= ~(EXYNOS_VIDCON2_WB_MASK |
-			EXYNOS_VIDCON2_TVFORMATSEL_MASK |
-			EXYNOS_VIDCON2_TVFORMATSEL_YUV_MASK);
-		cfg |= EXYNOS_VIDCON2_WB_DISABLE;
-		writel(cfg, &fimd_ctrl->vidcon2);
-
-		/* set polarity */
-		cfg = 0;
-		if (!pvid->vl_clkp)
-			cfg |= EXYNOS_VIDCON1_IVCLK_RISING_EDGE;
-		if (!pvid->vl_hsp)
-			cfg |= EXYNOS_VIDCON1_IHSYNC_INVERT;
-		if (!pvid->vl_vsp)
-			cfg |= EXYNOS_VIDCON1_IVSYNC_INVERT;
-		if (!pvid->vl_dp)
-			cfg |= EXYNOS_VIDCON1_IVDEN_INVERT;
-
-		writel(cfg, (unsigned int)&fimd_ctrl->vidcon1 + offset);
-
-		/* set timing */
-		cfg = EXYNOS_VIDTCON0_VFPD(pvid->vl_vfpd - 1);
-		cfg |= EXYNOS_VIDTCON0_VBPD(pvid->vl_vbpd - 1);
-		cfg |= EXYNOS_VIDTCON0_VSPW(pvid->vl_vspw - 1);
-		writel(cfg, (unsigned int)&fimd_ctrl->vidtcon0 + offset);
-
-		cfg = EXYNOS_VIDTCON1_HFPD(pvid->vl_hfpd - 1);
-		cfg |= EXYNOS_VIDTCON1_HBPD(pvid->vl_hbpd - 1);
-		cfg |= EXYNOS_VIDTCON1_HSPW(pvid->vl_hspw - 1);
-
-		writel(cfg, (unsigned int)&fimd_ctrl->vidtcon1 + offset);
-
-		/* set lcd size */
-		cfg = EXYNOS_VIDTCON2_HOZVAL(pvid->vl_col - 1) |
-			EXYNOS_VIDTCON2_LINEVAL(pvid->vl_row - 1) |
-			EXYNOS_VIDTCON2_HOZVAL_E(pvid->vl_col - 1) |
-			EXYNOS_VIDTCON2_LINEVAL_E(pvid->vl_row - 1);
-
-		writel(cfg, (unsigned int)&fimd_ctrl->vidtcon2 + offset);
-	}
-
-	/* set display mode */
-	cfg = readl(&fimd_ctrl->vidcon0);
-	cfg &= ~EXYNOS_VIDCON0_PNRMODE_MASK;
-	cfg |= (rgb_mode << EXYNOS_VIDCON0_PNRMODE_SHIFT);
-	writel(cfg, &fimd_ctrl->vidcon0);
-
-	/* set par */
-	exynos_fimd_set_par(pvid->win_id);
-
-	/* set memory address */
-	exynos_fimd_set_buffer_address(pvid->win_id);
-
-	/* set buffer size */
-	cfg = EXYNOS_VIDADDR_PAGEWIDTH(pvid->vl_col * NBITS(pvid->vl_bpix) / 8) |
-		EXYNOS_VIDADDR_PAGEWIDTH_E(pvid->vl_col * NBITS(pvid->vl_bpix) / 8) |
-		EXYNOS_VIDADDR_OFFSIZE(0) |
-		EXYNOS_VIDADDR_OFFSIZE_E(0);
-
-	writel(cfg, (unsigned int)&fimd_ctrl->vidw00add2 +
-					EXYNOS_BUFFER_SIZE(pvid->win_id));
-
-	/* set clock */
-	exynos_fimd_set_clock(pvid);
-
-	/* set rgb mode to dual lcd. */
-	exynos_fimd_set_dualrgb(pvid->dual_lcd_enabled);
-
-	/* display on */
-	exynos_fimd_lcd_on();
-
-	/* window on */
-	exynos_fimd_window_on(pvid->win_id);
-
-	exynos_fimd_set_dp_clkcon(pvid->dp_enabled);
-}
-
-unsigned long exynos_fimd_calc_fbsize(void)
-{
-	return pvid->vl_col * pvid->vl_row * (NBITS(pvid->vl_bpix) / 8);
-}
diff --git a/drivers/video/rockchip/rk_vop.c b/drivers/video/rockchip/rk_vop.c
index a54af17..db09d9a 100644
--- a/drivers/video/rockchip/rk_vop.c
+++ b/drivers/video/rockchip/rk_vop.c
@@ -326,6 +326,7 @@ static int rk_vop_probe(struct udevice *dev)
 		if (!ret)
 			break;
 	}
+	video_set_flush_dcache(dev, 1);
 
 	return ret;
 }
diff --git a/drivers/video/s6e8ax0.c b/drivers/video/s6e8ax0.c
index 8494817..1bd49ee 100644
--- a/drivers/video/s6e8ax0.c
+++ b/drivers/video/s6e8ax0.c
@@ -9,8 +9,8 @@
 #include <common.h>
 #include <asm/arch/mipi_dsim.h>
 
-#include "exynos_mipi_dsi_lowlevel.h"
-#include "exynos_mipi_dsi_common.h"
+#include "exynos/exynos_mipi_dsi_lowlevel.h"
+#include "exynos/exynos_mipi_dsi_common.h"
 
 static void s6e8ax0_panel_cond(struct mipi_dsim_device *dsim_dev)
 {
diff --git a/drivers/video/simple_panel.c b/drivers/video/simple_panel.c
index 6219300..b2fe345 100644
--- a/drivers/video/simple_panel.c
+++ b/drivers/video/simple_panel.c
@@ -88,6 +88,8 @@ static const struct panel_ops simple_panel_ops = {
 static const struct udevice_id simple_panel_ids[] = {
 	{ .compatible = "simple-panel" },
 	{ .compatible = "auo,b133xtn01" },
+	{ .compatible = "auo,b116xw03" },
+	{ .compatible = "auo,b133htn01" },
 	{ }
 };