diff --git a/drivers/usb/host/ehci-hcd.c b/drivers/usb/host/ehci-hcd.c
index fdad739..3c570ef 100644
--- a/drivers/usb/host/ehci-hcd.c
+++ b/drivers/usb/host/ehci-hcd.c
@@ -881,6 +881,11 @@ int usb_lowlevel_stop(int index)
 	return ehci_hcd_stop(index);
 }
 
+int __weak ehci_hcd_init_after_reset(int index)
+{
+	return 0;
+}
+
 int usb_lowlevel_init(int index, void **controller)
 {
 	uint32_t reg;
@@ -900,6 +905,8 @@ int usb_lowlevel_init(int index, void **controller)
 	if (ehci_hcd_init(index, &ehcic[index].hccr, &ehcic[index].hcor))
 		return -1;
 #endif
+	ehci_hcd_init_after_reset(index);
+
 	/* Set the high address word (aka segment) for 64-bit controller */
 	if (ehci_readl(&ehcic[index].hccr->cr_hccparams) & 1)
 		ehci_writel(ehcic[index].hcor->or_ctrldssegment, 0);
diff --git a/drivers/usb/host/ehci-mx6.c b/drivers/usb/host/ehci-mx6.c
index eb24af5..41b444e 100644
--- a/drivers/usb/host/ehci-mx6.c
+++ b/drivers/usb/host/ehci-mx6.c
@@ -35,6 +35,7 @@
 #define USBPHY_CTRL_CLKGATE			0x40000000
 #define USBPHY_CTRL_ENUTMILEVEL3		0x00008000
 #define USBPHY_CTRL_ENUTMILEVEL2		0x00004000
+#define USBPHY_CTRL_OTD_ID_BIT			27
 
 #define ANADIG_USB2_CHRG_DETECT_EN_B		0x00100000
 #define ANADIG_USB2_CHRG_DETECT_CHK_CHRG_B	0x00080000
@@ -49,52 +50,83 @@
 #define UCTRL_OVER_CUR_DIS	(1 << 7) /* Disable OTG Overcurrent Detection */
 
 /* USBCMD */
-#define UH1_USBCMD_OFFSET	0x140
 #define UCMD_RUN_STOP           (1 << 0) /* controller run/stop */
 #define UCMD_RESET		(1 << 1) /* controller reset */
 
-static void usbh1_internal_phy_clock_gate(int on)
+static const unsigned phy_bases[] = {
+	USB_PHY0_BASE_ADDR,
+	USB_PHY1_BASE_ADDR,
+};
+
+static void usb_internal_phy_clock_gate(int index, int on)
 {
-	void __iomem *phy_reg = (void __iomem *)USB_PHY1_BASE_ADDR;
+	void __iomem *phy_reg;
+
+	if (index >= ARRAY_SIZE(phy_bases))
+		return;
 
+	phy_reg = (void __iomem *)phy_bases[index];
 	phy_reg += on ? USBPHY_CTRL_CLR : USBPHY_CTRL_SET;
 	__raw_writel(USBPHY_CTRL_CLKGATE, phy_reg);
 }
 
-static void usbh1_power_config(void)
+static void usb_power_config(int index)
 {
 	struct anatop_regs __iomem *anatop =
 		(struct anatop_regs __iomem *)ANATOP_BASE_ADDR;
+	void __iomem *chrg_detect;
+	void __iomem *pll_480_ctrl_clr;
+	void __iomem *pll_480_ctrl_set;
+
+	switch (index) {
+	case 0:
+		chrg_detect = &anatop->usb1_chrg_detect;
+		pll_480_ctrl_clr = &anatop->usb1_pll_480_ctrl_clr;
+		pll_480_ctrl_set = &anatop->usb1_pll_480_ctrl_set;
+		break;
+	case 1:
+		chrg_detect = &anatop->usb2_chrg_detect;
+		pll_480_ctrl_clr = &anatop->usb2_pll_480_ctrl_clr;
+		pll_480_ctrl_set = &anatop->usb2_pll_480_ctrl_set;
+		break;
+	default:
+		return;
+	}
 	/*
-	 * Some phy and power's special controls for host1
+	 * Some phy and power's special controls
 	 * 1. The external charger detector needs to be disabled
 	 * or the signal at DP will be poor
-	 * 2. The PLL's power and output to usb for host 1
+	 * 2. The PLL's power and output to usb
 	 * is totally controlled by IC, so the Software only needs
 	 * to enable them at initializtion.
 	 */
 	__raw_writel(ANADIG_USB2_CHRG_DETECT_EN_B |
 		     ANADIG_USB2_CHRG_DETECT_CHK_CHRG_B,
-		     &anatop->usb2_chrg_detect);
+		     chrg_detect);
 
 	__raw_writel(ANADIG_USB2_PLL_480_CTRL_BYPASS,
-		     &anatop->usb2_pll_480_ctrl_clr);
+		     pll_480_ctrl_clr);
 
 	__raw_writel(ANADIG_USB2_PLL_480_CTRL_ENABLE |
 		     ANADIG_USB2_PLL_480_CTRL_POWER |
 		     ANADIG_USB2_PLL_480_CTRL_EN_USB_CLKS,
-		     &anatop->usb2_pll_480_ctrl_set);
+		     pll_480_ctrl_set);
 }
 
-static int usbh1_phy_enable(void)
+static int usb_phy_enable(int index, struct usb_ehci *ehci)
 {
-	void __iomem *phy_reg = (void __iomem *)USB_PHY1_BASE_ADDR;
-	void __iomem *phy_ctrl = (void __iomem *)(phy_reg + USBPHY_CTRL);
-	void __iomem *usb_cmd =	(void __iomem *)(USBOH3_USB_BASE_ADDR +
-						 USB_H1REGS_OFFSET +
-						 UH1_USBCMD_OFFSET);
+	void __iomem *phy_reg;
+	void __iomem *phy_ctrl;
+	void __iomem *usb_cmd;
 	u32 val;
 
+	if (index >= ARRAY_SIZE(phy_bases))
+		return;
+
+	phy_reg = (void __iomem *)phy_bases[index];
+	phy_ctrl = (void __iomem *)(phy_reg + USBPHY_CTRL);
+	usb_cmd = (void __iomem *)&ehci->usbcmd;
+
 	/* Stop then Reset */
 	val = __raw_readl(usb_cmd);
 	val &= ~UCMD_RUN_STOP;
@@ -123,31 +155,36 @@ static int usbh1_phy_enable(void)
 	/* Power up the PHY */
 	__raw_writel(0, phy_reg + USBPHY_PWD);
 	/* enable FS/LS device */
-	val = __raw_readl(phy_reg + USBPHY_CTRL);
+	val = __raw_readl(phy_ctrl);
 	val |= (USBPHY_CTRL_ENUTMILEVEL2 | USBPHY_CTRL_ENUTMILEVEL3);
-	__raw_writel(val, phy_reg + USBPHY_CTRL);
+	__raw_writel(val, phy_ctrl);
 
 	return 0;
 }
 
-static void usbh1_oc_config(void)
+struct usbnc_regs {
+	u32	ctrl[4];	/* otg/host1-3 */
+};
+
+static void usb_oc_config(int index)
 {
-	void __iomem *usb_base = (void __iomem *)USBOH3_USB_BASE_ADDR;
-	void __iomem *usbother_base = usb_base + USB_OTHERREGS_OFFSET;
+	struct usbnc_regs *usbnc = (struct usbnc_regs *)(USBOH3_USB_BASE_ADDR +
+			USB_OTHERREGS_OFFSET);
+	void __iomem *ctrl = (void __iomem *)(&usbnc->ctrl[index]);
 	u32 val;
 
-	val = __raw_readl(usbother_base + USB_H1_CTRL_OFFSET);
+	val = __raw_readl(ctrl);
 #if CONFIG_MACH_TYPE == MACH_TYPE_MX6Q_ARM2
 	/* mx6qarm2 seems to required a different setting*/
 	val &= ~UCTRL_OVER_CUR_POL;
 #else
 	val |= UCTRL_OVER_CUR_POL;
 #endif
-	__raw_writel(val, usbother_base + USB_H1_CTRL_OFFSET);
+	__raw_writel(val, ctrl);
 
-	val = __raw_readl(usbother_base + USB_H1_CTRL_OFFSET);
+	val = __raw_readl(ctrl);
 	val |= UCTRL_OVER_CUR_DIS;
-	__raw_writel(val, usbother_base + USB_H1_CTRL_OFFSET);
+	__raw_writel(val, ctrl);
 }
 
 int __weak board_ehci_hcd_init(int port)
@@ -157,31 +194,51 @@ int __weak board_ehci_hcd_init(int port)
 
 int ehci_hcd_init(int index, struct ehci_hccr **hccr, struct ehci_hcor **hcor)
 {
-	struct usb_ehci *ehci;
+	struct usb_ehci *ehci = (struct usb_ehci *)(USBOH3_USB_BASE_ADDR +
+		(0x200 * index));
 
+	if (index > 3)
+		return -EINVAL;
 	enable_usboh3_clk(1);
 	mdelay(1);
 
 	/* Do board specific initialization */
-	board_ehci_hcd_init(CONFIG_MXC_USB_PORT);
-
-#if CONFIG_MXC_USB_PORT == 1
-	/* USB Host 1 */
-	usbh1_power_config();
-	usbh1_oc_config();
-	usbh1_internal_phy_clock_gate(1);
-	usbh1_phy_enable();
-#else
-#error "MXC USB port not yet supported"
-#endif
+	board_ehci_hcd_init(index);
+
+	usb_power_config(index);
+	usb_oc_config(index);
+	usb_internal_phy_clock_gate(index, 1);
+	usb_phy_enable(index, ehci);
 
-	ehci = (struct usb_ehci *)(USBOH3_USB_BASE_ADDR +
-		(0x200 * CONFIG_MXC_USB_PORT));
 	*hccr = (struct ehci_hccr *)((uint32_t)&ehci->caplength);
 	*hcor = (struct ehci_hcor *)((uint32_t)*hccr +
 			HC_LENGTH(ehci_readl(&(*hccr)->cr_capbase)));
-	setbits_le32(&ehci->usbmode, CM_HOST);
+	return 0;
+}
+
+int __weak board_ehci_power(int port, int on)
+{
+	return 0;
+}
 
+int ehci_hcd_init_after_reset(int index)
+{
+	int otg_id;
+	void __iomem *phy_reg;
+	void __iomem *phy_ctrl;
+	struct usb_ehci *ehci = (struct usb_ehci *)(USBOH3_USB_BASE_ADDR +
+		(0x200 * index));
+
+	if (index >= ARRAY_SIZE(phy_bases))
+		return 0;
+
+	phy_reg = (void __iomem *)phy_bases[index];
+	phy_ctrl = (void __iomem *)(phy_reg + USBPHY_CTRL);
+	otg_id = (__raw_readl(phy_ctrl) >> USBPHY_CTRL_OTD_ID_BIT) & 1;
+	printf("!!!index=%d otg_id=%d\n", index, otg_id);
+	board_ehci_power(index, otg_id ? 0 : 1);
+	if (!otg_id)
+		setbits_le32(&ehci->usbmode, CM_HOST);
 	__raw_writel(CONFIG_MXC_USB_PORTSC, &ehci->portsc);
 	setbits_le32(&ehci->portsc, USB_EN);
 
diff --git a/include/configs/mx6qsabreauto.h b/include/configs/mx6qsabreauto.h
index 5530fc6..805c7be 100644
--- a/include/configs/mx6qsabreauto.h
+++ b/include/configs/mx6qsabreauto.h
@@ -23,7 +23,7 @@
 #define CONFIG_USB_STORAGE
 #define CONFIG_USB_HOST_ETHER
 #define CONFIG_USB_ETHER_ASIX
-#define CONFIG_MXC_USB_PORT	1
+#define CONFIG_USB_MAX_CONTROLLER_COUNT 2
 #define CONFIG_MXC_USB_PORTSC	(PORT_PTS_UTMI | PORT_PTS_PTW)
 #define CONFIG_MXC_USB_FLAGS	0
 
diff --git a/include/configs/nitrogen6x.h b/include/configs/nitrogen6x.h
index 3454b86..b1ca91e 100644
--- a/include/configs/nitrogen6x.h
+++ b/include/configs/nitrogen6x.h
@@ -119,7 +119,7 @@
 #define CONFIG_USB_HOST_ETHER
 #define CONFIG_USB_ETHER_ASIX
 #define CONFIG_USB_ETHER_SMSC95XX
-#define CONFIG_MXC_USB_PORT	1
+#define CONFIG_USB_MAX_CONTROLLER_COUNT 2
 #define CONFIG_MXC_USB_PORTSC	(PORT_PTS_UTMI | PORT_PTS_PTW)
 #define CONFIG_MXC_USB_FLAGS	0
 
