trace_xhci_inc_enq(ring);
 }
 
+static int xhci_num_trbs_to(struct xhci_segment *start_seg, union xhci_trb *start,
+                           struct xhci_segment *end_seg, union xhci_trb *end,
+                           unsigned int num_segs)
+{
+       union xhci_trb *last_on_seg;
+       int num = 0;
+       int i = 0;
+
+       do {
+               if (start_seg == end_seg && end >= start)
+                       return num + (end - start);
+               last_on_seg = &start_seg->trbs[TRBS_PER_SEGMENT - 1];
+               num += last_on_seg - start;
+               start_seg = start_seg->next;
+               start = start_seg->trbs;
+       } while (i++ <= num_segs);
+
+       return -EINVAL;
+}
+
 /*
  * Check to see if there's room to enqueue num_trbs on the ring and make sure
  * enqueue pointer will not advance into dequeue segment. See rules above.
                     u32 trb_comp_code)
 {
        struct xhci_ep_ctx *ep_ctx;
+       int trbs_freed;
 
        ep_ctx = xhci_get_ep_ctx(xhci, ep->vdev->out_ctx, ep->ep_index);
 
        }
 
        /* Update ring dequeue pointer */
+       trbs_freed = xhci_num_trbs_to(ep_ring->deq_seg, ep_ring->dequeue,
+                                     td->last_trb_seg, td->last_trb,
+                                     ep_ring->num_segs);
+       if (trbs_freed < 0)
+               xhci_dbg(xhci, "Failed to count freed trbs at TD finish\n");
+       else
+               ep_ring->num_trbs_free += trbs_freed;
        ep_ring->dequeue = td->last_trb;
        ep_ring->deq_seg = td->last_trb_seg;
-       ep_ring->num_trbs_free += td->num_trbs - 1;
        inc_deq(xhci, ep_ring);
 
        return xhci_td_cleanup(xhci, td, ep_ring, td->status);