3 files changed, 27 insertions, 13 deletions

diff --git a/drivers/usb/musb/musb_core.c b/drivers/usb/musb/musb_core.c
index 8bd6bb1b04e..93dd23a7eb6 100644
--- a/drivers/usb/musb/musb_core.c
+++ b/drivers/usb/musb/musb_core.c
@@ -587,28 +587,23 @@ static irqreturn_t musb_stage0_irq(struct musb *musb, u8 int_usb,
                 if (devctl & MUSB_DEVCTL_LSDEV)
                         musb->port1_status |= USB_PORT_STAT_LOW_SPEED;
 
-                if (hcd->status_urb)
-                        usb_hcd_poll_rh_status(hcd);
-                else
-                        usb_hcd_resume_root_hub(hcd);
-
-                MUSB_HST_MODE(musb);
-
                 /* indicate new connection to OTG machine */
                 switch (musb->xceiv->state) {
                 case OTG_STATE_B_PERIPHERAL:
                         if (int_usb & MUSB_INTR_SUSPEND) {
                                 DBG(1, "HNP: SUSPEND+CONNECT, now b_host\n");
-                                musb->xceiv->state = OTG_STATE_B_HOST;
-                                hcd->self.is_b_host = 1;
                                 int_usb &= ~MUSB_INTR_SUSPEND;
+                                goto b_host;
                         } else
                                 DBG(1, "CONNECT as b_peripheral???\n");
                         break;
                 case OTG_STATE_B_WAIT_ACON:
-                        DBG(1, "HNP: Waiting to switch to b_host state\n");
+                        DBG(1, "HNP: CONNECT, now b_host\n");
+b_host:
                         musb->xceiv->state = OTG_STATE_B_HOST;
                         hcd->self.is_b_host = 1;
+                        musb->ignore_disconnect = 0;
+                        del_timer(&musb->otg_timer);
                         break;
                 default:
                         if ((devctl & MUSB_DEVCTL_VBUS)
@@ -618,6 +613,14 @@ static irqreturn_t musb_stage0_irq(struct musb *musb, u8 int_usb,
                         }
                         break;
                 }
+
+                /* poke the root hub */
+                MUSB_HST_MODE(musb);
+                if (hcd->status_urb)
+                        usb_hcd_poll_rh_status(hcd);
+                else
+                        usb_hcd_resume_root_hub(hcd);
+
                 DBG(1, "CONNECT (%s) devctl %02x\n",
                                 otg_state_string(musb), devctl);
         }
@@ -662,7 +665,9 @@ static irqreturn_t musb_stage0_irq(struct musb *musb, u8 int_usb,
                                         + msecs_to_jiffies(TA_WAIT_BCON(musb)));
                                 break;
                         case OTG_STATE_A_PERIPHERAL:
-                                musb_hnp_stop(musb);
+                                musb->ignore_disconnect = 0;
+                                del_timer(&musb->otg_timer);
+                                musb_g_reset(musb);
                                 break;
                         case OTG_STATE_B_WAIT_ACON:
                                 DBG(1, "HNP: RESET (%s), to b_peripheral\n",
diff --git a/drivers/usb/musb/musb_gadget.c b/drivers/usb/musb/musb_gadget.c
index 8dfad1189da..3c4da75fbc7 100644
--- a/drivers/usb/musb/musb_gadget.c
+++ b/drivers/usb/musb/musb_gadget.c
@@ -1964,7 +1964,7 @@ void musb_g_disconnect(struct musb *musb)
                 musb->xceiv->state = OTG_STATE_A_IDLE;
                 break;
         case OTG_STATE_A_PERIPHERAL:
-                musb->xceiv->state = OTG_STATE_A_WAIT_VFALL;
+                musb->xceiv->state = OTG_STATE_A_WAIT_BCON;
                 break;
         case OTG_STATE_B_WAIT_ACON:
         case OTG_STATE_B_HOST:
diff --git a/drivers/usb/musb/musb_virthub.c b/drivers/usb/musb/musb_virthub.c
index d7e1bc44f00..c85a82a41d5 100644
--- a/drivers/usb/musb/musb_virthub.c
+++ b/drivers/usb/musb/musb_virthub.c
@@ -187,8 +187,17 @@ void musb_root_disconnect(struct musb *musb)
         musb->is_active = 0;
 
         switch (musb->xceiv->state) {
-        case OTG_STATE_A_HOST:
         case OTG_STATE_A_SUSPEND:
+#ifdef  CONFIG_USB_MUSB_OTG
+                if (is_otg_enabled(musb)
+                                && musb->xceiv->host->b_hnp_enable) {
+                        musb->xceiv->state = OTG_STATE_A_PERIPHERAL;
+                        musb->g.is_a_peripheral = 1;
+                        break;
+                }
+#endif
+                /* FALLTHROUGH */
+        case OTG_STATE_A_HOST:
                 musb->xceiv->state = OTG_STATE_A_WAIT_BCON;
                 musb->is_active = 0;
                 break;