1 files changed, 43 insertions, 0 deletions

diff --git a/drivers/usb/host/ohci-hub.c b/drivers/usb/host/ohci-hub.c
index c638e6b33c43..28d6d775eb5f 100644
--- a/drivers/usb/host/ohci-hub.c
+++ b/drivers/usb/host/ohci-hub.c
@@ -326,6 +326,49 @@ static int ohci_bus_resume (struct usb_hcd *hcd)
         return rc;
 }
 
+/* Carry out the final steps of resuming the controller device */
+static void ohci_finish_controller_resume(struct usb_hcd *hcd)
+{
+        struct ohci_hcd         *ohci = hcd_to_ohci(hcd);
+        int                     port;
+        bool                    need_reinit = false;
+
+        /* See if the controller is already running or has been reset */
+        ohci->hc_control = ohci_readl(ohci, &ohci->regs->control);
+        if (ohci->hc_control & (OHCI_CTRL_IR | OHCI_SCHED_ENABLES)) {
+                need_reinit = true;
+        } else {
+                switch (ohci->hc_control & OHCI_CTRL_HCFS) {
+                case OHCI_USB_OPER:
+                case OHCI_USB_RESET:
+                        need_reinit = true;
+                }
+        }
+
+        /* If needed, reinitialize and suspend the root hub */
+        if (need_reinit) {
+                spin_lock_irq(&ohci->lock);
+                hcd->state = HC_STATE_RESUMING;
+                ohci_rh_resume(ohci);
+                hcd->state = HC_STATE_QUIESCING;
+                ohci_rh_suspend(ohci, 0);
+                hcd->state = HC_STATE_SUSPENDED;
+                spin_unlock_irq(&ohci->lock);
+        }
+
+        /* Normally just turn on port power and enable interrupts */
+        else {
+                ohci_dbg(ohci, "powerup ports\n");
+                for (port = 0; port < ohci->num_ports; port++)
+                        ohci_writel(ohci, RH_PS_PPS,
+                                        &ohci->regs->roothub.portstatus[port]);
+
+                ohci_writel(ohci, OHCI_INTR_MIE, &ohci->regs->intrenable);
+                ohci_readl(ohci, &ohci->regs->intrenable);
+                msleep(20);
+        }
+}
+
 /* Carry out polling-, autostop-, and autoresume-related state changes */
 static int ohci_root_hub_state_changes(struct ohci_hcd *ohci, int changed,
                 int any_connected)