jz4760: Heavily rework USB driver to add working DMA support

 * DMA Bulk IN (ie our TX) results in sequential transfers 33-68% faster.
 * DMA Bulk OUT (ie RX) is mostly stripped out due to complete brokenness.
 * Interrupt and control endpoints remain PIO-driven.

Other improvements:

1) Use consistent endpoint references (no magic numbers)
2) Greatly enhanced logging
3) DMA support can be compiled out completely
4) Setting lockswitch will disable all DMA operations at runtime
5) Much more robust error checking and recovery

Change-Id: I57b82e655e55ced0dfe289e379b0b61d8fe443b4

1 files changed, 244 insertions, 139 deletions

diff --git a/firmware/target/mips/ingenic_jz47xx/usb-jz4760.c b/firmware/target/mips/ingenic_jz47xx/usb-jz4760.c
index 275fd3fd2b..8562d9253c 100644
--- a/firmware/target/mips/ingenic_jz47xx/usb-jz4760.c
+++ b/firmware/target/mips/ingenic_jz47xx/usb-jz4760.c
@@ -7,7 +7,8 @@
  *                     \/            \/     \/    \/            \/
  * $Id$
  *
- * Copyright (C) 2016 by Roman Stolyarov
+ * Copyright (C) 2016 Roman Stolyarov
+ * Copyright (C) 2020 Solomon Peachy
  *
  * This program is free software; you can redistribute it and/or
  * modify it under the terms of the GNU General Public License
@@ -20,7 +21,7 @@
  ****************************************************************************/
 
 #include "config.h"
-/*#define LOGF_ENABLE*/
+//#define LOGF_ENABLE
 #include "logf.h"
 #include "system.h"
 #include "usb_ch9.h"
@@ -29,6 +30,8 @@
 #include "cpu.h"
 #include "thread.h"
 
+#define USE_USB_DMA
+
 #define PIN_USB_DET      (32*4+19)
 #define IRQ_USB_DET      GPIO_IRQ(PIN_USB_DET)
 #define GPIO_USB_DET     GPIO147
@@ -43,6 +46,13 @@
 #define TOTAL_EP()       (sizeof(endpoints)/sizeof(struct usb_endpoint))
 #define EP_IS_IN(ep)     (EP_NUMBER((ep))%2)
 
+#define TXCSR_WZC_BITS (USB_INCSR_SENTSTALL | USB_INCSR_UNDERRUN | USB_INCSR_FFNOTEMPT | USB_INCSR_INCOMPTX)
+
+/* NOTE:  IN/OUT is from the HOST perspective.  We're a peripheral, so:
+     IN = DEV->HOST, (ie we send)
+     OUT = HOST->DEV, (ie we recv)
+*/
+
 enum ep_type
 {
     ep_control,
@@ -53,6 +63,16 @@ enum ep_type
 
 struct usb_endpoint
 {
+    const enum ep_type type;
+    const long fifo_addr;
+    unsigned short fifo_size;
+    bool allocated;
+    int use_dma;  /* -1 = no, 0 = mode_0, 1 = mode_1 */
+
+    struct semaphore complete;
+
+    uint8_t config;
+
     volatile void *buf;
     volatile size_t length;
     union
@@ -60,24 +80,14 @@ struct usb_endpoint
         volatile size_t sent;
         volatile size_t received;
     };
+    volatile int rc;
     volatile bool busy;
-
-    const enum ep_type type;
-    const bool use_dma;
-
-    const long fifo_addr;
-    unsigned short fifo_size;
-
     volatile bool wait;
-    struct semaphore complete;
-
-    volatile int rc;
-    bool allocated;
 };
 
-#define EP_INIT(_type, _fifo_addr, _fifo_size, _buf, _use_dma) \
+#define EP_INIT(_type, _fifo_addr, _fifo_size, _buf) \
     { .type = (_type), .fifo_addr = (_fifo_addr), .fifo_size = (_fifo_size), \
-      .buf = (_buf), .use_dma = (_use_dma), \
+      .buf = (_buf), .use_dma = -1, \
       .length = 0, .busy = false, .wait = false, .allocated = false }
 
 static union
@@ -91,12 +101,12 @@ static volatile bool ep0_data_requested = false;
 
 static struct usb_endpoint endpoints[] =
 {
-    EP_INIT(ep_control,   USB_FIFO_EP(0),  64, NULL,        false),
-    EP_INIT(ep_control,   USB_FIFO_EP(0),  64, &ep0_rx.buf, false),
-    EP_INIT(ep_bulk,      USB_FIFO_EP(1), 512, NULL,        false),
-    EP_INIT(ep_bulk,      USB_FIFO_EP(1), 512, NULL,        false),
-    EP_INIT(ep_interrupt, USB_FIFO_EP(2), 512, NULL,        false),
-    EP_INIT(ep_interrupt, USB_FIFO_EP(2), 512, NULL,        false),
+    EP_INIT(ep_control,   USB_FIFO_EP(0),  64, NULL),
+    EP_INIT(ep_control,   USB_FIFO_EP(0),  64, &ep0_rx.buf),
+    EP_INIT(ep_bulk,      USB_FIFO_EP(1), 512, NULL),
+    EP_INIT(ep_bulk,      USB_FIFO_EP(1), 512, NULL),
+    EP_INIT(ep_interrupt, USB_FIFO_EP(2), 512, NULL),
+    EP_INIT(ep_interrupt, USB_FIFO_EP(2), 512, NULL),
 };
 
 static inline void select_endpoint(int ep)
@@ -329,7 +339,8 @@ static void EP0_handler(void)
         EP0_send();
 }
 
-static void EPIN_handler(unsigned int endpoint)
+/* Does new work */
+static void EPIN_send(unsigned int endpoint)
 {
     struct usb_endpoint* ep = &endpoints[endpoint*2];
     unsigned int length, csr;
@@ -338,45 +349,153 @@ static void EPIN_handler(unsigned int endpoint)
     csr = REG_USB_INCSR;
     logf("%s(%d): 0x%x", __func__, endpoint, csr);
 
-    if(!ep->busy)
-    {
+    if (!ep->busy) {
         logf("Entered EPIN handler without work!");
         return;
     }
 
-    if(csr & USB_INCSR_SENTSTALL)
-    {
-        REG_USB_INCSR = csr & ~USB_INCSR_SENTSTALL;
+    if (csr & USB_INCSR_INPKTRDY) {
+        logf("PKTRDY %d", endpoint);
         return;
     }
 
-    if(ep->use_dma)
+    if (csr & USB_INCSR_SENTSTALL) {
+        logf("SENDSTALL %d", endpoint);
+        REG_USB_INCSR = csr & ~USB_INCSR_SENTSTALL;
         return;
+    }
 
-    if(csr & USB_INCSR_FFNOTEMPT)
-    {
+    if (csr & USB_INCSR_FFNOTEMPT) {
         logf("FIFO is not empty! 0x%x", csr);
         return;
     }
 
+#ifdef USE_USB_DMA
+    if(ep->use_dma >= 0) {
+	logf("DMA busy(%x %x %x)", REG_USB_ADDR(USB_INTR_DMA_BULKIN), REG_USB_COUNT(USB_INTR_DMA_BULKIN),REG_USB_CNTL(USB_INTR_DMA_BULKIN));
+
+        return;
+    }
+#endif
+
     logf("EP%d: %d -> %d", endpoint, ep->sent, ep->length);
 
+#ifdef USE_USB_DMA
+    /* Can we use DMA? */
+    if (ep->type == ep_bulk && ep->length && (!(((unsigned long)ep->buf + ep->sent) % 4)) && !button_hold()) {
+        if (ep->length >= ep->fifo_size)
+            ep->use_dma = 1;
+        else
+            ep->use_dma = 0;
+    } else {
+        ep->use_dma = -1;
+    }
+
+    if (ep->use_dma >= 0) {
+        commit_discard_dcache_range((void*)ep->buf + ep->sent, ep->length - ep->sent);
+        /* Set up DMA */
+        uint16_t dmacr = USB_CNTL_BURST_16 | USB_CNTL_EP(EP_NUMBER2(ep)) | USB_CNTL_ENA | USB_CNTL_INTR_EN | USB_CNTL_DIR_IN ;
+        if (ep->use_dma > 0)
+            dmacr |= USB_CNTL_MODE_1;
+
+        REG_USB_ADDR(USB_INTR_DMA_BULKIN) = PHYSADDR((unsigned long)ep->buf + ep->sent);
+        REG_USB_COUNT(USB_INTR_DMA_BULKIN) = ep->length - ep->sent;
+        REG_USB_CNTL(USB_INTR_DMA_BULKIN) = dmacr;
+
+	uint16_t csr = REG_USB_INCSR;
+	if (ep->use_dma == 0) {
+            csr &= ~((USB_INCSRH_AUTOSET | USB_INCSRH_DMAREQENAB) << 8);
+            REG_USB_INCSR = csr | TXCSR_WZC_BITS;
+            csr &= ~((USB_INCSRH_DMAREQMODE) << 8);
+            csr |= ((USB_INCSRH_DMAREQENAB | USB_INCSRH_MODE) << 8);
+        } else {
+            csr |= ((USB_INCSRH_DMAREQENAB | USB_INCSRH_MODE | USB_INCSRH_DMAREQMODE) << 8);
+            csr |= ((USB_INCSRH_AUTOSET) << 8);
+        }
+        csr &= ~USB_INCSR_UNDERRUN;
+
+        logf("DMA setup(%d: %x %x %x %x - %d)", EP_NUMBER2(ep), (unsigned int)PHYSADDR((unsigned long)ep->buf), ep->length, dmacr, csr, ep->use_dma);
+
+        REG_USB_INCSR = csr;
+
+        return;
+    }
+#endif
+
+    /* Non-DMA code */
     if(ep->sent == 0)
         length = MIN(ep->length, ep->fifo_size);
     else
         length = MIN(EP_BUF_LEFT(ep), ep->fifo_size);
 
     writeFIFO(ep, length);
-    REG_USB_INCSR = csr | USB_INCSR_INPKTRDY;
     ep->sent += length;
+    csr &= ~USB_INCSR_UNDERRUN;
+    csr |= USB_INCSR_INPKTRDY;
+    logf("Non-DMA TX %x", csr);
+    REG_USB_INCSR = csr;
+}
 
-    if(ep->sent >= ep->length)
-    {
+static void EPIN_complete(unsigned int endpoint)
+{
+    struct usb_endpoint* ep = &endpoints[endpoint*2];
+    uint16_t csr;
+
+    select_endpoint(endpoint);
+    csr = REG_USB_INCSR;
+    logf("%s(%d): 0x%x", __func__, endpoint, csr);
+
+    if (csr & USB_INCSR_SENTSTALL) {
+        logf("SENDSTALL %d\n", endpoint);
+        REG_USB_INCSR = csr & ~USB_INCSR_SENTSTALL; // XXX TXCSR_P_WZC_BITS
+        return;
+    }
+
+    if (csr & USB_INCSR_UNDERRUN) {
+        csr |= TXCSR_WZC_BITS;
+        csr &= ~(USB_INCSR_UNDERRUN | USB_INCSR_INPKTRDY);
+        REG_USB_INCSR = csr;
+        logf("underrun! %x", csr);
+    }
+
+    if (!ep->busy) {
+        logf("Entered EPIN_complete without work!");
+        return;
+    }
+
+    if (ep->use_dma >= 0) {
+        logf("DMA status (%x %x %x)", REG_USB_ADDR(USB_INTR_DMA_BULKIN), REG_USB_COUNT(USB_INTR_DMA_BULKIN),REG_USB_CNTL(USB_INTR_DMA_BULKIN));
+        return;
+    }
+
+    /* If we get here, the operation is completed, and we need to clean up */
+
+    /* Make sure DMA engine is idle */
+    if (csr & (USB_INCSRH_DMAREQENAB << 8)) {
+        csr |= TXCSR_WZC_BITS;
+        csr &= ~(USB_INCSR_UNDERRUN | USB_INCSR_INPKTRDY |
+                 ((USB_INCSRH_DMAREQENAB | USB_INCSRH_AUTOSET) << 8));
+        REG_USB_INCSR = csr;
+        csr = REG_USB_INCSR;
+        logf("DMA cleanup %x", csr);
+    }
+
+    // XXX send a zero-length packet if necessary.
+    // if tx complete, and ep->length > 0 and ep->length % fifo == 0,
+    // REG_USB_INCSR = MODE | PKTRDY;
+    // Not needed for mass storage as it counts packets but
+    // if we ever enable other protocls...
+
+    logf("EP%d: %d -> %d", endpoint, ep->sent, ep->length);
+
+    if(ep->sent >= ep->length) {
         if (!ep->wait)
             usb_core_transfer_complete(endpoint, USB_DIR_IN, 0, ep->sent);
         ep->rc = 0;
         ep_transfer_completed(ep);
-        logf("sent complete");
+        logf("send complete");
+    } else {
+        EPIN_send(endpoint);
     }
 }
 
@@ -403,9 +522,6 @@ static void EPOUT_handler(unsigned int endpoint)
             return;
         }
 
-        if(ep->use_dma)
-            return;
-
         if(csr & USB_OUTCSR_OUTPKTRDY) /* There is a packet in the fifo */
         {
             size = REG_USB_OUTCOUNT;
@@ -430,59 +546,55 @@ static void EPOUT_handler(unsigned int endpoint)
     }
 }
 
+#ifdef USE_USB_DMA
 static void EPDMA_handler(int number)
 {
     int endpoint = -1;
-    unsigned int size = 0;
+    int size = 0;
     struct usb_endpoint* ep = NULL;
 
-    if(number == USB_INTR_DMA_BULKIN)
-    {
-        endpoint = (REG_USB_CNTL(0) >> 4) & 0xF;
-        ep = &endpoints[endpoint*2];
-        size = (unsigned int)ep->buf - REG_USB_ADDR(0);
-    }
-    else if(number == USB_INTR_DMA_BULKOUT)
-    {
-        endpoint = (REG_USB_CNTL(1) >> 4) & 0xF;
-        ep = &endpoints[endpoint*2+1];
-        size = (unsigned int)ep->buf - REG_USB_ADDR(1);
-    }
-
-    logf("DMA_BULK%d %d", number, endpoint);
-
-    if(number == USB_INTR_DMA_BULKOUT)
-    {
-        /* Disable DMA */
-        REG_USB_CNTL(1) = 0;
-
-        commit_discard_dcache(); // XXX range?
-
-        select_endpoint(endpoint);
-        /* Read out last packet manually */
-        unsigned int lpack_size = REG_USB_OUTCOUNT;
-        if(lpack_size > 0)
-        {
-            ep->buf += ep->length - lpack_size;
-            readFIFO(ep, lpack_size);
-            REG_USB_OUTCSR &= ~USB_OUTCSR_OUTPKTRDY;
+    endpoint = (REG_USB_CNTL(number) >> 4) & 0xF;
+    ep = &endpoints[endpoint*2];
+    if (!(REG_USB_CNTL(number) & USB_CNTL_DIR_IN))
+        ep++;  /* RX endpoint is +1 in the array */
+    size = VIRTADDR(REG_USB_ADDR(number)) - ((unsigned int)ep->buf + ep->sent);
+
+    if (number == USB_INTR_DMA_BULKIN) {
+        if ((ep->use_dma == 0) || (size % ep->fifo_size)) {
+            /* DMA is completed, but the final (short) packet needs to
+               be manually initiated! */
+            uint16_t incsr;
+            select_endpoint(endpoint);
+            incsr = REG_USB_INCSR;
+
+            if (ep->use_dma == 1) {
+                /* Switch to Mode 0 DMA */
+                incsr &= ~((USB_INCSRH_AUTOSET | USB_INCSRH_DMAREQENAB) << 8);
+                REG_USB_INCSR = incsr;
+                incsr &= ~((USB_INCSRH_DMAREQMODE) << 8);
+                incsr |= ((USB_INCSRH_DMAREQENAB) << 8);
+            }
+            incsr |= USB_INCSR_INPKTRDY;
+            logf("DMA dangling %x", incsr);
+            REG_USB_INCSR = incsr;
         }
-    }
-    else if(number == USB_INTR_DMA_BULKIN && size % ep->fifo_size)
-    {
-        /* If the last packet is less than MAXP, set INPKTRDY manually */
-        REG_USB_INCSR |= USB_INCSR_INPKTRDY;
-    }
-
-    if (ep)
-    {
-        int dir = EP_IS_IN(ep) ? USB_DIR_IN : USB_DIR_OUT;
-        if ((dir ==  USB_DIR_OUT) || !ep->wait)
-            usb_core_transfer_complete(endpoint, dir, 0, ep->length);
-        ep->rc = 0;
-        ep_transfer_completed(ep);
+        logf("DMA TX%d %d @%d/%d", number, size, ep->sent, ep->length);
+        ep->sent += size;
+        ep->use_dma = -1; /* DMA is complete, mark channel as idle */
+
+        EPIN_complete(endpoint);
+    } else if (number == USB_INTR_DMA_BULKOUT) {
+        /* RX DMA completed */
+        logf("DMA RX%d %d @%d/%d", number, size, ep->received, ep->length);
+        ep->received += size;
+        ep->use_dma = -1;
+
+        EPOUT_handler(endpoint);
+    } else if (ep) {
+        ep->use_dma = -1;
     }
 }
+#endif
 
 static void setup_endpoint(struct usb_endpoint *ep)
 {
@@ -510,14 +622,13 @@ static void setup_endpoint(struct usb_endpoint *ep)
     if(ep->type != ep_control)
         ep->fifo_size = usb_drv_port_speed() ? 512 : 64;
 
+    ep->config = REG_USB_CONFIGDATA;
+
     if(EP_IS_IN(ep))
     {
         csr = (USB_INCSR_FF | USB_INCSR_CDT);
         csrh = USB_INCSRH_MODE;
 
-        if(ep->use_dma)
-            csrh |= (USB_INCSRH_DMAREQENAB | USB_INCSRH_AUTOSET | USB_INCSRH_DMAREQMODE);
-
         if(ep->type == ep_interrupt)
             csrh |= USB_INCSRH_FRCDATATOG;
 
@@ -525,6 +636,8 @@ static void setup_endpoint(struct usb_endpoint *ep)
         REG_USB_INCSR    = csr;
         REG_USB_INCSRH   = csrh;
 
+	logf("IN %d (%x %x %x)", endpoint, ep->fifo_size, csr, csrh);
+
         if (ep->allocated)
             REG_USB_INTRINE |= USB_INTR_EP(EP_NUMBER2(ep));
     }
@@ -536,13 +649,12 @@ static void setup_endpoint(struct usb_endpoint *ep)
         if(ep->type == ep_interrupt)
             csrh |= USB_OUTCSRH_DNYT;
 
-        if(ep->use_dma)
-            csrh |= (USB_OUTCSRH_DMAREQENAB | USB_OUTCSRH_AUTOCLR | USB_OUTCSRH_DMAREQMODE);
-
         REG_USB_OUTMAXP   = ep->fifo_size;
         REG_USB_OUTCSR    = csr;
         REG_USB_OUTCSRH   = csrh;
 
+	logf("OUT %d (%x %x %x)", endpoint, ep->fifo_size, csr, csrh);
+
         if (ep->allocated)
             REG_USB_INTROUTE |= USB_INTR_EP(EP_NUMBER2(ep));
     }
@@ -574,8 +686,8 @@ static void udc_reset(void)
     REG_USB_INTRUSBE = 0;
 
     /* Disable DMA */
-    REG_USB_CNTL(0) = 0;
-    REG_USB_CNTL(1) = 0;
+    REG_USB_CNTL(USB_INTR_DMA_BULKIN) = 0;
+    REG_USB_CNTL(USB_INTR_DMA_BULKOUT) = 0;
 
     /* High speed, softconnect */
     REG_USB_POWER = (USB_POWER_SOFTCONN | USB_POWER_HSENAB);
@@ -584,6 +696,9 @@ static void udc_reset(void)
     select_endpoint(0);
     REG_USB_CSR0 = (USB_CSR0_SVDOUTPKTRDY | USB_CSR0_SVDSETUPEND | USB_CSR0_FLUSHFIFO);
 
+    endpoints[0].config = REG_USB_CONFIGDATA;
+    endpoints[1].config = REG_USB_CONFIGDATA;
+
     if (endpoints[0].busy)
     {
         if (endpoints[0].wait)
@@ -627,17 +742,19 @@ void OTG(void)
     unsigned char    intrUSB = REG_USB_INTRUSB;
     unsigned short   intrIn  = REG_USB_INTRIN;
     unsigned short   intrOut = REG_USB_INTROUT;
+#ifdef USE_USB_DMA
     unsigned char    intrDMA = REG_USB_INTR;
+#endif
 
-    logf("%x %x %x %x", intrUSB, intrIn, intrOut, intrDMA);
+    logf("IRQ %x %x %x %x", intrUSB, intrIn, intrOut, intrDMA);
 
     /* EPIN & EPOUT are all handled in DMA */
     if(intrIn & USB_INTR_EP(0))
         EP0_handler();
     if(intrIn & USB_INTR_EP(1))
-        EPIN_handler(1);
+        EPIN_complete(1);
     if(intrIn & USB_INTR_EP(2))
-        EPIN_handler(2);
+        EPIN_complete(2);
     if(intrOut & USB_INTR_EP(1))
         EPOUT_handler(1);
     if(intrOut & USB_INTR_EP(2))
@@ -648,10 +765,12 @@ void OTG(void)
         logf("USB suspend");
     if(intrUSB & USB_INTR_RESUME)
         logf("USB resume");
-    if(intrDMA & USB_INTR_DMA_BULKIN)
+#ifdef USE_USB_DMA
+    if(intrDMA & (1<<USB_INTR_DMA_BULKIN))
         EPDMA_handler(USB_INTR_DMA_BULKIN);
-    if(intrDMA & USB_INTR_DMA_BULKOUT)
+    if(intrDMA & (1<<USB_INTR_DMA_BULKOUT))
         EPDMA_handler(USB_INTR_DMA_BULKOUT);
+#endif
 }
 
 bool usb_drv_stalled(int endpoint, bool in)
@@ -790,6 +909,10 @@ void usb_drv_exit(void)
 {
     logf("%s()", __func__);
 
+    select_endpoint(1);
+
+    logf("DMA X (%x %x %x %x)", REG_USB_ADDR(USB_INTR_DMA_BULKIN), REG_USB_COUNT(USB_INTR_DMA_BULKIN),REG_USB_CNTL(USB_INTR_DMA_BULKIN), REG_USB_INCSR);
+
     REG_USB_FADDR = 0;
     REG_USB_INDEX = 0;
 
@@ -798,9 +921,11 @@ void usb_drv_exit(void)
     REG_USB_INTROUTE = 0;
     REG_USB_INTRUSBE = 0;
 
+#ifdef USE_USB_DMA
     /* Disable DMA */
-    REG_USB_CNTL(0) = 0;
-    REG_USB_CNTL(1) = 0;
+    REG_USB_CNTL(USB_INTR_DMA_BULKIN) = 0;
+    REG_USB_CNTL(USB_INTR_DMA_BULKOUT) = 0;
+#endif
 
     /* Disconnect from USB */
     REG_USB_POWER &= ~USB_POWER_SOFTCONN;
@@ -817,10 +942,8 @@ static void usb_drv_send_internal(struct usb_endpoint* ep, void* ptr, int length
 {
     int flags = disable_irq_save();
 
-    if(ep->type == ep_control)
-    {
-        if ((ptr == NULL && length == 0) || !ep0_data_requested)
-        {
+    if (ep->type == ep_control) {
+        if ((ptr == NULL && length == 0) || !ep0_data_requested) {
             restore_irq(flags);
             return;
         }
@@ -831,36 +954,22 @@ static void usb_drv_send_internal(struct usb_endpoint* ep, void* ptr, int length
     ep->sent = 0;
     ep->length = length;
     ep->busy = true;
-    if(blocking)
-    {
+    if(blocking) {
         ep->rc = -1;
         ep->wait = true;
+    } else {
+        ep->rc = 0;
     }
-    else ep->rc = 0;
 
-    if(ep->type == ep_control)
-    {
+    if (ep->type == ep_control) {
         EP0_send();
-    }
-    else
-    {
-        if(ep->use_dma)
-        {
-            commit_discard_dcache_range(ptr, length);
-            REG_USB_ADDR(0) = PHYSADDR((unsigned long)ptr);
-            REG_USB_COUNT(0) = length;
-            REG_USB_CNTL(0) = (USB_CNTL_INTR_EN | USB_CNTL_MODE_1 |
-                                 USB_CNTL_DIR_IN  | USB_CNTL_ENA |
-                                 USB_CNTL_EP(EP_NUMBER2(ep)) | USB_CNTL_BURST_16);
-        }
-        else
-            EPIN_handler(EP_NUMBER2(ep));
+    } else {
+        EPIN_send(EP_NUMBER2(ep));
     }
 
     restore_irq(flags);
 
-    if(blocking)
-    {
+    if(blocking) {
         semaphore_wait(&ep->complete, HZ);
         ep->wait = false;
     }
@@ -918,23 +1027,12 @@ int usb_drv_recv(int endpoint, void* ptr, int length)
     ep->received = 0;
     ep->length = length;
     ep->busy = true;
-    if(ep->use_dma)
-    {
-        discard_dcache_range(ptr, length);
-        REG_USB_ADDR(1) = PHYSADDR((unsigned long)ptr);
-        REG_USB_COUNT(1) = length;
-        REG_USB_CNTL(1) = (USB_CNTL_INTR_EN | USB_CNTL_MODE_1 |
-                             USB_CNTL_ENA | USB_CNTL_EP(endpoint) |
-                             USB_CNTL_BURST_16);
-    }
-    else
-    {
-        if (endpoint == EP_CONTROL)
-        {
-            ep0_data_supplied = false;
-            EP0_handler();
-        }
-        else EPOUT_handler(endpoint);
+
+    if (endpoint == EP_CONTROL) {
+        ep0_data_supplied = false;
+        EP0_handler();
+    } else {
+        EPOUT_handler(endpoint);
     }
 
     restore_irq(flags);
@@ -976,6 +1074,12 @@ void usb_drv_cancel_all_transfers(void)
 
     unsigned int i, flags = disable_irq_save();
 
+#ifdef USE_USB_DMA
+    /* Disable DMA */
+    REG_USB_CNTL(USB_INTR_DMA_BULKIN) = 0;
+    REG_USB_CNTL(USB_INTR_DMA_BULKOUT) = 0;
+#endif
+
     for(i=0; i<TOTAL_EP(); i++)
     {
         if (endpoints[i].busy)
@@ -996,6 +1100,7 @@ void usb_drv_cancel_all_transfers(void)
         select_endpoint(i/2);
         flushFIFO(&endpoints[i]);
     }
+
     restore_irq(flags);
 }