5 files changed, 71 insertions, 50 deletions

diff --git a/drivers/usb/host/ehci-hcd.c b/drivers/usb/host/ehci-hcd.c
index f6cf1d178107..c13dad8a8503 100644
--- a/drivers/usb/host/ehci-hcd.c
+++ b/drivers/usb/host/ehci-hcd.c
@@ -410,8 +410,10 @@ static void ehci_work (struct ehci_hcd *ehci)
         ehci->need_rescan = false;
         if (ehci->async_count)
                 scan_async(ehci);
-        if (ehci->next_uframe != -1)
-                scan_periodic (ehci);
+        if (ehci->intr_count > 0)
+                scan_intr(ehci);
+        if (ehci->isoc_count > 0)
+                scan_isoc(ehci);
         if (ehci->need_rescan)
                 goto rescan;
         ehci->scanning = false;
@@ -509,6 +511,7 @@ static int ehci_init(struct usb_hcd *hcd)
          * periodic_size can shrink by USBCMD update if hcc_params allows.
          */
         ehci->periodic_size = DEFAULT_I_TDPS;
+        INIT_LIST_HEAD(&ehci->intr_qh_list);
         INIT_LIST_HEAD(&ehci->cached_itd_list);
         INIT_LIST_HEAD(&ehci->cached_sitd_list);
 
diff --git a/drivers/usb/host/ehci-q.c b/drivers/usb/host/ehci-q.c
index bae931767825..c9c7f7b3b7db 100644
--- a/drivers/usb/host/ehci-q.c
+++ b/drivers/usb/host/ehci-q.c
@@ -322,7 +322,7 @@ qh_completions (struct ehci_hcd *ehci, struct ehci_qh *qh)
          *
          * It's a bug for qh->qh_state to be anything other than
          * QH_STATE_IDLE, unless our caller is scan_async() or
-         * scan_periodic().
+         * scan_intr().
          */
         state = qh->qh_state;
         qh->qh_state = QH_STATE_COMPLETING;
@@ -832,7 +832,6 @@ qh_make (
                                 is_input, 0,
                                 hb_mult(maxp) * max_packet(maxp)));
                 qh->start = NO_FRAME;
-                qh->stamp = ehci->periodic_stamp;
 
                 if (urb->dev->speed == USB_SPEED_HIGH) {
                         qh->c_usecs = 0;
diff --git a/drivers/usb/host/ehci-sched.c b/drivers/usb/host/ehci-sched.c
index 11b2f21d7ac1..263b542985c0 100644
--- a/drivers/usb/host/ehci-sched.c
+++ b/drivers/usb/host/ehci-sched.c
@@ -569,7 +569,10 @@ static void qh_link_periodic(struct ehci_hcd *ehci, struct ehci_qh *qh)
                 ? ((qh->usecs + qh->c_usecs) / qh->period)
                 : (qh->usecs * 8);
 
+        list_add(&qh->intr_node, &ehci->intr_qh_list);
+
         /* maybe enable periodic schedule processing */
+        ++ehci->intr_count;
         enable_periodic(ehci);
 }
 
@@ -614,6 +617,11 @@ static void qh_unlink_periodic(struct ehci_hcd *ehci, struct ehci_qh *qh)
         /* qh->qh_next still "live" to HC */
         qh->qh_state = QH_STATE_UNLINK;
         qh->qh_next.ptr = NULL;
+
+        if (ehci->qh_scan_next == qh)
+                ehci->qh_scan_next = list_entry(qh->intr_node.next,
+                                struct ehci_qh, intr_node);
+        list_del(&qh->intr_node);
 }
 
 static void start_unlink_intr(struct ehci_hcd *ehci, struct ehci_qh *qh)
@@ -683,6 +691,7 @@ static void end_unlink_intr(struct ehci_hcd *ehci, struct ehci_qh *qh)
         }
 
         /* maybe turn off periodic schedule */
+        --ehci->intr_count;
         disable_periodic(ehci);
 }
 
@@ -920,6 +929,35 @@ done_not_linked:
         return status;
 }
 
+static void scan_intr(struct ehci_hcd *ehci)
+{
+        struct ehci_qh          *qh;
+
+        list_for_each_entry_safe(qh, ehci->qh_scan_next, &ehci->intr_qh_list,
+                        intr_node) {
+ rescan:
+                /* clean any finished work for this qh */
+                if (!list_empty(&qh->qtd_list)) {
+                        int temp;
+
+                        /*
+                         * Unlinks could happen here; completion reporting
+                         * drops the lock.  That's why ehci->qh_scan_next
+                         * always holds the next qh to scan; if the next qh
+                         * gets unlinked then ehci->qh_scan_next is adjusted
+                         * in qh_unlink_periodic().
+                         */
+                        temp = qh_completions(ehci, qh);
+                        if (unlikely(qh->needs_rescan ||
+                                        (list_empty(&qh->qtd_list) &&
+                                                qh->qh_state == QH_STATE_LINKED)))
+                                start_unlink_intr(ehci, qh);
+                        else if (temp != 0)
+                                goto rescan;
+                }
+        }
+}
+
 /*-------------------------------------------------------------------------*/
 
 /* ehci_iso_stream ops work with both ITD and SITD */
@@ -1450,6 +1488,10 @@ iso_stream_schedule (
         urb->start_frame = stream->next_uframe;
         if (!stream->highspeed)
                 urb->start_frame >>= 3;
+
+        /* Make sure scan_isoc() sees these */
+        if (ehci->isoc_count == 0)
+                ehci->next_uframe = now;
         return 0;
 
  fail:
@@ -1608,6 +1650,7 @@ static void itd_link_urb(
         urb->hcpriv = NULL;
 
         timer_action (ehci, TIMER_IO_WATCHDOG);
+        ++ehci->isoc_count;
         enable_periodic(ehci);
 }
 
@@ -1688,9 +1731,11 @@ itd_complete (
         ehci_urb_done(ehci, urb, 0);
         retval = true;
         urb = NULL;
+
+        --ehci->isoc_count;
         disable_periodic(ehci);
-        ehci_to_hcd(ehci)->self.bandwidth_isoc_reqs--;
 
+        ehci_to_hcd(ehci)->self.bandwidth_isoc_reqs--;
         if (ehci_to_hcd(ehci)->self.bandwidth_isoc_reqs == 0) {
                 if (ehci->amd_pll_fix == 1)
                         usb_amd_quirk_pll_enable();
@@ -2008,6 +2053,7 @@ static void sitd_link_urb(
         urb->hcpriv = NULL;
 
         timer_action (ehci, TIMER_IO_WATCHDOG);
+        ++ehci->isoc_count;
         enable_periodic(ehci);
 }
 
@@ -2074,9 +2120,11 @@ sitd_complete (
         ehci_urb_done(ehci, urb, 0);
         retval = true;
         urb = NULL;
+
+        --ehci->isoc_count;
         disable_periodic(ehci);
-        ehci_to_hcd(ehci)->self.bandwidth_isoc_reqs--;
 
+        ehci_to_hcd(ehci)->self.bandwidth_isoc_reqs--;
         if (ehci_to_hcd(ehci)->self.bandwidth_isoc_reqs == 0) {
                 if (ehci->amd_pll_fix == 1)
                         usb_amd_quirk_pll_enable();
@@ -2165,8 +2213,7 @@ static int sitd_submit (struct ehci_hcd *ehci, struct urb *urb,
 
 /*-------------------------------------------------------------------------*/
 
-static void
-scan_periodic (struct ehci_hcd *ehci)
+static void scan_isoc(struct ehci_hcd *ehci)
 {
         unsigned        now_uframe, frame, clock, clock_frame, mod;
         unsigned        modified;
@@ -2189,7 +2236,6 @@ scan_periodic (struct ehci_hcd *ehci)
         ehci->clock_frame = clock_frame;
         clock &= mod - 1;
         clock_frame = clock >> 3;
-        ++ehci->periodic_stamp;
 
         for (;;) {
                 union ehci_shadow       q, *q_p;
@@ -2208,36 +2254,10 @@ restart:
 
                 while (q.ptr != NULL) {
                         unsigned                uf;
-                        union ehci_shadow       temp;
                         int                     live;
 
                         live = (ehci->rh_state >= EHCI_RH_RUNNING);
                         switch (hc32_to_cpu(ehci, type)) {
-                        case Q_TYPE_QH:
-                                /* handle any completions */
-                                temp.qh = q.qh;
-                                type = Q_NEXT_TYPE(ehci, q.qh->hw->hw_next);
-                                q = q.qh->qh_next;
-                                if (temp.qh->stamp != ehci->periodic_stamp) {
-                                        modified = qh_completions(ehci, temp.qh);
-                                        if (!modified)
-                                                temp.qh->stamp = ehci->periodic_stamp;
-                                        if (unlikely(list_empty(&temp.qh->qtd_list) ||
-                                                        temp.qh->needs_rescan))
-                                                start_unlink_intr(ehci, temp.qh);
-                                }
-                                break;
-                        case Q_TYPE_FSTN:
-                                /* for "save place" FSTNs, look at QH entries
-                                 * in the previous frame for completions.
-                                 */
-                                if (q.fstn->hw_prev != EHCI_LIST_END(ehci)) {
-                                        ehci_dbg(ehci,
-                                                "ignoring completions from FSTNs\n");
-                                }
-                                type = Q_NEXT_TYPE(ehci, q.fstn->hw_next);
-                                q = q.fstn->fstn_next;
-                                break;
                         case Q_TYPE_ITD:
                                 /* If this ITD is still active, leave it for
                                  * later processing ... check the next entry.
@@ -2319,12 +2339,17 @@ restart:
                                 ehci_dbg(ehci, "corrupt type %d frame %d shadow %p\n",
                                         type, frame, q.ptr);
                                 // BUG ();
+                                /* FALL THROUGH */
+                        case Q_TYPE_QH:
+                        case Q_TYPE_FSTN:
+                                /* End of the iTDs and siTDs */
                                 q.ptr = NULL;
+                                break;
                         }
 
                         /* assume completion callbacks modify the queue */
                         if (unlikely (modified)) {
-                                if (likely(ehci->periodic_count > 0))
+                                if (likely(ehci->isoc_count > 0))
                                         goto restart;
                                 /* short-circuit this scan */
                                 now_uframe = clock;
@@ -2353,7 +2378,7 @@ restart:
                         unsigned        now;
 
                         if (ehci->rh_state < EHCI_RH_RUNNING
-                                        || ehci->periodic_count == 0)
+                                        || ehci->isoc_count == 0)
                                 break;
                         ehci->next_uframe = now_uframe;
                         now = ehci_read_frame_index(ehci) & (mod - 1);
@@ -2363,10 +2388,7 @@ restart:
                         /* rescan the rest of this frame, then ... */
                         clock = now;
                         clock_frame = clock >> 3;
-                        if (ehci->clock_frame != clock_frame) {
-                                ehci->clock_frame = clock_frame;
-                                ++ehci->periodic_stamp;
-                        }
+                        ehci->clock_frame = clock_frame;
                 } else {
                         now_uframe++;
                         now_uframe &= mod - 1;
diff --git a/drivers/usb/host/ehci-timer.c b/drivers/usb/host/ehci-timer.c
index a823290b5139..0e28bae78d18 100644
--- a/drivers/usb/host/ehci-timer.c
+++ b/drivers/usb/host/ehci-timer.c
@@ -168,13 +168,8 @@ static void ehci_poll_PSS(struct ehci_hcd *ehci)
 
         /* The status is up-to-date; restart or stop the schedule as needed */
         if (want == 0) {        /* Stopped */
-                if (ehci->periodic_count > 0) {
-
-                        /* make sure ehci_work scans these */
-                        ehci->next_uframe = ehci_read_frame_index(ehci)
-                                        & ((ehci->periodic_size << 3) - 1);
+                if (ehci->periodic_count > 0)
                         ehci_set_command_bit(ehci, CMD_PSE);
-                }
 
         } else {                /* Running */
                 if (ehci->periodic_count == 0) {
diff --git a/drivers/usb/host/ehci.h b/drivers/usb/host/ehci.h
index c462d52ac575..08637183aad0 100644
--- a/drivers/usb/host/ehci.h
+++ b/drivers/usb/host/ehci.h
@@ -117,6 +117,7 @@ struct ehci_hcd {			/* one per controller */
         bool                    need_rescan:1;
         bool                    intr_unlinking:1;
         bool                    async_unlinking:1;
+        struct ehci_qh          *qh_scan_next;
 
         /* async schedule support */
         struct ehci_qh          *async;
@@ -124,7 +125,6 @@ struct ehci_hcd {			/* one per controller */
         struct ehci_qh          *async_unlink;
         struct ehci_qh          *async_unlink_last;
         struct ehci_qh          *async_iaa;
-        struct ehci_qh          *qh_scan_next;
         unsigned                async_unlink_cycle;
         unsigned                async_count;    /* async activity count */
 
@@ -133,6 +133,7 @@ struct ehci_hcd {			/* one per controller */
         unsigned                periodic_size;
         __hc32                  *periodic;      /* hw periodic table */
         dma_addr_t              periodic_dma;
+        struct list_head        intr_qh_list;
         unsigned                i_thresh;       /* uframes HC might cache */
 
         union ehci_shadow       *pshadow;       /* mirror hw periodic table */
@@ -140,6 +141,8 @@ struct ehci_hcd {			/* one per controller */
         struct ehci_qh          *intr_unlink_last;
         unsigned                intr_unlink_cycle;
         int                     next_uframe;    /* scan periodic, start here */
+        unsigned                intr_count;     /* intr activity count */
+        unsigned                isoc_count;     /* isoc activity count */
         unsigned                periodic_count; /* periodic activity count */
         unsigned                uframe_periodic_max; /* max periodic time per uframe */
 
@@ -176,7 +179,6 @@ struct ehci_hcd {			/* one per controller */
 
         struct timer_list       watchdog;
         unsigned long           actions;
-        unsigned                periodic_stamp;
         unsigned                random_frame;
         unsigned long           next_statechange;
         ktime_t                 last_periodic_enable;
@@ -381,11 +383,11 @@ struct ehci_qh {
         dma_addr_t              qh_dma;         /* address of qh */
         union ehci_shadow       qh_next;        /* ptr to qh; or periodic */
         struct list_head        qtd_list;       /* sw qtd list */
+        struct list_head        intr_node;      /* list of intr QHs */
         struct ehci_qtd         *dummy;
         struct ehci_qh          *unlink_next;   /* next on unlink list */
 
         unsigned                unlink_cycle;
-        unsigned                stamp;
 
         u8                      needs_rescan;   /* Dequeue during giveback */
         u8                      qh_state;