1 files changed, 102 insertions, 94 deletions

diff --git a/drivers/usb/musb/musb_gadget.c b/drivers/usb/musb/musb_gadget.c
index 74073f9a43f0..c49b9ba025ab 100644
--- a/drivers/usb/musb/musb_gadget.c
+++ b/drivers/usb/musb/musb_gadget.c
@@ -429,112 +429,102 @@ void musb_g_tx(struct musb *musb, u8 epnum)
         DBG(4, "<== %s, txcsr %04x\n", musb_ep->end_point.name, csr);
 
         dma = is_dma_capable() ? musb_ep->dma : NULL;
-        do {
-                /* REVISIT for high bandwidth, MUSB_TXCSR_P_INCOMPTX
-                 * probably rates reporting as a host error
+
+        /*
+         * REVISIT: for high bandwidth, MUSB_TXCSR_P_INCOMPTX
+         * probably rates reporting as a host error.
+         */
+        if (csr & MUSB_TXCSR_P_SENTSTALL) {
+                csr |=  MUSB_TXCSR_P_WZC_BITS;
+                csr &= ~MUSB_TXCSR_P_SENTSTALL;
+                musb_writew(epio, MUSB_TXCSR, csr);
+                return;
+        }
+
+        if (csr & MUSB_TXCSR_P_UNDERRUN) {
+                /* We NAKed, no big deal... little reason to care. */
+                csr |=   MUSB_TXCSR_P_WZC_BITS;
+                csr &= ~(MUSB_TXCSR_P_UNDERRUN | MUSB_TXCSR_TXPKTRDY);
+                musb_writew(epio, MUSB_TXCSR, csr);
+                DBG(20, "underrun on ep%d, req %p\n", epnum, request);
+        }
+
+        if (dma_channel_status(dma) == MUSB_DMA_STATUS_BUSY) {
+                /*
+                 * SHOULD NOT HAPPEN... has with CPPI though, after
+                 * changing SENDSTALL (and other cases); harmless?
                  */
-                if (csr & MUSB_TXCSR_P_SENTSTALL) {
-                        csr |= MUSB_TXCSR_P_WZC_BITS;
-                        csr &= ~MUSB_TXCSR_P_SENTSTALL;
-                        musb_writew(epio, MUSB_TXCSR, csr);
-                        break;
-                }
+                DBG(5, "%s dma still busy?\n", musb_ep->end_point.name);
+                return;
+        }
+
+        if (request) {
+                u8      is_dma = 0;
 
-                if (csr & MUSB_TXCSR_P_UNDERRUN) {
-                        /* we NAKed, no big deal ... little reason to care */
+                if (dma && (csr & MUSB_TXCSR_DMAENAB)) {
+                        is_dma = 1;
                         csr |= MUSB_TXCSR_P_WZC_BITS;
-                        csr &= ~(MUSB_TXCSR_P_UNDERRUN
-                                        | MUSB_TXCSR_TXPKTRDY);
+                        csr &= ~(MUSB_TXCSR_DMAENAB | MUSB_TXCSR_P_UNDERRUN |
+                                 MUSB_TXCSR_TXPKTRDY);
                         musb_writew(epio, MUSB_TXCSR, csr);
-                        DBG(20, "underrun on ep%d, req %p\n", epnum, request);
+                        /* Ensure writebuffer is empty. */
+                        csr = musb_readw(epio, MUSB_TXCSR);
+                        request->actual += musb_ep->dma->actual_len;
+                        DBG(4, "TXCSR%d %04x, DMA off, len %zu, req %p\n",
+                                epnum, csr, musb_ep->dma->actual_len, request);
                 }
 
-                if (dma_channel_status(dma) == MUSB_DMA_STATUS_BUSY) {
-                        /* SHOULD NOT HAPPEN ... has with cppi though, after
-                         * changing SENDSTALL (and other cases); harmless?
+                if (is_dma || request->actual == request->length) {
+                        /*
+                         * First, maybe a terminating short packet. Some DMA
+                         * engines might handle this by themselves.
                          */
-                        DBG(5, "%s dma still busy?\n", musb_ep->end_point.name);
-                        break;
-                }
-
-                if (request) {
-                        u8      is_dma = 0;
-
-                        if (dma && (csr & MUSB_TXCSR_DMAENAB)) {
-                                is_dma = 1;
-                                csr |= MUSB_TXCSR_P_WZC_BITS;
-                                csr &= ~(MUSB_TXCSR_DMAENAB
-                                                | MUSB_TXCSR_P_UNDERRUN
-                                                | MUSB_TXCSR_TXPKTRDY);
-                                musb_writew(epio, MUSB_TXCSR, csr);
-                                /* ensure writebuffer is empty */
-                                csr = musb_readw(epio, MUSB_TXCSR);
-                                request->actual += musb_ep->dma->actual_len;
-                                DBG(4, "TXCSR%d %04x, dma off, "
-                                                "len %zu, req %p\n",
-                                        epnum, csr,
-                                        musb_ep->dma->actual_len,
-                                        request);
-                        }
-
-                        if (is_dma || request->actual == request->length) {
-
-                                /* First, maybe a terminating short packet.
-                                 * Some DMA engines might handle this by
-                                 * themselves.
-                                 */
-                                if ((request->zero
-                                                && request->length
-                                                && (request->length
-                                                        % musb_ep->packet_sz)
-                                                        == 0)
+                        if ((request->zero && request->length
+                                && request->length % musb_ep->packet_sz == 0)
 #ifdef CONFIG_USB_INVENTRA_DMA
-                                        || (is_dma &&
-                                                ((!dma->desired_mode) ||
-                                                    (request->actual &
-                                                    (musb_ep->packet_sz - 1))))
+                                || (is_dma && (!dma->desired_mode ||
+                                        (request->actual &
+                                                (musb_ep->packet_sz - 1))))
 #endif
-                                ) {
-                                        /* on dma completion, fifo may not
-                                         * be available yet ...
-                                         */
-                                        if (csr & MUSB_TXCSR_TXPKTRDY)
-                                                break;
-
-                                        DBG(4, "sending zero pkt\n");
-                                        musb_writew(epio, MUSB_TXCSR,
-                                                        MUSB_TXCSR_MODE
-                                                        | MUSB_TXCSR_TXPKTRDY);
-                                        request->zero = 0;
-                                }
-
-                                /* ... or if not, then complete it */
-                                musb_g_giveback(musb_ep, request, 0);
-
-                                /* kickstart next transfer if appropriate;
-                                 * the packet that just completed might not
-                                 * be transmitted for hours or days.
-                                 * REVISIT for double buffering...
-                                 * FIXME revisit for stalls too...
+                        ) {
+                                /*
+                                 * On DMA completion, FIFO may not be
+                                 * available yet...
                                  */
-                                musb_ep_select(mbase, epnum);
-                                csr = musb_readw(epio, MUSB_TXCSR);
-                                if (csr & MUSB_TXCSR_FIFONOTEMPTY)
-                                        break;
-                                request = musb_ep->desc
-                                                ? next_request(musb_ep)
-                                                : NULL;
-                                if (!request) {
-                                        DBG(4, "%s idle now\n",
-                                                musb_ep->end_point.name);
-                                        break;
-                                }
+                                if (csr & MUSB_TXCSR_TXPKTRDY)
+                                        return;
+
+                                DBG(4, "sending zero pkt\n");
+                                musb_writew(epio, MUSB_TXCSR, MUSB_TXCSR_MODE
+                                                | MUSB_TXCSR_TXPKTRDY);
+                                request->zero = 0;
                         }
 
-                        txstate(musb, to_musb_request(request));
+                        /* ... or if not, then complete it. */
+                        musb_g_giveback(musb_ep, request, 0);
+
+                        /*
+                         * Kickstart next transfer if appropriate;
+                         * the packet that just completed might not
+                         * be transmitted for hours or days.
+                         * REVISIT for double buffering...
+                         * FIXME revisit for stalls too...
+                         */
+                        musb_ep_select(mbase, epnum);
+                        csr = musb_readw(epio, MUSB_TXCSR);
+                        if (csr & MUSB_TXCSR_FIFONOTEMPTY)
+                                return;
+
+                        if (!musb_ep->desc) {
+                                DBG(4, "%s idle now\n",
+                                        musb_ep->end_point.name);
+                                return;
+                        } else
+                                request = next_request(musb_ep);
                 }
 
-        } while (0);
+                txstate(musb, to_musb_request(request));
+        }
 }
 
 /* ------------------------------------------------------------ */
@@ -966,6 +956,7 @@ static int musb_gadget_enable(struct usb_ep *ep,
 
         musb_ep->desc = desc;
         musb_ep->busy = 0;
+        musb_ep->wedged = 0;
         status = 0;
 
         pr_debug("%s periph: enabled %s for %s %s, %smaxpacket %d\n",
@@ -1220,7 +1211,7 @@ done:
  *
  * exported to ep0 code
  */
-int musb_gadget_set_halt(struct usb_ep *ep, int value)
+static int musb_gadget_set_halt(struct usb_ep *ep, int value)
 {
         struct musb_ep          *musb_ep = to_musb_ep(ep);
         u8                      epnum = musb_ep->current_epnum;
@@ -1262,7 +1253,8 @@ int musb_gadget_set_halt(struct usb_ep *ep, int value)
                                 goto done;
                         }
                 }
-        }
+        } else
+                musb_ep->wedged = 0;
 
         /* set/clear the stall and toggle bits */
         DBG(2, "%s: %s stall\n", ep->name, value ? "set" : "clear");
@@ -1301,6 +1293,21 @@ done:
         return status;
 }
 
+/*
+ * Sets the halt feature with the clear requests ignored
+ */
+static int musb_gadget_set_wedge(struct usb_ep *ep)
+{
+        struct musb_ep          *musb_ep = to_musb_ep(ep);
+
+        if (!ep)
+                return -EINVAL;
+
+        musb_ep->wedged = 1;
+
+        return usb_ep_set_halt(ep);
+}
+
 static int musb_gadget_fifo_status(struct usb_ep *ep)
 {
         struct musb_ep          *musb_ep = to_musb_ep(ep);
@@ -1371,6 +1378,7 @@ static const struct usb_ep_ops musb_ep_ops = {
         .queue          = musb_gadget_queue,
         .dequeue        = musb_gadget_dequeue,
         .set_halt       = musb_gadget_set_halt,
+        .set_wedge      = musb_gadget_set_wedge,
         .fifo_status    = musb_gadget_fifo_status,
         .fifo_flush     = musb_gadget_fifo_flush
 };