3 files changed, 322 insertions, 71 deletions

diff --git a/drivers/usb/host/ehci-hcd.c b/drivers/usb/host/ehci-hcd.c
index c81687820..e0f3e4b6c 100644
--- a/drivers/usb/host/ehci-hcd.c
+++ b/drivers/usb/host/ehci-hcd.c
@@ -122,6 +122,31 @@ static struct descriptor {
 #define ehci_is_TDI()	(0)
 #endif
 
+int __ehci_get_port_speed(struct ehci_hcor *hcor, uint32_t reg)
+{
+	return PORTSC_PSPD(reg);
+}
+
+int ehci_get_port_speed(struct ehci_hcor *hcor, uint32_t reg)
+	__attribute__((weak, alias("__ehci_get_port_speed")));
+
+void __ehci_set_usbmode(int index)
+{
+	uint32_t tmp;
+	uint32_t *reg_ptr;
+
+	reg_ptr = (uint32_t *)((u8 *)&ehcic[index].hcor->or_usbcmd + USBMODE);
+	tmp = ehci_readl(reg_ptr);
+	tmp |= USBMODE_CM_HC;
+#if defined(CONFIG_EHCI_MMIO_BIG_ENDIAN)
+	tmp |= USBMODE_BE;
+#endif
+	ehci_writel(reg_ptr, tmp);
+}
+
+void ehci_set_usbmode(int index)
+	__attribute__((weak, alias("__ehci_set_usbmode")));
+
 void __ehci_powerup_fixup(uint32_t *status_reg, uint32_t *reg)
 {
 	mdelay(50);
@@ -149,8 +174,6 @@ static int handshake(uint32_t *ptr, uint32_t mask, uint32_t done, int usec)
 static int ehci_reset(int index)
 {
 	uint32_t cmd;
-	uint32_t tmp;
-	uint32_t *reg_ptr;
 	int ret = 0;
 
 	cmd = ehci_readl(&ehcic[index].hcor->or_usbcmd);
@@ -163,15 +186,8 @@ static int ehci_reset(int index)
 		goto out;
 	}
 
-	if (ehci_is_TDI()) {
-		reg_ptr = (uint32_t *)((u8 *)ehcic[index].hcor + USBMODE);
-		tmp = ehci_readl(reg_ptr);
-		tmp |= USBMODE_CM_HC;
-#if defined(CONFIG_EHCI_MMIO_BIG_ENDIAN)
-		tmp |= USBMODE_BE;
-#endif
-		ehci_writel(reg_ptr, tmp);
-	}
+	if (ehci_is_TDI())
+		ehci_set_usbmode(index);
 
 #ifdef CONFIG_USB_EHCI_TXFIFO_THRESH
 	cmd = ehci_readl(&ehcic[index].hcor->or_txfilltuning);
@@ -587,16 +603,6 @@ fail:
 	return -1;
 }
 
-static inline int min3(int a, int b, int c)
-{
-
-	if (b < a)
-		a = b;
-	if (c < a)
-		a = c;
-	return a;
-}
-
 int
 ehci_submit_root(struct usb_device *dev, unsigned long pipe, void *buffer,
 		 int length, struct devrequest *req)
@@ -607,15 +613,14 @@ ehci_submit_root(struct usb_device *dev, unsigned long pipe, void *buffer,
 	int len, srclen;
 	uint32_t reg;
 	uint32_t *status_reg;
+	int port = le16_to_cpu(req->index) & 0xff;
 	struct ehci_ctrl *ctrl = dev->controller;
 
-	if (le16_to_cpu(req->index) > CONFIG_SYS_USB_EHCI_MAX_ROOT_PORTS) {
-		printf("The request port(%d) is not configured\n",
-			le16_to_cpu(req->index) - 1);
+	if (port > CONFIG_SYS_USB_EHCI_MAX_ROOT_PORTS) {
+		printf("The request port(%d) is not configured\n", port - 1);
 		return -1;
 	}
-	status_reg = (uint32_t *)&ctrl->hcor->or_portsc[
-						le16_to_cpu(req->index) - 1];
+	status_reg = (uint32_t *)&ctrl->hcor->or_portsc[port - 1];
 	srclen = 0;
 
 	debug("req=%u (%#x), type=%u (%#x), value=%u, index=%u\n",
@@ -711,7 +716,7 @@ ehci_submit_root(struct usb_device *dev, unsigned long pipe, void *buffer,
 			tmpbuf[1] |= USB_PORT_STAT_POWER >> 8;
 
 		if (ehci_is_TDI()) {
-			switch (PORTSC_PSPD(reg)) {
+			switch (ehci_get_port_speed(ctrl->hcor, reg)) {
 			case PORTSC_PSPD_FS:
 				break;
 			case PORTSC_PSPD_LS:
@@ -732,7 +737,7 @@ ehci_submit_root(struct usb_device *dev, unsigned long pipe, void *buffer,
 			tmpbuf[2] |= USB_PORT_STAT_C_ENABLE;
 		if (reg & EHCI_PS_OCC)
 			tmpbuf[2] |= USB_PORT_STAT_C_OVERCURRENT;
-		if (ctrl->portreset & (1 << le16_to_cpu(req->index)))
+		if (ctrl->portreset & (1 << port))
 			tmpbuf[2] |= USB_PORT_STAT_C_RESET;
 
 		srcptr = tmpbuf;
@@ -758,7 +763,7 @@ ehci_submit_root(struct usb_device *dev, unsigned long pipe, void *buffer,
 			    EHCI_PS_IS_LOWSPEED(reg)) {
 				/* Low speed device, give up ownership. */
 				debug("port %d low speed --> companion\n",
-				      req->index - 1);
+				      port - 1);
 				reg |= EHCI_PS_PO;
 				ehci_writel(status_reg, reg);
 				break;
@@ -784,13 +789,17 @@ ehci_submit_root(struct usb_device *dev, unsigned long pipe, void *buffer,
 				ret = handshake(status_reg, EHCI_PS_PR, 0,
 						2 * 1000);
 				if (!ret)
-					ctrl->portreset |=
-						1 << le16_to_cpu(req->index);
+					ctrl->portreset |= 1 << port;
 				else
 					printf("port(%d) reset error\n",
-					le16_to_cpu(req->index) - 1);
+					       port - 1);
 			}
 			break;
+		case USB_PORT_FEAT_TEST:
+			reg &= ~(0xf << 16);
+			reg |= ((le16_to_cpu(req->index) >> 8) & 0xf) << 16;
+			ehci_writel(status_reg, reg);
+			break;
 		default:
 			debug("unknown feature %x\n", le16_to_cpu(req->value));
 			goto unknown;
@@ -817,7 +826,7 @@ ehci_submit_root(struct usb_device *dev, unsigned long pipe, void *buffer,
 			reg = (reg & ~EHCI_PS_CLEAR) | EHCI_PS_OCC;
 			break;
 		case USB_PORT_FEAT_C_RESET:
-			ctrl->portreset &= ~(1 << le16_to_cpu(req->index));
+			ctrl->portreset &= ~(1 << port);
 			break;
 		default:
 			debug("unknown feature %x\n", le16_to_cpu(req->value));
diff --git a/drivers/usb/host/ehci-tegra.c b/drivers/usb/host/ehci-tegra.c
index 554145a25..a9603bc10 100644
--- a/drivers/usb/host/ehci-tegra.c
+++ b/drivers/usb/host/ehci-tegra.c
@@ -1,6 +1,6 @@
 /*
  * Copyright (c) 2011 The Chromium OS Authors.
- * Copyright (c) 2009-2012 NVIDIA Corporation
+ * Copyright (c) 2009-2013 NVIDIA Corporation
  * Copyright (c) 2013 Lucas Stach
  *
  * See file CREDITS for list of people who contributed to this
@@ -28,6 +28,8 @@
 #include <asm-generic/gpio.h>
 #include <asm/arch/clock.h>
 #include <asm/arch-tegra/usb.h>
+#include <asm/arch-tegra/clk_rst.h>
+#include <asm/arch/usb.h>
 #include <usb.h>
 #include <usb/ulpi.h>
 #include <libfdt.h>
@@ -35,6 +37,11 @@
 
 #include "ehci.h"
 
+#define USB1_ADDR_MASK	0xFFFF0000
+
+#define HOSTPC1_DEVLC	0x84
+#define HOSTPC1_PSPD(x)		(((x) >> 25) & 0x3)
+
 #ifdef CONFIG_USB_ULPI
 	#ifndef CONFIG_USB_ULPI_VIEWPORT
 	#error	"To use CONFIG_USB_ULPI on Tegra Boards you have to also \
@@ -87,6 +94,8 @@ struct fdt_usb {
 
 static struct fdt_usb port[USB_PORTS_MAX];	/* List of valid USB ports */
 static unsigned port_count;			/* Number of available ports */
+/* Port that needs to clear CSC after Port Reset */
+static u32 port_addr_clear_csc;
 
 /*
  * This table has USB timing parameters for each Oscillator frequency we
@@ -129,7 +138,7 @@ static unsigned port_count;			/* Number of available ports */
  *
  * 4. The 20 microsecond delay after bias cell operation.
  */
-static const unsigned usb_pll[CLOCK_OSC_FREQ_COUNT][PARAM_COUNT] = {
+static const unsigned T20_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 },
@@ -137,6 +146,22 @@ static const unsigned usb_pll[CLOCK_OSC_FREQ_COUNT][PARAM_COUNT] = {
 	{ 0x3C0, 0x1A, 0x00, 0xC,   0,  0x04, 0x66, 0x09, 0xFE, 0xFDE8, 9 }
 };
 
+static const unsigned T30_usb_pll[CLOCK_OSC_FREQ_COUNT][PARAM_COUNT] = {
+	/* DivN, DivM, DivP, CPCON, LFCON, Delays             Debounce, Bias */
+	{ 0x3C0, 0x0D, 0x00, 0xC,   1,  0x02, 0x33, 0x09, 0x7F, 0x7EF4, 5 },
+	{ 0x0C8, 0x04, 0x00, 0x3,   0,  0x03, 0x4B, 0x0C, 0xBB, 0xBB80, 7 },
+	{ 0x3C0, 0x0C, 0x00, 0xC,   1,  0x02, 0x2F, 0x08, 0x76, 0x7530, 5 },
+	{ 0x3C0, 0x1A, 0x00, 0xC,   1,  0x04, 0x66, 0x09, 0xFE, 0xFDE8, 9 }
+};
+
+static const unsigned T114_usb_pll[CLOCK_OSC_FREQ_COUNT][PARAM_COUNT] = {
+	/* DivN, DivM, DivP, CPCON, LFCON, Delays             Debounce, Bias */
+	{ 0x3C0, 0x0D, 0x00, 0xC,   2,  0x02, 0x33, 0x09, 0x7F, 0x7EF4, 6 },
+	{ 0x0C8, 0x04, 0x00, 0x3,   2,  0x03, 0x4B, 0x0C, 0xBB, 0xBB80, 8 },
+	{ 0x3C0, 0x0C, 0x00, 0xC,   2,  0x02, 0x2F, 0x08, 0x76, 0x7530, 5 },
+	{ 0x3C0, 0x1A, 0x00, 0xC,   2,  0x04, 0x66, 0x09, 0xFE, 0xFDE8, 0xB }
+};
+
 /* UTMIP Idle Wait Delay */
 static const u8 utmip_idle_wait_delay = 17;
 
@@ -146,6 +171,33 @@ static const u8 utmip_elastic_limit = 16;
 /* UTMIP High Speed Sync Start Delay */
 static const u8 utmip_hs_sync_start_delay = 9;
 
+struct fdt_usb_controller {
+	int compat;
+	/* flag to determine whether controller supports hostpc register */
+	u32 has_hostpc:1;
+	const unsigned *pll_parameter;
+};
+
+static struct fdt_usb_controller fdt_usb_controllers[] = {
+	{
+		.compat		= COMPAT_NVIDIA_TEGRA20_USB,
+		.has_hostpc	= 0,
+		.pll_parameter	= (const unsigned *)T20_usb_pll,
+	},
+	{
+		.compat		= COMPAT_NVIDIA_TEGRA30_USB,
+		.has_hostpc	= 1,
+		.pll_parameter	= (const unsigned *)T30_usb_pll,
+	},
+	{
+		.compat		= COMPAT_NVIDIA_TEGRA114_USB,
+		.has_hostpc	= 1,
+		.pll_parameter	= (const unsigned *)T114_usb_pll,
+	},
+};
+
+static struct fdt_usb_controller *controller;
+
 /*
  * A known hardware issue where Connect Status Change bit of PORTSC register
  * of USB1 controller will be set after Port Reset.
@@ -156,13 +208,52 @@ static const u8 utmip_hs_sync_start_delay = 9;
 void ehci_powerup_fixup(uint32_t *status_reg, uint32_t *reg)
 {
 	mdelay(50);
-	if (((u32) status_reg & TEGRA_USB_ADDR_MASK) != TEGRA_USB1_BASE)
+	/* This is to avoid PORT_ENABLE bit to be cleared in "ehci-hcd.c". */
+	if (controller->has_hostpc)
+		*reg |= EHCI_PS_PE;
+
+	if (((u32)status_reg & TEGRA_USB_ADDR_MASK) != port_addr_clear_csc)
 		return;
 	/* For EHCI_PS_CSC to be cleared in ehci_hcd.c */
 	if (ehci_readl(status_reg) & EHCI_PS_CSC)
 		*reg |= EHCI_PS_CSC;
 }
 
+/*
+ * This ehci_set_usbmode overrides the weak function ehci_set_usbmode
+ * in "ehci-hcd.c".
+ */
+void ehci_set_usbmode(int index)
+{
+	struct fdt_usb *config;
+	struct usb_ctlr *usbctlr;
+	uint32_t tmp;
+
+	config = &port[index];
+	usbctlr = config->reg;
+
+	tmp = ehci_readl(&usbctlr->usb_mode);
+	tmp |= USBMODE_CM_HC;
+	ehci_writel(&usbctlr->usb_mode, tmp);
+}
+
+/*
+ * This ehci_get_port_speed overrides the weak function ehci_get_port_speed
+ * in "ehci-hcd.c".
+ */
+int ehci_get_port_speed(struct ehci_hcor *hcor, uint32_t reg)
+{
+	uint32_t tmp;
+	uint32_t *reg_ptr;
+
+	if (controller->has_hostpc) {
+		reg_ptr = (uint32_t *)((u8 *)&hcor->or_usbcmd + HOSTPC1_DEVLC);
+		tmp = ehci_readl(reg_ptr);
+		return HOSTPC1_PSPD(tmp);
+	} else
+		return PORTSC_PSPD(reg);
+}
+
 /* Put the port into host mode */
 static void set_host_mode(struct fdt_usb *config)
 {
@@ -209,6 +300,16 @@ void usbf_reset_controller(struct fdt_usb *config, struct usb_ctlr *usbctlr)
 		setbits_le32(&usbctlr->susp_ctrl, UTMIP_PHY_ENB);
 }
 
+static const unsigned *get_pll_timing(void)
+{
+	const unsigned *timing;
+
+	timing = controller->pll_parameter +
+		clock_get_osc_freq() * PARAM_COUNT;
+
+	return timing;
+}
+
 /* set up the UTMI USB controller with the parameters provided */
 static int init_utmi_usb_controller(struct fdt_usb *config)
 {
@@ -216,6 +317,8 @@ static int init_utmi_usb_controller(struct fdt_usb *config)
 	int loop_count;
 	const unsigned *timing;
 	struct usb_ctlr *usbctlr = config->reg;
+	struct clk_rst_ctlr *clkrst;
+	struct usb_ctlr *usb1ctlr;
 
 	clock_enable(config->periph_id);
 
@@ -232,35 +335,97 @@ static int init_utmi_usb_controller(struct fdt_usb *config)
 	 * 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)) {
+	if (config->dr_mode == DR_MODE_OTG &&
+	    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()];
+	timing = get_pll_timing();
+
+	if (!controller->has_hostpc) {
+		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);
+	} else {
+		clkrst = (struct clk_rst_ctlr *)NV_PA_CLK_RST_BASE;
+
+		val = readl(&clkrst->crc_utmip_pll_cfg2);
+		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, &clkrst->crc_utmip_pll_cfg2);
+
+		/* Set PLL enable delay count and crystal frequency count */
+		val = readl(&clkrst->crc_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, &clkrst->crc_utmip_pll_cfg1);
 
-	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);
+		/* Disable Power Down state for PLL */
+		clrbits_le32(&clkrst->crc_utmip_pll_cfg1,
+			     PLLU_POWERDOWN | PLL_ENABLE_POWERDOWN |
+			     PLL_ACTIVE_POWERDOWN);
 
-	/* 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);
+		/* Recommended PHY settings for EYE diagram */
+		val = readl(&usbctlr->utmip_xcvr_cfg0);
+		clrsetbits_le32(&val, UTMIP_XCVR_SETUP_MASK,
+				0x4 << UTMIP_XCVR_SETUP_SHIFT);
+		clrsetbits_le32(&val, UTMIP_XCVR_SETUP_MSB_MASK,
+				0x3 << UTMIP_XCVR_SETUP_MSB_SHIFT);
+		clrsetbits_le32(&val, UTMIP_XCVR_HSSLEW_MSB_MASK,
+				0x8 << UTMIP_XCVR_HSSLEW_MSB_SHIFT);
+		writel(val, &usbctlr->utmip_xcvr_cfg0);
+		clrsetbits_le32(&usbctlr->utmip_xcvr_cfg1,
+				UTMIP_XCVR_TERM_RANGE_ADJ_MASK,
+				0x7 << UTMIP_XCVR_TERM_RANGE_ADJ_SHIFT);
+
+		/* Some registers can be controlled from USB1 only. */
+		if (config->periph_id != PERIPH_ID_USBD) {
+			clock_enable(PERIPH_ID_USBD);
+			/* Disable Reset if in Reset state */
+			reset_set_enable(PERIPH_ID_USBD, 0);
+		}
+		usb1ctlr = (struct usb_ctlr *)
+			((u32)config->reg & USB1_ADDR_MASK);
+		val = readl(&usb1ctlr->utmip_bias_cfg0);
+		setbits_le32(&val, UTMIP_HSDISCON_LEVEL_MSB);
+		clrsetbits_le32(&val, UTMIP_HSDISCON_LEVEL_MASK,
+				0x1 << UTMIP_HSDISCON_LEVEL_SHIFT);
+		clrsetbits_le32(&val, UTMIP_HSSQUELCH_LEVEL_MASK,
+				0x2 << UTMIP_HSSQUELCH_LEVEL_SHIFT);
+		writel(val, &usb1ctlr->utmip_bias_cfg0);
+
+		/* Miscellaneous setting mentioned in Programming Guide */
+		clrbits_le32(&usbctlr->utmip_misc_cfg0,
+			     UTMIP_SUSPEND_EXIT_ON_EDGE);
+	}
 
 	/* Setting the tracking length time */
 	clrsetbits_le32(&usbctlr->utmip_bias_cfg1,
@@ -308,6 +473,14 @@ static int init_utmi_usb_controller(struct fdt_usb *config)
 	/* Resuscitate crystal clock by setting UTMIP_PHY_XTAL_CLOCKEN */
 	setbits_le32(&usbctlr->utmip_misc_cfg1, UTMIP_PHY_XTAL_CLOCKEN);
 
+	if (controller->has_hostpc) {
+		if (config->periph_id == PERIPH_ID_USBD)
+			clrbits_le32(&clkrst->crc_utmip_pll_cfg2,
+				     UTMIP_FORCE_PD_SAMP_A_POWERDOWN);
+		if (config->periph_id == PERIPH_ID_USB3)
+			clrbits_le32(&clkrst->crc_utmip_pll_cfg2,
+				     UTMIP_FORCE_PD_SAMP_C_POWERDOWN);
+	}
 	/* Finished the per-controller init. */
 
 	/* De-assert UTMIP_RESET to bring out of reset. */
@@ -336,6 +509,18 @@ static int init_utmi_usb_controller(struct fdt_usb *config)
 	clrbits_le32(&usbctlr->utmip_xcvr_cfg1, UTMIP_FORCE_PDDISC_POWERDOWN |
 		UTMIP_FORCE_PDCHRP_POWERDOWN | UTMIP_FORCE_PDDR_POWERDOWN);
 
+	if (controller->has_hostpc) {
+		/*
+		 * BIAS Pad Power Down is common among all 3 USB
+		 * controllers and can be controlled from USB1 only.
+		 */
+		usb1ctlr = (struct usb_ctlr *)
+			((u32)config->reg & USB1_ADDR_MASK);
+		clrbits_le32(&usb1ctlr->utmip_bias_cfg0, UTMIP_BIASPD);
+		udelay(25);
+		clrbits_le32(&usb1ctlr->utmip_bias_cfg1,
+			     UTMIP_FORCE_PDTRK_POWERDOWN);
+	}
 	return 0;
 }
 
@@ -438,7 +623,7 @@ static void config_clock(const u32 timing[])
 		timing[PARAM_CPCON], timing[PARAM_LFCON]);
 }
 
-int fdt_decode_usb(const void *blob, int node, struct fdt_usb *config)
+static int fdt_decode_usb(const void *blob, int node, struct fdt_usb *config)
 {
 	const char *phy, *mode;
 
@@ -466,6 +651,8 @@ int fdt_decode_usb(const void *blob, int node, struct fdt_usb *config)
 	config->enabled = fdtdec_get_is_enabled(blob, node);
 	config->has_legacy_mode = fdtdec_get_bool(blob, node,
 						  "nvidia,has-legacy-mode");
+	if (config->has_legacy_mode)
+		port_addr_clear_csc = (u32) config->reg;
 	config->periph_id = clock_decode_periph_id(blob, node);
 	if (config->periph_id == PERIPH_ID_NONE) {
 		debug("%s: Missing/invalid peripheral ID\n", __func__);
@@ -483,20 +670,22 @@ int fdt_decode_usb(const void *blob, int node, struct fdt_usb *config)
 	return 0;
 }
 
-int board_usb_init(const void *blob)
+/*
+ * process_usb_nodes() - Process a list of USB nodes, adding them to our list
+ *			of USB ports.
+ * @blob:	fdt blob
+ * @node_list:	list of nodes to process (any <=0 are ignored)
+ * @count:	number of nodes to process
+ *
+ * Return:	0 - ok, -1 - error
+ */
+static int process_usb_nodes(const void *blob, int node_list[], int count)
 {
 	struct fdt_usb config;
-	enum clock_osc_freq freq;
-	int node_list[USB_PORTS_MAX];
-	int node, count, i;
+	int node, i;
+	int clk_done = 0;
 
-	/* 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);
+	port_count = 0;
 	for (i = 0; i < count; i++) {
 		if (port_count == USB_PORTS_MAX) {
 			printf("tegrausb: Cannot register more than %d ports\n",
@@ -513,6 +702,10 @@ int board_usb_init(const void *blob)
 			      fdt_get_name(blob, node, NULL));
 			return -1;
 		}
+		if (!clk_done) {
+			config_clock(get_pll_timing());
+			clk_done = 1;
+		}
 		config.initialized = 0;
 
 		/* add new USB port to the list of available ports */
@@ -522,6 +715,31 @@ int board_usb_init(const void *blob)
 	return 0;
 }
 
+int board_usb_init(const void *blob)
+{
+	int node_list[USB_PORTS_MAX];
+	int count, err = 0;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(fdt_usb_controllers); i++) {
+		controller = &fdt_usb_controllers[i];
+
+		count = fdtdec_find_aliases_for_id(blob, "usb",
+			controller->compat, node_list, USB_PORTS_MAX);
+		if (count) {
+			err = process_usb_nodes(blob, node_list, count);
+			if (err)
+				printf("%s: Error processing USB node!\n",
+				       __func__);
+			return err;
+		}
+	}
+	if (i == ARRAY_SIZE(fdt_usb_controllers))
+		controller = NULL;
+
+	return err;
+}
+
 /**
  * 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
@@ -564,6 +782,20 @@ success:
 	usbctlr = config->reg;
 	*hccr = (struct ehci_hccr *)&usbctlr->cap_length;
 	*hcor = (struct ehci_hcor *)&usbctlr->usb_cmd;
+
+	if (controller->has_hostpc) {
+		/* Set to Host mode after Controller Reset was done */
+		clrsetbits_le32(&usbctlr->usb_mode, USBMODE_CM_HC,
+				USBMODE_CM_HC);
+		/* Select UTMI parallel interface after setting host mode */
+		if (config->utmi) {
+			clrsetbits_le32((char *)&usbctlr->usb_cmd +
+					HOSTPC1_DEVLC, PTS_MASK,
+					PTS_UTMI << PTS_SHIFT);
+			clrbits_le32((char *)&usbctlr->usb_cmd +
+				     HOSTPC1_DEVLC, STS);
+		}
+	}
 	return 0;
 }
 
diff --git a/drivers/usb/host/ohci-at91.c b/drivers/usb/host/ohci-at91.c
index efd711d48..2060a3eb4 100644
--- a/drivers/usb/host/ohci-at91.c
+++ b/drivers/usb/host/ohci-at91.c
@@ -42,7 +42,7 @@ int usb_cpu_init(void)
 	while ((readl(&pmc->sr) & AT91_PMC_LOCKB) != AT91_PMC_LOCKB)
 		;
 #elif defined(CONFIG_AT91SAM9G45) || defined(CONFIG_AT91SAM9M10G45) || \
-	defined(CONFIG_AT91SAM9X5)
+	defined(CONFIG_AT91SAM9X5) || defined(CONFIG_SAMA5D3)
 	/* Enable UPLL */
 	writel(readl(&pmc->uckr) | AT91_PMC_UPLLEN | AT91_PMC_BIASEN,
 		&pmc->uckr);
@@ -54,8 +54,13 @@ int usb_cpu_init(void)
 #endif
 
 	/* Enable USB host clock. */
+#ifdef CONFIG_SAMA5D3
+	writel(1 << (ATMEL_ID_UHP - 32), &pmc->pcer1);
+#else
 	writel(1 << ATMEL_ID_UHP, &pmc->pcer);
-#ifdef CONFIG_AT91SAM9261
+#endif
+
+#if defined(CONFIG_AT91SAM9261) || defined(CONFIG_AT91SAM9G10)
 	writel(ATMEL_PMC_UHP | AT91_PMC_HCK0, &pmc->scer);
 #else
 	writel(ATMEL_PMC_UHP, &pmc->scer);
@@ -69,8 +74,13 @@ int usb_cpu_stop(void)
 	at91_pmc_t *pmc	= (at91_pmc_t *)ATMEL_BASE_PMC;
 
 	/* Disable USB host clock. */
+#ifdef CONFIG_SAMA5D3
+	writel(1 << (ATMEL_ID_UHP - 32), &pmc->pcdr1);
+#else
 	writel(1 << ATMEL_ID_UHP, &pmc->pcdr);
-#ifdef CONFIG_AT91SAM9261
+#endif
+
+#if defined(CONFIG_AT91SAM9261) || defined(CONFIG_AT91SAM9G10)
 	writel(ATMEL_PMC_UHP | AT91_PMC_HCK0, &pmc->scdr);
 #else
 	writel(ATMEL_PMC_UHP, &pmc->scdr);
@@ -83,7 +93,7 @@ int usb_cpu_stop(void)
 	while ((readl(&pmc->sr) & AT91_PMC_LOCKB) != 0)
 		;
 #elif defined(CONFIG_AT91SAM9G45) || defined(CONFIG_AT91SAM9M10G45) || \
-	defined(CONFIG_AT91SAM9X5)
+	defined(CONFIG_AT91SAM9X5) || defined(CONFIG_SAMA5D3)
 	/* Disable UPLL */
 	writel(readl(&pmc->uckr) & (~AT91_PMC_UPLLEN), &pmc->uckr);
 	while ((readl(&pmc->sr) & AT91_PMC_LOCKU) == AT91_PMC_LOCKU)