2 files changed, 52 insertions, 5 deletions

diff --git a/drivers/usb/musb/musb_gadget.c b/drivers/usb/musb/musb_gadget.c
index 0f59bf935c85..95fbaccb03e1 100644
--- a/drivers/usb/musb/musb_gadget.c
+++ b/drivers/usb/musb/musb_gadget.c
@@ -1877,6 +1877,10 @@ int usb_gadget_probe_driver(struct usb_gadget_driver *driver,
                 if (retval < 0) {
                         DBG(1, "add_hcd failed, %d\n", retval);
                         goto err2;
+
+                        if ((musb->xceiv->last_event == USB_EVENT_ID)
+                                                && musb->xceiv->set_vbus)
+                                otg_set_vbus(musb->xceiv, 1);
                 }
 
                 hcd->self.uses_pio_for_control = 1;
diff --git a/drivers/usb/musb/omap2430.c b/drivers/usb/musb/omap2430.c
index bb6e6cd330da..64cf2431c05e 100644
--- a/drivers/usb/musb/omap2430.c
+++ b/drivers/usb/musb/omap2430.c
@@ -328,16 +328,56 @@ static int omap2430_musb_init(struct musb *musb)
         if (status)
                 DBG(1, "notification register failed\n");
 
-        /* check whether cable is already connected */
-        if (musb->xceiv->state ==OTG_STATE_B_IDLE)
-                musb_otg_notifications(&musb->nb, 1,
-                                        musb->xceiv->gadget);
-
         setup_timer(&musb_idle_timer, musb_do_idle, (unsigned long) musb);
 
         return 0;
 }
 
+static void omap2430_musb_enable(struct musb *musb)
+{
+        u8              devctl;
+        unsigned long timeout = jiffies + msecs_to_jiffies(1000);
+        struct device *dev = musb->controller;
+        struct musb_hdrc_platform_data *pdata = dev->platform_data;
+        struct omap_musb_board_data *data = pdata->board_data;
+
+        switch (musb->xceiv->last_event) {
+
+        case USB_EVENT_ID:
+                otg_init(musb->xceiv);
+                if (data->interface_type == MUSB_INTERFACE_UTMI) {
+                        devctl = musb_readb(musb->mregs, MUSB_DEVCTL);
+                        /* start the session */
+                        devctl |= MUSB_DEVCTL_SESSION;
+                        musb_writeb(musb->mregs, MUSB_DEVCTL, devctl);
+                        while (musb_readb(musb->mregs, MUSB_DEVCTL) &
+                                                MUSB_DEVCTL_BDEVICE) {
+                                cpu_relax();
+
+                                if (time_after(jiffies, timeout)) {
+                                        dev_err(musb->controller,
+                                        "configured as A device timeout");
+                                        break;
+                                }
+                        }
+                }
+                break;
+
+        case USB_EVENT_VBUS:
+                otg_init(musb->xceiv);
+                break;
+
+        default:
+                break;
+        }
+}
+
+static void omap2430_musb_disable(struct musb *musb)
+{
+        if (musb->xceiv->last_event)
+                otg_shutdown(musb->xceiv);
+}
+
 static int omap2430_musb_exit(struct musb *musb)
 {
 
@@ -355,6 +395,9 @@ static const struct musb_platform_ops omap2430_ops = {
         .try_idle       = omap2430_musb_try_idle,
 
         .set_vbus       = omap2430_musb_set_vbus,
+
+        .enable         = omap2430_musb_enable,
+        .disable        = omap2430_musb_disable,
 };
 
 static u64 omap2430_dmamask = DMA_BIT_MASK(32);