Diff: USBHost/USBHost.cpp

Revision:

4:b320d68e98e7

Parent:

0:a554658735bf

Child:

6:1571e517a91b

--- 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;
 }
-