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

Albert's rework of the linker scripts conflicted with Simon's making
everyone use __bss_end.  We also had a minor conflict over
README.scrapyard being added to in mainline and enhanced in
u-boot-arm/master with proper formatting.

Conflicts:
	arch/arm/cpu/ixp/u-boot.lds
	arch/arm/cpu/u-boot.lds
	arch/arm/lib/Makefile
	board/actux1/u-boot.lds
	board/actux2/u-boot.lds
	board/actux3/u-boot.lds
	board/dvlhost/u-boot.lds
	board/freescale/mx31ads/u-boot.lds
	doc/README.scrapyard
	include/configs/tegra-common.h

Build tested for all of ARM and run-time tested on am335x_evm.

Signed-off-by: Tom Rini <trini@ti.com>

26 files changed, 1104 insertions, 451 deletions

diff --git a/drivers/gpio/da8xx_gpio.c b/drivers/gpio/da8xx_gpio.c
index ed6a118..76648d2 100644
--- a/drivers/gpio/da8xx_gpio.c
+++ b/drivers/gpio/da8xx_gpio.c
@@ -34,6 +34,138 @@ static struct gpio_registry {
 #if defined(CONFIG_SOC_DA8XX)
 #define pinmux(x)       (&davinci_syscfg_regs->pinmux[x])
 
+#if defined(CONFIG_SOC_DA8XX) && !defined(CONFIG_SOC_DA850)
+static const struct pinmux_config gpio_pinmux[] = {
+	{ pinmux(13), 8, 6 },	/* GP0[0] */
+	{ pinmux(13), 8, 7 },
+	{ pinmux(14), 8, 0 },
+	{ pinmux(14), 8, 1 },
+	{ pinmux(14), 8, 2 },
+	{ pinmux(14), 8, 3 },
+	{ pinmux(14), 8, 4 },
+	{ pinmux(14), 8, 5 },
+	{ pinmux(14), 8, 6 },
+	{ pinmux(14), 8, 7 },
+	{ pinmux(15), 8, 0 },
+	{ pinmux(15), 8, 1 },
+	{ pinmux(15), 8, 2 },
+	{ pinmux(15), 8, 3 },
+	{ pinmux(15), 8, 4 },
+	{ pinmux(15), 8, 5 },
+	{ pinmux(15), 8, 6 },	/* GP1[0] */
+	{ pinmux(15), 8, 7 },
+	{ pinmux(16), 8, 0 },
+	{ pinmux(16), 8, 1 },
+	{ pinmux(16), 8, 2 },
+	{ pinmux(16), 8, 3 },
+	{ pinmux(16), 8, 4 },
+	{ pinmux(16), 8, 5 },
+	{ pinmux(16), 8, 6 },
+	{ pinmux(16), 8, 7 },
+	{ pinmux(17), 8, 0 },
+	{ pinmux(17), 8, 1 },
+	{ pinmux(17), 8, 2 },
+	{ pinmux(17), 8, 3 },
+	{ pinmux(17), 8, 4 },
+	{ pinmux(17), 8, 5 },
+	{ pinmux(17), 8, 6 },	/* GP2[0] */
+	{ pinmux(17), 8, 7 },
+	{ pinmux(18), 8, 0 },
+	{ pinmux(18), 8, 1 },
+	{ pinmux(18), 8, 2 },
+	{ pinmux(18), 8, 3 },
+	{ pinmux(18), 8, 4 },
+	{ pinmux(18), 8, 5 },
+	{ pinmux(18), 8, 6 },
+	{ pinmux(18), 8, 7 },
+	{ pinmux(19), 8, 0 },
+	{ pinmux(9), 8, 2 },
+	{ pinmux(9), 8, 3 },
+	{ pinmux(9), 8, 4 },
+	{ pinmux(9), 8, 5 },
+	{ pinmux(9), 8, 6 },
+	{ pinmux(10), 8, 1 },	/* GP3[0] */
+	{ pinmux(10), 8, 2 },
+	{ pinmux(10), 8, 3 },
+	{ pinmux(10), 8, 4 },
+	{ pinmux(10), 8, 5 },
+	{ pinmux(10), 8, 6 },
+	{ pinmux(10), 8, 7 },
+	{ pinmux(11), 8, 0 },
+	{ pinmux(11), 8, 1 },
+	{ pinmux(11), 8, 2 },
+	{ pinmux(11), 8, 3 },
+	{ pinmux(11), 8, 4 },
+	{ pinmux(9), 8, 7 },
+	{ pinmux(2), 8, 6 },
+	{ pinmux(11), 8, 5 },
+	{ pinmux(11), 8, 6 },
+	{ pinmux(12), 8, 4 },	/* GP4[0] */
+	{ pinmux(12), 8, 5 },
+	{ pinmux(12), 8, 6 },
+	{ pinmux(12), 8, 7 },
+	{ pinmux(13), 8, 0 },
+	{ pinmux(13), 8, 1 },
+	{ pinmux(13), 8, 2 },
+	{ pinmux(13), 8, 3 },
+	{ pinmux(13), 8, 4 },
+	{ pinmux(13), 8, 5 },
+	{ pinmux(11), 8, 7 },
+	{ pinmux(12), 8, 0 },
+	{ pinmux(12), 8, 1 },
+	{ pinmux(12), 8, 2 },
+	{ pinmux(12), 8, 3 },
+	{ pinmux(9), 8, 1 },
+	{ pinmux(7), 8, 3 },	/* GP5[0] */
+	{ pinmux(7), 8, 4 },
+	{ pinmux(7), 8, 5 },
+	{ pinmux(7), 8, 6 },
+	{ pinmux(7), 8, 7 },
+	{ pinmux(8), 8, 0 },
+	{ pinmux(8), 8, 1 },
+	{ pinmux(8), 8, 2 },
+	{ pinmux(8), 8, 3 },
+	{ pinmux(8), 8, 4 },
+	{ pinmux(8), 8, 5 },
+	{ pinmux(8), 8, 6 },
+	{ pinmux(8), 8, 7 },
+	{ pinmux(9), 8, 0 },
+	{ pinmux(7), 8, 1 },
+	{ pinmux(7), 8, 2 },
+	{ pinmux(5), 8, 1 },	/* GP6[0] */
+	{ pinmux(5), 8, 2 },
+	{ pinmux(5), 8, 3 },
+	{ pinmux(5), 8, 4 },
+	{ pinmux(5), 8, 5 },
+	{ pinmux(5), 8, 6 },
+	{ pinmux(5), 8, 7 },
+	{ pinmux(6), 8, 0 },
+	{ pinmux(6), 8, 1 },
+	{ pinmux(6), 8, 2 },
+	{ pinmux(6), 8, 3 },
+	{ pinmux(6), 8, 4 },
+	{ pinmux(6), 8, 5 },
+	{ pinmux(6), 8, 6 },
+	{ pinmux(6), 8, 7 },
+	{ pinmux(7), 8, 0 },
+	{ pinmux(1), 8, 0 },	/* GP7[0] */
+	{ pinmux(1), 8, 1 },
+	{ pinmux(1), 8, 2 },
+	{ pinmux(1), 8, 3 },
+	{ pinmux(1), 8, 4 },
+	{ pinmux(1), 8, 5 },
+	{ pinmux(1), 8, 6 },
+	{ pinmux(1), 8, 7 },
+	{ pinmux(2), 8, 0 },
+	{ pinmux(2), 8, 1 },
+	{ pinmux(2), 8, 2 },
+	{ pinmux(2), 8, 3 },
+	{ pinmux(2), 8, 4 },
+	{ pinmux(2), 8, 5 },
+	{ pinmux(0), 1, 0 },
+	{ pinmux(0), 1, 1 },
+};
+#else /* CONFIG_SOC_DA8XX && CONFIG_SOC_DA850 */
 static const struct pinmux_config gpio_pinmux[] = {
 	{ pinmux(1), 8, 7 },	/* GP0[0] */
 	{ pinmux(1), 8, 6 },
@@ -180,9 +312,10 @@ static const struct pinmux_config gpio_pinmux[] = {
 	{ pinmux(18), 8, 3 },
 	{ pinmux(18), 8, 2 },
 };
-#else
+#endif /* CONFIG_SOC_DA8XX && !CONFIG_SOC_DA850 */
+#else /* !CONFIG_SOC_DA8XX */
 #define davinci_configure_pin_mux(a, b)
-#endif
+#endif /* CONFIG_SOC_DA8XX */
 
 int gpio_request(unsigned gpio, const char *label)
 {
diff --git a/drivers/i2c/tegra_i2c.c b/drivers/i2c/tegra_i2c.c
index efc77fa..ca71cd3 100644
--- a/drivers/i2c/tegra_i2c.c
+++ b/drivers/i2c/tegra_i2c.c
@@ -46,6 +46,7 @@ struct i2c_bus {
 	struct i2c_control	*control;
 	struct i2c_ctlr		*regs;
 	int			is_dvc;	/* DVC type, rather than I2C */
+	int			is_scs;	/* single clock source (T114+) */
 	int			inited;	/* bus is inited */
 };
 
@@ -88,7 +89,28 @@ static void i2c_init_controller(struct i2c_bus *i2c_bus)
 	 * 16 to get the right frequency.
 	 */
 	clock_start_periph_pll(i2c_bus->periph_id, CLOCK_ID_PERIPH,
-			       i2c_bus->speed * 2 * 8);
+		i2c_bus->speed * 2 * 8);
+
+	if (i2c_bus->is_scs) {
+		/*
+		 * T114 I2C went to a single clock source for standard/fast and
+		 * HS clock speeds. The new clock rate setting calculation is:
+		 *  SCL = CLK_SOURCE.I2C /
+		 *   (CLK_MULT_STD_FAST_MODE * (I2C_CLK_DIV_STD_FAST_MODE+1) *
+		 *   I2C FREQUENCY DIVISOR) as per the T114 TRM (sec 30.3.1).
+		 *
+		 * NOTE: We do this here, after the initial clock/pll start,
+		 * because if we read the clk_div reg before the controller
+		 * is running, we hang, and we need it for the new calc.
+		 */
+		int clk_div_stdfst_mode = readl(&i2c_bus->regs->clk_div) >> 16;
+		debug("%s: CLK_DIV_STD_FAST_MODE setting = %d\n", __func__,
+			clk_div_stdfst_mode);
+
+		clock_start_periph_pll(i2c_bus->periph_id, CLOCK_ID_PERIPH,
+			CLK_MULT_STD_FAST_MODE * (clk_div_stdfst_mode + 1) *
+			i2c_bus->speed * 2);
+	}
 
 	/* Reset I2C controller. */
 	i2c_reset_controller(i2c_bus);
@@ -352,10 +374,11 @@ static int i2c_get_config(const void *blob, int node, struct i2c_bus *i2c_bus)
  * @param node_list	list of nodes to process (any <=0 are ignored)
  * @param count		number of nodes to process
  * @param is_dvc	1 if these are DVC ports, 0 if standard I2C
+ * @param is_scs	1 if this HW uses a single clock source (T114+)
  * @return 0 if ok, -1 on error
  */
 static int process_nodes(const void *blob, int node_list[], int count,
-			 int is_dvc)
+			 int is_dvc, int is_scs)
 {
 	struct i2c_bus *i2c_bus;
 	int i;
@@ -375,6 +398,8 @@ static int process_nodes(const void *blob, int node_list[], int count,
 			return -1;
 		}
 
+		i2c_bus->is_scs = is_scs;
+
 		i2c_bus->is_dvc = is_dvc;
 		if (is_dvc) {
 			i2c_bus->control =
@@ -403,18 +428,25 @@ void i2c_init_board(void)
 	const void *blob = gd->fdt_blob;
 	int count;
 
-	/* First get the normal i2c ports */
+	/* First check for newer (T114+) I2C ports */
+	count = fdtdec_find_aliases_for_id(blob, "i2c",
+			COMPAT_NVIDIA_TEGRA114_I2C, node_list,
+			TEGRA_I2C_NUM_CONTROLLERS);
+	if (process_nodes(blob, node_list, count, 0, 1))
+		return;
+
+	/* Now get the older (T20/T30) normal I2C ports */
 	count = fdtdec_find_aliases_for_id(blob, "i2c",
 			COMPAT_NVIDIA_TEGRA20_I2C, node_list,
 			TEGRA_I2C_NUM_CONTROLLERS);
-	if (process_nodes(blob, node_list, count, 0))
+	if (process_nodes(blob, node_list, count, 0, 0))
 		return;
 
 	/* Now look for dvc ports */
 	count = fdtdec_add_aliases_for_id(blob, "i2c",
 			COMPAT_NVIDIA_TEGRA20_DVC, node_list,
 			TEGRA_I2C_NUM_CONTROLLERS);
-	if (process_nodes(blob, node_list, count, 1))
+	if (process_nodes(blob, node_list, count, 1, 0))
 		return;
 }
 
diff --git a/drivers/mmc/arm_pl180_mmci.c b/drivers/mmc/arm_pl180_mmci.c
index af1380a..ab2e81e 100644
--- a/drivers/mmc/arm_pl180_mmci.c
+++ b/drivers/mmc/arm_pl180_mmci.c
@@ -377,6 +377,7 @@ int arm_pl180_mmci_init(struct pl180_mmc_host *host)
 	dev->set_ios = host_set_ios;
 	dev->init = mmc_host_reset;
 	dev->getcd = NULL;
+	dev->getwp = NULL;
 	dev->host_caps = host->caps;
 	dev->voltages = host->voltages;
 	dev->f_min = host->clock_min;
diff --git a/drivers/mmc/bfin_sdh.c b/drivers/mmc/bfin_sdh.c
index 0f98b96..2631174 100644
--- a/drivers/mmc/bfin_sdh.c
+++ b/drivers/mmc/bfin_sdh.c
@@ -287,6 +287,7 @@ int bfin_mmc_init(bd_t *bis)
 	mmc->set_ios = bfin_sdh_set_ios;
 	mmc->init = bfin_sdh_init;
 	mmc->getcd = NULL;
+	mmc->getwp = NULL;
 	mmc->host_caps = MMC_MODE_4BIT;
 
 	mmc->voltages = MMC_VDD_32_33 | MMC_VDD_33_34;
diff --git a/drivers/mmc/davinci_mmc.c b/drivers/mmc/davinci_mmc.c
index ee8f261..e2379e3 100644
--- a/drivers/mmc/davinci_mmc.c
+++ b/drivers/mmc/davinci_mmc.c
@@ -388,6 +388,7 @@ int davinci_mmc_init(bd_t *bis, struct davinci_mmc *host)
 	mmc->set_ios = dmmc_set_ios;
 	mmc->init = dmmc_init;
 	mmc->getcd = NULL;
+	mmc->getwp = NULL;
 
 	mmc->f_min = 200000;
 	mmc->f_max = 25000000;
diff --git a/drivers/mmc/fsl_esdhc.c b/drivers/mmc/fsl_esdhc.c
index b90f3e7..54b5363 100644
--- a/drivers/mmc/fsl_esdhc.c
+++ b/drivers/mmc/fsl_esdhc.c
@@ -552,6 +552,7 @@ int fsl_esdhc_initialize(bd_t *bis, struct fsl_esdhc_cfg *cfg)
 	mmc->set_ios = esdhc_set_ios;
 	mmc->init = esdhc_init;
 	mmc->getcd = esdhc_getcd;
+	mmc->getwp = NULL;
 
 	voltage_caps = 0;
 	caps = regs->hostcapblt;
diff --git a/drivers/mmc/ftsdc010_esdhc.c b/drivers/mmc/ftsdc010_esdhc.c
index f1702fe..42f0e0c 100644
--- a/drivers/mmc/ftsdc010_esdhc.c
+++ b/drivers/mmc/ftsdc010_esdhc.c
@@ -666,6 +666,7 @@ int ftsdc010_mmc_init(int dev_index)
 	mmc->set_ios = ftsdc010_set_ios;
 	mmc->init = ftsdc010_core_init;
 	mmc->getcd = NULL;
+	mmc->getwp = NULL;
 
 	mmc->voltages = MMC_VDD_32_33 | MMC_VDD_33_34;
 
diff --git a/drivers/mmc/gen_atmel_mci.c b/drivers/mmc/gen_atmel_mci.c
index 67b2dbe..70a9f91 100644
--- a/drivers/mmc/gen_atmel_mci.c
+++ b/drivers/mmc/gen_atmel_mci.c
@@ -349,6 +349,7 @@ int atmel_mci_init(void *regs)
 	mmc->set_ios = mci_set_ios;
 	mmc->init = mci_init;
 	mmc->getcd = NULL;
+	mmc->getwp = NULL;
 
 	/* need to be able to pass these in on a board by board basis */
 	mmc->voltages = MMC_VDD_32_33 | MMC_VDD_33_34;
diff --git a/drivers/mmc/mmc.c b/drivers/mmc/mmc.c
index 72e8ce6..7b5fdd9 100644
--- a/drivers/mmc/mmc.c
+++ b/drivers/mmc/mmc.c
@@ -40,6 +40,23 @@
 static struct list_head mmc_devices;
 static int cur_dev_num = -1;
 
+int __weak board_mmc_getwp(struct mmc *mmc)
+{
+	return -1;
+}
+
+int mmc_getwp(struct mmc *mmc)
+{
+	int wp;
+
+	wp = board_mmc_getwp(mmc);
+
+	if ((wp < 0) && mmc->getwp)
+		wp = mmc->getwp(mmc);
+
+	return wp;
+}
+
 int __board_mmc_getcd(struct mmc *mmc) {
 	return -1;
 }
diff --git a/drivers/mmc/mmc_spi.c b/drivers/mmc/mmc_spi.c
index 11ba532..fe6a5a1 100644
--- a/drivers/mmc/mmc_spi.c
+++ b/drivers/mmc/mmc_spi.c
@@ -273,6 +273,7 @@ struct mmc *mmc_spi_init(uint bus, uint cs, uint speed, uint mode)
 	mmc->set_ios = mmc_spi_set_ios;
 	mmc->init = mmc_spi_init_p;
 	mmc->getcd = NULL;
+	mmc->getwp = NULL;
 	mmc->host_caps = MMC_MODE_SPI;
 
 	mmc->voltages = MMC_SPI_VOLTAGE;
diff --git a/drivers/mmc/mxcmmc.c b/drivers/mmc/mxcmmc.c
index d58c18b..4f99617 100644
--- a/drivers/mmc/mxcmmc.c
+++ b/drivers/mmc/mxcmmc.c
@@ -499,6 +499,7 @@ static int mxcmci_initialize(bd_t *bis)
 	mmc->set_ios = mxcmci_set_ios;
 	mmc->init = mxcmci_init;
 	mmc->getcd = NULL;
+	mmc->getwp = NULL;
 	mmc->host_caps = MMC_MODE_4BIT;
 
 	host->base = (struct mxcmci_regs *)CONFIG_MXC_MCI_REGS_BASE;
diff --git a/drivers/mmc/mxsmmc.c b/drivers/mmc/mxsmmc.c
index a72f66c..a89660f 100644
--- a/drivers/mmc/mxsmmc.c
+++ b/drivers/mmc/mxsmmc.c
@@ -53,12 +53,6 @@ struct mxsmmc_priv {
 	struct mxs_dma_desc	*desc;
 };
 
-#if defined(CONFIG_MX23)
-static const unsigned int mxsmmc_id_offset = 1;
-#elif defined(CONFIG_MX28)
-static const unsigned int mxsmmc_id_offset = 0;
-#endif
-
 #define	MXSMMC_MAX_TIMEOUT	10000
 #define MXSMMC_SMALL_TRANSFER	512
 
@@ -137,7 +131,7 @@ static int mxsmmc_send_cmd_dma(struct mxsmmc_priv *priv, struct mmc_data *data)
 	priv->desc->cmd.data |= MXS_DMA_DESC_IRQ | MXS_DMA_DESC_DEC_SEM |
 				(data_count << MXS_DMA_DESC_BYTES_OFFSET);
 
-	dmach = MXS_DMA_CHANNEL_AHB_APBH_SSP0 + priv->id + mxsmmc_id_offset;
+	dmach = MXS_DMA_CHANNEL_AHB_APBH_SSP0 + priv->id;
 	mxs_dma_desc_append(dmach, priv->desc);
 	if (mxs_dma_go(dmach)) {
 		bounce_buffer_stop(&bbstate);
@@ -390,15 +384,9 @@ int mxsmmc_initialize(bd_t *bis, int id, int (*wp)(int), int (*cd)(int))
 	struct mmc *mmc = NULL;
 	struct mxsmmc_priv *priv = NULL;
 	int ret;
-#if defined(CONFIG_MX23)
-	const unsigned int mxsmmc_max_id = 2;
-	const unsigned int mxsmmc_clk_id = 0;
-#elif defined(CONFIG_MX28)
-	const unsigned int mxsmmc_max_id = 4;
-	const unsigned int mxsmmc_clk_id = id;
-#endif
+	const unsigned int mxsmmc_clk_id = mxs_ssp_clock_by_bus(id);
 
-	if (id >= mxsmmc_max_id)
+	if (!mxs_ssp_bus_id_valid(id))
 		return -ENODEV;
 
 	mmc = malloc(sizeof(struct mmc));
@@ -418,7 +406,7 @@ int mxsmmc_initialize(bd_t *bis, int id, int (*wp)(int), int (*cd)(int))
 		return -ENOMEM;
 	}
 
-	ret = mxs_dma_init_channel(id + mxsmmc_id_offset);
+	ret = mxs_dma_init_channel(MXS_DMA_CHANNEL_AHB_APBH_SSP0 + id);
 	if (ret)
 		return ret;
 
@@ -432,6 +420,7 @@ int mxsmmc_initialize(bd_t *bis, int id, int (*wp)(int), int (*cd)(int))
 	mmc->set_ios = mxsmmc_set_ios;
 	mmc->init = mxsmmc_init;
 	mmc->getcd = NULL;
+	mmc->getwp = NULL;
 	mmc->priv = priv;
 
 	mmc->voltages = MMC_VDD_32_33 | MMC_VDD_33_34;
diff --git a/drivers/mmc/omap_hsmmc.c b/drivers/mmc/omap_hsmmc.c
index afd9b30..67cfcc2 100644
--- a/drivers/mmc/omap_hsmmc.c
+++ b/drivers/mmc/omap_hsmmc.c
@@ -30,6 +30,7 @@
 #include <twl4030.h>
 #include <twl6030.h>
 #include <twl6035.h>
+#include <asm/gpio.h>
 #include <asm/io.h>
 #include <asm/arch/mmc_host_def.h>
 #include <asm/arch/sys_proto.h>
@@ -38,30 +39,71 @@
 #define SYSCTL_SRC	(1 << 25)
 #define SYSCTL_SRD	(1 << 26)
 
+struct omap_hsmmc_data {
+	struct hsmmc *base_addr;
+	int cd_gpio;
+	int wp_gpio;
+};
+
 /* If we fail after 1 second wait, something is really bad */
 #define MAX_RETRY_MS	1000
 
 static int mmc_read_data(struct hsmmc *mmc_base, char *buf, unsigned int size);
 static int mmc_write_data(struct hsmmc *mmc_base, const char *buf,
 			unsigned int siz);
-static struct mmc hsmmc_dev[2];
+static struct mmc hsmmc_dev[3];
+static struct omap_hsmmc_data hsmmc_dev_data[3];
+
+#if (defined(CONFIG_OMAP_GPIO) && !defined(CONFIG_SPL_BUILD)) || \
+	(defined(CONFIG_SPL_BUILD) && defined(CONFIG_SPL_GPIO_SUPPORT))
+static int omap_mmc_setup_gpio_in(int gpio, const char *label)
+{
+	if (!gpio_is_valid(gpio))
+		return -1;
+
+	if (gpio_request(gpio, label) < 0)
+		return -1;
+
+	if (gpio_direction_input(gpio) < 0)
+		return -1;
+
+	return gpio;
+}
+
+static int omap_mmc_getcd(struct mmc *mmc)
+{
+	int cd_gpio = ((struct omap_hsmmc_data *)mmc->priv)->cd_gpio;
+	return gpio_get_value(cd_gpio);
+}
+
+static int omap_mmc_getwp(struct mmc *mmc)
+{
+	int wp_gpio = ((struct omap_hsmmc_data *)mmc->priv)->wp_gpio;
+	return gpio_get_value(wp_gpio);
+}
+#else
+static inline int omap_mmc_setup_gpio_in(int gpio, const char *label)
+{
+	return -1;
+}
+
+#define omap_mmc_getcd NULL
+#define omap_mmc_getwp NULL
+#endif
 
 #if defined(CONFIG_OMAP44XX) && defined(CONFIG_TWL6030_POWER)
 static void omap4_vmmc_pbias_config(struct mmc *mmc)
 {
 	u32 value = 0;
-	struct omap_sys_ctrl_regs *const ctrl =
-		(struct omap_sys_ctrl_regs *) SYSCTRL_GENERAL_CORE_BASE;
 
-
-	value = readl(&ctrl->control_pbiaslite);
+	value = readl((*ctrl)->control_pbiaslite);
 	value &= ~(MMC1_PBIASLITE_PWRDNZ | MMC1_PWRDNZ);
-	writel(value, &ctrl->control_pbiaslite);
+	writel(value, (*ctrl)->control_pbiaslite);
 	/* set VMMC to 3V */
 	twl6030_power_mmc_init();
-	value = readl(&ctrl->control_pbiaslite);
+	value = readl((*ctrl)->control_pbiaslite);
 	value |= MMC1_PBIASLITE_VMODE | MMC1_PBIASLITE_PWRDNZ | MMC1_PWRDNZ;
-	writel(value, &ctrl->control_pbiaslite);
+	writel(value, (*ctrl)->control_pbiaslite);
 }
 #endif
 
@@ -69,26 +111,24 @@ static void omap4_vmmc_pbias_config(struct mmc *mmc)
 static void omap5_pbias_config(struct mmc *mmc)
 {
 	u32 value = 0;
-	struct omap_sys_ctrl_regs *const ctrl =
-		(struct omap_sys_ctrl_regs *) SYSCTRL_GENERAL_CORE_BASE;
 
-	value = readl(&ctrl->control_pbias);
+	value = readl((*ctrl)->control_pbias);
 	value &= ~(SDCARD_PWRDNZ | SDCARD_BIAS_PWRDNZ);
 	value |= SDCARD_BIAS_HIZ_MODE;
-	writel(value, &ctrl->control_pbias);
+	writel(value, (*ctrl)->control_pbias);
 
 	twl6035_mmc1_poweron_ldo();
 
-	value = readl(&ctrl->control_pbias);
+	value = readl((*ctrl)->control_pbias);
 	value &= ~SDCARD_BIAS_HIZ_MODE;
 	value |= SDCARD_PBIASLITE_VMODE | SDCARD_PWRDNZ | SDCARD_BIAS_PWRDNZ;
-	writel(value, &ctrl->control_pbias);
+	writel(value, (*ctrl)->control_pbias);
 
-	value = readl(&ctrl->control_pbias);
+	value = readl((*ctrl)->control_pbias);
 	if (value & (1 << 23)) {
 		value &= ~(SDCARD_PWRDNZ | SDCARD_BIAS_PWRDNZ);
 		value |= SDCARD_BIAS_HIZ_MODE;
-		writel(value, &ctrl->control_pbias);
+		writel(value, (*ctrl)->control_pbias);
 	}
 }
 #endif
@@ -177,11 +217,12 @@ void mmc_init_stream(struct hsmmc *mmc_base)
 
 static int mmc_init_setup(struct mmc *mmc)
 {
-	struct hsmmc *mmc_base = (struct hsmmc *)mmc->priv;
+	struct hsmmc *mmc_base;
 	unsigned int reg_val;
 	unsigned int dsor;
 	ulong start;
 
+	mmc_base = ((struct omap_hsmmc_data *)mmc->priv)->base_addr;
 	mmc_board_init(mmc);
 
 	writel(readl(&mmc_base->sysconfig) | MMC_SOFTRESET,
@@ -262,10 +303,11 @@ static void mmc_reset_controller_fsm(struct hsmmc *mmc_base, u32 bit)
 static int mmc_send_cmd(struct mmc *mmc, struct mmc_cmd *cmd,
 			struct mmc_data *data)
 {
-	struct hsmmc *mmc_base = (struct hsmmc *)mmc->priv;
+	struct hsmmc *mmc_base;
 	unsigned int flags, mmc_stat;
 	ulong start;
 
+	mmc_base = ((struct omap_hsmmc_data *)mmc->priv)->base_addr;
 	start = get_timer(0);
 	while ((readl(&mmc_base->pstate) & (DATI_MASK | CMDI_MASK)) != 0) {
 		if (get_timer(0) - start > MAX_RETRY_MS) {
@@ -489,10 +531,11 @@ static int mmc_write_data(struct hsmmc *mmc_base, const char *buf,
 
 static void mmc_set_ios(struct mmc *mmc)
 {
-	struct hsmmc *mmc_base = (struct hsmmc *)mmc->priv;
+	struct hsmmc *mmc_base;
 	unsigned int dsor = 0;
 	ulong start;
 
+	mmc_base = ((struct omap_hsmmc_data *)mmc->priv)->base_addr;
 	/* configue bus width */
 	switch (mmc->bus_width) {
 	case 8:
@@ -540,36 +583,40 @@ static void mmc_set_ios(struct mmc *mmc)
 	writel(readl(&mmc_base->sysctl) | CEN_ENABLE, &mmc_base->sysctl);
 }
 
-int omap_mmc_init(int dev_index, uint host_caps_mask, uint f_max)
+int omap_mmc_init(int dev_index, uint host_caps_mask, uint f_max, int cd_gpio,
+		int wp_gpio)
 {
-	struct mmc *mmc;
-
-	mmc = &hsmmc_dev[dev_index];
+	struct mmc *mmc = &hsmmc_dev[dev_index];
+	struct omap_hsmmc_data *priv_data = &hsmmc_dev_data[dev_index];
 
 	sprintf(mmc->name, "OMAP SD/MMC");
 	mmc->send_cmd = mmc_send_cmd;
 	mmc->set_ios = mmc_set_ios;
 	mmc->init = mmc_init_setup;
-	mmc->getcd = NULL;
+	mmc->getcd = omap_mmc_getcd;
+	mmc->getwp = omap_mmc_getwp;
+	mmc->priv = priv_data;
 
 	switch (dev_index) {
 	case 0:
-		mmc->priv = (struct hsmmc *)OMAP_HSMMC1_BASE;
+		priv_data->base_addr = (struct hsmmc *)OMAP_HSMMC1_BASE;
 		break;
 #ifdef OMAP_HSMMC2_BASE
 	case 1:
-		mmc->priv = (struct hsmmc *)OMAP_HSMMC2_BASE;
+		priv_data->base_addr = (struct hsmmc *)OMAP_HSMMC2_BASE;
 		break;
 #endif
 #ifdef OMAP_HSMMC3_BASE
 	case 2:
-		mmc->priv = (struct hsmmc *)OMAP_HSMMC3_BASE;
+		priv_data->base_addr = (struct hsmmc *)OMAP_HSMMC3_BASE;
 		break;
 #endif
 	default:
-		mmc->priv = (struct hsmmc *)OMAP_HSMMC1_BASE;
+		priv_data->base_addr = (struct hsmmc *)OMAP_HSMMC1_BASE;
 		return 1;
 	}
+	priv_data->cd_gpio = omap_mmc_setup_gpio_in(cd_gpio, "mmc_cd");
+	priv_data->wp_gpio = omap_mmc_setup_gpio_in(wp_gpio, "mmc_wp");
 	mmc->voltages = MMC_VDD_32_33 | MMC_VDD_33_34 | MMC_VDD_165_195;
 	mmc->host_caps = (MMC_MODE_4BIT | MMC_MODE_HS_52MHz | MMC_MODE_HS |
 				MMC_MODE_HC) & ~host_caps_mask;
diff --git a/drivers/mmc/sdhci.c b/drivers/mmc/sdhci.c
index b9cbe34..daca0ea 100644
--- a/drivers/mmc/sdhci.c
+++ b/drivers/mmc/sdhci.c
@@ -438,6 +438,7 @@ int add_sdhci(struct sdhci_host *host, u32 max_clk, u32 min_clk)
 	mmc->set_ios = sdhci_set_ios;
 	mmc->init = sdhci_init;
 	mmc->getcd = NULL;
+	mmc->getwp = NULL;
 
 	caps = sdhci_readl(host, SDHCI_CAPABILITIES);
 #ifdef CONFIG_MMC_SDMA
diff --git a/drivers/mmc/sh_mmcif.c b/drivers/mmc/sh_mmcif.c
index 4588568..011d4f3 100644
--- a/drivers/mmc/sh_mmcif.c
+++ b/drivers/mmc/sh_mmcif.c
@@ -599,6 +599,7 @@ int mmcif_mmc_init(void)
 	mmc->set_ios = sh_mmcif_set_ios;
 	mmc->init = sh_mmcif_init;
 	mmc->getcd = NULL;
+	mmc->getwp = NULL;
 	host->regs = (struct sh_mmcif_regs *)CONFIG_SH_MMCIF_ADDR;
 	host->clk = CONFIG_SH_MMCIF_CLK;
 	mmc->priv = host;
diff --git a/drivers/mmc/tegra_mmc.c b/drivers/mmc/tegra_mmc.c
index d749ab0..e86bc68 100644
--- a/drivers/mmc/tegra_mmc.c
+++ b/drivers/mmc/tegra_mmc.c
@@ -2,7 +2,7 @@
  * (C) Copyright 2009 SAMSUNG Electronics
  * Minkyu Kang <mk7.kang@samsung.com>
  * Jaehoon Chung <jh80.chung@samsung.com>
- * Portions Copyright 2011-2012 NVIDIA Corporation
+ * Portions Copyright 2011-2013 NVIDIA Corporation
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -28,43 +28,45 @@
 #include <asm/arch-tegra/tegra_mmc.h>
 #include <mmc.h>
 
-/* support 4 mmc hosts */
-struct mmc mmc_dev[4];
-struct mmc_host mmc_host[4];
+DECLARE_GLOBAL_DATA_PTR;
 
+struct mmc mmc_dev[MAX_HOSTS];
+struct mmc_host mmc_host[MAX_HOSTS];
 
-/**
- * Get the host address and peripheral ID for a device. Devices are numbered
- * from 0 to 3.
- *
- * @param host		Structure to fill in (base, reg, mmc_id)
- * @param dev_index	Device index (0-3)
- */
-static void tegra_get_setup(struct mmc_host *host, int dev_index)
+#ifndef CONFIG_OF_CONTROL
+#error "Please enable device tree support to use this driver"
+#endif
+
+static void mmc_set_power(struct mmc_host *host, unsigned short power)
 {
-	debug("tegra_get_setup: dev_index = %d\n", dev_index);
-
-	switch (dev_index) {
-	case 1:
-		host->base = TEGRA_SDMMC3_BASE;
-		host->mmc_id = PERIPH_ID_SDMMC3;
-		break;
-	case 2:
-		host->base = TEGRA_SDMMC2_BASE;
-		host->mmc_id = PERIPH_ID_SDMMC2;
-		break;
-	case 3:
-		host->base = TEGRA_SDMMC1_BASE;
-		host->mmc_id = PERIPH_ID_SDMMC1;
-		break;
-	case 0:
-	default:
-		host->base = TEGRA_SDMMC4_BASE;
-		host->mmc_id = PERIPH_ID_SDMMC4;
-		break;
+	u8 pwr = 0;
+	debug("%s: power = %x\n", __func__, power);
+
+	if (power != (unsigned short)-1) {
+		switch (1 << power) {
+		case MMC_VDD_165_195:
+			pwr = TEGRA_MMC_PWRCTL_SD_BUS_VOLTAGE_V1_8;
+			break;
+		case MMC_VDD_29_30:
+		case MMC_VDD_30_31:
+			pwr = TEGRA_MMC_PWRCTL_SD_BUS_VOLTAGE_V3_0;
+			break;
+		case MMC_VDD_32_33:
+		case MMC_VDD_33_34:
+			pwr = TEGRA_MMC_PWRCTL_SD_BUS_VOLTAGE_V3_3;
+			break;
+		}
 	}
+	debug("%s: pwr = %X\n", __func__, pwr);
 
-	host->reg = (struct tegra_mmc *)host->base;
+	/* Set the bus voltage first (if any) */
+	writeb(pwr, &host->reg->pwrcon);
+	if (pwr == 0)
+		return;
+
+	/* Now enable bus power */
+	pwr |= TEGRA_MMC_PWRCTL_SD_BUS_POWER;
+	writeb(pwr, &host->reg->pwrcon);
 }
 
 static void mmc_prepare_data(struct mmc_host *host, struct mmc_data *data,
@@ -363,8 +365,7 @@ static void mmc_change_clock(struct mmc_host *host, uint clock)
 	debug(" mmc_change_clock called\n");
 
 	/*
-	 * Change Tegra SDMMCx clock divisor here. Source is 216MHz,
-	 * PLLP_OUT0
+	 * Change Tegra SDMMCx clock divisor here. Source is PLLP_OUT0
 	 */
 	if (clock == 0)
 		goto out;
@@ -439,7 +440,7 @@ static void mmc_set_ios(struct mmc *mmc)
 	debug("mmc_set_ios: hostctl = %08X\n", ctrl);
 }
 
-static void mmc_reset(struct mmc_host *host)
+static void mmc_reset(struct mmc_host *host, struct mmc *mmc)
 {
 	unsigned int timeout;
 	debug(" mmc_reset called\n");
@@ -465,6 +466,14 @@ static void mmc_reset(struct mmc_host *host)
 		timeout--;
 		udelay(1000);
 	}
+
+	/* Set SD bus voltage & enable bus power */
+	mmc_set_power(host, fls(mmc->voltages) - 1);
+	debug("%s: power control = %02X, host control = %02X\n", __func__,
+		readb(&host->reg->pwrcon), readb(&host->reg->hostctl));
+
+	/* Make sure SDIO pads are set up */
+	pad_init_mmc(host);
 }
 
 static int mmc_core_init(struct mmc *mmc)
@@ -473,7 +482,7 @@ static int mmc_core_init(struct mmc *mmc)
 	unsigned int mask;
 	debug(" mmc_core_init called\n");
 
-	mmc_reset(host);
+	mmc_reset(host, mmc);
 
 	host->version = readw(&host->reg->hcver);
 	debug("host version = %x\n", host->version);
@@ -518,41 +527,43 @@ int tegra_mmc_getcd(struct mmc *mmc)
 
 	debug("tegra_mmc_getcd called\n");
 
-	if (host->cd_gpio >= 0)
-		return !gpio_get_value(host->cd_gpio);
+	if (fdt_gpio_isvalid(&host->cd_gpio))
+		return fdtdec_get_gpio(&host->cd_gpio);
 
 	return 1;
 }
 
-int tegra_mmc_init(int dev_index, int bus_width, int pwr_gpio, int cd_gpio)
+static int do_mmc_init(int dev_index)
 {
 	struct mmc_host *host;
 	char gpusage[12]; /* "SD/MMCn PWR" or "SD/MMCn CD" */
 	struct mmc *mmc;
 
-	debug(" tegra_mmc_init: index %d, bus width %d "
-		"pwr_gpio %d cd_gpio %d\n",
-		dev_index, bus_width, pwr_gpio, cd_gpio);
-
+	/* DT should have been read & host config filled in */
 	host = &mmc_host[dev_index];
+	if (!host->enabled)
+		return -1;
 
-	host->clock = 0;
-	host->pwr_gpio = pwr_gpio;
-	host->cd_gpio = cd_gpio;
-	tegra_get_setup(host, dev_index);
+	debug(" do_mmc_init: index %d, bus width %d "
+		"pwr_gpio %d cd_gpio %d\n",
+		dev_index, host->width,
+		host->pwr_gpio.gpio, host->cd_gpio.gpio);
 
+	host->clock = 0;
 	clock_start_periph_pll(host->mmc_id, CLOCK_ID_PERIPH, 20000000);
 
-	if (host->pwr_gpio >= 0) {
+	if (fdt_gpio_isvalid(&host->pwr_gpio)) {
 		sprintf(gpusage, "SD/MMC%d PWR", dev_index);
-		gpio_request(host->pwr_gpio, gpusage);
-		gpio_direction_output(host->pwr_gpio, 1);
+		gpio_request(host->pwr_gpio.gpio, gpusage);
+		gpio_direction_output(host->pwr_gpio.gpio, 1);
+		debug(" Power GPIO name = %s\n", host->pwr_gpio.name);
 	}
 
-	if (host->cd_gpio >= 0) {
+	if (fdt_gpio_isvalid(&host->cd_gpio)) {
 		sprintf(gpusage, "SD/MMC%d CD", dev_index);
-		gpio_request(host->cd_gpio, gpusage);
-		gpio_direction_input(host->cd_gpio);
+		gpio_request(host->cd_gpio.gpio, gpusage);
+		gpio_direction_input(host->cd_gpio.gpio);
+		debug(" CD GPIO name = %s\n", host->cd_gpio.name);
 	}
 
 	mmc = &mmc_dev[dev_index];
@@ -563,12 +574,13 @@ int tegra_mmc_init(int dev_index, int bus_width, int pwr_gpio, int cd_gpio)
 	mmc->set_ios = mmc_set_ios;
 	mmc->init = mmc_core_init;
 	mmc->getcd = tegra_mmc_getcd;
+	mmc->getwp = NULL;
 
 	mmc->voltages = MMC_VDD_32_33 | MMC_VDD_33_34 | MMC_VDD_165_195;
 	mmc->host_caps = 0;
-	if (bus_width == 8)
+	if (host->width == 8)
 		mmc->host_caps |= MMC_MODE_8BIT;
-	if (bus_width >= 4)
+	if (host->width >= 4)
 		mmc->host_caps |= MMC_MODE_4BIT;
 	mmc->host_caps |= MMC_MODE_HS_52MHz | MMC_MODE_HS | MMC_MODE_HC;
 
@@ -577,8 +589,6 @@ int tegra_mmc_init(int dev_index, int bus_width, int pwr_gpio, int cd_gpio)
 	 *  low-speed SDIO card frequency (actually 400KHz)
 	 * max freq is highest HS eMMC clock as per the SD/MMC spec
 	 *  (actually 52MHz)
-	 * Both of these are the closest equivalents w/216MHz source
-	 *  clock and Tegra SDMMC divisors.
 	 */
 	mmc->f_min = 375000;
 	mmc->f_max = 48000000;
@@ -587,3 +597,104 @@ int tegra_mmc_init(int dev_index, int bus_width, int pwr_gpio, int cd_gpio)
 
 	return 0;
 }
+
+/**
+ * Get the host address and peripheral ID for a node.
+ *
+ * @param blob		fdt blob
+ * @param node		Device index (0-3)
+ * @param host		Structure to fill in (reg, width, mmc_id)
+ */
+static int mmc_get_config(const void *blob, int node, struct mmc_host *host)
+{
+	debug("%s: node = %d\n", __func__, node);
+
+	host->enabled = fdtdec_get_is_enabled(blob, node);
+
+	host->reg = (struct tegra_mmc *)fdtdec_get_addr(blob, node, "reg");
+	if ((fdt_addr_t)host->reg == FDT_ADDR_T_NONE) {
+		debug("%s: no sdmmc base reg info found\n", __func__);
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	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;
+	}
+
+	/*
+	 * NOTE: mmc->bus_width is determined by mmc.c dynamically.
+	 * TBD: Override it with this value?
+	 */
+	host->width = fdtdec_get_int(blob, node, "bus-width", 0);
+	if (!host->width)
+		debug("%s: no sdmmc width found\n", __func__);
+
+	/* These GPIOs are optional */
+	fdtdec_decode_gpio(blob, node, "cd-gpios", &host->cd_gpio);
+	fdtdec_decode_gpio(blob, node, "wp-gpios", &host->wp_gpio);
+	fdtdec_decode_gpio(blob, node, "power-gpios", &host->pwr_gpio);
+
+	debug("%s: found controller at %p, width = %d, periph_id = %d\n",
+		__func__, host->reg, host->width, host->mmc_id);
+	return 0;
+}
+
+/*
+ * Process a list of nodes, adding them to our list of SDMMC ports.
+ *
+ * @param blob          fdt blob
+ * @param node_list     list of nodes to process (any <=0 are ignored)
+ * @param count         number of nodes to process
+ * @return 0 if ok, -1 on error
+ */
+static int process_nodes(const void *blob, int node_list[], int count)
+{
+	struct mmc_host *host;
+	int i, node;
+
+	debug("%s: count = %d\n", __func__, count);
+
+	/* build mmc_host[] for each controller */
+	for (i = 0; i < count; i++) {
+		node = node_list[i];
+		if (node <= 0)
+			continue;
+
+		host = &mmc_host[i];
+		host->id = i;
+
+		if (mmc_get_config(blob, node, host)) {
+			printf("%s: failed to decode dev %d\n",	__func__, i);
+			return -1;
+		}
+		do_mmc_init(i);
+	}
+	return 0;
+}
+
+void tegra_mmc_init(void)
+{
+	int node_list[MAX_HOSTS], count;
+	const void *blob = gd->fdt_blob;
+	debug("%s entry\n", __func__);
+
+	/* See if any Tegra30 MMC controllers are present */
+	count = fdtdec_find_aliases_for_id(blob, "sdhci",
+		COMPAT_NVIDIA_TEGRA30_SDMMC, node_list, MAX_HOSTS);
+	debug("%s: count of T30 sdhci nodes is %d\n", __func__, count);
+	if (process_nodes(blob, node_list, count)) {
+		printf("%s: Error processing T30 mmc node(s)!\n", __func__);
+		return;
+	}
+
+	/* Now look for any Tegra20 MMC controllers */
+	count = fdtdec_find_aliases_for_id(blob, "sdhci",
+		COMPAT_NVIDIA_TEGRA20_SDMMC, node_list, MAX_HOSTS);
+	debug("%s: count of T20 sdhci nodes is %d\n", __func__, count);
+	if (process_nodes(blob, node_list, count)) {
+		printf("%s: Error processing T20 mmc node(s)!\n", __func__);
+		return;
+	}
+}
diff --git a/drivers/mtd/onenand/onenand_spl.c b/drivers/mtd/onenand/onenand_spl.c
index 50eaa71..4bec2c2 100644
--- a/drivers/mtd/onenand/onenand_spl.c
+++ b/drivers/mtd/onenand/onenand_spl.c
@@ -112,7 +112,7 @@ static int onenand_spl_read_page(uint32_t block, uint32_t page, uint32_t *buf,
 void onenand_spl_load_image(uint32_t offs, uint32_t size, void *dst)
 {
 	uint32_t *addr = (uint32_t *)dst;
-	uint32_t total_pages;
+	uint32_t to_page;
 	uint32_t block;
 	uint32_t page, rpage;
 	enum onenand_spl_pagesize pagesize;
@@ -125,22 +125,20 @@ void onenand_spl_load_image(uint32_t offs, uint32_t size, void *dst)
 	 * pulling further unwanted functions into the SPL.
 	 */
 	if (pagesize == 2048) {
-		total_pages = DIV_ROUND_UP(size, 2048);
 		page = offs / 2048;
+		to_page = page + DIV_ROUND_UP(size, 2048);
 	} else {
-		total_pages = DIV_ROUND_UP(size, 4096);
 		page = offs / 4096;
+		to_page = page + DIV_ROUND_UP(size, 4096);
 	}
 
-	for (; page <= total_pages; page++) {
+	for (; page <= to_page; page++) {
 		block = page / ONENAND_PAGES_PER_BLOCK;
 		rpage = page & (ONENAND_PAGES_PER_BLOCK - 1);
 		ret = onenand_spl_read_page(block, rpage, addr, pagesize);
-		if (ret) {
-			total_pages += ONENAND_PAGES_PER_BLOCK;
+		if (ret)
 			page += ONENAND_PAGES_PER_BLOCK - 1;
-		} else {
+		else
 			addr += pagesize / 4;
-		}
 	}
 }
diff --git a/drivers/net/cpsw.c b/drivers/net/cpsw.c
index db04795..93f8417 100644
--- a/drivers/net/cpsw.c
+++ b/drivers/net/cpsw.c
@@ -227,6 +227,9 @@ struct cpsw_priv {
 	struct cpsw_slave		*slaves;
 	struct phy_device		*phydev;
 	struct mii_dev			*bus;
+
+	u32				mdio_link;
+	u32				phy_mask;
 };
 
 static inline int cpsw_ale_get_field(u32 *ale_entry, u32 start, u32 bits)
@@ -598,10 +601,21 @@ static int cpsw_update_link(struct cpsw_priv *priv)
 
 	for_each_slave(slave, priv)
 		cpsw_slave_update_link(slave, priv, &link);
-
+	priv->mdio_link = readl(&mdio_regs->link);
 	return link;
 }
 
+static int cpsw_check_link(struct cpsw_priv *priv)
+{
+	u32 link = 0;
+
+	link = __raw_readl(&mdio_regs->link) & priv->phy_mask;
+	if ((link) && (link == priv->mdio_link))
+		return 1;
+
+	return cpsw_update_link(priv);
+}
+
 static inline u32  cpsw_get_slave_port(struct cpsw_priv *priv, u32 slave_num)
 {
 	if (priv->host_port == 0)
@@ -631,6 +645,8 @@ static void cpsw_slave_init(struct cpsw_slave *slave, struct cpsw_priv *priv)
 	cpsw_ale_port_state(priv, slave_port, ALE_PORT_STATE_FORWARD);
 
 	cpsw_ale_add_mcast(priv, NetBcastAddr, 1 << slave_port);
+
+	priv->phy_mask |= 1 << slave->data->phy_id;
 }
 
 static struct cpdma_desc *cpdma_desc_alloc(struct cpsw_priv *priv)
@@ -862,7 +878,7 @@ static int cpsw_send(struct eth_device *dev, void *packet, int length)
 	int len;
 	int timeout = CPDMA_TIMEOUT;
 
-	if (!cpsw_update_link(priv))
+	if (!cpsw_check_link(priv))
 		return -EIO;
 
 	flush_dcache_range((unsigned long)packet,
diff --git a/drivers/serial/Makefile b/drivers/serial/Makefile
index 5e8b648..de3f471 100644
--- a/drivers/serial/Makefile
+++ b/drivers/serial/Makefile
@@ -33,7 +33,6 @@ COBJS-$(CONFIG_ARM_DCC) += arm_dcc.o
 COBJS-$(CONFIG_ATMEL_USART) += atmel_usart.o
 COBJS-$(CONFIG_LPC32XX_HSUART) += lpc32xx_hsuart.o
 COBJS-$(CONFIG_MCFUART) += mcfuart.o
-COBJS-$(CONFIG_NS9750_UART) += ns9750_serial.o
 COBJS-$(CONFIG_OPENCORES_YANU) += opencores_yanu.o
 COBJS-$(CONFIG_SYS_NS16550) += ns16550.o
 COBJS-$(CONFIG_S3C64XX) += s3c64xx.o
diff --git a/drivers/serial/ns9750_serial.c b/drivers/serial/ns9750_serial.c
deleted file mode 100644
index 85fc68a..0000000
--- a/drivers/serial/ns9750_serial.c
+++ /dev/null
@@ -1,218 +0,0 @@
-/***********************************************************************
- *
- * Copyright (C) 2004 by FS Forth-Systeme GmbH.
- * All rights reserved.
- *
- * $Id: ns9750_serial.c,v 1.1 2004/02/16 10:37:20 mpietrek Exp $
- * @Author: Markus Pietrek
- * @Descr: Serial driver for the NS9750. Only one UART is supported yet.
- * @References: [1] NS9750 Hardware Reference/December 2003
- * @TODO: Implement Character GAP Timer when chip is fixed for PLL bypass
- *
- * This program is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License as
- * published by the Free Software Foundation; either version 2 of
- * the License, or (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.	 See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
- * MA 02111-1307 USA
- *
- ***********************************************************************/
-
-#include <common.h>
-
-#include "ns9750_bbus.h"	/* for GPIOs */
-#include "ns9750_ser.h"		/* for serial configuration */
-
-DECLARE_GLOBAL_DATA_PTR;
-
-#if !defined(CONFIG_CONS_INDEX)
-#error "No console index specified."
-#endif
-
-#define CONSOLE CONFIG_CONS_INDEX
-
-static unsigned int calcBitrateRegister( void );
-static unsigned int calcRxCharGapRegister( void );
-
-static char cCharsAvailable; /* Numbers of chars in unCharCache */
-static unsigned int unCharCache; /* unCharCache is only valid if
-				  * cCharsAvailable > 0 */
-
-/***********************************************************************
- * @Function: serial_init
- * @Return: 0
- * @Descr: configures GPIOs and UART. Requires BBUS Master Reset turned off
- ***********************************************************************/
-
-static int ns9750_serial_init(void)
-{
-	unsigned int aunGPIOTxD[] = { 0, 8, 40, 44 };
-	unsigned int aunGPIORxD[] = { 1, 9, 41, 45 };
-
-	cCharsAvailable = 0;
-
-	/* configure TxD and RxD pins for their special function */
-	set_gpio_cfg_reg_val( aunGPIOTxD[ CONSOLE ],
-			      NS9750_GPIO_CFG_FUNC_0 | NS9750_GPIO_CFG_OUTPUT );
-	set_gpio_cfg_reg_val( aunGPIORxD[ CONSOLE ],
-			      NS9750_GPIO_CFG_FUNC_0 | NS9750_GPIO_CFG_INPUT );
-
-	/* configure serial engine */
-	*get_ser_reg_addr_channel( NS9750_SER_CTRL_A, CONSOLE ) =
-		NS9750_SER_CTRL_A_CE |
-		NS9750_SER_CTRL_A_STOP |
-		NS9750_SER_CTRL_A_WLS_8;
-
-	serial_setbrg();
-
-	*get_ser_reg_addr_channel( NS9750_SER_CTRL_B, CONSOLE ) =
-		NS9750_SER_CTRL_B_RCGT;
-
-	return 0;
-}
-
-/***********************************************************************
- * @Function: serial_putc
- * @Return: n/a
- * @Descr: writes one character to the FIFO. Blocks until FIFO is not full
- ***********************************************************************/
-
-static void ns9750_serial_putc(const char c)
-{
-	if (c == '\n')
-		serial_putc( '\r' );
-
-	while (!(*get_ser_reg_addr_channel( NS9750_SER_STAT_A, CONSOLE) &
-		 NS9750_SER_STAT_A_TRDY ) ) {
-		/* do nothing, wait for characters in FIFO sent */
-	}
-
-	*(volatile char*) get_ser_reg_addr_channel( NS9750_SER_FIFO,
-						    CONSOLE) = c;
-}
-
-/***********************************************************************
- * @Function: serial_getc
- * @Return: the character read
- * @Descr: performs only 8bit accesses to the FIFO. No error handling
- ***********************************************************************/
-
-static int ns9750_serial_getc(void)
-{
-	int i;
-
-	while (!serial_tstc() ) {
-		/* do nothing, wait for incoming characters */
-	}
-
-	/*  at least one character in unCharCache */
-	i = (int) (unCharCache & 0xff);
-
-	unCharCache >>= 8;
-	cCharsAvailable--;
-
-	return i;
-}
-
-/***********************************************************************
- * @Function: serial_tstc
- * @Return: 0 if no input available, otherwise != 0
- * @Descr: checks for incoming FIFO not empty. Stores the incoming chars in
- *	   unCharCache and the numbers of characters in cCharsAvailable
- ***********************************************************************/
-
-static int ns9750_serial_tstc(void)
-{
-	unsigned int unRegCache;
-
-	if ( cCharsAvailable )
-		return 1;
-
-	unRegCache = *get_ser_reg_addr_channel( NS9750_SER_STAT_A,CONSOLE );
-	if( unRegCache & NS9750_SER_STAT_A_RBC ) {
-		*get_ser_reg_addr_channel( NS9750_SER_STAT_A, CONSOLE ) =
-			NS9750_SER_STAT_A_RBC;
-		unRegCache = *get_ser_reg_addr_channel( NS9750_SER_STAT_A,
-							CONSOLE );
-	}
-
-	if ( unRegCache & NS9750_SER_STAT_A_RRDY ) {
-		cCharsAvailable = (unRegCache & NS9750_SER_STAT_A_RXFDB_MA)>>20;
-		if ( !cCharsAvailable )
-			cCharsAvailable = 4;
-
-		unCharCache = *get_ser_reg_addr_channel( NS9750_SER_FIFO,
-							 CONSOLE );
-		return 1;
-	}
-
-	/* no chars available */
-	return 0;
-}
-
-static void ns9750_serial_setbrg(void)
-{
-	*get_ser_reg_addr_channel( NS9750_SER_BITRATE, CONSOLE ) =
-		calcBitrateRegister();
-	*get_ser_reg_addr_channel( NS9750_SER_RX_CHAR_TIMER, CONSOLE ) =
-		calcRxCharGapRegister();
-}
-
-/***********************************************************************
- * @Function: calcBitrateRegister
- * @Return: value for the serial bitrate register
- * @Descr: register value depends on clock frequency and baudrate
- ***********************************************************************/
-
-static unsigned int calcBitrateRegister( void )
-{
-	return ( NS9750_SER_BITRATE_EBIT |
-		 NS9750_SER_BITRATE_CLKMUX_BCLK |
-		 NS9750_SER_BITRATE_TMODE |
-		 NS9750_SER_BITRATE_TCDR_16 |
-		 NS9750_SER_BITRATE_RCDR_16 |
-		 ( ( ( ( CONFIG_SYS_CLK_FREQ / 8 ) / /* BBUS clock,[1] Fig. 38 */
-		       ( gd->baudrate * 16 ) ) - 1 ) &
-		   NS9750_SER_BITRATE_N_MA ) );
-}
-
-/***********************************************************************
- * @Function: calcRxCharGapRegister
- * @Return: value for the character gap timer register
- * @Descr: register value depends on clock frequency and baudrate. Currently 0
- *	   is used as there is a bug with the gap timer in PLL bypass mode.
- ***********************************************************************/
-
-static unsigned int calcRxCharGapRegister( void )
-{
-	return NS9750_SER_RX_CHAR_TIMER_TRUN;
-}
-
-static struct serial_device ns9750_serial_drv = {
-	.name	= "ns9750_serial",
-	.start	= ns9750_serial_init,
-	.stop	= NULL,
-	.setbrg	= ns9750_serial_setbrg,
-	.putc	= ns9750_serial_putc,
-	.puts	= default_serial_puts,
-	.getc	= ns9750_serial_getc,
-	.tstc	= ns9750_serial_tstc,
-};
-
-void ns9750_serial_initialize(void)
-{
-	serial_register(&ns9750_serial_drv);
-}
-
-__weak struct serial_device *default_serial_console(void)
-{
-	return &ns9750_serial_drv;
-}
diff --git a/drivers/serial/serial.c b/drivers/serial/serial.c
index 1f8955a..7922bf0 100644
--- a/drivers/serial/serial.c
+++ b/drivers/serial/serial.c
@@ -164,7 +164,6 @@ serial_initfunc(altera_serial_initialize);
 serial_initfunc(atmel_serial_initialize);
 serial_initfunc(lpc32xx_serial_initialize);
 serial_initfunc(mcf_serial_initialize);
-serial_initfunc(ns9750_serial_initialize);
 serial_initfunc(oc_serial_initialize);
 serial_initfunc(s3c64xx_serial_initialize);
 serial_initfunc(sandbox_serial_initialize);
@@ -259,7 +258,6 @@ void serial_initialize(void)
 	atmel_serial_initialize();
 	lpc32xx_serial_initialize();
 	mcf_serial_initialize();
-	ns9750_serial_initialize();
 	oc_serial_initialize();
 	s3c64xx_serial_initialize();
 	sandbox_serial_initialize();
diff --git a/drivers/spi/mxs_spi.c b/drivers/spi/mxs_spi.c
index bb865b7..ffa3c1d 100644
--- a/drivers/spi/mxs_spi.c
+++ b/drivers/spi/mxs_spi.c
@@ -40,17 +40,6 @@
 
 #define MXSSSP_SMALL_TRANSFER	512
 
-/*
- * CONFIG_MXS_SPI_DMA_ENABLE: Experimental mixed PIO/DMA support for MXS SPI
- *                            host. Use with utmost caution!
- *
- *                            Enabling this is not yet recommended since this
- *                            still doesn't support transfers to/from unaligned
- *                            addresses. Therefore this driver will not work
- *                            for example with saving environment. This is
- *                            caused by DMA alignment constraints on MXS.
- */
-
 struct mxs_spi_slave {
 	struct spi_slave	slave;
 	uint32_t		max_khz;
@@ -70,7 +59,7 @@ void spi_init(void)
 int spi_cs_is_valid(unsigned int bus, unsigned int cs)
 {
 	/* MXS SPI: 4 ports and 3 chip selects maximum */
-	if (bus > 3 || cs > 2)
+	if (!mxs_ssp_bus_id_valid(bus) || cs > 2)
 		return 0;
 	else
 		return 1;
@@ -92,7 +81,7 @@ struct spi_slave *spi_setup_slave(unsigned int bus, unsigned int cs,
 	if (!mxs_slave)
 		return NULL;
 
-	if (mxs_dma_init_channel(bus))
+	if (mxs_dma_init_channel(MXS_DMA_CHANNEL_AHB_APBH_SSP0 + bus))
 		goto err_init;
 
 	mxs_slave->slave.bus = bus;
@@ -168,7 +157,12 @@ static int mxs_spi_xfer_pio(struct mxs_spi_slave *slave,
 
 	while (length--) {
 		/* We transfer 1 byte */
+#if defined(CONFIG_MX23)
+		writel(SSP_CTRL0_XFER_COUNT_MASK, &ssp_regs->hw_ssp_ctrl0_clr);
+		writel(1, &ssp_regs->hw_ssp_ctrl0_set);
+#elif defined(CONFIG_MX28)
 		writel(1, &ssp_regs->hw_ssp_xfer_size);
+#endif
 
 		if ((flags & SPI_XFER_END) && !length)
 			mxs_spi_end_xfer(ssp_regs);
@@ -226,6 +220,12 @@ static int mxs_spi_xfer_dma(struct mxs_spi_slave *slave,
 	int tl;
 	int ret = 0;
 
+#if defined(CONFIG_MX23)
+	const int mxs_spi_pio_words = 1;
+#elif defined(CONFIG_MX28)
+	const int mxs_spi_pio_words = 4;
+#endif
+
 	ALLOC_CACHE_ALIGN_BUFFER(struct mxs_dma_desc, desc, desc_count);
 
 	memset(desc, 0, sizeof(struct mxs_dma_desc) * desc_count);
@@ -281,7 +281,7 @@ static int mxs_spi_xfer_dma(struct mxs_spi_slave *slave,
 
 		dp->cmd.data |=
 			((tl & 0xffff) << MXS_DMA_DESC_BYTES_OFFSET) |
-			(4 << MXS_DMA_DESC_PIO_WORDS_OFFSET) |
+			(mxs_spi_pio_words << MXS_DMA_DESC_PIO_WORDS_OFFSET) |
 			MXS_DMA_DESC_HALT_ON_TERMINATE |
 			MXS_DMA_DESC_TERMINATE_FLUSH;
 
@@ -298,15 +298,19 @@ static int mxs_spi_xfer_dma(struct mxs_spi_slave *slave,
 		}
 
 		/*
-		 * Write CTRL0, CMD0, CMD1, XFER_SIZE registers. It is
+		 * Write CTRL0, CMD0, CMD1 and XFER_SIZE registers in
+		 * case of MX28, write only CTRL0 in case of MX23 due
+		 * to the difference in register layout. It is utterly
 		 * essential that the XFER_SIZE register is written on
 		 * a per-descriptor basis with the same size as is the
 		 * descriptor!
 		 */
 		dp->cmd.pio_words[0] = ctrl0;
+#ifdef CONFIG_MX28
 		dp->cmd.pio_words[1] = 0;
 		dp->cmd.pio_words[2] = 0;
 		dp->cmd.pio_words[3] = tl;
+#endif
 
 		mxs_dma_desc_append(dmach, dp);
 
@@ -332,12 +336,7 @@ int spi_xfer(struct spi_slave *slave, unsigned int bitlen,
 	char dummy;
 	int write = 0;
 	char *data = NULL;
-
-#ifdef CONFIG_MXS_SPI_DMA_ENABLE
 	int dma = 1;
-#else
-	int dma = 0;
-#endif
 
 	if (bitlen == 0) {
 		if (flags & SPI_XFER_END) {
diff --git a/drivers/usb/host/ehci-mx6.c b/drivers/usb/host/ehci-mx6.c
index 1b20e41..c1ae3d9 100644
--- a/drivers/usb/host/ehci-mx6.c
+++ b/drivers/usb/host/ehci-mx6.c
@@ -21,7 +21,6 @@
 #include <asm/io.h>
 #include <asm/arch/imx-regs.h>
 #include <asm/arch/clock.h>
-#include <asm/arch/mx6x_pins.h>
 #include <asm/imx-common/iomux-v3.h>
 
 #include "ehci.h"
diff --git a/drivers/usb/host/ehci-mxs.c b/drivers/usb/host/ehci-mxs.c
index 5062af5..f320d3e 100644
--- a/drivers/usb/host/ehci-mxs.c
+++ b/drivers/usb/host/ehci-mxs.c
@@ -21,91 +21,107 @@
 
 #include <common.h>
 #include <asm/io.h>
-#include <asm/arch/regs-common.h>
-#include <asm/arch/regs-base.h>
-#include <asm/arch/regs-clkctrl-mx28.h>
-#include <asm/arch/regs-usb.h>
-#include <asm/arch/regs-usbphy.h>
+#include <asm/arch/imx-regs.h>
+#include <errno.h>
 
 #include "ehci.h"
 
-#if	(CONFIG_EHCI_MXS_PORT != 0) && (CONFIG_EHCI_MXS_PORT != 1)
-#error	"MXS EHCI: Invalid port selected!"
-#endif
-
-#ifndef	CONFIG_EHCI_MXS_PORT
-#error	"MXS EHCI: Please define correct port using CONFIG_EHCI_MXS_PORT!"
-#endif
+/* This DIGCTL register ungates clock to USB */
+#define	HW_DIGCTL_CTRL			0x8001c000
+#define	HW_DIGCTL_CTRL_USB0_CLKGATE	(1 << 2)
+#define	HW_DIGCTL_CTRL_USB1_CLKGATE	(1 << 16)
 
-static struct ehci_mxs {
-	struct mxs_usb_regs	*usb_regs;
+struct ehci_mxs_port {
+	uint32_t		usb_regs;
 	struct mxs_usbphy_regs	*phy_regs;
-} ehci_mxs;
 
-int mxs_ehci_get_port(struct ehci_mxs *mxs_usb, int port)
+	struct mxs_register_32	*pll;
+	uint32_t		pll_en_bits;
+	uint32_t		pll_dis_bits;
+	uint32_t		gate_bits;
+};
+
+static const struct ehci_mxs_port mxs_port[] = {
+#ifdef CONFIG_EHCI_MXS_PORT0
+	{
+		MXS_USBCTRL0_BASE,
+		(struct mxs_usbphy_regs *)MXS_USBPHY0_BASE,
+		(struct mxs_register_32 *)(MXS_CLKCTRL_BASE +
+			offsetof(struct mxs_clkctrl_regs,
+			hw_clkctrl_pll0ctrl0_reg)),
+		CLKCTRL_PLL0CTRL0_EN_USB_CLKS | CLKCTRL_PLL0CTRL0_POWER,
+		CLKCTRL_PLL0CTRL0_EN_USB_CLKS,
+		HW_DIGCTL_CTRL_USB0_CLKGATE,
+	},
+#endif
+#ifdef CONFIG_EHCI_MXS_PORT1
+	{
+		MXS_USBCTRL1_BASE,
+		(struct mxs_usbphy_regs *)MXS_USBPHY1_BASE,
+		(struct mxs_register_32 *)(MXS_CLKCTRL_BASE +
+			offsetof(struct mxs_clkctrl_regs,
+			hw_clkctrl_pll1ctrl0_reg)),
+		CLKCTRL_PLL1CTRL0_EN_USB_CLKS | CLKCTRL_PLL1CTRL0_POWER,
+		CLKCTRL_PLL1CTRL0_EN_USB_CLKS,
+		HW_DIGCTL_CTRL_USB1_CLKGATE,
+	},
+#endif
+};
+
+static int ehci_mxs_toggle_clock(const struct ehci_mxs_port *port, int enable)
 {
-	uint32_t usb_base, phy_base;
-	switch (port) {
-	case 0:
-		usb_base = MXS_USBCTRL0_BASE;
-		phy_base = MXS_USBPHY0_BASE;
-		break;
-	case 1:
-		usb_base = MXS_USBCTRL1_BASE;
-		phy_base = MXS_USBPHY1_BASE;
-		break;
-	default:
-		printf("CONFIG_EHCI_MXS_PORT (port = %d)\n", port);
-		return -1;
+	struct mxs_register_32 *digctl_ctrl =
+		(struct mxs_register_32 *)HW_DIGCTL_CTRL;
+	int pll_offset, dig_offset;
+
+	if (enable) {
+		pll_offset = offsetof(struct mxs_register_32, reg_set);
+		dig_offset = offsetof(struct mxs_register_32, reg_clr);
+		writel(port->gate_bits, (u32)&digctl_ctrl->reg + dig_offset);
+		writel(port->pll_en_bits, (u32)port->pll + pll_offset);
+	} else {
+		pll_offset = offsetof(struct mxs_register_32, reg_clr);
+		dig_offset = offsetof(struct mxs_register_32, reg_set);
+		writel(port->pll_dis_bits, (u32)port->pll + pll_offset);
+		writel(port->gate_bits, (u32)&digctl_ctrl->reg + dig_offset);
 	}
 
-	mxs_usb->usb_regs = (struct mxs_usb_regs *)usb_base;
-	mxs_usb->phy_regs = (struct mxs_usbphy_regs *)phy_base;
 	return 0;
 }
 
-/* This DIGCTL register ungates clock to USB */
-#define	HW_DIGCTL_CTRL			0x8001c000
-#define	HW_DIGCTL_CTRL_USB0_CLKGATE	(1 << 2)
-#define	HW_DIGCTL_CTRL_USB1_CLKGATE	(1 << 16)
-
 int ehci_hcd_init(int index, struct ehci_hccr **hccr, struct ehci_hcor **hcor)
 {
 
 	int ret;
 	uint32_t usb_base, cap_base;
-	struct mxs_register_32 *digctl_ctrl =
-		(struct mxs_register_32 *)HW_DIGCTL_CTRL;
-	struct mxs_clkctrl_regs *clkctrl_regs =
-		(struct mxs_clkctrl_regs *)MXS_CLKCTRL_BASE;
+	const struct ehci_mxs_port *port;
 
-	ret = mxs_ehci_get_port(&ehci_mxs, CONFIG_EHCI_MXS_PORT);
-	if (ret)
-		return ret;
+	if ((index < 0) || (index >= ARRAY_SIZE(mxs_port))) {
+		printf("Invalid port index (index = %d)!\n", index);
+		return -EINVAL;
+	}
+
+	port = &mxs_port[index];
 
 	/* Reset the PHY block */
-	writel(USBPHY_CTRL_SFTRST, &ehci_mxs.phy_regs->hw_usbphy_ctrl_set);
+	writel(USBPHY_CTRL_SFTRST, &port->phy_regs->hw_usbphy_ctrl_set);
 	udelay(10);
 	writel(USBPHY_CTRL_SFTRST | USBPHY_CTRL_CLKGATE,
-		&ehci_mxs.phy_regs->hw_usbphy_ctrl_clr);
+		&port->phy_regs->hw_usbphy_ctrl_clr);
 
 	/* Enable USB clock */
-	writel(CLKCTRL_PLL0CTRL0_EN_USB_CLKS | CLKCTRL_PLL0CTRL0_POWER,
-			&clkctrl_regs->hw_clkctrl_pll0ctrl0_set);
-	writel(CLKCTRL_PLL1CTRL0_EN_USB_CLKS | CLKCTRL_PLL1CTRL0_POWER,
-			&clkctrl_regs->hw_clkctrl_pll1ctrl0_set);
-
-	writel(HW_DIGCTL_CTRL_USB0_CLKGATE | HW_DIGCTL_CTRL_USB1_CLKGATE,
-		&digctl_ctrl->reg_clr);
+	ret = ehci_mxs_toggle_clock(port, 1);
+	if (ret)
+		return ret;
 
 	/* Start USB PHY */
-	writel(0, &ehci_mxs.phy_regs->hw_usbphy_pwd);
+	writel(0, &port->phy_regs->hw_usbphy_pwd);
 
 	/* Enable UTMI+ Level 2 and Level 3 compatibility */
 	writel(USBPHY_CTRL_ENUTMILEVEL3 | USBPHY_CTRL_ENUTMILEVEL2 | 1,
-		&ehci_mxs.phy_regs->hw_usbphy_ctrl_set);
+		&port->phy_regs->hw_usbphy_ctrl_set);
 
-	usb_base = ((uint32_t)ehci_mxs.usb_regs) + 0x100;
+	usb_base = port->usb_regs + 0x100;
 	*hccr = (struct ehci_hccr *)usb_base;
 
 	cap_base = ehci_readl(&(*hccr)->cr_capbase);
@@ -118,19 +134,19 @@ int ehci_hcd_stop(int index)
 {
 	int ret;
 	uint32_t usb_base, cap_base, tmp;
-	struct mxs_register_32 *digctl_ctrl =
-		(struct mxs_register_32 *)HW_DIGCTL_CTRL;
-	struct mxs_clkctrl_regs *clkctrl_regs =
-		(struct mxs_clkctrl_regs *)MXS_CLKCTRL_BASE;
 	struct ehci_hccr *hccr;
 	struct ehci_hcor *hcor;
+	const struct ehci_mxs_port *port;
 
-	ret = mxs_ehci_get_port(&ehci_mxs, CONFIG_EHCI_MXS_PORT);
-	if (ret)
-		return ret;
+	if ((index < 0) || (index >= ARRAY_SIZE(mxs_port))) {
+		printf("Invalid port index (index = %d)!\n", index);
+		return -EINVAL;
+	}
+
+	port = &mxs_port[index];
 
 	/* Stop the USB port */
-	usb_base = ((uint32_t)ehci_mxs.usb_regs) + 0x100;
+	usb_base = port->usb_regs + 0x100;
 	hccr = (struct ehci_hccr *)usb_base;
 	cap_base = ehci_readl(&hccr->cr_capbase);
 	hcor = (struct ehci_hcor *)(usb_base + HC_LENGTH(cap_base));
@@ -144,17 +160,10 @@ int ehci_hcd_stop(int index)
 		USBPHY_PWD_RXPWD1PT1 | USBPHY_PWD_RXPWDENV |
 		USBPHY_PWD_TXPWDV2I | USBPHY_PWD_TXPWDIBIAS |
 		USBPHY_PWD_TXPWDFS;
-	writel(tmp, &ehci_mxs.phy_regs->hw_usbphy_pwd);
+	writel(tmp, &port->phy_regs->hw_usbphy_pwd);
 
 	/* Disable USB clock */
-	writel(CLKCTRL_PLL0CTRL0_EN_USB_CLKS,
-			&clkctrl_regs->hw_clkctrl_pll0ctrl0_clr);
-	writel(CLKCTRL_PLL1CTRL0_EN_USB_CLKS,
-			&clkctrl_regs->hw_clkctrl_pll1ctrl0_clr);
+	ret = ehci_mxs_toggle_clock(port, 0);
 
-	/* Gate off the USB clock */
-	writel(HW_DIGCTL_CTRL_USB0_CLKGATE | HW_DIGCTL_CTRL_USB1_CLKGATE,
-		&digctl_ctrl->reg_set);
-
-	return 0;
+	return ret;
 }
diff --git a/drivers/usb/host/ehci-tegra.c b/drivers/usb/host/ehci-tegra.c
index a1c43f8..554145a 100644
--- a/drivers/usb/host/ehci-tegra.c
+++ b/drivers/usb/host/ehci-tegra.c
@@ -1,5 +1,7 @@
 /*
+ * Copyright (c) 2011 The Chromium OS Authors.
  * Copyright (c) 2009-2012 NVIDIA Corporation
+ * Copyright (c) 2013 Lucas Stach
  *
  * See file CREDITS for list of people who contributed to this
  * project.
@@ -21,12 +23,128 @@
  */
 
 #include <common.h>
+#include <asm/errno.h>
+#include <asm/io.h>
+#include <asm-generic/gpio.h>
+#include <asm/arch/clock.h>
+#include <asm/arch-tegra/usb.h>
 #include <usb.h>
+#include <usb/ulpi.h>
+#include <libfdt.h>
+#include <fdtdec.h>
 
 #include "ehci.h"
 
-#include <asm/errno.h>
-#include <asm/arch/usb.h>
+#ifdef CONFIG_USB_ULPI
+	#ifndef CONFIG_USB_ULPI_VIEWPORT
+	#error	"To use CONFIG_USB_ULPI on Tegra Boards you have to also \
+		define CONFIG_USB_ULPI_VIEWPORT"
+	#endif
+#endif
+
+enum {
+	USB_PORTS_MAX	= 3,		/* Maximum ports we allow */
+};
+
+/* Parameters we need for USB */
+enum {
+	PARAM_DIVN,                     /* PLL FEEDBACK DIVIDer */
+	PARAM_DIVM,                     /* PLL INPUT DIVIDER */
+	PARAM_DIVP,                     /* POST DIVIDER (2^N) */
+	PARAM_CPCON,                    /* BASE PLLC CHARGE Pump setup ctrl */
+	PARAM_LFCON,                    /* BASE PLLC LOOP FILter setup ctrl */
+	PARAM_ENABLE_DELAY_COUNT,       /* PLL-U Enable Delay Count */
+	PARAM_STABLE_COUNT,             /* PLL-U STABLE count */
+	PARAM_ACTIVE_DELAY_COUNT,       /* PLL-U Active delay count */
+	PARAM_XTAL_FREQ_COUNT,          /* PLL-U XTAL frequency count */
+	PARAM_DEBOUNCE_A_TIME,          /* 10MS DELAY for BIAS_DEBOUNCE_A */
+	PARAM_BIAS_TIME,                /* 20US DELAY AFter bias cell op */
+
+	PARAM_COUNT
+};
+
+/* Possible port types (dual role mode) */
+enum dr_mode {
+	DR_MODE_NONE = 0,
+	DR_MODE_HOST,		/* supports host operation */
+	DR_MODE_DEVICE,		/* supports device operation */
+	DR_MODE_OTG,		/* supports both */
+};
+
+/* Information about a USB port */
+struct fdt_usb {
+	struct usb_ctlr *reg;	/* address of registers in physical memory */
+	unsigned utmi:1;	/* 1 if port has external tranceiver, else 0 */
+	unsigned ulpi:1;	/* 1 if port has external ULPI transceiver */
+	unsigned enabled:1;	/* 1 to enable, 0 to disable */
+	unsigned has_legacy_mode:1; /* 1 if this port has legacy mode */
+	unsigned initialized:1; /* has this port already been initialized? */
+	enum dr_mode dr_mode;	/* dual role mode */
+	enum periph_id periph_id;/* peripheral id */
+	struct fdt_gpio_state vbus_gpio;	/* GPIO for vbus enable */
+	struct fdt_gpio_state phy_reset_gpio; /* GPIO to reset ULPI phy */
+};
+
+static struct fdt_usb port[USB_PORTS_MAX];	/* List of valid USB ports */
+static unsigned port_count;			/* Number of available ports */
+
+/*
+ * This table has USB timing parameters for each Oscillator frequency we
+ * support. There are four sets of values:
+ *
+ * 1. PLLU configuration information (reference clock is osc/clk_m and
+ * PLLU-FOs are fixed at 12MHz/60MHz/480MHz).
+ *
+ *  Reference frequency     13.0MHz      19.2MHz      12.0MHz      26.0MHz
+ *  ----------------------------------------------------------------------
+ *      DIVN                960 (0x3c0)  200 (0c8)    960 (3c0h)   960 (3c0)
+ *      DIVM                13 (0d)      4 (04)       12 (0c)      26 (1a)
+ * Filter frequency (MHz)   1            4.8          6            2
+ * CPCON                    1100b        0011b        1100b        1100b
+ * LFCON0                   0            0            0            0
+ *
+ * 2. PLL CONFIGURATION & PARAMETERS for different clock generators:
+ *
+ * Reference frequency     13.0MHz         19.2MHz         12.0MHz     26.0MHz
+ * ---------------------------------------------------------------------------
+ * PLLU_ENABLE_DLY_COUNT   02 (0x02)       03 (03)         02 (02)     04 (04)
+ * PLLU_STABLE_COUNT       51 (33)         75 (4B)         47 (2F)    102 (66)
+ * PLL_ACTIVE_DLY_COUNT    05 (05)         06 (06)         04 (04)     09 (09)
+ * XTAL_FREQ_COUNT        127 (7F)        187 (BB)        118 (76)    254 (FE)
+ *
+ * 3. Debounce values IdDig, Avalid, Bvalid, VbusValid, VbusWakeUp, and
+ * SessEnd. Each of these signals have their own debouncer and for each of
+ * those one out of two debouncing times can be chosen (BIAS_DEBOUNCE_A or
+ * BIAS_DEBOUNCE_B).
+ *
+ * The values of DEBOUNCE_A and DEBOUNCE_B are calculated as follows:
+ *    0xffff -> No debouncing at all
+ *    <n> ms = <n> *1000 / (1/19.2MHz) / 4
+ *
+ * So to program a 1 ms debounce for BIAS_DEBOUNCE_A, we have:
+ * BIAS_DEBOUNCE_A[15:0] = 1000 * 19.2 / 4  = 4800 = 0x12c0
+ *
+ * We need to use only DebounceA for BOOTROM. We don't need the DebounceB
+ * values, so we can keep those to default.
+ *
+ * 4. The 20 microsecond delay after bias cell operation.
+ */
+static const unsigned usb_pll[CLOCK_OSC_FREQ_COUNT][PARAM_COUNT] = {
+	/* DivN, DivM, DivP, CPCON, LFCON, Delays             Debounce, Bias */
+	{ 0x3C0, 0x0D, 0x00, 0xC,   0,  0x02, 0x33, 0x05, 0x7F, 0x7EF4, 5 },
+	{ 0x0C8, 0x04, 0x00, 0x3,   0,  0x03, 0x4B, 0x06, 0xBB, 0xBB80, 7 },
+	{ 0x3C0, 0x0C, 0x00, 0xC,   0,  0x02, 0x2F, 0x04, 0x76, 0x7530, 5 },
+	{ 0x3C0, 0x1A, 0x00, 0xC,   0,  0x04, 0x66, 0x09, 0xFE, 0xFDE8, 9 }
+};
+
+/* UTMIP Idle Wait Delay */
+static const u8 utmip_idle_wait_delay = 17;
+
+/* UTMIP Elastic limit */
+static const u8 utmip_elastic_limit = 16;
+
+/* UTMIP High Speed Sync Start Delay */
+static const u8 utmip_hs_sync_start_delay = 9;
 
 /*
  * A known hardware issue where Connect Status Change bit of PORTSC register
@@ -45,32 +163,428 @@ void ehci_powerup_fixup(uint32_t *status_reg, uint32_t *reg)
 		*reg |= EHCI_PS_CSC;
 }
 
-/*
- * Create the appropriate control structures to manage
- * a new EHCI host controller.
- */
-int ehci_hcd_init(int index, struct ehci_hccr **hccr, struct ehci_hcor **hcor)
+/* Put the port into host mode */
+static void set_host_mode(struct fdt_usb *config)
+{
+	/*
+	 * If we are an OTG port, check if remote host is driving VBus and
+	 * bail out in this case.
+	 */
+	if (config->dr_mode == DR_MODE_OTG &&
+		(readl(&config->reg->phy_vbus_sensors) & VBUS_VLD_STS))
+		return;
+
+	/*
+	 * If not driving, we set the GPIO to enable VBUS. We assume
+	 * that the pinmux is set up correctly for this.
+	 */
+	if (fdt_gpio_isvalid(&config->vbus_gpio)) {
+		fdtdec_setup_gpio(&config->vbus_gpio);
+		gpio_direction_output(config->vbus_gpio.gpio,
+			(config->vbus_gpio.flags & FDT_GPIO_ACTIVE_LOW) ?
+				 0 : 1);
+		debug("set_host_mode: GPIO %d %s\n", config->vbus_gpio.gpio,
+			(config->vbus_gpio.flags & FDT_GPIO_ACTIVE_LOW) ?
+				"low" : "high");
+	}
+}
+
+void usbf_reset_controller(struct fdt_usb *config, struct usb_ctlr *usbctlr)
+{
+	/* Reset the USB controller with 2us delay */
+	reset_periph(config->periph_id, 2);
+
+	/*
+	 * Set USB1_NO_LEGACY_MODE to 1, Registers are accessible under
+	 * base address
+	 */
+	if (config->has_legacy_mode)
+		setbits_le32(&usbctlr->usb1_legacy_ctrl, USB1_NO_LEGACY_MODE);
+
+	/* Put UTMIP1/3 in reset */
+	setbits_le32(&usbctlr->susp_ctrl, UTMIP_RESET);
+
+	/* Enable the UTMIP PHY */
+	if (config->utmi)
+		setbits_le32(&usbctlr->susp_ctrl, UTMIP_PHY_ENB);
+}
+
+/* set up the UTMI USB controller with the parameters provided */
+static int init_utmi_usb_controller(struct fdt_usb *config)
 {
-	u32 our_hccr, our_hcor;
+	u32 val;
+	int loop_count;
+	const unsigned *timing;
+	struct usb_ctlr *usbctlr = config->reg;
+
+	clock_enable(config->periph_id);
+
+	/* Reset the usb controller */
+	usbf_reset_controller(config, usbctlr);
+
+	/* Stop crystal clock by setting UTMIP_PHY_XTAL_CLOCKEN low */
+	clrbits_le32(&usbctlr->utmip_misc_cfg1, UTMIP_PHY_XTAL_CLOCKEN);
+
+	/* Follow the crystal clock disable by >100ns delay */
+	udelay(1);
+
+	/*
+	 * To Use the A Session Valid for cable detection logic, VBUS_WAKEUP
+	 * mux must be switched to actually use a_sess_vld threshold.
+	 */
+	if (fdt_gpio_isvalid(&config->vbus_gpio)) {
+		clrsetbits_le32(&usbctlr->usb1_legacy_ctrl,
+			VBUS_SENSE_CTL_MASK,
+			VBUS_SENSE_CTL_A_SESS_VLD << VBUS_SENSE_CTL_SHIFT);
+	}
+
+	/*
+	 * PLL Delay CONFIGURATION settings. The following parameters control
+	 * the bring up of the plls.
+	 */
+	timing = usb_pll[clock_get_osc_freq()];
+
+	val = readl(&usbctlr->utmip_misc_cfg1);
+	clrsetbits_le32(&val, UTMIP_PLLU_STABLE_COUNT_MASK,
+		timing[PARAM_STABLE_COUNT] << UTMIP_PLLU_STABLE_COUNT_SHIFT);
+	clrsetbits_le32(&val, UTMIP_PLL_ACTIVE_DLY_COUNT_MASK,
+		timing[PARAM_ACTIVE_DELAY_COUNT] <<
+			UTMIP_PLL_ACTIVE_DLY_COUNT_SHIFT);
+	writel(val, &usbctlr->utmip_misc_cfg1);
+
+	/* Set PLL enable delay count and crystal frequency count */
+	val = readl(&usbctlr->utmip_pll_cfg1);
+	clrsetbits_le32(&val, UTMIP_PLLU_ENABLE_DLY_COUNT_MASK,
+		timing[PARAM_ENABLE_DELAY_COUNT] <<
+			UTMIP_PLLU_ENABLE_DLY_COUNT_SHIFT);
+	clrsetbits_le32(&val, UTMIP_XTAL_FREQ_COUNT_MASK,
+		timing[PARAM_XTAL_FREQ_COUNT] <<
+			UTMIP_XTAL_FREQ_COUNT_SHIFT);
+	writel(val, &usbctlr->utmip_pll_cfg1);
+
+	/* Setting the tracking length time */
+	clrsetbits_le32(&usbctlr->utmip_bias_cfg1,
+		UTMIP_BIAS_PDTRK_COUNT_MASK,
+		timing[PARAM_BIAS_TIME] << UTMIP_BIAS_PDTRK_COUNT_SHIFT);
+
+	/* Program debounce time for VBUS to become valid */
+	clrsetbits_le32(&usbctlr->utmip_debounce_cfg0,
+		UTMIP_DEBOUNCE_CFG0_MASK,
+		timing[PARAM_DEBOUNCE_A_TIME] << UTMIP_DEBOUNCE_CFG0_SHIFT);
+
+	setbits_le32(&usbctlr->utmip_tx_cfg0, UTMIP_FS_PREAMBLE_J);
+
+	/* Disable battery charge enabling bit */
+	setbits_le32(&usbctlr->utmip_bat_chrg_cfg0, UTMIP_PD_CHRG);
+
+	clrbits_le32(&usbctlr->utmip_xcvr_cfg0, UTMIP_XCVR_LSBIAS_SE);
+	setbits_le32(&usbctlr->utmip_spare_cfg0, FUSE_SETUP_SEL);
 
 	/*
-	 * Select the first port, as we don't have a way of selecting others
-	 * yet
+	 * Configure the UTMIP_IDLE_WAIT and UTMIP_ELASTIC_LIMIT
+	 * Setting these fields, together with default values of the
+	 * other fields, results in programming the registers below as
+	 * follows:
+	 *         UTMIP_HSRX_CFG0 = 0x9168c000
+	 *         UTMIP_HSRX_CFG1 = 0x13
 	 */
-	if (tegrausb_start_port(index, &our_hccr, &our_hcor))
+
+	/* Set PLL enable delay count and Crystal frequency count */
+	val = readl(&usbctlr->utmip_hsrx_cfg0);
+	clrsetbits_le32(&val, UTMIP_IDLE_WAIT_MASK,
+		utmip_idle_wait_delay << UTMIP_IDLE_WAIT_SHIFT);
+	clrsetbits_le32(&val, UTMIP_ELASTIC_LIMIT_MASK,
+		utmip_elastic_limit << UTMIP_ELASTIC_LIMIT_SHIFT);
+	writel(val, &usbctlr->utmip_hsrx_cfg0);
+
+	/* Configure the UTMIP_HS_SYNC_START_DLY */
+	clrsetbits_le32(&usbctlr->utmip_hsrx_cfg1,
+		UTMIP_HS_SYNC_START_DLY_MASK,
+		utmip_hs_sync_start_delay << UTMIP_HS_SYNC_START_DLY_SHIFT);
+
+	/* Preceed the crystal clock disable by >100ns delay. */
+	udelay(1);
+
+	/* Resuscitate crystal clock by setting UTMIP_PHY_XTAL_CLOCKEN */
+	setbits_le32(&usbctlr->utmip_misc_cfg1, UTMIP_PHY_XTAL_CLOCKEN);
+
+	/* Finished the per-controller init. */
+
+	/* De-assert UTMIP_RESET to bring out of reset. */
+	clrbits_le32(&usbctlr->susp_ctrl, UTMIP_RESET);
+
+	/* Wait for the phy clock to become valid in 100 ms */
+	for (loop_count = 100000; loop_count != 0; loop_count--) {
+		if (readl(&usbctlr->susp_ctrl) & USB_PHY_CLK_VALID)
+			break;
+		udelay(1);
+	}
+	if (!loop_count)
+		return -1;
+
+	/* Disable ICUSB FS/LS transceiver */
+	clrbits_le32(&usbctlr->icusb_ctrl, IC_ENB1);
+
+	/* Select UTMI parallel interface */
+	clrsetbits_le32(&usbctlr->port_sc1, PTS_MASK,
+			PTS_UTMI << PTS_SHIFT);
+	clrbits_le32(&usbctlr->port_sc1, STS);
+
+	/* Deassert power down state */
+	clrbits_le32(&usbctlr->utmip_xcvr_cfg0, UTMIP_FORCE_PD_POWERDOWN |
+		UTMIP_FORCE_PD2_POWERDOWN | UTMIP_FORCE_PDZI_POWERDOWN);
+	clrbits_le32(&usbctlr->utmip_xcvr_cfg1, UTMIP_FORCE_PDDISC_POWERDOWN |
+		UTMIP_FORCE_PDCHRP_POWERDOWN | UTMIP_FORCE_PDDR_POWERDOWN);
+
+	return 0;
+}
+
+#ifdef CONFIG_USB_ULPI
+/* if board file does not set a ULPI reference frequency we default to 24MHz */
+#ifndef CONFIG_ULPI_REF_CLK
+#define CONFIG_ULPI_REF_CLK 24000000
+#endif
+
+/* set up the ULPI USB controller with the parameters provided */
+static int init_ulpi_usb_controller(struct fdt_usb *config)
+{
+	u32 val;
+	int loop_count;
+	struct ulpi_viewport ulpi_vp;
+	struct usb_ctlr *usbctlr = config->reg;
+
+	/* set up ULPI reference clock on pllp_out4 */
+	clock_enable(PERIPH_ID_DEV2_OUT);
+	clock_set_pllout(CLOCK_ID_PERIPH, PLL_OUT4, CONFIG_ULPI_REF_CLK);
+
+	/* reset ULPI phy */
+	if (fdt_gpio_isvalid(&config->phy_reset_gpio)) {
+		fdtdec_setup_gpio(&config->phy_reset_gpio);
+		gpio_direction_output(config->phy_reset_gpio.gpio, 0);
+		mdelay(5);
+		gpio_set_value(config->phy_reset_gpio.gpio, 1);
+	}
+
+	/* Reset the usb controller */
+	clock_enable(config->periph_id);
+	usbf_reset_controller(config, usbctlr);
+
+	/* enable pinmux bypass */
+	setbits_le32(&usbctlr->ulpi_timing_ctrl_0,
+			ULPI_CLKOUT_PINMUX_BYP | ULPI_OUTPUT_PINMUX_BYP);
+
+	/* Select ULPI parallel interface */
+	clrsetbits_le32(&usbctlr->port_sc1, PTS_MASK, PTS_ULPI << PTS_SHIFT);
+
+	/* enable ULPI transceiver */
+	setbits_le32(&usbctlr->susp_ctrl, ULPI_PHY_ENB);
+
+	/* configure ULPI transceiver timings */
+	val = 0;
+	writel(val, &usbctlr->ulpi_timing_ctrl_1);
+
+	val |= ULPI_DATA_TRIMMER_SEL(4);
+	val |= ULPI_STPDIRNXT_TRIMMER_SEL(4);
+	val |= ULPI_DIR_TRIMMER_SEL(4);
+	writel(val, &usbctlr->ulpi_timing_ctrl_1);
+	udelay(10);
+
+	val |= ULPI_DATA_TRIMMER_LOAD;
+	val |= ULPI_STPDIRNXT_TRIMMER_LOAD;
+	val |= ULPI_DIR_TRIMMER_LOAD;
+	writel(val, &usbctlr->ulpi_timing_ctrl_1);
+
+	/* set up phy for host operation with external vbus supply */
+	ulpi_vp.port_num = 0;
+	ulpi_vp.viewport_addr = (u32)&usbctlr->ulpi_viewport;
+
+	if (ulpi_init(&ulpi_vp)) {
+		printf("Tegra ULPI viewport init failed\n");
+		return -1;
+	}
+
+	ulpi_set_vbus(&ulpi_vp, 1, 1);
+	ulpi_set_vbus_indicator(&ulpi_vp, 1, 1, 0);
+
+	/* enable wakeup events */
+	setbits_le32(&usbctlr->port_sc1, WKCN | WKDS | WKOC);
+
+	/* Enable and wait for the phy clock to become valid in 100 ms */
+	setbits_le32(&usbctlr->susp_ctrl, USB_SUSP_CLR);
+	for (loop_count = 100000; loop_count != 0; loop_count--) {
+		if (readl(&usbctlr->susp_ctrl) & USB_PHY_CLK_VALID)
+			break;
+		udelay(1);
+	}
+	if (!loop_count)
+		return -1;
+	clrbits_le32(&usbctlr->susp_ctrl, USB_SUSP_CLR);
+
+	return 0;
+}
+#else
+static int init_ulpi_usb_controller(struct fdt_usb *config)
+{
+	printf("No code to set up ULPI controller, please enable"
+			"CONFIG_USB_ULPI and CONFIG_USB_ULPI_VIEWPORT");
+	return -1;
+}
+#endif
+
+static void config_clock(const u32 timing[])
+{
+	clock_start_pll(CLOCK_ID_USB,
+		timing[PARAM_DIVM], timing[PARAM_DIVN], timing[PARAM_DIVP],
+		timing[PARAM_CPCON], timing[PARAM_LFCON]);
+}
+
+int fdt_decode_usb(const void *blob, int node, struct fdt_usb *config)
+{
+	const char *phy, *mode;
+
+	config->reg = (struct usb_ctlr *)fdtdec_get_addr(blob, node, "reg");
+	mode = fdt_getprop(blob, node, "dr_mode", NULL);
+	if (mode) {
+		if (0 == strcmp(mode, "host"))
+			config->dr_mode = DR_MODE_HOST;
+		else if (0 == strcmp(mode, "peripheral"))
+			config->dr_mode = DR_MODE_DEVICE;
+		else if (0 == strcmp(mode, "otg"))
+			config->dr_mode = DR_MODE_OTG;
+		else {
+			debug("%s: Cannot decode dr_mode '%s'\n", __func__,
+			      mode);
+			return -FDT_ERR_NOTFOUND;
+		}
+	} else {
+		config->dr_mode = DR_MODE_HOST;
+	}
+
+	phy = fdt_getprop(blob, node, "phy_type", NULL);
+	config->utmi = phy && 0 == strcmp("utmi", phy);
+	config->ulpi = phy && 0 == strcmp("ulpi", phy);
+	config->enabled = fdtdec_get_is_enabled(blob, node);
+	config->has_legacy_mode = fdtdec_get_bool(blob, node,
+						  "nvidia,has-legacy-mode");
+	config->periph_id = clock_decode_periph_id(blob, node);
+	if (config->periph_id == PERIPH_ID_NONE) {
+		debug("%s: Missing/invalid peripheral ID\n", __func__);
+		return -FDT_ERR_NOTFOUND;
+	}
+	fdtdec_decode_gpio(blob, node, "nvidia,vbus-gpio", &config->vbus_gpio);
+	fdtdec_decode_gpio(blob, node, "nvidia,phy-reset-gpio",
+			&config->phy_reset_gpio);
+	debug("enabled=%d, legacy_mode=%d, utmi=%d, ulpi=%d, periph_id=%d, "
+		"vbus=%d, phy_reset=%d, dr_mode=%d\n",
+		config->enabled, config->has_legacy_mode, config->utmi,
+		config->ulpi, config->periph_id, config->vbus_gpio.gpio,
+		config->phy_reset_gpio.gpio, config->dr_mode);
+
+	return 0;
+}
+
+int board_usb_init(const void *blob)
+{
+	struct fdt_usb config;
+	enum clock_osc_freq freq;
+	int node_list[USB_PORTS_MAX];
+	int node, count, i;
+
+	/* Set up the USB clocks correctly based on our oscillator frequency */
+	freq = clock_get_osc_freq();
+	config_clock(usb_pll[freq]);
+
+	/* count may return <0 on error */
+	count = fdtdec_find_aliases_for_id(blob, "usb",
+			COMPAT_NVIDIA_TEGRA20_USB, node_list, USB_PORTS_MAX);
+	for (i = 0; i < count; i++) {
+		if (port_count == USB_PORTS_MAX) {
+			printf("tegrausb: Cannot register more than %d ports\n",
+				USB_PORTS_MAX);
+			return -1;
+		}
+
+		debug("USB %d: ", i);
+		node = node_list[i];
+		if (!node)
+			continue;
+		if (fdt_decode_usb(blob, node, &config)) {
+			debug("Cannot decode USB node %s\n",
+			      fdt_get_name(blob, node, NULL));
+			return -1;
+		}
+		config.initialized = 0;
+
+		/* add new USB port to the list of available ports */
+		port[port_count++] = config;
+	}
+
+	return 0;
+}
+
+/**
+ * Start up the given port number (ports are numbered from 0 on each board).
+ * This returns values for the appropriate hccr and hcor addresses to use for
+ * USB EHCI operations.
+ *
+ * @param index	port number to start
+ * @param hccr		returns start address of EHCI HCCR registers
+ * @param hcor		returns start address of EHCI HCOR registers
+ * @return 0 if ok, -1 on error (generally invalid port number)
+ */
+int ehci_hcd_init(int index, struct ehci_hccr **hccr, struct ehci_hcor **hcor)
+{
+	struct fdt_usb *config;
+	struct usb_ctlr *usbctlr;
+
+	if (index >= port_count)
 		return -1;
 
-	*hccr = (struct ehci_hccr *)our_hccr;
-	*hcor = (struct ehci_hcor *)our_hcor;
+	config = &port[index];
+
+	/* skip init, if the port is already initialized */
+	if (config->initialized)
+		goto success;
+
+	if (config->utmi && init_utmi_usb_controller(config)) {
+		printf("tegrausb: Cannot init port %d\n", index);
+		return -1;
+	}
+
+	if (config->ulpi && init_ulpi_usb_controller(config)) {
+		printf("tegrausb: Cannot init port %d\n", index);
+		return -1;
+	}
 
+	set_host_mode(config);
+
+	config->initialized = 1;
+
+success:
+	usbctlr = config->reg;
+	*hccr = (struct ehci_hccr *)&usbctlr->cap_length;
+	*hcor = (struct ehci_hcor *)&usbctlr->usb_cmd;
 	return 0;
 }
 
 /*
- * Destroy the appropriate control structures corresponding
- * the the EHCI host controller.
+ * Bring down the specified USB controller
  */
 int ehci_hcd_stop(int index)
 {
-	return tegrausb_stop_port(index);
+	struct usb_ctlr *usbctlr;
+
+	usbctlr = port[index].reg;
+
+	/* Stop controller */
+	writel(0, &usbctlr->usb_cmd);
+	udelay(1000);
+
+	/* Initiate controller reset */
+	writel(2, &usbctlr->usb_cmd);
+	udelay(1000);
+
+	port[index].initialized = 0;
+
+	return 0;
 }
diff --git a/drivers/video/omap3_dss.c b/drivers/video/omap3_dss.c
index b1424bf..6efba12 100644
--- a/drivers/video/omap3_dss.c
+++ b/drivers/video/omap3_dss.c
@@ -121,7 +121,7 @@ void omap3_dss_panel_config(const struct panel_config *panel_cfg)
 	if (!panel_cfg->frame_buffer)
 		return;
 
-	writel(8 << GFX_FORMAT_SHIFT | GFX_ENABLE, &dispc->gfx_attributes);
+	writel(panel_cfg->gfx_format | GFX_ENABLE, &dispc->gfx_attributes);
 	writel(1, &dispc->gfx_row_inc);
 	writel(1, &dispc->gfx_pixel_inc);
 	writel(panel_cfg->lcd_size, &dispc->gfx_size);