[PATCH 1/2] usb: host: ehci-sched: add itd/sitd_unlink_urb() functions

[Xu Yang <xu.yang_2@xxxxxxx>]

This will add itd/sitd_unlink_urb() functions in case of the driver needs
to unlink these urbs manually.

Signed-off-by: Xu Yang <xu.yang_2@xxxxxxx>
---
 drivers/usb/host/ehci-sched.c | 94 +++++++++++++++++++++++++++++++++++
 1 file changed, 94 insertions(+)

diff --git a/drivers/usb/host/ehci-sched.c b/drivers/usb/host/ehci-sched.c
index bd542b6fc46b..b95a8bc4d3ba 100644
--- a/drivers/usb/host/ehci-sched.c
+++ b/drivers/usb/host/ehci-sched.c
@@ -1805,6 +1805,67 @@ static void itd_link_urb(
 	enable_periodic(ehci);
 }
 
+/* unlink itd/sitd from the periodic list */
+static inline void
+unlink(struct ehci_hcd *ehci, unsigned frame, void *ptr)
+{
+	union ehci_shadow	*prev = &ehci->pshadow[frame];
+	__hc32			*hw_p = &ehci->periodic[frame];
+	union ehci_shadow	here = *prev;
+
+	while (here.ptr && here.ptr != ptr) {
+		prev = periodic_next_shadow(ehci, prev,
+			Q_NEXT_TYPE(ehci, *hw_p));
+		hw_p = shadow_next_periodic(ehci, &here,
+			Q_NEXT_TYPE(ehci, *hw_p));
+		here = *prev;
+	}
+
+	*prev = *periodic_next_shadow(ehci, &here,
+			Q_NEXT_TYPE(ehci, *hw_p));
+
+	if (!ehci->use_dummy_qh ||
+		*shadow_next_periodic(ehci, &here, Q_NEXT_TYPE(ehci, *hw_p)
+		!= EHCI_LIST_END(ehci)))
+		*hw_p = *shadow_next_periodic(ehci, &here,
+					Q_NEXT_TYPE(ehci, *hw_p));
+	else
+		*hw_p = cpu_to_hc32(ehci, ehci->dummy->qh_dma);
+}
+
+static void itd_unlink_urb(
+	struct ehci_hcd         *ehci,
+	struct urb              *urb
+)
+{
+	struct ehci_itd         *itd, *n;
+	struct ehci_iso_stream	*stream = urb->hcpriv;
+
+	if (unlikely(list_empty(&stream->td_list)))
+		return;
+
+	list_for_each_entry_safe(itd, n, &stream->td_list, itd_list) {
+		if (itd->urb != urb)
+			continue;
+		unlink(ehci, itd->frame, itd);
+		itd->urb = NULL;
+		list_move_tail(&itd->itd_list, &stream->free_list);
+	}
+
+	ehci_to_hcd(ehci)->self.bandwidth_isoc_reqs--;
+	if (unlikely(list_empty(&stream->td_list)))
+		ehci_to_hcd(ehci)->self.bandwidth_allocated -= stream->bandwidth;
+
+	ehci_urb_done(ehci, urb, -ENOENT);
+
+	--ehci->isoc_count;
+	disable_periodic(ehci);
+
+	list_splice_tail_init(&stream->free_list,
+			&ehci->cached_itd_list);
+	start_free_itds(ehci);
+}
+
 #define	ISO_ERRS (EHCI_ISOC_BUF_ERR | EHCI_ISOC_BABBLE | EHCI_ISOC_XACTERR)
 
 /* Process and recycle a completed ITD.  Return true iff its urb completed,
@@ -2196,6 +2257,39 @@ static void sitd_link_urb(
 	enable_periodic(ehci);
 }
 
+static void sitd_unlink_urb(
+	struct ehci_hcd         *ehci,
+	struct urb              *urb
+)
+{
+	struct ehci_sitd         *sitd, *n;
+	struct ehci_iso_stream	*stream = urb->hcpriv;
+
+	if (unlikely(list_empty(&stream->td_list)))
+		return;
+
+	list_for_each_entry_safe(sitd, n, &stream->td_list, sitd_list) {
+		if (sitd->urb != urb)
+			continue;
+		unlink(ehci, sitd->frame, sitd);
+		sitd->urb = NULL;
+		list_move_tail(&sitd->sitd_list, &stream->free_list);
+	}
+
+	ehci_to_hcd(ehci)->self.bandwidth_isoc_reqs--;
+	if (unlikely(list_empty(&stream->td_list)))
+		ehci_to_hcd(ehci)->self.bandwidth_allocated -= stream->bandwidth;
+
+	ehci_urb_done(ehci, urb, -ENOENT);
+
+	--ehci->isoc_count;
+	disable_periodic(ehci);
+
+	list_splice_tail_init(&stream->free_list,
+			&ehci->cached_sitd_list);
+	start_free_itds(ehci);
+}
+
 /*-------------------------------------------------------------------------*/
 
 #define	SITD_ERRS (SITD_STS_ERR | SITD_STS_DBE | SITD_STS_BABBLE \
-- 
2.34.1