[PATCH 23/31] usb: usbssp: added implementation of transfer events.

[Pawel Laszczak]

Patch add functionality related to handling transfer events.
This kind of events are added to event ring after completion transfer.

This patch add supports for handling transfer events only for
control, interrupt and bulk transfer.

After the transfer, usbssp driver release the TD and informs about it
gadget core driver by means of usbssp_gadget_giveback function.

Signed-off-by: Pawel Laszczak <pawell@xxxxxxxxxxx>
---
 drivers/usb/usbssp/gadget-mem.c  |   9 +
 drivers/usb/usbssp/gadget-ring.c | 602 ++++++++++++++++++++++++++++++-
 drivers/usb/usbssp/gadget.h      |   5 +
 3 files changed, 615 insertions(+), 1 deletion(-)

diff --git a/drivers/usb/usbssp/gadget-mem.c b/drivers/usb/usbssp/gadget-mem.c
index 858bee77b0dc..d5d732b94454 100644
--- a/drivers/usb/usbssp/gadget-mem.c
+++ b/drivers/usb/usbssp/gadget-mem.c
@@ -555,6 +555,15 @@ static void usbssp_free_stream_ctx(struct usbssp_udc *usbssp_data,
 				stream_ctx, dma);
 }
 
+struct usbssp_ring *usbssp_dma_to_transfer_ring(struct usbssp_ep *ep,
+						u64 address)
+{
+	if (ep->ep_state & EP_HAS_STREAMS)
+		return radix_tree_lookup(&ep->stream_info->trb_address_map,
+					 address >> TRB_SEGMENT_SHIFT);
+	return ep->ring;
+}
+
 struct usbssp_ring *usbssp_stream_id_to_ring(struct usbssp_device *dev,
 					     unsigned int ep_index,
 					     unsigned int stream_id)
diff --git a/drivers/usb/usbssp/gadget-ring.c b/drivers/usb/usbssp/gadget-ring.c
index cfb31120eef8..8cc6e4247eef 100644
--- a/drivers/usb/usbssp/gadget-ring.c
+++ b/drivers/usb/usbssp/gadget-ring.c
@@ -62,6 +62,13 @@
 #include "gadget-trace.h"
 #include "gadget.h"
 
+static void giveback_first_trb(struct usbssp_udc *usbssp_data,
+			       unsigned int ep_index,
+			       unsigned int stream_id,
+			       int start_cycle,
+			       struct usbssp_generic_trb
+			       *start_trb);
+
 /*
  * Returns zero if the TRB isn't in this segment, otherwise it returns the DMA
  * address of the TRB.
@@ -80,6 +87,11 @@ dma_addr_t usbssp_trb_virt_to_dma(struct usbssp_segment *seg,
 	return seg->dma + (segment_offset * sizeof(*trb));
 }
 
+static bool trb_is_noop(union usbssp_trb *trb)
+{
+	return TRB_TYPE_NOOP_LE32(trb->generic.field[3]);
+}
+
 static bool trb_is_link(union usbssp_trb *trb)
 {
 	return TRB_TYPE_LINK_LE32(trb->link.control);
@@ -1528,6 +1540,289 @@ int usbssp_is_vendor_info_code(struct usbssp_udc *usbssp_data,
 	return 0;
 }
 
+static int usbssp_td_cleanup(struct usbssp_udc *usbssp_data,
+			     struct usbssp_td *td, struct usbssp_ring *ep_ring,
+			     int *status)
+{
+	struct usbssp_request *req_priv = NULL;
+
+	/* Clean up the endpoint's TD list */
+	req_priv = td->priv_request;
+
+	/* if a bounce buffer was used to align this td then unmap it */
+	usbssp_unmap_td_bounce_buffer(usbssp_data, ep_ring, td);
+
+	/*
+	 * Do one last check of the actual transfer length.
+	 * If the DC controller said we transferred more data than the buffer
+	 * length, req_priv->request.actual will be a very big number (since it's
+	 * unsigned). Play it safe and say we didn't transfer anything.
+	 */
+	if (req_priv->request.actual > req_priv->request.length) {
+		dev_warn(usbssp_data->dev,
+			"USB req %u and actual %u transfer length mismatch\n",
+			req_priv->request.length, req_priv->request.actual);
+		req_priv->request.actual = 0;
+		*status = 0;
+	}
+	list_del_init(&td->td_list);
+
+	inc_td_cnt(req_priv);
+	/* Giveback the USB request when all the tds are completed */
+	if (last_td_in_request(td)) {
+		if ((req_priv->request.actual != req_priv->request.length &&
+		    td->priv_request->request.short_not_ok) || (*status != 0 &&
+		    !usb_endpoint_xfer_isoc(req_priv->dep->endpoint.desc)))
+			dev_dbg(usbssp_data->dev,
+				"Giveback Request %p, len = %d, expected = %d"
+				" status = %d\n",
+				req_priv, req_priv->request.actual,
+				req_priv->request.length, *status);
+
+		if (usb_endpoint_xfer_isoc(req_priv->dep->endpoint.desc))
+			*status = 0;
+
+		usbssp_giveback_request_in_irq(usbssp_data, td, *status);
+	}
+
+	return 0;
+}
+
+static int finish_td(struct usbssp_udc *usbssp_data, struct usbssp_td *td,
+		     struct usbssp_transfer_event *event, struct usbssp_ep *ep,
+		     int *status)
+{
+	struct usbssp_device *dev_priv;
+	struct usbssp_ring *ep_ring;
+	unsigned int slot_id;
+	int ep_index;
+	struct usbssp_ep_ctx *ep_ctx;
+	u32 trb_comp_code;
+
+	slot_id = TRB_TO_SLOT_ID(le32_to_cpu(event->flags));
+	dev_priv = &usbssp_data->devs;
+	ep_index = TRB_TO_EP_ID(le32_to_cpu(event->flags)) - 1;
+	ep_ring = usbssp_dma_to_transfer_ring(ep, le64_to_cpu(event->buffer));
+	ep_ctx = usbssp_get_ep_ctx(usbssp_data, dev_priv->out_ctx, ep_index);
+	trb_comp_code = GET_COMP_CODE(le32_to_cpu(event->transfer_len));
+
+	if (trb_comp_code == COMP_STOPPED_LENGTH_INVALID ||
+	    trb_comp_code == COMP_STOPPED ||
+	    trb_comp_code == COMP_STOPPED_SHORT_PACKET) {
+		/*
+		 * The Endpoint Stop Command completion will take care of any
+		 * stopped TDs. A stopped TD may be restarted, so don't update
+		 * the ring dequeue pointer or take this TD off any lists yet.
+		 */
+		return 0;
+	}
+
+	/* Update ring dequeue pointer */
+	while (ep_ring->dequeue != td->last_trb)
+		inc_deq(usbssp_data, ep_ring);
+
+	inc_deq(usbssp_data, ep_ring);
+
+	return usbssp_td_cleanup(usbssp_data, td, ep_ring, status);
+}
+
+/* sum trb lengths from ring dequeue up to stop_trb, _excluding_ stop_trb */
+static int sum_trb_lengths(struct usbssp_udc *usbssp_data,
+			   struct usbssp_ring *ring,
+			   union usbssp_trb *stop_trb)
+{
+	u32 sum;
+	union usbssp_trb *trb = ring->dequeue;
+	struct usbssp_segment *seg = ring->deq_seg;
+
+	for (sum = 0; trb != stop_trb; next_trb(usbssp_data, ring, &seg, &trb)) {
+		if (!trb_is_noop(trb) && !trb_is_link(trb))
+			sum += TRB_LEN(le32_to_cpu(trb->generic.field[2]));
+	}
+	return sum;
+}
+
+/*
+ * Process control tds, update USB request status and actual_length.
+ */
+static int process_ctrl_td(struct usbssp_udc *usbssp_data, struct usbssp_td *td,
+			   union usbssp_trb *event_trb,
+			   struct usbssp_transfer_event *event,
+			   struct usbssp_ep *ep_priv, int *status)
+{
+	struct usbssp_device *dev_priv;
+	struct usbssp_ring *ep_ring;
+	unsigned int slot_id;
+	int ep_index;
+	struct usbssp_ep_ctx *ep_ctx;
+	u32 trb_comp_code;
+	u32 remaining, requested;
+	u32 trb_type;
+
+	trb_type = TRB_FIELD_TO_TYPE(le32_to_cpu(event_trb->generic.field[3]));
+	slot_id = TRB_TO_SLOT_ID(le32_to_cpu(event->flags));
+	dev_priv = &usbssp_data->devs;
+	ep_index = TRB_TO_EP_ID(le32_to_cpu(event->flags)) - 1;
+	ep_ring = usbssp_dma_to_transfer_ring(ep_priv, le64_to_cpu(event->buffer));
+	ep_ctx = usbssp_get_ep_ctx(usbssp_data, dev_priv->out_ctx, ep_index);
+	trb_comp_code = GET_COMP_CODE(le32_to_cpu(event->transfer_len));
+	requested = td->priv_request->request.length;
+	remaining = EVENT_TRB_LEN(le32_to_cpu(event->transfer_len));
+
+	switch (trb_comp_code) {
+	case COMP_SUCCESS:
+		*status = 0;
+		break;
+	case COMP_SHORT_PACKET:
+		*status = 0;
+		break;
+	case COMP_STOPPED_SHORT_PACKET:
+		if (trb_type == TRB_DATA || trb_type == TRB_NORMAL)
+			td->priv_request->request.actual = remaining;
+	goto finish_td;
+	case COMP_STOPPED:
+		switch (trb_type) {
+		case TRB_DATA:
+		case TRB_NORMAL:
+			td->priv_request->request.actual = requested - remaining;
+			goto finish_td;
+		case TRB_STATUS:
+			td->priv_request->request.actual = requested;
+			goto finish_td;
+		default:
+			dev_warn(usbssp_data->dev,
+				"WARN: unexpected TRB Type %d\n",
+				trb_type);
+			goto finish_td;
+		}
+	case COMP_STOPPED_LENGTH_INVALID:
+		goto finish_td;
+	default:
+		dev_dbg(usbssp_data->dev, "TRB error code %u, "
+			"halted endpoint index = %u\n",
+			trb_comp_code, ep_index);
+	}
+
+	/*
+	 * if on data stage then update the actual_length of the USB
+	 * request and flag it as set, so it won't be overwritten in the event
+	 * for the last TRB.
+	 */
+	if (trb_type == TRB_DATA ||
+	    trb_type == TRB_NORMAL) {
+		td->request_length_set = true;
+		td->priv_request->request.actual = requested - remaining;
+	}
+
+	/* at status stage */
+	if (!td->request_length_set)
+		td->priv_request->request.actual = requested;
+
+	if (usbssp_data->ep0state == USBSSP_EP0_DATA_PHASE
+	   && ep_priv->number == 0
+	   && usbssp_data->three_stage_setup) {
+
+		td = list_entry(ep_ring->td_list.next, struct usbssp_td, td_list);
+		usbssp_data->ep0state = USBSSP_EP0_STATUS_PHASE;
+		dev_dbg(usbssp_data->dev, "Arm Status stage\n");
+		giveback_first_trb(usbssp_data, ep_index, 0,
+				ep_ring->cycle_state, &td->last_trb->generic);
+		return 0;
+	}
+finish_td:
+	return finish_td(usbssp_data, td, event, ep_priv, status);
+}
+
+/*
+ * Process isochronous tds, update usb request status and actual_length.
+ */
+static int process_isoc_td(struct usbssp_udc *usbssp_data, struct usbssp_td *td,
+			   union usbssp_trb *ep_trb,
+			   struct usbssp_transfer_event *event,
+			   struct usbssp_ep *ep_priv, int *status)
+{
+	/*TODO: this function must be implemented*/
+	return 0;
+}
+
+static int skip_isoc_td(struct usbssp_udc *usbssp_data,
+			struct usbssp_td *td,
+			struct usbssp_transfer_event *event,
+			struct usbssp_ep *ep_priv,
+			int *status)
+{
+	/*TODO: this function must be implemented*/
+	return 0;
+}
+
+/*
+ * Process bulk and interrupt tds, update usb request status and actual_length.
+ */
+static int process_bulk_intr_td(struct usbssp_udc *usbssp_data,
+				struct usbssp_td *td,
+				union usbssp_trb *ep_trb,
+				struct usbssp_transfer_event *event,
+				struct usbssp_ep *ep, int *status)
+{
+	struct usbssp_ring *ep_ring;
+	u32 trb_comp_code;
+	u32 remaining, requested, ep_trb_len;
+
+	ep_ring = usbssp_dma_to_transfer_ring(ep, le64_to_cpu(event->buffer));
+	trb_comp_code = GET_COMP_CODE(le32_to_cpu(event->transfer_len));
+	remaining = EVENT_TRB_LEN(le32_to_cpu(event->transfer_len));
+	ep_trb_len = TRB_LEN(le32_to_cpu(ep_trb->generic.field[2]));
+	requested = td->priv_request->request.length;
+
+	switch (trb_comp_code) {
+	case COMP_SUCCESS:
+		/* handle success with untransferred data as short packet */
+		if (ep_trb != td->last_trb || remaining) {
+			dev_warn(usbssp_data->dev, "WARN Successful completion "
+				"on short TX\n");
+			dev_dbg(usbssp_data->dev,
+				"ep %#x - asked for %d bytes, %d bytes untransferred\n",
+				td->priv_request->dep->endpoint.desc->bEndpointAddress,
+				requested, remaining);
+		}
+		*status = 0;
+		break;
+	case COMP_SHORT_PACKET:
+		dev_dbg(usbssp_data->dev,
+			"ep %#x - asked for %d bytes, %d bytes untransferred\n",
+			 td->priv_request->dep->endpoint.desc->bEndpointAddress,
+			 requested, remaining);
+
+		*status = 0;
+		break;
+	case COMP_STOPPED_SHORT_PACKET:
+		td->priv_request->request.length = remaining;
+		goto finish_td;
+	case COMP_STOPPED_LENGTH_INVALID:
+		/* stopped on ep trb with invalid length, exclude it */
+		ep_trb_len = 0;
+		remaining = 0;
+		break;
+	default:
+		/* Others already handled above */
+		break;
+	}
+
+	if (ep_trb == td->last_trb)
+		td->priv_request->request.actual = requested - remaining;
+	else
+		td->priv_request->request.actual =
+			sum_trb_lengths(usbssp_data, ep_ring, ep_trb) + ep_trb_len - remaining;
+finish_td:
+	if (remaining > requested) {
+		dev_warn(usbssp_data->dev,
+			"bad transfer trb length %d in event trb\n",
+			remaining);
+		td->priv_request->request.actual = 0;
+	}
+
+	return finish_td(usbssp_data, td, event, ep, status);
+}
 /*
  * If this function returns an error condition, it means it got a Transfer
  * event with a corrupted Slot ID, Endpoint ID, or TRB DMA address.
@@ -1536,8 +1831,313 @@ int usbssp_is_vendor_info_code(struct usbssp_udc *usbssp_data,
 static int handle_tx_event(struct usbssp_udc *usbssp_data,
 			   struct usbssp_transfer_event *event)
 {
-	/*TODO: implement function handling transfer event*/
+	struct usbssp_device *dev_priv;
+	struct usbssp_ep *ep_priv;
+	struct usbssp_ring *ep_ring;
+	unsigned int slot_id;
+	int ep_index;
+	struct usbssp_td *td = NULL;
+	dma_addr_t ep_trb_dma;
+	struct usbssp_segment *ep_seg;
+	union usbssp_trb *ep_trb;
+	int status = -EINPROGRESS;
+	struct usbssp_ep_ctx *ep_ctx;
+	struct list_head *tmp;
+	u32 trb_comp_code;
+	int ret = 0;
+	int td_num = 0;
+	bool handling_skipped_tds = false;
+	const struct usb_endpoint_descriptor *desc;
+
+	slot_id = TRB_TO_SLOT_ID(le32_to_cpu(event->flags));
+	ep_index = TRB_TO_EP_ID(le32_to_cpu(event->flags)) - 1;
+	trb_comp_code = GET_COMP_CODE(le32_to_cpu(event->transfer_len));
+	ep_trb_dma = le64_to_cpu(event->buffer);
+
+	dev_priv = &usbssp_data->devs;
+
+	ep_priv = &dev_priv->eps[ep_index];
+	ep_ring = usbssp_dma_to_transfer_ring(ep_priv, le64_to_cpu(event->buffer));
+	ep_ctx = usbssp_get_ep_ctx(usbssp_data, dev_priv->out_ctx, ep_index);
+
+	if (GET_EP_CTX_STATE(ep_ctx) == EP_STATE_DISABLED) {
+		dev_err(usbssp_data->dev,
+			"ERROR Transfer event for disabled endpoint slot %u ep %u\n",
+			slot_id, ep_index);
+		goto err_out;
+	}
+
+	/* Some transfer events don't always point to a trb*/
+	if (!ep_ring) {
+		switch (trb_comp_code) {
+		case COMP_USB_TRANSACTION_ERROR:
+		case COMP_INVALID_STREAM_TYPE_ERROR:
+		case COMP_INVALID_STREAM_ID_ERROR:
+			goto cleanup;
+		case COMP_RING_UNDERRUN:
+		case COMP_RING_OVERRUN:
+			goto cleanup;
+		default:
+			dev_err(usbssp_data->dev, "ERROR Transfer event for "
+				"unknown stream ring slot %u ep %u\n",
+				 slot_id, ep_index);
+			goto err_out;
+		}
+	}
+
+	/* Count current td numbers if ep->skip is set */
+	if (ep_priv->skip) {
+		list_for_each(tmp, &ep_ring->td_list)
+			td_num++;
+	}
+
+	/* Look for common error cases */
+	switch (trb_comp_code) {
+	/*
+	 * Skip codes that require special handling depending on
+	 * transfer type
+	 */
+	case COMP_SUCCESS:
+		if (EVENT_TRB_LEN(le32_to_cpu(event->transfer_len)) == 0)
+			break;
+
+		dev_warn_ratelimited(usbssp_data->dev,
+			"WARN Successful completion on short TX\n");
+	case COMP_SHORT_PACKET:
+		break;
+	case COMP_STOPPED:
+		dev_dbg(usbssp_data->dev, "Stopped on Transfer TRB for ep %u\n",
+			ep_index);
+		break;
+	case COMP_STOPPED_LENGTH_INVALID:
+		dev_dbg(usbssp_data->dev,
+			"Stopped on No-op or Link TRB for ep %u\n",
+			ep_index);
+		break;
+	case COMP_STOPPED_SHORT_PACKET:
+		dev_dbg(usbssp_data->dev,
+			"Stopped with short packet transfer detected for ep %u\n",
+			ep_index);
+		break;
+	case COMP_BABBLE_DETECTED_ERROR:
+		dev_dbg(usbssp_data->dev, "Babble error for ep %u on endpoint\n",
+			ep_index);
+		status = -EOVERFLOW;
+		break;
+	case COMP_TRB_ERROR:
+		dev_warn(usbssp_data->dev, "WARN: TRB error on endpoint %u\n",
+			ep_index);
+		status = -EILSEQ;
+		break;
+	/* completion codes not indicating endpoint state change */
+	case COMP_DATA_BUFFER_ERROR:
+		dev_warn(usbssp_data->dev,
+			"WARN: USBSSP couldn't access mem fast enough for ep %u\n",
+			ep_index);
+		status = -ENOSR;
+		break;
+	case COMP_ISOCH_BUFFER_OVERRUN:
+		dev_warn(usbssp_data->dev,
+			"WARN: buffer overrun event for ep %u on endpoint",
+			ep_index);
+		break;
+	case COMP_RING_UNDERRUN:
+		/*
+		 * When the Isoch ring is empty, the DC will generate
+		 * a Ring Overrun Event for IN Isoch endpoint or Ring
+		 * Underrun Event for OUT Isoch endpoint.
+		 */
+		dev_dbg(usbssp_data->dev, "underrun event on endpoint\n");
+		if (!list_empty(&ep_ring->td_list))
+			dev_dbg(usbssp_data->dev, "Underrun Event for ep %d "
+				"still with TDs queued?\n", ep_index);
+		goto cleanup;
+	case COMP_RING_OVERRUN:
+		dev_dbg(usbssp_data->dev, "overrun event on endpoint\n");
+		if (!list_empty(&ep_ring->td_list))
+			dev_dbg(usbssp_data->dev, "Overrun Event for ep %d "
+				"still with TDs queued?\n",
+				ep_index);
+		goto cleanup;
+	case COMP_MISSED_SERVICE_ERROR:
+		/*
+		 * When encounter missed service error, one or more isoc tds
+		 * may be missed by DC.
+		 * Set skip flag of the ep_ring; Complete the missed tds as
+		 * short transfer when process the ep_ring next time.
+		 */
+		ep_priv->skip = true;
+		dev_dbg(usbssp_data->dev,
+			"Miss service interval error for ep %u, set skip flag\n",
+			ep_index);
+		goto cleanup;
+	case COMP_INCOMPATIBLE_DEVICE_ERROR:
+		/* needs disable slot command to recover */
+		dev_warn(usbssp_data->dev,
+			"WARN: detect an incompatible device for ep %u",
+			ep_index);
+		status = -EPROTO;
+		break;
+	default:
+		if (usbssp_is_vendor_info_code(usbssp_data, trb_comp_code)) {
+			status = 0;
+			break;
+		}
+
+		dev_warn(usbssp_data->dev,
+			"ERROR Unknown event condition %u, for ep %u - USBSSP probably busted\n",
+			trb_comp_code, ep_index);
+		goto cleanup;
+	}
+
+	do {
+		/*
+		 * This TRB should be in the TD at the head of this ring's TD
+		 * list.
+		 */
+		if (list_empty(&ep_ring->td_list)) {
+			/*
+			 * Don't print wanings if it's due to a stopped endpoint
+			 * generating an extra completion event if the device
+			 * was suspended. Or, a event for the last TRB of a
+			 * short TD we already got a short event for.
+			 * The short TD is already removed from the TD list.
+			 */
+			if (!(trb_comp_code == COMP_STOPPED ||
+			    trb_comp_code == COMP_STOPPED_LENGTH_INVALID ||
+			    ep_ring->last_td_was_short)) {
+				dev_warn(usbssp_data->dev,
+					"WARN Event TRB for ep %d with no TDs queued?\n",
+					ep_index);
+			}
+
+			if (ep_priv->skip) {
+				ep_priv->skip = false;
+				dev_dbg(usbssp_data->dev,
+					"td_list is empty while skip "
+					"flag set. Clear skip flag for ep %u.\n",
+					ep_index);
+			}
+			goto cleanup;
+		}
+
+		/* We've skipped all the TDs on the ep ring when ep->skip set */
+		if (ep_priv->skip && td_num == 0) {
+			ep_priv->skip = false;
+			dev_dbg(usbssp_data->dev,
+				"All tds on the ep_ring skipped. "
+				"Clear skip flag for ep %u.\n", ep_index);
+			goto cleanup;
+		}
+
+		td = list_entry(ep_ring->td_list.next, struct usbssp_td, td_list);
+
+		if (ep_priv->skip)
+			td_num--;
+
+		/* Is this a TRB in the currently executing TD? */
+		ep_seg = usbssp_trb_in_td(usbssp_data, ep_ring->deq_seg,
+					ep_ring->dequeue, td->last_trb,
+					ep_trb_dma, false);
+
+		/*
+		 * Skip the Force Stopped Event. The event_trb(ep_trb_dma)
+		 * of FSE is not in the current TD pointed by ep_ring->dequeue
+		 * because that the hardware dequeue pointer still at the
+		 * previous TRB of the current TD. The previous TRB maybe a
+		 * Link TD or the last TRB of the previous TD. The command
+		 * completion handle will take care the rest.
+		 */
+		if (!ep_seg && (trb_comp_code == COMP_STOPPED ||
+		    trb_comp_code == COMP_STOPPED_LENGTH_INVALID)) {
+			goto cleanup;
+		}
+
+		desc = td->priv_request->dep->endpoint.desc;
+		if (!ep_seg) {
+			if (!ep_priv->skip || !usb_endpoint_xfer_isoc(desc)) {
+
+				/* USBSSP is busted, give up! */
+				dev_err(usbssp_data->dev,
+					"ERROR Transfer event TRB DMA ptr not "
+					"part of current TD ep_index %d "
+					"comp_code %u\n", ep_index,
+					trb_comp_code);
+
+				usbssp_trb_in_td(usbssp_data, ep_ring->deq_seg,
+					ep_ring->dequeue, td->last_trb,
+					ep_trb_dma, true);
+				return -ESHUTDOWN;
+			}
+
+			ret = skip_isoc_td(usbssp_data, td, event, ep_priv,
+					&status);
+			goto cleanup;
+		}
+
+		if (trb_comp_code == COMP_SHORT_PACKET)
+			ep_ring->last_td_was_short = true;
+		else
+			ep_ring->last_td_was_short = false;
+
+		if (ep_priv->skip) {
+			dev_dbg(usbssp_data->dev,
+				"Found td. Clear skip flag for ep %u.\n",
+				ep_index);
+			ep_priv->skip = false;
+		}
+
+		ep_trb = &ep_seg->trbs[(ep_trb_dma - ep_seg->dma) / sizeof(*ep_trb)];
+
+		trace_usbssp_handle_transfer(ep_ring,
+					(struct usbssp_generic_trb *) ep_trb);
+
+		if (trb_is_noop(ep_trb)) {
+			dev_dbg(usbssp_data->dev,
+				"event_trb is a no-op TRB. Skip it\n");
+			goto cleanup;
+		}
+
+		if (usb_endpoint_xfer_control(desc)) {
+			ret = process_ctrl_td(usbssp_data, td, ep_trb, event,
+					ep_priv, &status);
+		} else if (usb_endpoint_xfer_isoc(desc)) {
+			ret = process_isoc_td(usbssp_data, td, ep_trb,
+					event, ep_priv, &status);
+		} else {
+			ret = process_bulk_intr_td(usbssp_data, td, ep_trb,
+						event, ep_priv, &status);
+		}
+cleanup:
+		handling_skipped_tds = ep_priv->skip &&
+			trb_comp_code != COMP_MISSED_SERVICE_ERROR;
+
+		/*
+		 * Do not update event ring dequeue pointer if we're in a loop
+		 * processing missed tds.
+		 */
+		if (!handling_skipped_tds)
+			inc_deq(usbssp_data, usbssp_data->event_ring);
+	/*
+	 * If ep->skip is set, it means there are missed tds on the
+	 * endpoint ring need to take care of.
+	 * Process them as short transfer until reach the td pointed by
+	 * the event.
+	 */
+	} while (handling_skipped_tds);
+
 	return 0;
+
+err_out:
+	dev_err(usbssp_data->dev, "@%016llx %08x %08x %08x %08x\n",
+		 (unsigned long long) usbssp_trb_virt_to_dma(
+					usbssp_data->event_ring->deq_seg,
+					usbssp_data->event_ring->dequeue),
+		 lower_32_bits(le64_to_cpu(event->buffer)),
+		 upper_32_bits(le64_to_cpu(event->buffer)),
+		 le32_to_cpu(event->transfer_len),
+		 le32_to_cpu(event->flags));
+	return -ENODEV;
 }
 
 /*
diff --git a/drivers/usb/usbssp/gadget.h b/drivers/usb/usbssp/gadget.h
index 000f2cb93723..c4e440db6b23 100644
--- a/drivers/usb/usbssp/gadget.h
+++ b/drivers/usb/usbssp/gadget.h
@@ -1699,6 +1699,11 @@ int usbssp_ring_expansion(struct usbssp_udc *usbssp_data,
 struct usbssp_ring *usbssp_stream_id_to_ring(struct usbssp_device *dev,
 					unsigned int ep_index,
 					unsigned int stream_id);
+struct usbssp_ring *usbssp_dma_to_transfer_ring(struct usbssp_ep *ep,
+						u64 address);
+struct usbssp_ring *usbssp_stream_id_to_ring(struct usbssp_device *dev,
+					unsigned int ep_index,
+					unsigned int stream_id);
 
 struct usbssp_command *usbssp_alloc_command(struct usbssp_udc *usbssp_data,
 					bool allocate_completion,
-- 
2.17.1