Revision 4:b320d68e98e7, committed 2013-03-12

Comitter:

samux

Date:

Tue Mar 12 17:23:37 2013 +0000

Parent:

3:0f5c32575eb8

Child:

5:e48791a1ef18

Commit message:

bug fixes - support composite device

Changed in this revision

--- a/USBHost/USBDeviceConnected.cpp	Wed Mar 06 17:50:07 2013 +0000
+++ b/USBHost/USBDeviceConnected.cpp	Tue Mar 12 17:23:37 2013 +0000
@@ -36,23 +36,27 @@
     device_subclass = 0;
     proto = 0;
     speed = false;
-    strcpy(name, "Unknown");
     for (int i = 0; i < MAX_INTF; i++) {
         memset((void *)&intf[i], 0, sizeof(INTERFACE));
         intf[i].in_use = false;
         for (int j = 0; j < MAX_ENDPOINT_PER_INTERFACE; j++) {
             intf[i].ep[j] = NULL;
+            strcpy(intf[i].name, "Unknown");
         }
     }
     hub_parent = NULL;
     hub = NULL;
+    nb_interf = 0;
 }
 
 INTERFACE * USBDeviceConnected::getInterface(uint8_t index) {
-    if (index >= MAX_INTF) {
+    if (index >= MAX_INTF)
         return NULL;
-    }
-    return &intf[index];
+    
+    if (intf[index].in_use)
+        return &intf[index];
+    
+    return NULL;
 }
 
 bool USBDeviceConnected::addInterface(uint8_t intf_nb, uint8_t intf_class, uint8_t intf_subclass, uint8_t intf_protocol) {
@@ -64,7 +68,6 @@
     intf[intf_nb].intf_subclass = intf_subclass;
     intf[intf_nb].intf_protocol = intf_protocol;
     intf[intf_nb].nb_endpoint = 0;
-    nb_interf++;
     return true;
 }
 
@@ -92,7 +95,7 @@
 }
 
 void USBDeviceConnected::disconnect() {
-    for(int i = 0; i < nb_interf; i++) {
+    for(int i = 0; i < MAX_INTF; i++) {
         intf[i].detach.call();
     }
     init();
@@ -121,4 +124,3 @@
     }
     return intf[intf_nb].ep[index];
 }
-

--- a/USBHost/USBDeviceConnected.h	Wed Mar 06 17:50:07 2013 +0000
+++ b/USBHost/USBDeviceConnected.h	Tue Mar 12 17:23:37 2013 +0000
@@ -22,6 +22,7 @@
 #include "stdint.h"
 #include "USBEndpoint.h"
 #include "USBHostConf.h"
+#include "rtos.h"
 
 class USBHostHub;
 
@@ -33,6 +34,7 @@
     uint8_t intf_protocol;
     USBEndpoint * ep[MAX_ENDPOINT_PER_INTERFACE];
     FunctionPointer detach;
+    char name[10];
 } INTERFACE; 
 
 class USBDeviceConnected
@@ -141,8 +143,9 @@
     inline void setSizeControlEndpoint(uint32_t size) { sizeControlEndpoint = size; };
     inline void activeAddress(bool active) { activeAddr = active; };
     inline void setEnumerated() { enumerated = true; };
+    inline void setNbIntf(uint8_t nb_intf) {nb_interf = nb_intf; };
     inline void setHubParent(USBHostHub * hub) { hub_parent = hub; };
-    inline void setName(const char * name_) { strcpy(name, name_); };
+    inline void setName(const char * name_, uint8_t intf_nb) { strcpy(intf[intf_nb].name, name_); };
 
     //getters
     inline uint8_t     getPort() { return port; };
@@ -158,7 +161,8 @@
     inline bool        isActiveAddress() { return activeAddr; };
     inline bool        isEnumerated() { return enumerated; };
     inline USBHostHub * getHubParent() { return hub_parent; };
-    inline const char * getName() { return name; };
+    inline uint8_t      getNbIntf() { return nb_interf; };
+    inline const char * getName(uint8_t intf_nb) { return intf[intf_nb].name; };
     
     // in case this device is a hub
     USBHostHub * hub;
@@ -181,10 +185,7 @@
     volatile bool enumerated;
     uint8_t nb_interf;
 
-    char name[10];
-
     void init();
 };
 
 #endif
-

--- a/USBHost/USBEndpoint.cpp	Wed Mar 06 17:50:07 2013 +0000
+++ b/USBHost/USBEndpoint.cpp	Tue Mar 12 17:23:37 2013 +0000
@@ -54,6 +54,8 @@
 
     td_current = td_list[0];
     td_next = td_list[1];
+    
+    intf_nb = 0;
 
     state = USB_TYPE_IDLE;
 }
@@ -140,7 +142,7 @@
 
     //Now add this free TD at this end of the queue
     state = USB_TYPE_PROCESSING;
-    td_current->nextTD = (uint32_t)td_next;
+    td_current->nextTD = td_next;
     hced->tailTD = td_next;
 }
 
@@ -158,6 +160,5 @@
 void USBEndpoint::queueEndpoint(USBEndpoint * ed)
 {
     nextEp = ed;
-    hced->nextED = (ed == NULL) ? 0 : (uint32_t)ed->getHCED();
+    hced->nextED = (ed == NULL) ? 0 : ed->getHCED();
 }
-

--- a/USBHost/USBEndpoint.h	Wed Mar 06 17:50:07 2013 +0000
+++ b/USBHost/USBEndpoint.h	Tue Mar 12 17:23:37 2013 +0000
@@ -114,6 +114,7 @@
     void setSpeed(uint8_t speed);
     void setSize(uint32_t size);
     inline void setDir(ENDPOINT_DIRECTION d) { dir = d; }
+    inline void setIntfNb(uint8_t intf_nb_) { intf_nb = intf_nb_; };
 
     // getters
     const char *                getStateString();
@@ -132,6 +133,7 @@
     inline volatile HCTD*       getNextTD() { return td_current; };
     inline bool                 isSetup() { return setup; }
     inline USBEndpoint *        nextEndpoint() { return (USBEndpoint*)nextEp; };
+    inline uint8_t              getIntfNb() { return intf_nb; };
     
     USBDeviceConnected * dev;
 
@@ -157,8 +159,9 @@
     volatile HCTD * td_list[2];
     volatile HCTD * td_current;
     volatile HCTD * td_next;
+    
+    uint8_t intf_nb;
 
 };
 
 #endif
-

--- a/USBHost/USBHALHost.cpp	Wed Mar 06 17:50:07 2013 +0000
+++ b/USBHost/USBHALHost.cpp	Tue Mar 12 17:23:37 2013 +0000
@@ -127,7 +127,7 @@
     // Check for any connected devices
     if (LPC_USB->HcRhPortStatus1 & OR_RH_PORT_CCS) {
         //Device connected
-        wait_ms(100);
+        wait_ms(150);
         USB_DBG("Device connected (%08x)\n\r", LPC_USB->HcRhPortStatus1);
         deviceConnected(0, 1, LPC_USB->HcRhPortStatus1 & OR_RH_PORT_LSDA);
     }
@@ -250,7 +250,7 @@
 
 void USBHALHost::resetRootHub() {
     // Initiate port reset
-    LPC_USB->HcRhPortStatus1 = OR_RH_PORT_PRS; 
+    LPC_USB->HcRhPortStatus1 = OR_RH_PORT_PRS;
     
     while (LPC_USB->HcRhPortStatus1 & OR_RH_PORT_PRS);
     
@@ -282,8 +282,8 @@
                     //Root device connected
                     if (LPC_USB->HcRhPortStatus1 & OR_RH_PORT_CCS) { 
                         
-                        // wait 100ms to avoid bounce
-                        wait_ms(100);
+                        // wait 150ms to avoid bounce
+                        wait_ms(150);
                         
                         //Hub 0 (root hub), Port 1 (count starts at 1), Low or High speed
                         deviceConnected(0, 1, LPC_USB->HcRhPortStatus1 & OR_RH_PORT_LSDA); 
@@ -296,8 +296,8 @@
                             usb_hcca->DoneHead = 0;
                         }
                         
-                        // wait 100ms to avoid bounce
-                        wait_ms(100);
+                        // wait 200ms to avoid bounce
+                        wait_ms(200);
                         
                         deviceDisconnected(0, 1, NULL, usb_hcca->DoneHead & 0xFFFFFFFE);
                         
@@ -322,4 +322,3 @@
         }
     }
 }
-

--- a/USBHost/USBHALHost.h	Wed Mar 06 17:50:07 2013 +0000
+++ b/USBHost/USBHALHost.h	Tue Mar 12 17:23:37 2013 +0000
@@ -164,4 +164,3 @@
 };
 
 #endif
-

--- a/USBHost/USBHost.cpp	Wed Mar 06 17:50:07 2013 +0000
+++ b/USBHost/USBHost.cpp	Tue Mar 12 17:23:37 2013 +0000
@@ -70,12 +70,15 @@
                 case DEVICE_CONNECTED_EVENT:
                     too_many_hub = false;
                     buf[4] = 0;
+                
+                    usb_mutex.lock();
                     
                     for (i = 0; i < MAX_DEVICE_CONNECTED; i++) {
                         if (!deviceInUse[i]) {
                             USB_DBG_EVENT("new device connected: %p\r\n", &devices[i]);
                             devices[i].init(usb_msg->hub, usb_msg->port, usb_msg->lowSpeed);
                             deviceReset[i] = false;
+                            deviceInited[i] = true;
                             break;
                         }
                     }
@@ -97,9 +100,10 @@
                         devices[i].setHubParent((USBHostHub *)(usb_msg->hub_parent));
 #endif
                     
-                    resetDevice(&devices[i]);
-                    
                     for (j = 0; j < timeout_set_addr; j++) {
+                        
+                        resetDevice(&devices[i]);
+                        
                         // set size of control endpoint
                         devices[i].setSizeControlEndpoint(8);
                         
@@ -136,7 +140,7 @@
                             break;
                         }
                         
-                        wait_ms(100);
+                        Thread::wait(100);
                     }
                     
                     USB_INFO("New device connected: %p [hub: %d - port: %d]", &devices[i], usb_msg->hub, usb_msg->port);
@@ -171,6 +175,8 @@
                         deviceInUse[i] = true;
                     }
                     
+                    usb_mutex.unlock();
+                    
                     break;
                     
                 // a device has been disconnected
@@ -200,13 +206,13 @@
                 // we are not in ISR -> users can use printf in their callback method
                 case TD_PROCESSED_EVENT:
                     ep = (USBEndpoint *) ((HCTD *)usb_msg->td_addr)->ep;
-                    ep->setState(usb_msg->td_state);
-                    if (ep->getState() == USB_TYPE_IDLE) {
-                        USB_DBG_EVENT("call callback on td %p [ep: %p state: %s - dev: %p - %s]", usb_msg->td_addr, ep, ep->getStateString(), ep->dev, ep->dev->getName());
+                    if (usb_msg->td_state == USB_TYPE_IDLE) {
+                        USB_DBG_EVENT("call callback on td %p [ep: %p state: %s - dev: %p - %s]", usb_msg->td_addr, ep, ep->getStateString(), ep->dev, ep->dev->getName(ep->getIntfNb()));
+
 #if DEBUG_TRANSFER
                         if (ep->getDir() == IN) {
                             buf_transfer = ep->getBufStart();
-                            printf("READ SUCCESS [%d bytes transferred] on ep: [%p - addr: %02X]: ", ep->getLengthTransferred(), ep, ep->getAddress());
+                            printf("READ SUCCESS [%d bytes transferred - td: 0x%08X] on ep: [%p - addr: %02X]: ",  ep->getLengthTransferred(), usb_msg->td_addr, ep, ep->getAddress());
                             for (int i = 0; i < ep->getLengthTransferred(); i++)
                                 printf("%02X ", buf_transfer[i]);
                             printf("\r\n\r\n");
@@ -216,8 +222,10 @@
                     } else {
                         idx = findDevice(ep->dev);
                         if (idx != -1) {
-                            if (deviceInUse[idx])
-                                USB_WARN("td %p processed but not in idle state: %s [dev: %p - %s]", usb_msg->td_addr, ep->getStateString(), ep->dev, ep->dev->getName());
+                            if (deviceInUse[idx]) {
+                                USB_WARN("td %p processed but not in idle state: %s [ep: %p - dev: %p - %s]", usb_msg->td_addr, ep->getStateString(), ep, ep->dev, ep->dev->getName(ep->getIntfNb()));
+                                ep->setState(USB_TYPE_IDLE);
+                            }
                         }
                     }
                     break;
@@ -249,7 +257,9 @@
         deviceInUse[i] = false;
         devices[i].setAddress(i + 1);
         deviceReset[i] = false;
-        deviceAttachedDriver[i] = false;
+        deviceInited[i] = false;
+        for (uint8_t j = 0; j < MAX_INTF; j++)
+            deviceAttachedDriver[i][j] = false;
     }
     
 #if MAX_HUB_NB
@@ -273,8 +283,8 @@
     //First we must reverse the list order and dequeue each TD
     do {
         volatile HCTD* td = (volatile HCTD*)addr;
-        addr = td->nextTD; //Dequeue from physical list
-        td->nextTD = (uint32_t)tdList; //Enqueue into reversed list
+        addr = (uint32_t)td->nextTD; //Dequeue from physical list
+        td->nextTD = tdList; //Enqueue into reversed list
         tdList = td;
     } while(addr);
 
@@ -301,9 +311,8 @@
                 usb_msg->td_addr = (void *)td;
                 usb_msg->td_state = state;
                 queue_usb_event.put(usb_msg);
-            } else {
-                ep->setState(state);
             }
+            ep->setState(state);
         }
     }
 }
@@ -327,7 +336,7 @@
     // be sure that the new device connected is not already connected...
     int idx = findDevice(hub, port, hub_parent);
     if (idx != -1) {
-        if (deviceInUse[idx])
+        if (deviceInited[idx])
             return;
     }
     
@@ -393,26 +402,28 @@
     if (idx != -1) {
         deviceInUse[idx] = false;
         deviceReset[idx] = false;
-        deviceAttachedDriver[idx] = false;
 
-        for (int j = 0; j < dev->getNbInterface(); j++) {
-            USB_DBG("FREE INTF %d on dev: %p, %p, nb_endpot: %d, %s", j, (void *)dev->getInterface(j), dev, dev->getInterface(j)->nb_endpoint, dev->getName());
-            for (int i = 0; i < dev->getInterface(j)->nb_endpoint; i++) {
-                if ((ep = dev->getEndpoint(j, i)) != NULL) {
-                    ed = (HCED *)ep->getHCED();
-                    ed->control |= (1 << 13); //sKip bit
-                    unqueueEndpoint(ep);
+        for (uint8_t j = 0; j < MAX_INTF; j++) {
+            deviceAttachedDriver[idx][j] = false;
+            if (dev->getInterface(j) != NULL) {
+                USB_DBG("FREE INTF %d on dev: %p, %p, nb_endpot: %d, %s", j, (void *)dev->getInterface(j), dev, dev->getInterface(j)->nb_endpoint, dev->getName(j));
+                for (int i = 0; i < dev->getInterface(j)->nb_endpoint; i++) {
+                    if ((ep = dev->getEndpoint(j, i)) != NULL) {
+                        ed = (HCED *)ep->getHCED();
+                        ed->control |= (1 << 13); //sKip bit
+                        unqueueEndpoint(ep);
 
-                    freeTD((volatile uint8_t*)ep->getTDList()[0]);
-                    freeTD((volatile uint8_t*)ep->getTDList()[1]);
+                        freeTD((volatile uint8_t*)ep->getTDList()[0]);
+                        freeTD((volatile uint8_t*)ep->getTDList()[1]);
 
-                    freeED((uint8_t *)ep->getHCED());
+                        freeED((uint8_t *)ep->getHCED());
+                    }
+                    printList(BULK_ENDPOINT);
+                    printList(INTERRUPT_ENDPOINT);
                 }
-                printList(BULK_ENDPOINT);
-                printList(INTERRUPT_ENDPOINT);
+                USB_INFO("Device disconnected [%p - %s - hub: %d - port: %d]", dev, dev->getName(j), dev->getHub(), dev->getPort());
             }
         }
-        USB_INFO("Device disconnected [%p - %s - hub: %d - port: %d]", dev, dev->getName(), dev->getHub(), dev->getPort());
         dev->disconnect();
     }
 }
@@ -472,7 +483,7 @@
 
 USBDeviceConnected * USBHost::getDevice(uint8_t index)
 {
-    if ((index >= MAX_DEVICE_CONNECTED) || (!deviceInUse[index]) || (deviceAttachedDriver[index])) {
+    if ((index >= MAX_DEVICE_CONNECTED) || (!deviceInUse[index])) {
         return NULL;
     }
     return (USBDeviceConnected*)&devices[index];
@@ -492,7 +503,7 @@
     for (i = 0; i < MAX_ENDPOINT; i++) {
         if (endpoints[i].getState() == USB_TYPE_FREE) {
             endpoints[i].init(ed, type, dir, size, addr, td_list);
-            USB_DBG("USBEndpoint created (%p): type: %d, dir: %d, size: %d, addr: %d", &endpoints[i], type, dir, size, addr);
+            USB_DBG("USBEndpoint created (%p): type: %d, dir: %d, size: %d, addr: %d, state: %s", &endpoints[i], type, dir, size, addr, endpoints[i].getStateString());
             return &endpoints[i];
         }
     }
@@ -506,7 +517,7 @@
     int index = findDevice(dev);
     if (index != -1) {
         USB_DBG("Resetting hub %d, port %d\n", dev->getHub(), dev->getPort());
-        wait_ms(50);
+        wait_ms(100);
         if (dev->getHub() == 0) {
             resetRootHub();
         }
@@ -519,8 +530,6 @@
         deviceReset[index] = true;
         return USB_TYPE_OK;
     }
-    if (deviceReset[index] && !deviceAttachedDriver[index])
-        return USB_TYPE_OK;
     
     return USB_TYPE_ERROR;
 }
@@ -545,6 +554,8 @@
     if ((dev != NULL) && dev->getSpeed()) {
         ep->setSpeed(dev->getSpeed());
     }
+    
+    ep->setIntfNb(intf_nb);
 
     // queue the new USBEndpoint on the ED list
     switch (ep->getType()) {
@@ -553,7 +564,7 @@
             prevEd = ( HCED*) controlHeadED();
             if (!prevEd) {
                 updateControlHeadED((uint32_t) ep->getHCED());
-                USB_DBG("First control USBEndpoint: %08X", (uint32_t) ep->getHCED());
+                USB_DBG_TRANSFER("First control USBEndpoint: %08X", (uint32_t) ep->getHCED());
                 headControlEndpoint = ep;
                 tailControlEndpoint = ep;
                 return true;
@@ -566,12 +577,12 @@
             prevEd = ( HCED*) bulkHeadED();
             if (!prevEd) {
                 updateBulkHeadED((uint32_t) ep->getHCED());
-                USB_DBG("First bulk USBEndpoint: %08X\r\n", (uint32_t) ep->getHCED());
+                USB_DBG_TRANSFER("First bulk USBEndpoint: %08X\r\n", (uint32_t) ep->getHCED());
                 headBulkEndpoint = ep;
                 tailBulkEndpoint = ep;
                 break;
             }
-            USB_DBG("Queue BULK Ed %p after %p\r\n",ep->getHCED(), prevEd);
+            USB_DBG_TRANSFER("Queue BULK Ed %p after %p\r\n",ep->getHCED(), prevEd);
             tailBulkEndpoint->queueEndpoint(ep);
             tailBulkEndpoint = ep;
             break;
@@ -580,12 +591,12 @@
             prevEd = ( HCED*) interruptHeadED();
             if (!prevEd) {
                 updateInterruptHeadED((uint32_t) ep->getHCED());
-                USB_DBG("First interrupt USBEndpoint: %08X\r\n", (uint32_t) ep->getHCED());
+                USB_DBG_TRANSFER("First interrupt USBEndpoint: %08X\r\n", (uint32_t) ep->getHCED());
                 headInterruptEndpoint = ep;
                 tailInterruptEndpoint = ep;
                 break;
             }
-            USB_DBG("Queue INTERRUPT Ed %p after %p\r\n",ep->getHCED(), prevEd);
+            USB_DBG_TRANSFER("Queue INTERRUPT Ed %p after %p\r\n",ep->getHCED(), prevEd);
             tailInterruptEndpoint->queueEndpoint(ep);
             tailInterruptEndpoint = ep;
             break;
@@ -628,7 +639,7 @@
 void USBHost::printList(ENDPOINT_TYPE type)
 {
 #if DEBUG_EP_STATE
-    HCED * hced;
+    volatile HCED * hced;
     switch(type) {
         case CONTROL_ENDPOINT:
             hced = (HCED *)controlHeadED();
@@ -640,7 +651,7 @@
             hced = (HCED *)interruptHeadED();
             break;
     }
-    HCTD * hctd = NULL;
+    volatile HCTD * hctd = NULL;
     const char * type_str = (type == BULK_ENDPOINT) ? "BULK" :
                             ((type == INTERRUPT_ENDPOINT) ? "INTERRUPT" :
                             ((type == CONTROL_ENDPOINT) ? "CONTROL" : "ISOCHRONOUS"));
@@ -652,12 +663,12 @@
                                                    (dir == 1) ? "OUT" : ((dir == 0) ? "FROM_TD":"IN"),
                                                     (hced->control & (0xf << 7)) >> 7);
         hctd = (HCTD *)((uint32_t)(hced->headTD) & ~(0xf));
-        while (hctd != (hced->tailTD)) {
+        while (hctd != hced->tailTD) {
             printf("\thctd: %p [DIR: %s]\r\n", hctd, ((hctd->control & (3 << 19)) >> 19) == 1 ? "OUT" : "IN");
-            hctd = (HCTD *)((uint32_t)(hctd->nextTD));
+            hctd = hctd->nextTD;
         }
         printf("\thctd: %p\r\n", hctd);
-        hced = (HCED *)((uint32_t)(hced->nextED));
+        hced = hced->nextED;
     }
     printf("\r\n\r\n");
 #endif
@@ -767,6 +778,17 @@
                          0, NULL, 0);
 }
 
+uint8_t USBHost::numberDriverAttached(USBDeviceConnected * dev) {
+    int index = findDevice(dev);
+    uint8_t cnt = 0;
+    if (index == -1)
+        return 0;
+    for (uint8_t i = 0; i < MAX_INTF; i++) {
+        if (deviceAttachedDriver[index][i])
+            cnt++;
+    }
+    return cnt;
+}
 
 // enumerate a device with the control USBEndpoint
 USB_TYPE USBHost::enumerate(USBDeviceConnected * dev, IUSBEnumerator* pEnumerator)
@@ -774,17 +796,33 @@
     uint16_t total_conf_descr_length = 0;
     USB_TYPE res;
     
-    uint8_t index = findDevice(dev);
-
-    if (dev->isEnumerated() && deviceAttachedDriver[index]) {
+    usb_mutex.lock();
+    
+    // don't enumerate a device which all interfaces are registered to a specific driver
+    int index = findDevice(dev);
+    
+    if (index == -1) {
+        usb_mutex.unlock();
+        return USB_TYPE_ERROR;
+    }
+    
+    uint8_t nb_intf_attached = numberDriverAttached(dev);
+    USB_DBG("dev: %p nb_intf: %d", dev, dev->getNbIntf());
+    USB_DBG("dev: %p nb_intf_attached: %d", dev, nb_intf_attached);
+    if ((nb_intf_attached != 0) && (dev->getNbIntf() == nb_intf_attached)) {
+        USB_DBG("Don't enumerate dev: %p because all intf are registered with a driver", dev);
+        usb_mutex.unlock();
         return USB_TYPE_OK;
     }
     
+    USB_DBG("Enumerate dev: %p", dev);
+    
     // third step: get the whole device descriptor to see vid, pid
     res = getDeviceDescriptor(dev, data, DEVICE_DESCRIPTOR_LENGTH);
 
-    if ((res != USB_TYPE_OK) && (res != USB_TYPE_DEVICE_NOT_RESPONDING_ERROR)) {
+    if (res != USB_TYPE_OK) {
         USB_DBG("GET DEV DESCR FAILED");
+        usb_mutex.unlock();
         return res;
     }
     
@@ -799,6 +837,7 @@
 
     res = getConfigurationDescriptor(dev, data, 400, &total_conf_descr_length);
     if (res != USB_TYPE_OK) {
+        usb_mutex.unlock();
         return res;
     }
 
@@ -812,21 +851,28 @@
     // Parse the configuration descriptor
     parseConfDescr(dev, data, total_conf_descr_length, pEnumerator);
 
-
-    // sixth step: set configuration (only 1 supported)
-    res = setConfiguration(dev, 1);
+    // only set configuration if not enumerated before
+    if (!dev->isEnumerated()) {
+        
+        USB_DBG("Set configuration 1 on dev: %p", dev);
+        // sixth step: set configuration (only 1 supported)
+        res = setConfiguration(dev, 1);
 
-    if (res != USB_TYPE_OK) {
-        USB_DBG("SET CONF FAILED");
-        return res;
+        if (res != USB_TYPE_OK) {
+            USB_DBG("SET CONF FAILED");
+            usb_mutex.unlock();
+            return res;
+        }
     }
+    
+    dev->setEnumerated();
 
     // Now the device is enumerated!
-    dev->setEnumerated();
-    USB_DBG("device enumerated!!!!");
+    USB_DBG("dev %p is enumerated\r\n", dev);
+    usb_mutex.unlock();
 
     // Some devices may require this delay
-    Thread::wait(100);
+    wait_ms(100);
 
     return USB_TYPE_OK;
 }
@@ -841,19 +887,23 @@
     USBEndpoint * ep = NULL;
     uint8_t intf_nb = 0;
     bool parsing_intf = false;
+    uint8_t current_intf = 0;
 
     while (index < len) {
         len_desc = conf_descr[index];
         id = conf_descr[index+1];
         switch (id) {
             case CONFIGURATION_DESCRIPTOR:
+                USB_DBG("dev: %p has %d intf", dev, conf_descr[4]);
+                dev->setNbIntf(conf_descr[4]);
                 break;
             case INTERFACE_DESCRIPTOR:
-                if(pEnumerator->parseInterface(intf_nb, conf_descr[index + 5], conf_descr[index + 6], conf_descr[index + 7])) {
+                if(pEnumerator->parseInterface(conf_descr[index + 2], conf_descr[index + 5], conf_descr[index + 6], conf_descr[index + 7])) {
                     if (intf_nb++ <= MAX_INTF) {
-                        dev->addInterface(intf_nb - 1, conf_descr[index + 5], conf_descr[index + 6], conf_descr[index + 7]);
+                        current_intf = conf_descr[index + 2];
+                        dev->addInterface(current_intf, conf_descr[index + 5], conf_descr[index + 6], conf_descr[index + 7]);
                         nb_endpoints_used = 0;
-                        USB_DBG("ADD INTF %d on device %p: class: %d, subclass: %d, proto: %d", intf_nb - 1, (void *)dev, conf_descr[index + 5],conf_descr[index + 6],conf_descr[index + 7]);
+                        USB_DBG("ADD INTF %d on device %p: class: %d, subclass: %d, proto: %d", current_intf, dev, conf_descr[index + 5],conf_descr[index + 6],conf_descr[index + 7]);
                     } else {
                         USB_DBG("Drop intf...");
                     }
@@ -865,16 +915,16 @@
             case ENDPOINT_DESCRIPTOR:
                 if (parsing_intf && (intf_nb <= MAX_INTF) ) {
                     if (nb_endpoints_used < MAX_ENDPOINT_PER_INTERFACE) {
-                        if( pEnumerator->useEndpoint(intf_nb - 1, (ENDPOINT_TYPE)(conf_descr[index + 3] & 0x03), (ENDPOINT_DIRECTION)((conf_descr[index + 2] >> 7) + 1)) ) {
+                        if( pEnumerator->useEndpoint(current_intf, (ENDPOINT_TYPE)(conf_descr[index + 3] & 0x03), (ENDPOINT_DIRECTION)((conf_descr[index + 2] >> 7) + 1)) ) {
                             // if the USBEndpoint is isochronous -> skip it (TODO: fix this)
                             if ((conf_descr[index + 3] & 0x03) != ISOCHRONOUS_ENDPOINT) {
                                 ep = newEndpoint((ENDPOINT_TYPE)(conf_descr[index+3] & 0x03),
                                                  (ENDPOINT_DIRECTION)((conf_descr[index + 2] >> 7) + 1),
                                                  conf_descr[index + 4] | (conf_descr[index + 5] << 8),
                                                  conf_descr[index + 2] & 0x0f);
-                                USB_DBG("ADD USBEndpoint %p, on interf %d on device %p", (void *)ep, intf_nb - 1, (void *)dev);
+                                USB_DBG("ADD USBEndpoint %p, on interf %d on device %p", ep, current_intf, dev);
                                 if (ep != NULL && dev != NULL) {
-                                    addEndpoint(dev, intf_nb - 1, ep);
+                                    addEndpoint(dev, current_intf, ep);
                                 } else {
                                     USB_DBG("EP NULL");
                                 }
@@ -918,42 +968,47 @@
 }
 
 USB_TYPE USBHost::generalTransfer(USBDeviceConnected * dev, USBEndpoint * ep, uint8_t * buf, uint32_t len, bool blocking, ENDPOINT_TYPE type, bool write) {
-
+    
 #if DEBUG_TRANSFER
     const char * type_str = (type == BULK_ENDPOINT) ? "BULK" : ((type == INTERRUPT_ENDPOINT) ? "INTERRUPT" : "ISOCHRONOUS");
-    USB_DBG_TRANSFER("----- %s %s [dev: %p - %s - hub: %d - port: %d - addr: %d]------", type_str, (write) ? "WRITE" : "READ", dev, dev->getName(), dev->getHub(), dev->getPort(), dev->getAddress());
+    USB_DBG_TRANSFER("----- %s %s [dev: %p - %s - hub: %d - port: %d - addr: %d - ep: %02X]------", type_str, (write) ? "WRITE" : "READ", dev, dev->getName(ep->getIntfNb()), dev->getHub(), dev->getPort(), dev->getAddress(), ep->getAddress());
 #endif
-
+    
     USB_TYPE res;
     ENDPOINT_DIRECTION dir = (write) ? OUT : IN;
-
+    
+    usb_mutex.lock();
+    
     if (dev == NULL) {
         USB_ERR("dev NULL");
+        usb_mutex.unlock();
         return USB_TYPE_ERROR;
     }
 
     if (ep == NULL) {
         USB_ERR("ep NULL");
+        usb_mutex.unlock();
         return USB_TYPE_ERROR;
     }
 
     if (ep->getState() != USB_TYPE_IDLE) {
-        USB_WARN("[ep: %p - dev: %p] NOT IDLE: %s", ep, ep->dev, ep->getStateString());
+        USB_WARN("[ep: %p - dev: %p - %s] NOT IDLE: %s", ep, ep->dev, ep->dev->getName(ep->getIntfNb()), ep->getStateString());
+        usb_mutex.unlock();
         return ep->getState();
     }
 
     if ((ep->getDir() != dir) || (ep->getType() != type)) {
         USB_ERR("[ep: %p - dev: %p] wrong dir or bad USBEndpoint type", ep, ep->dev);
+        usb_mutex.unlock();
         return USB_TYPE_ERROR;
     }
 
     if (dev->getAddress() != ep->getDeviceAddress()) {
         USB_ERR("[ep: %p - dev: %p] USBEndpoint addr and device addr don't match", ep, ep->dev);
+        usb_mutex.unlock();
         return USB_TYPE_ERROR;
     }
     
-
-
 #if DEBUG_TRANSFER
     if (write) {
         USB_DBG_TRANSFER("%s WRITE buffer", type_str);
@@ -967,19 +1022,21 @@
     usb_mutex.unlock();
 
     if (blocking) {
-
-        while((res = ep->getState()) == USB_TYPE_PROCESSING) {
-            Thread::wait(1);
-        }
-
-        USB_DBG_TRANSFER("%s TRANSFER res: %s", type_str, ep->getStateString());
-
+        
+        while((res = ep->getState()) == USB_TYPE_PROCESSING);
+        
+        USB_DBG_TRANSFER("%s TRANSFER res: %s on ep: %p\r\n", type_str, ep->getStateString(), ep);
+        
         if (res != USB_TYPE_IDLE) {
+            usb_mutex.unlock();
             return res;
         }
         
+        usb_mutex.unlock();
         return USB_TYPE_OK;
     }
+    
+    usb_mutex.unlock();
     return USB_TYPE_PROCESSING;
 
 }
@@ -993,16 +1050,15 @@
     return controlTransfer(dev, requestType, request, value, index, buf, len, true);
 }
 
-
 USB_TYPE USBHost::controlTransfer(USBDeviceConnected * dev, uint8_t requestType, uint8_t request, uint32_t value, uint32_t index, uint8_t * buf, uint32_t len, bool write)
 {
-    USB_DBG_TRANSFER("----- CONTROL %s [dev: %p - %s - hub: %d - port: %d] ------", (write) ? "WRITE" : "READ", dev, dev->getName(), dev->getHub(), dev->getPort());
+    usb_mutex.lock();
+    USB_DBG_TRANSFER("----- CONTROL %s [dev: %p - hub: %d - port: %d] ------", (write) ? "WRITE" : "READ", dev, dev->getHub(), dev->getPort());
 
     int length_transfer = len;
     USB_TYPE res;
     uint32_t token;
 
-    usb_mutex.lock();
     control->setSpeed(dev->getSpeed());
     control->setSize(dev->getSizeControlEndpoint());
     if (dev->isActiveAddress()) {
@@ -1010,7 +1066,7 @@
     } else {
         control->setDeviceAddress(0);
     }
-    
+
     USB_DBG_TRANSFER("Control transfer on device: %d\r\n", control->getDeviceAddress());
     fillControlBuf(requestType, request, value, index, len);
 
@@ -1024,9 +1080,7 @@
     control->setNextToken(TD_SETUP);
     addTransfer(control, (uint8_t*)setupPacket, 8);
 
-    while((res = control->getState()) == USB_TYPE_PROCESSING) {
-        Thread::wait(1);
-    }
+    while((res = control->getState()) == USB_TYPE_PROCESSING);
 
     USB_DBG_TRANSFER("CONTROL setup stage %s", control->getStateString());
 
@@ -1040,9 +1094,7 @@
         control->setNextToken(token);
         addTransfer(control, (uint8_t *)buf, length_transfer);
 
-        while((res = control->getState()) == USB_TYPE_PROCESSING) {
-            Thread::wait(1);
-        }
+        while((res = control->getState()) == USB_TYPE_PROCESSING);
 
 #if DEBUG_TRANSFER
         USB_DBG_TRANSFER("CONTROL %s stage %s", (write) ? "WRITE" : "READ", control->getStateString());
@@ -1069,9 +1121,7 @@
     control->setNextToken(token);
     addTransfer(control, NULL, 0);
 
-    while((res = control->getState()) == USB_TYPE_PROCESSING) {
-        Thread::wait(1);
-    }
+    while((res = control->getState()) == USB_TYPE_PROCESSING);
 
     USB_DBG_TRANSFER("CONTROL ack stage %s", control->getStateString());
     usb_mutex.unlock();
@@ -1095,4 +1145,3 @@
     *((uint16_t*)&setupPacket[4]) = index;
     *((uint16_t*)&setupPacket[6]) = (uint32_t) len;
 }
-

--- a/USBHost/USBHost.h	Wed Mar 06 17:50:07 2013 +0000
+++ b/USBHost/USBHost.h	Tue Mar 12 17:23:37 2013 +0000
@@ -156,6 +156,7 @@
      *  register a driver into the host associated with a callback function called when the device is disconnected
      *
      *  @param dev device
+     *  @param intf interface number
      *  @param tptr pointer to the object to call the member function on
      *  @param mptr pointer to the member function to be called
      */
@@ -163,8 +164,8 @@
     inline void registerDriver(USBDeviceConnected * dev, uint8_t intf, T* tptr, void (T::*mptr)(void)) {
         int index = findDevice(dev);
         if ((index != -1) && (mptr != NULL) && (tptr != NULL)) {
-            USB_DBG("register driver for dev: %p", dev);
-            deviceAttachedDriver[index] = true;
+            USB_DBG("register driver for dev: %p on intf: %d", dev, intf);
+            deviceAttachedDriver[index][intf] = true;
             dev->onDisconnect(intf, tptr, mptr);
         }
     }
@@ -173,13 +174,14 @@
      * register a driver into the host associated with a callback function called when the device is disconnected
      *
      * @param dev device
+     * @param intf interface number
      * @param fn callback called when the specified device has been disconnected
      */
     inline void registerDriver(USBDeviceConnected * dev, uint8_t intf, void (*fn)(void)) {
         int index = findDevice(dev);
         if ((index != -1) && (fn != NULL)) {
-            USB_DBG("register driver for dev: %p", dev);
-            deviceAttachedDriver[index] = true;
+            USB_DBG("register driver for dev: %p on intf: %d", dev, intf);
+            deviceAttachedDriver[index][intf] = true;
             dev->onDisconnect(intf, fn);
         }
     }
@@ -244,8 +246,9 @@
     // devices connected
     USBDeviceConnected devices[MAX_DEVICE_CONNECTED];
     volatile bool  deviceInUse[MAX_DEVICE_CONNECTED];
-    volatile bool  deviceAttachedDriver[MAX_DEVICE_CONNECTED];
+    volatile bool  deviceAttachedDriver[MAX_DEVICE_CONNECTED][MAX_INTF];
     volatile bool  deviceReset[MAX_DEVICE_CONNECTED];
+    volatile bool  deviceInited[MAX_DEVICE_CONNECTED];
     
 #if MAX_HUB_NB
     USBHostHub hubs[MAX_HUB_NB];
@@ -273,7 +276,7 @@
     Mutex usb_mutex;
     
     // buffer for conf descriptor
-    uint8_t data[400];
+    uint8_t data[300];
     
     /**
     * Add a transfer on the TD linked list associated to an ED
@@ -365,6 +368,7 @@
     void parseConfDescr(USBDeviceConnected * dev, uint8_t * conf_descr, uint32_t len, IUSBEnumerator* pEnumerator) ;
     int findDevice(USBDeviceConnected * dev) ;
     int findDevice(uint8_t hub, uint8_t port, USBHostHub * hub_parent = NULL) ;
+    uint8_t numberDriverAttached(USBDeviceConnected * dev);
 
     /////////////////////////
     /// FOR DEBUG
@@ -374,4 +378,3 @@
 };
 
 #endif
-

--- a/USBHost/USBHostConf.h	Wed Mar 06 17:50:07 2013 +0000
+++ b/USBHost/USBHostConf.h	Tue Mar 12 17:23:37 2013 +0000
@@ -5,12 +5,12 @@
 * Maximum number of devices that can be connected
 * to the usb host
 */
-#define MAX_DEVICE_CONNECTED        10
+#define MAX_DEVICE_CONNECTED        5
 
 /*
 * Maximum of Hub connected to the usb host
 */
-#define MAX_HUB_NB                  5
+#define MAX_HUB_NB                  2
 
 /*
 * Maximum number of ports on a USB hub
@@ -40,7 +40,7 @@
 /*
 * Maximum number of interfaces of a usb device
 */
-#define MAX_INTF                    2
+#define MAX_INTF                    3
 
 /*
 * Maximum number of endpoints on each interface
@@ -60,7 +60,6 @@
 /*
 * usb_thread stack size
 */
-#define USB_THREAD_STACK            (128*4 + MAX_HUB_NB*128*4)
+#define USB_THREAD_STACK            (256*4 + MAX_HUB_NB*256*4)
 
 #endif
-

--- a/USBHost/USBHostTypes.h	Wed Mar 06 17:50:07 2013 +0000
+++ b/USBHost/USBHostTypes.h	Tue Mar 12 17:23:37 2013 +0000
@@ -137,10 +137,10 @@
 #define CONFIGURATION_DESCRIPTOR_LENGTH     0x09
 
 // ------------ HostController Transfer Descriptor ------------
-typedef __packed struct hcTd {
-    __IO  uint32_t  control;        // Transfer descriptor control
+typedef __packed struct HCTD {
+    __IO  uint32_t   control;        // Transfer descriptor control
     __IO  uint8_t *  currBufPtr;    // Physical address of current buffer pointer
-    __IO  uint32_t  nextTD;         // Physical pointer to next Transfer Descriptor
+    __IO  HCTD *     nextTD;         // Physical pointer to next Transfer Descriptor
     __IO  uint8_t *  bufEnd;        // Physical address of end of buffer
     void * ep;                      // ep address where a td is linked in
     uint32_t dummy[3];              // padding
@@ -151,7 +151,7 @@
     __IO  uint32_t  control;        // Endpoint descriptor control
     __IO  HCTD *  tailTD;           // Physical address of tail in Transfer descriptor list
     __IO  HCTD *  headTD;           // Physcial address of head in Transfer descriptor list
-    __IO  uint32_t  nextED;         // Physical address of next Endpoint descriptor
+    __IO  hcEd *  nextED;         // Physical address of next Endpoint descriptor
 } HCED;
 
 
@@ -225,4 +225,3 @@
 } HubDescriptor;              
 
 #endif
-

--- a/USBHostHID/USBHostKeyboard.cpp	Wed Mar 06 17:50:07 2013 +0000
+++ b/USBHostHID/USBHostKeyboard.cpp	Tue Mar 12 17:23:37 2013 +0000
@@ -121,8 +121,8 @@
                 if (!int_in)
                     break;
                 
-                USB_INFO("New Keyboard device: VID:%04x PID:%04x [dev: %p]", dev->getVid(), dev->getPid(), dev);
-                dev->setName("Keyboard");
+                USB_INFO("New Keyboard device: VID:%04x PID:%04x [dev: %p - intf: %d]", dev->getVid(), dev->getPid(), dev, keyboard_intf);
+                dev->setName("Keyboard", keyboard_intf);
                 host->registerDriver(dev, keyboard_intf, this, &USBHostKeyboard::init);
                 
                 int_in->attach(this, &USBHostKeyboard::rxHandler);
@@ -186,4 +186,3 @@
 }
 
 #endif
-

--- a/USBHostHID/USBHostKeyboard.h	Wed Mar 06 17:50:07 2013 +0000
+++ b/USBHostHID/USBHostKeyboard.h	Tue Mar 12 17:23:37 2013 +0000
@@ -102,4 +102,3 @@
 #endif
 
 #endif
-

--- a/USBHostHID/USBHostMouse.cpp	Wed Mar 06 17:50:07 2013 +0000
+++ b/USBHostHID/USBHostMouse.cpp	Tue Mar 12 17:23:37 2013 +0000
@@ -59,8 +59,8 @@
                 if (!int_in)
                     break;
                 
-                USB_INFO("New Mouse device: VID:%04x PID:%04x [dev: %p]", dev->getVid(), dev->getPid(), dev);
-                dev->setName("Mouse");
+                USB_INFO("New Mouse device: VID:%04x PID:%04x [dev: %p - intf: %d]", dev->getVid(), dev->getPid(), dev, mouse_intf);
+                dev->setName("Mouse", mouse_intf);
                 host->registerDriver(dev, mouse_intf, this, &USBHostMouse::init);
                 
                 int_in->attach(this, &USBHostMouse::rxHandler);
@@ -114,4 +114,3 @@
 }
 
 #endif
-

--- a/USBHostHID/USBHostMouse.h	Wed Mar 06 17:50:07 2013 +0000
+++ b/USBHostHID/USBHostMouse.h	Tue Mar 12 17:23:37 2013 +0000
@@ -86,4 +86,3 @@
 #endif
 
 #endif
-

--- a/USBHostHub/USBHostHub.cpp	Wed Mar 06 17:50:07 2013 +0000
+++ b/USBHostHub/USBHostHub.cpp	Tue Mar 12 17:23:37 2013 +0000
@@ -102,8 +102,8 @@
             return false;
         }
         
-        USB_INFO("New HUB: VID:%04x PID:%04x [dev: %p]", dev->getVid(), dev->getPid(), dev);
-        dev->setName("Hub");
+        USB_INFO("New HUB: VID:%04x PID:%04x [dev: %p - intf: %d]", dev->getVid(), dev->getPid(), dev, hub_intf);
+        dev->setName("Hub", hub_intf);
         host->registerDriver(dev, hub_intf, this, &USBHostHub::disconnect);
         
         int_in->attach(this, &USBHostHub::rxHandler);
@@ -271,5 +271,3 @@
 }
 
 #endif
-
-

--- a/USBHostHub/USBHostHub.h	Wed Mar 06 17:50:07 2013 +0000
+++ b/USBHostHub/USBHostHub.h	Tue Mar 12 17:23:37 2013 +0000
@@ -125,4 +125,3 @@
 #endif
 
 #endif
-

--- a/USBHostMSD/USBHostMSD.cpp	Wed Mar 06 17:50:07 2013 +0000
+++ b/USBHostMSD/USBHostMSD.cpp	Tue Mar 12 17:23:37 2013 +0000
@@ -28,7 +28,8 @@
 #define DEVICE_TO_HOST  0x80
 #define HOST_TO_DEVICE  0x00
 
-#define GET_MAX_LUN     (0xFE)
+#define GET_MAX_LUN             (0xFE)
+#define BO_MASS_STORAGE_RESET   (0xFF)
 
 USBHostMSD::USBHostMSD(const char * rootdir) : FATFileSystem(rootdir)
 {
@@ -48,6 +49,7 @@
     msd_device_found = false;
     disk_init = false;
     dev_connected = false;
+    nb_ep = 0;
 }
 
 
@@ -67,6 +69,8 @@
     for (i = 0; i < MAX_DEVICE_CONNECTED; i++) {
         if ((dev = host->getDevice(i)) != NULL) {
             
+            USB_DBG("Trying to connect MSD device\r\n");
+            
             if(host->enumerate(dev, this))
                 break;
 
@@ -75,10 +79,10 @@
                 bulk_out = dev->getEndpoint(msd_intf, BULK_ENDPOINT, OUT);
                 
                 if (!bulk_in || !bulk_out)
-                    break;
+                    continue;
                 
-                USB_INFO("New MSD device: VID:%04x PID:%04x [dev: %p]", dev->getVid(), dev->getPid(), dev);
-                dev->setName("MSD");
+                USB_INFO("New MSD device: VID:%04x PID:%04x [dev: %p - intf: %d]", dev->getVid(), dev->getPid(), dev, msd_intf);
+                dev->setName("MSD", msd_intf);
                 host->registerDriver(dev, msd_intf, this, &USBHostMSD::init);
 
                 dev_connected = true;
@@ -86,6 +90,7 @@
             }
         } //if()
     } //for()
+    init();
     return false;
 }
 
@@ -110,7 +115,9 @@
 {
     if (intf_nb == msd_intf) {
         if (type == BULK_ENDPOINT) {
-            msd_device_found = true;
+            nb_ep++;
+            if (nb_ep == 2)
+                msd_device_found = true;
             return true;
         }
     }
@@ -119,43 +126,49 @@
 
 
 int USBHostMSD::testUnitReady() {
+    USB_DBG("Test unit ready");
     return SCSITransfer(NULL, 6, DEVICE_TO_HOST, 0, 0);
 }
 
+
 int USBHostMSD::readCapacity() {
+    USB_DBG("Read capacity");
     uint8_t cmd[10] = {0x25,0,0,0,0,0,0,0,0,0};
     uint8_t result[8];
     int status = SCSITransfer(cmd, 10, DEVICE_TO_HOST, result, 8);
     if (status == 0) {
         blockCount = (result[0] << 24) | (result[1] << 16) | (result[2] << 8) | result[3];
         blockSize = (result[4] << 24) | (result[5] << 16) | (result[6] << 8) | result[7];
-        USB_INFO("MSD [dev: %p] - blockCount: %lld, blockSize: %d\r\n", dev, blockCount, blockSize);
+        USB_INFO("MSD [dev: %p] - blockCount: %lld, blockSize: %d, Capacity: %lld\r\n", dev, blockCount, blockSize, blockCount*blockSize);
     }
     return status;
 }
 
 
 int USBHostMSD::SCSIRequestSense() {
-    uint8_t cmd[5] = {0x03,0,0,0,0x13};
-    uint8_t result[19];
-    int status = SCSITransfer(cmd, 5, DEVICE_TO_HOST, result, 19);
+    USB_DBG("Request sense");
+    uint8_t cmd[6] = {0x03,0,0,0,18,0};
+    uint8_t result[18];
+    int status = SCSITransfer(cmd, 6, DEVICE_TO_HOST, result, 18);
     return status;
 }
 
 
 int USBHostMSD::inquiry(uint8_t lun, uint8_t page_code) {
+    USB_DBG("Inquiry");
     uint8_t evpd = (page_code == 0) ? 0 : 1;
     uint8_t cmd[6] = {0x12, (lun << 5) | evpd, page_code, 0, 36, 0};
     uint8_t result[36];
     int status = SCSITransfer(cmd, 6, DEVICE_TO_HOST, result, 36);
     if (status == 0) {
-        char vid_pid[9];
+        char vid_pid[17];
         memcpy(vid_pid, &result[8], 8);
         vid_pid[8] = 0;
         USB_INFO("MSD [dev: %p] - Vendor ID: %s", dev, vid_pid);
 
-        memcpy(vid_pid, &result[16], 8);
-        USB_INFO("MSD [dev: %p] - Produc ID: %s", dev, vid_pid);
+        memcpy(vid_pid, &result[16], 16);
+        vid_pid[16] = 0;
+        USB_INFO("MSD [dev: %p] - Product ID: %s", dev, vid_pid);
 
         memcpy(vid_pid, &result[32], 4);
         vid_pid[4] = 0;
@@ -170,10 +183,7 @@
         res = host->controlWrite(   dev,
                                     USB_RECIPIENT_ENDPOINT | USB_HOST_TO_DEVICE | USB_REQUEST_TYPE_STANDARD,
                                     CLEAR_FEATURE,
-                                    0,
-                                    ep->getAddress(),
-                                    NULL,
-                                    0);
+                                    0, ep->getAddress(), NULL, 0);
         // set state to IDLE if clear feature successful
         if (res == USB_TYPE_OK) {
             ep->setState(USB_TYPE_IDLE);
@@ -203,14 +213,16 @@
     }
 
     // send the cbw
+    USB_DBG("Send CBW");
     res = host->bulkWrite(dev, bulk_out,(uint8_t *)&cbw, 31);
     if (checkResult(res, bulk_out))
         return -1;
 
     // data stage if needed
     if (data) {
+        USB_DBG("data stage");
         if (flags == HOST_TO_DEVICE) {
-
+            
             res = host->bulkWrite(dev, bulk_out, data, transfer_len);
             if (checkResult(res, bulk_out))
                 return -1;
@@ -225,6 +237,7 @@
 
     // status stage
     csw.Signature = 0;
+    USB_DBG("Read CSW");
     res = host->bulkRead(dev, bulk_in,(uint8_t *)&csw, 13);
     if (checkResult(res, bulk_in))
         return -1;
@@ -232,12 +245,37 @@
     if (csw.Signature != CSW_SIGNATURE) {
         return -1;
     }
+    
+    USB_DBG("recv csw: status: %d", csw.Status);
 
     // ModeSense?
-    if ((csw.Status == 1) && (cmd[0] != 0x03))
+    if ((csw.Status == 1) && (cmd[0] != 0x03)) {
+        USB_DBG("request mode sense");
         return SCSIRequestSense();
+    }
+    
+    // perform reset recovery
+    if ((csw.Status == 2) && (cmd[0] != 0x03)) {
+        
+        // send Bulk-Only Mass Storage Reset request
+        res = host->controlWrite(   dev,
+                                    USB_RECIPIENT_INTERFACE | USB_HOST_TO_DEVICE | USB_REQUEST_TYPE_CLASS,
+                                    BO_MASS_STORAGE_RESET,
+                                    0, msd_intf, NULL, 0);
+        
+        // unstall both endpoints
+        res = host->controlWrite(   dev,
+                                    USB_RECIPIENT_ENDPOINT | USB_HOST_TO_DEVICE | USB_REQUEST_TYPE_STANDARD,
+                                    CLEAR_FEATURE,
+                                    0, bulk_in->getAddress(), NULL, 0);
+        
+        res = host->controlWrite(   dev,
+                                    USB_RECIPIENT_ENDPOINT | USB_HOST_TO_DEVICE | USB_REQUEST_TYPE_STANDARD,
+                                    CLEAR_FEATURE,
+                                    0, bulk_out->getAddress(), NULL, 0);
+        
+    }
 
-    USB_DBG("recv csw: status: %d", csw.Status);
     return csw.Status;
 }
 
@@ -258,12 +296,22 @@
     return SCSITransfer(cmd, 10, direction, buf, blockSize*nbBlock);
 }
 
+int USBHostMSD::getMaxLun() {
+    uint8_t buf[1], res;
+    res = host->controlRead(    dev, USB_RECIPIENT_INTERFACE | USB_DEVICE_TO_HOST | USB_REQUEST_TYPE_CLASS,
+                                0xfe, 0, msd_intf, buf, 1);
+    USB_DBG("max lun: %d", buf[0]);
+    return res;
+}
 
 int USBHostMSD::disk_initialize() {
     USB_DBG("FILESYSTEM: init");
-    U8 i, timeout = 10;
+    U16 i, timeout = 10;
+    
+    getMaxLun();
     
     for (i = 0; i < timeout; i++) {
+        Thread::wait(100);
         if (!testUnitReady())
             break;
     }
@@ -272,6 +320,7 @@
         disk_init = false;
         return -1;
     }
+    
     inquiry(0, 0);
     disk_init = 1;
     return readCapacity();
@@ -308,4 +357,3 @@
 }
 
 #endif
-

--- a/USBHostMSD/USBHostMSD.h	Wed Mar 06 17:50:07 2013 +0000
+++ b/USBHostMSD/USBHostMSD.h	Tue Mar 12 17:23:37 2013 +0000
@@ -72,6 +72,7 @@
     bool dev_connected;
     USBEndpoint * bulk_in;
     USBEndpoint * bulk_out;
+    uint8_t nb_ep;
 
     // Bulk-only CBW
     typedef __packed struct {
@@ -102,6 +103,7 @@
     int SCSIRequestSense();
     int dataTransfer(uint8_t * buf, uint32_t block, uint8_t nbBlock, int direction);
     int checkResult(uint8_t res, USBEndpoint * ep);
+    int getMaxLun();
 
     int blockSize;
     uint64_t blockCount;
@@ -117,4 +119,3 @@
 #endif
 
 #endif
-

--- a/USBHostSerial/MtxCircBuffer.h	Wed Mar 06 17:50:07 2013 +0000
+++ b/USBHostSerial/MtxCircBuffer.h	Tue Mar 12 17:23:37 2013 +0000
@@ -45,6 +45,11 @@
         mtx.unlock();
         return r;
     }
+    
+    void flush() {
+        write = 0;
+        read = 0;
+    }
 
     void queue(T k) {
         mtx.lock();
@@ -84,5 +89,3 @@
 };
 
 #endif
-
-

--- a/USBHostSerial/USBHostSerial.cpp	Wed Mar 06 17:50:07 2013 +0000
+++ b/USBHostSerial/USBHostSerial.cpp	Tue Mar 12 17:23:37 2013 +0000
@@ -22,6 +22,8 @@
 
 #include "dbg.h"
 
+#define SET_LINE_CODING 0x20
+
 USBHostSerial::USBHostSerial(): circ_buf() {
     host = USBHost::getHostInst();
     size_bulk_in = 0;
@@ -36,6 +38,8 @@
     dev_connected = false;
     serial_intf = -1;
     serial_device_found = false;
+    memcpy(&line_coding, 0, sizeof(LINE_CODING));
+    circ_buf.flush();
 }
 
 bool USBHostSerial::connected()
@@ -50,6 +54,8 @@
     }
     for (int i = 0; i < MAX_DEVICE_CONNECTED; i++) {
         if ((dev = host->getDevice(i)) != NULL) {
+            
+            USB_DBG("Trying to connect serial device\r\n");
 
             if(host->enumerate(dev, this))
                 break;
@@ -61,10 +67,12 @@
                 if (!bulk_in || !bulk_out)
                     break;
                 
-                USB_INFO("New Serial device: VID:%04x PID:%04x [dev: %p]", dev->getVid(), dev->getPid(), dev);
-                dev->setName("Serial");
+                USB_INFO("New Serial device: VID:%04x PID:%04x [dev: %p - intf: %d]", dev->getVid(), dev->getPid(), dev, serial_intf);
+                dev->setName("Serial", serial_intf);
                 host->registerDriver(dev, serial_intf, this, &USBHostSerial::init);
                 
+                baud(9600);
+                
                 size_bulk_in = bulk_in->getSize();
                 size_bulk_out = bulk_out->getSize();
                 
@@ -89,8 +97,8 @@
                 circ_buf.queue(buf[i]);
             }
             rx.call();
+            host->bulkRead(dev, bulk_in, buf, size_bulk_in, false);
         }
-        host->bulkRead(dev, bulk_in, buf, size_bulk_in, false);
     }
 }
 
@@ -111,13 +119,30 @@
     return -1;
 }
 
+void USBHostSerial::baud(int baudrate) {
+    line_coding.baudrate = baudrate;
+    format();
+}
+
+void USBHostSerial::format(int bits, Parity parity, int stop_bits) {
+    line_coding.data_bits = bits;
+    line_coding.parity = parity;
+    line_coding.stop_bits = (stop_bits == 1) ? 0 : 2;
+    
+    // set line coding
+    int res = host->controlWrite(   dev,
+                                    USB_RECIPIENT_INTERFACE | USB_HOST_TO_DEVICE | USB_REQUEST_TYPE_CLASS,
+                                    SET_LINE_CODING,
+                                    0, serial_intf, (uint8_t *)&line_coding, 7);
+}
 
 int USBHostSerial::_getc() {
     uint8_t c = 0;
-    while ((bulk_in != NULL) && (circ_buf.isEmpty()));
     if (bulk_in == NULL) {
+        init();
         return -1;
     }
+    while (circ_buf.isEmpty());
     circ_buf.dequeue(&c);
     return c;
 }
@@ -156,5 +181,3 @@
 }
 
 #endif
-
-

--- a/USBHostSerial/USBHostSerial.h	Wed Mar 06 17:50:07 2013 +0000
+++ b/USBHostSerial/USBHostSerial.h	Tue Mar 12 17:23:37 2013 +0000
@@ -41,6 +41,14 @@
         RxIrq,
         TxIrq
     };
+    
+    enum Parity {
+        None = 0,
+        Odd,
+        Even,
+        Mark,
+        Space
+    };
 
     /**
     * Check if a virtual serial port is connected
@@ -95,6 +103,9 @@
             }
         }
     }
+    
+    void baud(int baudrate);
+    void format(int bits = 8, Parity parity = USBHostSerial::None, int stop_bits = 1);
 
 
 protected:
@@ -122,6 +133,15 @@
 
     uint8_t buf[64];
 
+    typedef __packed struct {
+        uint32_t baudrate;
+        uint8_t stop_bits;
+        uint8_t parity;
+        uint8_t data_bits;
+    } LINE_CODING;
+    
+    LINE_CODING line_coding;
+
     void rxHandler();
     void txHandler();
     FunctionPointer rx;
@@ -135,4 +155,3 @@
 #endif
 
 #endif
-