USB: EHCI: Fix test mode sequence

The sequence to put port in test mode is not complete.
According EHCI specification all enabled ports must be
put in suspend.

Signed-off-by: Boris Todorov <boris.st.todorov@gmail.com>
Acked-by: Alan Stern <stern@rowland.harvard.edu>
Signed-off-by: Greg Kroah-Hartman <gregkh@suse.de>

diff --git a/drivers/usb/host/ehci-hub.c b/drivers/usb/host/ehci-hub.c
index bf2c8f65e1aefc6c4d89e9dfe15a9f4a804f090e..e051b30c1847804f6b94e188521a865e036108ea 100644 (file)
--- a/drivers/usb/host/ehci-hub.c
+++ b/drivers/usb/host/ehci-hub.c
@@ -1046,7 +1046,19 @@ static int ehci_hub_control (
                        if (!selector || selector > 5)
                                goto error;
                        ehci_quiesce(ehci);
+
+                       /* Put all enabled ports into suspend */
+                       while (ports--) {
+                               u32 __iomem *sreg =
+                                               &ehci->regs->port_status[ports];
+
+                               temp = ehci_readl(ehci, sreg) & ~PORT_RWC_BITS;
+                               if (temp & PORT_PE)
+                                       ehci_writel(ehci, temp | PORT_SUSPEND,
+                                                       sreg);
+                       }
                        ehci_halt(ehci);
+                       temp = ehci_readl(ehci, status_reg);
                        temp |= selector << 16;
                        ehci_writel(ehci, temp, status_reg);
                        break;