1 files changed, 36 insertions, 2 deletions

diff --git a/drivers/usb/host/xhci-hub.c b/drivers/usb/host/xhci-hub.c
index a686cf4905bb..68914429482f 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;
                         }
@@ -957,6 +984,7 @@ int xhci_hub_status_data(struct usb_hcd *hcd, char *buf)
         int max_ports;
         __le32 __iomem **port_array;
         struct xhci_bus_state *bus_state;
+        bool reset_change = false;
 
         max_ports = xhci_get_ports(hcd, &port_array);
         bus_state = &xhci->bus_state[hcd_index(hcd)];
@@ -988,6 +1016,12 @@ int xhci_hub_status_data(struct usb_hcd *hcd, char *buf)
                         buf[(i + 1) / 8] |= 1 << (i + 1) % 8;
                         status = 1;
                 }
+                if ((temp & PORT_RC))
+                        reset_change = true;
+        }
+        if (!status && !reset_change) {
+                xhci_dbg(xhci, "%s: stopping port polling.\n", __func__);
+                clear_bit(HCD_FLAG_POLL_RH, &hcd->flags);
         }
         spin_unlock_irqrestore(&xhci->lock, flags);
         return status ? retval : 0;