usb-drv-as3525v2.c: simplify a lot

code, especially init sequence, is much more similar to usb-s3c6400x.c

git-svn-id: svn://svn.rockbox.org/rockbox/trunk@31231 a1c6a512-1295-4272-9138-f99709370657

3 files changed, 170 insertions, 415 deletions

diff --git a/firmware/export/usb-s3c6400x.h b/firmware/export/usb-s3c6400x.h
index c788cd3..4c5f57e 100644
--- a/firmware/export/usb-s3c6400x.h
+++ b/firmware/export/usb-s3c6400x.h
@@ -326,11 +326,12 @@
 
 /** Device IN Endpoint Common Interrupt Mask Register */
 #define DIEPMSK     (*((uint32_t volatile*)(OTGBASE + 0x810)))
-/* the following apply to DIEPMSK and DIEPINT */
-#define DIEPINT_xfercompl       (1 << 0) /** Transfer complete */
-#define DIEPINT_epdisabled      (1 << 1) /** Endpoint disabled */
-#define DIEPINT_ahberr          (1 << 2) /** AHB error */
+/* the following apply to DEPMSK and DEPINT */
+#define DEPINT_xfercompl       (1 << 0) /** Transfer complete */
+#define DEPINT_epdisabled      (1 << 1) /** Endpoint disabled */
+#define DEPINT_ahberr          (1 << 2) /** AHB error */
 #define DIEPINT_timeout         (1 << 3) /** Timeout handshake (non-iso TX) */
+#define DOEPINT_setup           (1 << 3) /** Setup Phase Done (control EPs)*/
 #define DIEPINT_intktxfemp      (1 << 4) /** IN token received with tx fifo empty */
 #define DIEPINT_intknepmis      (1 << 5) /** IN token received with ep mismatch */
 #define DIEPINT_inepnakeff      (1 << 6) /** IN endpoint NAK effective */
@@ -339,11 +340,6 @@
 
 /** Device OUT Endpoint Common Interrupt Mask Register */
 #define DOEPMSK     (*((uint32_t volatile*)(OTGBASE + 0x814)))
-/* the following apply to DOEPMSK and DOEPINT */
-#define DOEPINT_xfercompl       (1 << 0) /** Transfer complete */
-#define DOEPINT_epdisabled      (1 << 1) /** Endpoint disabled */
-#define DOEPINT_ahberr          (1 << 2) /** AHB error */
-#define DOEPINT_setup           (1 << 3) /** Setup Phase Done (control EPs)*/
 
 /** Device All Endpoints Interrupt Register */
 #define DAINT       (*((uint32_t volatile*)(OTGBASE + 0x818)))
diff --git a/firmware/target/arm/as3525/usb-drv-as3525v2.c b/firmware/target/arm/as3525/usb-drv-as3525v2.c
index 9be6284..3836825 100644
--- a/firmware/target/arm/as3525/usb-drv-as3525v2.c
+++ b/firmware/target/arm/as3525/usb-drv-as3525v2.c
@@ -39,24 +39,6 @@
 static const uint8_t in_ep_list[] = {0, 3, 5};
 static const uint8_t out_ep_list[] = {0, 2, 4};
 
-/* iterate through each in/out ep except EP0
- * 'i' is the counter, 'ep' is the actual value */
-#define FOR_EACH_EP(list, start, i, ep) \
-    for(ep = list[i = start]; \
-        i < (sizeof(list)/sizeof(*list)); \
-        i++, ep = list[i])
-
-#define FOR_EACH_IN_EP_EX(include_ep0, i, ep) \
-    FOR_EACH_EP(in_ep_list, !include_ep0, i, ep)
-
-#define FOR_EACH_OUT_EP_EX(include_ep0, i, ep) \
-    FOR_EACH_EP(out_ep_list, !include_ep0, i, ep)
-
-#define FOR_EACH_IN_EP(i, ep)           FOR_EACH_IN_EP_EX(false, i, ep)
-#define FOR_EACH_IN_EP_AND_EP0(i, ep)   FOR_EACH_IN_EP_EX(true,  i, ep)
-#define FOR_EACH_OUT_EP(i, ep)          FOR_EACH_OUT_EP_EX(false, i, ep)
-#define FOR_EACH_OUT_EP_AND_EP0(i, ep)  FOR_EACH_OUT_EP_EX(true,  i, ep)
-
 /* store per endpoint, per direction, information */
 struct usb_endpoint
 {
@@ -64,7 +46,7 @@ struct usb_endpoint
     struct semaphore complete; /* wait object */
     int8_t status; /* completion status (0 for success) */
     bool active; /* true is endpoint has been requested (true for EP0) */
-    bool wait; /* true if usb thread is blocked on completion */
+    bool done; /* transfer completed */
     bool busy; /* true is a transfer is pending */
 };
 
@@ -107,114 +89,26 @@ static enum ep0state ep0_state;
 
 void usb_attach(void)
 {
-    logf("usb-drv: attach");
+    logf("%s", __func__);
     /* Nothing to do */
 }
 
-static inline void usb_delay(void)
-{
-    register int i = 0;
-    asm volatile(
-        "1: nop             \n"
-        "   add %0, %0, #1  \n"
-        "   cmp %0, #0x300  \n"
-        "   bne 1b          \n"
-        : "+r"(i)
-    );
-}
-
-static void as3525v2_connect(void)
-{
-    logf("usb-drv: init as3525v2");
-    /* 1) enable usb core clock */
-    bitset32(&CGU_PERI, CGU_USB_CLOCK_ENABLE);
-    usb_delay();
-    /* 2) enable usb phy clock */
-    CCU_USB = (CCU_USB & ~(3<<24)) | (1 << 24); /* ?? */
-    /* PHY clock */
-    CGU_USB = 1<<5  /* enable */
-        | 0 << 2
-        | 0; /* source = ? (24MHz crystal?) */
-    usb_delay();
-    /* 3) clear "stop pclk" */
-    PCGCCTL &= ~0x1;
-    usb_delay();
-    /* 4) clear "power clamp" */
-    PCGCCTL &= ~0x4;
-    usb_delay();
-    /* 5) clear "reset power down module" */
-    PCGCCTL &= ~0x8;
-    usb_delay();
-    /* 6) set "power on program done" */
-    DCTL |= DCTL_pwronprgdone;
-    usb_delay();
-    /* 7) core soft reset */
-    GRSTCTL |= GRSTCTL_csftrst;
-    usb_delay();
-    /* 8) hclk soft reset */
-    GRSTCTL |= GRSTCTL_hsftrst;
-    usb_delay();
-    /* 9) flush and reset everything */
-    GRSTCTL |= 0x3f;
-    usb_delay();
-    /* 10) force device mode*/
-    GUSBCFG &= ~GUSBCFG_force_host_mode;
-    GUSBCFG |= GUSBCFG_force_device_mode;
-    usb_delay();
-    /* 11) Do something that is probably CCU related but undocumented*/
-    CCU_USB |= 0x1000;
-    CCU_USB &= ~0x300000;
-    usb_delay();
-    /* 12) reset usb core parameters (dev addr, speed, ...) */
-    DCFG = 0;
-    usb_delay();
-}
-
-static void as3525v2_disconnect(void)
-{
-    /* Disconnect */
-    DCTL |= DCTL_sftdiscon;
-    sleep(HZ/20);
-    /* Disable clock */
-    CGU_USB = 0;
-    usb_delay();
-    bitclr32(&CGU_PERI, CGU_USB_CLOCK_ENABLE);
-}
-
-static void enable_device_interrupts(void)
-{
-    /* Clear any pending interrupt */
-    GINTSTS = 0xffffffff;
-    /* Clear any pending otg interrupt */
-    GOTGINT = 0xffffffff;
-    /* Enable interrupts */
-    GINTMSK = GINTMSK_usbreset
-            | GINTMSK_enumdone
-            | GINTMSK_inepintr
-            | GINTMSK_outepintr
-            | GINTMSK_disconnect
-            | GINTMSK_usbsuspend
-            | GINTMSK_wkupintr
-            | GINTMSK_otgintr;
-}
-
-static void flush_tx_fifos(int nums)
+static void flush_tx_fifos(void)
 {
     unsigned int i = 0;
 
-    GRSTCTL = (nums << GRSTCTL_txfnum_bitp)
-            | GRSTCTL_txfflsh_flush;
-    while(GRSTCTL & GRSTCTL_txfflsh_flush && i < 0x300)
-        i++;
-    if(GRSTCTL & GRSTCTL_txfflsh_flush)
-        panicf("usb-drv: hang of flush tx fifos (%x)", nums);
+    GRSTCTL = (0x10 << GRSTCTL_txfnum_bitp) | GRSTCTL_txfflsh_flush;
+    while(GRSTCTL & GRSTCTL_txfflsh_flush)
+        if (i++ >= 0x300)
+            panicf("usb-drv: hang of flush tx fifos");
+
     /* wait 3 phy clocks */
     udelay(1);
 }
 
 static void prepare_setup_ep0(void)
 {
-    logf("usb-drv: prepare EP0");
+    logf("%s", __func__);
     /* setup DMA */
     DEPDMA(0, true) = (void*)AS3525_PHYSICAL_ADDR(&_ep0_setup_pkt);
 
@@ -253,11 +147,7 @@ static void handle_ep0_complete(bool is_ack)
                 prepare_setup_ep0();
             break;
         case EP0_WAIT_DATA_ACK:
-            /* update state */
-            if(is_ack)
-                ep0_state = EP0_WAIT_DATA;
-            else
-                ep0_state = EP0_WAIT_ACK;
+            ep0_state = is_ack ? EP0_WAIT_DATA : EP0_WAIT_ACK;
             break;
         default:
             panicf("usb-drv: invalid EP0 state");
@@ -284,42 +174,29 @@ static void handle_ep0_setup(void)
 
 static void reset_endpoints(void)
 {
-    unsigned i;
-    int ep;
-    /* disable all endpoints except EP0 */
-    FOR_EACH_IN_EP_AND_EP0(i, ep)
-    {
-        endpoints[ep][DIR_IN].active = false;
-        endpoints[ep][DIR_IN].busy = false;
-        endpoints[ep][DIR_IN].status = -1;
-        if(endpoints[ep][DIR_IN].wait)
+    for (int dir = 0; dir < 2; dir++)
+        for (unsigned i = 0; i < sizeof((dir == DIR_IN) ? in_ep_list : out_ep_list); i++)
         {
-            endpoints[ep][DIR_IN].wait = false;
-            semaphore_release(&endpoints[ep][DIR_IN].complete);
+            int ep = ((dir == DIR_IN) ? in_ep_list : out_ep_list)[i];
+            endpoints[ep][dir == DIR_OUT].active = false;
+            endpoints[ep][dir == DIR_OUT].busy = false;
+            endpoints[ep][dir == DIR_OUT].status = -1;
+            endpoints[ep][dir == DIR_OUT].done = false;
+            semaphore_release(&endpoints[ep][dir == DIR_OUT].complete);
+
+            if (i != 0)
+                DEPCTL(ep, dir == DIR_OUT) = (DEPCTL(ep, dir == DIR_OUT) & DEPCTL_epena)
+                    ? DEPCTL_snak
+                    : 0;
+            else
+                DEPCTL(0, dir == DIR_OUT)  = (DEPCTL_MPS_64 << DEPCTL_mps_bitp) | DEPCTL_usbactep | DEPCTL_snak;
         }
-        DEPCTL(ep, false) = (DEPCTL(ep, false) & DEPCTL_epena) 
-            ? DEPCTL_snak
-            : 0;
-    }
 
-    FOR_EACH_OUT_EP_AND_EP0(i, ep)
-    {
-        endpoints[ep][DIR_OUT].active = false;
-        endpoints[ep][DIR_OUT].busy = false;
-        endpoints[ep][DIR_OUT].status = -1;
-        if(endpoints[ep][DIR_OUT].wait)
-        {
-            endpoints[ep][DIR_OUT].wait = false;
-            semaphore_release(&endpoints[ep][DIR_OUT].complete);
-        }
-        DEPCTL(ep, true) = (DEPCTL(ep, true) & DEPCTL_epena) ? DEPCTL_snak : 0;
-    }
-    /* 64 bytes packet size, active endpoint */
-    DEPCTL(0, true)  = (DEPCTL_MPS_64 << DEPCTL_mps_bitp) | DEPCTL_usbactep | DEPCTL_snak;
-    DEPCTL(0, false) = (DEPCTL_MPS_64 << DEPCTL_mps_bitp) | DEPCTL_usbactep | DEPCTL_snak;
+
     /* Setup next chain for IN eps */
-    FOR_EACH_IN_EP_AND_EP0(i, ep)
+    for (unsigned i = 0; i < sizeof(in_ep_list); i++)
     {
+        int ep = in_ep_list[i];
         int next_ep = in_ep_list[(i + 1) % sizeof(in_ep_list)];
         DEPCTL(ep, false) = (DEPCTL(ep, false) & ~bitm(DEPCTL, nextep)) | (next_ep << DEPCTL_nextep_bitp);
     }
@@ -327,45 +204,61 @@ static void reset_endpoints(void)
 
 static void cancel_all_transfers(bool cancel_ep0)
 {
-    logf("usb-drv: cancel all transfers");
+    logf("%s", __func__);
     int flags = disable_irq_save();
-    int ep;
-    unsigned i;
 
-    FOR_EACH_IN_EP_EX(cancel_ep0, i, ep)
-    {
-        endpoints[ep][DIR_IN].status = -1;
-        endpoints[ep][DIR_IN].busy = false;
-        if(endpoints[ep][DIR_IN].wait)
+    for (int dir = 0; dir < 2; dir++)
+        for (unsigned i = !!cancel_ep0; i < sizeof((dir == DIR_IN) ? in_ep_list : out_ep_list); i++)
         {
-            endpoints[ep][DIR_IN].wait = false;
-            semaphore_release(&endpoints[ep][DIR_IN].complete);
+            int ep = ((dir == DIR_IN) ? in_ep_list : out_ep_list)[i];
+            endpoints[ep][dir == DIR_OUT].status = -1;
+            endpoints[ep][dir == DIR_OUT].busy = false;
+            endpoints[ep][dir == DIR_OUT].done = false;
+            semaphore_release(&endpoints[ep][dir == DIR_OUT].complete);
+            DEPCTL(ep, dir) = (DEPCTL(ep, dir) & ~DEPCTL_usbactep) | DEPCTL_snak;
         }
-        DEPCTL(ep, false) = (DEPCTL(ep, false) & ~DEPCTL_usbactep) | DEPCTL_snak;
-    }
-    FOR_EACH_OUT_EP_EX(cancel_ep0, i, ep)
-    {
-        endpoints[ep][DIR_OUT].status = -1;
-        endpoints[ep][DIR_OUT].busy = false;
-        if(endpoints[ep][DIR_OUT].wait)
-        {
-            endpoints[ep][DIR_OUT].wait = false;
-            semaphore_release(&endpoints[ep][DIR_OUT].complete);
-        }
-        DEPCTL(ep, true) = (DEPCTL(ep, true) & ~DEPCTL_usbactep) | DEPCTL_snak;
-    }
 
     restore_irq(flags);
 }
 
-static void core_dev_init(void)
+void usb_drv_init(void)
 {
-    int ep;
-    unsigned int i;
-    /* Restart the phy clock */
+    for (int i = 0; i < USB_NUM_ENDPOINTS; i++)
+        for (int dir = 0; dir < 2; dir++)
+            semaphore_init(&endpoints[i][dir].complete, 1, 0);
+
+    bitset32(&CGU_PERI, CGU_USB_CLOCK_ENABLE);
+    CCU_USB = (CCU_USB & ~(3<<24)) | (1 << 24); /* ?? */
+    /* PHY clock */
+    CGU_USB = 1<<5  /* enable */
+        | 0 << 2
+        | 0; /* source = ? (24MHz crystal?) */
+
     PCGCCTL = 0;
-    /* Set phy speed : high speed */
-    DCFG = (DCFG & ~bitm(DCFG, devspd)) | DCFG_devspd_hs_phy_hs;
+    DCTL = DCTL_pwronprgdone | DCTL_sftdiscon;
+
+    GRSTCTL = GRSTCTL_csftrst;
+    while (GRSTCTL & GRSTCTL_csftrst);  /* Wait for OTG to ack reset */
+    while (!(GRSTCTL & GRSTCTL_ahbidle));  /* Wait for OTG AHB master idle */
+
+    GRXFSIZ = 512;
+    GNPTXFSIZ = MAKE_FIFOSIZE_DATA(512);
+
+    /* fixme: the current code is for internal DMA only, the clip+ architecture
+     *        define the internal DMA model */
+    /* Set burstlen and enable DMA*/
+    GAHBCFG = (GAHBCFG_INT_DMA_BURST_INCR << GAHBCFG_hburstlen_bitp)
+                | GAHBCFG_dma_enable;
+
+    /* Select UTMI+ 16 */
+    GUSBCFG = GUSBCFG_force_device_mode | GUSBCFG_phy_if | 7 << GUSBCFG_toutcal_bitp;
+
+    /* Do something that is probably CCU related but undocumented*/
+    CCU_USB |= 0x1000;
+    CCU_USB &= ~0x300000;
+
+    DCFG = DCFG_nzstsouthshk | DCFG_devspd_hs_phy_hs; /* Address 0, high speed */
+    DCTL = DCTL_pwronprgdone;
 
     /* Check hardware capabilities */
     if(extract(GHWCFG2, arch) != GHWCFG2_ARCH_INTERNAL_DMA)
@@ -379,140 +272,80 @@ static void core_dev_init(void)
     if(!(GHWCFG4 & GHWCFG4_ded_fifo_en)) /* it seems to be multiple tx fifo support */
         panicf("usb-drv: no multiple tx fifo");
 
-    #ifdef USE_CUSTOM_FIFO_LAYOUT
-    if(!(GHWCFG2 & GHWCFG2_dyn_fifo))
-        panicf("usb-drv: no dynamic fifo");
-    if(GRXFSIZ != DATA_FIFO_DEPTH)
-        panicf("usb-drv: wrong data fifo size");
-    #endif /* USE_CUSTOM_FIFO_LAYOUT */
-
     if(USB_NUM_ENDPOINTS != extract(GHWCFG2, num_ep))
         panicf("usb-drv: wrong endpoint number");
 
-    FOR_EACH_IN_EP_AND_EP0(i, ep)
-    {
-        int type = (GHWCFG1 >> GHWCFG1_epdir_bitp(ep)) & GHWCFG1_epdir_bits;
-        if(type != GHWCFG1_EPDIR_BIDIR && type != GHWCFG1_EPDIR_IN)
-            panicf("usb-drv: EP%d is no IN or BIDIR", ep);
-    }
-    FOR_EACH_OUT_EP_AND_EP0(i, ep)
-    {
-        int type = (GHWCFG1 >> GHWCFG1_epdir_bitp(ep)) & GHWCFG1_epdir_bits;
-        if(type != GHWCFG1_EPDIR_BIDIR && type != GHWCFG1_EPDIR_OUT)
-            panicf("usb-drv: EP%d is no OUT or BIDIR", ep);
-    }
-
-    /* Setup FIFOs */
-    GRXFSIZ = 512;
-    GNPTXFSIZ = MAKE_FIFOSIZE_DATA(512);
+    for (int dir = 0; dir < 2; dir++)
+        for (unsigned i = 0; i < sizeof((dir == DIR_IN) ? in_ep_list : out_ep_list); i++)
+        {
+            int ep = ((dir == DIR_IN) ? in_ep_list : out_ep_list)[i];
+            int type = (GHWCFG1 >> GHWCFG1_epdir_bitp(ep)) & GHWCFG1_epdir_bits;
+            int flag = (dir == DIR_IN) ? GHWCFG1_EPDIR_IN : GHWCFG1_EPDIR_OUT;
+            if(type != GHWCFG1_EPDIR_BIDIR && type != flag)
+                panicf("usb-drv: EP%d not in correct direction", ep);
+        }
 
-    /* Setup interrupt masks for endpoints */
-    /* Setup interrupt masks */
-    DOEPMSK = DOEPINT_setup | DOEPINT_xfercompl | DOEPINT_ahberr;
-    DIEPMSK = DIEPINT_xfercompl | DIEPINT_timeout | DIEPINT_ahberr;
+    DOEPMSK = DEPINT_xfercompl | DEPINT_ahberr | DOEPINT_setup;
+    DIEPMSK = DEPINT_xfercompl | DEPINT_ahberr | DIEPINT_timeout;
     DAINTMSK = 0xffffffff;
 
     reset_endpoints();
 
     prepare_setup_ep0();
 
-    /* enable USB interrupts */
-    enable_device_interrupts();
-}
-
-static void core_init(void)
-{
-    /* Disconnect */
-    DCTL |= DCTL_sftdiscon;
-    /* Select UTMI+ 16 */
-    GUSBCFG |= GUSBCFG_phy_if;
-    GUSBCFG = (GUSBCFG & ~bitm(GUSBCFG, toutcal)) | 7 << GUSBCFG_toutcal_bitp;
-
-    /* fixme: the current code is for internal DMA only, the clip+ architecture
-     *        define the internal DMA model */
-    /* Set burstlen and enable DMA*/
-    GAHBCFG = (GAHBCFG_INT_DMA_BURST_INCR << GAHBCFG_hburstlen_bitp)
-                | GAHBCFG_dma_enable;
-    /* Disable HNP and SRP, not sure it's useful because we already forced dev mode */
-    GUSBCFG &= ~(GUSBCFG_srpcap | GUSBCFG_hnpcapp);
-
-    /* perform device model specific init */
-    core_dev_init();
-
-    /* Reconnect */
-    DCTL &= ~DCTL_sftdiscon;
-}
+    GINTMSK = GINTMSK_usbreset
+            | GINTMSK_enumdone
+            | GINTMSK_inepintr
+            | GINTMSK_outepintr
+            | GINTMSK_disconnect
+            | GINTMSK_usbsuspend
+            | GINTMSK_wkupintr
+            | GINTMSK_otgintr;
 
-static void enable_global_interrupts(void)
-{
     VIC_INT_ENABLE = INTERRUPT_USB;
     GAHBCFG |= GAHBCFG_glblintrmsk;
 }
 
-static void disable_global_interrupts(void)
+void usb_drv_exit(void)
 {
+    logf("%s", __func__);
+
     GAHBCFG &= ~GAHBCFG_glblintrmsk;
     VIC_INT_EN_CLEAR = INTERRUPT_USB;
-}
 
-void usb_drv_init(void)
-{
-    unsigned i, ep;
-    logf("usb_drv_init");
-    /* Boost cpu */
-    cpu_boost(1);
-    /* Enable PHY and clocks (but leave pullups disabled) */
-    as3525v2_connect();
-    logf("usb-drv: synopsis id: %lx", GSNPSID);
-    /* Core init */
-    core_init();
-    FOR_EACH_IN_EP_AND_EP0(i, ep)
-        semaphore_init(&endpoints[ep][DIR_IN].complete, 1, 0);
-    FOR_EACH_OUT_EP_AND_EP0(i, ep)
-        semaphore_init(&endpoints[ep][DIR_OUT].complete, 1, 0);
-    /* Enable global interrupts */
-    enable_global_interrupts();
-}
-
-void usb_drv_exit(void)
-{
-    logf("usb_drv_exit");
+    DCTL = DCTL_pwronprgdone | DCTL_sftdiscon;
+    sleep(HZ/20);
 
-    disable_global_interrupts();
-    as3525v2_disconnect();
-    cpu_boost(0);
+    CGU_USB = 0;
+    bitclr32(&CGU_PERI, CGU_USB_CLOCK_ENABLE);
 }
 
-static void handle_ep_in_int(int ep)
+static void handle_ep_int(int ep, bool out)
 {
-    struct usb_endpoint *endpoint = &endpoints[ep][DIR_IN];
-    unsigned long sts = DEPINT(ep, false);
-    if(sts & DIEPINT_ahberr)
-        panicf("usb-drv: ahb error on EP%d IN", ep);
-    if(sts & DIEPINT_xfercompl)
+    struct usb_endpoint *endpoint = &endpoints[ep][out ? DIR_OUT : DIR_IN];
+    unsigned long sts = DEPINT(ep, out);
+    if(sts & DEPINT_ahberr)
+        panicf("usb-drv: ahb error on EP%d %s", ep, out ? "OUT" : "IN");
+    if(sts & DEPINT_xfercompl)
     {
         if(endpoint->busy)
         {
             endpoint->busy = false;
             endpoint->status = 0;
             /* works even for EP0 */
-            int size = (DEPTSIZ(ep, false) & DEPTSIZ_xfersize_bits);
+            int size = (DEPTSIZ(ep, out) & DEPTSIZ_xfersize_bits);
             int transfered = endpoint->len - size;
-            logf("len=%d reg=%d xfer=%d", endpoint->len, size, transfered);
-            /* handle EP0 state if necessary,
-             * this is a ack if length is 0 */
+            /* handle EP0 state if necessary, this is a ack if length is 0 */
             if(ep == 0)
                 handle_ep0_complete(endpoint->len == 0);
-            endpoint->len = size;
-            usb_core_transfer_complete(ep, USB_DIR_IN, 0, transfered);
-            if(endpoint->wait)
-            {
-                endpoint->wait = false;
-                semaphore_release(&endpoint->complete);
-            }
+            if (!out)
+                endpoint->len = size;
+            usb_core_transfer_complete(ep, out ? USB_DIR_OUT : USB_DIR_IN, 0, transfered);
+            endpoint->done = true;
+            semaphore_release(&endpoint->complete);
         }
     }
-    if(sts & DIEPINT_timeout)
+    if(!out && (sts & DIEPINT_timeout))
     {
         panicf("usb-drv: timeout on EP%d IN", ep);
         if(endpoint->busy)
@@ -522,48 +355,11 @@ static void handle_ep_in_int(int ep)
             /* for safety, act as if no bytes as been transfered */
             endpoint->len = 0;
             usb_core_transfer_complete(ep, USB_DIR_IN, 1, 0);
-            if(endpoint->wait)
-            {
-                endpoint->wait = false;
-                semaphore_release(&endpoint->complete);
-            }
+            endpoint->done = true;
+            semaphore_release(&endpoint->complete);
         }
     }
-    /* clear interrupts */
-    DEPINT(ep, false) = sts;
-}
-
-static void handle_ep_out_int(int ep)
-{
-    struct usb_endpoint *endpoint = &endpoints[ep][DIR_OUT];
-    unsigned long sts = DEPINT(ep, true);
-    if(sts & DOEPINT_ahberr)
-        panicf("usb-drv: ahb error on EP%d OUT", ep);
-    if(sts & DOEPINT_xfercompl)
-    {
-        logf("usb-drv: xfer complete on EP%d OUT", ep);
-        if(endpoint->busy)
-        {
-            endpoint->busy = false;
-            endpoint->status = 0;
-            /* works even for EP0 */
-            int transfered = endpoint->len - (DEPTSIZ(ep, true) & DEPTSIZ_xfersize_bits);
-            logf("len=%d reg=%ld xfer=%d", endpoint->len,
-                (DEPTSIZ(ep, true) & DEPTSIZ_xfersize_bits),
-                transfered);
-            /* handle EP0 state if necessary,
-             * this is a ack if length is 0 */
-            if(ep == 0)
-                handle_ep0_complete(endpoint->len == 0);
-            usb_core_transfer_complete(ep, USB_DIR_OUT, 0, transfered);
-            if(endpoint->wait)
-            {
-                endpoint->wait = false;
-                semaphore_release(&endpoint->complete);
-            }
-        }
-    }
-    if(sts & DOEPINT_setup)
+    if(out && (sts & DOEPINT_setup))
     {
         logf("usb-drv: setup on EP%d OUT", ep);
         if(ep != 0)
@@ -588,24 +384,24 @@ static void handle_ep_out_int(int ep)
             usb_core_control_request(ep0_setup_pkt);
         }
     }
+
     /* clear interrupts */
-    DEPINT(ep, true) = sts;
+    DEPINT(ep, out) = sts;
 }
 
 static void handle_ep_ints(void)
 {
     logf("usb-drv: ep int");
-    /* we must read it */
-    unsigned long daint = DAINT;
-    unsigned i, ep;
 
-    FOR_EACH_IN_EP_AND_EP0(i, ep)
-        if(daint & DAINT_IN_EP(ep))
-            handle_ep_in_int(ep);
+    unsigned long daint = DAINT;
 
-    FOR_EACH_OUT_EP_AND_EP0(i, ep)
-        if(daint & DAINT_OUT_EP(ep))
-            handle_ep_out_int(ep);
+    for (int i = 0; i < USB_NUM_ENDPOINTS; i++)
+    {
+        if (daint & DAINT_IN_EP(i))
+            handle_ep_int(i, false);
+        if (daint & DAINT_OUT_EP(i))
+            handle_ep_int(i, true);
+    }
 
     /* write back to clear status */
     DAINT = daint;
@@ -625,8 +421,7 @@ void INT_USB(void)
         /* Clear the Remote Wakeup Signalling */
         DCTL &= ~DCTL_rmtwkupsig;
 
-        /* Flush FIFOs */
-        flush_tx_fifos(0x10);
+        flush_tx_fifos();
 
         /* Flush the Learning Queue */
         GRSTCTL = GRSTCTL_intknqflsh;
@@ -641,15 +436,7 @@ void INT_USB(void)
     }
 
     if(sts & GINTMSK_enumdone)
-    {
-        logf("usb-drv: enum done");
-
-        /* read speed */
-        if(usb_drv_port_speed())
-            logf("usb-drv: HS");
-        else
-            logf("usb-drv: FS");
-    }
+        logf("usb-drv: enum done: speed %cS", usb_drv_port_speed() ? 'H' : 'F');
 
     if(sts & GINTMSK_otgintr)
     {
@@ -658,14 +445,10 @@ void INT_USB(void)
     }
 
     if(sts & (GINTMSK_outepintr | GINTMSK_inepintr))
-    {
         handle_ep_ints();
-    }
 
     if(sts & GINTMSK_disconnect)
-    {
         cancel_all_transfers(true);
-    }
 
     GINTSTS = sts;
 }
@@ -701,45 +484,28 @@ static unsigned long usb_drv_mps_by_type(int type)
 
 int usb_drv_request_endpoint(int type, int dir)
 {
-    int ep, ret = -1;
-    unsigned i;
     logf("usb-drv: request endpoint (type=%d,dir=%s)", type, dir == USB_DIR_IN ? "IN" : "OUT");
 
-    if(dir == USB_DIR_IN)
-        FOR_EACH_IN_EP(i, ep)
-        {
-            if(endpoints[ep][DIR_IN].active)
-                continue;
-            endpoints[ep][DIR_IN].active = true;
-            ret = ep | dir;
-            break;
-        }
-    else
-        FOR_EACH_OUT_EP(i, ep)
-        {
-            if(endpoints[ep][DIR_OUT].active)
-                continue;
-            endpoints[ep][DIR_OUT].active = true;
-            ret = ep | dir;
-            break;
-        }
-
-    if(ret == -1)
+    for (unsigned i = 1; i < sizeof((dir == USB_DIR_IN) ? in_ep_list : out_ep_list); i++)
     {
-        logf("usb-drv: request failed");
-        return -1;
+        int ep = ((dir == USB_DIR_IN) ? in_ep_list : out_ep_list)[i];
+        bool *active = &endpoints[ep][(dir == USB_DIR_IN) ? DIR_IN : DIR_OUT].active;
+        if(*active)
+            continue;
+        *active = true;
+        DEPCTL(ep, dir != USB_DIR_IN) = (DEPCTL(ep, dir != USB_DIR_IN) & ~(bitm(DEPCTL, eptype) | bitm(DEPCTL, mps)))
+            | DEPCTL_setd0pid | (type << DEPCTL_eptype_bitp)
+            | (usb_drv_mps_by_type(type) << DEPCTL_mps_bitp) | DEPCTL_usbactep | DEPCTL_snak;
+        return ep | dir;
     }
 
-    DEPCTL(ep, dir != USB_DIR_IN) = (DEPCTL(ep, true) & ~(bitm(DEPCTL, eptype) | bitm(DEPCTL, mps)))
-        | DEPCTL_setd0pid | (type << DEPCTL_eptype_bitp)
-        | (usb_drv_mps_by_type(type) << DEPCTL_mps_bitp) | DEPCTL_usbactep | DEPCTL_snak;
-
-    return ret;
+    logf("usb-drv: request failed");
+    return -1;
 }
 
 void usb_drv_release_endpoint(int ep)
 {
-    logf("usb-drv: release EP%d %s", EP_NUM(ep), EP_DIR(ep) == DIR_IN ? "IN" : "OUT");
+    logf("usb-drv: release EP%d %s", EP_NUM(ep), EP_DIR(ep) == USB_DIR_IN ? "IN" : "OUT");
     endpoints[EP_NUM(ep)][EP_DIR(ep)].active = false;
 }
 
@@ -748,23 +514,18 @@ void usb_drv_cancel_all_transfers()
     cancel_all_transfers(false);
 }
 
-static void usb_drv_transfer(int ep, void *ptr, int len, bool dir_in, bool blocking)
+static void usb_drv_transfer(int ep, void *ptr, int len, bool dir_in)
 {
     ep = EP_NUM(ep);
     struct usb_endpoint *endpoint = &endpoints[ep][dir_in];
 
-    logf("usb-drv: xfer EP%d, len=%d, dir_in=%d, blocking=%d", ep,
-        len, dir_in, blocking);
+    logf("usb-drv: xfer EP%d, len=%d, dir_in=%d, busy=%d", ep, len, dir_in, endpoint->busy);
 
     /* disable interrupts to avoid any race */
     int oldlevel = disable_irq_save();
     
-    if(endpoint->busy)
-        logf("usb-drv: EP%d %s is already busy", ep, dir_in ? "IN" : "OUT");
-
     endpoint->busy = true;
     endpoint->len = len;
-    endpoint->wait = blocking;
     endpoint->status = -1;
 
     DEPCTL(ep, !dir_in) = (DEPCTL(ep, !dir_in) & ~DEPCTL_stall) | DEPCTL_usbactep;
@@ -772,23 +533,19 @@ static void usb_drv_transfer(int ep, void *ptr, int len, bool dir_in, bool block
     int type = (DEPCTL(ep, !dir_in) >> DEPCTL_eptype_bitp) & DEPCTL_eptype_bits;
     int mps = usb_drv_mps_by_type(type);
     int nb_packets = (len + mps - 1) / mps;
-
-    if(len == 0)
-    {
-        DEPDMA(ep, !dir_in) = (void*)0x10000000;
-        DEPTSIZ(ep, !dir_in) = 1 << DEPTSIZ_pkcnt_bitp;
-    }
+    if (nb_packets == 0)
+        nb_packets = 1;
+
+    DEPDMA(ep, !dir_in) = len
+        ? (void*)AS3525_PHYSICAL_ADDR(ptr)
+        : (void*)0x10000000;
+    DEPTSIZ(ep, !dir_in) = (nb_packets << DEPTSIZ_pkcnt_bitp) | len;
+    if(dir_in)
+        clean_dcache_range(ptr, len);
     else
-    {
-        DEPDMA(ep, !dir_in) = (void*)AS3525_PHYSICAL_ADDR(ptr);
-        DEPTSIZ(ep, !dir_in) = (nb_packets << DEPTSIZ_pkcnt_bitp) | len;
-        if(dir_in)
-            clean_dcache_range(ptr, len);
-        else
-            dump_dcache_range(ptr, len);
-    }
+        dump_dcache_range(ptr, len);
 
-    logf("pkt=%d dma=%lx", nb_packets, DEPDMA);
+    logf("pkt=%d dma=%lx", nb_packets, DEPDMA(ep, !dir_in));
 
     DEPCTL(ep, !dir_in) |= DEPCTL_epena | DEPCTL_cnak;
 
@@ -797,21 +554,23 @@ static void usb_drv_transfer(int ep, void *ptr, int len, bool dir_in, bool block
 
 int usb_drv_recv(int ep, void *ptr, int len)
 {
-    usb_drv_transfer(ep, ptr, len, false, false);
+    usb_drv_transfer(ep, ptr, len, false);
     return 0;
 }
 
 int usb_drv_send(int ep, void *ptr, int len)
 {
-    usb_drv_transfer(ep, ptr, len, true, true);
     struct usb_endpoint *endpoint = &endpoints[ep][1];
-    semaphore_wait(&endpoint->complete, TIMEOUT_BLOCK);
+    endpoint->done = false;
+    usb_drv_transfer(ep, ptr, len, true);
+    while (endpoint->busy && !endpoint->done)
+        semaphore_wait(&endpoint->complete, TIMEOUT_BLOCK);
     return endpoint->status;
 }
 
 int usb_drv_send_nonblocking(int ep, void *ptr, int len)
 {
-    usb_drv_transfer(ep, ptr, len, true, false);
+    usb_drv_transfer(ep, ptr, len, true);
     return 0;
 }
 
diff --git a/firmware/target/arm/usb-s3c6400x.c b/firmware/target/arm/usb-s3c6400x.c
index ea250d1..66699ba 100644
--- a/firmware/target/arm/usb-s3c6400x.c
+++ b/firmware/target/arm/usb-s3c6400x.c
@@ -155,8 +155,8 @@ static void usb_reset(void)
 
     DCFG = DCFG_nzstsouthshk;  /* Address 0 */
     DCTL = DCTL_pwronprgdone;  /* Soft Reconnect */
-    DIEPMSK = DIEPINT_timeout | DIEPINT_ahberr | DIEPINT_xfercompl;
-    DOEPMSK = DIEPINT_timeout | DIEPINT_ahberr | DIEPINT_xfercompl;
+    DIEPMSK = DIEPINT_timeout | DEPINT_ahberr | DEPINT_xfercompl;
+    DOEPMSK = DOEPINT_setup | DEPINT_ahberr | DEPINT_xfercompl;
     DAINTMSK = 0xFFFFFFFF;  /* Enable interrupts on all endpoints */
     GINTMSK = GINTMSK_outepintr | GINTMSK_inepintr | GINTMSK_usbreset | GINTMSK_enumdone;
 
@@ -172,7 +172,7 @@ static void handle_ep_int(int out)
         if (!epints)
             continue;
 
-        if (epints & DIEPINT_xfercompl)
+        if (epints & DEPINT_xfercompl)
         {
             invalidate_dcache();
             int bytes = endpoints[ep].size - (DEPTSIZ(ep, out) & (DEPTSIZ_xfersize_bits < DEPTSIZ_xfersize_bitp));
@@ -185,7 +185,7 @@ static void handle_ep_int(int out)
                 semaphore_release(&endpoints[ep].complete);
             }
         }
-        if (epints & DIEPINT_ahberr)
+        if (epints & DEPINT_ahberr)
             panicf("USB: AHB error on EP%d (dir %d)", ep, out);
         if (epints & DIEPINT_timeout)
         {
@@ -200,7 +200,7 @@ static void handle_ep_int(int out)
                 }
             }
             else
-            {   /* SETUP phase done */
+            {   /* DOEPINT_setup */
                 invalidate_dcache();
                 if (ep == 0)
                 {