1 files changed, 31 insertions, 36 deletions

diff --git a/drivers/usb/host/xhci-ring.c b/drivers/usb/host/xhci-ring.c
index 5f926bea5ab1..7a0e3c720c00 100644
--- a/drivers/usb/host/xhci-ring.c
+++ b/drivers/usb/host/xhci-ring.c
@@ -550,6 +550,7 @@ void xhci_find_new_dequeue_state(struct xhci_hcd *xhci,
         struct xhci_ring *ep_ring;
         struct xhci_generic_trb *trb;
         dma_addr_t addr;
+        u64 hw_dequeue;
 
         ep_ring = xhci_triad_to_transfer_ring(xhci, slot_id,
                         ep_index, stream_id);
@@ -559,16 +560,6 @@ void xhci_find_new_dequeue_state(struct xhci_hcd *xhci,
                                 stream_id);
                 return;
         }
-        state->new_cycle_state = 0;
-        xhci_dbg_trace(xhci, trace_xhci_dbg_cancel_urb,
-                        "Finding segment containing stopped TRB.");
-        state->new_deq_seg = find_trb_seg(cur_td->start_seg,
-                        dev->eps[ep_index].stopped_trb,
-                        &state->new_cycle_state);
-        if (!state->new_deq_seg) {
-                WARN_ON(1);
-                return;
-        }
 
         /* Dig out the cycle state saved by the xHC during the stop ep cmd */
         xhci_dbg_trace(xhci, trace_xhci_dbg_cancel_urb,
@@ -577,46 +568,57 @@ void xhci_find_new_dequeue_state(struct xhci_hcd *xhci,
         if (ep->ep_state & EP_HAS_STREAMS) {
                 struct xhci_stream_ctx *ctx =
                         &ep->stream_info->stream_ctx_array[stream_id];
-                state->new_cycle_state = 0x1 & le64_to_cpu(ctx->stream_ring);
+                hw_dequeue = le64_to_cpu(ctx->stream_ring);
         } else {
                 struct xhci_ep_ctx *ep_ctx
                         = xhci_get_ep_ctx(xhci, dev->out_ctx, ep_index);
-                state->new_cycle_state = 0x1 & le64_to_cpu(ep_ctx->deq);
+                hw_dequeue = le64_to_cpu(ep_ctx->deq);
         }
 
+        /* Find virtual address and segment of hardware dequeue pointer */
+        state->new_deq_seg = ep_ring->deq_seg;
+        state->new_deq_ptr = ep_ring->dequeue;
+        while (xhci_trb_virt_to_dma(state->new_deq_seg, state->new_deq_ptr)
+                        != (dma_addr_t)(hw_dequeue & ~0xf)) {
+                next_trb(xhci, ep_ring, &state->new_deq_seg,
+                                        &state->new_deq_ptr);
+                if (state->new_deq_ptr == ep_ring->dequeue) {
+                        WARN_ON(1);
+                        return;
+                }
+        }
+        /*
+         * Find cycle state for last_trb, starting at old cycle state of
+         * hw_dequeue. If there is only one segment ring, find_trb_seg() will
+         * return immediately and cannot toggle the cycle state if this search
+         * wraps around, so add one more toggle manually in that case.
+         */
+        state->new_cycle_state = hw_dequeue & 0x1;
+        if (ep_ring->first_seg == ep_ring->first_seg->next &&
+                        cur_td->last_trb < state->new_deq_ptr)
+                state->new_cycle_state ^= 0x1;
+
         state->new_deq_ptr = cur_td->last_trb;
         xhci_dbg_trace(xhci, trace_xhci_dbg_cancel_urb,
                         "Finding segment containing last TRB in TD.");
         state->new_deq_seg = find_trb_seg(state->new_deq_seg,
-                        state->new_deq_ptr,
-                        &state->new_cycle_state);
+                        state->new_deq_ptr, &state->new_cycle_state);
         if (!state->new_deq_seg) {
                 WARN_ON(1);
                 return;
         }
 
+        /* Increment to find next TRB after last_trb. Cycle if appropriate. */
         trb = &state->new_deq_ptr->generic;
         if (TRB_TYPE_LINK_LE32(trb->field[3]) &&
             (trb->field[3] & cpu_to_le32(LINK_TOGGLE)))
                 state->new_cycle_state ^= 0x1;
         next_trb(xhci, ep_ring, &state->new_deq_seg, &state->new_deq_ptr);
 
-        /*
-         * If there is only one segment in a ring, find_trb_seg()'s while loop
-         * will not run, and it will return before it has a chance to see if it
-         * needs to toggle the cycle bit.  It can't tell if the stalled transfer
-         * ended just before the link TRB on a one-segment ring, or if the TD
-         * wrapped around the top of the ring, because it doesn't have the TD in
-         * question.  Look for the one-segment case where stalled TRB's address
-         * is greater than the new dequeue pointer address.
-         */
-        if (ep_ring->first_seg == ep_ring->first_seg->next &&
-                        state->new_deq_ptr < dev->eps[ep_index].stopped_trb)
-                state->new_cycle_state ^= 0x1;
+        /* Don't update the ring cycle state for the producer (us). */
         xhci_dbg_trace(xhci, trace_xhci_dbg_cancel_urb,
                         "Cycle state = 0x%x", state->new_cycle_state);
 
-        /* Don't update the ring cycle state for the producer (us). */
         xhci_dbg_trace(xhci, trace_xhci_dbg_cancel_urb,
                         "New dequeue segment = %p (virtual)",
                         state->new_deq_seg);
@@ -799,7 +801,6 @@ static void xhci_handle_cmd_stop_ep(struct xhci_hcd *xhci, int slot_id,
         if (list_empty(&ep->cancelled_td_list)) {
                 xhci_stop_watchdog_timer_in_irq(xhci, ep);
                 ep->stopped_td = NULL;
-                ep->stopped_trb = NULL;
                 ring_doorbell_for_active_rings(xhci, slot_id, ep_index);
                 return;
         }
@@ -867,11 +868,9 @@ remove_finished_td:
                 ring_doorbell_for_active_rings(xhci, slot_id, ep_index);
         }
 
-        /* Clear stopped_td and stopped_trb if endpoint is not halted */
-        if (!(ep->ep_state & EP_HALTED)) {
+        /* Clear stopped_td if endpoint is not halted */
+        if (!(ep->ep_state & EP_HALTED))
                 ep->stopped_td = NULL;
-                ep->stopped_trb = NULL;
-        }
 
         /*
          * Drop the lock and complete the URBs in the cancelled TD list.
@@ -1941,14 +1940,12 @@ static void xhci_cleanup_halted_endpoint(struct xhci_hcd *xhci,
         struct xhci_virt_ep *ep = &xhci->devs[slot_id]->eps[ep_index];
         ep->ep_state |= EP_HALTED;
         ep->stopped_td = td;
-        ep->stopped_trb = event_trb;
         ep->stopped_stream = stream_id;
 
         xhci_queue_reset_ep(xhci, slot_id, ep_index);
         xhci_cleanup_stalled_ring(xhci, td->urb->dev, ep_index);
 
         ep->stopped_td = NULL;
-        ep->stopped_trb = NULL;
         ep->stopped_stream = 0;
 
         xhci_ring_cmd_db(xhci);
@@ -2030,7 +2027,6 @@ static int finish_td(struct xhci_hcd *xhci, struct xhci_td *td,
                  * the ring dequeue pointer or take this TD off any lists yet.
                  */
                 ep->stopped_td = td;
-                ep->stopped_trb = event_trb;
                 return 0;
         } else {
                 if (trb_comp_code == COMP_STALL) {
@@ -2042,7 +2038,6 @@ static int finish_td(struct xhci_hcd *xhci, struct xhci_td *td,
                          * USB class driver clear the stall later.
                          */
                         ep->stopped_td = td;
-                        ep->stopped_trb = event_trb;
                         ep->stopped_stream = ep_ring->stream_id;
                 } else if (xhci_requires_manual_halt_cleanup(xhci,
                                         ep_ctx, trb_comp_code)) {