Index: firmware/target/arm/usb-drv-arc.c
===================================================================
--- firmware/target/arm/usb-drv-arc.c	(revision 24365)
+++ firmware/target/arm/usb-drv-arc.c	(working copy)
@@ -30,7 +30,7 @@
 #include "panic.h"
 #include "usb_drv.h"
 
-/*#define LOGF_ENABLE*/
+#define LOGF_ENABLE
 #include "logf.h"
 
 /* USB device mode registers (Little Endian) */
@@ -270,19 +270,17 @@
 #define OTGSC_B_SESSION_VALID                  (0x00000800)
 #define OTGSC_A_VBUS_VALID                     (0x00000200)
 
-#define QH_MULT_POS                            (30)
+#define QH_MULT_OFFEST                         (30)
 #define QH_ZLT_SEL                             (0x20000000)
-#define QH_MAX_PKT_LEN_POS                     (16)
+#define QH_MAX_PKT_LEN_OFFSET                  (16)
 #define QH_IOS                                 (0x00008000)
 #define QH_NEXT_TERMINATE                      (0x00000001)
 #define QH_IOC                                 (0x00008000)
 #define QH_MULTO                               (0x00000C00)
-#define QH_STATUS_HALT                         (0x00000040)
-#define QH_STATUS_ACTIVE                       (0x00000080)
+//#define QH_STATUS_HALT                         (0x00000040)
+//#define QH_STATUS_ACTIVE                       (0x00000080)
 #define EP_QUEUE_CURRENT_OFFSET_MASK         (0x00000FFF)
 #define EP_QUEUE_HEAD_NEXT_POINTER_MASK      (0xFFFFFFE0)
-#define EP_QUEUE_FRINDEX_MASK                (0x000007FF)
-#define EP_MAX_LENGTH_TRANSFER               (0x4000)
 
 #define DTD_NEXT_TERMINATE                   (0x00000001)
 #define DTD_IOC                              (0x00008000)
@@ -292,22 +290,28 @@
 #define DTD_STATUS_TRANSACTION_ERR           (0x00000008)
 #define DTD_RESERVED_FIELDS                  (0x80007300)
 #define DTD_ADDR_MASK                        (0xFFFFFFE0)
-#define DTD_PACKET_SIZE                      (0x7FFF0000)
-#define DTD_LENGTH_BIT_POS                   (16)
+#define DTD_STATUS_MASK                      (0x000000FF)
+#define DTD_TOTAL_BYTES_MASK                 (0x7FFF0000)
+#define DTD_TOTAL_BYTES_OFFSET               (16)
+#define DTD_TOTAL_BYTES_MAX_VALUE            (0x4000)
+#define DTD_FRAME_NUMBER_MASK                (0x000007FF)
+#define DTD_BUFFER_POINTER_MASK              (0xFFFFF000)
 #define DTD_ERROR_MASK                       (DTD_STATUS_HALTED | \
                                                DTD_STATUS_DATA_BUFF_ERR | \
                                                DTD_STATUS_TRANSACTION_ERR)
 
 #define DTD_RESERVED_LENGTH_MASK             0x0001ffff
 #define DTD_RESERVED_IN_USE                  0x80000000
-#define DTD_RESERVED_PIPE_MASK               0x0ff00000
-#define DTD_RESERVED_PIPE_OFFSET             20
 /*-------------------------------------------------------------------------*/
 
 /* 4 transfer descriptors per endpoint allow 64k transfers, which is the usual MSC
    transfer size, so it seems like a good size */
-#define NUM_TDS_PER_EP 4
+#define NUM_TDS_PER_PIPE 4
 
+#define USB_NUM_PIPES               (USB_NUM_ENDPOINTS*2)
+#define PIPE_GET(endpoint, is_send) (EP_NUM((endpoint))*2+(is_send?1:0))
+#define PIPE_TD_GET(pipe)           (pipe*NUM_TDS_PER_PIPE)
+
 typedef struct usb_endpoint
 {
     bool allocated[2];
@@ -327,10 +331,10 @@
     unsigned int buff_ptr2;             /* Buffer pointer Page 2 */
     unsigned int buff_ptr3;             /* Buffer pointer Page 3 */
     unsigned int buff_ptr4;             /* Buffer pointer Page 4 */
-    unsigned int reserved;
+    unsigned int reserved;              /* Used by software to hold length */
 } __attribute__ ((packed));
 
-static struct transfer_descriptor td_array[USB_NUM_ENDPOINTS*2*NUM_TDS_PER_EP]
+static struct transfer_descriptor td_array[PIPE_TD_GET(USB_NUM_PIPES)]
     USB_DEVBSS_ATTR __attribute__((aligned(32)));
 
 /* manual: 32.13.1 Endpoint Queue Head (dQH) */
@@ -340,18 +344,21 @@
     unsigned int curr_dtd_ptr;      /* Current dTD Pointer(31-5) */
     struct transfer_descriptor dtd; /* dTD overlay */
     unsigned int setup_buffer[2];   /* Setup data 8 bytes */
-    unsigned int reserved;          /* for software use, pointer to the first TD */
-    unsigned int status;            /* for software use, status of chain in progress */
-    unsigned int length;            /* for software use, transfered bytes of chain in progress */
-    unsigned int wait;              /* for softwate use, indicates if the transfer is blocking */
+    /* software use part */
+    unsigned int head;              /* pointer to the first TD */
+    unsigned int tail;              /* pointer to the last TD */
+    unsigned int xfer;              /* pointer to the first TD in xfer */
+    unsigned int status;            /* status of chain in progress */
+    unsigned int wait;              /* indicates if the transfer is blocking */
 } __attribute__((packed));
 
-static struct queue_head qh_array[USB_NUM_ENDPOINTS*2]
+static struct queue_head qh_array[USB_NUM_PIPES]
     USB_QHARRAY_ATTR;
 
-static struct wakeup transfer_completion_signal[USB_NUM_ENDPOINTS*2]
+static struct wakeup transfer_completion_signal[USB_NUM_PIPES]
     SHAREDBSS_ATTR;
 
+/* even places - receive. odd - transmit */
 static const unsigned int pipe2mask[] = {
     0x01, 0x010000,
     0x02, 0x020000,
@@ -361,11 +368,11 @@
 };
 
 /*-------------------------------------------------------------------------*/
-static void transfer_completed(void);
+static int transfer_completed(void);
 static void control_received(void);
 static int prime_transfer(int ep_num, void* ptr, int len, bool send, bool wait);
-static void prepare_td(struct transfer_descriptor* td,
-        struct transfer_descriptor* previous_td, void *ptr, int len,int pipe);
+static void prepare_td(unsigned int td, unsigned int previous_td, void *ptr,
+        int len);
 static void bus_reset(void);
 static void init_control_queue_heads(void);
 static void init_bulk_queue_heads(void);
@@ -373,6 +380,7 @@
 /*-------------------------------------------------------------------------*/
 static void usb_drv_stop(void)
 {
+    logf("usb_drv_stop()");
     /* disable interrupts */
     REG_USBINTR = 0;
     /* stop usb controller (disconnect) */
@@ -381,6 +389,7 @@
 
 void usb_drv_reset(void)
 {
+    logf("usb_drv_reset()");
     int oldlevel = disable_irq_save();
     REG_USBCMD &= ~USBCMD_RUN;
     restore_irq(oldlevel);
@@ -421,9 +430,10 @@
 /* One-time driver startup init */
 void usb_drv_startup(void)
 {
+    logf("usb_drv_startup()");
     /* Initialize all the signal objects once */
     int i;
-    for(i=0;i<USB_NUM_ENDPOINTS*2;i++) {
+    for(i=0;i<USB_NUM_PIPES;i++) {
         wakeup_init(&transfer_completion_signal[i]);
     }
 }
@@ -431,6 +441,8 @@
 /* manual: 32.14.1 Device Controller Initialization */
 static void _usb_drv_init(bool attach)
 {
+//    int i;
+    logf("usb_drv_init(attach=%d)", attach);
     usb_drv_reset();
 
     REG_USBMODE = USBMODE_CTRL_MODE_DEVICE;
@@ -442,7 +454,7 @@
 #endif
 
     init_control_queue_heads();
-    memset(td_array, 0, sizeof td_array);
+    memset(td_array, 0, sizeof td_array); /* redundant */
 
     REG_ENDPOINTLISTADDR = (unsigned int)qh_array;
     REG_DEVICEADDR = 0;
@@ -498,6 +510,7 @@
 
 void usb_drv_init(void)
 {
+    logf("usb_drv_init()");
     _usb_drv_init(false);
 }
 
@@ -511,6 +524,7 @@
 
 void usb_drv_exit(void)
 {
+    logf("usb_drv_exit()");
     usb_drv_stop();
 
     /* TODO : is one of these needed to save power ?
@@ -526,7 +540,8 @@
     unsigned int usbintr = REG_USBINTR; /* Only watch enabled ints */
     unsigned int status = REG_USBSTS & usbintr;
 
-#if 0
+    logf("usb_drv_int()");
+#if 1
     if (status & USBSTS_INT) logf("int: usb ioc");
     if (status & USBSTS_ERR) logf("int: usb err");
     if (status & USBSTS_PORT_CHANGE) logf("int: portchange");
@@ -544,6 +559,14 @@
 
         if (REG_ENDPTCOMPLETE)
             transfer_completed();
+#if 0
+        if (REG_ENDPTCOMPLETE && transfer_completed())
+        {
+            bus_reset();
+            usb_core_bus_reset(); /* tell mom */
+            return;
+        }
+#endif
     }
 
     /* error interrupt */
@@ -576,6 +599,7 @@
 
 bool usb_drv_stalled(int endpoint,bool in)
 {
+    logf("usb_drv_stalled(endpoint=%d, in=%d)", endpoint, in);
     if(in) {
         return ((REG_ENDPTCTRL(EP_NUM(endpoint)) & EPCTRL_TX_EP_STALL)!=0);
     }
@@ -588,7 +612,7 @@
 {
     int ep_num = EP_NUM(endpoint);
 
-    logf("%sstall %d", stall ? "" : "un", ep_num);
+    logf("usb_drv_stall(endpoint=%d, stall=%d, in=%d)", endpoint, stall, in);
 
     if(in) {
         if (stall) {
@@ -610,27 +634,31 @@
 
 int usb_drv_send_nonblocking(int endpoint, void* ptr, int length)
 {
+    logf("usb_drv_send_nonblocking(endpoint=0x%x, length=%d)", endpoint, length);
     return prime_transfer(EP_NUM(endpoint), ptr, length, true, false);
 }
 
 int usb_drv_send(int endpoint, void* ptr, int length)
 {
+    logf("usb_drv_send(endpoint=0x%x, length=%d)", endpoint, length);
     return prime_transfer(EP_NUM(endpoint), ptr, length, true, true);
 }
 
 int usb_drv_recv(int endpoint, void* ptr, int length)
 {
-    //logf("usbrecv(%x, %d)", ptr, length);
+    logf("usb_drv_recv(endpoint=0x%x, length=%d)", endpoint, length);
     return prime_transfer(EP_NUM(endpoint), ptr, length, false, false);
 }
 
 int usb_drv_port_speed(void)
 {
-    return (REG_PORTSC1 & 0x08000000) ? 1 : 0;
+    logf("usb_drv_port_speed()");
+    return (REG_PORTSC1 & PORTSCX_PORT_SPEED_HIGH) ? 1 : 0;
 }
 
 bool usb_drv_connected(void)
 {
+    logf("usb_drv_connected()");
     return (REG_PORTSC1 &
         (PORTSCX_PORT_SUSPEND | PORTSCX_CURRENT_CONNECT_STATUS))
             == PORTSCX_CURRENT_CONNECT_STATUS;
@@ -638,12 +666,14 @@
 
 bool usb_drv_powered(void)
 {
+    logf("usb_drv_powered()");
     /* true = bus 4V4 ok */
     return (REG_OTGSC & OTGSC_A_VBUS_VALID) ? true : false;
 }
 
 void usb_drv_set_address(int address)
 {
+    logf("usb_drv_set_address(address=%d)", address);
     REG_DEVICEADDR = address << USBDEVICEADDRESS_BIT_POS;
     init_bulk_queue_heads();
     init_endpoints();
@@ -651,14 +681,16 @@
 
 void usb_drv_reset_endpoint(int endpoint, bool send)
 {
-    int pipe = EP_NUM(endpoint) * 2 + (send ? 1 : 0);
+    int pipe = PIPE_GET(endpoint, send);
     unsigned int mask = pipe2mask[pipe];
+    logf("usb_drv_reset_endpoint(endpoint=%d, send=%d)", endpoint, send);
     REG_ENDPTFLUSH = mask;
     while (REG_ENDPTFLUSH & mask);
 }
 
 void usb_drv_set_test_mode(int mode)
 {
+    logf("usb_drv_set_test_mode(mode=%d)", mode);
     switch(mode){
         case 0:
             REG_PORTSC1 &= ~PORTSCX_PORT_TEST_CTRL;
@@ -685,64 +717,104 @@
 
 /*-------------------------------------------------------------------------*/
 
+static inline unsigned int next_td_get(struct queue_head* qh, unsigned int cur_td)
+{
+    if (cur_td == qh->tail)
+        return qh->head;
+    else
+        return cur_td + sizeof(struct transfer_descriptor);
+}
+
 /* manual: 32.14.5.2 */
 static int prime_transfer(int ep_num, void* ptr, int len, bool send, bool wait)
 {
     int rc = 0;
-    int pipe = ep_num * 2 + (send ? 1 : 0);
+    int pipe = PIPE_GET(ep_num, send);
     unsigned int mask = pipe2mask[pipe];
     struct queue_head* qh = &qh_array[pipe];
     static long last_tick;
-    struct transfer_descriptor *new_td, *cur_td, *prev_td;
+    unsigned int new_td;
+    unsigned int cur_td;
+    unsigned int prev_td;
 
     int oldlevel = disable_irq_save();
-/*
-    if (send && ep_num > EP_CONTROL) {
-        logf("usb: sent %d bytes", len);
+
+    logf("prime_transfer(ep_num=%d, len=%d, send=%d, wait=%d)", ep_num, len, send, wait);
+#ifdef LOGF_ENABLE
+    if (ep_num > EP_CONTROL) {
+        logf("usb: %s %d bytes", send ? "sending" : "receiving", len);
     }
-*/
+#endif
+
     qh->status = 0;
     qh->wait = wait;
 
-    new_td=&td_array[pipe*NUM_TDS_PER_EP];
-    cur_td=new_td;
-    prev_td=0;
-    int tdlen;
+    /* initialize prev_td and new_td according to qh state */
+    if (qh->curr_dtd_ptr == DTD_NEXT_TERMINATE)
+    {
+        /* currently not transferring anything */
+        prev_td=0;
+        new_td=(unsigned int)&td_array[PIPE_TD_GET(pipe)]; /* first in array */
+        qh->xfer = new_td; /* mark first item in xfer */
+    }
+    else
+    {
+        /* transfer in progress - find last array item in use */
+        for (prev_td = qh->curr_dtd_ptr;
+                ((struct transfer_descriptor *)prev_td)->next_td_ptr !=
+                    DTD_NEXT_TERMINATE;
+                prev_td = (unsigned int)
+                    ((struct transfer_descriptor *)prev_td)->next_td_ptr);
 
-    do
+        new_td = next_td_get(qh, prev_td);
+    }
+
+    /* add current transfer to td array */
+    cur_td = new_td;
+    while (len > 0)
     {
-        tdlen=MIN(len,16384);
-        prepare_td(cur_td, prev_td, ptr, tdlen,pipe);
+        int tdlen=MIN(len,16384);
+        //logf("preparing td. tdlen %d", tdlen);
+        prepare_td(cur_td, prev_td, ptr, tdlen);
         ptr+=tdlen;
+        len-=tdlen;
         prev_td=cur_td;
-        cur_td++;
-        len-=tdlen;
+        cur_td = next_td_get(qh, cur_td);
+        if (cur_td == qh->curr_dtd_ptr)
+        {
+            logf("not enough space to queue transfer");
+            rc = -1;
+            goto pt_error;
+        }
     }
-    while(len>0);
-    //logf("starting ep %d %s",ep_num,send?"send":"receive");
 
-    qh->dtd.next_td_ptr = (unsigned int)new_td;
-    qh->dtd.size_ioc_sts &= ~(QH_STATUS_HALT | QH_STATUS_ACTIVE);
+    logf("starting ep %d %s",ep_num,send?"send":"receive");
 
+    if (qh->curr_dtd_ptr == DTD_NEXT_TERMINATE)
+    {
+        qh->dtd.next_td_ptr = new_td & DTD_ADDR_MASK;
+        qh->dtd.size_ioc_sts &= ~(DTD_STATUS_HALTED | DTD_STATUS_ACTIVE);
+    }
+
     REG_ENDPTPRIME |= mask;
 
     if(ep_num == EP_CONTROL && (REG_ENDPTSETUPSTAT & EPSETUP_STATUS_EP0)) {
         /* 32.14.3.2.2 */
         logf("new setup arrived");
-        rc = -4;
+        rc = -2;
         goto pt_error;
     }
 
     last_tick = current_tick;
     while ((REG_ENDPTPRIME & mask)) {
         if (REG_USBSTS & USBSTS_RESET) {
-            rc = -1;
+            rc = -3;
             goto pt_error;
         }
 
         if (TIME_AFTER(current_tick, last_tick + HZ/4)) {
             logf("prime timeout");
-            rc = -2;
+            rc = -4;
             goto pt_error;
         }
     }
@@ -750,19 +822,21 @@
     if (!(REG_ENDPTSTATUS & mask)) {
         if(REG_ENDPTCOMPLETE & mask)
         {
-            logf("endpoint completed fast! %d %d %x", ep_num, pipe, qh->dtd.size_ioc_sts & 0xff);
+            logf("endpoint completed fast! ep %d pipe %d status %x", ep_num,
+                    pipe, qh->dtd.size_ioc_sts & DTD_STATUS_MASK);
         }
         else
         {
-            logf("no prime! %d %d %x", ep_num, pipe, qh->dtd.size_ioc_sts & 0xff);
-            rc = -3;
+            logf("no prime! ep %d pipe %d status %x", ep_num, pipe,
+                    qh->dtd.size_ioc_sts & DTD_STATUS_MASK);
+            rc = -6;
             goto pt_error;
         }
     }
     if(ep_num == EP_CONTROL && (REG_ENDPTSETUPSTAT & EPSETUP_STATUS_EP0)) {
         /* 32.14.3.2.2 */
         logf("new setup arrived");
-        rc = -4;
+        rc = -7;
         goto pt_error;
     }
 
@@ -774,7 +848,7 @@
         if(qh->status!=0) {
             /* No need to cancel wait here since it was done and the signal
              * came. */
-            return -5;
+            return -8;
         }
         //logf("all tds done");
     }
@@ -801,8 +875,9 @@
     REG_ENDPTFLUSH = ~0;
     while (REG_ENDPTFLUSH);
 
+    logf("usb_drv_cancel_all_transfers()");
     memset(td_array, 0, sizeof td_array);
-    for(i=0;i<USB_NUM_ENDPOINTS*2;i++) {
+    for(i=0;i<USB_NUM_PIPES;i++) {
         if(qh_array[i].wait) {
             qh_array[i].wait=0;
             qh_array[i].status=DTD_STATUS_HALTED;
@@ -816,6 +891,7 @@
     int ep_num, ep_dir;
     short ep_type;
 
+    logf("usb_drv_request_endpoint(type=%d, dir=%d)", type, dir);
     /* Safety */
     ep_dir = EP_DIR(dir);
     ep_type = type & USB_ENDPOINT_XFERTYPE_MASK;
@@ -852,28 +928,30 @@
     int ep_num = EP_NUM(ep);
     int ep_dir = EP_DIR(ep);
 
+    logf("usb_drv_release_endpoint(ep=0x%x)", ep);
     log_ep(ep_num, ep_dir, "rel");
     endpoints[ep_num].allocated[ep_dir] = 0;
 }
 
 
-static void prepare_td(struct transfer_descriptor* td,
-                       struct transfer_descriptor* previous_td,
-                       void *ptr, int len,int pipe)
+static void prepare_td(unsigned int cur_td, unsigned int prev_td, void *ptr,
+        int len)
 {
+    struct transfer_descriptor *td = (struct transfer_descriptor *)cur_td;
+    struct transfer_descriptor *previous_td = (struct transfer_descriptor *)prev_td;
+    logf("prepare_td(len=%d)", len);
     //logf("adding a td : %d",len);
     memset(td, 0, sizeof(struct transfer_descriptor));
     td->next_td_ptr = DTD_NEXT_TERMINATE;
-    td->size_ioc_sts = (len<< DTD_LENGTH_BIT_POS) |
+    td->size_ioc_sts = (len << DTD_TOTAL_BYTES_OFFSET) |
         DTD_STATUS_ACTIVE | DTD_IOC;
     td->buff_ptr0 = (unsigned int)ptr;
-    td->buff_ptr1 = ((unsigned int)ptr & 0xfffff000) + 0x1000;
-    td->buff_ptr2 = ((unsigned int)ptr & 0xfffff000) + 0x2000;
-    td->buff_ptr3 = ((unsigned int)ptr & 0xfffff000) + 0x3000;
-    td->buff_ptr4 = ((unsigned int)ptr & 0xfffff000) + 0x4000;
+    td->buff_ptr1 = ((unsigned int)ptr & DTD_BUFFER_POINTER_MASK) + 0x1000;
+    td->buff_ptr2 = ((unsigned int)ptr & DTD_BUFFER_POINTER_MASK) + 0x2000;
+    td->buff_ptr3 = ((unsigned int)ptr & DTD_BUFFER_POINTER_MASK) + 0x3000;
+    td->buff_ptr4 = ((unsigned int)ptr & DTD_BUFFER_POINTER_MASK) + 0x4000;
     td->reserved |= DTD_RESERVED_LENGTH_MASK & len;
     td->reserved |= DTD_RESERVED_IN_USE;
-    td->reserved |= (pipe << DTD_RESERVED_PIPE_OFFSET);
 
     if (previous_td != 0) {
         previous_td->next_td_ptr=(unsigned int)td;
@@ -881,6 +959,7 @@
     }
 }
 
+/* manual: 32.14.3.2.1 Setup Phase */
 static void control_received(void)
 {
     int i;
@@ -889,11 +968,12 @@
     tmp[0] = qh_array[0].setup_buffer[0];
     tmp[1] = qh_array[0].setup_buffer[1];
 
+    logf("control_received()");
     /* acknowledge packet recieved */
     REG_ENDPTSETUPSTAT = EPSETUP_STATUS_EP0;
 
     /* Stop pending control transfers */
-    for(i=0;i<2;i++) {
+    for(i=PIPE_GET(EP_CONTROL, 0);i<PIPE_GET(EP_CONTROL, 1);i++) {
         if(qh_array[i].wait) {
             qh_array[i].wait=0;
             qh_array[i].status=DTD_STATUS_HALTED;
@@ -904,42 +984,65 @@
     usb_core_control_request((struct usb_ctrlrequest*)tmp);
 }
 
-static void transfer_completed(void)
+static int transfer_completed(void)
 {
     int ep;
     unsigned int mask = REG_ENDPTCOMPLETE;
     REG_ENDPTCOMPLETE = mask;
 
+    logf("transfer_completed()");
     for (ep=0; ep<USB_NUM_ENDPOINTS; ep++) {
         int dir;
         for (dir=0; dir<2; dir++) {
-            int pipe = ep * 2 + dir;
-            if (mask & pipe2mask[pipe]) {
+            int pipe = PIPE_GET(ep, dir);
+            if (mask & pipe2mask[pipe]) { /* pipe completed */
+                int length;
+                struct transfer_descriptor *td;
                 struct queue_head* qh = &qh_array[pipe];
                 if(qh->wait) {
                     qh->wait=0;
                     wakeup_signal(&transfer_completion_signal[pipe]);
+                    continue;
                 }
-                int length=0;
-                struct transfer_descriptor* td=&td_array[pipe*NUM_TDS_PER_EP];
-                while(td!=(struct transfer_descriptor*)DTD_NEXT_TERMINATE && td!=0)
+
+                length = 0;
+                for (td = (struct transfer_descriptor *)qh->xfer;
+                    td != 0 && (unsigned int)td != DTD_NEXT_TERMINATE;
+                    td=(struct transfer_descriptor*)td->next_td_ptr)
                 {
-                    length += ((td->reserved & DTD_RESERVED_LENGTH_MASK) -
-                        ((td->size_ioc_sts & DTD_PACKET_SIZE) >> DTD_LENGTH_BIT_POS));
-                    td=(struct transfer_descriptor*) td->next_td_ptr;
+                    int td_len;
+
+                    /* redundant? pipe still xfering */
+                    if ((unsigned int)td == qh->curr_dtd_ptr)
+                        break;
+
+                    td_len = (td->reserved & DTD_RESERVED_LENGTH_MASK) -
+                        ((td->size_ioc_sts & DTD_TOTAL_BYTES_MASK) >> DTD_TOTAL_BYTES_OFFSET);
+
+                    logf("pipe %d(ep %d, dir %d): Adding length %d",
+                            pipe, ep, dir, td_len);
+
+                    length += td_len;
+
+                    if (td_len < 0)
+                        return 1;
                 }
+                logf("pipe %d(ep %d, dir %d): total completed length=%d, status=%u",
+                        pipe, ep, dir, length, qh->status);
                 usb_core_transfer_complete(ep, dir?USB_DIR_IN:USB_DIR_OUT,
                         qh->status, length);
             }
         }
     }
+
+    return 0;
 }
 
 /* manual: 32.14.2.1 Bus Reset */
 static void bus_reset(void)
 {
     int i;
-    logf("usb bus_reset");
+    logf("usb bus_reset()");
 
     REG_DEVICEADDR = 0;
     REG_ENDPTSETUPSTAT = REG_ENDPTSETUPSTAT;
@@ -970,28 +1073,39 @@
 /* manual: 32.14.4.1 Queue Head Initialization */
 static void init_control_queue_heads(void)
 {
+    logf("init_control_queue_heads()");
     memset(qh_array, 0, sizeof qh_array);
 
     /*** control ***/
-    qh_array[EP_CONTROL].max_pkt_length = 64 << QH_MAX_PKT_LEN_POS | QH_IOS;
+    qh_array[EP_CONTROL].max_pkt_length = 64 << QH_MAX_PKT_LEN_OFFSET | QH_IOS;
     qh_array[EP_CONTROL].dtd.next_td_ptr = QH_NEXT_TERMINATE;
-    qh_array[EP_CONTROL+1].max_pkt_length = 64 << QH_MAX_PKT_LEN_POS;
+    qh_array[EP_CONTROL+1].max_pkt_length = 64 << QH_MAX_PKT_LEN_OFFSET;
     qh_array[EP_CONTROL+1].dtd.next_td_ptr = QH_NEXT_TERMINATE;
 }
 /* manual: 32.14.4.1 Queue Head Initialization */
 static void init_bulk_queue_heads(void)
 {
     int packetsize = (usb_drv_port_speed() ? 512 : 64);
-    int i;
+    int i, dir;
 
-    /* TODO: this should take ep_allocation into account */
+    logf("init_bulk_queue_heads()");
     for (i=1;i<USB_NUM_ENDPOINTS;i++) {
-        qh_array[i*2].max_pkt_length = packetsize << QH_MAX_PKT_LEN_POS |
-            QH_ZLT_SEL;
-        qh_array[i*2].dtd.next_td_ptr = QH_NEXT_TERMINATE;
-        qh_array[i*2+1].max_pkt_length = packetsize << QH_MAX_PKT_LEN_POS |
-            QH_ZLT_SEL;
-        qh_array[i*2+1].dtd.next_td_ptr = QH_NEXT_TERMINATE;
+        usb_endpoint_t *endpoint = &endpoints[i];
+
+        for (dir=0;dir<2;dir++) {
+            switch(endpoint->type[dir])
+            {
+                /* TODO: INTERRUPT xfer might need a different initialization */
+                case USB_ENDPOINT_XFER_INT:
+                case USB_ENDPOINT_XFER_BULK:
+                    qh_array[i*2 + dir].max_pkt_length =
+                        (packetsize << QH_MAX_PKT_LEN_OFFSET) | QH_ZLT_SEL;
+                    qh_array[i*2 + dir].dtd.next_td_ptr = QH_NEXT_TERMINATE;
+                    break;
+                default:
+                    break;
+            }
+        }
     }
 }
 
@@ -999,7 +1113,7 @@
 {
     int ep_num;
 
-    logf("init_endpoints");
+    logf("init_endpoints()");
     /* RX/TX from the device POV: OUT/IN, respectively */
     for(ep_num=1;ep_num<USB_NUM_ENDPOINTS;ep_num++) {
         usb_endpoint_t *endpoint = &endpoints[ep_num];