4 files changed, 85 insertions, 4 deletions

diff --git a/drivers/usb/host/xhci-hcd.c b/drivers/usb/host/xhci-hcd.c
index dba3e07ccd09..1c5901ad6eb8 100644
--- a/drivers/usb/host/xhci-hcd.c
+++ b/drivers/usb/host/xhci-hcd.c
@@ -787,8 +787,11 @@ int xhci_add_endpoint(struct usb_hcd *hcd, struct usb_device *udev,
         int ret = 0;
 
         ret = xhci_check_args(hcd, udev, ep, 1, __func__);
-        if (ret <= 0)
+        if (ret <= 0) {
+                /* So we won't queue a reset ep command for a root hub */
+                ep->hcpriv = NULL;
                 return ret;
+        }
         xhci = hcd_to_xhci(hcd);
 
         added_ctxs = xhci_get_endpoint_flag(&ep->desc);
@@ -851,6 +854,9 @@ int xhci_add_endpoint(struct usb_hcd *hcd, struct usb_device *udev,
         }
         new_slot_info = in_ctx->slot.dev_info;
 
+        /* Store the usb_device pointer for later use */
+        ep->hcpriv = udev;
+
         xhci_dbg(xhci, "add ep 0x%x, slot id %d, new drop flags = %#x, new add flags = %#x, new slot info = %#x\n",
                         (unsigned int) ep->desc.bEndpointAddress,
                         udev->slot_id,
@@ -1026,6 +1032,42 @@ void xhci_reset_bandwidth(struct usb_hcd *hcd, struct usb_device *udev)
         xhci_zero_in_ctx(virt_dev);
 }
 
+/* Deal with stalled endpoints.  The core should have sent the control message
+ * to clear the halt condition.  However, we need to make the xHCI hardware
+ * reset its sequence number, since a device will expect a sequence number of
+ * zero after the halt condition is cleared.
+ * Context: in_interrupt
+ */
+void xhci_endpoint_reset(struct usb_hcd *hcd,
+                struct usb_host_endpoint *ep)
+{
+        struct xhci_hcd *xhci;
+        struct usb_device *udev;
+        unsigned int ep_index;
+        unsigned long flags;
+        int ret;
+
+        xhci = hcd_to_xhci(hcd);
+        udev = (struct usb_device *) ep->hcpriv;
+        /* Called with a root hub endpoint (or an endpoint that wasn't added
+         * with xhci_add_endpoint()
+         */
+        if (!ep->hcpriv)
+                return;
+        ep_index = xhci_get_endpoint_index(&ep->desc);
+
+        xhci_dbg(xhci, "Queueing reset endpoint command\n");
+        spin_lock_irqsave(&xhci->lock, flags);
+        ret = xhci_queue_reset_ep(xhci, udev->slot_id, ep_index);
+        if (!ret) {
+                xhci_ring_cmd_db(xhci);
+        }
+        spin_unlock_irqrestore(&xhci->lock, flags);
+
+        if (ret)
+                xhci_warn(xhci, "FIXME allocate a new ring segment\n");
+}
+
 /*
  * At this point, the struct usb_device is about to go away, the device has
  * disconnected, and all traffic has been stopped and the endpoints have been
diff --git a/drivers/usb/host/xhci-pci.c b/drivers/usb/host/xhci-pci.c
index 1462709e26c0..592fe7e623f7 100644
--- a/drivers/usb/host/xhci-pci.c
+++ b/drivers/usb/host/xhci-pci.c
@@ -117,6 +117,7 @@ static const struct hc_driver xhci_pci_hc_driver = {
         .free_dev =             xhci_free_dev,
         .add_endpoint =         xhci_add_endpoint,
         .drop_endpoint =        xhci_drop_endpoint,
+        .endpoint_reset =       xhci_endpoint_reset,
         .check_bandwidth =      xhci_check_bandwidth,
         .reset_bandwidth =      xhci_reset_bandwidth,
         .address_device =       xhci_address_device,
diff --git a/drivers/usb/host/xhci-ring.c b/drivers/usb/host/xhci-ring.c
index d5b952997423..d672ba14ff80 100644
--- a/drivers/usb/host/xhci-ring.c
+++ b/drivers/usb/host/xhci-ring.c
@@ -279,7 +279,8 @@ static void ring_ep_doorbell(struct xhci_hcd *xhci,
         /* Don't ring the doorbell for this endpoint if there are pending
          * cancellations because the we don't want to interrupt processing.
          */
-        if (!ep_ring->cancels_pending && !(ep_ring->state & SET_DEQ_PENDING)) {
+        if (!ep_ring->cancels_pending && !(ep_ring->state & SET_DEQ_PENDING)
+                        && !(ep_ring->state & EP_HALTED)) {
                 field = xhci_readl(xhci, db_addr) & DB_MASK;
                 xhci_writel(xhci, field | EPI_TO_DB(ep_index), db_addr);
                 /* Flush PCI posted writes - FIXME Matthew Wilcox says this
@@ -603,6 +604,25 @@ static void handle_set_deq_completion(struct xhci_hcd *xhci,
         ring_ep_doorbell(xhci, slot_id, ep_index);
 }
 
+static void handle_reset_ep_completion(struct xhci_hcd *xhci,
+                struct xhci_event_cmd *event,
+                union xhci_trb *trb)
+{
+        int slot_id;
+        unsigned int ep_index;
+
+        slot_id = TRB_TO_SLOT_ID(trb->generic.field[3]);
+        ep_index = TRB_TO_EP_INDEX(trb->generic.field[3]);
+        /* This command will only fail if the endpoint wasn't halted,
+         * but we don't care.
+         */
+        xhci_dbg(xhci, "Ignoring reset ep completion code of %u\n",
+                        (unsigned int) GET_COMP_CODE(event->status));
+
+        /* Clear our internal halted state and restart the ring */
+        xhci->devs[slot_id]->ep_rings[ep_index]->state &= ~EP_HALTED;
+        ring_ep_doorbell(xhci, slot_id, ep_index);
+}
 
 static void handle_cmd_completion(struct xhci_hcd *xhci,
                 struct xhci_event_cmd *event)
@@ -653,6 +673,9 @@ static void handle_cmd_completion(struct xhci_hcd *xhci,
         case TRB_TYPE(TRB_CMD_NOOP):
                 ++xhci->noops_handled;
                 break;
+        case TRB_TYPE(TRB_RESET_EP):
+                handle_reset_ep_completion(xhci, event, xhci->cmd_ring->dequeue);
+                break;
         default:
                 /* Skip over unknown commands on the event ring */
                 xhci->error_bitmask |= 1 << 6;
@@ -823,6 +846,7 @@ static int handle_tx_event(struct xhci_hcd *xhci,
                 break;
         case COMP_STALL:
                 xhci_warn(xhci, "WARN: Stalled endpoint\n");
+                ep_ring->state |= EP_HALTED;
                 status = -EPIPE;
                 break;
         case COMP_TRB_ERR:
@@ -1656,3 +1680,13 @@ static int queue_set_tr_deq(struct xhci_hcd *xhci, int slot_id,
         return queue_command(xhci, (u32) addr | cycle_state, 0, 0,
                         trb_slot_id | trb_ep_index | type);
 }
+
+int xhci_queue_reset_ep(struct xhci_hcd *xhci, int slot_id,
+                unsigned int ep_index)
+{
+        u32 trb_slot_id = SLOT_ID_FOR_TRB(slot_id);
+        u32 trb_ep_index = EP_ID_FOR_TRB(ep_index);
+        u32 type = TRB_TYPE(TRB_RESET_EP);
+
+        return queue_command(xhci, 0, 0, 0, trb_slot_id | trb_ep_index | type);
+}
diff --git a/drivers/usb/host/xhci.h b/drivers/usb/host/xhci.h
index 8936eeb5588b..cde648a524f5 100644
--- a/drivers/usb/host/xhci.h
+++ b/drivers/usb/host/xhci.h
@@ -848,8 +848,8 @@ union xhci_trb {
 #define TRB_CONFIG_EP           12
 /* Evaluate Context Command */
 #define TRB_EVAL_CONTEXT        13
-/* Reset Transfer Ring Command */
-#define TRB_RESET_RING          14
+/* Reset Endpoint Command */
+#define TRB_RESET_EP            14
 /* Stop Transfer Ring Command */
 #define TRB_STOP_RING           15
 /* Set Transfer Ring Dequeue Pointer Command */
@@ -929,6 +929,7 @@ struct xhci_ring {
         unsigned int            cancels_pending;
         unsigned int            state;
 #define SET_DEQ_PENDING         (1 << 0)
+#define EP_HALTED               (1 << 1)
         /* The TRB that was last reported in a stopped endpoint ring */
         union xhci_trb          *stopped_trb;
         struct xhci_td          *stopped_td;
@@ -1128,6 +1129,7 @@ int xhci_urb_enqueue(struct usb_hcd *hcd, struct urb *urb, gfp_t mem_flags);
 int xhci_urb_dequeue(struct usb_hcd *hcd, struct urb *urb, int status);
 int xhci_add_endpoint(struct usb_hcd *hcd, struct usb_device *udev, struct usb_host_endpoint *ep);
 int xhci_drop_endpoint(struct usb_hcd *hcd, struct usb_device *udev, struct usb_host_endpoint *ep);
+void xhci_endpoint_reset(struct usb_hcd *hcd, struct usb_host_endpoint *ep);
 int xhci_check_bandwidth(struct usb_hcd *hcd, struct usb_device *udev);
 void xhci_reset_bandwidth(struct usb_hcd *hcd, struct usb_device *udev);
 
@@ -1148,6 +1150,8 @@ int xhci_queue_bulk_tx(struct xhci_hcd *xhci, gfp_t mem_flags, struct urb *urb,
                 int slot_id, unsigned int ep_index);
 int xhci_queue_configure_endpoint(struct xhci_hcd *xhci, dma_addr_t in_ctx_ptr,
                 u32 slot_id);
+int xhci_queue_reset_ep(struct xhci_hcd *xhci, int slot_id,
+                unsigned int ep_index);
 
 /* xHCI roothub code */
 int xhci_hub_control(struct usb_hcd *hcd, u16 typeReq, u16 wValue, u16 wIndex,