3 files changed, 33 insertions, 47 deletions

diff --git a/drivers/usb/host/xhci-hub.c b/drivers/usb/host/xhci-hub.c
index 7a62b023b4d1..ef03c18a0128 100644
--- a/drivers/usb/host/xhci-hub.c
+++ b/drivers/usb/host/xhci-hub.c
@@ -392,13 +392,25 @@ static int xhci_get_ports(struct usb_hcd *hcd, __le32 __iomem ***port_array)
 	return max_ports;
 }
 
+void xhci_set_link_state(struct xhci_hcd *xhci, __le32 __iomem **port_array,
+				int port_id, u32 link_state)
+{
+	u32 temp;
+
+	temp = xhci_readl(xhci, port_array[port_id]);
+	temp = xhci_port_state_to_neutral(temp);
+	temp &= ~PORT_PLS_MASK;
+	temp |= PORT_LINK_STROBE | link_state;
+	xhci_writel(xhci, temp, port_array[port_id]);
+}
+
 int xhci_hub_control(struct usb_hcd *hcd, u16 typeReq, u16 wValue,
 		u16 wIndex, char *buf, u16 wLength)
 {
 	struct xhci_hcd	*xhci = hcd_to_xhci(hcd);
 	int max_ports;
 	unsigned long flags;
-	u32 temp, temp1, status;
+	u32 temp, status;
 	int retval = 0;
 	__le32 __iomem **port_array;
 	int slot_id;
@@ -472,11 +484,8 @@ int xhci_hub_control(struct usb_hcd *hcd, u16 typeReq, u16 wValue,
 				xhci_dbg(xhci, "Resume USB2 port %d\n",
 					wIndex + 1);
 				bus_state->resume_done[wIndex] = 0;
-				temp1 = xhci_port_state_to_neutral(temp);
-				temp1 &= ~PORT_PLS_MASK;
-				temp1 |= PORT_LINK_STROBE | XDEV_U0;
-				xhci_writel(xhci, temp1, port_array[wIndex]);
-
+				xhci_set_link_state(xhci, port_array, wIndex,
+							XDEV_U0);
 				xhci_dbg(xhci, "set port %d resume\n",
 					wIndex + 1);
 				slot_id = xhci_find_slot_id_by_port(hcd, xhci,
@@ -573,10 +582,7 @@ int xhci_hub_control(struct usb_hcd *hcd, u16 typeReq, u16 wValue,
 			xhci_stop_device(xhci, slot_id, 1);
 			spin_lock_irqsave(&xhci->lock, flags);
 
-			temp = xhci_port_state_to_neutral(temp);
-			temp &= ~PORT_PLS_MASK;
-			temp |= PORT_LINK_STROBE | XDEV_U3;
-			xhci_writel(xhci, temp, port_array[wIndex]);
+			xhci_set_link_state(xhci, port_array, wIndex, XDEV_U3);
 
 			spin_unlock_irqrestore(&xhci->lock, flags);
 			msleep(10); /* wait device to enter */
@@ -610,10 +616,8 @@ int xhci_hub_control(struct usb_hcd *hcd, u16 typeReq, u16 wValue,
 				}
 			}
 
-			temp = xhci_port_state_to_neutral(temp);
-			temp &= ~PORT_PLS_MASK;
-			temp |= PORT_LINK_STROBE | link_state;
-			xhci_writel(xhci, temp, port_array[wIndex]);
+			xhci_set_link_state(xhci, port_array, wIndex,
+						link_state);
 
 			spin_unlock_irqrestore(&xhci->lock, flags);
 			msleep(20); /* wait device to enter */
@@ -677,24 +681,13 @@ int xhci_hub_control(struct usb_hcd *hcd, u16 typeReq, u16 wValue,
 				if ((temp & PORT_PE) == 0)
 					goto error;
 
-				temp = xhci_port_state_to_neutral(temp);
-				temp &= ~PORT_PLS_MASK;
-				temp |= PORT_LINK_STROBE | XDEV_RESUME;
-				xhci_writel(xhci, temp,
-						port_array[wIndex]);
-
-				spin_unlock_irqrestore(&xhci->lock,
-						       flags);
+				xhci_set_link_state(xhci, port_array, wIndex,
+							XDEV_RESUME);
+				spin_unlock_irqrestore(&xhci->lock, flags);
 				msleep(20);
 				spin_lock_irqsave(&xhci->lock, flags);
-
-				temp = xhci_readl(xhci,
-						port_array[wIndex]);
-				temp = xhci_port_state_to_neutral(temp);
-				temp &= ~PORT_PLS_MASK;
-				temp |= PORT_LINK_STROBE | XDEV_U0;
-				xhci_writel(xhci, temp,
-						port_array[wIndex]);
+				xhci_set_link_state(xhci, port_array, wIndex,
+							XDEV_U0);
 			}
 			bus_state->port_c_suspend |= 1 << wIndex;
 
@@ -910,25 +903,18 @@ int xhci_bus_resume(struct usb_hcd *hcd)
 		if (test_bit(port_index, &bus_state->bus_suspended) &&
 		    (temp & PORT_PLS_MASK)) {
 			if (DEV_SUPERSPEED(temp)) {
-				temp = xhci_port_state_to_neutral(temp);
-				temp &= ~PORT_PLS_MASK;
-				temp |= PORT_LINK_STROBE | XDEV_U0;
-				xhci_writel(xhci, temp, port_array[port_index]);
+				xhci_set_link_state(xhci, port_array,
+							port_index, XDEV_U0);
 			} else {
-				temp = xhci_port_state_to_neutral(temp);
-				temp &= ~PORT_PLS_MASK;
-				temp |= PORT_LINK_STROBE | XDEV_RESUME;
-				xhci_writel(xhci, temp, port_array[port_index]);
+				xhci_set_link_state(xhci, port_array,
+						port_index, XDEV_RESUME);
 
 				spin_unlock_irqrestore(&xhci->lock, flags);
 				msleep(20);
 				spin_lock_irqsave(&xhci->lock, flags);
 
-				temp = xhci_readl(xhci, port_array[port_index]);
-				temp = xhci_port_state_to_neutral(temp);
-				temp &= ~PORT_PLS_MASK;
-				temp |= PORT_LINK_STROBE | XDEV_U0;
-				xhci_writel(xhci, temp, port_array[port_index]);
+				xhci_set_link_state(xhci, port_array,
+							port_index, XDEV_U0);
 			}
 			/* wait for the port to enter U0 and report port link
 			 * state change.
diff --git a/drivers/usb/host/xhci-ring.c b/drivers/usb/host/xhci-ring.c
index d14f3fbe5762..0ef4f20c7f41 100644
--- a/drivers/usb/host/xhci-ring.c
+++ b/drivers/usb/host/xhci-ring.c
@@ -1329,10 +1329,8 @@ static void handle_port_status(struct xhci_hcd *xhci,
 
 		if (DEV_SUPERSPEED(temp)) {
 			xhci_dbg(xhci, "resume SS port %d\n", port_id);
-			temp = xhci_port_state_to_neutral(temp);
-			temp &= ~PORT_PLS_MASK;
-			temp |= PORT_LINK_STROBE | XDEV_U0;
-			xhci_writel(xhci, temp, port_array[faked_port_index]);
+			xhci_set_link_state(xhci, port_array, faked_port_index,
+						XDEV_U0);
 			slot_id = xhci_find_slot_id_by_port(hcd, xhci,
 					faked_port_index);
 			if (!slot_id) {
diff --git a/drivers/usb/host/xhci.h b/drivers/usb/host/xhci.h
index 2882074eb2c8..92eba69b9239 100644
--- a/drivers/usb/host/xhci.h
+++ b/drivers/usb/host/xhci.h
@@ -1717,6 +1717,8 @@ void xhci_ring_ep_doorbell(struct xhci_hcd *xhci, unsigned int slot_id,
 		unsigned int ep_index, unsigned int stream_id);
 
 /* xHCI roothub code */
+void xhci_set_link_state(struct xhci_hcd *xhci, __le32 __iomem **port_array,
+				int port_id, u32 link_state);
 int xhci_hub_control(struct usb_hcd *hcd, u16 typeReq, u16 wValue, u16 wIndex,
 		char *buf, u16 wLength);
 int xhci_hub_status_data(struct usb_hcd *hcd, char *buf);