ENGR00114795 fix usb OTG co-work with remote wakeup issue.

This patch fix OTG pin-detect failure issue.
via sysfs set power/wakeup, i.MX51 can:
Put usb phy into low power mode when system enters stop or
standby mode; Enable usb device remote wakeup system by Vbus
and usb host remote wakeup system by device connection or
disconnection.

Signed-off-by: Li Jun <r65092@freescale.com>

9 files changed, 171 insertions, 140 deletions

diff --git a/drivers/usb/core/hcd.c b/drivers/usb/core/hcd.c
index 0221294a4e3f..3208360664c8 100644
--- a/drivers/usb/core/hcd.c
+++ b/drivers/usb/core/hcd.c
@@ -117,10 +117,9 @@ static inline int is_root_hub(struct usb_device *udev)
 	return (udev->parent == NULL);
 }
 
-#if defined(CONFIG_USB_EHCI_ARC_H2_WAKE_UP) || \
-	defined(CONFIG_USB_EHCI_ARC_OTG_WAKE_UP)
-extern int usb_wakeup_irq(struct device *wkup_dev);
-extern void usb_wakeup_set(struct device *wkup_dev, int para);
+#if CONFIG_PM
+extern int usb_host_wakeup_irq(struct device *wkup_dev);
+extern void usb_host_set_wakeup(struct device *wkup_dev, bool para);
 #endif
 /*-------------------------------------------------------------------------*/
 
@@ -1729,22 +1728,19 @@ irqreturn_t usb_hcd_irq (int irq, void *__hcd)
 	 * assume it's never used.
 	 */
 	local_irq_save(flags);
-#if defined(CONFIG_USB_EHCI_ARC_H2_WAKE_UP) || \
-	defined(CONFIG_USB_EHCI_ARC_OTG_WAKE_UP)
-	/* if receive a remote wakeup interrrupt when suspend */
-	if (usb_wakeup_irq(hcd->self.controller)) {
-		/* disable remote wake up irq */
-		usb_wakeup_set(hcd->self.controller, 0);
-		set_bit(HCD_FLAG_HW_ACCESSIBLE, &hcd->flags);
-		rc = hcd->driver->irq(hcd);
-	} else if (unlikely(hcd->state == HC_STATE_HALT ||
-		!test_bit(HCD_FLAG_HW_ACCESSIBLE, &hcd->flags))) {
-		rc = IRQ_NONE;
-#else
-	if (unlikely(hcd->state == HC_STATE_HALT ||
-		!test_bit(HCD_FLAG_HW_ACCESSIBLE, &hcd->flags))) {
+
+	if (!test_bit(HCD_FLAG_HW_ACCESSIBLE, &hcd->flags)) {
+		/* if receive a remote wakeup interrrupt after suspend */
+		if (usb_host_wakeup_irq(hcd->self.controller)) {
+			/* disable remote wake up irq */
+			usb_host_set_wakeup(hcd->self.controller, false);
+			set_bit(HCD_FLAG_HW_ACCESSIBLE, &hcd->flags);
+			hcd->driver->irq(hcd);
+			rc = IRQ_HANDLED;
+		} else
+			rc = IRQ_NONE;
+	} else if (unlikely(hcd->state == HC_STATE_HALT)) {
 		rc = IRQ_NONE;
-#endif
 	} else if (hcd->driver->irq(hcd) == IRQ_NONE) {
 		rc = IRQ_NONE;
 	} else {
diff --git a/drivers/usb/gadget/Kconfig b/drivers/usb/gadget/Kconfig
index 71fe9ca3a15d..09284dee18b3 100644
--- a/drivers/usb/gadget/Kconfig
+++ b/drivers/usb/gadget/Kconfig
@@ -487,12 +487,6 @@ config USB_GADGET_ARC_OTG
 	help
 	  Enable support for the Freescale Dual Role port in peripheral mode.
 
-config USB_GADGET_WAKE_UP
-	bool "Support gadget wake up by DR port"
-	depends on USB_GADGET_ARC_OTG && ARCH_MXC
-	default n
-	help
-	  Enable system wake up from SR or DSM mode by connected host.
 choice
 	prompt "Select transceiver for DR port"
 	depends on USB_GADGET_ARC_OTG
diff --git a/drivers/usb/gadget/arcotg_udc.c b/drivers/usb/gadget/arcotg_udc.c
index 40df0fb754e6..250d5e9b95ac 100644
--- a/drivers/usb/gadget/arcotg_udc.c
+++ b/drivers/usb/gadget/arcotg_udc.c
@@ -256,6 +256,64 @@ static void nuke(struct fsl_ep *ep, int status)
 	Internal Hardware related function
  ------------------------------------------------------------------*/
 
+static void dr_phy_low_power_mode(struct fsl_udc *udc, bool enable)
+{
+	u32 temp;
+
+	if (!device_may_wakeup(&(udc->pdata->pdev->dev)))
+		return;
+
+	temp = fsl_readl(&dr_regs->portsc1);
+	if ((enable) && !(temp & PORTSCX_PHY_LOW_POWER_SPD)) {
+		temp |= PORTSCX_PHY_LOW_POWER_SPD;
+		fsl_writel(temp, &dr_regs->portsc1);
+
+		if (udc_controller->pdata->usb_clock_for_pm)
+			udc_controller->pdata->usb_clock_for_pm(false);
+	} else if ((!enable) && (temp & PORTSCX_PHY_LOW_POWER_SPD)) {
+		if (udc_controller->pdata->usb_clock_for_pm)
+			udc_controller->pdata->usb_clock_for_pm(true);
+
+		temp &= ~PORTSCX_PHY_LOW_POWER_SPD;
+		fsl_writel(temp, &dr_regs->portsc1);
+	}
+}
+
+static void dr_wake_up_enable(struct fsl_udc *udc, bool enable)
+{
+	u32 temp;
+
+	temp = fsl_readl(&dr_regs->usbctrl);
+
+	if (!enable) {
+		temp &= ~USB_CTRL_OTG_WUIE;
+		fsl_writel(temp, &dr_regs->usbctrl);
+
+		/* OTG vbus Wakeup disable */
+		temp = fsl_readl(&dr_regs->uh2ctrl);
+		temp &= ~USB_UH2_OVBWK_EN;
+		fsl_writel(temp, &dr_regs->uh2ctrl);
+
+		/* disable conf2 */
+		temp = fsl_readl(&dr_regs->phyctrl0);
+		temp &= ~PHY_CTRL0_CONF2;
+		fsl_writel(temp, &dr_regs->phyctrl0);
+	} else if (device_may_wakeup(&(udc->pdata->pdev->dev))) {
+		temp |= USB_CTRL_OTG_WUIE;
+		fsl_writel(temp, &dr_regs->usbctrl);
+
+		/* OTG vbus wakeup enable */
+		temp = fsl_readl(&dr_regs->uh2ctrl);
+		temp |= USB_UH2_OVBWK_EN;
+		fsl_writel(temp, &dr_regs->uh2ctrl);
+
+		/* enable conf2 */
+		temp = fsl_readl(&dr_regs->phyctrl0);
+		temp |= PHY_CTRL0_CONF2;
+		fsl_writel(temp, &dr_regs->phyctrl0);
+	}
+}
+
 static int dr_controller_setup(struct fsl_udc *udc)
 {
 	unsigned int tmp = 0, portctrl = 0;
@@ -1864,20 +1922,14 @@ static void suspend_irq(struct fsl_udc *udc)
 		udc->driver->suspend(&udc->gadget);
 }
 
-#ifdef CONFIG_USB_GADGET_WAKE_UP
 /* Process Wake up interrupt */
 static void wake_up_irq(struct fsl_udc *udc)
 {
-	u32 irq_src;
-
 	pr_debug("%s\n", __func__);
 
 	/* disable wake up irq */
-	irq_src = fsl_readl(&dr_regs->usbctrl);
-	irq_src &= ~USB_CTRL_OTG_WUIE;
-	fsl_writel(irq_src, &dr_regs->usbctrl);
+	dr_wake_up_enable(udc, false);
 }
-#endif
 
 static void bus_resume(struct fsl_udc *udc)
 {
@@ -1968,28 +2020,31 @@ static irqreturn_t fsl_udc_irq(int irq, void *_udc)
 	irqreturn_t status = IRQ_NONE;
 	unsigned long flags;
 
-#ifdef CONFIG_USB_GADGET_WAKE_UP
-	spin_lock_irqsave(&udc->lock, flags);
-	/* check USBCTRL register to see if wake up irq */
-	irq_src =  fsl_readl(&dr_regs->usbctrl);
-	if (irq_src & USB_CTRL_OTG_WUIR) {
-		wake_up_irq(udc);
-		irq_src = fsl_readl(&dr_regs->usbsts) &
-			fsl_readl(&dr_regs->usbintr);
-		if (irq_src)
-			udc->stopped = 0;
-		else
-			status = IRQ_HANDLED;
+	/* when udc is stopped, only handle wake up irq */
+	if (udc->stopped) {
+		if (!device_may_wakeup(&(udc->pdata->pdev->dev)))
+			return IRQ_NONE;
+
+		spin_lock_irqsave(&udc->lock, flags);
+		/* check to see if wake up irq */
+		irq_src =  fsl_readl(&dr_regs->usbctrl);
+		if (irq_src & USB_CTRL_OTG_WUIR) {
+			wake_up_irq(udc);
+			irq_src = fsl_readl(&dr_regs->usbsts) &
+				fsl_readl(&dr_regs->usbintr);
+			spin_unlock_irqrestore(&udc->lock, flags);
+			if (irq_src)
+				/* Some udc irq to be handled */
+				udc->stopped = 0;
+			else
+				return IRQ_HANDLED;
+		} else {
+			/* If udc is stopped and irq is not wake up */
+			spin_unlock_irqrestore(&udc->lock, flags);
+			return IRQ_NONE;
+		}
 	}
-	spin_unlock_irqrestore(&udc->lock, flags);
-
-	if (status == IRQ_HANDLED)
-		return IRQ_HANDLED;
-#endif
 
-	/* Disable ISR for OTG host mode */
-	if (udc->stopped)
-		return IRQ_NONE;
 	spin_lock_irqsave(&udc->lock, flags);
 	irq_src = fsl_readl(&dr_regs->usbsts) & fsl_readl(&dr_regs->usbintr);
 	/* Clear notification bits */
@@ -2095,6 +2150,8 @@ int usb_gadget_register_driver(struct usb_gadget_driver *driver)
 		/* Suspend the controller until OTG enable it */
 		udc_controller->stopped = 1;
 		printk(KERN_INFO "Suspend udc for OTG auto detect\n");
+		dr_wake_up_enable(udc_controller, true);
+		dr_phy_low_power_mode(udc_controller, true);
 
 		/* export udc suspend/resume call to OTG */
 		udc_controller->gadget.dev.driver->suspend = (dev_sus)fsl_udc_suspend;
@@ -2840,13 +2897,20 @@ static int udc_suspend(struct fsl_udc *udc)
 		return 0;
 	}
 
+	udc->stopped = 1;
+	/* if the suspend is not for switch to host in otg mode */
+	if ((!(udc->gadget.is_otg)) ||
+			(fsl_readl(&dr_regs->otgsc) & OTGSC_STS_USB_ID)) {
+		dr_wake_up_enable(udc, true);
+		dr_phy_low_power_mode(udc, true);
+	}
+
 	/* stop the controller */
 	usbcmd = fsl_readl(&dr_regs->usbcmd) & ~USB_CMD_RUN_STOP;
 	fsl_writel(usbcmd, &dr_regs->usbcmd);
 
 	printk(KERN_INFO "USB Gadget suspended\n");
 
-	udc->stopped = 1;
 	return 0;
 }
 
@@ -2856,35 +2920,10 @@ static int udc_suspend(struct fsl_udc *udc)
  -----------------------------------------------------------------*/
 static int fsl_udc_suspend(struct platform_device *pdev, pm_message_t state)
 {
-	unsigned int port_status, temp;
-
 	if ((udc_controller->usb_state > USB_STATE_POWERED) &&
 			(udc_controller->usb_state < USB_STATE_SUSPENDED))
 		return -EBUSY;
 
-#ifdef CONFIG_USB_GADGET_WAKE_UP
-	temp = fsl_readl(&dr_regs->usbctrl);
-	/* if usb wake up irq is disabled, enable it */
-	if (!(temp & USB_CTRL_OTG_WUIE)) {
-		temp |= USB_CTRL_OTG_WUIE;
-		fsl_writel(temp, &dr_regs->usbctrl);
-	}
-#else
-	temp = fsl_readl(&dr_regs->usbctrl);
-	/* if usb wake up irq is enabled, disable it */
-	if (temp & USB_CTRL_OTG_WUIE) {
-		temp &= ~USB_CTRL_OTG_WUIE;
-		fsl_writel(temp, &dr_regs->usbctrl);
-	}
-#endif
-
-	/* close UBS PHY clock if PHCD is 0 */
-	port_status = fsl_readl(&dr_regs->portsc1);
-	if (!(port_status & PORTSCX_PHY_LOW_POWER_SPD)) {
-		port_status |= PORTSCX_PHY_LOW_POWER_SPD;
-		fsl_writel(port_status, &dr_regs->portsc1);
-	}
-
 	return udc_suspend(udc_controller);
 }
 
@@ -2909,22 +2948,9 @@ static int fsl_udc_resume(struct platform_device *pdev)
 
 	/* Enable DR irq reg and set controller Run */
 	if (udc_controller->stopped) {
-		u32 temp;
-
-#ifdef CONFIG_USB_GADGET_WAKE_UP
-		/* disable wake up irq */
-		temp = fsl_readl(&dr_regs->usbctrl);
-		temp &= ~UCTRL_OWIE;
-		fsl_writel(temp, &dr_regs->usbctrl);
-#endif
-
-		/* Enable PHY clock if it's disabled */
-		temp = fsl_readl(&dr_regs->portsc1);
-		if (temp & PORTSCX_PHY_LOW_POWER_SPD) {
-			temp &= ~PORTSCX_PHY_LOW_POWER_SPD;
-			fsl_writel(temp, &dr_regs->portsc1);
-			mdelay(1);
-		}
+		dr_wake_up_enable(udc_controller, false);
+		dr_phy_low_power_mode(udc_controller, false);
+		mdelay(1);
 
 		dr_controller_setup(udc_controller);
 		dr_controller_run(udc_controller);
@@ -2958,7 +2984,7 @@ static int __init udc_init(void)
 	return platform_driver_register(&udc_driver);
 }
 
-late_initcall(udc_init);
+module_init(udc_init);
 
 static void __exit udc_exit(void)
 {
diff --git a/drivers/usb/gadget/arcotg_udc.h b/drivers/usb/gadget/arcotg_udc.h
index fbdb3203f12e..e7cca079e184 100644
--- a/drivers/usb/gadget/arcotg_udc.h
+++ b/drivers/usb/gadget/arcotg_udc.h
@@ -101,7 +101,15 @@ struct usb_dr_device {
 	u32 endptcomplete;	/* Endpoint Complete Register */
 	u32 endptctrl[8 * 2];	/* Endpoint Control Registers */
 	u32 res8[256];
+#ifdef CONFIG_ARCH_MX51
+	u32 res9[128];		/* i.MX51 start from 0x800 */
+#endif
 	u32 usbctrl;
+	u32 otgmirror;
+	u32 phyctrl0;
+	u32 phyctrl1;
+	u32 ctrl1;
+	u32 uh2ctrl;
 };
 
  /* non-EHCI USB system interface registers (Big Endian) */
@@ -344,7 +352,15 @@ struct usb_sys_interface {
 #define  USB_CTRL_ULPI_INT0EN                 (0x00000001)
 #define  USB_CTRL_OTG_WUIR                   (0x80000000)
 #define  USB_CTRL_OTG_WUIE                   (0x08000000)
+#define  USB_CTRL_OTG_VWUE			(0x00001000)
+#define  USB_CTRL_OTG_IWUE			(0x00100000)
+
+/* PHY control0 Register Bit Masks */
+#define	PHY_CTRL0_CONF2			(1 << 26)
 
+/* USB UH2 CTRL Register Bits */
+#define USB_UH2_OVBWK_EN		(1 << 6) /* OTG VBUS Wakeup Enable */
+#define USB_UH2_OIDWK_EN		(1 << 5) /* OTG ID Wakeup Enable */
 /*!
  * Endpoint Queue Head data struct
  * Rem: all the variables of qh are LittleEndian Mode
diff --git a/drivers/usb/gadget/file_storage.c b/drivers/usb/gadget/file_storage.c
index 58e278e24f7c..2529b1f578e6 100644
--- a/drivers/usb/gadget/file_storage.c
+++ b/drivers/usb/gadget/file_storage.c
@@ -4148,7 +4148,7 @@ static int __init fsg_init(void)
 		kref_put(&fsg->ref, fsg_release);
 	return rc;
 }
-late_initcall(fsg_init);
+module_init(fsg_init);
 
 
 static void __exit fsg_cleanup(void)
diff --git a/drivers/usb/host/Kconfig b/drivers/usb/host/Kconfig
index 78bbd6ee55ad..4e539e30f1f6 100644
--- a/drivers/usb/host/Kconfig
+++ b/drivers/usb/host/Kconfig
@@ -65,14 +65,6 @@ config USB_EHCI_ARC_H2
 	---help---
 	  Enable support for the USB Host2 port.
 
-config USB_EHCI_ARC_H2_WAKE_UP
-        bool "Support wake up from Freescale Host2 port"
-	depends on PM && USB_EHCI_ARC_H2
-	default n
-	---help---
-	  Enable support system wake up from Host2 port usb device
-	  connection and disconnection.
-
 config USB_EHCI_ARC_OTG
 	bool "Support for DR host port on Freescale controller"
 	depends on USB_EHCI_ARC
@@ -80,14 +72,6 @@ config USB_EHCI_ARC_OTG
 	---help---
 	  Enable support for the USB OTG port in HS/FS Host mode.
 
-config USB_EHCI_ARC_OTG_WAKE_UP
-        bool "Support wake up from Freescale OTG port"
-	depends on PM && ARCH_MXC && USB_EHCI_ARC_OTG
-	default n
-	---help---
-	  Enable support system wake up from Host1 port usb device
-	  connection and disconnection.
-
 config USB_STATIC_IRAM
 	bool "Use IRAM for USB"
 	depends on USB_EHCI_ARC
diff --git a/drivers/usb/host/ehci-arc.c b/drivers/usb/host/ehci-arc.c
index f1617e9e90c3..c91634d68c0c 100644
--- a/drivers/usb/host/ehci-arc.c
+++ b/drivers/usb/host/ehci-arc.c
@@ -115,7 +115,6 @@ static int usb_hcd_fsl_probe(const struct hc_driver *driver,
 	struct resource *res;
 	int irq;
 	int retval;
-	u32 temp;
 
 	pr_debug("initializing FSL-SOC USB Controller\n");
 
@@ -245,6 +244,12 @@ static int usb_hcd_fsl_probe(const struct hc_driver *driver,
 	}
 #endif
 
+	if (pdata->suspended) {
+		pdata->suspended = 0;
+		if (pdata->already_suspended)
+			pdata->already_suspended = 0;
+	}
+
 	fsl_platform_set_ahb_burst(hcd);
 	ehci_testmode_init(hcd_to_ehci(hcd));
 	return retval;
@@ -421,8 +426,6 @@ static const struct hc_driver ehci_fsl_hc_driver = {
 
 static int ehci_fsl_drv_probe(struct platform_device *pdev)
 {
-	struct fsl_usb2_platform_data *pdata = pdev->dev.platform_data;
-
 	if (usb_disabled())
 		return -ENODEV;
 
@@ -431,21 +434,19 @@ static int ehci_fsl_drv_probe(struct platform_device *pdev)
 
 static int ehci_fsl_drv_remove(struct platform_device *pdev)
 {
-	struct fsl_usb2_platform_data *pdata = pdev->dev.platform_data;
 	struct usb_hcd *hcd = platform_get_drvdata(pdev);
 
 	usb_hcd_fsl_remove(hcd, pdev);
 	return 0;
 }
 
-static int ehci_fsl_drv_shutdown(struct platform_device *pdev)
+static void ehci_fsl_drv_shutdown(struct platform_device *pdev)
 {
-	struct fsl_usb2_platform_data *pdata = pdev->dev.platform_data;
-
 	usb_hcd_platform_shutdown(pdev);
 }
 
 #ifdef CONFIG_PM
+extern void usb_host_set_wakeup(struct device *wkup_dev, bool para);
 /* suspend/resume, section 4.3 */
 
 /* These routines rely on the bus (pci, platform, etc)
@@ -455,16 +456,12 @@ static int ehci_fsl_drv_shutdown(struct platform_device *pdev)
  *
  * They're also used for turning on/off the port when doing OTG.
  */
-#if defined(CONFIG_USB_EHCI_ARC_H2_WAKE_UP) || \
-	defined(CONFIG_USB_EHCI_ARC_OTG_WAKE_UP)
-extern void usb_wakeup_set(struct device *wkup_dev, int para);
-#endif
 static int ehci_fsl_drv_suspend(struct platform_device *pdev,
 				pm_message_t message)
 {
 	struct usb_hcd *hcd = platform_get_drvdata(pdev);
 	struct ehci_hcd *ehci = hcd_to_ehci(hcd);
-	u32 tmp;
+	u32 tmp, port_status;
 	struct fsl_usb2_platform_data *pdata = pdev->dev.platform_data;
 
 #ifdef DEBUG
@@ -492,6 +489,7 @@ static int ehci_fsl_drv_suspend(struct platform_device *pdev,
 
 	printk(KERN_INFO "USB Host suspended\n");
 
+	port_status = ehci_readl(ehci, &ehci->regs->port_status[0]);
 	hcd->state = HC_STATE_SUSPENDED;
 	pdev->dev.power.power_state = PMSG_SUSPEND;
 
@@ -520,16 +518,26 @@ static int ehci_fsl_drv_suspend(struct platform_device *pdev,
 	pdata->pm_portsc &= cpu_to_hc32(ehci, ~PORT_RWC_BITS);
 
 	pdata->suspended = 1;
-#if defined(CONFIG_USB_EHCI_ARC_H2_WAKE_UP) || \
-	defined(CONFIG_USB_EHCI_ARC_OTG_WAKE_UP)
-	/* enable remote wake up irq */
-	usb_wakeup_set(&(pdev->dev), 1);
+
+	if (!device_may_wakeup(&(pdev->dev))) {
+		/* clear PP to cut power to the port */
+		tmp = ehci_readl(ehci, &ehci->regs->port_status[0]);
+		tmp &= ~PORT_POWER;
+		ehci_writel(ehci, tmp, &ehci->regs->port_status[0]);
+		return 0;
+	}
+
+	/* device_may_wakeup */
+	if (!((ehci->transceiver) &&
+			(readl(hcd->regs + 0x1A4) & (1 << 8)))) {
+		/* enable remote wake up irq */
+		usb_host_set_wakeup(&(pdev->dev), true);
 
 	/* We CAN NOT enable wake up by connetion and disconnection
 	 * concurrently */
 	tmp = ehci_readl(ehci, &ehci->regs->port_status[0]);
 	/* if there is no usb device connectted */
-	if (tmp & PORT_CONNECT) {
+	if (port_status & PORT_CONNECT) {
 		/* enable wake up by usb device disconnection */
 		tmp |= PORT_WKDISC_E;
 		tmp &= ~(PORT_WKOC_E | PORT_WKCONN_E);
@@ -547,14 +555,10 @@ static int ehci_fsl_drv_suspend(struct platform_device *pdev,
 
 	/* Disable PHY clock */
 	tmp = ehci_readl(ehci, &ehci->regs->port_status[0]);
-	tmp |= PORT_PHCD;
-	ehci_writel(ehci, tmp, &ehci->regs->port_status[0]);
-#else
-	/* clear PP to cut power to the port */
-	tmp = ehci_readl(ehci, &ehci->regs->port_status[0]);
-	tmp &= ~PORT_POWER;
+	tmp |= (1 << 23);
 	ehci_writel(ehci, tmp, &ehci->regs->port_status[0]);
-#endif
+	}
+
 	return 0;
 }
 
@@ -565,8 +569,6 @@ static int ehci_fsl_drv_resume(struct platform_device *pdev)
 	u32 tmp;
 	struct fsl_usb2_platform_data *pdata = pdev->dev.platform_data;
 
-	printk(KERN_INFO "USB Host resumed\n");
-
 	pr_debug("%s('%s'): suspend=%d already_suspended=%d\n", __func__,
 		pdata->name, pdata->suspended, pdata->already_suspended);
 
@@ -585,6 +587,15 @@ static int ehci_fsl_drv_resume(struct platform_device *pdev)
 		return 0;
 	}
 
+	if (device_may_wakeup(&(pdev->dev))) {
+		tmp = ehci_readl(ehci, &ehci->regs->port_status[0]);
+		if (tmp & (1 << 23)) {
+			tmp &= ~(1 << 23);
+			ehci_writel(ehci, tmp, &ehci->regs->port_status[0]);
+			msleep(10);
+		}
+	}
+
 	pdata->suspended = 0;
 
 	pr_debug("%s resuming...\n", __func__);
@@ -613,6 +624,7 @@ static int ehci_fsl_drv_resume(struct platform_device *pdev)
 
 	usb_hcd_resume_root_hub(hcd);
 
+	printk(KERN_INFO "USB Host resumed\n");
 	return 0;
 }
 #endif				/* CONFIG_USB_OTG */
diff --git a/drivers/usb/host/ehci-hcd.c b/drivers/usb/host/ehci-hcd.c
index 58bd08e2898f..50b6fa08e987 100644
--- a/drivers/usb/host/ehci-hcd.c
+++ b/drivers/usb/host/ehci-hcd.c
@@ -1133,7 +1133,7 @@ err_debug:
 	clear_bit(USB_EHCI_LOADED, &usb_hcds_loaded);
 	return retval;
 }
-late_initcall(ehci_hcd_init);
+module_init(ehci_hcd_init);
 
 static void __exit ehci_hcd_cleanup(void)
 {
diff --git a/drivers/usb/otg/fsl_otg.h b/drivers/usb/otg/fsl_otg.h
index c358045fe7c6..03e232112b83 100644
--- a/drivers/usb/otg/fsl_otg.h
+++ b/drivers/usb/otg/fsl_otg.h
@@ -1,4 +1,4 @@
-/* Copyright (C) 2005-2008 Freescale Semiconductor, Inc.
+/* Copyright 2005-2009 Freescale Semiconductor, Inc.
  *
  * 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
@@ -348,6 +348,9 @@ struct usb_dr_mmap {
 	u32 pri_ctrl;		/* Priority Control Register */
 	u32 si_ctrl;		/* System Interface Control Register */
 	u8 res10[236];
+#ifdef CONFIG_ARCH_MX51
+	u32 res11[128];
+#endif
 	u32 control;		/* General Purpose Control Register */
 };