3 files changed, 89 insertions, 14 deletions

diff --git a/drivers/usb/host/ehci-hcd.c b/drivers/usb/host/ehci-hcd.c
index 1cc02052f5..1edb344d0f 100644
--- a/drivers/usb/host/ehci-hcd.c
+++ b/drivers/usb/host/ehci-hcd.c
@@ -298,6 +298,51 @@ static void ehci_update_endpt2_dev_n_port(struct usb_device *udev,
 				     QH_ENDPT2_HUBADDR(hubaddr));
 }
 
+static int ehci_enable_async(struct ehci_ctrl *ctrl)
+{
+	u32 cmd;
+	int ret;
+
+	/* Enable async. schedule. */
+	cmd = ehci_readl(&ctrl->hcor->or_usbcmd);
+	if (cmd & CMD_ASE)
+		return 0;
+
+	cmd |= CMD_ASE;
+	ehci_writel(&ctrl->hcor->or_usbcmd, cmd);
+
+	ret = handshake((uint32_t *)&ctrl->hcor->or_usbsts, STS_ASS, STS_ASS,
+			100 * 1000);
+	if (ret < 0)
+		printf("EHCI fail timeout STS_ASS set\n");
+
+	return ret;
+}
+
+static int ehci_disable_async(struct ehci_ctrl *ctrl)
+{
+	u32 cmd;
+	int ret;
+
+	if (ctrl->async_locked)
+		return 0;
+
+	/* Disable async schedule. */
+	cmd = ehci_readl(&ctrl->hcor->or_usbcmd);
+	if (!(cmd & CMD_ASE))
+		return 0;
+
+	cmd &= ~CMD_ASE;
+	ehci_writel(&ctrl->hcor->or_usbcmd, cmd);
+
+	ret = handshake((uint32_t *)&ctrl->hcor->or_usbsts, STS_ASS, 0,
+			100 * 1000);
+	if (ret < 0)
+		printf("EHCI fail timeout STS_ASS reset\n");
+
+	return ret;
+}
+
 static int
 ehci_submit_async(struct usb_device *dev, unsigned long pipe, void *buffer,
 		   int length, struct devrequest *req)
@@ -311,7 +356,6 @@ ehci_submit_async(struct usb_device *dev, unsigned long pipe, void *buffer,
 	uint32_t *tdp;
 	uint32_t endpt, maxpacket, token, usbsts, qhtoken;
 	uint32_t c, toggle;
-	uint32_t cmd;
 	int timeout;
 	int ret = 0;
 	struct ehci_ctrl *ctrl = ehci_get_ctrl(dev);
@@ -556,19 +600,9 @@ ehci_submit_async(struct usb_device *dev, unsigned long pipe, void *buffer,
 	usbsts = ehci_readl(&ctrl->hcor->or_usbsts);
 	ehci_writel(&ctrl->hcor->or_usbsts, (usbsts & 0x3f));
 
-	/* Enable async. schedule. */
-	cmd = ehci_readl(&ctrl->hcor->or_usbcmd);
-	if (!(cmd & CMD_ASE)) {
-		cmd |= CMD_ASE;
-		ehci_writel(&ctrl->hcor->or_usbcmd, cmd);
-
-		ret = handshake((uint32_t *)&ctrl->hcor->or_usbsts, STS_ASS, STS_ASS,
-				100 * 1000);
-		if (ret < 0) {
-			printf("EHCI fail timeout STS_ASS set\n");
-			goto fail;
-		}
-	}
+	ret = ehci_enable_async(ctrl);
+	if (ret)
+		goto fail;
 
 	/* Wait for TDs to be processed. */
 	ts = get_timer(0);
@@ -611,6 +645,10 @@ ehci_submit_async(struct usb_device *dev, unsigned long pipe, void *buffer,
 	if (QT_TOKEN_GET_STATUS(token) & QT_TOKEN_STATUS_ACTIVE)
 		printf("EHCI timed out on TD - token=%#x\n", token);
 
+	ret = ehci_disable_async(ctrl);
+	if (ret)
+		goto fail;
+
 	if (!(QT_TOKEN_GET_STATUS(qhtoken) & QT_TOKEN_STATUS_ACTIVE)) {
 		debug("TOKEN=%#x\n", qhtoken);
 		switch (QT_TOKEN_GET_STATUS(qhtoken) &
@@ -1512,6 +1550,16 @@ static int _ehci_submit_int_msg(struct usb_device *dev, unsigned long pipe,
 	return result;
 }
 
+static int _ehci_lock_async(struct ehci_ctrl *ctrl, int lock)
+{
+	ctrl->async_locked = lock;
+
+	if (lock)
+		return 0;
+
+	return ehci_disable_async(ctrl);
+}
+
 #if !CONFIG_IS_ENABLED(DM_USB)
 int submit_bulk_msg(struct usb_device *dev, unsigned long pipe,
 			    void *buffer, int length)
@@ -1549,6 +1597,13 @@ int destroy_int_queue(struct usb_device *dev, struct int_queue *queue)
 {
 	return _ehci_destroy_int_queue(dev, queue);
 }
+
+int usb_lock_async(struct usb_device *dev, int lock)
+{
+	struct ehci_ctrl *ctrl = ehci_get_ctrl(dev);
+
+	return _ehci_lock_async(ctrl, lock);
+}
 #endif
 
 #if CONFIG_IS_ENABLED(DM_USB)
@@ -1612,6 +1667,13 @@ static int ehci_get_max_xfer_size(struct udevice *dev, size_t *size)
 	return 0;
 }
 
+static int ehci_lock_async(struct udevice *dev, int lock)
+{
+	struct ehci_ctrl *ctrl = dev_get_priv(dev);
+
+	return _ehci_lock_async(ctrl, lock);
+}
+
 int ehci_register(struct udevice *dev, struct ehci_hccr *hccr,
 		  struct ehci_hcor *hcor, const struct ehci_ops *ops,
 		  uint tweaks, enum usb_init_type init)
@@ -1678,6 +1740,7 @@ struct dm_usb_ops ehci_usb_ops = {
 	.poll_int_queue = ehci_poll_int_queue,
 	.destroy_int_queue = ehci_destroy_int_queue,
 	.get_max_xfer_size  = ehci_get_max_xfer_size,
+	.lock_async = ehci_lock_async,
 };
 
 #endif
diff --git a/drivers/usb/host/ehci.h b/drivers/usb/host/ehci.h
index 6c359af90c..66c1d61dbf 100644
--- a/drivers/usb/host/ehci.h
+++ b/drivers/usb/host/ehci.h
@@ -255,6 +255,7 @@ struct ehci_ctrl {
 	int periodic_schedules;
 	int ntds;
 	bool has_fsl_erratum_a005275;	/* Freescale HS silicon quirk */
+	bool async_locked;
 	struct ehci_ops ops;
 	void *priv;	/* client's private data */
 };
diff --git a/drivers/usb/host/usb-uclass.c b/drivers/usb/host/usb-uclass.c
index 8521651588..5e423012df 100644
--- a/drivers/usb/host/usb-uclass.c
+++ b/drivers/usb/host/usb-uclass.c
@@ -22,6 +22,17 @@ struct usb_uclass_priv {
 	int companion_device_count;
 };
 
+int usb_lock_async(struct usb_device *udev, int lock)
+{
+	struct udevice *bus = udev->controller_dev;
+	struct dm_usb_ops *ops = usb_get_ops(bus);
+
+	if (!ops->lock_async)
+		return -ENOSYS;
+
+	return ops->lock_async(bus, lock);
+}
+
 int usb_disable_asynch(int disable)
 {
 	int old_value = asynch_allowed;