2 files changed, 90 insertions, 4 deletions

diff --git a/drivers/usb/core/hub.c b/drivers/usb/core/hub.c
index 3bc50fc110eb..968ec37a0f02 100644
--- a/drivers/usb/core/hub.c
+++ b/drivers/usb/core/hub.c
@@ -877,6 +877,60 @@ static int hub_hub_status(struct usb_hub *hub,
         return ret;
 }
 
+static int hub_set_port_link_state(struct usb_hub *hub, int port1,
+                        unsigned int link_status)
+{
+        return set_port_feature(hub->hdev,
+                        port1 | (link_status << 3),
+                        USB_PORT_FEAT_LINK_STATE);
+}
+
+/*
+ * If USB 3.0 ports are placed into the Disabled state, they will no longer
+ * detect any device connects or disconnects.  This is generally not what the
+ * USB core wants, since it expects a disabled port to produce a port status
+ * change event when a new device connects.
+ *
+ * Instead, set the link state to Disabled, wait for the link to settle into
+ * that state, clear any change bits, and then put the port into the RxDetect
+ * state.
+ */
+static int hub_usb3_port_disable(struct usb_hub *hub, int port1)
+{
+        int ret;
+        int total_time;
+        u16 portchange, portstatus;
+
+        if (!hub_is_superspeed(hub->hdev))
+                return -EINVAL;
+
+        ret = hub_set_port_link_state(hub, port1, USB_SS_PORT_LS_SS_DISABLED);
+        if (ret) {
+                dev_err(hub->intfdev, "cannot disable port %d (err = %d)\n",
+                                port1, ret);
+                return ret;
+        }
+
+        /* Wait for the link to enter the disabled state. */
+        for (total_time = 0; ; total_time += HUB_DEBOUNCE_STEP) {
+                ret = hub_port_status(hub, port1, &portstatus, &portchange);
+                if (ret < 0)
+                        return ret;
+
+                if ((portstatus & USB_PORT_STAT_LINK_STATE) ==
+                                USB_SS_PORT_LS_SS_DISABLED)
+                        break;
+                if (total_time >= HUB_DEBOUNCE_TIMEOUT)
+                        break;
+                msleep(HUB_DEBOUNCE_STEP);
+        }
+        if (total_time >= HUB_DEBOUNCE_TIMEOUT)
+                dev_warn(hub->intfdev, "Could not disable port %d after %d ms\n",
+                                port1, total_time);
+
+        return hub_set_port_link_state(hub, port1, USB_SS_PORT_LS_RX_DETECT);
+}
+
 static int hub_port_disable(struct usb_hub *hub, int port1, int set_state)
 {
         struct usb_device *hdev = hub->hdev;
@@ -885,8 +939,13 @@ static int hub_port_disable(struct usb_hub *hub, int port1, int set_state)
         if (hub->ports[port1 - 1]->child && set_state)
                 usb_set_device_state(hub->ports[port1 - 1]->child,
                                 USB_STATE_NOTATTACHED);
-        if (!hub->error && !hub_is_superspeed(hub->hdev))
-                ret = clear_port_feature(hdev, port1, USB_PORT_FEAT_ENABLE);
+        if (!hub->error) {
+                if (hub_is_superspeed(hub->hdev))
+                        ret = hub_usb3_port_disable(hub, port1);
+                else
+                        ret = clear_port_feature(hdev, port1,
+                                        USB_PORT_FEAT_ENABLE);
+        }
         if (ret)
                 dev_err(hub->intfdev, "cannot disable port %d (err = %d)\n",
                                 port1, ret);
diff --git a/drivers/usb/host/xhci-hub.c b/drivers/usb/host/xhci-hub.c
index a686cf4905bb..a7dd4f311e14 100644
--- a/drivers/usb/host/xhci-hub.c
+++ b/drivers/usb/host/xhci-hub.c
@@ -761,12 +761,39 @@ int xhci_hub_control(struct usb_hcd *hcd, u16 typeReq, u16 wValue,
                         break;
                 case USB_PORT_FEAT_LINK_STATE:
                         temp = xhci_readl(xhci, port_array[wIndex]);
+
+                        /* Disable port */
+                        if (link_state == USB_SS_PORT_LS_SS_DISABLED) {
+                                xhci_dbg(xhci, "Disable port %d\n", wIndex);
+                                temp = xhci_port_state_to_neutral(temp);
+                                /*
+                                 * Clear all change bits, so that we get a new
+                                 * connection event.
+                                 */
+                                temp |= PORT_CSC | PORT_PEC | PORT_WRC |
+                                        PORT_OCC | PORT_RC | PORT_PLC |
+                                        PORT_CEC;
+                                xhci_writel(xhci, temp | PORT_PE,
+                                        port_array[wIndex]);
+                                temp = xhci_readl(xhci, port_array[wIndex]);
+                                break;
+                        }
+
+                        /* Put link in RxDetect (enable port) */
+                        if (link_state == USB_SS_PORT_LS_RX_DETECT) {
+                                xhci_dbg(xhci, "Enable port %d\n", wIndex);
+                                xhci_set_link_state(xhci, port_array, wIndex,
+                                                link_state);
+                                temp = xhci_readl(xhci, port_array[wIndex]);
+                                break;
+                        }
+
                         /* Software should not attempt to set
-                         * port link state above '5' (Rx.Detect) and the port
+                         * port link state above '3' (U3) and the port
                          * must be enabled.
                          */
                         if ((temp & PORT_PE) == 0 ||
-                                (link_state > USB_SS_PORT_LS_RX_DETECT)) {
+                                (link_state > USB_SS_PORT_LS_U3)) {
                                 xhci_warn(xhci, "Cannot set link state.\n");
                                 goto error;
                         }