Diff: USBHost/USBHost.cpp

Revision:

0:ae46a0638b2c

Child:

3:4394986752db

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/USBHost/USBHost.cpp	Thu May 24 16:39:35 2012 +0000
@@ -0,0 +1,945 @@
+/* Copyright (c) 2010-2011 mbed.org, MIT License
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy of this software
+ * and associated documentation files (the "Software"), to deal in the Software without
+ * restriction, including without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all copies or
+ * substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
+ * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
+ * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+
+#define __DEBUG__ 0 //WARN: ENABLING DEBUGGING HERE WILL PRINTF IN IRQS!! UNEXPECTED BEHAVIOUR MAY RESULT...
+#ifndef __MODULE__
+#define __MODULE__ "USBHost.cpp"
+#endif
+
+#include "dbg.h"
+#include <cstdint>
+
+#include "USBHost.h"
+#include "rtos.h"
+
+
+#define NB_MAX_INTF 3
+
+USBHost * USBHost::instHost = NULL;
+
+USBHost::USBHost()
+#if 0 //try not to use this
+    : m_usbQueue(), m_usbThread(3, this, &USBHost::usbProcess)
+#endif
+{
+  headControlEndpoint = NULL;
+  headBulkEndpoint = NULL;
+  headInterruptEndpoint = NULL;
+  tailControlEndpoint = NULL;
+  tailBulkEndpoint = NULL;
+  tailInterruptEndpoint = NULL;
+
+  nb_devices = 0;
+  lenReportDescr = 0;
+
+  controlEndpointAllocated = false;
+
+  for (int i = 0; i < MAX_DEVICE_NB; i++) {
+    deviceInUse[i] = false;
+    devices[i].setAddress(i + 1);
+    deviceReset[i] = false;
+  }
+}
+
+
+void USBHost::transferCompleted(volatile uint32_t addr)  {
+#if 0 //try not to use this
+  Interrupt::enter();
+  m_usbQueue.post(addr);
+  Interrupt::leave();
+#else
+
+  if(addr == NULL) //Nothing to process?
+  {
+    return;
+  }
+
+  volatile HCTD* tdList = NULL;
+
+  //First we must reverse the list order and dequeue each TD
+  do
+  {
+    volatile HCTD* td = (volatile HCTD*)addr;
+
+    if(td->control & 0xF0000000 != 0)
+    {
+      WARN("Condition code %02x", td->control >> 28);
+    }
+
+    addr = td->nextTD; //Dequeue from physical list
+    td->nextTD = (uint32_t)tdList; //Enqueue into reversed list
+    tdList = td;
+  } while(addr);
+
+  //Now we can process the list
+  Endpoint * volatile iter = NULL;
+
+  while(tdList != NULL)
+  {
+    bool found = false;
+    volatile HCTD* td = tdList;
+    tdList = (volatile HCTD*)td->nextTD; //Dequeue element now as it could be modified below
+    for (int i = 0; i < 3; i++) {
+      if (found) {
+        break;
+      }
+      iter = (i == 0) ? headControlEndpoint : ( (i == 1) ? headBulkEndpoint : headInterruptEndpoint);
+      while (iter != NULL) {
+        if (iter->getProcessedTD() == td) {
+          DBG("td=%p FOUND ed: %08X", td, (void *)iter->getHCED());
+          if (((HCTD *)td)->control >> 28) {
+            DBG("TD Error: %d", td->control >> 28);
+            iter->setState(USB_TYPE_TDFAIL);
+          } else if ((uint32_t)iter->getHCED() & 0x1) {
+            DBG("HALTED");
+            iter->setState(USB_TYPE_HALTED);
+          } else if (!td->currBufPtr) {
+            DBG("!%p", iter);
+            iter->setState(USB_TYPE_IDLE);
+            found=true;
+          } else {
+            DBG("!%p", iter);
+            iter->setState(USB_TYPE_IDLE);
+            iter->setLengthTransferred(td->currBufPtr - iter->getBufStart());
+            found=true;
+          }
+          break;
+        }
+        iter = iter->nextEndpoint();
+      }
+    }
+
+
+    if (found) {
+      iter->unqueueTransfer(td);
+
+      if (iter->getType() != CONTROL_ENDPOINT) {
+        iter->call();
+      }
+    }
+    else
+    {
+      WARN("TD not found!!!");
+      freeTD((uint8_t *)td); //Device must have been disconnected meanwhile
+    }
+
+  }
+#endif
+}
+
+USBHost * USBHost::getHostInst() {
+  if (instHost == NULL) {
+    instHost = new USBHost();
+    instHost->init();
+  }
+  return instHost;
+}
+
+
+/*
+ * Call in ISR when a device has been connected
+ */
+void USBHost::deviceConnected(int hub, int port, bool lowSpeed)  {
+
+  for (int i = 0; i < MAX_DEVICE_NB; i++) {
+    if (!deviceInUse[i]) {
+      deviceInUse[i] = true;
+      WARN("will call init on device %p: speed: %d", (void *)&devices[i], lowSpeed);
+      devices[i].init(hub, port, lowSpeed);
+      deviceReset[i] = false;
+      break;
+    }
+  }
+
+  if (!controlEndpointAllocated) {
+    control = newEndpoint(CONTROL_ENDPOINT, OUT, 0x08, 0x00);
+    addEndpoint(NULL, 0, (Endpoint*)control);
+    controlEndpointAllocated = true;
+  }
+}
+
+/*
+ * Call in ISR when a device has been disconnected
+ */
+void USBHost::deviceDisconnected(int hub, int port, volatile uint32_t addr)  {
+
+  bool controlListState = disableControlList();
+  bool bulkListState = disableBulkList();
+  bool interruptListState = disableInterruptList();
+
+  transferCompleted(addr); //Finish processing any pending completed TD
+
+  for (int i = 0; i < MAX_DEVICE_NB; i++) {
+    if ((devices[i].getHub() == hub) && (devices[i].getPort() == port)) {
+      WARN("device disconnected: %p", (void *)&devices[i]);
+      deviceInUse[i] = false;
+      deviceReset[i] = false;
+      freeDevice((USBDeviceConnected*)&devices[i]);
+      break;
+    }
+  }
+  nb_devices--;
+
+  if (controlListState) enableControlList();
+  if (bulkListState) enableBulkList();
+  if (interruptListState) enableInterruptList();
+}
+
+void USBHost::freeDevice(USBDeviceConnected * dev)  {
+  Endpoint * ep = NULL;
+//  HCTD * td = NULL;
+  HCED * ed = NULL;
+
+  for (int j = 0; j < dev->getNbInterface(); j++) {
+    DBG("FREE INTF %d, %p, nb_endpot: %d", j, (void *)dev->getInterface(j), dev->getInterface(j)->nb_endpoint);
+    for (int i = 0; i < dev->getInterface(j)->nb_endpoint; i++) {
+      if ((ep = dev->getEndpoint(j, i)) != NULL) {
+        DBG("Freeing endpoint");
+        ed = (HCED *)ep->getHCED();
+        ed->control |= (1 << 13); //sKip bit
+        DBG("Dequeueing endpoint");
+        unqueueEndpoint(ep);
+
+        DBG("Freeing head transfer descriptor");
+        freeTD((volatile uint8_t*)ep->getHeadTD());
+        DBG("Freeing tail transfer descriptor");
+        freeTD((volatile uint8_t*)ep->getNextTD());
+
+        DBG("Freeing endpoint descriptor");
+        freeED((uint8_t *)ep->getHCED());
+      }
+      //printBulk();
+      //printInt();
+    }
+  }
+  DBG("Disconnecting device");
+  dev->disconnect();
+  DBG("Device disconnected");
+}
+
+
+void USBHost::unqueueEndpoint(Endpoint * ep)  {
+  Endpoint * prec = NULL;
+  Endpoint * current = NULL;
+  bool found = false;
+
+  DBG("want to unqueue ep: %p", (void *)ep->getHCED());
+
+  for (int i = 0; i < 2; i++) {
+    if (found) {
+      DBG("endpoint unqueued: %p", (void *)ep->getHCED());
+      break;
+    }
+    current = (i == 0) ? (Endpoint*)headBulkEndpoint : (Endpoint*)headInterruptEndpoint;
+    prec = current;
+    while (current != NULL) {
+      if (current == ep) {
+        if (current->nextEndpoint() != NULL) {
+          prec->queueEndpoint(current->nextEndpoint());
+          if (current == headBulkEndpoint) {
+            updateBulkHeadED((uint32_t)current->nextEndpoint()->getHCED());
+            headBulkEndpoint = current->nextEndpoint();
+          }
+          if (current == headInterruptEndpoint) {
+            updateInterruptHeadED((uint32_t)current->nextEndpoint()->getHCED());
+            headInterruptEndpoint = current->nextEndpoint();
+          }
+        } else {
+          prec->queueEndpoint(NULL);
+          if (current == headBulkEndpoint) {
+            updateBulkHeadED(0);
+            headBulkEndpoint = current->nextEndpoint();
+          }
+          if (current == headInterruptEndpoint) {
+            updateInterruptHeadED(0);
+            headInterruptEndpoint = current->nextEndpoint();
+          }
+        }
+        found = true;
+        current->setState(USB_TYPE_FREE);
+        break;
+      }
+      prec = current;
+      current = current->nextEndpoint();
+    }
+  }
+  //printBulk();
+  //printInt();
+}
+
+
+USBDeviceConnected * USBHost::getDevice(uint8_t index)  {
+  if ((index >= MAX_DEVICE_NB) || (!deviceInUse[index])) {
+    return NULL;
+  }
+  return (USBDeviceConnected*)&devices[index];
+}
+
+
+
+// create an endpoint descriptor. the endpoint is not linked
+Endpoint * USBHost::newEndpoint(ENDPOINT_TYPE type, ENDPOINT_DIRECTION dir, uint32_t size, uint8_t addr)  {
+  int i = 0;
+  HCED * ed = (HCED *)getED();
+  HCTD* td_list[2] = { (HCTD*)getTD(), (HCTD*)getTD() };
+
+  memset((void *)td_list[0], 0x00, sizeof(HCTD));
+  memset((void *)td_list[1], 0x00, sizeof(HCTD));
+
+  // search a free endpoint
+  for (i = 0; i < MAX_ENDPOINT; i++) {
+    if (endpoints[i].getState() == USB_TYPE_FREE) {
+      DBG("Trying to create ep");
+      endpoints[i].init(ed, type, dir, size, addr, td_list);
+      //endpoints[i].queueTransfer(nullTd);
+      DBG("Endpoint created (%p): type: %d, dir: %d, size: %d, addr: %d", &endpoints[i], type, dir, size, addr);
+      return &endpoints[i];
+    }
+  }
+  DBG("could not allocate more endpoints!!!!");
+  return NULL;
+}
+
+
+void USBHost::resetDevice(USBDeviceConnected * dev)  {
+  int index = findDevice(dev);
+  if ((index != -1) && (!deviceReset[index])) {
+    resetPort(dev->getHub(), dev->getPort());
+    deviceReset[index] = true;
+  }
+}
+
+// link the endpoint to the linked list and attach an endpoint to a device
+bool USBHost::addEndpoint(USBDeviceConnected * dev, uint8_t intf_nb, Endpoint * ep)  {
+
+  if (ep == NULL) {
+    return false;
+  }
+
+  DBG("New ep %p", ep);
+
+   HCED * prevEd;
+
+  // set device address in the endpoint descriptor
+  if (dev == NULL) {
+    ep->setDeviceAddress(0);
+  } else {
+    ep->setDeviceAddress(dev->getAddress());
+  }
+
+  if (dev != NULL && dev->getSpeed()) {
+    DBG("add endpoint: set speed");
+    ep->setSpeed(dev->getSpeed());
+  }
+
+  // queue the new endpoint on the ED list
+  switch (ep->getType()) {
+
+  case CONTROL_ENDPOINT:
+    prevEd = ( HCED*) controlHeadED();
+    if (!prevEd) {
+      updateControlHeadED((uint32_t) ep->getHCED());
+      DBG("First control endpoint: %08X", (uint32_t) ep->getHCED());
+      headControlEndpoint = ep;
+      tailControlEndpoint = ep;
+      return true;
+    }
+    tailControlEndpoint->queueEndpoint(ep);
+    tailControlEndpoint = ep;
+    return true;
+
+  case BULK_ENDPOINT:
+    prevEd = ( HCED*) bulkHeadED();
+    if (!prevEd) {
+      updateBulkHeadED((uint32_t) ep->getHCED());
+      //DBG("First bulk endpoint: %08X\r\n", (uint32_t) ep->getHCED());
+      headBulkEndpoint = ep;
+      tailBulkEndpoint = ep;
+      break;
+    }
+    tailBulkEndpoint->queueEndpoint(ep);
+    tailBulkEndpoint = ep;
+    break;
+
+  case INTERRUPT_ENDPOINT:
+    prevEd = ( HCED*) interruptHeadED();
+    if (!prevEd) {
+      updateInterruptHeadED((uint32_t) ep->getHCED());
+      //DBG("First interrupt endpoint: %08X\r\n", (uint32_t) ep->getHCED());
+      headInterruptEndpoint = ep;
+      tailInterruptEndpoint = ep;
+      break;
+    }
+    tailInterruptEndpoint->queueEndpoint(ep);
+    tailInterruptEndpoint = ep;
+    break;
+  default:
+    return false;
+  }
+
+  dev->addEndpoint(intf_nb, ep);
+  //printBulk();
+  //printInt();
+
+  return true;
+}
+
+
+int USBHost::findDevice(USBDeviceConnected * dev)  {
+  for (int i = 0; i < MAX_DEVICE_NB; i++) {
+    if (dev == &devices[i]) {
+      return i;
+    }
+  }
+  return -1;
+}
+
+void USBHost::printBulk() {
+  HCED * hced = (HCED *)bulkHeadED();
+  HCTD * hctd = NULL;
+  printf("---------State of Bulk:--------\r\n");
+  while (hced != NULL) {
+    printf("hced: %p\r\n", hced);
+    hctd = (HCTD *)((uint32_t)(hced->headTD) & ~(0x0f));
+    while (((uint32_t)hctd & ~(0x0f)) != ((hced->tailTD) & ~(0x0f))) {
+      printf("\thctd: %p\r\n", hctd);
+      hctd = (HCTD *)((uint32_t)(hctd->nextTD) & ~(0x0f));
+    }
+    printf("\thctd: %p\r\n", hctd);
+    hced = (HCED *)((uint32_t)(hced->nextED) & ~(0x0f));
+  }
+  printf("--------------------\r\n");
+}
+
+void USBHost::printInt() {
+  HCED * hced = (HCED *)interruptHeadED();
+  HCTD * hctd = NULL;
+  printf("---------State of Int:--------\r\n");
+  while (hced != NULL) {
+    printf("hced: %p\r\n", hced);
+    hctd = (HCTD *)((uint32_t)(hced->headTD) & ~(0x0f));
+    while (((uint32_t)hctd & ~(0x0f)) != ((hced->tailTD) & ~(0x0f))) {
+      printf("\thctd: %p\r\n", hctd);
+      hctd = (HCTD *)((uint32_t)(hctd->nextTD) & ~(0x0f));
+    }
+    printf("\thctd: %p\r\n", hctd);
+    hced = (HCED *)((uint32_t)(hced->nextED) & ~(0x0f));
+  }
+  printf("--------------------\r\n");
+}
+
+
+// add a transfer on the TD linked list
+USB_TYPE USBHost::addTransfer(Endpoint * ed, uint8_t * buf, uint32_t len)  {
+
+  // allocate a TD which will be freed in TDcompletion
+  volatile HCTD * td = ed->getNextTD();
+  if (td == NULL) {
+    return USB_TYPE_ERROR;
+  }
+
+  DBG("Next td = %p",td);
+
+  uint32_t token = (ed->isSetup() ? TD_SETUP : ( (ed->getDir() == IN) ? TD_IN : TD_OUT ));
+
+  uint32_t td_toggle;
+
+  if (ed->getType() == CONTROL_ENDPOINT) {
+    if (ed->isSetup()) {
+      td_toggle = TD_TOGGLE_0;
+    } else {
+      td_toggle = TD_TOGGLE_1;
+    }
+  } else {
+    td_toggle = 0;
+  }
+
+  DBG("Buf=%d, len=%d", buf, len);
+  td->control      = (TD_ROUNDING | token | TD_DELAY_INT(0) | td_toggle | TD_CC);
+  td->currBufPtr   = (uint32_t) buf;
+  td->bufEnd       = (uint32_t)(buf + (len - 1));
+
+  DBG("Now do queue transfer on ep %p", ed);
+
+  ed->queueTransfer();
+
+  DBG("Enable list if needed");
+
+  switch (ed->getType()) {
+  case CONTROL_ENDPOINT:
+    enableControlList();
+    break;
+  case BULK_ENDPOINT:
+    enableBulkList();
+    break;
+  case INTERRUPT_ENDPOINT:
+    //printInt();
+    enableInterruptList();
+    break;
+  }
+
+  DBG("Wait for HC to process TD");
+
+
+  return USB_TYPE_PROCESSING;
+}
+
+
+
+USB_TYPE USBHost::getDeviceDescriptor(USBDeviceConnected * dev, uint8_t * buf)  {
+  return controlRead(  dev,
+      USB_DEVICE_TO_HOST | USB_RECIPIENT_DEVICE,
+      GET_DESCRIPTOR,
+      (DEVICE_DESCRIPTOR << 8) | (0),
+      0,
+      buf,
+      DEVICE_DESCRIPTOR_LENGTH);
+}
+
+USB_TYPE USBHost::getConfigurationDescriptor(USBDeviceConnected * dev, uint8_t * buf, uint8_t * len_conf_descr)  {
+  USB_TYPE res;
+  uint8_t total_conf_descr_length = 0;
+
+  // fourth step: get the beginning of the configuration descriptor to have the total length of the conf descr
+  res = controlRead(  dev,
+      USB_DEVICE_TO_HOST | USB_RECIPIENT_DEVICE,
+      GET_DESCRIPTOR,
+      (CONFIGURATION_DESCRIPTOR << 8) | (0),
+      0,
+      buf,
+      CONFIGURATION_DESCRIPTOR_LENGTH);
+
+  if (res != USB_TYPE_OK) {
+    ERR("GET CONF 1 DESCR FAILED");
+    return res;
+  }
+  total_conf_descr_length = buf[2] | (buf[3] << 8);
+  if (len_conf_descr != NULL) {
+    *len_conf_descr = total_conf_descr_length;
+  }
+  DBG("TOTAL_LENGTH: %d \t NUM_INTERF: %d", total_conf_descr_length, buf[4]);
+
+  return controlRead(  dev,
+      USB_DEVICE_TO_HOST | USB_RECIPIENT_DEVICE,
+      GET_DESCRIPTOR,
+      (CONFIGURATION_DESCRIPTOR << 8) | (0),
+      0,
+      buf,
+      total_conf_descr_length);
+
+}
+
+USB_TYPE USBHost::setConfiguration(USBDeviceConnected * dev, uint8_t conf)  {
+  return controlWrite( dev,
+      USB_HOST_TO_DEVICE | USB_RECIPIENT_DEVICE,
+      SET_CONFIGURATION,
+      conf,
+      0,
+      NULL,
+      0);
+
+}
+
+
+// enumerate a device with the control endpoint
+USB_TYPE USBHost::enumerate(USBDeviceConnected * dev)  {
+  uint8_t data[256];
+  uint8_t total_conf_descr_length = 0;
+  USB_TYPE res;
+
+  DBG("data = %p", data);
+
+  if (dev->isEnumerated()) {
+    return USB_TYPE_OK;
+  }
+
+  // first step: get the size of endpoint 0
+  DBG("Get size of EP 0");
+  res = controlRead(  dev,
+      USB_DEVICE_TO_HOST | USB_RECIPIENT_DEVICE,
+      GET_DESCRIPTOR,
+      (DEVICE_DESCRIPTOR << 8) | (0),
+      0,
+      data,
+      8);
+
+ if (res != USB_TYPE_OK) {
+    ERR("Control read failed!!");
+    return res;
+  }
+  dev->setSizeControlEndpoint(data[7]);
+  DBG("size control Endpoint: %d", dev->getSizeControlEndpoint());
+
+DBG("Now set addr");
+  // second step: set an address to the device
+  res = controlWrite( dev,
+      USB_HOST_TO_DEVICE | USB_RECIPIENT_DEVICE,
+      SET_ADDRESS,
+      dev->getAddress(),
+      0,
+      NULL,
+      0);
+
+  if (res != USB_TYPE_OK) {
+    DBG("SET ADDR FAILED");
+    freeDevice(dev);
+    return res;
+  }
+  dev->activeAddress();
+
+
+  // third step: get the whole device descriptor to see vid, pid
+  res = getDeviceDescriptor(dev, data);
+
+  if (res != USB_TYPE_OK) {
+    DBG("GET DEV DESCR FAILED");
+    return res;
+  }
+  dev->setClass(data[4]);
+  dev->setSubClass(data[5]);
+  dev->setProtocol(data[6]);
+  dev->setVid(data[8] | (data[9] << 8));
+  dev->setPid(data[10] | (data[11] << 8));
+  DBG("CLASS: %02X \t VID: %04X \t PID: %04X", data[4], data[8] | (data[9] << 8), data[10] | (data[11] << 8));
+
+  res = getConfigurationDescriptor(dev, data, &total_conf_descr_length);
+  if (res != USB_TYPE_OK) {
+    return res;
+  }
+
+  // Parse the configuration descriptor
+  parseConfDescr(dev, data, total_conf_descr_length);
+
+
+  // sixth step: set configuration (only 1 supported)
+  res = setConfiguration(dev, 1);
+
+  if (res != USB_TYPE_OK) {
+    DBG("SET CONF FAILED");
+    freeDevice(dev);
+    return res;
+  }
+
+  // Now the device is enumerated!
+  dev->setEnumerated();
+  DBG("device enumerated!!!!");
+
+  // Some devices may require this delay
+  Thread::wait(100);
+
+  return USB_TYPE_OK;
+}
+
+// this method fills the USBDeviceConnected object: class,.... . It also add endpoints found in the descriptor.
+void USBHost::parseConfDescr(USBDeviceConnected * dev, uint8_t * conf_descr, uint32_t len)  {
+  uint32_t index = 0;
+  uint32_t len_desc = 0;
+  uint8_t id = 0;
+  int nb_endpoints_used = 0;
+  Endpoint * ep = NULL;
+  uint8_t intf_nb = 0;
+
+  while (index < len) {
+    len_desc = conf_descr[index];
+    id = conf_descr[index+1];
+    switch (id) {
+    case CONFIGURATION_DESCRIPTOR:
+      break;
+    case INTERFACE_DESCRIPTOR:
+      if (intf_nb++ < NB_MAX_INTF) {
+        dev->addInterface(intf_nb - 1, conf_descr[index + 5], conf_descr[index + 6], conf_descr[index + 7]);
+        nb_endpoints_used = 0;
+        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]);
+      } else {
+        DBG("Drop intf...");
+      }
+      break;
+    case ENDPOINT_DESCRIPTOR:
+      if (intf_nb < NB_MAX_INTF) {
+        if (nb_endpoints_used < MAX_ENDPOINT_PER_INTERFACE) {
+          // if the endpoint 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);
+            DBG("ADD ENDPOINT %p, on interf %d on device %p", (void *)ep, intf_nb - 1, (void *)dev);
+            if (ep != NULL && dev != NULL) {
+              addEndpoint(dev, intf_nb - 1, ep);
+            } else {
+              DBG("EP NULL\r\n");
+            }
+            nb_endpoints_used++;
+          } else {
+            DBG("ISO ENDPOINT NOT SUPPORTED");
+          }
+        }
+      }
+      //DBG("ENDPOINT DESCR\r\n");
+      break;
+    case HID_DESCRIPTOR:
+      lenReportDescr = conf_descr[index + 7] | (conf_descr[index + 8] << 8);
+      break;
+    default:
+      break;
+    }
+    index += len_desc;
+  }
+}
+
+
+USB_TYPE USBHost::bulkRead(USBDeviceConnected * dev, Endpoint * ep, uint8_t * buf, uint32_t len, bool blocking)  {
+  USB_TYPE res;
+
+  if (dev == NULL || ep == NULL) {
+    return USB_TYPE_ERROR;
+  }
+
+  if ((ep->getDir() != IN) || (ep->getType() != BULK_ENDPOINT)) {
+    DBG("wrong dir or bad endpoint type");
+    return USB_TYPE_ERROR;
+  }
+  if (dev->getAddress() != ep->getDeviceAddress()) {
+    DBG("endpoint addr and device addr don't match");
+    return USB_TYPE_ERROR;
+  }
+  addTransfer(ep, buf, len);
+  if (blocking) {
+    unlock();
+    while ((res = control->getState()) == USB_TYPE_PROCESSING);
+    lock();
+    if (res != USB_TYPE_IDLE) {
+      return res;
+    }
+    return USB_TYPE_OK;
+  }
+  return USB_TYPE_PROCESSING;
+}
+
+USB_TYPE USBHost::bulkWrite(USBDeviceConnected * dev, Endpoint * ep, uint8_t * buf, uint32_t len, bool blocking)  {
+  USB_TYPE res;
+
+  if (dev == NULL || ep == NULL) {
+    return USB_TYPE_ERROR;
+  }
+
+  if ((ep->getDir() != OUT) || (ep->getType() != BULK_ENDPOINT)) {
+    DBG("wrong dir or bad endpoint type");
+    return USB_TYPE_ERROR;
+  }
+  if (dev->getAddress() != ep->getDeviceAddress()) {
+    DBG("endpoint addr and device addr don't match");
+    return USB_TYPE_ERROR;
+  }
+  addTransfer(ep, buf, len);
+  if (blocking) {
+    unlock();
+    while ((res = control->getState()) == USB_TYPE_PROCESSING)
+    {
+      DBG("!!!!!!!!!!!!!wait bulkwrite");
+      Thread::wait(100);
+    }
+    lock();
+    DBG("!!!!!!!!!!!!! bulkwrite finished");
+    if (res != USB_TYPE_IDLE) {
+      return res;
+    }
+    return USB_TYPE_OK;
+  }
+  return USB_TYPE_PROCESSING;
+}
+
+USB_TYPE USBHost::interruptWrite(USBDeviceConnected * dev, Endpoint * ep, uint8_t * buf, uint32_t len, bool blocking)  {
+  USB_TYPE res;
+
+  if (dev == NULL || ep == NULL) {
+    return USB_TYPE_ERROR;
+  }
+
+  if (ep->getState() != USB_TYPE_IDLE) {
+    return ep->getState();
+  }
+
+  if ((ep->getDir() != OUT) || (ep->getType() != INTERRUPT_ENDPOINT)) {
+    ERR("wrong dir or bad endpoint type: %d, %d", ep->getDir(), ep->getType());
+    return USB_TYPE_ERROR;
+  }
+  if (dev->getAddress() != ep->getDeviceAddress()) {
+    ERR("endpoint addr and device addr don't match");
+    return USB_TYPE_ERROR;
+  }
+  addTransfer(ep, buf, len);
+  if (blocking) {
+    while ((res = ep->getState()) == USB_TYPE_PROCESSING);
+    if (res != USB_TYPE_IDLE) {
+      return res;
+    }
+    return USB_TYPE_OK;
+  }
+  return USB_TYPE_PROCESSING;
+}
+
+USB_TYPE USBHost::interruptRead(USBDeviceConnected * dev, Endpoint * ep, uint8_t * buf, uint32_t len, bool blocking)  {
+  USB_TYPE res;
+
+  if (dev == NULL || ep == NULL) {
+    return USB_TYPE_ERROR;
+  }
+
+  if (ep->getState() != USB_TYPE_IDLE) {
+    return ep->getState();
+  }
+
+  if ((ep->getDir() != IN) || (ep->getType() != INTERRUPT_ENDPOINT)) {
+    ERR("wrong dir or bad endpoint type");
+    return USB_TYPE_ERROR;
+  }
+
+  if (dev->getAddress() != ep->getDeviceAddress()) {
+    ERR("endpoint addr and device addr don't match");
+    return USB_TYPE_ERROR;
+  }
+  addTransfer(ep, buf, len);
+  if (blocking) {
+    while ((res = ep->getState()) == USB_TYPE_PROCESSING);
+    if (res != USB_TYPE_IDLE) {
+      return res;
+    }
+    return USB_TYPE_OK;
+  }
+  return USB_TYPE_PROCESSING;
+}
+
+
+USB_TYPE USBHost::controlRead(USBDeviceConnected * dev, uint8_t requestType, uint8_t request, uint32_t value, uint32_t index, uint8_t * buf, uint32_t len)  {
+  int length_transfer = len;
+  //DBG("want to transfer: %d bytes\r\n", length_transfer);
+  USB_TYPE res;
+  control->setSpeed(dev->getSpeed());
+  control->setSize(dev->getSizeControlEndpoint());
+  if (dev->isActiveAddress()) {
+    control->setDeviceAddress(dev->getAddress());
+  } else {
+    control->setDeviceAddress(0);
+  }
+  fillControlBuf(requestType, request, value, index, len);
+ /* DBG("will call transfer: ");
+    for (int i = 0; i < 8; i++) {
+        DBG("%02X ", setupPacket[i]);
+    }*/
+  control->setNextToken(TD_SETUP);
+  addTransfer(control, (uint8_t*)setupPacket, 8);
+  DBG("Now wait for TD to be processed");
+  unlock();
+  DBG("Unlocked");
+  while ((res = control->getState()) == USB_TYPE_PROCESSING);
+  lock();
+  DBG("TD processed with result %d", res);
+  if (res != USB_TYPE_IDLE) {
+    return res;
+  }
+
+  if (length_transfer) {
+    DBG("In data to be transfered...");
+    control->setNextToken(TD_IN);
+    addTransfer(control, (uint8_t *)buf, length_transfer);
+    unlock();
+    while ((res = control->getState()) == USB_TYPE_PROCESSING);
+    lock();
+    if (res != USB_TYPE_IDLE) {
+      return res;
+    }
+  }
+
+  DBG("Transfer NULL packet (OUT)");
+  control->setNextToken(TD_OUT);
+  addTransfer(control, NULL, 0);
+  unlock();
+  while ((res = control->getState()) == USB_TYPE_PROCESSING);
+  lock();
+  if (res != USB_TYPE_IDLE) {
+    return res;
+  }
+  return USB_TYPE_OK;
+}
+
+
+USB_TYPE USBHost::controlWrite(USBDeviceConnected * dev, uint8_t requestType, uint8_t request, uint32_t value, uint32_t index, uint8_t * buf, uint32_t len)  {
+  control->setSpeed(dev->getSpeed());
+
+  int length_transfer = len;
+  USB_TYPE res;
+
+  control->setSize(dev->getSizeControlEndpoint());
+  if (dev->isActiveAddress()) {
+    control->setDeviceAddress(dev->getAddress());
+  } else {
+    control->setDeviceAddress(0);
+  }
+  fillControlBuf(requestType, request, value, index, len);
+  /*DBG("will call transfer: ");
+    for (int i = 0; i < 8; i++) {
+        printf("%01X ", setupPacket[i]);
+    }
+    printf("\r\n");*/
+  control->setNextToken(TD_SETUP);
+  addTransfer(control, (uint8_t*)setupPacket, 8);
+  unlock();
+  while ((res = control->getState()) == USB_TYPE_PROCESSING);
+  lock();
+  if (res != USB_TYPE_IDLE) {
+    return res;
+  }
+
+  if (length_transfer) {
+    control->setNextToken(TD_OUT);
+    addTransfer(control, (uint8_t *)buf, length_transfer);
+    unlock();
+    while ((res = control->getState()) == USB_TYPE_PROCESSING);
+    lock();
+    if (res != USB_TYPE_IDLE) {
+      return res;
+    }
+  }
+
+  control->setNextToken(TD_IN);
+  addTransfer(control, NULL, 0);
+  unlock();
+  while ((res = control->getState()) == USB_TYPE_PROCESSING);
+  lock();
+  if (res != USB_TYPE_IDLE) {
+    return res;
+  }
+  return USB_TYPE_OK;
+}
+
+
+void USBHost::fillControlBuf(uint8_t requestType, uint8_t request, uint32_t value, uint32_t index, int len)  {
+#ifdef __BIG_ENDIAN
+#error "Must implement BE to LE conv here"
+#endif
+  setupPacket[0] = requestType;
+  setupPacket[1] = request;
+  //We are in LE so it's fine
+  *((uint32_t*)&setupPacket[2]) = value;
+  *((uint32_t*)&setupPacket[4]) = index;
+  *((uint32_t*)&setupPacket[6]) = (uint32_t) len;
+}
+
+