1 files changed, 37 insertions, 4 deletions

diff --git a/drivers/usb/renesas_usbhs/mod_gadget.c b/drivers/usb/renesas_usbhs/mod_gadget.c
index 2d17c10a0428..8697e6efcabf 100644
--- a/drivers/usb/renesas_usbhs/mod_gadget.c
+++ b/drivers/usb/renesas_usbhs/mod_gadget.c
@@ -56,6 +56,7 @@ struct usbhsg_gpriv {
 #define USBHSG_STATUS_REGISTERD         (1 << 1)
 #define USBHSG_STATUS_WEDGE             (1 << 2)
 #define USBHSG_STATUS_SELF_POWERED      (1 << 3)
+#define USBHSG_STATUS_SOFT_CONNECT      (1 << 4)
 };
 
 struct usbhsg_recip_handle {
@@ -484,6 +485,9 @@ static int usbhsg_irq_ctrl_stage(struct usbhs_priv *priv,
         case NODATA_STATUS_STAGE:
                 pipe->handler = &usbhs_ctrl_stage_end_handler;
                 break;
+        case READ_STATUS_STAGE:
+        case WRITE_STATUS_STAGE:
+                usbhs_dcp_control_transfer_done(pipe);
         default:
                 return ret;
         }
@@ -602,6 +606,9 @@ static int usbhsg_ep_disable(struct usb_ep *ep)
         struct usbhsg_uep *uep = usbhsg_ep_to_uep(ep);
         struct usbhs_pipe *pipe = usbhsg_uep_to_pipe(uep);
 
+        if (!pipe)
+                return -EINVAL;
+
         usbhsg_pipe_disable(uep);
         usbhs_pipe_free(pipe);
 
@@ -723,6 +730,25 @@ static struct usb_ep_ops usbhsg_ep_ops = {
 };
 
 /*
+ *              pullup control
+ */
+static int usbhsg_can_pullup(struct usbhs_priv *priv)
+{
+        struct usbhsg_gpriv *gpriv = usbhsg_priv_to_gpriv(priv);
+
+        return gpriv->driver &&
+               usbhsg_status_has(gpriv, USBHSG_STATUS_SOFT_CONNECT);
+}
+
+static void usbhsg_update_pullup(struct usbhs_priv *priv)
+{
+        if (usbhsg_can_pullup(priv))
+                usbhs_sys_function_pullup(priv, 1);
+        else
+                usbhs_sys_function_pullup(priv, 0);
+}
+
+/*
  *              usb module start/end
  */
 static int usbhsg_try_start(struct usbhs_priv *priv, u32 status)
@@ -756,9 +782,9 @@ static int usbhsg_try_start(struct usbhs_priv *priv, u32 status)
         /*
          * pipe initialize and enable DCP
          */
+        usbhs_fifo_init(priv);
         usbhs_pipe_init(priv,
                         usbhsg_dma_map_ctrl);
-        usbhs_fifo_init(priv);
 
         /* dcp init instead of usbhsg_ep_enable() */
         dcp->pipe               = usbhs_dcp_malloc(priv);
@@ -772,6 +798,7 @@ static int usbhsg_try_start(struct usbhs_priv *priv, u32 status)
          * - usb module
          */
         usbhs_sys_function_ctrl(priv, 1);
+        usbhsg_update_pullup(priv);
 
         /*
          * enable irq callback
@@ -851,8 +878,7 @@ static int usbhsg_gadget_start(struct usb_gadget *gadget,
         return usbhsg_try_start(priv, USBHSG_STATUS_REGISTERD);
 }
 
-static int usbhsg_gadget_stop(struct usb_gadget *gadget,
-                struct usb_gadget_driver *driver)
+static int usbhsg_gadget_stop(struct usb_gadget *gadget)
 {
         struct usbhsg_gpriv *gpriv = usbhsg_gadget_to_gpriv(gadget);
         struct usbhs_priv *priv = usbhsg_gpriv_to_priv(gpriv);
@@ -878,8 +904,15 @@ static int usbhsg_pullup(struct usb_gadget *gadget, int is_on)
 {
         struct usbhsg_gpriv *gpriv = usbhsg_gadget_to_gpriv(gadget);
         struct usbhs_priv *priv = usbhsg_gpriv_to_priv(gpriv);
+        unsigned long flags;
 
-        usbhs_sys_function_pullup(priv, is_on);
+        usbhs_lock(priv, flags);
+        if (is_on)
+                usbhsg_status_set(gpriv, USBHSG_STATUS_SOFT_CONNECT);
+        else
+                usbhsg_status_clr(gpriv, USBHSG_STATUS_SOFT_CONNECT);
+        usbhsg_update_pullup(priv);
+        usbhs_unlock(priv, flags);
 
         return 0;
 }