2 files changed, 23 insertions, 1 deletions

diff --git a/drivers/usb/early/ehci-dbgp.c b/drivers/usb/early/ehci-dbgp.c
index b88cb65b64e0..f0a41c647bef 100644
--- a/drivers/usb/early/ehci-dbgp.c
+++ b/drivers/usb/early/ehci-dbgp.c
@@ -478,10 +478,13 @@ int dbgp_external_startup(void)
         int devnum;
         struct usb_debug_descriptor dbgp_desc;
         int ret;
-        u32 ctrl, portsc;
+        u32 ctrl, portsc, cmd;
         int dbg_port = dbgp_phys_port;
         int tries = 3;
+        int reset_port_tries = 1;
+        int try_hard_once = 1;
 
+try_port_reset_again:
         ret = dbgp_ehci_startup();
         if (ret)
                 return ret;
@@ -490,6 +493,24 @@ int dbgp_external_startup(void)
         ret = ehci_wait_for_port(dbg_port);
         if (ret < 0) {
                 portsc = readl(&ehci_regs->port_status[dbg_port - 1]);
+                if (!(portsc & PORT_CONNECT) && try_hard_once) {
+                        /* Last ditch effort to try to force enable
+                         * the debug device by using the packet test
+                         * ehci command to try and wake it up. */
+                        try_hard_once = 0;
+                        cmd = readl(&ehci_regs->command);
+                        cmd &= ~CMD_RUN;
+                        writel(cmd, &ehci_regs->command);
+                        portsc = readl(&ehci_regs->port_status[dbg_port - 1]);
+                        portsc |= PORT_TEST_PKT;
+                        writel(portsc, &ehci_regs->port_status[dbg_port - 1]);
+                        dbgp_ehci_status("Trying to force debug port online");
+                        mdelay(50);
+                        dbgp_ehci_controller_reset();
+                        goto try_port_reset_again;
+                } else if (reset_port_tries--) {
+                        goto try_port_reset_again;
+                }
                 dbgp_printk("No device found in debug port\n");
                 return -EIO;
         }
diff --git a/include/linux/usb/ehci_def.h b/include/linux/usb/ehci_def.h
index 1909d924f816..af4b86f3aca3 100644
--- a/include/linux/usb/ehci_def.h
+++ b/include/linux/usb/ehci_def.h
@@ -105,6 +105,7 @@ struct ehci_regs {
 #define PORT_WKDISC_E   (1<<21)         /* wake on disconnect (enable) */
 #define PORT_WKCONN_E   (1<<20)         /* wake on connect (enable) */
 /* 19:16 for port testing */
+#define PORT_TEST_PKT   (0x4<<16)       /* Port Test Control - packet test */
 #define PORT_LED_OFF    (0<<14)
 #define PORT_LED_AMBER  (1<<14)
 #define PORT_LED_GREEN  (2<<14)