1 files changed, 32 insertions, 13 deletions

diff --git a/drivers/usb/host/ehci-hub.c b/drivers/usb/host/ehci-hub.c
index 5728829cf6ef..118edb7bdca2 100644
--- a/drivers/usb/host/ehci-hub.c
+++ b/drivers/usb/host/ehci-hub.c
@@ -69,10 +69,8 @@ static void ehci_handover_companion_ports(struct ehci_hcd *ehci)
                 if (test_bit(port, &ehci->owned_ports)) {
                         reg = &ehci->regs->port_status[port];
                         status = ehci_readl(ehci, reg) & ~PORT_RWC_BITS;
-                        if (!(status & PORT_POWER)) {
-                                status |= PORT_POWER;
-                                ehci_writel(ehci, status, reg);
-                        }
+                        if (!(status & PORT_POWER))
+                                ehci_port_power(ehci, port, true);
                 }
         }
 
@@ -952,9 +950,11 @@ int ehci_hub_control(
                         clear_bit(wIndex, &ehci->port_c_suspend);
                         break;
                 case USB_PORT_FEAT_POWER:
-                        if (HCS_PPC (ehci->hcs_params))
-                                ehci_writel(ehci, temp & ~PORT_POWER,
-                                                status_reg);
+                        if (HCS_PPC(ehci->hcs_params)) {
+                                spin_unlock_irqrestore(&ehci->lock, flags);
+                                ehci_port_power(ehci, wIndex, false);
+                                spin_lock_irqsave(&ehci->lock, flags);
+                        }
                         break;
                 case USB_PORT_FEAT_C_CONNECTION:
                         ehci_writel(ehci, temp | PORT_CSC, status_reg);
@@ -1004,9 +1004,9 @@ int ehci_hub_control(
                          */
                         if (((temp & PORT_OC) || (ehci->need_oc_pp_cycle))
                                         && HCS_PPC(ehci->hcs_params)) {
-                                ehci_writel(ehci,
-                                        temp & ~(PORT_RWC_BITS | PORT_POWER),
-                                        status_reg);
+                                spin_unlock_irqrestore(&ehci->lock, flags);
+                                ehci_port_power(ehci, wIndex, false);
+                                spin_lock_irqsave(&ehci->lock, flags);
                                 temp = ehci_readl(ehci, status_reg);
                         }
                 }
@@ -1187,9 +1187,11 @@ int ehci_hub_control(
                         set_bit(wIndex, &ehci->suspended_ports);
                         break;
                 case USB_PORT_FEAT_POWER:
-                        if (HCS_PPC (ehci->hcs_params))
-                                ehci_writel(ehci, temp | PORT_POWER,
-                                                status_reg);
+                        if (HCS_PPC(ehci->hcs_params)) {
+                                spin_unlock_irqrestore(&ehci->lock, flags);
+                                ehci_port_power(ehci, wIndex, true);
+                                spin_lock_irqsave(&ehci->lock, flags);
+                        }
                         break;
                 case USB_PORT_FEAT_RESET:
                         if (temp & (PORT_SUSPEND|PORT_RESUME))
@@ -1297,3 +1299,20 @@ static int ehci_port_handed_over(struct usb_hcd *hcd, int portnum)
         reg = &ehci->regs->port_status[portnum - 1];
         return ehci_readl(ehci, reg) & PORT_OWNER;
 }
+
+static int ehci_port_power(struct ehci_hcd *ehci, int portnum, bool enable)
+{
+        struct usb_hcd *hcd = ehci_to_hcd(ehci);
+        u32 __iomem *status_reg = &ehci->regs->port_status[portnum];
+        u32 temp = ehci_readl(ehci, status_reg) & ~PORT_RWC_BITS;
+
+        if (enable)
+                ehci_writel(ehci, temp | PORT_POWER, status_reg);
+        else
+                ehci_writel(ehci, temp & ~PORT_POWER, status_reg);
+
+        if (hcd->driver->port_power)
+                hcd->driver->port_power(hcd, portnum, enable);
+
+        return 0;
+}