1 files changed, 34 insertions, 20 deletions

diff --git a/drivers/usb/musb/sunxi.c b/drivers/usb/musb/sunxi.c
index fdab4232cfbf..76500515dd8b 100644
--- a/drivers/usb/musb/sunxi.c
+++ b/drivers/usb/musb/sunxi.c
@@ -80,7 +80,8 @@ static struct musb *sunxi_musb;
 
 struct sunxi_glue {
         struct device           *dev;
-        struct platform_device  *musb;
+        struct musb             *musb;
+        struct platform_device  *musb_pdev;
         struct clk              *clk;
         struct reset_control    *rst;
         struct phy              *phy;
@@ -102,7 +103,7 @@ static void sunxi_musb_work(struct work_struct *work)
                 return;
 
         if (test_and_clear_bit(SUNXI_MUSB_FL_HOSTMODE_PEND, &glue->flags)) {
-                struct musb *musb = platform_get_drvdata(glue->musb);
+                struct musb *musb = glue->musb;
                 unsigned long flags;
                 u8 devctl;
 
@@ -112,7 +113,7 @@ static void sunxi_musb_work(struct work_struct *work)
                 if (test_bit(SUNXI_MUSB_FL_HOSTMODE, &glue->flags)) {
                         set_bit(SUNXI_MUSB_FL_VBUS_ON, &glue->flags);
                         musb->xceiv->otg->default_a = 1;
-                        musb->xceiv->otg->state = OTG_STATE_A_IDLE;
+                        musb->xceiv->otg->state = OTG_STATE_A_WAIT_VRISE;
                         MUSB_HST_MODE(musb);
                         devctl |= MUSB_DEVCTL_SESSION;
                 } else {
@@ -145,10 +146,12 @@ static void sunxi_musb_set_vbus(struct musb *musb, int is_on)
 {
         struct sunxi_glue *glue = dev_get_drvdata(musb->controller->parent);
 
-        if (is_on)
+        if (is_on) {
                 set_bit(SUNXI_MUSB_FL_VBUS_ON, &glue->flags);
-        else
+                musb->xceiv->otg->state = OTG_STATE_A_WAIT_VRISE;
+        } else {
                 clear_bit(SUNXI_MUSB_FL_VBUS_ON, &glue->flags);
+        }
 
         schedule_work(&glue->work);
 }
@@ -264,15 +267,6 @@ static int sunxi_musb_init(struct musb *musb)
         if (ret)
                 goto error_unregister_notifier;
 
-        if (musb->port_mode == MUSB_PORT_MODE_HOST) {
-                ret = phy_power_on(glue->phy);
-                if (ret)
-                        goto error_phy_exit;
-                set_bit(SUNXI_MUSB_FL_PHY_ON, &glue->flags);
-                /* Stop musb work from turning vbus off again */
-                set_bit(SUNXI_MUSB_FL_VBUS_ON, &glue->flags);
-        }
-
         musb->isr = sunxi_musb_interrupt;
 
         /* Stop the musb-core from doing runtime pm (not supported on sunxi) */
@@ -280,8 +274,6 @@ static int sunxi_musb_init(struct musb *musb)
 
         return 0;
 
-error_phy_exit:
-        phy_exit(glue->phy);
 error_unregister_notifier:
         if (musb->port_mode == MUSB_PORT_MODE_DUAL_ROLE)
                 extcon_unregister_notifier(glue->extcon, EXTCON_USB_HOST,
@@ -323,10 +315,31 @@ static int sunxi_musb_exit(struct musb *musb)
         return 0;
 }
 
+static int sunxi_set_mode(struct musb *musb, u8 mode)
+{
+        struct sunxi_glue *glue = dev_get_drvdata(musb->controller->parent);
+        int ret;
+
+        if (mode == MUSB_HOST) {
+                ret = phy_power_on(glue->phy);
+                if (ret)
+                        return ret;
+
+                set_bit(SUNXI_MUSB_FL_PHY_ON, &glue->flags);
+                /* Stop musb work from turning vbus off again */
+                set_bit(SUNXI_MUSB_FL_VBUS_ON, &glue->flags);
+                musb->xceiv->otg->state = OTG_STATE_A_WAIT_VRISE;
+        }
+
+        return 0;
+}
+
 static void sunxi_musb_enable(struct musb *musb)
 {
         struct sunxi_glue *glue = dev_get_drvdata(musb->controller->parent);
 
+        glue->musb = musb;
+
         /* musb_core does not call us in a balanced manner */
         if (test_and_set_bit(SUNXI_MUSB_FL_ENABLED, &glue->flags))
                 return;
@@ -569,6 +582,7 @@ static const struct musb_platform_ops sunxi_musb_ops = {
         .exit           = sunxi_musb_exit,
         .enable         = sunxi_musb_enable,
         .disable        = sunxi_musb_disable,
+        .set_mode       = sunxi_set_mode,
         .fifo_offset    = sunxi_musb_fifo_offset,
         .ep_offset      = sunxi_musb_ep_offset,
         .busctl_offset  = sunxi_musb_busctl_offset,
@@ -721,9 +735,9 @@ static int sunxi_musb_probe(struct platform_device *pdev)
         pinfo.data      = &pdata;
         pinfo.size_data = sizeof(pdata);
 
-        glue->musb = platform_device_register_full(&pinfo);
-        if (IS_ERR(glue->musb)) {
-                ret = PTR_ERR(glue->musb);
+        glue->musb_pdev = platform_device_register_full(&pinfo);
+        if (IS_ERR(glue->musb_pdev)) {
+                ret = PTR_ERR(glue->musb_pdev);
                 dev_err(&pdev->dev, "Error registering musb dev: %d\n", ret);
                 goto err_unregister_usb_phy;
         }
@@ -740,7 +754,7 @@ static int sunxi_musb_remove(struct platform_device *pdev)
         struct sunxi_glue *glue = platform_get_drvdata(pdev);
         struct platform_device *usb_phy = glue->usb_phy;
 
-        platform_device_unregister(glue->musb); /* Frees glue ! */
+        platform_device_unregister(glue->musb_pdev);
         usb_phy_generic_unregister(usb_phy);
 
         return 0;