4 files changed, 85 insertions, 4 deletions

diff --git a/drivers/usb/host/xhci-hcd.c b/drivers/usb/host/xhci-hcd.c
index dba3e07..1c5901a 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 1462709..592fe7e 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 d5b9529..d672ba1 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 8936eeb..cde648a 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,