[U-Boot] [PATCH] usb: ehci: Fix test mode for connected ports

[Julius Werner]

The EHCI controller has some very specific requirements for the USB 2.0
port test modes, which were not closely followed in the initial test
mode commit. It demands that the host controller is completely shut down
(all ports suspended, Run/Stop bit unset) when activating test mode, and
will not work on an already enumerated port.

This patch fixes that by introducing a new ehci_shutdown() function that
closely follows the procedure listed in EHCI 4.14. Also, when we have
such a function anyway, we might as well also use it in
usb_lowlevel_stop() to make the normal host controller shutdown cleaner.

Signed-off-by: Julius Werner <jwerner at chromium.org>
---
 drivers/usb/host/ehci-hcd.c | 31 +++++++++++++++++++++++++++++++
 1 file changed, 31 insertions(+)

diff --git a/drivers/usb/host/ehci-hcd.c b/drivers/usb/host/ehci-hcd.c
index fdad739..3143021 100644
--- a/drivers/usb/host/ehci-hcd.c
+++ b/drivers/usb/host/ehci-hcd.c
@@ -190,6 +190,35 @@ out:
 	return ret;
 }
 
+static int ehci_shutdown(struct ehci_ctrl *ctrl)
+{
+	int i, ret = 0;
+	uint32_t cmd, reg;
+
+	cmd = ehci_readl(&ctrl->hcor->or_usbcmd);
+	cmd &= ~(CMD_PSE | CMD_ASE);
+	ehci_writel(&ctrl->hcor->or_usbcmd, cmd);
+	ret = handshake(&ctrl->hcor->or_usbsts, STS_ASS | STS_PSS, 0, 8 * 1000);
+
+	if (!ret) {
+		for (i = 0; i < CONFIG_SYS_USB_EHCI_MAX_ROOT_PORTS; i++) {
+			reg = ehci_readl(&ctrl->hcor->or_portsc[i]);
+			reg |= EHCI_PS_SUSP;
+			ehci_writel(&ctrl->hcor->or_portsc[i], reg);
+		}
+
+		cmd &= ~CMD_RUN;
+		ehci_writel(&ctrl->hcor->or_usbcmd, cmd);
+		ret = handshake(&ctrl->hcor->or_usbsts, STS_HALT, STS_HALT,
+			8 * 1000);
+	}
+
+	if (ret)
+		printf("EHCI failed to shut down host controller.\n");
+
+	return ret;
+}
+
 static int ehci_td_buffer(struct qTD *td, void *buf, size_t sz)
 {
 	uint32_t delta, next;
@@ -808,6 +837,7 @@ ehci_submit_root(struct usb_device *dev, unsigned long pipe, void *buffer,
 			}
 			break;
 		case USB_PORT_FEAT_TEST:
+			ehci_shutdown(ctrl);
 			reg &= ~(0xf << 16);
 			reg |= ((le16_to_cpu(req->index) >> 8) & 0xf) << 16;
 			ehci_writel(status_reg, reg);
@@ -878,6 +908,7 @@ unknown:
 
 int usb_lowlevel_stop(int index)
 {
+	ehci_shutdown(&ehcic[index]);
 	return ehci_hcd_stop(index);
 }
 
-- 
1.7.12.4