Merge branch 'master' into fix-nrf51-port

16 files changed, 4147 insertions, 4147 deletions

diff --git a/os/hal/src/crc.c b/os/hal/src/hal_crc.c
index 63799e4..63799e4 100644
--- a/os/hal/src/crc.c
+++ b/os/hal/src/hal_crc.c
diff --git a/os/hal/src/ee24xx.c b/os/hal/src/hal_ee24xx.c
index c89635f..632ffbb 100644
--- a/os/hal/src/ee24xx.c
+++ b/os/hal/src/hal_ee24xx.c
@@ -42,7 +42,7 @@ Note:
   being written to the next page as might be expected.
 *********************************************************************/
 
-#include "ee24xx.h"
+#include "hal_ee24xx.h"
 #include <string.h>
 
 #if (defined(HAL_USE_EEPROM) && HAL_USE_EEPROM && EEPROM_USE_EE24XX) || defined(__DOXYGEN__)
diff --git a/os/hal/src/ee25xx.c b/os/hal/src/hal_ee25xx.c
index 07f4373..102aef8 100644
--- a/os/hal/src/ee25xx.c
+++ b/os/hal/src/hal_ee25xx.c
@@ -42,7 +42,7 @@ Note:
   being written to the next page as might be expected.
 *********************************************************************/
 
-#include "ee25xx.h"
+#include "hal_ee25xx.h"
 #include <string.h>
 
 #if (defined(HAL_USE_EEPROM) && HAL_USE_EEPROM && EEPROM_USE_EE25XX) || defined(__DOXYGEN__)
diff --git a/os/hal/src/eeprom.c b/os/hal/src/hal_eeprom.c
index 60d90ed..f77d616 100644
--- a/os/hal/src/eeprom.c
+++ b/os/hal/src/hal_eeprom.c
@@ -26,7 +26,7 @@
   The work is provided "as is" without warranty of any kind, neither express nor implied.
 */
 
-#include "eeprom.h"
+#include "hal_eeprom.h"
 #include <string.h>
 
 #if defined(HAL_USE_EEPROM) && HAL_USE_EEPROM
diff --git a/os/hal/src/eicu.c b/os/hal/src/hal_eicu.c
index f75c58b..f75c58b 100644
--- a/os/hal/src/eicu.c
+++ b/os/hal/src/hal_eicu.c
diff --git a/os/hal/src/nand.c b/os/hal/src/hal_nand.c
index 24dd6de..24dd6de 100644
--- a/os/hal/src/nand.c
+++ b/os/hal/src/hal_nand.c
diff --git a/os/hal/src/onewire.c b/os/hal/src/hal_onewire.c
index 85f0fdc..85f0fdc 100644
--- a/os/hal/src/onewire.c
+++ b/os/hal/src/hal_onewire.c
diff --git a/os/hal/src/rng.c b/os/hal/src/hal_rng.c
index 5ff6d2d..5ff6d2d 100644
--- a/os/hal/src/rng.c
+++ b/os/hal/src/hal_rng.c
diff --git a/os/hal/src/timcap.c b/os/hal/src/hal_timcap.c
index 6ee97ad..a352490 100644
--- a/os/hal/src/timcap.c
+++ b/os/hal/src/hal_timcap.c
@@ -26,7 +26,7 @@
  * @{
  */
 
-#include "timcap.h"
+#include "hal_timcap.h"
 
 #if HAL_USE_TIMCAP || defined(__DOXYGEN__)
 
diff --git a/os/hal/src/usbh.c b/os/hal/src/hal_usbh.c
index 3bdbac3..1caa183 100644
--- a/os/hal/src/usbh.c
+++ b/os/hal/src/hal_usbh.c
@@ -1,1395 +1,1395 @@
-/*

-    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio

-              Copyright (C) 2015 Diego Ismirlian, TISA, (dismirlian (at) google's mail)

-

-    Licensed under the Apache License, Version 2.0 (the "License");

-    you may not use this file except in compliance with the License.

-    You may obtain a copy of the License at

-

-        http://www.apache.org/licenses/LICENSE-2.0

-

-    Unless required by applicable law or agreed to in writing, software

-    distributed under the License is distributed on an "AS IS" BASIS,

-    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.

-    See the License for the specific language governing permissions and

-    limitations under the License.

-*/

-

-#include "hal.h"

-

-#if HAL_USE_USBH

-

-#include "usbh/dev/hub.h"

-#include "usbh/internal.h"

-#include <string.h>

-

-#if USBH_DEBUG_ENABLE_TRACE

-#define udbgf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)

-#define udbg(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)

-#else

-#define udbgf(f, ...)  do {} while(0)

-#define udbg(f, ...)   do {} while(0)

-#endif

-

-#if USBH_DEBUG_ENABLE_INFO

-#define uinfof(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)

-#define uinfo(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)

-#else

-#define uinfof(f, ...)  do {} while(0)

-#define uinfo(f, ...)   do {} while(0)

-#endif

-

-#if USBH_DEBUG_ENABLE_WARNINGS

-#define uwarnf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)

-#define uwarn(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)

-#else

-#define uwarnf(f, ...)  do {} while(0)

-#define uwarn(f, ...)   do {} while(0)

-#endif

-

-#if USBH_DEBUG_ENABLE_ERRORS

-#define uerrf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)

-#define uerr(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)

-#else

-#define uerrf(f, ...)  do {} while(0)

-#define uerr(f, ...)   do {} while(0)

-#endif

-

-#if STM32_USBH_USE_OTG1

-USBHDriver USBHD1;

-#endif

-#if STM32_USBH_USE_OTG2

-USBHDriver USBHD2;

-#endif

-

-

-static void _classdriver_process_device(usbh_device_t *dev);

-static bool _classdriver_load(usbh_device_t *dev, uint8_t class,

-		uint8_t subclass, uint8_t protocol, uint8_t *descbuff, uint16_t rem);

-

-

-/*===========================================================================*/

-/* Checks.								                                     */

-/*===========================================================================*/

-

-static inline void _check_dev(usbh_device_t *dev) {

-	osalDbgCheck(dev);

-	//TODO: add more checks.

-}

-

-static inline void _check_ep(usbh_ep_t *ep) {

-	osalDbgCheck(ep != 0);

-	_check_dev(ep->device);

-	osalDbgCheck(ep->type <= 3);

-	//TODO: add more checks.

-}

-

-static inline void _check_urb(usbh_urb_t *urb) {

-	osalDbgCheck(urb != 0);

-	_check_ep(urb->ep);

-	osalDbgCheck((urb->buff != NULL) || (urb->requestedLength == 0));

-	//TODO: add more checks.

-}

-

-/*===========================================================================*/

-/* Main driver API.						                                     */

-/*===========================================================================*/

-

-void usbhObjectInit(USBHDriver *usbh) {

-	memset(usbh, 0, sizeof(*usbh));

-	usbh->status = USBH_STATUS_STOPPED;

-#if HAL_USBH_USE_HUB

-	INIT_LIST_HEAD(&usbh->hubs);

-	_usbhub_port_object_init(&usbh->rootport, usbh, 0, 1);

-#else

-	_usbhub_port_object_init(&usbh->rootport, usbh, 1);

-#endif

-}

-

-void usbhInit(void) {

-#if HAL_USBH_USE_HUB

-	uint8_t i;

-	for (i = 0; i < HAL_USBHHUB_MAX_INSTANCES; i++) {

-		usbhhubObjectInit(&USBHHUBD[i]);

-	}

-#endif

-	usbh_lld_init();

-}

-

-void usbhStart(USBHDriver *usbh) {

-	usbDbgInit(usbh);

-

-	osalSysLock();

-	osalDbgAssert((usbh->status == USBH_STATUS_STOPPED) || (usbh->status == USBH_STATUS_STARTED),

-				"invalid state");

-	usbh_lld_start(usbh);

-	usbh->status = USBH_STATUS_STARTED;

-	osalOsRescheduleS();

-	osalSysUnlock();

-}

-

-

-void usbhStop(USBHDriver *usbh) {

-	//TODO: implement

-	(void)usbh;

-}

-void usbhSuspend(USBHDriver *usbh) {

-	//TODO: implement

-	(void)usbh;

-}

-void usbhResume(USBHDriver *usbh) {

-	//TODO: implement

-	(void)usbh;

-}

-

-/*===========================================================================*/

-/* Endpoint API.						                                     */

-/*===========================================================================*/

-

-void usbhEPObjectInit(usbh_ep_t *ep, usbh_device_t *dev, const usbh_endpoint_descriptor_t *desc) {

-	osalDbgCheck(ep);

-	_check_dev(dev);

-	osalDbgCheck(desc);

-

-	memset(ep, 0, sizeof(*ep));

-	ep->device = dev;

-	ep->wMaxPacketSize = desc->wMaxPacketSize;

-	ep->address = desc->bEndpointAddress & 0x0F;

-	ep->type = (usbh_eptype_t) (desc->bmAttributes & 0x03);

-	if (ep->type != USBH_EPTYPE_CTRL) {

-		ep->in = (desc->bEndpointAddress & 0x80) ? TRUE : FALSE;

-	}

-	ep->bInterval = desc->bInterval;

-

-	/* low-level part */

-	usbh_lld_ep_object_init(ep);

-

-	ep->status = USBH_EPSTATUS_CLOSED;

-}

-

-

-static void _ep0_object_init(usbh_device_t *dev, uint16_t wMaxPacketSize) {

-	const usbh_endpoint_descriptor_t ep0_descriptor = {

-		7,	//bLength

-		5,	//bDescriptorType

-		0,	//bEndpointAddress

-		0,	//bmAttributes

-		wMaxPacketSize,

-		0,	//bInterval

-	};

-	usbhEPObjectInit(&dev->ctrl, dev, &ep0_descriptor);

-	usbhEPSetName(&dev->ctrl, "DEV[CTRL]");

-}

-

-

-/*===========================================================================*/

-/* URB API.				    	                                     		 */

-/*===========================================================================*/

-

-void usbhURBObjectInit(usbh_urb_t *urb, usbh_ep_t *ep, usbh_completion_cb callback,

-		void *user, void *buff, uint32_t len) {

-

-	osalDbgCheck(urb != 0);

-	_check_ep(ep);

-

-	/* initialize the common part: */

-	urb->ep = ep;

-	urb->callback = callback;

-	urb->userData = user;

-	urb->buff = buff;

-	urb->requestedLength = len;

-	urb->actualLength = 0;

-	urb->status = USBH_URBSTATUS_INITIALIZED;

-	urb->waitingThread = 0;

-	urb->abortingThread = 0;

-

-	/* initialize the ll part: */

-	usbh_lld_urb_object_init(urb);

-}

-

-void usbhURBObjectResetI(usbh_urb_t *urb) {

-	osalDbgAssert(!usbhURBIsBusy(urb), "invalid status");

-

-	osalDbgCheck((urb->waitingThread == 0) && (urb->abortingThread == 0));

-

-	urb->actualLength = 0;

-	urb->status = USBH_URBSTATUS_INITIALIZED;

-

-	/* reset the ll part: */

-	usbh_lld_urb_object_reset(urb);

-}

-

-void usbhURBSubmitI(usbh_urb_t *urb) {

-	osalDbgCheckClassI();

-	_check_urb(urb);

-	osalDbgAssert(urb->status == USBH_URBSTATUS_INITIALIZED, "invalid status");

-	usbh_ep_t *const ep = urb->ep;

-	if (ep->status == USBH_EPSTATUS_HALTED) {

-		_usbh_urb_completeI(urb, USBH_URBSTATUS_STALL);

-		return;

-	}

-	if (ep->status != USBH_EPSTATUS_OPEN) {

-		_usbh_urb_completeI(urb, USBH_URBSTATUS_DISCONNECTED);

-		return;

-	}

-	if (!(usbhDeviceGetPort(ep->device)->status & USBH_PORTSTATUS_ENABLE)) {

-		_usbh_urb_completeI(urb, USBH_URBSTATUS_DISCONNECTED);

-		return;

-	}

-	urb->status = USBH_URBSTATUS_PENDING;

-	usbh_lld_urb_submit(urb);

-}

-

-bool _usbh_urb_abortI(usbh_urb_t *urb, usbh_urbstatus_t status) {

-	osalDbgCheckClassI();

-	_check_urb(urb);

-

-	switch (urb->status) {

-/*	case USBH_URBSTATUS_UNINITIALIZED:

- * 	case USBH_URBSTATUS_INITIALIZED:

- *	case USBH_URBSTATUS_ERROR:

- *	case USBH_URBSTATUS_TIMEOUT:

- *	case USBH_URBSTATUS_CANCELLED:

- *	case USBH_URBSTATUS_STALL:

- *	case USBH_URBSTATUS_DISCONNECTED:

- *	case USBH_URBSTATUS_OK: */

-	default:

-		/* already finished */

-		_usbh_urb_completeI(urb, status);

-		return TRUE;

-

-//	case USBH_URBSTATUS_QUEUED:

-	case USBH_URBSTATUS_PENDING:

-		return usbh_lld_urb_abort(urb, status);

-	}

-}

-

-void _usbh_urb_abort_and_waitS(usbh_urb_t *urb, usbh_urbstatus_t status) {

-	osalDbgCheckClassS();

-	_check_urb(urb);

-

-	if (_usbh_urb_abortI(urb, status) == FALSE) {

-		uwarn("URB wasn't aborted immediately, suspend");

-		osalThreadSuspendS(&urb->abortingThread);

-		urb->abortingThread = 0;

-	} else {

-		osalOsRescheduleS();

-	}

-	uwarn("URB aborted");

-}

-

-bool usbhURBCancelI(usbh_urb_t *urb) {

-	return _usbh_urb_abortI(urb, USBH_URBSTATUS_CANCELLED);

-}

-

-void usbhURBCancelAndWaitS(usbh_urb_t *urb) {

-	_usbh_urb_abort_and_waitS(urb, USBH_URBSTATUS_CANCELLED);

-}

-

-msg_t usbhURBWaitTimeoutS(usbh_urb_t *urb, systime_t timeout) {

-	msg_t ret;

-

-	osalDbgCheckClassS();

-	_check_urb(urb);

-

-	switch (urb->status) {

-	case USBH_URBSTATUS_INITIALIZED:

-	case USBH_URBSTATUS_PENDING:

-//	case USBH_URBSTATUS_QUEUED:

-		ret = osalThreadSuspendTimeoutS(&urb->waitingThread, timeout);

-		urb->waitingThread = 0;

-		break;

-

-	case USBH_URBSTATUS_OK:

-		ret = MSG_OK;

-		osalOsRescheduleS();

-		break;

-

-/*	case USBH_URBSTATUS_UNINITIALIZED:

- *	case USBH_URBSTATUS_ERROR:

- *	case USBH_URBSTATUS_TIMEOUT:

- *	case USBH_URBSTATUS_CANCELLED:

- *	case USBH_URBSTATUS_STALL:

- *	case USBH_URBSTATUS_DISCONNECTED: */

-	default:

-		ret = MSG_RESET;

-		osalOsRescheduleS();

-		break;

-	}

-	return ret;

-}

-

-msg_t usbhURBSubmitAndWaitS(usbh_urb_t *urb, systime_t timeout) {

-	msg_t ret;

-

-	osalDbgCheckClassS();

-	_check_urb(urb);

-

-	usbhURBSubmitI(urb);

-	ret = usbhURBWaitTimeoutS(urb, timeout);

-	if (ret == MSG_TIMEOUT)

-		_usbh_urb_abort_and_waitS(urb, USBH_URBSTATUS_TIMEOUT);

-

-	return ret;

-}

-

-static inline msg_t _wakeup_message(usbh_urbstatus_t status) {

-	if (status == USBH_URBSTATUS_OK) return MSG_OK;

-	if (status == USBH_URBSTATUS_TIMEOUT) return MSG_TIMEOUT;

-	return MSG_RESET;

-}

-

-void _usbh_urb_completeI(usbh_urb_t *urb, usbh_urbstatus_t status) {

-	osalDbgCheckClassI();

-	_check_urb(urb);

-	urb->status = status;

-	osalThreadResumeI(&urb->waitingThread, _wakeup_message(status));

-	osalThreadResumeI(&urb->abortingThread, MSG_RESET);

-	if (urb->callback)

-		urb->callback(urb);

-}

-

-/*===========================================================================*/

-/* Synchronous API.		    	                                     		 */

-/*===========================================================================*/

-

-usbh_urbstatus_t usbhBulkTransfer(usbh_ep_t *ep,

-		void *data,

-		uint32_t len,

-		uint32_t *actual_len,

-		systime_t timeout) {

-

-	osalDbgCheck(ep != NULL);

-	osalDbgCheck((data != NULL) || (len == 0));

-	osalDbgAssert(ep->type == USBH_EPTYPE_BULK, "wrong ep");

-

-	usbh_urb_t urb;

-	usbhURBObjectInit(&urb, ep, 0, 0, data, len);

-

-	osalSysLock();

-	usbhURBSubmitAndWaitS(&urb, timeout);

-	osalSysUnlock();

-

-	if (actual_len != NULL)

-		*actual_len = urb.actualLength;

-

-	return urb.status;

-}

-

-usbh_urbstatus_t usbhControlRequestExtended(usbh_device_t *dev,

-		const usbh_control_request_t *req,

-		uint8_t *buff,

-		uint32_t *actual_len,

-		systime_t timeout) {

-

-	_check_dev(dev);

-	osalDbgCheck(req != NULL);

-

-	usbh_urb_t urb;

-

-	usbhURBObjectInit(&urb, &dev->ctrl, 0, 0, buff, req->wLength);

-	urb.setup_buff = req;

-

-	osalSysLock();

-	usbhURBSubmitAndWaitS(&urb, timeout);

-	osalSysUnlock();

-

-	if (actual_len != NULL)

-		*actual_len = urb.actualLength;

-

-	return urb.status;

-}

-

-usbh_urbstatus_t usbhControlRequest(usbh_device_t *dev,

-		uint8_t bmRequestType,

-		uint8_t bRequest,

-		uint16_t wValue,

-		uint16_t wIndex,

-		uint16_t wLength,

-		uint8_t *buff) {

-

-	const USBH_DEFINE_BUFFER(usbh_control_request_t, req) = {

-			bmRequestType,

-			bRequest,

-			wValue,

-			wIndex,

-			wLength

-	};

-	return usbhControlRequestExtended(dev, &req, buff, NULL, MS2ST(1000));

-}

-

-/*===========================================================================*/

-/* Standard request helpers.   	                                     		 */

-/*===========================================================================*/

-

-#define USBH_GET_DESCRIPTOR(type, value, index)	\

-	USBH_STANDARDIN(type, \

-	USBH_REQ_GET_DESCRIPTOR, \

-	value, \

-	index) \

-

-#define USBH_GETDEVICEDESCRIPTOR \

-	USBH_GET_DESCRIPTOR(USBH_REQTYPE_DEVICE, (USBH_DT_DEVICE << 8) | 0, 0)

-

-#define USBH_GETCONFIGURATIONDESCRIPTOR(index) \

-	USBH_GET_DESCRIPTOR(USBH_REQTYPE_DEVICE, (USBH_DT_CONFIG << 8) | index, 0)

-

-#define USBH_GETSTRINGDESCRIPTOR(index, langID) \

-	USBH_GET_DESCRIPTOR(USBH_REQTYPE_DEVICE, (USBH_DT_STRING << 8) | index, langID)

-

-bool usbhStdReqGetDeviceDescriptor(usbh_device_t *dev,

-		uint16_t wLength,

-		uint8_t *buf) {

-	usbh_device_descriptor_t *desc;

-	usbh_urbstatus_t ret = usbhControlRequest(dev, USBH_GETDEVICEDESCRIPTOR, wLength, buf);

-	desc = (usbh_device_descriptor_t *)buf;

-	if ((ret != USBH_URBSTATUS_OK)

-			|| (desc->bLength != USBH_DT_DEVICE_SIZE)

-			|| (desc->bDescriptorType != USBH_DT_DEVICE)) {

-		return HAL_FAILED;

-	}

-	return HAL_SUCCESS;

-}

-

-bool usbhStdReqGetConfigurationDescriptor(usbh_device_t *dev,

-		uint8_t index,

-		uint16_t wLength,

-		uint8_t *buf) {

-	usbh_urbstatus_t ret = usbhControlRequest(dev, USBH_GETCONFIGURATIONDESCRIPTOR(index), wLength, buf);

-	usbh_config_descriptor_t *const desc = (usbh_config_descriptor_t *)buf;

-	if ((ret != USBH_URBSTATUS_OK)

-			|| (desc->bLength < USBH_DT_CONFIG_SIZE)

-			|| (desc->bDescriptorType != USBH_DT_CONFIG)) {

-		return HAL_FAILED;

-	}

-	return HAL_SUCCESS;

-}

-

-bool usbhStdReqGetStringDescriptor(usbh_device_t *dev,

-		uint8_t index,

-		uint16_t langID,

-		uint16_t wLength,

-		uint8_t *buf) {

-

-	osalDbgAssert(wLength >= USBH_DT_STRING_SIZE, "wrong size");

-	usbh_string_descriptor_t *desc = (usbh_string_descriptor_t *)buf;

-	usbh_urbstatus_t ret = usbhControlRequest(dev, USBH_GETSTRINGDESCRIPTOR(index, langID), wLength, buf);

-	if ((ret != USBH_URBSTATUS_OK)

-			|| (desc->bLength < USBH_DT_STRING_SIZE)

-			|| (desc->bDescriptorType != USBH_DT_STRING)) {

-		return HAL_FAILED;

-	}

-	return HAL_SUCCESS;

-}

-

-

-

-#define USBH_SET_INTERFACE(interface, alt)	\

-	USBH_STANDARDOUT(USBH_REQTYPE_INTERFACE, \

-	USBH_REQ_SET_INTERFACE, \

-	alt, \

-	interface) \

-

-#define USBH_GET_INTERFACE(interface)	\

-	USBH_STANDARDIN(USBH_REQTYPE_INTERFACE, \

-	USBH_REQ_GET_INTERFACE, \

-	0, \

-	interface) \

-

-bool usbhStdReqSetInterface(usbh_device_t *dev,

-		uint8_t bInterfaceNumber,

-		uint8_t bAlternateSetting) {

-

-	usbh_urbstatus_t ret = usbhControlRequest(dev, USBH_SET_INTERFACE(bInterfaceNumber, bAlternateSetting), 0, NULL);

-	if (ret != USBH_URBSTATUS_OK)

-		return HAL_FAILED;

-

-	return HAL_SUCCESS;

-}

-

-bool usbhStdReqGetInterface(usbh_device_t *dev,

-		uint8_t bInterfaceNumber,

-		uint8_t *bAlternateSetting) {

-

-	USBH_DEFINE_BUFFER(uint8_t, alt);

-

-	usbh_urbstatus_t ret = usbhControlRequest(dev, USBH_GET_INTERFACE(bInterfaceNumber), 1, &alt);

-	if (ret != USBH_URBSTATUS_OK)

-		return HAL_FAILED;

-

-	*bAlternateSetting = alt;

-	return HAL_SUCCESS;

-}

-

-

-/*===========================================================================*/

-/* Device-related functions.   	                                     		 */

-/*===========================================================================*/

-

-static uint8_t _find_address(USBHDriver *host) {

-	uint8_t addr, i, j;

-	for (i = 0; i < sizeof_array(host->address_bitmap); i++) {

-		addr = host->address_bitmap[i];

-		for (j = 0; j < 8; j++) {

-			if ((addr & (1 << j)) == 0) {

-				//found:

-				addr = i * 8 + j + 1;

-				host->address_bitmap[i] |= (1 << j);

-				return addr;

-			}

-		}

-	}

-	return 0;

-}

-

-static void _free_address(USBHDriver *host, uint8_t addr) {

-	uinfof("Free address %d", addr);

-	host->address_bitmap[addr / 8] &= ~(1 << ((addr - 1) & 7));

-}

-

-static void _device_initialize(usbh_device_t *dev, usbh_devspeed_t speed) {

-	dev->address = 0;

-	dev->speed = speed;

-	dev->status = USBH_DEVSTATUS_DEFAULT;

-	dev->langID0 = 0;

-	dev->keepFullCfgDesc = 0;

-	_ep0_object_init(dev, 64);

-}

-

-static bool _device_setaddress(usbh_device_t *dev, uint8_t address) {

-	usbh_urbstatus_t ret = usbhControlRequest(dev,

-			USBH_STANDARDOUT(USBH_REQTYPE_DEVICE, USBH_REQ_SET_ADDRESS, address, 0),

-			0,

-			0);

-	if (ret != USBH_URBSTATUS_OK)

-		return HAL_FAILED;

-

-	dev->address = address;

-	return HAL_SUCCESS;

-}

-

-static inline bool _device_read_basic_cfgdesc(usbh_device_t *dev, uint8_t bConfiguration) {

-	/* get configuration descriptor */

-	return usbhStdReqGetConfigurationDescriptor(dev, bConfiguration,

-			sizeof(dev->basicConfigDesc), (uint8_t *)&dev->basicConfigDesc);

-}

-

-static void _device_read_full_cfgdesc(usbh_device_t *dev, uint8_t bConfiguration) {

-	_check_dev(dev);

-

-	uint8_t i;

-

-	if (dev->fullConfigurationDescriptor != NULL) {

-		chHeapFree(dev->fullConfigurationDescriptor);

-	}

-

-	dev->fullConfigurationDescriptor =

-			(uint8_t *)chHeapAlloc(0, dev->basicConfigDesc.wTotalLength);

-

-	if (!dev->fullConfigurationDescriptor)

-		return;

-

-	for (i = 0; i < 3; i++) {

-		if (usbhStdReqGetConfigurationDescriptor(dev, bConfiguration,

-				dev->basicConfigDesc.wTotalLength,

-				dev->fullConfigurationDescriptor) == HAL_SUCCESS) {

-			return;

-		}

-		osalThreadSleepMilliseconds(200);

-	}

-

-	/* error */

-	chHeapFree(dev->fullConfigurationDescriptor);

-	dev->fullConfigurationDescriptor = NULL;

-}

-

-static void _device_free_full_cfgdesc(usbh_device_t *dev) {

-	osalDbgCheck(dev);

-	if (dev->fullConfigurationDescriptor != NULL) {

-		chHeapFree(dev->fullConfigurationDescriptor);

-		dev->fullConfigurationDescriptor = NULL;

-	}

-}

-

-

-#define USBH_SET_CONFIGURATION(type, value, index)	\

-	USBH_STANDARDOUT(type, \

-	USBH_REQ_SET_CONFIGURATION, \

-	value, \

-	index) \

-

-#define USBH_SETDEVICECONFIGURATION(index) \

-	USBH_SET_CONFIGURATION(USBH_REQTYPE_DEVICE, index, 0)

-

-

-static bool _device_set_configuration(usbh_device_t *dev, uint8_t configuration) {

-	usbh_urbstatus_t ret = usbhControlRequest(dev,

-			USBH_SETDEVICECONFIGURATION(configuration),

-			0,

-			0);

-	if (ret != USBH_URBSTATUS_OK)

-		return HAL_FAILED;

-	return HAL_SUCCESS;

-}

-

-static bool _device_configure(usbh_device_t *dev, uint8_t bConfiguration) {

-	uint8_t i;

-

-	uinfof("Reading basic configuration descriptor %d", bConfiguration);

-	for (i = 0; i < 3; i++) {

-		if (!_device_read_basic_cfgdesc(dev, bConfiguration))

-			break;

-	}

-

-	if (i == 3) {

-		uerrf("Could not read basic configuration descriptor %d; "

-					"won't configure device", bConfiguration);

-		return HAL_FAILED;

-	}

-

-	uinfof("Selecting configuration %d", bConfiguration);

-	for (i = 0; i < 3; i++) {

-		if (!_device_set_configuration(dev, dev->basicConfigDesc.bConfigurationValue)) {

-			/* TODO: check if correctly configured using GET_CONFIGURATION */

-			dev->status = USBH_DEVSTATUS_CONFIGURED;

-			dev->bConfiguration = bConfiguration;

-

-			uinfo("Device configured.");

-			return HAL_SUCCESS;

-		}

-	}

-

-	return HAL_FAILED;

-}

-

-static bool _device_enumerate(usbh_device_t *dev) {

-

-	uinfo("Enumerate.");

-	uinfo("Get first 8 bytes of device descriptor");

-

-	/* get first 8 bytes of device descriptor */

-	if (usbhStdReqGetDeviceDescriptor(dev, 8, (uint8_t *)&dev->devDesc)) {

-		uerr("Error");

-		return HAL_FAILED;

-	}

-

-	uinfof("Configure bMaxPacketSize0 = %d", dev->devDesc.bMaxPacketSize0);

-	/* configure EP0 wMaxPacketSize */

-	usbhEPClose(&dev->ctrl);

-	_ep0_object_init(dev, dev->devDesc.bMaxPacketSize0);

-	usbhEPOpen(&dev->ctrl);

-

-	uint8_t addr = _find_address(dev->host);

-	if (addr == 0) {

-		uerr("No free addresses found");

-		return HAL_FAILED;

-	}

-

-	/* set device address */

-	uinfof("Set device address: %d", addr);

-	if (_device_setaddress(dev, addr)) {

-		uerr("Error");

-		_free_address(dev->host, addr);

-		return HAL_FAILED;

-	}

-

-	/* update EP because of the address change */

-	usbhEPClose(&dev->ctrl);

-	_ep0_object_init(dev, dev->devDesc.bMaxPacketSize0);

-	usbhEPOpen(&dev->ctrl);

-

-	uinfof("Wait stabilization...");

-	osalThreadSleepMilliseconds(HAL_USBH_DEVICE_ADDRESS_STABILIZATION);

-

-	/* address is set */

-	dev->status = USBH_DEVSTATUS_ADDRESS;

-

-	uinfof("Get full device desc");

-	/* get full device descriptor */

-	if (usbhStdReqGetDeviceDescriptor(dev, sizeof(dev->devDesc),

-			(uint8_t *)&dev->devDesc)) {

-		uerr("Error");

-		_device_setaddress(dev, 0);

-		_free_address(dev->host, addr);

-		return HAL_FAILED;

-	}

-

-	uinfof("Enumeration finished.");

-	return HAL_SUCCESS;

-}

-

-#if USBH_DEBUG_ENABLE && USBH_DEBUG_ENABLE_INFO

-void usbhDevicePrintInfo(usbh_device_t *dev) {

-	USBH_DEFINE_BUFFER(char, str[64]);

-	usbh_device_descriptor_t *const desc = &dev->devDesc;

-

-	uinfo("----- Device info -----");

-	uinfo("Device descriptor:");

-	uinfof("\tUSBSpec=%04x, #configurations=%d, langID0=%04x",

-			desc->bcdUSB,

-			desc->bNumConfigurations,

-			dev->langID0);

-

-	uinfof("\tClass=%02x, Subclass=%02x, Protocol=%02x",

-			desc->bDeviceClass,

-			desc->bDeviceSubClass,

-			desc->bDeviceProtocol);

-

-	uinfof("\tVID=%04x, PID=%04x, Release=%04x",

-			desc->idVendor,

-			desc->idProduct,

-			desc->bcdDevice);

-

-	if (dev->langID0) {

-		usbhDeviceReadString(dev, str, sizeof(str), desc->iManufacturer, dev->langID0);

-		uinfof("\tManufacturer: %s", str);

-		usbhDeviceReadString(dev, str, sizeof(str), desc->iProduct, dev->langID0);

-		uinfof("\tProduct: %s", str);

-		usbhDeviceReadString(dev, str, sizeof(str), desc->iSerialNumber, dev->langID0);

-		uinfof("\tSerial Number: %s", str);

-	}

-

-	if (dev->status == USBH_DEVSTATUS_CONFIGURED) {

-		uinfo("Configuration descriptor (partial):");

-		usbh_config_descriptor_t *const cfg = &dev->basicConfigDesc;

-		uinfof("\tbConfigurationValue=%d, Length=%d, #interfaces=%d",

-				cfg->bConfigurationValue,

-				cfg->wTotalLength,

-				cfg->bNumInterfaces);

-

-		uinfof("\tCurrent=%dmA", cfg->bMaxPower * 2);

-		uinfof("\tSelfPowered=%d, RemoteWakeup=%d",

-				cfg->bmAttributes & 0x40 ? 1 : 0,

-				cfg->bmAttributes & 0x20 ? 1 : 0);

-		if (dev->langID0) {

-			usbhDeviceReadString(dev, str, sizeof(str), cfg->iConfiguration, dev->langID0);

-			uinfof("\tName: %s", str);

-		}

-	}

-

-	uinfo("----- End Device info -----");

-

-}

-

-void usbhDevicePrintConfiguration(const uint8_t *descriptor, uint16_t rem) {

-	generic_iterator_t iep, icfg, ics;

-	if_iterator_t iif;

-

-	uinfo("----- Configuration info -----");

-	uinfo("Configuration descriptor:");

-	cfg_iter_init(&icfg, descriptor, rem);

-	const usbh_config_descriptor_t *const cfgdesc = cfg_get(&icfg);

-	uinfof("Configuration %d, #IFs=%d", cfgdesc->bConfigurationValue, cfgdesc->bNumInterfaces);

-

-	for (if_iter_init(&iif, &icfg); iif.valid; if_iter_next(&iif)) {

-		const usbh_interface_descriptor_t *const ifdesc = if_get(&iif);

-

-		uinfof("  Interface %d, alt=%d, #EPs=%d, "

-				"Class=%02x, Subclass=%02x, Protocol=%02x",

-				ifdesc->bInterfaceNumber, ifdesc->bAlternateSetting, ifdesc->bNumEndpoints,

-				ifdesc->bInterfaceClass, ifdesc->bInterfaceSubClass, ifdesc->bInterfaceProtocol);

-

-		for (cs_iter_init(&ics, (generic_iterator_t *)&iif); ics.valid; cs_iter_next(&ics)) {

-			uinfof("    Class-Specific descriptor, Length=%d, Type=%02x",

-					ics.curr[0], ics.curr[1]);

-		}

-

-		for (ep_iter_init(&iep, &iif); iep.valid; ep_iter_next(&iep)) {

-			const usbh_endpoint_descriptor_t *const epdesc = ep_get(&iep);

-

-			uinfof("    Endpoint descriptor, Address=%02x, Type=%d, MaxPacket=%d, Interval=%d",

-					epdesc->bEndpointAddress,

-					epdesc->bmAttributes & 3,

-					epdesc->wMaxPacketSize,

-					epdesc->bInterval);

-

-			for (cs_iter_init(&ics, &iep); ics.valid; cs_iter_next(&ics)) {

-				uinfof("    Class-Specific descriptor, Length=%d, Type=%02x",

-						ics.curr[0], ics.curr[1]);

-			}

-		}

-	}

-	uinfo("----- End Configuration info -----");

-}

-#endif

-

-bool usbhDeviceReadString(usbh_device_t *dev, char *dest, uint8_t size,

-		uint8_t index, uint16_t langID) {

-

-	usbh_string_descriptor_t *const desc = (usbh_string_descriptor_t *)dest;

-	osalDbgAssert(size >= 2, "wrong size");

-

-	*dest = 0;

-	if (index == 0)

-		return HAL_SUCCESS;

-	if (usbhStdReqGetStringDescriptor(dev, index, langID, size, (uint8_t *)dest))

-		return HAL_FAILED;

-	if (desc->bLength & 1)

-		return HAL_FAILED;

-	if (desc->bLength <= 2)

-		return HAL_SUCCESS;

-

-	uint8_t nchars = desc->bLength / 2;		/* including the trailing 0 */

-	if (size < nchars)

-		nchars = size;

-

-	char *src = (char *)&desc->wData[0];

-	while (--nchars) {

-		*dest++ = *src;

-		src += 2;

-	}

-	*dest = 0;

-	return HAL_SUCCESS;

-}

-

-

-

-

-/*===========================================================================*/

-/* Port processing functions.  	                                     		 */

-/*===========================================================================*/

-

-static void _port_connected(usbh_port_t *port);

-

-static void _port_reset(usbh_port_t *port) {

-	usbhhubControlRequest(port->device.host,

-#if HAL_USBH_USE_HUB

-			port->hub,

-#endif

-			USBH_REQTYPE_OUT | USBH_REQTYPE_CLASS | USBH_REQTYPE_OTHER,

-			USBH_REQ_SET_FEATURE,

-			USBH_PORT_FEAT_RESET,

-			port->number,

-			0,

-			0);

-}

-

-static void _port_update_status(usbh_port_t *port) {

-	uint32_t stat;

-	if (usbhhubControlRequest(port->device.host,

-#if HAL_USBH_USE_HUB

-			port->hub,

-#endif

-			USBH_REQTYPE_IN | USBH_REQTYPE_CLASS | USBH_REQTYPE_OTHER,

-			USBH_REQ_GET_STATUS,

-			0,

-			port->number,

-			4,

-			(uint8_t *)&stat) != USBH_URBSTATUS_OK) {

-		return;

-	}

-	port->status = stat & 0xffff;

-	port->c_status |= stat >> 16;

-}

-

-static void _port_process_status_change(usbh_port_t *port) {

-

-	_port_update_status(port);

-

-	if (port->c_status & USBH_PORTSTATUS_C_CONNECTION) {

-		/* port connected status changed */

-		port->c_status &= ~USBH_PORTSTATUS_C_CONNECTION;

-		usbhhubClearFeaturePort(port, USBH_PORT_FEAT_C_CONNECTION);

-		if ((port->status & (USBH_PORTSTATUS_CONNECTION | USBH_PORTSTATUS_ENABLE))

-				== USBH_PORTSTATUS_CONNECTION) {

-			if (port->device.status != USBH_DEVSTATUS_DISCONNECTED) {

-				_usbh_port_disconnected(port);

-			}

-

-			/* connected, disabled */

-			_port_connected(port);

-		} else {

-			/* disconnected */

-			_usbh_port_disconnected(port);

-		}

-	}

-

-	if (port->c_status & USBH_PORTSTATUS_C_RESET) {

-		port->c_status &= ~USBH_PORTSTATUS_C_RESET;

-		usbhhubClearFeaturePort(port, USBH_PORT_FEAT_C_RESET);

-	}

-

-	if (port->c_status & USBH_PORTSTATUS_C_ENABLE) {

-		port->c_status &= ~USBH_PORTSTATUS_C_ENABLE;

-		usbhhubClearFeaturePort(port, USBH_PORT_FEAT_C_ENABLE);

-	}

-

-	if (port->c_status & USBH_PORTSTATUS_C_OVERCURRENT) {

-		port->c_status &= ~USBH_PORTSTATUS_C_OVERCURRENT;

-		usbhhubClearFeaturePort(port, USBH_PORT_FEAT_C_OVERCURRENT);

-	}

-

-	if (port->c_status & USBH_PORTSTATUS_C_SUSPEND) {

-		port->c_status &= ~USBH_PORTSTATUS_C_SUSPEND;

-		usbhhubClearFeaturePort(port, USBH_PORT_FEAT_C_SUSPEND);

-	}

-

-}

-

-

-static void _port_connected(usbh_port_t *port) {

-	/* connected */

-

-	systime_t start;

-	uint8_t i;

-	uint8_t retries;

-	usbh_devspeed_t speed;

-	USBH_DEFINE_BUFFER(usbh_string_descriptor_t, strdesc);

-

-	uinfof("Port %d connected, wait debounce...", port->number);

-

-	port->device.status = USBH_DEVSTATUS_ATTACHED;

-

-	/* wait for attach de-bounce */

-	osalThreadSleepMilliseconds(HAL_USBH_PORT_DEBOUNCE_TIME);

-

-	/* check disconnection */

-	_port_update_status(port);

-	if (port->c_status & USBH_PORTSTATUS_C_CONNECTION) {

-		/* connection state changed; abort */

-		goto abort;

-	}

-

-	port->device.status = USBH_DEVSTATUS_CONNECTED;

-	retries = 3;

-

-reset:

-	for (i = 0; i < 3; i++) {

-		uinfo("Try reset...");

-		port->c_status &= ~(USBH_PORTSTATUS_C_RESET | USBH_PORTSTATUS_C_ENABLE);

-		_port_reset(port);

-		osalThreadSleepMilliseconds(20);	/* give it some time to reset (min. 10ms) */

-		start = osalOsGetSystemTimeX();

-		while (TRUE) {

-			_port_update_status(port);

-

-			/* check for disconnection */

-			if (port->c_status & USBH_PORTSTATUS_C_CONNECTION)

-				goto abort;

-

-			/* check for reset completion */

-			if (port->c_status & USBH_PORTSTATUS_C_RESET) {

-				port->c_status &= ~USBH_PORTSTATUS_C_RESET;

-				usbhhubClearFeaturePort(port, USBH_PORT_FEAT_C_RESET);

-

-				if ((port->status & (USBH_PORTSTATUS_ENABLE | USBH_PORTSTATUS_CONNECTION))

-						== (USBH_PORTSTATUS_ENABLE | USBH_PORTSTATUS_CONNECTION)) {

-					goto reset_success;

-				}

-			}

-

-			/* check for timeout */

-			if (osalOsGetSystemTimeX() - start > HAL_USBH_PORT_RESET_TIMEOUT) break;

-		}

-	}

-

-	/* reset procedure failed; abort */

-	goto abort;

-

-reset_success:

-

-	uinfo("Reset OK, recovery...");

-

-	/* reset recovery */

-	osalThreadSleepMilliseconds(100);

-

-	/* initialize object */

-	if (port->status & USBH_PORTSTATUS_LOW_SPEED) {

-		speed = USBH_DEVSPEED_LOW;

-	} else if (port->status & USBH_PORTSTATUS_HIGH_SPEED) {

-		speed = USBH_DEVSPEED_HIGH;

-	} else {

-		speed = USBH_DEVSPEED_FULL;

-	}

-	_device_initialize(&port->device, speed);

-	usbhEPOpen(&port->device.ctrl);

-

-	/* device with default address (0), try enumeration */

-	if (_device_enumerate(&port->device)) {

-		/* enumeration failed */

-		usbhEPClose(&port->device.ctrl);

-

-		if (!--retries)

-			goto abort;

-

-		/* retry reset & enumeration */

-		goto reset;

-	}

-

-	/* load the default language ID */

-	uinfo("Loading langID0...");

-	if (!usbhStdReqGetStringDescriptor(&port->device, 0, 0,

-			USBH_DT_STRING_SIZE, (uint8_t *)&strdesc)

-		&& (strdesc.bLength >= 4)

-		&& !usbhStdReqGetStringDescriptor(&port->device, 0, 0,

-			4, (uint8_t *)&strdesc)) {

-

-		port->device.langID0 = strdesc.wData[0];

-		uinfof("langID0=%04x", port->device.langID0);

-	}

-

-	/* check if the device has only one configuration */

-	if (port->device.devDesc.bNumConfigurations == 1) {

-		uinfo("Device has only one configuration");

-		_device_configure(&port->device, 0);

-	}

-

-	_classdriver_process_device(&port->device);

-	return;

-

-abort:

-	uerr("Abort");

-	port->device.status = USBH_DEVSTATUS_DISCONNECTED;

-}

-

-void _usbh_port_disconnected(usbh_port_t *port) {

-	if (port->device.status == USBH_DEVSTATUS_DISCONNECTED)

-		return;

-

-	uinfo("Port disconnected");

-

-	/* unload drivers */

-	while (port->device.drivers) {

-		usbh_baseclassdriver_t *drv = port->device.drivers;

-

-		/* unload */

-		uinfof("Unload driver %s", drv->info->name);

-		drv->info->vmt->unload(drv);

-

-		/* unlink */

-		drv->dev = 0;

-		port->device.drivers = drv->next;

-	}

-

-	/* close control endpoint */

-	osalSysLock();

-	usbhEPCloseS(&port->device.ctrl);

-	osalSysUnlock();

-

-	/* free address */

-	if (port->device.address)

-		_free_address(port->device.host, port->device.address);

-

-	_device_free_full_cfgdesc(&port->device);

-

-	port->device.status = USBH_DEVSTATUS_DISCONNECTED;

-}

-

-

-

-/*===========================================================================*/

-/* Hub processing functions.  	                                     		 */

-/*===========================================================================*/

-

-#if HAL_USBH_USE_HUB

-static void _hub_update_status(USBHDriver *host, USBHHubDriver *hub) {

-	uint32_t stat;

-	if (usbhhubControlRequest(host,

-			hub,

-			USBH_REQTYPE_IN | USBH_REQTYPE_CLASS | USBH_REQTYPE_DEVICE,

-			USBH_REQ_GET_STATUS,

-			0,

-			0,

-			4,

-			(uint8_t *)&stat) != USBH_URBSTATUS_OK) {

-		return;

-	}

-	if (hub) {

-		hub->status = stat & 0xffff;

-		hub->c_status |= stat >> 16;

-	}

-}

-

-static void _hub_process_status_change(USBHDriver *host, USBHHubDriver *hub) {

-	uinfo("Hub status change. GET_STATUS.");

-	_hub_update_status(host, hub);

-

-	if (hub->c_status & USBH_HUBSTATUS_C_HUB_LOCAL_POWER) {

-		hub->c_status &= ~USBH_HUBSTATUS_C_HUB_LOCAL_POWER;

-		uinfo("Clear USBH_HUB_FEAT_C_HUB_LOCAL_POWER");

-		usbhhubClearFeatureHub(host, hub, USBH_HUB_FEAT_C_HUB_LOCAL_POWER);

-	}

-

-	if (hub->c_status & USBH_HUBSTATUS_C_HUB_OVER_CURRENT) {

-		hub->c_status &= ~USBH_HUBSTATUS_C_HUB_OVER_CURRENT;

-		uinfo("Clear USBH_HUB_FEAT_C_HUB_OVER_CURRENT");

-		usbhhubClearFeatureHub(host, hub, USBH_HUB_FEAT_C_HUB_OVER_CURRENT);

-	}

-}

-

-static uint32_t _hub_get_status_change_bitmap(USBHDriver *host, USBHHubDriver *hub) {

-	if (hub != NULL) {

-		osalSysLock();

-		uint32_t ret = hub->statuschange;

-		hub->statuschange = 0;

-		osalOsRescheduleS();

-		osalSysUnlock();

-		return ret;

-	}

-	return usbh_lld_roothub_get_statuschange_bitmap(host);

-}

-

-#else

-//TODO: replace the functions above

-#endif

-

-#if HAL_USBH_USE_HUB

-static void _hub_process(USBHDriver *host, USBHHubDriver *hub) {

-	uint32_t bitmap = _hub_get_status_change_bitmap(host, hub);

-	if (!bitmap)

-		return;

-

-	if (bitmap & 1) {

-		_hub_process_status_change(host, hub);

-		bitmap &= ~1;

-	}

-

-	usbh_port_t *port = (hub == NULL) ? &host->rootport : hub->ports;

-	uint8_t i;

-	for (i = 1; i < 32; i++) {

-		if (!bitmap || !port)

-			break;

-		if (bitmap & (1 << i)) {

-			bitmap &= ~(1 << i);

-			_port_process_status_change(port);

-		}

-		port = port->next;

-	}

-

-}

-#else

-static void _hub_process(USBHDriver *host) {

-	uint32_t bitmap = usbh_lld_roothub_get_statuschange_bitmap(host);

-

-#if 0	//TODO: complete _hub_process_status_change for root hub

-	if (bitmap & 1) {

-		_hub_process_status_change(host, hub);

-		bitmap &= ~1;

-	}

-#endif

-

-	if (!bitmap)

-		return;

-

-	_port_process_status_change(&host->rootport);

-}

-#endif

-

-/*===========================================================================*/

-/* Main processing loop (enumeration, loading/unloading drivers, etc). 		 */

-/*===========================================================================*/

-void usbhMainLoop(USBHDriver *usbh) {

-

-	if (usbh->status == USBH_STATUS_STOPPED)

-		return;

-

-#if HAL_USBH_USE_HUB

-	/* process root hub */

-	_hub_process(usbh, NULL);

-

-	/* process connected hubs */

-	USBHHubDriver *hub;

-    list_for_each_entry(hub, USBHHubDriver, &usbh->hubs, node) {

-		_hub_process(usbh, hub);

-	}

-#else

-	/* process root hub */

-	_hub_process(usbh);

-#endif

-}

-

-

-/*===========================================================================*/

-/* IAD class driver.											 		 	 */

-/*===========================================================================*/

-#if HAL_USBH_USE_IAD

-static usbh_baseclassdriver_t *iad_load(usbh_device_t *dev, const uint8_t *descriptor, uint16_t rem);

-static void iad_unload(usbh_baseclassdriver_t *drv);

-static const usbh_classdriver_vmt_t usbhiadClassDriverVMT = {

-	iad_load,

-	iad_unload

-};

-static const usbh_classdriverinfo_t usbhiadClassDriverInfo = {

-	0xef, 0x02, 0x01, "IAD", &usbhiadClassDriverVMT

-};

-

-static usbh_baseclassdriver_t *iad_load(usbh_device_t *dev,

-		const uint8_t *descriptor, uint16_t rem) {

-	(void)rem;

-

-	if (descriptor[1] != USBH_DT_DEVICE)

-		return 0;

-

-	uinfo("Load a driver for each IF collection.");

-

-	generic_iterator_t icfg;

-	if_iterator_t iif;

-	const usbh_ia_descriptor_t *last_iad = 0;

-

-	cfg_iter_init(&icfg, dev->fullConfigurationDescriptor,

-			dev->basicConfigDesc.wTotalLength);

-	if (!icfg.valid) {

-		uerr("Invalid configuration descriptor.");

-		return 0;

-	}

-

-	for (if_iter_init(&iif, &icfg); iif.valid; if_iter_next(&iif)) {

-		if (iif.iad && (iif.iad != last_iad)) {

-			last_iad = iif.iad;

-			if (_classdriver_load(dev, iif.iad->bFunctionClass,

-					iif.iad->bFunctionSubClass,

-					iif.iad->bFunctionProtocol,

-					(uint8_t *)iif.iad,

-					(uint8_t *)iif.curr - (uint8_t *)iif.iad + iif.rem) != HAL_SUCCESS) {

-				uwarnf("No drivers found for IF collection #%d:%d",

-						iif.iad->bFirstInterface,

-						iif.iad->bFirstInterface + iif.iad->bInterfaceCount - 1);

-			}

-		}

-	}

-

-	return 0;

-}

-

-static void iad_unload(usbh_baseclassdriver_t *drv) {

-	(void)drv;

-}

-#endif

-

-

-/*===========================================================================*/

-/* Class driver loader.											 		 	 */

-/*===========================================================================*/

-

-static const usbh_classdriverinfo_t *usbh_classdrivers_lookup[] = {

-#if HAL_USBH_USE_FTDI

-	&usbhftdiClassDriverInfo,

-#endif

-#if HAL_USBH_USE_IAD

-	&usbhiadClassDriverInfo,

-#endif

-#if HAL_USBH_USE_UVC

-	&usbhuvcClassDriverInfo,

-#endif

-#if HAL_USBH_USE_MSD

-	&usbhmsdClassDriverInfo,

-#endif

-#if HAL_USBH_USE_HUB

-	&usbhhubClassDriverInfo

-#endif

-};

-

-static bool _classdriver_load(usbh_device_t *dev, uint8_t class,

-		uint8_t subclass, uint8_t protocol, uint8_t *descbuff, uint16_t rem) {

-	uint8_t i;

-	usbh_baseclassdriver_t *drv = NULL;

-	for (i = 0; i < sizeof_array(usbh_classdrivers_lookup); i++) {

-		const usbh_classdriverinfo_t *const info = usbh_classdrivers_lookup[i];

-		if (class == 0xff) {

-			/* vendor specific */

-			if (info->class == 0xff) {

-				uinfof("Try load vendor-specific driver %s", info->name);

-				drv = info->vmt->load(dev, descbuff, rem);

-				if (drv != NULL)

-					goto success;

-			}

-		} else if ((info->class < 0) || ((info->class == class)

-			&& ((info->subclass < 0) || ((info->subclass == subclass)

-			&& ((info->protocol < 0) || (info->protocol == protocol)))))) {

-			uinfof("Try load driver %s", info->name);

-			drv = info->vmt->load(dev, descbuff, rem);

-

-#if HAL_USBH_USE_IAD

-			/* special case: */

-			if (info == &usbhiadClassDriverInfo)

-				return HAL_SUCCESS;

-#endif

-

-			if (drv != NULL)

-				goto success;

-		}

-	}

-	return HAL_FAILED;

-

-success:

-	/* Link this driver to the device */

-	drv->next = dev->drivers;

-	dev->drivers = drv;

-	drv->dev = dev;

-	return HAL_SUCCESS;

-}

-

-static void _classdriver_process_device(usbh_device_t *dev) {

-	uinfo("New device found.");

-	const usbh_device_descriptor_t *const devdesc = &dev->devDesc;

-

-	usbhDevicePrintInfo(dev);

-

-	/* TODO: Support multiple configurations

-	 *

-	 * Windows doesn't support them, so it's unlikely that any commercial USB device

-	 * will have multiple configurations.

-	 */

-	if (dev->status != USBH_DEVSTATUS_CONFIGURED) {

-		uwarn("Multiple configurations not supported, selecting configuration #0");

-		if (_device_configure(dev, 0) != HAL_SUCCESS) {

-			uerr("Couldn't configure device; abort.");

-			return;

-		}

-	}

-

-	_device_read_full_cfgdesc(dev, dev->bConfiguration);

-	if (dev->fullConfigurationDescriptor == NULL) {

-		uerr("Couldn't read full configuration descriptor; abort.");

-		return;

-	}

-

-	usbhDevicePrintConfiguration(dev->fullConfigurationDescriptor,

-			dev->basicConfigDesc.wTotalLength);

-

-	if (devdesc->bDeviceClass == 0) {

-		/* each interface defines its own device class/subclass/protocol */

-		uinfo("Load a driver for each IF.");

-

-		generic_iterator_t icfg;

-		if_iterator_t iif;

-		uint8_t last_if = 0xff;

-

-		cfg_iter_init(&icfg, dev->fullConfigurationDescriptor,

-				dev->basicConfigDesc.wTotalLength);

-		if (!icfg.valid) {

-			uerr("Invalid configuration descriptor.");

-			goto exit;

-		}

-

-		for (if_iter_init(&iif, &icfg); iif.valid; if_iter_next(&iif)) {

-			const usbh_interface_descriptor_t *const ifdesc = if_get(&iif);

-			if (ifdesc->bInterfaceNumber != last_if) {

-				last_if = ifdesc->bInterfaceNumber;

-				if (_classdriver_load(dev, ifdesc->bInterfaceClass,

-						ifdesc->bInterfaceSubClass,

-						ifdesc->bInterfaceProtocol,

-						(uint8_t *)ifdesc, iif.rem) != HAL_SUCCESS) {

-					uwarnf("No drivers found for IF #%d", ifdesc->bInterfaceNumber);

-				}

-			}

-		}

-

-	} else {

-		if (_classdriver_load(dev, devdesc->bDeviceClass,

-				devdesc->bDeviceSubClass,

-				devdesc->bDeviceProtocol,

-				(uint8_t *)devdesc, USBH_DT_DEVICE_SIZE) != HAL_SUCCESS) {

-			uwarn("No drivers found.");

-		}

-	}

-

-exit:

-	if (dev->keepFullCfgDesc == 0) {

-		_device_free_full_cfgdesc(dev);

-	}

-}

-

-

-#endif

-

+/*
+    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio
+              Copyright (C) 2015 Diego Ismirlian, TISA, (dismirlian (at) google's mail)
+
+    Licensed under the Apache License, Version 2.0 (the "License");
+    you may not use this file except in compliance with the License.
+    You may obtain a copy of the License at
+
+        http://www.apache.org/licenses/LICENSE-2.0
+
+    Unless required by applicable law or agreed to in writing, software
+    distributed under the License is distributed on an "AS IS" BASIS,
+    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+    See the License for the specific language governing permissions and
+    limitations under the License.
+*/
+
+#include "hal.h"
+
+#if HAL_USE_USBH
+
+#include "usbh/dev/hub.h"
+#include "usbh/internal.h"
+#include <string.h>
+
+#if USBH_DEBUG_ENABLE_TRACE
+#define udbgf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)
+#define udbg(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)
+#else
+#define udbgf(f, ...)  do {} while(0)
+#define udbg(f, ...)   do {} while(0)
+#endif
+
+#if USBH_DEBUG_ENABLE_INFO
+#define uinfof(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)
+#define uinfo(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)
+#else
+#define uinfof(f, ...)  do {} while(0)
+#define uinfo(f, ...)   do {} while(0)
+#endif
+
+#if USBH_DEBUG_ENABLE_WARNINGS
+#define uwarnf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)
+#define uwarn(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)
+#else
+#define uwarnf(f, ...)  do {} while(0)
+#define uwarn(f, ...)   do {} while(0)
+#endif
+
+#if USBH_DEBUG_ENABLE_ERRORS
+#define uerrf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)
+#define uerr(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)
+#else
+#define uerrf(f, ...)  do {} while(0)
+#define uerr(f, ...)   do {} while(0)
+#endif
+
+#if STM32_USBH_USE_OTG1
+USBHDriver USBHD1;
+#endif
+#if STM32_USBH_USE_OTG2
+USBHDriver USBHD2;
+#endif
+
+
+static void _classdriver_process_device(usbh_device_t *dev);
+static bool _classdriver_load(usbh_device_t *dev, uint8_t class,
+		uint8_t subclass, uint8_t protocol, uint8_t *descbuff, uint16_t rem);
+
+
+/*===========================================================================*/
+/* Checks.								                                     */
+/*===========================================================================*/
+
+static inline void _check_dev(usbh_device_t *dev) {
+	osalDbgCheck(dev);
+	//TODO: add more checks.
+}
+
+static inline void _check_ep(usbh_ep_t *ep) {
+	osalDbgCheck(ep != 0);
+	_check_dev(ep->device);
+	osalDbgCheck(ep->type <= 3);
+	//TODO: add more checks.
+}
+
+static inline void _check_urb(usbh_urb_t *urb) {
+	osalDbgCheck(urb != 0);
+	_check_ep(urb->ep);
+	osalDbgCheck((urb->buff != NULL) || (urb->requestedLength == 0));
+	//TODO: add more checks.
+}
+
+/*===========================================================================*/
+/* Main driver API.						                                     */
+/*===========================================================================*/
+
+void usbhObjectInit(USBHDriver *usbh) {
+	memset(usbh, 0, sizeof(*usbh));
+	usbh->status = USBH_STATUS_STOPPED;
+#if HAL_USBH_USE_HUB
+	INIT_LIST_HEAD(&usbh->hubs);
+	_usbhub_port_object_init(&usbh->rootport, usbh, 0, 1);
+#else
+	_usbhub_port_object_init(&usbh->rootport, usbh, 1);
+#endif
+}
+
+void usbhInit(void) {
+#if HAL_USBH_USE_HUB
+	uint8_t i;
+	for (i = 0; i < HAL_USBHHUB_MAX_INSTANCES; i++) {
+		usbhhubObjectInit(&USBHHUBD[i]);
+	}
+#endif
+	usbh_lld_init();
+}
+
+void usbhStart(USBHDriver *usbh) {
+	usbDbgInit(usbh);
+
+	osalSysLock();
+	osalDbgAssert((usbh->status == USBH_STATUS_STOPPED) || (usbh->status == USBH_STATUS_STARTED),
+				"invalid state");
+	usbh_lld_start(usbh);
+	usbh->status = USBH_STATUS_STARTED;
+	osalOsRescheduleS();
+	osalSysUnlock();
+}
+
+
+void usbhStop(USBHDriver *usbh) {
+	//TODO: implement
+	(void)usbh;
+}
+void usbhSuspend(USBHDriver *usbh) {
+	//TODO: implement
+	(void)usbh;
+}
+void usbhResume(USBHDriver *usbh) {
+	//TODO: implement
+	(void)usbh;
+}
+
+/*===========================================================================*/
+/* Endpoint API.						                                     */
+/*===========================================================================*/
+
+void usbhEPObjectInit(usbh_ep_t *ep, usbh_device_t *dev, const usbh_endpoint_descriptor_t *desc) {
+	osalDbgCheck(ep);
+	_check_dev(dev);
+	osalDbgCheck(desc);
+
+	memset(ep, 0, sizeof(*ep));
+	ep->device = dev;
+	ep->wMaxPacketSize = desc->wMaxPacketSize;
+	ep->address = desc->bEndpointAddress & 0x0F;
+	ep->type = (usbh_eptype_t) (desc->bmAttributes & 0x03);
+	if (ep->type != USBH_EPTYPE_CTRL) {
+		ep->in = (desc->bEndpointAddress & 0x80) ? TRUE : FALSE;
+	}
+	ep->bInterval = desc->bInterval;
+
+	/* low-level part */
+	usbh_lld_ep_object_init(ep);
+
+	ep->status = USBH_EPSTATUS_CLOSED;
+}
+
+
+static void _ep0_object_init(usbh_device_t *dev, uint16_t wMaxPacketSize) {
+	const usbh_endpoint_descriptor_t ep0_descriptor = {
+		7,	//bLength
+		5,	//bDescriptorType
+		0,	//bEndpointAddress
+		0,	//bmAttributes
+		wMaxPacketSize,
+		0,	//bInterval
+	};
+	usbhEPObjectInit(&dev->ctrl, dev, &ep0_descriptor);
+	usbhEPSetName(&dev->ctrl, "DEV[CTRL]");
+}
+
+
+/*===========================================================================*/
+/* URB API.				    	                                     		 */
+/*===========================================================================*/
+
+void usbhURBObjectInit(usbh_urb_t *urb, usbh_ep_t *ep, usbh_completion_cb callback,
+		void *user, void *buff, uint32_t len) {
+
+	osalDbgCheck(urb != 0);
+	_check_ep(ep);
+
+	/* initialize the common part: */
+	urb->ep = ep;
+	urb->callback = callback;
+	urb->userData = user;
+	urb->buff = buff;
+	urb->requestedLength = len;
+	urb->actualLength = 0;
+	urb->status = USBH_URBSTATUS_INITIALIZED;
+	urb->waitingThread = 0;
+	urb->abortingThread = 0;
+
+	/* initialize the ll part: */
+	usbh_lld_urb_object_init(urb);
+}
+
+void usbhURBObjectResetI(usbh_urb_t *urb) {
+	osalDbgAssert(!usbhURBIsBusy(urb), "invalid status");
+
+	osalDbgCheck((urb->waitingThread == 0) && (urb->abortingThread == 0));
+
+	urb->actualLength = 0;
+	urb->status = USBH_URBSTATUS_INITIALIZED;
+
+	/* reset the ll part: */
+	usbh_lld_urb_object_reset(urb);
+}
+
+void usbhURBSubmitI(usbh_urb_t *urb) {
+	osalDbgCheckClassI();
+	_check_urb(urb);
+	osalDbgAssert(urb->status == USBH_URBSTATUS_INITIALIZED, "invalid status");
+	usbh_ep_t *const ep = urb->ep;
+	if (ep->status == USBH_EPSTATUS_HALTED) {
+		_usbh_urb_completeI(urb, USBH_URBSTATUS_STALL);
+		return;
+	}
+	if (ep->status != USBH_EPSTATUS_OPEN) {
+		_usbh_urb_completeI(urb, USBH_URBSTATUS_DISCONNECTED);
+		return;
+	}
+	if (!(usbhDeviceGetPort(ep->device)->status & USBH_PORTSTATUS_ENABLE)) {
+		_usbh_urb_completeI(urb, USBH_URBSTATUS_DISCONNECTED);
+		return;
+	}
+	urb->status = USBH_URBSTATUS_PENDING;
+	usbh_lld_urb_submit(urb);
+}
+
+bool _usbh_urb_abortI(usbh_urb_t *urb, usbh_urbstatus_t status) {
+	osalDbgCheckClassI();
+	_check_urb(urb);
+
+	switch (urb->status) {
+/*	case USBH_URBSTATUS_UNINITIALIZED:
+ * 	case USBH_URBSTATUS_INITIALIZED:
+ *	case USBH_URBSTATUS_ERROR:
+ *	case USBH_URBSTATUS_TIMEOUT:
+ *	case USBH_URBSTATUS_CANCELLED:
+ *	case USBH_URBSTATUS_STALL:
+ *	case USBH_URBSTATUS_DISCONNECTED:
+ *	case USBH_URBSTATUS_OK: */
+	default:
+		/* already finished */
+		_usbh_urb_completeI(urb, status);
+		return TRUE;
+
+//	case USBH_URBSTATUS_QUEUED:
+	case USBH_URBSTATUS_PENDING:
+		return usbh_lld_urb_abort(urb, status);
+	}
+}
+
+void _usbh_urb_abort_and_waitS(usbh_urb_t *urb, usbh_urbstatus_t status) {
+	osalDbgCheckClassS();
+	_check_urb(urb);
+
+	if (_usbh_urb_abortI(urb, status) == FALSE) {
+		uwarn("URB wasn't aborted immediately, suspend");
+		osalThreadSuspendS(&urb->abortingThread);
+		urb->abortingThread = 0;
+	} else {
+		osalOsRescheduleS();
+	}
+	uwarn("URB aborted");
+}
+
+bool usbhURBCancelI(usbh_urb_t *urb) {
+	return _usbh_urb_abortI(urb, USBH_URBSTATUS_CANCELLED);
+}
+
+void usbhURBCancelAndWaitS(usbh_urb_t *urb) {
+	_usbh_urb_abort_and_waitS(urb, USBH_URBSTATUS_CANCELLED);
+}
+
+msg_t usbhURBWaitTimeoutS(usbh_urb_t *urb, systime_t timeout) {
+	msg_t ret;
+
+	osalDbgCheckClassS();
+	_check_urb(urb);
+
+	switch (urb->status) {
+	case USBH_URBSTATUS_INITIALIZED:
+	case USBH_URBSTATUS_PENDING:
+//	case USBH_URBSTATUS_QUEUED:
+		ret = osalThreadSuspendTimeoutS(&urb->waitingThread, timeout);
+		urb->waitingThread = 0;
+		break;
+
+	case USBH_URBSTATUS_OK:
+		ret = MSG_OK;
+		osalOsRescheduleS();
+		break;
+
+/*	case USBH_URBSTATUS_UNINITIALIZED:
+ *	case USBH_URBSTATUS_ERROR:
+ *	case USBH_URBSTATUS_TIMEOUT:
+ *	case USBH_URBSTATUS_CANCELLED:
+ *	case USBH_URBSTATUS_STALL:
+ *	case USBH_URBSTATUS_DISCONNECTED: */
+	default:
+		ret = MSG_RESET;
+		osalOsRescheduleS();
+		break;
+	}
+	return ret;
+}
+
+msg_t usbhURBSubmitAndWaitS(usbh_urb_t *urb, systime_t timeout) {
+	msg_t ret;
+
+	osalDbgCheckClassS();
+	_check_urb(urb);
+
+	usbhURBSubmitI(urb);
+	ret = usbhURBWaitTimeoutS(urb, timeout);
+	if (ret == MSG_TIMEOUT)
+		_usbh_urb_abort_and_waitS(urb, USBH_URBSTATUS_TIMEOUT);
+
+	return ret;
+}
+
+static inline msg_t _wakeup_message(usbh_urbstatus_t status) {
+	if (status == USBH_URBSTATUS_OK) return MSG_OK;
+	if (status == USBH_URBSTATUS_TIMEOUT) return MSG_TIMEOUT;
+	return MSG_RESET;
+}
+
+void _usbh_urb_completeI(usbh_urb_t *urb, usbh_urbstatus_t status) {
+	osalDbgCheckClassI();
+	_check_urb(urb);
+	urb->status = status;
+	osalThreadResumeI(&urb->waitingThread, _wakeup_message(status));
+	osalThreadResumeI(&urb->abortingThread, MSG_RESET);
+	if (urb->callback)
+		urb->callback(urb);
+}
+
+/*===========================================================================*/
+/* Synchronous API.		    	                                     		 */
+/*===========================================================================*/
+
+usbh_urbstatus_t usbhBulkTransfer(usbh_ep_t *ep,
+		void *data,
+		uint32_t len,
+		uint32_t *actual_len,
+		systime_t timeout) {
+
+	osalDbgCheck(ep != NULL);
+	osalDbgCheck((data != NULL) || (len == 0));
+	osalDbgAssert(ep->type == USBH_EPTYPE_BULK, "wrong ep");
+
+	usbh_urb_t urb;
+	usbhURBObjectInit(&urb, ep, 0, 0, data, len);
+
+	osalSysLock();
+	usbhURBSubmitAndWaitS(&urb, timeout);
+	osalSysUnlock();
+
+	if (actual_len != NULL)
+		*actual_len = urb.actualLength;
+
+	return urb.status;
+}
+
+usbh_urbstatus_t usbhControlRequestExtended(usbh_device_t *dev,
+		const usbh_control_request_t *req,
+		uint8_t *buff,
+		uint32_t *actual_len,
+		systime_t timeout) {
+
+	_check_dev(dev);
+	osalDbgCheck(req != NULL);
+
+	usbh_urb_t urb;
+
+	usbhURBObjectInit(&urb, &dev->ctrl, 0, 0, buff, req->wLength);
+	urb.setup_buff = req;
+
+	osalSysLock();
+	usbhURBSubmitAndWaitS(&urb, timeout);
+	osalSysUnlock();
+
+	if (actual_len != NULL)
+		*actual_len = urb.actualLength;
+
+	return urb.status;
+}
+
+usbh_urbstatus_t usbhControlRequest(usbh_device_t *dev,
+		uint8_t bmRequestType,
+		uint8_t bRequest,
+		uint16_t wValue,
+		uint16_t wIndex,
+		uint16_t wLength,
+		uint8_t *buff) {
+
+	const USBH_DEFINE_BUFFER(usbh_control_request_t, req) = {
+			bmRequestType,
+			bRequest,
+			wValue,
+			wIndex,
+			wLength
+	};
+	return usbhControlRequestExtended(dev, &req, buff, NULL, MS2ST(1000));
+}
+
+/*===========================================================================*/
+/* Standard request helpers.   	                                     		 */
+/*===========================================================================*/
+
+#define USBH_GET_DESCRIPTOR(type, value, index)	\
+	USBH_STANDARDIN(type, \
+	USBH_REQ_GET_DESCRIPTOR, \
+	value, \
+	index) \
+
+#define USBH_GETDEVICEDESCRIPTOR \
+	USBH_GET_DESCRIPTOR(USBH_REQTYPE_DEVICE, (USBH_DT_DEVICE << 8) | 0, 0)
+
+#define USBH_GETCONFIGURATIONDESCRIPTOR(index) \
+	USBH_GET_DESCRIPTOR(USBH_REQTYPE_DEVICE, (USBH_DT_CONFIG << 8) | index, 0)
+
+#define USBH_GETSTRINGDESCRIPTOR(index, langID) \
+	USBH_GET_DESCRIPTOR(USBH_REQTYPE_DEVICE, (USBH_DT_STRING << 8) | index, langID)
+
+bool usbhStdReqGetDeviceDescriptor(usbh_device_t *dev,
+		uint16_t wLength,
+		uint8_t *buf) {
+	usbh_device_descriptor_t *desc;
+	usbh_urbstatus_t ret = usbhControlRequest(dev, USBH_GETDEVICEDESCRIPTOR, wLength, buf);
+	desc = (usbh_device_descriptor_t *)buf;
+	if ((ret != USBH_URBSTATUS_OK)
+			|| (desc->bLength != USBH_DT_DEVICE_SIZE)
+			|| (desc->bDescriptorType != USBH_DT_DEVICE)) {
+		return HAL_FAILED;
+	}
+	return HAL_SUCCESS;
+}
+
+bool usbhStdReqGetConfigurationDescriptor(usbh_device_t *dev,
+		uint8_t index,
+		uint16_t wLength,
+		uint8_t *buf) {
+	usbh_urbstatus_t ret = usbhControlRequest(dev, USBH_GETCONFIGURATIONDESCRIPTOR(index), wLength, buf);
+	usbh_config_descriptor_t *const desc = (usbh_config_descriptor_t *)buf;
+	if ((ret != USBH_URBSTATUS_OK)
+			|| (desc->bLength < USBH_DT_CONFIG_SIZE)
+			|| (desc->bDescriptorType != USBH_DT_CONFIG)) {
+		return HAL_FAILED;
+	}
+	return HAL_SUCCESS;
+}
+
+bool usbhStdReqGetStringDescriptor(usbh_device_t *dev,
+		uint8_t index,
+		uint16_t langID,
+		uint16_t wLength,
+		uint8_t *buf) {
+
+	osalDbgAssert(wLength >= USBH_DT_STRING_SIZE, "wrong size");
+	usbh_string_descriptor_t *desc = (usbh_string_descriptor_t *)buf;
+	usbh_urbstatus_t ret = usbhControlRequest(dev, USBH_GETSTRINGDESCRIPTOR(index, langID), wLength, buf);
+	if ((ret != USBH_URBSTATUS_OK)
+			|| (desc->bLength < USBH_DT_STRING_SIZE)
+			|| (desc->bDescriptorType != USBH_DT_STRING)) {
+		return HAL_FAILED;
+	}
+	return HAL_SUCCESS;
+}
+
+
+
+#define USBH_SET_INTERFACE(interface, alt)	\
+	USBH_STANDARDOUT(USBH_REQTYPE_INTERFACE, \
+	USBH_REQ_SET_INTERFACE, \
+	alt, \
+	interface) \
+
+#define USBH_GET_INTERFACE(interface)	\
+	USBH_STANDARDIN(USBH_REQTYPE_INTERFACE, \
+	USBH_REQ_GET_INTERFACE, \
+	0, \
+	interface) \
+
+bool usbhStdReqSetInterface(usbh_device_t *dev,
+		uint8_t bInterfaceNumber,
+		uint8_t bAlternateSetting) {
+
+	usbh_urbstatus_t ret = usbhControlRequest(dev, USBH_SET_INTERFACE(bInterfaceNumber, bAlternateSetting), 0, NULL);
+	if (ret != USBH_URBSTATUS_OK)
+		return HAL_FAILED;
+
+	return HAL_SUCCESS;
+}
+
+bool usbhStdReqGetInterface(usbh_device_t *dev,
+		uint8_t bInterfaceNumber,
+		uint8_t *bAlternateSetting) {
+
+	USBH_DEFINE_BUFFER(uint8_t, alt);
+
+	usbh_urbstatus_t ret = usbhControlRequest(dev, USBH_GET_INTERFACE(bInterfaceNumber), 1, &alt);
+	if (ret != USBH_URBSTATUS_OK)
+		return HAL_FAILED;
+
+	*bAlternateSetting = alt;
+	return HAL_SUCCESS;
+}
+
+
+/*===========================================================================*/
+/* Device-related functions.   	                                     		 */
+/*===========================================================================*/
+
+static uint8_t _find_address(USBHDriver *host) {
+	uint8_t addr, i, j;
+	for (i = 0; i < sizeof_array(host->address_bitmap); i++) {
+		addr = host->address_bitmap[i];
+		for (j = 0; j < 8; j++) {
+			if ((addr & (1 << j)) == 0) {
+				//found:
+				addr = i * 8 + j + 1;
+				host->address_bitmap[i] |= (1 << j);
+				return addr;
+			}
+		}
+	}
+	return 0;
+}
+
+static void _free_address(USBHDriver *host, uint8_t addr) {
+	uinfof("Free address %d", addr);
+	host->address_bitmap[addr / 8] &= ~(1 << ((addr - 1) & 7));
+}
+
+static void _device_initialize(usbh_device_t *dev, usbh_devspeed_t speed) {
+	dev->address = 0;
+	dev->speed = speed;
+	dev->status = USBH_DEVSTATUS_DEFAULT;
+	dev->langID0 = 0;
+	dev->keepFullCfgDesc = 0;
+	_ep0_object_init(dev, 64);
+}
+
+static bool _device_setaddress(usbh_device_t *dev, uint8_t address) {
+	usbh_urbstatus_t ret = usbhControlRequest(dev,
+			USBH_STANDARDOUT(USBH_REQTYPE_DEVICE, USBH_REQ_SET_ADDRESS, address, 0),
+			0,
+			0);
+	if (ret != USBH_URBSTATUS_OK)
+		return HAL_FAILED;
+
+	dev->address = address;
+	return HAL_SUCCESS;
+}
+
+static inline bool _device_read_basic_cfgdesc(usbh_device_t *dev, uint8_t bConfiguration) {
+	/* get configuration descriptor */
+	return usbhStdReqGetConfigurationDescriptor(dev, bConfiguration,
+			sizeof(dev->basicConfigDesc), (uint8_t *)&dev->basicConfigDesc);
+}
+
+static void _device_read_full_cfgdesc(usbh_device_t *dev, uint8_t bConfiguration) {
+	_check_dev(dev);
+
+	uint8_t i;
+
+	if (dev->fullConfigurationDescriptor != NULL) {
+		chHeapFree(dev->fullConfigurationDescriptor);
+	}
+
+	dev->fullConfigurationDescriptor =
+			(uint8_t *)chHeapAlloc(0, dev->basicConfigDesc.wTotalLength);
+
+	if (!dev->fullConfigurationDescriptor)
+		return;
+
+	for (i = 0; i < 3; i++) {
+		if (usbhStdReqGetConfigurationDescriptor(dev, bConfiguration,
+				dev->basicConfigDesc.wTotalLength,
+				dev->fullConfigurationDescriptor) == HAL_SUCCESS) {
+			return;
+		}
+		osalThreadSleepMilliseconds(200);
+	}
+
+	/* error */
+	chHeapFree(dev->fullConfigurationDescriptor);
+	dev->fullConfigurationDescriptor = NULL;
+}
+
+static void _device_free_full_cfgdesc(usbh_device_t *dev) {
+	osalDbgCheck(dev);
+	if (dev->fullConfigurationDescriptor != NULL) {
+		chHeapFree(dev->fullConfigurationDescriptor);
+		dev->fullConfigurationDescriptor = NULL;
+	}
+}
+
+
+#define USBH_SET_CONFIGURATION(type, value, index)	\
+	USBH_STANDARDOUT(type, \
+	USBH_REQ_SET_CONFIGURATION, \
+	value, \
+	index) \
+
+#define USBH_SETDEVICECONFIGURATION(index) \
+	USBH_SET_CONFIGURATION(USBH_REQTYPE_DEVICE, index, 0)
+
+
+static bool _device_set_configuration(usbh_device_t *dev, uint8_t configuration) {
+	usbh_urbstatus_t ret = usbhControlRequest(dev,
+			USBH_SETDEVICECONFIGURATION(configuration),
+			0,
+			0);
+	if (ret != USBH_URBSTATUS_OK)
+		return HAL_FAILED;
+	return HAL_SUCCESS;
+}
+
+static bool _device_configure(usbh_device_t *dev, uint8_t bConfiguration) {
+	uint8_t i;
+
+	uinfof("Reading basic configuration descriptor %d", bConfiguration);
+	for (i = 0; i < 3; i++) {
+		if (!_device_read_basic_cfgdesc(dev, bConfiguration))
+			break;
+	}
+
+	if (i == 3) {
+		uerrf("Could not read basic configuration descriptor %d; "
+					"won't configure device", bConfiguration);
+		return HAL_FAILED;
+	}
+
+	uinfof("Selecting configuration %d", bConfiguration);
+	for (i = 0; i < 3; i++) {
+		if (!_device_set_configuration(dev, dev->basicConfigDesc.bConfigurationValue)) {
+			/* TODO: check if correctly configured using GET_CONFIGURATION */
+			dev->status = USBH_DEVSTATUS_CONFIGURED;
+			dev->bConfiguration = bConfiguration;
+
+			uinfo("Device configured.");
+			return HAL_SUCCESS;
+		}
+	}
+
+	return HAL_FAILED;
+}
+
+static bool _device_enumerate(usbh_device_t *dev) {
+
+	uinfo("Enumerate.");
+	uinfo("Get first 8 bytes of device descriptor");
+
+	/* get first 8 bytes of device descriptor */
+	if (usbhStdReqGetDeviceDescriptor(dev, 8, (uint8_t *)&dev->devDesc)) {
+		uerr("Error");
+		return HAL_FAILED;
+	}
+
+	uinfof("Configure bMaxPacketSize0 = %d", dev->devDesc.bMaxPacketSize0);
+	/* configure EP0 wMaxPacketSize */
+	usbhEPClose(&dev->ctrl);
+	_ep0_object_init(dev, dev->devDesc.bMaxPacketSize0);
+	usbhEPOpen(&dev->ctrl);
+
+	uint8_t addr = _find_address(dev->host);
+	if (addr == 0) {
+		uerr("No free addresses found");
+		return HAL_FAILED;
+	}
+
+	/* set device address */
+	uinfof("Set device address: %d", addr);
+	if (_device_setaddress(dev, addr)) {
+		uerr("Error");
+		_free_address(dev->host, addr);
+		return HAL_FAILED;
+	}
+
+	/* update EP because of the address change */
+	usbhEPClose(&dev->ctrl);
+	_ep0_object_init(dev, dev->devDesc.bMaxPacketSize0);
+	usbhEPOpen(&dev->ctrl);
+
+	uinfof("Wait stabilization...");
+	osalThreadSleepMilliseconds(HAL_USBH_DEVICE_ADDRESS_STABILIZATION);
+
+	/* address is set */
+	dev->status = USBH_DEVSTATUS_ADDRESS;
+
+	uinfof("Get full device desc");
+	/* get full device descriptor */
+	if (usbhStdReqGetDeviceDescriptor(dev, sizeof(dev->devDesc),
+			(uint8_t *)&dev->devDesc)) {
+		uerr("Error");
+		_device_setaddress(dev, 0);
+		_free_address(dev->host, addr);
+		return HAL_FAILED;
+	}
+
+	uinfof("Enumeration finished.");
+	return HAL_SUCCESS;
+}
+
+#if USBH_DEBUG_ENABLE && USBH_DEBUG_ENABLE_INFO
+void usbhDevicePrintInfo(usbh_device_t *dev) {
+	USBH_DEFINE_BUFFER(char, str[64]);
+	usbh_device_descriptor_t *const desc = &dev->devDesc;
+
+	uinfo("----- Device info -----");
+	uinfo("Device descriptor:");
+	uinfof("\tUSBSpec=%04x, #configurations=%d, langID0=%04x",
+			desc->bcdUSB,
+			desc->bNumConfigurations,
+			dev->langID0);
+
+	uinfof("\tClass=%02x, Subclass=%02x, Protocol=%02x",
+			desc->bDeviceClass,
+			desc->bDeviceSubClass,
+			desc->bDeviceProtocol);
+
+	uinfof("\tVID=%04x, PID=%04x, Release=%04x",
+			desc->idVendor,
+			desc->idProduct,
+			desc->bcdDevice);
+
+	if (dev->langID0) {
+		usbhDeviceReadString(dev, str, sizeof(str), desc->iManufacturer, dev->langID0);
+		uinfof("\tManufacturer: %s", str);
+		usbhDeviceReadString(dev, str, sizeof(str), desc->iProduct, dev->langID0);
+		uinfof("\tProduct: %s", str);
+		usbhDeviceReadString(dev, str, sizeof(str), desc->iSerialNumber, dev->langID0);
+		uinfof("\tSerial Number: %s", str);
+	}
+
+	if (dev->status == USBH_DEVSTATUS_CONFIGURED) {
+		uinfo("Configuration descriptor (partial):");
+		usbh_config_descriptor_t *const cfg = &dev->basicConfigDesc;
+		uinfof("\tbConfigurationValue=%d, Length=%d, #interfaces=%d",
+				cfg->bConfigurationValue,
+				cfg->wTotalLength,
+				cfg->bNumInterfaces);
+
+		uinfof("\tCurrent=%dmA", cfg->bMaxPower * 2);
+		uinfof("\tSelfPowered=%d, RemoteWakeup=%d",
+				cfg->bmAttributes & 0x40 ? 1 : 0,
+				cfg->bmAttributes & 0x20 ? 1 : 0);
+		if (dev->langID0) {
+			usbhDeviceReadString(dev, str, sizeof(str), cfg->iConfiguration, dev->langID0);
+			uinfof("\tName: %s", str);
+		}
+	}
+
+	uinfo("----- End Device info -----");
+
+}
+
+void usbhDevicePrintConfiguration(const uint8_t *descriptor, uint16_t rem) {
+	generic_iterator_t iep, icfg, ics;
+	if_iterator_t iif;
+
+	uinfo("----- Configuration info -----");
+	uinfo("Configuration descriptor:");
+	cfg_iter_init(&icfg, descriptor, rem);
+	const usbh_config_descriptor_t *const cfgdesc = cfg_get(&icfg);
+	uinfof("Configuration %d, #IFs=%d", cfgdesc->bConfigurationValue, cfgdesc->bNumInterfaces);
+
+	for (if_iter_init(&iif, &icfg); iif.valid; if_iter_next(&iif)) {
+		const usbh_interface_descriptor_t *const ifdesc = if_get(&iif);
+
+		uinfof("  Interface %d, alt=%d, #EPs=%d, "
+				"Class=%02x, Subclass=%02x, Protocol=%02x",
+				ifdesc->bInterfaceNumber, ifdesc->bAlternateSetting, ifdesc->bNumEndpoints,
+				ifdesc->bInterfaceClass, ifdesc->bInterfaceSubClass, ifdesc->bInterfaceProtocol);
+
+		for (cs_iter_init(&ics, (generic_iterator_t *)&iif); ics.valid; cs_iter_next(&ics)) {
+			uinfof("    Class-Specific descriptor, Length=%d, Type=%02x",
+					ics.curr[0], ics.curr[1]);
+		}
+
+		for (ep_iter_init(&iep, &iif); iep.valid; ep_iter_next(&iep)) {
+			const usbh_endpoint_descriptor_t *const epdesc = ep_get(&iep);
+
+			uinfof("    Endpoint descriptor, Address=%02x, Type=%d, MaxPacket=%d, Interval=%d",
+					epdesc->bEndpointAddress,
+					epdesc->bmAttributes & 3,
+					epdesc->wMaxPacketSize,
+					epdesc->bInterval);
+
+			for (cs_iter_init(&ics, &iep); ics.valid; cs_iter_next(&ics)) {
+				uinfof("    Class-Specific descriptor, Length=%d, Type=%02x",
+						ics.curr[0], ics.curr[1]);
+			}
+		}
+	}
+	uinfo("----- End Configuration info -----");
+}
+#endif
+
+bool usbhDeviceReadString(usbh_device_t *dev, char *dest, uint8_t size,
+		uint8_t index, uint16_t langID) {
+
+	usbh_string_descriptor_t *const desc = (usbh_string_descriptor_t *)dest;
+	osalDbgAssert(size >= 2, "wrong size");
+
+	*dest = 0;
+	if (index == 0)
+		return HAL_SUCCESS;
+	if (usbhStdReqGetStringDescriptor(dev, index, langID, size, (uint8_t *)dest))
+		return HAL_FAILED;
+	if (desc->bLength & 1)
+		return HAL_FAILED;
+	if (desc->bLength <= 2)
+		return HAL_SUCCESS;
+
+	uint8_t nchars = desc->bLength / 2;		/* including the trailing 0 */
+	if (size < nchars)
+		nchars = size;
+
+	char *src = (char *)&desc->wData[0];
+	while (--nchars) {
+		*dest++ = *src;
+		src += 2;
+	}
+	*dest = 0;
+	return HAL_SUCCESS;
+}
+
+
+
+
+/*===========================================================================*/
+/* Port processing functions.  	                                     		 */
+/*===========================================================================*/
+
+static void _port_connected(usbh_port_t *port);
+
+static void _port_reset(usbh_port_t *port) {
+	usbhhubControlRequest(port->device.host,
+#if HAL_USBH_USE_HUB
+			port->hub,
+#endif
+			USBH_REQTYPE_OUT | USBH_REQTYPE_CLASS | USBH_REQTYPE_OTHER,
+			USBH_REQ_SET_FEATURE,
+			USBH_PORT_FEAT_RESET,
+			port->number,
+			0,
+			0);
+}
+
+static void _port_update_status(usbh_port_t *port) {
+	uint32_t stat;
+	if (usbhhubControlRequest(port->device.host,
+#if HAL_USBH_USE_HUB
+			port->hub,
+#endif
+			USBH_REQTYPE_IN | USBH_REQTYPE_CLASS | USBH_REQTYPE_OTHER,
+			USBH_REQ_GET_STATUS,
+			0,
+			port->number,
+			4,
+			(uint8_t *)&stat) != USBH_URBSTATUS_OK) {
+		return;
+	}
+	port->status = stat & 0xffff;
+	port->c_status |= stat >> 16;
+}
+
+static void _port_process_status_change(usbh_port_t *port) {
+
+	_port_update_status(port);
+
+	if (port->c_status & USBH_PORTSTATUS_C_CONNECTION) {
+		/* port connected status changed */
+		port->c_status &= ~USBH_PORTSTATUS_C_CONNECTION;
+		usbhhubClearFeaturePort(port, USBH_PORT_FEAT_C_CONNECTION);
+		if ((port->status & (USBH_PORTSTATUS_CONNECTION | USBH_PORTSTATUS_ENABLE))
+				== USBH_PORTSTATUS_CONNECTION) {
+			if (port->device.status != USBH_DEVSTATUS_DISCONNECTED) {
+				_usbh_port_disconnected(port);
+			}
+
+			/* connected, disabled */
+			_port_connected(port);
+		} else {
+			/* disconnected */
+			_usbh_port_disconnected(port);
+		}
+	}
+
+	if (port->c_status & USBH_PORTSTATUS_C_RESET) {
+		port->c_status &= ~USBH_PORTSTATUS_C_RESET;
+		usbhhubClearFeaturePort(port, USBH_PORT_FEAT_C_RESET);
+	}
+
+	if (port->c_status & USBH_PORTSTATUS_C_ENABLE) {
+		port->c_status &= ~USBH_PORTSTATUS_C_ENABLE;
+		usbhhubClearFeaturePort(port, USBH_PORT_FEAT_C_ENABLE);
+	}
+
+	if (port->c_status & USBH_PORTSTATUS_C_OVERCURRENT) {
+		port->c_status &= ~USBH_PORTSTATUS_C_OVERCURRENT;
+		usbhhubClearFeaturePort(port, USBH_PORT_FEAT_C_OVERCURRENT);
+	}
+
+	if (port->c_status & USBH_PORTSTATUS_C_SUSPEND) {
+		port->c_status &= ~USBH_PORTSTATUS_C_SUSPEND;
+		usbhhubClearFeaturePort(port, USBH_PORT_FEAT_C_SUSPEND);
+	}
+
+}
+
+
+static void _port_connected(usbh_port_t *port) {
+	/* connected */
+
+	systime_t start;
+	uint8_t i;
+	uint8_t retries;
+	usbh_devspeed_t speed;
+	USBH_DEFINE_BUFFER(usbh_string_descriptor_t, strdesc);
+
+	uinfof("Port %d connected, wait debounce...", port->number);
+
+	port->device.status = USBH_DEVSTATUS_ATTACHED;
+
+	/* wait for attach de-bounce */
+	osalThreadSleepMilliseconds(HAL_USBH_PORT_DEBOUNCE_TIME);
+
+	/* check disconnection */
+	_port_update_status(port);
+	if (port->c_status & USBH_PORTSTATUS_C_CONNECTION) {
+		/* connection state changed; abort */
+		goto abort;
+	}
+
+	port->device.status = USBH_DEVSTATUS_CONNECTED;
+	retries = 3;
+
+reset:
+	for (i = 0; i < 3; i++) {
+		uinfo("Try reset...");
+		port->c_status &= ~(USBH_PORTSTATUS_C_RESET | USBH_PORTSTATUS_C_ENABLE);
+		_port_reset(port);
+		osalThreadSleepMilliseconds(20);	/* give it some time to reset (min. 10ms) */
+		start = osalOsGetSystemTimeX();
+		while (TRUE) {
+			_port_update_status(port);
+
+			/* check for disconnection */
+			if (port->c_status & USBH_PORTSTATUS_C_CONNECTION)
+				goto abort;
+
+			/* check for reset completion */
+			if (port->c_status & USBH_PORTSTATUS_C_RESET) {
+				port->c_status &= ~USBH_PORTSTATUS_C_RESET;
+				usbhhubClearFeaturePort(port, USBH_PORT_FEAT_C_RESET);
+
+				if ((port->status & (USBH_PORTSTATUS_ENABLE | USBH_PORTSTATUS_CONNECTION))
+						== (USBH_PORTSTATUS_ENABLE | USBH_PORTSTATUS_CONNECTION)) {
+					goto reset_success;
+				}
+			}
+
+			/* check for timeout */
+			if (osalOsGetSystemTimeX() - start > HAL_USBH_PORT_RESET_TIMEOUT) break;
+		}
+	}
+
+	/* reset procedure failed; abort */
+	goto abort;
+
+reset_success:
+
+	uinfo("Reset OK, recovery...");
+
+	/* reset recovery */
+	osalThreadSleepMilliseconds(100);
+
+	/* initialize object */
+	if (port->status & USBH_PORTSTATUS_LOW_SPEED) {
+		speed = USBH_DEVSPEED_LOW;
+	} else if (port->status & USBH_PORTSTATUS_HIGH_SPEED) {
+		speed = USBH_DEVSPEED_HIGH;
+	} else {
+		speed = USBH_DEVSPEED_FULL;
+	}
+	_device_initialize(&port->device, speed);
+	usbhEPOpen(&port->device.ctrl);
+
+	/* device with default address (0), try enumeration */
+	if (_device_enumerate(&port->device)) {
+		/* enumeration failed */
+		usbhEPClose(&port->device.ctrl);
+
+		if (!--retries)
+			goto abort;
+
+		/* retry reset & enumeration */
+		goto reset;
+	}
+
+	/* load the default language ID */
+	uinfo("Loading langID0...");
+	if (!usbhStdReqGetStringDescriptor(&port->device, 0, 0,
+			USBH_DT_STRING_SIZE, (uint8_t *)&strdesc)
+		&& (strdesc.bLength >= 4)
+		&& !usbhStdReqGetStringDescriptor(&port->device, 0, 0,
+			4, (uint8_t *)&strdesc)) {
+
+		port->device.langID0 = strdesc.wData[0];
+		uinfof("langID0=%04x", port->device.langID0);
+	}
+
+	/* check if the device has only one configuration */
+	if (port->device.devDesc.bNumConfigurations == 1) {
+		uinfo("Device has only one configuration");
+		_device_configure(&port->device, 0);
+	}
+
+	_classdriver_process_device(&port->device);
+	return;
+
+abort:
+	uerr("Abort");
+	port->device.status = USBH_DEVSTATUS_DISCONNECTED;
+}
+
+void _usbh_port_disconnected(usbh_port_t *port) {
+	if (port->device.status == USBH_DEVSTATUS_DISCONNECTED)
+		return;
+
+	uinfo("Port disconnected");
+
+	/* unload drivers */
+	while (port->device.drivers) {
+		usbh_baseclassdriver_t *drv = port->device.drivers;
+
+		/* unload */
+		uinfof("Unload driver %s", drv->info->name);
+		drv->info->vmt->unload(drv);
+
+		/* unlink */
+		drv->dev = 0;
+		port->device.drivers = drv->next;
+	}
+
+	/* close control endpoint */
+	osalSysLock();
+	usbhEPCloseS(&port->device.ctrl);
+	osalSysUnlock();
+
+	/* free address */
+	if (port->device.address)
+		_free_address(port->device.host, port->device.address);
+
+	_device_free_full_cfgdesc(&port->device);
+
+	port->device.status = USBH_DEVSTATUS_DISCONNECTED;
+}
+
+
+
+/*===========================================================================*/
+/* Hub processing functions.  	                                     		 */
+/*===========================================================================*/
+
+#if HAL_USBH_USE_HUB
+static void _hub_update_status(USBHDriver *host, USBHHubDriver *hub) {
+	uint32_t stat;
+	if (usbhhubControlRequest(host,
+			hub,
+			USBH_REQTYPE_IN | USBH_REQTYPE_CLASS | USBH_REQTYPE_DEVICE,
+			USBH_REQ_GET_STATUS,
+			0,
+			0,
+			4,
+			(uint8_t *)&stat) != USBH_URBSTATUS_OK) {
+		return;
+	}
+	if (hub) {
+		hub->status = stat & 0xffff;
+		hub->c_status |= stat >> 16;
+	}
+}
+
+static void _hub_process_status_change(USBHDriver *host, USBHHubDriver *hub) {
+	uinfo("Hub status change. GET_STATUS.");
+	_hub_update_status(host, hub);
+
+	if (hub->c_status & USBH_HUBSTATUS_C_HUB_LOCAL_POWER) {
+		hub->c_status &= ~USBH_HUBSTATUS_C_HUB_LOCAL_POWER;
+		uinfo("Clear USBH_HUB_FEAT_C_HUB_LOCAL_POWER");
+		usbhhubClearFeatureHub(host, hub, USBH_HUB_FEAT_C_HUB_LOCAL_POWER);
+	}
+
+	if (hub->c_status & USBH_HUBSTATUS_C_HUB_OVER_CURRENT) {
+		hub->c_status &= ~USBH_HUBSTATUS_C_HUB_OVER_CURRENT;
+		uinfo("Clear USBH_HUB_FEAT_C_HUB_OVER_CURRENT");
+		usbhhubClearFeatureHub(host, hub, USBH_HUB_FEAT_C_HUB_OVER_CURRENT);
+	}
+}
+
+static uint32_t _hub_get_status_change_bitmap(USBHDriver *host, USBHHubDriver *hub) {
+	if (hub != NULL) {
+		osalSysLock();
+		uint32_t ret = hub->statuschange;
+		hub->statuschange = 0;
+		osalOsRescheduleS();
+		osalSysUnlock();
+		return ret;
+	}
+	return usbh_lld_roothub_get_statuschange_bitmap(host);
+}
+
+#else
+//TODO: replace the functions above
+#endif
+
+#if HAL_USBH_USE_HUB
+static void _hub_process(USBHDriver *host, USBHHubDriver *hub) {
+	uint32_t bitmap = _hub_get_status_change_bitmap(host, hub);
+	if (!bitmap)
+		return;
+
+	if (bitmap & 1) {
+		_hub_process_status_change(host, hub);
+		bitmap &= ~1;
+	}
+
+	usbh_port_t *port = (hub == NULL) ? &host->rootport : hub->ports;
+	uint8_t i;
+	for (i = 1; i < 32; i++) {
+		if (!bitmap || !port)
+			break;
+		if (bitmap & (1 << i)) {
+			bitmap &= ~(1 << i);
+			_port_process_status_change(port);
+		}
+		port = port->next;
+	}
+
+}
+#else
+static void _hub_process(USBHDriver *host) {
+	uint32_t bitmap = usbh_lld_roothub_get_statuschange_bitmap(host);
+
+#if 0	//TODO: complete _hub_process_status_change for root hub
+	if (bitmap & 1) {
+		_hub_process_status_change(host, hub);
+		bitmap &= ~1;
+	}
+#endif
+
+	if (!bitmap)
+		return;
+
+	_port_process_status_change(&host->rootport);
+}
+#endif
+
+/*===========================================================================*/
+/* Main processing loop (enumeration, loading/unloading drivers, etc). 		 */
+/*===========================================================================*/
+void usbhMainLoop(USBHDriver *usbh) {
+
+	if (usbh->status == USBH_STATUS_STOPPED)
+		return;
+
+#if HAL_USBH_USE_HUB
+	/* process root hub */
+	_hub_process(usbh, NULL);
+
+	/* process connected hubs */
+	USBHHubDriver *hub;
+    list_for_each_entry(hub, USBHHubDriver, &usbh->hubs, node) {
+		_hub_process(usbh, hub);
+	}
+#else
+	/* process root hub */
+	_hub_process(usbh);
+#endif
+}
+
+
+/*===========================================================================*/
+/* IAD class driver.											 		 	 */
+/*===========================================================================*/
+#if HAL_USBH_USE_IAD
+static usbh_baseclassdriver_t *iad_load(usbh_device_t *dev, const uint8_t *descriptor, uint16_t rem);
+static void iad_unload(usbh_baseclassdriver_t *drv);
+static const usbh_classdriver_vmt_t usbhiadClassDriverVMT = {
+	iad_load,
+	iad_unload
+};
+static const usbh_classdriverinfo_t usbhiadClassDriverInfo = {
+	0xef, 0x02, 0x01, "IAD", &usbhiadClassDriverVMT
+};
+
+static usbh_baseclassdriver_t *iad_load(usbh_device_t *dev,
+		const uint8_t *descriptor, uint16_t rem) {
+	(void)rem;
+
+	if (descriptor[1] != USBH_DT_DEVICE)
+		return 0;
+
+	uinfo("Load a driver for each IF collection.");
+
+	generic_iterator_t icfg;
+	if_iterator_t iif;
+	const usbh_ia_descriptor_t *last_iad = 0;
+
+	cfg_iter_init(&icfg, dev->fullConfigurationDescriptor,
+			dev->basicConfigDesc.wTotalLength);
+	if (!icfg.valid) {
+		uerr("Invalid configuration descriptor.");
+		return 0;
+	}
+
+	for (if_iter_init(&iif, &icfg); iif.valid; if_iter_next(&iif)) {
+		if (iif.iad && (iif.iad != last_iad)) {
+			last_iad = iif.iad;
+			if (_classdriver_load(dev, iif.iad->bFunctionClass,
+					iif.iad->bFunctionSubClass,
+					iif.iad->bFunctionProtocol,
+					(uint8_t *)iif.iad,
+					(uint8_t *)iif.curr - (uint8_t *)iif.iad + iif.rem) != HAL_SUCCESS) {
+				uwarnf("No drivers found for IF collection #%d:%d",
+						iif.iad->bFirstInterface,
+						iif.iad->bFirstInterface + iif.iad->bInterfaceCount - 1);
+			}
+		}
+	}
+
+	return 0;
+}
+
+static void iad_unload(usbh_baseclassdriver_t *drv) {
+	(void)drv;
+}
+#endif
+
+
+/*===========================================================================*/
+/* Class driver loader.											 		 	 */
+/*===========================================================================*/
+
+static const usbh_classdriverinfo_t *usbh_classdrivers_lookup[] = {
+#if HAL_USBH_USE_FTDI
+	&usbhftdiClassDriverInfo,
+#endif
+#if HAL_USBH_USE_IAD
+	&usbhiadClassDriverInfo,
+#endif
+#if HAL_USBH_USE_UVC
+	&usbhuvcClassDriverInfo,
+#endif
+#if HAL_USBH_USE_MSD
+	&usbhmsdClassDriverInfo,
+#endif
+#if HAL_USBH_USE_HUB
+	&usbhhubClassDriverInfo
+#endif
+};
+
+static bool _classdriver_load(usbh_device_t *dev, uint8_t class,
+		uint8_t subclass, uint8_t protocol, uint8_t *descbuff, uint16_t rem) {
+	uint8_t i;
+	usbh_baseclassdriver_t *drv = NULL;
+	for (i = 0; i < sizeof_array(usbh_classdrivers_lookup); i++) {
+		const usbh_classdriverinfo_t *const info = usbh_classdrivers_lookup[i];
+		if (class == 0xff) {
+			/* vendor specific */
+			if (info->class == 0xff) {
+				uinfof("Try load vendor-specific driver %s", info->name);
+				drv = info->vmt->load(dev, descbuff, rem);
+				if (drv != NULL)
+					goto success;
+			}
+		} else if ((info->class < 0) || ((info->class == class)
+			&& ((info->subclass < 0) || ((info->subclass == subclass)
+			&& ((info->protocol < 0) || (info->protocol == protocol)))))) {
+			uinfof("Try load driver %s", info->name);
+			drv = info->vmt->load(dev, descbuff, rem);
+
+#if HAL_USBH_USE_IAD
+			/* special case: */
+			if (info == &usbhiadClassDriverInfo)
+				return HAL_SUCCESS;
+#endif
+
+			if (drv != NULL)
+				goto success;
+		}
+	}
+	return HAL_FAILED;
+
+success:
+	/* Link this driver to the device */
+	drv->next = dev->drivers;
+	dev->drivers = drv;
+	drv->dev = dev;
+	return HAL_SUCCESS;
+}
+
+static void _classdriver_process_device(usbh_device_t *dev) {
+	uinfo("New device found.");
+	const usbh_device_descriptor_t *const devdesc = &dev->devDesc;
+
+	usbhDevicePrintInfo(dev);
+
+	/* TODO: Support multiple configurations
+	 *
+	 * Windows doesn't support them, so it's unlikely that any commercial USB device
+	 * will have multiple configurations.
+	 */
+	if (dev->status != USBH_DEVSTATUS_CONFIGURED) {
+		uwarn("Multiple configurations not supported, selecting configuration #0");
+		if (_device_configure(dev, 0) != HAL_SUCCESS) {
+			uerr("Couldn't configure device; abort.");
+			return;
+		}
+	}
+
+	_device_read_full_cfgdesc(dev, dev->bConfiguration);
+	if (dev->fullConfigurationDescriptor == NULL) {
+		uerr("Couldn't read full configuration descriptor; abort.");
+		return;
+	}
+
+	usbhDevicePrintConfiguration(dev->fullConfigurationDescriptor,
+			dev->basicConfigDesc.wTotalLength);
+
+	if (devdesc->bDeviceClass == 0) {
+		/* each interface defines its own device class/subclass/protocol */
+		uinfo("Load a driver for each IF.");
+
+		generic_iterator_t icfg;
+		if_iterator_t iif;
+		uint8_t last_if = 0xff;
+
+		cfg_iter_init(&icfg, dev->fullConfigurationDescriptor,
+				dev->basicConfigDesc.wTotalLength);
+		if (!icfg.valid) {
+			uerr("Invalid configuration descriptor.");
+			goto exit;
+		}
+
+		for (if_iter_init(&iif, &icfg); iif.valid; if_iter_next(&iif)) {
+			const usbh_interface_descriptor_t *const ifdesc = if_get(&iif);
+			if (ifdesc->bInterfaceNumber != last_if) {
+				last_if = ifdesc->bInterfaceNumber;
+				if (_classdriver_load(dev, ifdesc->bInterfaceClass,
+						ifdesc->bInterfaceSubClass,
+						ifdesc->bInterfaceProtocol,
+						(uint8_t *)ifdesc, iif.rem) != HAL_SUCCESS) {
+					uwarnf("No drivers found for IF #%d", ifdesc->bInterfaceNumber);
+				}
+			}
+		}
+
+	} else {
+		if (_classdriver_load(dev, devdesc->bDeviceClass,
+				devdesc->bDeviceSubClass,
+				devdesc->bDeviceProtocol,
+				(uint8_t *)devdesc, USBH_DT_DEVICE_SIZE) != HAL_SUCCESS) {
+			uwarn("No drivers found.");
+		}
+	}
+
+exit:
+	if (dev->keepFullCfgDesc == 0) {
+		_device_free_full_cfgdesc(dev);
+	}
+}
+
+
+#endif
+
diff --git a/os/hal/src/usbh/usbh_debug.c b/os/hal/src/usbh/hal_usbh_debug.c
index 63505aa..9f17189 100644
--- a/os/hal/src/usbh/usbh_debug.c
+++ b/os/hal/src/usbh/hal_usbh_debug.c
@@ -1,536 +1,536 @@
-/*

-    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio

-              Copyright (C) 2015 Diego Ismirlian, TISA, (dismirlian (at) google's mail)

-

-    Licensed under the Apache License, Version 2.0 (the "License");

-    you may not use this file except in compliance with the License.

-    You may obtain a copy of the License at

-

-        http://www.apache.org/licenses/LICENSE-2.0

-

-    Unless required by applicable law or agreed to in writing, software

-    distributed under the License is distributed on an "AS IS" BASIS,

-    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.

-    See the License for the specific language governing permissions and

-    limitations under the License.

-*/

-

-#include "hal.h"

-

-#if HAL_USE_USBH

-

-#include "ch.h"

-#include "usbh/debug.h"

-#include <stdarg.h>

-#include "chprintf.h"

-

-#if USBH_DEBUG_ENABLE

-

-#define MAX_FILLER 11

-#define FLOAT_PRECISION 9

-#define MPRINTF_USE_FLOAT 0

-

-static char *long_to_string_with_divisor(char *p, long num, unsigned radix, long divisor) 

-{

-	int i;

-	char *q;

-	long l, ll;

-

-	l = num;

-	if (divisor == 0) {

-		ll = num;

-	} else {

-		ll = divisor;

-	}

-

-	q = p + MAX_FILLER;

-	do {

-		i = (int)(l % radix);

-		i += '0';

-		if (i > '9') {

-			i += 'A' - '0' - 10;

-		}

-		*--q = i;

-		l /= radix;

-	} while ((ll /= radix) != 0);

-

-	i = (int)(p + MAX_FILLER - q);

-	do {

-		*p++ = *q++;

-	} while (--i);

-

-	return p;

-}

-

-static char *ltoa(char *p, long num, unsigned radix) {

-

-	return long_to_string_with_divisor(p, num, radix, 0);

-}

-

-#if MPRINTF_USE_FLOAT

-static const long _pow10[FLOAT_PRECISION] = {10, 100, 1000, 10000, 100000, 1000000,

-	10000000, 100000000, 1000000000};

-static const double m10[FLOAT_PRECISION] = {5.0/100, 5.0/1000, 5.0/10000, 5.0/100000, 5.0/1000000,

-	5.0/10000000, 5.0/100000000, 5.0/1000000000, 5.0/10000000000};

-

-static char *ftoa(char *p, double num, unsigned long precision, bool dot) {

-	long l;

-	char *q;

-	double r;

-

-

-	if (precision == 0) {

-		l = (long)(num + 0.5);

-		return long_to_string_with_divisor(p, l, 10, 0);

-	} else {

-		if (precision > FLOAT_PRECISION) precision = FLOAT_PRECISION;

-		r = m10[precision - 1];

-		precision = _pow10[precision - 1];

-

-		l = (long)num;

-		p = long_to_string_with_divisor(p, l, 10, 0);

-		if (dot) *p++ = '.';

-		l = (long)((num - l + r) * precision);

-		q = long_to_string_with_divisor(p, l, 10, precision / 10) - 1;

-

-		while (q > p) {

-			if (*q != '0') {

-				break;

-			}

-			--q;

-		}

-		return ++q;

-	}

-

-	

-

-

-}

-#endif

-

-static inline void _put(char c) {

-	input_queue_t *iqp = &USBH_DEBUG_USBHD.iq;

-

-	if (chIQIsFullI(iqp))

-		return;

-

-	iqp->q_counter++;

-	*iqp->q_wrptr++ = c;

-	if (iqp->q_wrptr >= iqp->q_top)

-		iqp->q_wrptr = iqp->q_buffer;

-

-}

-

-int _dbg_printf(const char *fmt, va_list ap) {

-	char *p, *s, c, filler;

-	int i, precision, width;

-	int n = 0;

-	bool is_long, left_align, sign;

-	long l;

-#if MPRINTF_USE_FLOAT

-	double f;

-	char tmpbuf[2*MAX_FILLER + 1];

-#else

-	char tmpbuf[MAX_FILLER + 1];

-#endif

-

-	for (;;) {

-

-		//agarrar nuevo caracter de formato

-		c = *fmt++;

-

-		//chequeo eos

-		if (c == 0) return n;

-

-		//copio los caracteres comunes

-		if (c != '%') {

-			_put(c);

-			n++;

-			continue;

-		}

-

-		//encontré un '%'

-		p = tmpbuf;

-		s = tmpbuf;

-

-		//left align

-		left_align = FALSE;

-		if (*fmt == '-') {

-			fmt++;

-			left_align = TRUE;

-		}

-		

-		sign = FALSE;

-		if (*fmt == '+') {

-			fmt++;

-			sign = TRUE;

-		}

-

-		//filler

-		filler = ' ';

-		if (*fmt == '0') {

-			fmt++;

-			filler = '0';

-		}

-

-		//width

-		width = 0;

-		while (TRUE) {

-			c = *fmt++;

-			if (c >= '0' && c <= '9')

-				c -= '0';

-			else if (c == '*')

-				c = va_arg(ap, int);

-			else

-				break;

-			width = width * 10 + c;

-		}

-

-		//precision

-		precision = 0;

-		if (c == '.') {

-

-			if (*fmt == 'n') {

-				fmt++;

-

-			}

-			while (TRUE) {

-				c = *fmt++;

-				if (c >= '0' && c <= '9')

-					c -= '0';

-				else if (c == '*')

-					c = va_arg(ap, int);

-				else

-					break;

-				precision = precision * 10 + c;

-			}

-		}

-

-		//long modifier

-		if (c == 'l' || c == 'L') {

-			is_long = TRUE;

-			if (*fmt)

-				c = *fmt++;

-		}

-		else

-			is_long = (c >= 'A') && (c <= 'Z');

-

-		/* Command decoding.*/

-		switch (c) {

-		//char

-		case 'c':

-			filler = ' ';

-			*p++ = va_arg(ap, int);

-			break;

-

-		//string

-		case 's':

-			filler = ' ';

-			if ((s = va_arg(ap, char *)) == 0)

-				s = (char *)"(null)";

-			if (precision == 0)

-				precision = 32767;

-

-			//strlen con límite hasta precision

-			for (p = s; *p && (--precision >= 0); p++)

-				;

-			break;

-

-

-

-		case 'D':

-		case 'd':

-		case 'I':

-		case 'i':

-			if (is_long)

-				l = va_arg(ap, long);

-			else

-				l = va_arg(ap, int);

-			if (l < 0) {

-				*p++ = '-';

-				l = -l;

-				sign = TRUE;

-			} else if (sign) {

-				*p++ = '+';

-			}

-			p = ltoa(p, l, 10);

-			break;

-

-#if MPRINTF_USE_FLOAT

-		case 'f':

-			f = va_arg(ap, double);

-			if (f < 0) {

-				*p++ = '-';

-				f = -f;

-				sign = TRUE;

-			} else if (sign) {

-				*p++ = '+';

-			}

-			if (prec == FALSE) precision = 6;

-			p = ftoa(p, f, precision, dot);

-			break;

-#endif

-

-

-		case 'X':

-		case 'x':

-			c = 16;

-			goto unsigned_common;

-		case 'U':

-		case 'u':

-			c = 10;

-			goto unsigned_common;

-		case 'O':

-		case 'o':

-			c = 8;

-

-unsigned_common:

-			if (is_long)

-				l = va_arg(ap, unsigned long);

-			else

-				l = va_arg(ap, unsigned int);

-			p = ltoa(p, l, c);

-			break;

-

-		//copiar

-		default:

-			*p++ = c;

-			break;

-		}

-

-		//longitud

-		i = (int)(p - s);

-

-		//calculo cuántos caracteres de filler debo poner

-		if ((width -= i) < 0)

-			width = 0;

-

-		if (left_align == FALSE)

-			width = -width;

-

-		if (width < 0) {

-			//alineado a la derecha

-

-			//poner el signo adelante

-			if (sign && filler == '0') {

-				_put(*s++);

-				n++;

-				i--;

-			}

-

-			//fill a la izquierda

-			do {

-				_put(filler);

-				n++;

-			} while (++width != 0);

-		}

-

-		//copiar los caracteres

-		while (--i >= 0) {

-			_put(*s++);

-			n++;

-		}

-

-		//fill a la derecha

-		while (width) {

-			_put(filler);

-			n++;

-			width--;

-		}

-	}

-

-	//return n; // can raise 'code is unreachable' warning

-

-}

-

-static void _print_hdr(void)

-{

-	uint32_t hfnum = USBH_DEBUG_USBHD.otg->HFNUM;

-	uint16_t hfir = USBH_DEBUG_USBHD.otg->HFIR;

-

-	_put(0xff);

-	_put(0xff);

-	_put(hfir & 0xff);

-	_put(hfir >> 8);

-	_put(hfnum & 0xff);

-	_put((hfnum >> 8) & 0xff);

-	_put((hfnum >> 16) & 0xff);

-	_put((hfnum >> 24) & 0xff);

-}

-

-void usbDbgPrintf(const char *fmt, ...)

-{

-	va_list ap;

-	va_start(ap, fmt);

-	syssts_t sts = chSysGetStatusAndLockX();

-	_print_hdr();

-	_dbg_printf(fmt, ap);

-	_put(0);

-	chThdDequeueNextI(&USBH_DEBUG_USBHD.iq.q_waiting, Q_OK);

-	chSysRestoreStatusX(sts);

-	va_end(ap);

-}

-

-

-void usbDbgPuts(const char *s)

-{

-	uint32_t buff[2] = {

-		0xffff | (USBH_DEBUG_USBHD.otg->HFIR << 16),

-		USBH_DEBUG_USBHD.otg->HFNUM

-	};

-	uint8_t *p = (uint8_t *)buff;

-	uint8_t *top = p + 8;

-

-	syssts_t sts = chSysGetStatusAndLockX();

-	input_queue_t *iqp = &USBH_DEBUG_USBHD.iq;

-	int rem = sizeof(USBH_DEBUG_USBHD.dbg_buff) - iqp->q_counter;

-	while (rem) {

-		*iqp->q_wrptr++ = *p;

-		if (iqp->q_wrptr >= iqp->q_top)

-			iqp->q_wrptr = iqp->q_buffer;

-		rem--;

-		if (++p == top) break;

-	}

-	while (rem) {

-		*iqp->q_wrptr++ = *s;

-		if (iqp->q_wrptr >= iqp->q_top)

-			iqp->q_wrptr = iqp->q_buffer;

-		rem--;

-		if (!*s++) break;

-	}

-	iqp->q_counter = sizeof(USBH_DEBUG_USBHD.dbg_buff) - rem;

-	chThdDequeueNextI(&USBH_DEBUG_USBHD.iq.q_waiting, Q_OK);

-	chSysRestoreStatusX(sts);

-}

-

-void usbDbgReset(void) {

-	const char *msg = "\r\n\r\n==== DEBUG OUTPUT RESET ====\r\n";

-

-	syssts_t sts = chSysGetStatusAndLockX();

-	chIQResetI(&USBH_DEBUG_USBHD.iq);

-	chOQResetI(&USBH_DEBUG_SD.oqueue);

-	while (*msg) {

-		*USBH_DEBUG_SD.oqueue.q_wrptr++ = *msg++;

-		USBH_DEBUG_SD.oqueue.q_counter--;

-	}

-	chSysRestoreStatusX(sts);

-}

-

-static int _get(void) {

-	if (!USBH_DEBUG_USBHD.iq.q_counter) return -1;

-	USBH_DEBUG_USBHD.iq.q_counter--;

-	uint8_t b = *USBH_DEBUG_USBHD.iq.q_rdptr++;

-	if (USBH_DEBUG_USBHD.iq.q_rdptr >= USBH_DEBUG_USBHD.iq.q_top) {

-		USBH_DEBUG_USBHD.iq.q_rdptr = USBH_DEBUG_USBHD.iq.q_buffer;

-	}

-	return b;

-}

-

-void usbDbgSystemHalted(void) {

-	while (true) {

-		if (!((bool)((USBH_DEBUG_SD.oqueue.q_wrptr == USBH_DEBUG_SD.oqueue.q_rdptr) && (USBH_DEBUG_SD.oqueue.q_counter != 0U))))

-			break;

-		USBH_DEBUG_SD.oqueue.q_counter++;

-		while (!(USART1->SR & USART_SR_TXE));

-		USART1->DR = *USBH_DEBUG_SD.oqueue.q_rdptr++;

-		if (USBH_DEBUG_SD.oqueue.q_rdptr >= USBH_DEBUG_SD.oqueue.q_top) {

-			USBH_DEBUG_SD.oqueue.q_rdptr = USBH_DEBUG_SD.oqueue.q_buffer;

-		}

-	}

-

-	int c;

-	int state = 0;

-	for (;;) {

-		c = _get(); if (c < 0) break;

-

-		if (state == 0) {

-			if (c == 0xff) state = 1;

-		} else if (state == 1) {

-			if (c == 0xff) state = 2;

-			else (state = 0);

-		} else {

-			c = _get(); if (c < 0) return;

-			c = _get(); if (c < 0) return;

-			c = _get(); if (c < 0) return;

-			c = _get(); if (c < 0) return;

-			c = _get(); if (c < 0) return;

-

-			while (true) {

-				c = _get(); if (c < 0) return;

-				if (!c) {

-					while (!(USART1->SR & USART_SR_TXE));

-					USART1->DR = '\r';

-					while (!(USART1->SR & USART_SR_TXE));

-					USART1->DR = '\n';

-					state = 0;

-					break;

-				}

-				while (!(USART1->SR & USART_SR_TXE));

-				USART1->DR = c;

-			}

-		}

-	}

-}

-

-static void usb_debug_thread(void *p) {

-	USBHDriver *host = (USBHDriver *)p;

-	uint8_t state = 0;

-

-	chRegSetThreadName("USBH_DBG");

-	while (true) {

-		msg_t c = chIQGet(&host->iq);

-		if (c < 0) goto reset;

-

-		if (state == 0) {

-			if (c == 0xff) state = 1;

-		} else if (state == 1) {

-			if (c == 0xff) state = 2;

-			else (state = 0);

-		} else {

-			uint16_t hfir;

-			uint32_t hfnum;

-

-			hfir = c;

-			c = chIQGet(&host->iq); if (c < 0) goto reset;

-			hfir |= c << 8;

-

-			c = chIQGet(&host->iq); if (c < 0) goto reset;

-			hfnum = c;

-			c = chIQGet(&host->iq); if (c < 0) goto reset;

-			hfnum |= c << 8;

-			c = chIQGet(&host->iq); if (c < 0) goto reset;

-			hfnum |= c << 16;

-			c = chIQGet(&host->iq); if (c < 0) goto reset;

-			hfnum |= c << 24;

-

-			uint32_t f = hfnum & 0xffff;

-			uint32_t p = 1000 - ((hfnum >> 16) / (hfir / 1000));

-			chprintf((BaseSequentialStream *)&USBH_DEBUG_SD, "%05d.%03d  ", f, p);

-

-			while (true) {

-				c = chIQGet(&host->iq); if (c < 0) goto reset;

-				if (!c) {

-					sdPut(&USBH_DEBUG_SD, '\r');

-					sdPut(&USBH_DEBUG_SD, '\n');

-					state = 0;

-					break;

-				}

-				sdPut(&USBH_DEBUG_SD, (uint8_t)c);

-			}

-		}

-

-		continue;

-reset:

-		state = 0;

-	}

-}

-

-void usbDbgInit(USBHDriver *host) {

-	if (host != &USBH_DEBUG_USBHD)

-		return;

-	chIQObjectInit(&USBH_DEBUG_USBHD.iq, USBH_DEBUG_USBHD.dbg_buff, sizeof(USBH_DEBUG_USBHD.dbg_buff), 0, 0);

-	chThdCreateStatic(USBH_DEBUG_USBHD.waDebug, sizeof(USBH_DEBUG_USBHD.waDebug), NORMALPRIO, usb_debug_thread, &USBH_DEBUG_USBHD);

-}

-#endif

-

-#endif

+/*
+    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio
+              Copyright (C) 2015 Diego Ismirlian, TISA, (dismirlian (at) google's mail)
+
+    Licensed under the Apache License, Version 2.0 (the "License");
+    you may not use this file except in compliance with the License.
+    You may obtain a copy of the License at
+
+        http://www.apache.org/licenses/LICENSE-2.0
+
+    Unless required by applicable law or agreed to in writing, software
+    distributed under the License is distributed on an "AS IS" BASIS,
+    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+    See the License for the specific language governing permissions and
+    limitations under the License.
+*/
+
+#include "hal.h"
+
+#if HAL_USE_USBH
+
+#include "ch.h"
+#include "usbh/debug.h"
+#include <stdarg.h>
+#include "chprintf.h"
+
+#if USBH_DEBUG_ENABLE
+
+#define MAX_FILLER 11
+#define FLOAT_PRECISION 9
+#define MPRINTF_USE_FLOAT 0
+
+static char *long_to_string_with_divisor(char *p, long num, unsigned radix, long divisor) 
+{
+	int i;
+	char *q;
+	long l, ll;
+
+	l = num;
+	if (divisor == 0) {
+		ll = num;
+	} else {
+		ll = divisor;
+	}
+
+	q = p + MAX_FILLER;
+	do {
+		i = (int)(l % radix);
+		i += '0';
+		if (i > '9') {
+			i += 'A' - '0' - 10;
+		}
+		*--q = i;
+		l /= radix;
+	} while ((ll /= radix) != 0);
+
+	i = (int)(p + MAX_FILLER - q);
+	do {
+		*p++ = *q++;
+	} while (--i);
+
+	return p;
+}
+
+static char *ltoa(char *p, long num, unsigned radix) {
+
+	return long_to_string_with_divisor(p, num, radix, 0);
+}
+
+#if MPRINTF_USE_FLOAT
+static const long _pow10[FLOAT_PRECISION] = {10, 100, 1000, 10000, 100000, 1000000,
+	10000000, 100000000, 1000000000};
+static const double m10[FLOAT_PRECISION] = {5.0/100, 5.0/1000, 5.0/10000, 5.0/100000, 5.0/1000000,
+	5.0/10000000, 5.0/100000000, 5.0/1000000000, 5.0/10000000000};
+
+static char *ftoa(char *p, double num, unsigned long precision, bool dot) {
+	long l;
+	char *q;
+	double r;
+
+
+	if (precision == 0) {
+		l = (long)(num + 0.5);
+		return long_to_string_with_divisor(p, l, 10, 0);
+	} else {
+		if (precision > FLOAT_PRECISION) precision = FLOAT_PRECISION;
+		r = m10[precision - 1];
+		precision = _pow10[precision - 1];
+
+		l = (long)num;
+		p = long_to_string_with_divisor(p, l, 10, 0);
+		if (dot) *p++ = '.';
+		l = (long)((num - l + r) * precision);
+		q = long_to_string_with_divisor(p, l, 10, precision / 10) - 1;
+
+		while (q > p) {
+			if (*q != '0') {
+				break;
+			}
+			--q;
+		}
+		return ++q;
+	}
+
+	
+
+
+}
+#endif
+
+static inline void _put(char c) {
+	input_queue_t *iqp = &USBH_DEBUG_USBHD.iq;
+
+	if (chIQIsFullI(iqp))
+		return;
+
+	iqp->q_counter++;
+	*iqp->q_wrptr++ = c;
+	if (iqp->q_wrptr >= iqp->q_top)
+		iqp->q_wrptr = iqp->q_buffer;
+
+}
+
+int _dbg_printf(const char *fmt, va_list ap) {
+	char *p, *s, c, filler;
+	int i, precision, width;
+	int n = 0;
+	bool is_long, left_align, sign;
+	long l;
+#if MPRINTF_USE_FLOAT
+	double f;
+	char tmpbuf[2*MAX_FILLER + 1];
+#else
+	char tmpbuf[MAX_FILLER + 1];
+#endif
+
+	for (;;) {
+
+		//agarrar nuevo caracter de formato
+		c = *fmt++;
+
+		//chequeo eos
+		if (c == 0) return n;
+
+		//copio los caracteres comunes
+		if (c != '%') {
+			_put(c);
+			n++;
+			continue;
+		}
+
+		//encontré un '%'
+		p = tmpbuf;
+		s = tmpbuf;
+
+		//left align
+		left_align = FALSE;
+		if (*fmt == '-') {
+			fmt++;
+			left_align = TRUE;
+		}
+		
+		sign = FALSE;
+		if (*fmt == '+') {
+			fmt++;
+			sign = TRUE;
+		}
+
+		//filler
+		filler = ' ';
+		if (*fmt == '0') {
+			fmt++;
+			filler = '0';
+		}
+
+		//width
+		width = 0;
+		while (TRUE) {
+			c = *fmt++;
+			if (c >= '0' && c <= '9')
+				c -= '0';
+			else if (c == '*')
+				c = va_arg(ap, int);
+			else
+				break;
+			width = width * 10 + c;
+		}
+
+		//precision
+		precision = 0;
+		if (c == '.') {
+
+			if (*fmt == 'n') {
+				fmt++;
+
+			}
+			while (TRUE) {
+				c = *fmt++;
+				if (c >= '0' && c <= '9')
+					c -= '0';
+				else if (c == '*')
+					c = va_arg(ap, int);
+				else
+					break;
+				precision = precision * 10 + c;
+			}
+		}
+
+		//long modifier
+		if (c == 'l' || c == 'L') {
+			is_long = TRUE;
+			if (*fmt)
+				c = *fmt++;
+		}
+		else
+			is_long = (c >= 'A') && (c <= 'Z');
+
+		/* Command decoding.*/
+		switch (c) {
+		//char
+		case 'c':
+			filler = ' ';
+			*p++ = va_arg(ap, int);
+			break;
+
+		//string
+		case 's':
+			filler = ' ';
+			if ((s = va_arg(ap, char *)) == 0)
+				s = (char *)"(null)";
+			if (precision == 0)
+				precision = 32767;
+
+			//strlen con límite hasta precision
+			for (p = s; *p && (--precision >= 0); p++)
+				;
+			break;
+
+
+
+		case 'D':
+		case 'd':
+		case 'I':
+		case 'i':
+			if (is_long)
+				l = va_arg(ap, long);
+			else
+				l = va_arg(ap, int);
+			if (l < 0) {
+				*p++ = '-';
+				l = -l;
+				sign = TRUE;
+			} else if (sign) {
+				*p++ = '+';
+			}
+			p = ltoa(p, l, 10);
+			break;
+
+#if MPRINTF_USE_FLOAT
+		case 'f':
+			f = va_arg(ap, double);
+			if (f < 0) {
+				*p++ = '-';
+				f = -f;
+				sign = TRUE;
+			} else if (sign) {
+				*p++ = '+';
+			}
+			if (prec == FALSE) precision = 6;
+			p = ftoa(p, f, precision, dot);
+			break;
+#endif
+
+
+		case 'X':
+		case 'x':
+			c = 16;
+			goto unsigned_common;
+		case 'U':
+		case 'u':
+			c = 10;
+			goto unsigned_common;
+		case 'O':
+		case 'o':
+			c = 8;
+
+unsigned_common:
+			if (is_long)
+				l = va_arg(ap, unsigned long);
+			else
+				l = va_arg(ap, unsigned int);
+			p = ltoa(p, l, c);
+			break;
+
+		//copiar
+		default:
+			*p++ = c;
+			break;
+		}
+
+		//longitud
+		i = (int)(p - s);
+
+		//calculo cuántos caracteres de filler debo poner
+		if ((width -= i) < 0)
+			width = 0;
+
+		if (left_align == FALSE)
+			width = -width;
+
+		if (width < 0) {
+			//alineado a la derecha
+
+			//poner el signo adelante
+			if (sign && filler == '0') {
+				_put(*s++);
+				n++;
+				i--;
+			}
+
+			//fill a la izquierda
+			do {
+				_put(filler);
+				n++;
+			} while (++width != 0);
+		}
+
+		//copiar los caracteres
+		while (--i >= 0) {
+			_put(*s++);
+			n++;
+		}
+
+		//fill a la derecha
+		while (width) {
+			_put(filler);
+			n++;
+			width--;
+		}
+	}
+
+	//return n; // can raise 'code is unreachable' warning
+
+}
+
+static void _print_hdr(void)
+{
+	uint32_t hfnum = USBH_DEBUG_USBHD.otg->HFNUM;
+	uint16_t hfir = USBH_DEBUG_USBHD.otg->HFIR;
+
+	_put(0xff);
+	_put(0xff);
+	_put(hfir & 0xff);
+	_put(hfir >> 8);
+	_put(hfnum & 0xff);
+	_put((hfnum >> 8) & 0xff);
+	_put((hfnum >> 16) & 0xff);
+	_put((hfnum >> 24) & 0xff);
+}
+
+void usbDbgPrintf(const char *fmt, ...)
+{
+	va_list ap;
+	va_start(ap, fmt);
+	syssts_t sts = chSysGetStatusAndLockX();
+	_print_hdr();
+	_dbg_printf(fmt, ap);
+	_put(0);
+	chThdDequeueNextI(&USBH_DEBUG_USBHD.iq.q_waiting, Q_OK);
+	chSysRestoreStatusX(sts);
+	va_end(ap);
+}
+
+
+void usbDbgPuts(const char *s)
+{
+	uint32_t buff[2] = {
+		0xffff | (USBH_DEBUG_USBHD.otg->HFIR << 16),
+		USBH_DEBUG_USBHD.otg->HFNUM
+	};
+	uint8_t *p = (uint8_t *)buff;
+	uint8_t *top = p + 8;
+
+	syssts_t sts = chSysGetStatusAndLockX();
+	input_queue_t *iqp = &USBH_DEBUG_USBHD.iq;
+	int rem = sizeof(USBH_DEBUG_USBHD.dbg_buff) - iqp->q_counter;
+	while (rem) {
+		*iqp->q_wrptr++ = *p;
+		if (iqp->q_wrptr >= iqp->q_top)
+			iqp->q_wrptr = iqp->q_buffer;
+		rem--;
+		if (++p == top) break;
+	}
+	while (rem) {
+		*iqp->q_wrptr++ = *s;
+		if (iqp->q_wrptr >= iqp->q_top)
+			iqp->q_wrptr = iqp->q_buffer;
+		rem--;
+		if (!*s++) break;
+	}
+	iqp->q_counter = sizeof(USBH_DEBUG_USBHD.dbg_buff) - rem;
+	chThdDequeueNextI(&USBH_DEBUG_USBHD.iq.q_waiting, Q_OK);
+	chSysRestoreStatusX(sts);
+}
+
+void usbDbgReset(void) {
+	const char *msg = "\r\n\r\n==== DEBUG OUTPUT RESET ====\r\n";
+
+	syssts_t sts = chSysGetStatusAndLockX();
+	chIQResetI(&USBH_DEBUG_USBHD.iq);
+	chOQResetI(&USBH_DEBUG_SD.oqueue);
+	while (*msg) {
+		*USBH_DEBUG_SD.oqueue.q_wrptr++ = *msg++;
+		USBH_DEBUG_SD.oqueue.q_counter--;
+	}
+	chSysRestoreStatusX(sts);
+}
+
+static int _get(void) {
+	if (!USBH_DEBUG_USBHD.iq.q_counter) return -1;
+	USBH_DEBUG_USBHD.iq.q_counter--;
+	uint8_t b = *USBH_DEBUG_USBHD.iq.q_rdptr++;
+	if (USBH_DEBUG_USBHD.iq.q_rdptr >= USBH_DEBUG_USBHD.iq.q_top) {
+		USBH_DEBUG_USBHD.iq.q_rdptr = USBH_DEBUG_USBHD.iq.q_buffer;
+	}
+	return b;
+}
+
+void usbDbgSystemHalted(void) {
+	while (true) {
+		if (!((bool)((USBH_DEBUG_SD.oqueue.q_wrptr == USBH_DEBUG_SD.oqueue.q_rdptr) && (USBH_DEBUG_SD.oqueue.q_counter != 0U))))
+			break;
+		USBH_DEBUG_SD.oqueue.q_counter++;
+		while (!(USART1->SR & USART_SR_TXE));
+		USART1->DR = *USBH_DEBUG_SD.oqueue.q_rdptr++;
+		if (USBH_DEBUG_SD.oqueue.q_rdptr >= USBH_DEBUG_SD.oqueue.q_top) {
+			USBH_DEBUG_SD.oqueue.q_rdptr = USBH_DEBUG_SD.oqueue.q_buffer;
+		}
+	}
+
+	int c;
+	int state = 0;
+	for (;;) {
+		c = _get(); if (c < 0) break;
+
+		if (state == 0) {
+			if (c == 0xff) state = 1;
+		} else if (state == 1) {
+			if (c == 0xff) state = 2;
+			else (state = 0);
+		} else {
+			c = _get(); if (c < 0) return;
+			c = _get(); if (c < 0) return;
+			c = _get(); if (c < 0) return;
+			c = _get(); if (c < 0) return;
+			c = _get(); if (c < 0) return;
+
+			while (true) {
+				c = _get(); if (c < 0) return;
+				if (!c) {
+					while (!(USART1->SR & USART_SR_TXE));
+					USART1->DR = '\r';
+					while (!(USART1->SR & USART_SR_TXE));
+					USART1->DR = '\n';
+					state = 0;
+					break;
+				}
+				while (!(USART1->SR & USART_SR_TXE));
+				USART1->DR = c;
+			}
+		}
+	}
+}
+
+static void usb_debug_thread(void *p) {
+	USBHDriver *host = (USBHDriver *)p;
+	uint8_t state = 0;
+
+	chRegSetThreadName("USBH_DBG");
+	while (true) {
+		msg_t c = chIQGet(&host->iq);
+		if (c < 0) goto reset;
+
+		if (state == 0) {
+			if (c == 0xff) state = 1;
+		} else if (state == 1) {
+			if (c == 0xff) state = 2;
+			else (state = 0);
+		} else {
+			uint16_t hfir;
+			uint32_t hfnum;
+
+			hfir = c;
+			c = chIQGet(&host->iq); if (c < 0) goto reset;
+			hfir |= c << 8;
+
+			c = chIQGet(&host->iq); if (c < 0) goto reset;
+			hfnum = c;
+			c = chIQGet(&host->iq); if (c < 0) goto reset;
+			hfnum |= c << 8;
+			c = chIQGet(&host->iq); if (c < 0) goto reset;
+			hfnum |= c << 16;
+			c = chIQGet(&host->iq); if (c < 0) goto reset;
+			hfnum |= c << 24;
+
+			uint32_t f = hfnum & 0xffff;
+			uint32_t p = 1000 - ((hfnum >> 16) / (hfir / 1000));
+			chprintf((BaseSequentialStream *)&USBH_DEBUG_SD, "%05d.%03d  ", f, p);
+
+			while (true) {
+				c = chIQGet(&host->iq); if (c < 0) goto reset;
+				if (!c) {
+					sdPut(&USBH_DEBUG_SD, '\r');
+					sdPut(&USBH_DEBUG_SD, '\n');
+					state = 0;
+					break;
+				}
+				sdPut(&USBH_DEBUG_SD, (uint8_t)c);
+			}
+		}
+
+		continue;
+reset:
+		state = 0;
+	}
+}
+
+void usbDbgInit(USBHDriver *host) {
+	if (host != &USBH_DEBUG_USBHD)
+		return;
+	chIQObjectInit(&USBH_DEBUG_USBHD.iq, USBH_DEBUG_USBHD.dbg_buff, sizeof(USBH_DEBUG_USBHD.dbg_buff), 0, 0);
+	chThdCreateStatic(USBH_DEBUG_USBHD.waDebug, sizeof(USBH_DEBUG_USBHD.waDebug), NORMALPRIO, usb_debug_thread, &USBH_DEBUG_USBHD);
+}
+#endif
+
+#endif
diff --git a/os/hal/src/usbh/usbh_desciter.c b/os/hal/src/usbh/hal_usbh_desciter.c
index 80e4728..63137d4 100644
--- a/os/hal/src/usbh/usbh_desciter.c
+++ b/os/hal/src/usbh/hal_usbh_desciter.c
@@ -1,165 +1,165 @@
-/*

-    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio

-              Copyright (C) 2015 Diego Ismirlian, TISA, (dismirlian (at) google's mail)

-

-    Licensed under the Apache License, Version 2.0 (the "License");

-    you may not use this file except in compliance with the License.

-    You may obtain a copy of the License at

-

-        http://www.apache.org/licenses/LICENSE-2.0

-

-    Unless required by applicable law or agreed to in writing, software

-    distributed under the License is distributed on an "AS IS" BASIS,

-    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.

-    See the License for the specific language governing permissions and

-    limitations under the License.

-*/

-

-#include "hal.h"

-

-#if HAL_USE_USBH

-

-#include "usbh/defs.h"

-#include "usbh/desciter.h"

-

-void cfg_iter_init(generic_iterator_t *icfg, const uint8_t *buff, uint16_t rem) {

-	icfg->valid = 0;

-

-	if ((buff[0] < 2) || (rem < 2) || (rem < buff[0])

-			|| (buff[0] < USBH_DT_CONFIG_SIZE)

-			|| (buff[1] != USBH_DT_CONFIG))

-		return;

-

-	if (rem > ((usbh_config_descriptor_t *)buff)->wTotalLength) {

-		rem = ((usbh_config_descriptor_t *)buff)->wTotalLength;

-	}

-

-	icfg->valid = 1;

-	icfg->rem = rem;

-	icfg->curr = buff;

-}

-

-void if_iter_next(if_iterator_t *iif) {

-	const uint8_t *curr = iif->curr;

-	uint16_t rem = iif->rem;

-

-	iif->valid = 0;

-

-	if ((curr[0] < 2) || (rem < 2) || (rem < curr[0]))

-		return;

-

-	for (;;) {

-		rem -= curr[0];

-		curr += curr[0];

-

-		if ((curr[0] < 2) || (rem < 2) || (rem < curr[0]))

-			return;

-

-		if (curr[1] == USBH_DT_INTERFACE_ASSOCIATION) {

-			if (curr[0] < USBH_DT_INTERFACE_ASSOCIATION_SIZE)

-				return;

-

-			iif->iad = (usbh_ia_descriptor_t *)curr;

-

-		} else if (curr[1] == USBH_DT_INTERFACE) {

-			if (curr[0] < USBH_DT_INTERFACE_SIZE)

-				return;

-

-			if (iif->iad) {

-				if ((curr[2] < iif->iad->bFirstInterface)

-					|| (curr[2] >= (iif->iad->bFirstInterface + iif->iad->bInterfaceCount)))

-					iif->iad = 0;

-			}

-			break;

-		}

-	}

-

-	iif->valid = 1;

-	iif->rem = rem;

-	iif->curr = curr;

-}

-

-void if_iter_init(if_iterator_t *iif, const generic_iterator_t *icfg) {

-	iif->iad = 0;

-	iif->curr = icfg->curr;

-	iif->rem = icfg->rem;

-	if_iter_next(iif);

-}

-

-void ep_iter_next(generic_iterator_t *iep) {

-	const uint8_t *curr = iep->curr;

-	uint16_t rem = iep->rem;

-

-	iep->valid = 0;

-

-	if ((curr[0] < 2) || (rem < 2) || (rem < curr[0]))

-		return;

-

-	for (;;) {

-		rem -= curr[0];

-		curr += curr[0];

-

-		if ((curr[0] < 2) || (rem < 2) || (rem < curr[0]))

-			return;

-

-		if ((curr[1] == USBH_DT_INTERFACE_ASSOCIATION)

-				|| (curr[1] == USBH_DT_INTERFACE)

-				|| (curr[1] == USBH_DT_CONFIG)) {

-			return;

-		} else if (curr[1] == USBH_DT_ENDPOINT) {

-			if (curr[0] < USBH_DT_ENDPOINT_SIZE)

-				return;

-

-			break;

-		}

-	}

-

-	iep->valid = 1;

-	iep->rem = rem;

-	iep->curr = curr;

-}

-

-void ep_iter_init(generic_iterator_t *iep, const if_iterator_t *iif) {

-	iep->curr = iif->curr;

-	iep->rem = iif->rem;

-	ep_iter_next(iep);

-}

-

-void cs_iter_next(generic_iterator_t *ics) {

-	const uint8_t *curr = ics->curr;

-	uint16_t rem = ics->rem;

-

-	ics->valid = 0;

-

-	if ((curr[0] < 2) || (rem < 2) || (rem < curr[0]))

-		return;

-

-	//for (;;) {

-		rem -= curr[0];

-		curr += curr[0];

-

-		if ((curr[0] < 2) || (rem < 2) || (rem < curr[0]))

-			return;

-

-		if ((curr[1] == USBH_DT_INTERFACE_ASSOCIATION)

-				|| (curr[1] == USBH_DT_INTERFACE)

-				|| (curr[1] == USBH_DT_CONFIG)

-				|| (curr[1] == USBH_DT_ENDPOINT)) {

-			return;

-		}

-

-	//	break;

-	//}

-

-	ics->valid = 1;

-	ics->rem = rem;

-	ics->curr = curr;

-}

-

-void cs_iter_init(generic_iterator_t *ics, const generic_iterator_t *iter) {

-	ics->curr = iter->curr;

-	ics->rem = iter->rem;

-	cs_iter_next(ics);

-}

-

-#endif

+/*
+    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio
+              Copyright (C) 2015 Diego Ismirlian, TISA, (dismirlian (at) google's mail)
+
+    Licensed under the Apache License, Version 2.0 (the "License");
+    you may not use this file except in compliance with the License.
+    You may obtain a copy of the License at
+
+        http://www.apache.org/licenses/LICENSE-2.0
+
+    Unless required by applicable law or agreed to in writing, software
+    distributed under the License is distributed on an "AS IS" BASIS,
+    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+    See the License for the specific language governing permissions and
+    limitations under the License.
+*/
+
+#include "hal.h"
+
+#if HAL_USE_USBH
+
+#include "usbh/defs.h"
+#include "usbh/desciter.h"
+
+void cfg_iter_init(generic_iterator_t *icfg, const uint8_t *buff, uint16_t rem) {
+	icfg->valid = 0;
+
+	if ((buff[0] < 2) || (rem < 2) || (rem < buff[0])
+			|| (buff[0] < USBH_DT_CONFIG_SIZE)
+			|| (buff[1] != USBH_DT_CONFIG))
+		return;
+
+	if (rem > ((usbh_config_descriptor_t *)buff)->wTotalLength) {
+		rem = ((usbh_config_descriptor_t *)buff)->wTotalLength;
+	}
+
+	icfg->valid = 1;
+	icfg->rem = rem;
+	icfg->curr = buff;
+}
+
+void if_iter_next(if_iterator_t *iif) {
+	const uint8_t *curr = iif->curr;
+	uint16_t rem = iif->rem;
+
+	iif->valid = 0;
+
+	if ((curr[0] < 2) || (rem < 2) || (rem < curr[0]))
+		return;
+
+	for (;;) {
+		rem -= curr[0];
+		curr += curr[0];
+
+		if ((curr[0] < 2) || (rem < 2) || (rem < curr[0]))
+			return;
+
+		if (curr[1] == USBH_DT_INTERFACE_ASSOCIATION) {
+			if (curr[0] < USBH_DT_INTERFACE_ASSOCIATION_SIZE)
+				return;
+
+			iif->iad = (usbh_ia_descriptor_t *)curr;
+
+		} else if (curr[1] == USBH_DT_INTERFACE) {
+			if (curr[0] < USBH_DT_INTERFACE_SIZE)
+				return;
+
+			if (iif->iad) {
+				if ((curr[2] < iif->iad->bFirstInterface)
+					|| (curr[2] >= (iif->iad->bFirstInterface + iif->iad->bInterfaceCount)))
+					iif->iad = 0;
+			}
+			break;
+		}
+	}
+
+	iif->valid = 1;
+	iif->rem = rem;
+	iif->curr = curr;
+}
+
+void if_iter_init(if_iterator_t *iif, const generic_iterator_t *icfg) {
+	iif->iad = 0;
+	iif->curr = icfg->curr;
+	iif->rem = icfg->rem;
+	if_iter_next(iif);
+}
+
+void ep_iter_next(generic_iterator_t *iep) {
+	const uint8_t *curr = iep->curr;
+	uint16_t rem = iep->rem;
+
+	iep->valid = 0;
+
+	if ((curr[0] < 2) || (rem < 2) || (rem < curr[0]))
+		return;
+
+	for (;;) {
+		rem -= curr[0];
+		curr += curr[0];
+
+		if ((curr[0] < 2) || (rem < 2) || (rem < curr[0]))
+			return;
+
+		if ((curr[1] == USBH_DT_INTERFACE_ASSOCIATION)
+				|| (curr[1] == USBH_DT_INTERFACE)
+				|| (curr[1] == USBH_DT_CONFIG)) {
+			return;
+		} else if (curr[1] == USBH_DT_ENDPOINT) {
+			if (curr[0] < USBH_DT_ENDPOINT_SIZE)
+				return;
+
+			break;
+		}
+	}
+
+	iep->valid = 1;
+	iep->rem = rem;
+	iep->curr = curr;
+}
+
+void ep_iter_init(generic_iterator_t *iep, const if_iterator_t *iif) {
+	iep->curr = iif->curr;
+	iep->rem = iif->rem;
+	ep_iter_next(iep);
+}
+
+void cs_iter_next(generic_iterator_t *ics) {
+	const uint8_t *curr = ics->curr;
+	uint16_t rem = ics->rem;
+
+	ics->valid = 0;
+
+	if ((curr[0] < 2) || (rem < 2) || (rem < curr[0]))
+		return;
+
+	//for (;;) {
+		rem -= curr[0];
+		curr += curr[0];
+
+		if ((curr[0] < 2) || (rem < 2) || (rem < curr[0]))
+			return;
+
+		if ((curr[1] == USBH_DT_INTERFACE_ASSOCIATION)
+				|| (curr[1] == USBH_DT_INTERFACE)
+				|| (curr[1] == USBH_DT_CONFIG)
+				|| (curr[1] == USBH_DT_ENDPOINT)) {
+			return;
+		}
+
+	//	break;
+	//}
+
+	ics->valid = 1;
+	ics->rem = rem;
+	ics->curr = curr;
+}
+
+void cs_iter_init(generic_iterator_t *ics, const generic_iterator_t *iter) {
+	ics->curr = iter->curr;
+	ics->rem = iter->rem;
+	cs_iter_next(ics);
+}
+
+#endif
diff --git a/os/hal/src/usbh/usbh_ftdi.c b/os/hal/src/usbh/hal_usbh_ftdi.c
index cdf3410..4bd7296 100644
--- a/os/hal/src/usbh/usbh_ftdi.c
+++ b/os/hal/src/usbh/hal_usbh_ftdi.c
@@ -1,717 +1,717 @@
-/*

-    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio

-              Copyright (C) 2015 Diego Ismirlian, TISA, (dismirlian (at) google's mail)

-

-    Licensed under the Apache License, Version 2.0 (the "License");

-    you may not use this file except in compliance with the License.

-    You may obtain a copy of the License at

-

-        http://www.apache.org/licenses/LICENSE-2.0

-

-    Unless required by applicable law or agreed to in writing, software

-    distributed under the License is distributed on an "AS IS" BASIS,

-    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.

-    See the License for the specific language governing permissions and

-    limitations under the License.

-*/

-

-#include "hal.h"

-#include "usbh.h"

-

-#if HAL_USBH_USE_FTDI

-

-#if !HAL_USE_USBH

-#error "USBHFTDI needs USBH"

-#endif

-

-#include <string.h>

-#include "usbh/dev/ftdi.h"

-#include "usbh/internal.h"

-

-//#pragma GCC optimize("Og")

-

-

-#if USBHFTDI_DEBUG_ENABLE_TRACE

-#define udbgf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)

-#define udbg(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)

-#else

-#define udbgf(f, ...)  do {} while(0)

-#define udbg(f, ...)   do {} while(0)

-#endif

-

-#if USBHFTDI_DEBUG_ENABLE_INFO

-#define uinfof(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)

-#define uinfo(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)

-#else

-#define uinfof(f, ...)  do {} while(0)

-#define uinfo(f, ...)   do {} while(0)

-#endif

-

-#if USBHFTDI_DEBUG_ENABLE_WARNINGS

-#define uwarnf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)

-#define uwarn(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)

-#else

-#define uwarnf(f, ...)  do {} while(0)

-#define uwarn(f, ...)   do {} while(0)

-#endif

-

-#if USBHFTDI_DEBUG_ENABLE_ERRORS

-#define uerrf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)

-#define uerr(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)

-#else

-#define uerrf(f, ...)  do {} while(0)

-#define uerr(f, ...)   do {} while(0)

-#endif

-

-

-/*===========================================================================*/

-/* USB Class driver loader for FTDI								 		 	 */

-/*===========================================================================*/

-USBHFTDIDriver USBHFTDID[HAL_USBHFTDI_MAX_INSTANCES];

-

-static usbh_baseclassdriver_t *_ftdi_load(usbh_device_t *dev, const uint8_t *descriptor, uint16_t rem);

-static void _ftdi_unload(usbh_baseclassdriver_t *drv);

-

-static const usbh_classdriver_vmt_t class_driver_vmt = {

-	_ftdi_load,

-	_ftdi_unload

-};

-

-const usbh_classdriverinfo_t usbhftdiClassDriverInfo = {

-	0xff, 0xff, 0xff, "FTDI", &class_driver_vmt

-};

-

-static USBHFTDIPortDriver *_find_port(void) {

-	uint8_t i;

-	for (i = 0; i < HAL_USBHFTDI_MAX_PORTS; i++) {

-		if (FTDIPD[i].ftdip == NULL)

-			return &FTDIPD[i];

-	}

-	return NULL;

-}

-

-static usbh_baseclassdriver_t *_ftdi_load(usbh_device_t *dev, const uint8_t *descriptor, uint16_t rem) {

-	int i;

-	USBHFTDIDriver *ftdip;

-

-	if (dev->devDesc.idVendor != 0x0403) {

-		uerr("FTDI: Unrecognized VID");

-		return NULL;

-	}

-

-	switch (dev->devDesc.idProduct) {

-	case 0x6001:

-	case 0x6010:

-	case 0x6011:

-	case 0x6014:

-	case 0x6015:

-		break;

-	default:

-		uerr("FTDI: Unrecognized PID");

-		return NULL;

-	}

-

-	if ((rem < descriptor[0]) || (descriptor[1] != USBH_DT_INTERFACE))

-		return NULL;

-

-	const usbh_interface_descriptor_t * const ifdesc = (const usbh_interface_descriptor_t * const)descriptor;

-	if (ifdesc->bInterfaceNumber != 0) {

-		uwarn("FTDI: Will allocate driver along with IF #0");

-	}

-

-	/* alloc driver */

-	for (i = 0; i < HAL_USBHFTDI_MAX_INSTANCES; i++) {

-		if (USBHFTDID[i].dev == NULL) {

-			ftdip = &USBHFTDID[i];

-			goto alloc_ok;

-		}

-	}

-

-	uwarn("FTDI: Can't alloc driver");

-

-	/* can't alloc */

-	return NULL;

-

-alloc_ok:

-	/* initialize the driver's variables */

-	ftdip->ports = 0;

-	switch (dev->devDesc.bcdDevice) {

-	case 0x200:		//AM

-		uinfo("FTDI: Type A chip");

-		ftdip->type = USBHFTDI_TYPE_A;

-		break;

-	case 0x400:		//BM

-	case 0x500:		//2232C

-	case 0x600:		//R

-	case 0x1000:	//230X

-		uinfo("FTDI: Type B chip");

-		ftdip->type = USBHFTDI_TYPE_B;

-		break;

-	case 0x700:		//2232H;

-	case 0x800:		//4232H;

-	case 0x900:		//232H;

-		uinfo("FTDI: Type H chip");

-		ftdip->type = USBHFTDI_TYPE_H;

-	default:

-		uerr("FTDI: Unrecognized chip type");

-		return NULL;

-	}

-	usbhEPSetName(&dev->ctrl, "FTD[CTRL]");

-

-	/* parse the configuration descriptor */

-	generic_iterator_t iep, icfg;

-	if_iterator_t iif;

-	cfg_iter_init(&icfg, dev->fullConfigurationDescriptor, dev->basicConfigDesc.wTotalLength);

-	for (if_iter_init(&iif, &icfg); iif.valid; if_iter_next(&iif)) {

-		const usbh_interface_descriptor_t *const ifdesc = if_get(&iif);

-		uinfof("FTDI: Interface #%d", ifdesc->bInterfaceNumber);

-

-		USBHFTDIPortDriver *const prt = _find_port();

-		if (prt == NULL) {

-			uwarn("\tCan't alloc port for this interface");

-			break;

-		}

-

-		prt->ifnum = ifdesc->bInterfaceNumber;

-		prt->epin.status = USBH_EPSTATUS_UNINITIALIZED;

-		prt->epout.status = USBH_EPSTATUS_UNINITIALIZED;

-

-		for (ep_iter_init(&iep, &iif); iep.valid; ep_iter_next(&iep)) {

-			const usbh_endpoint_descriptor_t *const epdesc = ep_get(&iep);

-			if ((epdesc->bEndpointAddress & 0x80) && (epdesc->bmAttributes == USBH_EPTYPE_BULK)) {

-				uinfof("BULK IN endpoint found: bEndpointAddress=%02x", epdesc->bEndpointAddress);

-				usbhEPObjectInit(&prt->epin, dev, epdesc);

-				usbhEPSetName(&prt->epin, "FTD[BIN ]");

-			} else if (((epdesc->bEndpointAddress & 0x80) == 0)

-					&& (epdesc->bmAttributes == USBH_EPTYPE_BULK)) {

-				uinfof("BULK OUT endpoint found: bEndpointAddress=%02x", epdesc->bEndpointAddress);

-				usbhEPObjectInit(&prt->epout, dev, epdesc);

-				usbhEPSetName(&prt->epout, "FTD[BOUT]");

-			} else {

-				uinfof("unsupported endpoint found: bEndpointAddress=%02x, bmAttributes=%02x",

-						epdesc->bEndpointAddress, epdesc->bmAttributes);

-			}

-		}

-

-		if ((prt->epin.status != USBH_EPSTATUS_CLOSED)

-				|| (prt->epout.status != USBH_EPSTATUS_CLOSED)) {

-			uwarn("\tCouldn't find endpoints; can't alloc port for this interface");

-			continue;

-		}

-

-		/* link the new block driver to the list */

-		prt->next = ftdip->ports;

-		ftdip->ports = prt;

-		prt->ftdip = ftdip;

-

-		prt->state = USBHFTDIP_STATE_ACTIVE;

-	}

-

-	return (usbh_baseclassdriver_t *)ftdip;

-

-}

-

-static void _stop(USBHFTDIPortDriver *ftdipp);

-static void _ftdi_unload(usbh_baseclassdriver_t *drv) {

-	osalDbgCheck(drv != NULL);

-	USBHFTDIDriver *const ftdip = (USBHFTDIDriver *)drv;

-	USBHFTDIPortDriver *ftdipp = ftdip->ports;

-

-	osalMutexLock(&ftdip->mtx);

-	while (ftdipp) {

-		_stop(ftdipp);

-		ftdipp = ftdipp->next;

-	}

-

-	ftdipp = ftdip->ports;

-	osalSysLock();

-	while (ftdipp) {

-		USBHFTDIPortDriver *next = ftdipp->next;

-		usbhftdipObjectInit(ftdipp);

-		ftdipp = next;

-	}

-	osalSysUnlock();

-	osalMutexUnlock(&ftdip->mtx);

-}

-

-

-USBHFTDIPortDriver FTDIPD[HAL_USBHFTDI_MAX_PORTS];

-

-

-#define FTDI_COMMAND_RESET		0

-#define FTDI_RESET_ALL 			0

-#define FTDI_RESET_PURGE_RX 	1

-#define FTDI_RESET_PURGE_TX 	2

-

-#define FTDI_COMMAND_SETFLOW    2

-

-#define FTDI_COMMAND_SETBAUD    3

-

-#define FTDI_COMMAND_SETDATA    4

-#define FTDI_SETDATA_BREAK      (0x1 << 14)

-

-#if 0

-#define FTDI_COMMAND_MODEMCTRL  	1

-#define FTDI_COMMAND_GETMODEMSTATUS       5 /* Retrieve current value of modem status register */

-#define FTDI_COMMAND_SETEVENTCHAR         6 /* Set the event character */

-#define FTDI_COMMAND_SETERRORCHAR         7 /* Set the error character */

-#define FTDI_COMMAND_SETLATENCYTIMER      9 /* Set the latency timer */

-#define FTDI_COMMAND_GETLATENCYTIMER      10 /* Get the latency timer */

-#endif

-

-/*

- * DATA FORMAT

- *

- * IN Endpoint

- *

- * The device reserves the first two bytes of data on this endpoint to contain

- * the current values of the modem and line status registers. In the absence of

- * data, the device generates a message consisting of these two status bytes

- * every 40 ms

- *

- * Byte 0: Modem Status

- *

- * Offset       Description

- * B0   Reserved - must be 1

- * B1   Reserved - must be 0

- * B2   Reserved - must be 0

- * B3   Reserved - must be 0

- * B4   Clear to Send (CTS)

- * B5   Data Set Ready (DSR)

- * B6   Ring Indicator (RI)

- * B7   Receive Line Signal Detect (RLSD)

- *

- * Byte 1: Line Status

- *

- * Offset       Description

- * B0   Data Ready (DR)

- * B1   Overrun Error (OE)

- * B2   Parity Error (PE)

- * B3   Framing Error (FE)

- * B4   Break Interrupt (BI)

- * B5   Transmitter Holding Register (THRE)

- * B6   Transmitter Empty (TEMT)

- * B7   Error in RCVR FIFO

- *

- */

-#define FTDI_RS0_CTS    (1 << 4)

-#define FTDI_RS0_DSR    (1 << 5)

-#define FTDI_RS0_RI     (1 << 6)

-#define FTDI_RS0_RLSD   (1 << 7)

-

-#define FTDI_RS_DR      1

-#define FTDI_RS_OE      (1<<1)

-#define FTDI_RS_PE      (1<<2)

-#define FTDI_RS_FE      (1<<3)

-#define FTDI_RS_BI      (1<<4)

-#define FTDI_RS_THRE    (1<<5)

-#define FTDI_RS_TEMT    (1<<6)

-#define FTDI_RS_FIFO    (1<<7)

-

-

-static usbh_urbstatus_t _ftdi_port_control(USBHFTDIPortDriver *ftdipp,

-		uint8_t bRequest, uint8_t wValue, uint8_t bHIndex, uint16_t wLength,

-		uint8_t *buff) {

-

-	static const uint8_t bmRequestType[] = {

-		USBH_REQTYPE_VENDOR | USBH_REQTYPE_OUT | USBH_REQTYPE_DEVICE, //0 FTDI_COMMAND_RESET

-		USBH_REQTYPE_VENDOR | USBH_REQTYPE_OUT | USBH_REQTYPE_DEVICE, //1 FTDI_COMMAND_MODEMCTRL

-		USBH_REQTYPE_VENDOR | USBH_REQTYPE_OUT | USBH_REQTYPE_DEVICE, //2 FTDI_COMMAND_SETFLOW

-		USBH_REQTYPE_VENDOR | USBH_REQTYPE_OUT | USBH_REQTYPE_DEVICE, //3 FTDI_COMMAND_SETBAUD

-		USBH_REQTYPE_VENDOR | USBH_REQTYPE_OUT | USBH_REQTYPE_DEVICE, //4 FTDI_COMMAND_SETDATA

-	};

-

-	osalDbgCheck(bRequest < sizeof_array(bmRequestType));

-	osalDbgCheck(bRequest != 1);

-

-	const USBH_DEFINE_BUFFER(usbh_control_request_t, req) = {

-			bmRequestType[bRequest],

-			bRequest,

-			wValue,

-			(bHIndex << 8) | (ftdipp->ifnum + 1),

-			wLength

-	};

-

-	return usbhControlRequestExtended(ftdipp->ftdip->dev, &req, buff, NULL, MS2ST(1000));

-}

-

-static uint32_t _get_divisor(uint32_t baud, usbhftdi_type_t type) {

-	static const uint8_t divfrac[8] = {0, 3, 2, 4, 1, 5, 6, 7};

-	uint32_t divisor;

-

-	if (type == USBHFTDI_TYPE_A) {

-		uint32_t divisor3 = ((48000000UL / 2) + baud / 2) / baud;

-		uinfof("FTDI: desired=%dbps, real=%dbps", baud, (48000000UL / 2) / divisor3);

-		if ((divisor3 & 0x7) == 7)

-			divisor3++; /* round x.7/8 up to x+1 */

-

-		divisor = divisor3 >> 3;

-	    divisor3 &= 0x7;

-		if (divisor3 == 1)

-			divisor |= 0xc000;

-		else if (divisor3 >= 4)

-			divisor |= 0x4000;

-		else if (divisor3 != 0)

-			divisor |= 0x8000;

-		else if (divisor == 1)

-			divisor = 0;    /* special case for maximum baud rate */

-	} else {

-		if (type == USBHFTDI_TYPE_B) {

-			divisor = ((48000000UL / 2) + baud / 2) / baud;

-			uinfof("FTDI: desired=%dbps, real=%dbps", baud, (48000000UL / 2) / divisor);

-		} else {

-			/* hi-speed baud rate is 10-bit sampling instead of 16-bit */

-			if (baud < 1200)

-				baud = 1200;

-			divisor = (120000000UL * 8 + baud * 5) / (baud * 10);

-			uinfof("FTDI: desired=%dbps, real=%dbps", baud, (120000000UL * 8) / divisor / 10);

-		}

-		divisor = (divisor >> 3) | (divfrac[divisor & 0x7] << 14);

-

-		/* Deal with special cases for highest baud rates. */

-		if (divisor == 1)

-			divisor = 0;

-		else if (divisor == 0x4001)

-			divisor = 1;

-

-		if (type == USBHFTDI_TYPE_H)

-			divisor |= 0x00020000;

-	}

-	return divisor;

-}

-

-static usbh_urbstatus_t _set_baudrate(USBHFTDIPortDriver *ftdipp, uint32_t baudrate) {

-	uint32_t divisor = _get_divisor(baudrate, ftdipp->ftdip->type);

-	uint16_t wValue = (uint16_t)divisor;

-	uint16_t wIndex = (uint16_t)(divisor >> 16);

-	if (ftdipp->ftdip->dev->basicConfigDesc.bNumInterfaces > 1)

-		wIndex = (wIndex << 8) | (ftdipp->ifnum + 1);

-

-	const USBH_DEFINE_BUFFER(usbh_control_request_t, req) = {

-		USBH_REQTYPE_VENDOR | USBH_REQTYPE_OUT | USBH_REQTYPE_DEVICE,

-		FTDI_COMMAND_SETBAUD,

-		wValue,

-		wIndex,

-		0

-	};

-	return usbhControlRequestExtended(ftdipp->ftdip->dev, &req, NULL, NULL, MS2ST(1000));

-}

-

-

-static void _submitOutI(USBHFTDIPortDriver *ftdipp, uint32_t len) {

-	udbgf("FTDI: Submit OUT %d", len);

-	ftdipp->oq_urb.requestedLength = len;

-	usbhURBObjectResetI(&ftdipp->oq_urb);

-	usbhURBSubmitI(&ftdipp->oq_urb);

-}

-

-static void _out_cb(usbh_urb_t *urb) {

-	USBHFTDIPortDriver *const ftdipp = (USBHFTDIPortDriver *)urb->userData;

-	switch (urb->status) {

-	case USBH_URBSTATUS_OK:

-		ftdipp->oq_ptr = ftdipp->oq_buff;

-		ftdipp->oq_counter = 64;

-		chThdDequeueNextI(&ftdipp->oq_waiting, Q_OK);

-		return;

-	case USBH_URBSTATUS_DISCONNECTED:

-		uwarn("FTDI: URB OUT disconnected");

-		chThdDequeueNextI(&ftdipp->oq_waiting, Q_RESET);

-		return;

-	default:

-		uerrf("FTDI: URB OUT status unexpected = %d", urb->status);

-		break;

-	}

-	usbhURBObjectResetI(&ftdipp->oq_urb);

-	usbhURBSubmitI(&ftdipp->oq_urb);

-}

-

-static size_t _write_timeout(USBHFTDIPortDriver *ftdipp, const uint8_t *bp,

-		size_t n, systime_t timeout) {

-	chDbgCheck(n > 0U);

-

-	size_t w = 0;

-	chSysLock();

-	while (true) {

-		if (ftdipp->state != USBHFTDIP_STATE_READY) {

-			chSysUnlock();

-			return w;

-		}

-		while (usbhURBIsBusy(&ftdipp->oq_urb)) {

-			if (chThdEnqueueTimeoutS(&ftdipp->oq_waiting, timeout) != Q_OK) {

-				chSysUnlock();

-				return w;

-			}

-		}

-

-		*ftdipp->oq_ptr++ = *bp++;

-		if (--ftdipp->oq_counter == 0) {

-			_submitOutI(ftdipp, 64);

-			chSchRescheduleS();

-		}

-		chSysUnlock(); /* Gives a preemption chance in a controlled point.*/

-

-		w++;

-		if (--n == 0U)

-			return w;

-

-		chSysLock();

-	}

-}

-

-static msg_t _put_timeout(USBHFTDIPortDriver *ftdipp, uint8_t b, systime_t timeout) {

-

-	chSysLock();

-	if (ftdipp->state != USBHFTDIP_STATE_READY) {

-		chSysUnlock();

-		return Q_RESET;

-	}

-

-	while (usbhURBIsBusy(&ftdipp->oq_urb)) {

-		msg_t msg = chThdEnqueueTimeoutS(&ftdipp->oq_waiting, timeout);

-		if (msg < Q_OK) {

-			chSysUnlock();

-			return msg;

-		}

-	}

-

-	*ftdipp->oq_ptr++ = b;

-	if (--ftdipp->oq_counter == 0) {

-		_submitOutI(ftdipp, 64);

-		chSchRescheduleS();

-	}

-	chSysUnlock();

-	return Q_OK;

-}

-

-static size_t _write(USBHFTDIPortDriver *ftdipp, const uint8_t *bp, size_t n) {

-	return _write_timeout(ftdipp, bp, n, TIME_INFINITE);

-}

-

-static msg_t _put(USBHFTDIPortDriver *ftdipp, uint8_t b) {

-	return _put_timeout(ftdipp, b, TIME_INFINITE);

-}

-

-static void _submitInI(USBHFTDIPortDriver *ftdipp) {

-	udbg("FTDI: Submit IN");

-	usbhURBObjectResetI(&ftdipp->iq_urb);

-	usbhURBSubmitI(&ftdipp->iq_urb);

-}

-

-static void _in_cb(usbh_urb_t *urb) {

-	USBHFTDIPortDriver *const ftdipp = (USBHFTDIPortDriver *)urb->userData;

-	switch (urb->status) {

-	case USBH_URBSTATUS_OK:

-		if (urb->actualLength < 2) {

-			uwarnf("FTDI: URB IN actualLength = %d, < 2", urb->actualLength);

-		} else if (urb->actualLength > 2) {

-			udbgf("FTDI: URB IN data len=%d, status=%02x %02x",

-					urb->actualLength - 2,

-					((uint8_t *)urb->buff)[0],

-					((uint8_t *)urb->buff)[1]);

-			ftdipp->iq_ptr = ftdipp->iq_buff + 2;

-			ftdipp->iq_counter = urb->actualLength - 2;

-			chThdDequeueNextI(&ftdipp->iq_waiting, Q_OK);

-			return;

-		} else {

-			udbgf("FTDI: URB IN no data, status=%02x %02x",

-					((uint8_t *)urb->buff)[0],

-					((uint8_t *)urb->buff)[1]);

-			return;

-		}

-		break;

-	case USBH_URBSTATUS_DISCONNECTED:

-		uwarn("FTDI: URB IN disconnected");

-		chThdDequeueNextI(&ftdipp->iq_waiting, Q_RESET);

-		return;

-	default:

-		uerrf("FTDI: URB IN status unexpected = %d", urb->status);

-		break;

-	}

-	_submitInI(ftdipp);

-}

-

-static size_t _read_timeout(USBHFTDIPortDriver *ftdipp, uint8_t *bp,

-		size_t n, systime_t timeout) {

-	size_t r = 0;

-

-	chDbgCheck(n > 0U);

-

-	chSysLock();

-	while (true) {

-		if (ftdipp->state != USBHFTDIP_STATE_READY) {

-			chSysUnlock();

-			return r;

-		}

-		while (ftdipp->iq_counter == 0) {

-			if (!usbhURBIsBusy(&ftdipp->iq_urb))

-				_submitInI(ftdipp);

-			if (chThdEnqueueTimeoutS(&ftdipp->iq_waiting, timeout) != Q_OK) {

-				chSysUnlock();

-				return r;

-			}

-		}

-		*bp++ = *ftdipp->iq_ptr++;

-		if (--ftdipp->iq_counter == 0) {

-			_submitInI(ftdipp);

-			chSchRescheduleS();

-		}

-		chSysUnlock();

-

-		r++;

-		if (--n == 0U)

-			return r;

-

-		chSysLock();

-	}

-}

-

-static msg_t _get_timeout(USBHFTDIPortDriver *ftdipp, systime_t timeout) {

-	uint8_t b;

-

-	chSysLock();

-	if (ftdipp->state != USBHFTDIP_STATE_READY) {

-		chSysUnlock();

-		return Q_RESET;

-	}

-	while (ftdipp->iq_counter == 0) {

-		if (!usbhURBIsBusy(&ftdipp->iq_urb))

-			_submitInI(ftdipp);

-		msg_t msg = chThdEnqueueTimeoutS(&ftdipp->iq_waiting, timeout);

-		if (msg < Q_OK) {

-			chSysUnlock();

-			return msg;

-		}

-	}

-	b = *ftdipp->iq_ptr++;

-	if (--ftdipp->iq_counter == 0) {

-		_submitInI(ftdipp);

-		chSchRescheduleS();

-	}

-	chSysUnlock();

-

-	return (msg_t)b;

-}

-

-static msg_t _get(USBHFTDIPortDriver *ftdipp) {

-	return _get_timeout(ftdipp, TIME_INFINITE);

-}

-

-static size_t _read(USBHFTDIPortDriver *ftdipp, uint8_t *bp, size_t n) {

-	return _read_timeout(ftdipp, bp, n, TIME_INFINITE);

-}

-

-static void _vt(void *p) {

-	USBHFTDIPortDriver *const ftdipp = (USBHFTDIPortDriver *)p;

-	chSysLockFromISR();

-	uint32_t len = ftdipp->oq_ptr - ftdipp->oq_buff;

-	if (len && !usbhURBIsBusy(&ftdipp->oq_urb)) {

-		_submitOutI(ftdipp, len);

-	}

-	if ((ftdipp->iq_counter == 0) && !usbhURBIsBusy(&ftdipp->iq_urb)) {

-		_submitInI(ftdipp);

-	}

-	chVTSetI(&ftdipp->vt, MS2ST(16), _vt, ftdipp);

-	chSysUnlockFromISR();

-}

-

-static const struct FTDIPortDriverVMT async_channel_vmt = {

-	(size_t (*)(void *, const uint8_t *, size_t))_write,

-	(size_t (*)(void *, uint8_t *, size_t))_read,

-	(msg_t (*)(void *, uint8_t))_put,

-	(msg_t (*)(void *))_get,

-	(msg_t (*)(void *, uint8_t, systime_t))_put_timeout,

-	(msg_t (*)(void *, systime_t))_get_timeout,

-	(size_t (*)(void *, const uint8_t *, size_t, systime_t))_write_timeout,

-	(size_t (*)(void *, uint8_t *, size_t, systime_t))_read_timeout

-};

-

-

-static void _stop(USBHFTDIPortDriver *ftdipp) {

-	osalSysLock();

-	chVTResetI(&ftdipp->vt);

-	usbhEPCloseS(&ftdipp->epin);

-	usbhEPCloseS(&ftdipp->epout);

-	chThdDequeueAllI(&ftdipp->iq_waiting, Q_RESET);

-	chThdDequeueAllI(&ftdipp->oq_waiting, Q_RESET);

-	osalOsRescheduleS();

-	ftdipp->state = USBHFTDIP_STATE_ACTIVE;

-	osalSysUnlock();

-}

-

-void usbhftdipStop(USBHFTDIPortDriver *ftdipp) {

-	osalDbgCheck((ftdipp->state == USBHFTDIP_STATE_ACTIVE)

-			|| (ftdipp->state == USBHFTDIP_STATE_READY));

-

-	if (ftdipp->state == USBHFTDIP_STATE_ACTIVE) {

-		return;

-	}

-

-	osalMutexLock(&ftdipp->ftdip->mtx);

-	_stop(ftdipp);

-	osalMutexUnlock(&ftdipp->ftdip->mtx);

-}

-

-void usbhftdipStart(USBHFTDIPortDriver *ftdipp, const USBHFTDIPortConfig *config) {

-	static const USBHFTDIPortConfig default_config = {

-		HAL_USBHFTDI_DEFAULT_SPEED,

-		HAL_USBHFTDI_DEFAULT_FRAMING,

-		HAL_USBHFTDI_DEFAULT_HANDSHAKE,

-		HAL_USBHFTDI_DEFAULT_XON,

-		HAL_USBHFTDI_DEFAULT_XOFF

-	};

-

-	osalDbgCheck((ftdipp->state == USBHFTDIP_STATE_ACTIVE)

-			|| (ftdipp->state == USBHFTDIP_STATE_READY));

-

-	if (ftdipp->state == USBHFTDIP_STATE_READY)

-		return;

-

-	osalMutexLock(&ftdipp->ftdip->mtx);

-	if (config == NULL)

-		config = &default_config;

-

-	uint16_t wValue = 0;

-	_ftdi_port_control(ftdipp, FTDI_COMMAND_RESET, FTDI_RESET_ALL, 0, 0, NULL);

-	_set_baudrate(ftdipp, config->speed);

-	_ftdi_port_control(ftdipp, FTDI_COMMAND_SETDATA, config->framing, 0, 0, NULL);

-	if (config->handshake & USBHFTDI_HANDSHAKE_XON_XOFF)

-		wValue = (config->xoff_character << 8) | config->xon_character;

-	_ftdi_port_control(ftdipp, FTDI_COMMAND_SETFLOW, wValue, config->handshake, 0, NULL);

-

-	usbhURBObjectInit(&ftdipp->oq_urb, &ftdipp->epout, _out_cb, ftdipp, ftdipp->oq_buff, 0);

-	chThdQueueObjectInit(&ftdipp->oq_waiting);

-	ftdipp->oq_counter = 64;

-	ftdipp->oq_ptr = ftdipp->oq_buff;

-	usbhEPOpen(&ftdipp->epout);

-

-	usbhURBObjectInit(&ftdipp->iq_urb, &ftdipp->epin, _in_cb, ftdipp, ftdipp->iq_buff, 64);

-	chThdQueueObjectInit(&ftdipp->iq_waiting);

-	ftdipp->iq_counter = 0;

-	ftdipp->iq_ptr = ftdipp->iq_buff;

-	usbhEPOpen(&ftdipp->epin);

-	osalSysLock();

-	usbhURBSubmitI(&ftdipp->iq_urb);

-	osalSysUnlock();

-

-	chVTObjectInit(&ftdipp->vt);

-	chVTSet(&ftdipp->vt, MS2ST(16), _vt, ftdipp);

-

-	ftdipp->state = USBHFTDIP_STATE_READY;

-	osalMutexUnlock(&ftdipp->ftdip->mtx);

-}

-

-void usbhftdiObjectInit(USBHFTDIDriver *ftdip) {

-	osalDbgCheck(ftdip != NULL);

-	memset(ftdip, 0, sizeof(*ftdip));

-	ftdip->info = &usbhftdiClassDriverInfo;

-	osalMutexObjectInit(&ftdip->mtx);

-}

-

-void usbhftdipObjectInit(USBHFTDIPortDriver *ftdipp) {

-	osalDbgCheck(ftdipp != NULL);

-	memset(ftdipp, 0, sizeof(*ftdipp));

-	ftdipp->vmt = &async_channel_vmt;

-	ftdipp->state = USBHFTDIP_STATE_STOP;

-}

-

-#endif

+/*
+    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio
+              Copyright (C) 2015 Diego Ismirlian, TISA, (dismirlian (at) google's mail)
+
+    Licensed under the Apache License, Version 2.0 (the "License");
+    you may not use this file except in compliance with the License.
+    You may obtain a copy of the License at
+
+        http://www.apache.org/licenses/LICENSE-2.0
+
+    Unless required by applicable law or agreed to in writing, software
+    distributed under the License is distributed on an "AS IS" BASIS,
+    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+    See the License for the specific language governing permissions and
+    limitations under the License.
+*/
+
+#include "hal.h"
+#include "hal_usbh.h"
+
+#if HAL_USBH_USE_FTDI
+
+#if !HAL_USE_USBH
+#error "USBHFTDI needs USBH"
+#endif
+
+#include <string.h>
+#include "usbh/dev/ftdi.h"
+#include "usbh/internal.h"
+
+//#pragma GCC optimize("Og")
+
+
+#if USBHFTDI_DEBUG_ENABLE_TRACE
+#define udbgf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)
+#define udbg(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)
+#else
+#define udbgf(f, ...)  do {} while(0)
+#define udbg(f, ...)   do {} while(0)
+#endif
+
+#if USBHFTDI_DEBUG_ENABLE_INFO
+#define uinfof(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)
+#define uinfo(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)
+#else
+#define uinfof(f, ...)  do {} while(0)
+#define uinfo(f, ...)   do {} while(0)
+#endif
+
+#if USBHFTDI_DEBUG_ENABLE_WARNINGS
+#define uwarnf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)
+#define uwarn(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)
+#else
+#define uwarnf(f, ...)  do {} while(0)
+#define uwarn(f, ...)   do {} while(0)
+#endif
+
+#if USBHFTDI_DEBUG_ENABLE_ERRORS
+#define uerrf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)
+#define uerr(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)
+#else
+#define uerrf(f, ...)  do {} while(0)
+#define uerr(f, ...)   do {} while(0)
+#endif
+
+
+/*===========================================================================*/
+/* USB Class driver loader for FTDI								 		 	 */
+/*===========================================================================*/
+USBHFTDIDriver USBHFTDID[HAL_USBHFTDI_MAX_INSTANCES];
+
+static usbh_baseclassdriver_t *_ftdi_load(usbh_device_t *dev, const uint8_t *descriptor, uint16_t rem);
+static void _ftdi_unload(usbh_baseclassdriver_t *drv);
+
+static const usbh_classdriver_vmt_t class_driver_vmt = {
+	_ftdi_load,
+	_ftdi_unload
+};
+
+const usbh_classdriverinfo_t usbhftdiClassDriverInfo = {
+	0xff, 0xff, 0xff, "FTDI", &class_driver_vmt
+};
+
+static USBHFTDIPortDriver *_find_port(void) {
+	uint8_t i;
+	for (i = 0; i < HAL_USBHFTDI_MAX_PORTS; i++) {
+		if (FTDIPD[i].ftdip == NULL)
+			return &FTDIPD[i];
+	}
+	return NULL;
+}
+
+static usbh_baseclassdriver_t *_ftdi_load(usbh_device_t *dev, const uint8_t *descriptor, uint16_t rem) {
+	int i;
+	USBHFTDIDriver *ftdip;
+
+	if (dev->devDesc.idVendor != 0x0403) {
+		uerr("FTDI: Unrecognized VID");
+		return NULL;
+	}
+
+	switch (dev->devDesc.idProduct) {
+	case 0x6001:
+	case 0x6010:
+	case 0x6011:
+	case 0x6014:
+	case 0x6015:
+		break;
+	default:
+		uerr("FTDI: Unrecognized PID");
+		return NULL;
+	}
+
+	if ((rem < descriptor[0]) || (descriptor[1] != USBH_DT_INTERFACE))
+		return NULL;
+
+	const usbh_interface_descriptor_t * const ifdesc = (const usbh_interface_descriptor_t * const)descriptor;
+	if (ifdesc->bInterfaceNumber != 0) {
+		uwarn("FTDI: Will allocate driver along with IF #0");
+	}
+
+	/* alloc driver */
+	for (i = 0; i < HAL_USBHFTDI_MAX_INSTANCES; i++) {
+		if (USBHFTDID[i].dev == NULL) {
+			ftdip = &USBHFTDID[i];
+			goto alloc_ok;
+		}
+	}
+
+	uwarn("FTDI: Can't alloc driver");
+
+	/* can't alloc */
+	return NULL;
+
+alloc_ok:
+	/* initialize the driver's variables */
+	ftdip->ports = 0;
+	switch (dev->devDesc.bcdDevice) {
+	case 0x200:		//AM
+		uinfo("FTDI: Type A chip");
+		ftdip->type = USBHFTDI_TYPE_A;
+		break;
+	case 0x400:		//BM
+	case 0x500:		//2232C
+	case 0x600:		//R
+	case 0x1000:	//230X
+		uinfo("FTDI: Type B chip");
+		ftdip->type = USBHFTDI_TYPE_B;
+		break;
+	case 0x700:		//2232H;
+	case 0x800:		//4232H;
+	case 0x900:		//232H;
+		uinfo("FTDI: Type H chip");
+		ftdip->type = USBHFTDI_TYPE_H;
+	default:
+		uerr("FTDI: Unrecognized chip type");
+		return NULL;
+	}
+	usbhEPSetName(&dev->ctrl, "FTD[CTRL]");
+
+	/* parse the configuration descriptor */
+	generic_iterator_t iep, icfg;
+	if_iterator_t iif;
+	cfg_iter_init(&icfg, dev->fullConfigurationDescriptor, dev->basicConfigDesc.wTotalLength);
+	for (if_iter_init(&iif, &icfg); iif.valid; if_iter_next(&iif)) {
+		const usbh_interface_descriptor_t *const ifdesc = if_get(&iif);
+		uinfof("FTDI: Interface #%d", ifdesc->bInterfaceNumber);
+
+		USBHFTDIPortDriver *const prt = _find_port();
+		if (prt == NULL) {
+			uwarn("\tCan't alloc port for this interface");
+			break;
+		}
+
+		prt->ifnum = ifdesc->bInterfaceNumber;
+		prt->epin.status = USBH_EPSTATUS_UNINITIALIZED;
+		prt->epout.status = USBH_EPSTATUS_UNINITIALIZED;
+
+		for (ep_iter_init(&iep, &iif); iep.valid; ep_iter_next(&iep)) {
+			const usbh_endpoint_descriptor_t *const epdesc = ep_get(&iep);
+			if ((epdesc->bEndpointAddress & 0x80) && (epdesc->bmAttributes == USBH_EPTYPE_BULK)) {
+				uinfof("BULK IN endpoint found: bEndpointAddress=%02x", epdesc->bEndpointAddress);
+				usbhEPObjectInit(&prt->epin, dev, epdesc);
+				usbhEPSetName(&prt->epin, "FTD[BIN ]");
+			} else if (((epdesc->bEndpointAddress & 0x80) == 0)
+					&& (epdesc->bmAttributes == USBH_EPTYPE_BULK)) {
+				uinfof("BULK OUT endpoint found: bEndpointAddress=%02x", epdesc->bEndpointAddress);
+				usbhEPObjectInit(&prt->epout, dev, epdesc);
+				usbhEPSetName(&prt->epout, "FTD[BOUT]");
+			} else {
+				uinfof("unsupported endpoint found: bEndpointAddress=%02x, bmAttributes=%02x",
+						epdesc->bEndpointAddress, epdesc->bmAttributes);
+			}
+		}
+
+		if ((prt->epin.status != USBH_EPSTATUS_CLOSED)
+				|| (prt->epout.status != USBH_EPSTATUS_CLOSED)) {
+			uwarn("\tCouldn't find endpoints; can't alloc port for this interface");
+			continue;
+		}
+
+		/* link the new block driver to the list */
+		prt->next = ftdip->ports;
+		ftdip->ports = prt;
+		prt->ftdip = ftdip;
+
+		prt->state = USBHFTDIP_STATE_ACTIVE;
+	}
+
+	return (usbh_baseclassdriver_t *)ftdip;
+
+}
+
+static void _stop(USBHFTDIPortDriver *ftdipp);
+static void _ftdi_unload(usbh_baseclassdriver_t *drv) {
+	osalDbgCheck(drv != NULL);
+	USBHFTDIDriver *const ftdip = (USBHFTDIDriver *)drv;
+	USBHFTDIPortDriver *ftdipp = ftdip->ports;
+
+	osalMutexLock(&ftdip->mtx);
+	while (ftdipp) {
+		_stop(ftdipp);
+		ftdipp = ftdipp->next;
+	}
+
+	ftdipp = ftdip->ports;
+	osalSysLock();
+	while (ftdipp) {
+		USBHFTDIPortDriver *next = ftdipp->next;
+		usbhftdipObjectInit(ftdipp);
+		ftdipp = next;
+	}
+	osalSysUnlock();
+	osalMutexUnlock(&ftdip->mtx);
+}
+
+
+USBHFTDIPortDriver FTDIPD[HAL_USBHFTDI_MAX_PORTS];
+
+
+#define FTDI_COMMAND_RESET		0
+#define FTDI_RESET_ALL 			0
+#define FTDI_RESET_PURGE_RX 	1
+#define FTDI_RESET_PURGE_TX 	2
+
+#define FTDI_COMMAND_SETFLOW    2
+
+#define FTDI_COMMAND_SETBAUD    3
+
+#define FTDI_COMMAND_SETDATA    4
+#define FTDI_SETDATA_BREAK      (0x1 << 14)
+
+#if 0
+#define FTDI_COMMAND_MODEMCTRL  	1
+#define FTDI_COMMAND_GETMODEMSTATUS       5 /* Retrieve current value of modem status register */
+#define FTDI_COMMAND_SETEVENTCHAR         6 /* Set the event character */
+#define FTDI_COMMAND_SETERRORCHAR         7 /* Set the error character */
+#define FTDI_COMMAND_SETLATENCYTIMER      9 /* Set the latency timer */
+#define FTDI_COMMAND_GETLATENCYTIMER      10 /* Get the latency timer */
+#endif
+
+/*
+ * DATA FORMAT
+ *
+ * IN Endpoint
+ *
+ * The device reserves the first two bytes of data on this endpoint to contain
+ * the current values of the modem and line status registers. In the absence of
+ * data, the device generates a message consisting of these two status bytes
+ * every 40 ms
+ *
+ * Byte 0: Modem Status
+ *
+ * Offset       Description
+ * B0   Reserved - must be 1
+ * B1   Reserved - must be 0
+ * B2   Reserved - must be 0
+ * B3   Reserved - must be 0
+ * B4   Clear to Send (CTS)
+ * B5   Data Set Ready (DSR)
+ * B6   Ring Indicator (RI)
+ * B7   Receive Line Signal Detect (RLSD)
+ *
+ * Byte 1: Line Status
+ *
+ * Offset       Description
+ * B0   Data Ready (DR)
+ * B1   Overrun Error (OE)
+ * B2   Parity Error (PE)
+ * B3   Framing Error (FE)
+ * B4   Break Interrupt (BI)
+ * B5   Transmitter Holding Register (THRE)
+ * B6   Transmitter Empty (TEMT)
+ * B7   Error in RCVR FIFO
+ *
+ */
+#define FTDI_RS0_CTS    (1 << 4)
+#define FTDI_RS0_DSR    (1 << 5)
+#define FTDI_RS0_RI     (1 << 6)
+#define FTDI_RS0_RLSD   (1 << 7)
+
+#define FTDI_RS_DR      1
+#define FTDI_RS_OE      (1<<1)
+#define FTDI_RS_PE      (1<<2)
+#define FTDI_RS_FE      (1<<3)
+#define FTDI_RS_BI      (1<<4)
+#define FTDI_RS_THRE    (1<<5)
+#define FTDI_RS_TEMT    (1<<6)
+#define FTDI_RS_FIFO    (1<<7)
+
+
+static usbh_urbstatus_t _ftdi_port_control(USBHFTDIPortDriver *ftdipp,
+		uint8_t bRequest, uint8_t wValue, uint8_t bHIndex, uint16_t wLength,
+		uint8_t *buff) {
+
+	static const uint8_t bmRequestType[] = {
+		USBH_REQTYPE_VENDOR | USBH_REQTYPE_OUT | USBH_REQTYPE_DEVICE, //0 FTDI_COMMAND_RESET
+		USBH_REQTYPE_VENDOR | USBH_REQTYPE_OUT | USBH_REQTYPE_DEVICE, //1 FTDI_COMMAND_MODEMCTRL
+		USBH_REQTYPE_VENDOR | USBH_REQTYPE_OUT | USBH_REQTYPE_DEVICE, //2 FTDI_COMMAND_SETFLOW
+		USBH_REQTYPE_VENDOR | USBH_REQTYPE_OUT | USBH_REQTYPE_DEVICE, //3 FTDI_COMMAND_SETBAUD
+		USBH_REQTYPE_VENDOR | USBH_REQTYPE_OUT | USBH_REQTYPE_DEVICE, //4 FTDI_COMMAND_SETDATA
+	};
+
+	osalDbgCheck(bRequest < sizeof_array(bmRequestType));
+	osalDbgCheck(bRequest != 1);
+
+	const USBH_DEFINE_BUFFER(usbh_control_request_t, req) = {
+			bmRequestType[bRequest],
+			bRequest,
+			wValue,
+			(bHIndex << 8) | (ftdipp->ifnum + 1),
+			wLength
+	};
+
+	return usbhControlRequestExtended(ftdipp->ftdip->dev, &req, buff, NULL, MS2ST(1000));
+}
+
+static uint32_t _get_divisor(uint32_t baud, usbhftdi_type_t type) {
+	static const uint8_t divfrac[8] = {0, 3, 2, 4, 1, 5, 6, 7};
+	uint32_t divisor;
+
+	if (type == USBHFTDI_TYPE_A) {
+		uint32_t divisor3 = ((48000000UL / 2) + baud / 2) / baud;
+		uinfof("FTDI: desired=%dbps, real=%dbps", baud, (48000000UL / 2) / divisor3);
+		if ((divisor3 & 0x7) == 7)
+			divisor3++; /* round x.7/8 up to x+1 */
+
+		divisor = divisor3 >> 3;
+	    divisor3 &= 0x7;
+		if (divisor3 == 1)
+			divisor |= 0xc000;
+		else if (divisor3 >= 4)
+			divisor |= 0x4000;
+		else if (divisor3 != 0)
+			divisor |= 0x8000;
+		else if (divisor == 1)
+			divisor = 0;    /* special case for maximum baud rate */
+	} else {
+		if (type == USBHFTDI_TYPE_B) {
+			divisor = ((48000000UL / 2) + baud / 2) / baud;
+			uinfof("FTDI: desired=%dbps, real=%dbps", baud, (48000000UL / 2) / divisor);
+		} else {
+			/* hi-speed baud rate is 10-bit sampling instead of 16-bit */
+			if (baud < 1200)
+				baud = 1200;
+			divisor = (120000000UL * 8 + baud * 5) / (baud * 10);
+			uinfof("FTDI: desired=%dbps, real=%dbps", baud, (120000000UL * 8) / divisor / 10);
+		}
+		divisor = (divisor >> 3) | (divfrac[divisor & 0x7] << 14);
+
+		/* Deal with special cases for highest baud rates. */
+		if (divisor == 1)
+			divisor = 0;
+		else if (divisor == 0x4001)
+			divisor = 1;
+
+		if (type == USBHFTDI_TYPE_H)
+			divisor |= 0x00020000;
+	}
+	return divisor;
+}
+
+static usbh_urbstatus_t _set_baudrate(USBHFTDIPortDriver *ftdipp, uint32_t baudrate) {
+	uint32_t divisor = _get_divisor(baudrate, ftdipp->ftdip->type);
+	uint16_t wValue = (uint16_t)divisor;
+	uint16_t wIndex = (uint16_t)(divisor >> 16);
+	if (ftdipp->ftdip->dev->basicConfigDesc.bNumInterfaces > 1)
+		wIndex = (wIndex << 8) | (ftdipp->ifnum + 1);
+
+	const USBH_DEFINE_BUFFER(usbh_control_request_t, req) = {
+		USBH_REQTYPE_VENDOR | USBH_REQTYPE_OUT | USBH_REQTYPE_DEVICE,
+		FTDI_COMMAND_SETBAUD,
+		wValue,
+		wIndex,
+		0
+	};
+	return usbhControlRequestExtended(ftdipp->ftdip->dev, &req, NULL, NULL, MS2ST(1000));
+}
+
+
+static void _submitOutI(USBHFTDIPortDriver *ftdipp, uint32_t len) {
+	udbgf("FTDI: Submit OUT %d", len);
+	ftdipp->oq_urb.requestedLength = len;
+	usbhURBObjectResetI(&ftdipp->oq_urb);
+	usbhURBSubmitI(&ftdipp->oq_urb);
+}
+
+static void _out_cb(usbh_urb_t *urb) {
+	USBHFTDIPortDriver *const ftdipp = (USBHFTDIPortDriver *)urb->userData;
+	switch (urb->status) {
+	case USBH_URBSTATUS_OK:
+		ftdipp->oq_ptr = ftdipp->oq_buff;
+		ftdipp->oq_counter = 64;
+		chThdDequeueNextI(&ftdipp->oq_waiting, Q_OK);
+		return;
+	case USBH_URBSTATUS_DISCONNECTED:
+		uwarn("FTDI: URB OUT disconnected");
+		chThdDequeueNextI(&ftdipp->oq_waiting, Q_RESET);
+		return;
+	default:
+		uerrf("FTDI: URB OUT status unexpected = %d", urb->status);
+		break;
+	}
+	usbhURBObjectResetI(&ftdipp->oq_urb);
+	usbhURBSubmitI(&ftdipp->oq_urb);
+}
+
+static size_t _write_timeout(USBHFTDIPortDriver *ftdipp, const uint8_t *bp,
+		size_t n, systime_t timeout) {
+	chDbgCheck(n > 0U);
+
+	size_t w = 0;
+	chSysLock();
+	while (true) {
+		if (ftdipp->state != USBHFTDIP_STATE_READY) {
+			chSysUnlock();
+			return w;
+		}
+		while (usbhURBIsBusy(&ftdipp->oq_urb)) {
+			if (chThdEnqueueTimeoutS(&ftdipp->oq_waiting, timeout) != Q_OK) {
+				chSysUnlock();
+				return w;
+			}
+		}
+
+		*ftdipp->oq_ptr++ = *bp++;
+		if (--ftdipp->oq_counter == 0) {
+			_submitOutI(ftdipp, 64);
+			chSchRescheduleS();
+		}
+		chSysUnlock(); /* Gives a preemption chance in a controlled point.*/
+
+		w++;
+		if (--n == 0U)
+			return w;
+
+		chSysLock();
+	}
+}
+
+static msg_t _put_timeout(USBHFTDIPortDriver *ftdipp, uint8_t b, systime_t timeout) {
+
+	chSysLock();
+	if (ftdipp->state != USBHFTDIP_STATE_READY) {
+		chSysUnlock();
+		return Q_RESET;
+	}
+
+	while (usbhURBIsBusy(&ftdipp->oq_urb)) {
+		msg_t msg = chThdEnqueueTimeoutS(&ftdipp->oq_waiting, timeout);
+		if (msg < Q_OK) {
+			chSysUnlock();
+			return msg;
+		}
+	}
+
+	*ftdipp->oq_ptr++ = b;
+	if (--ftdipp->oq_counter == 0) {
+		_submitOutI(ftdipp, 64);
+		chSchRescheduleS();
+	}
+	chSysUnlock();
+	return Q_OK;
+}
+
+static size_t _write(USBHFTDIPortDriver *ftdipp, const uint8_t *bp, size_t n) {
+	return _write_timeout(ftdipp, bp, n, TIME_INFINITE);
+}
+
+static msg_t _put(USBHFTDIPortDriver *ftdipp, uint8_t b) {
+	return _put_timeout(ftdipp, b, TIME_INFINITE);
+}
+
+static void _submitInI(USBHFTDIPortDriver *ftdipp) {
+	udbg("FTDI: Submit IN");
+	usbhURBObjectResetI(&ftdipp->iq_urb);
+	usbhURBSubmitI(&ftdipp->iq_urb);
+}
+
+static void _in_cb(usbh_urb_t *urb) {
+	USBHFTDIPortDriver *const ftdipp = (USBHFTDIPortDriver *)urb->userData;
+	switch (urb->status) {
+	case USBH_URBSTATUS_OK:
+		if (urb->actualLength < 2) {
+			uwarnf("FTDI: URB IN actualLength = %d, < 2", urb->actualLength);
+		} else if (urb->actualLength > 2) {
+			udbgf("FTDI: URB IN data len=%d, status=%02x %02x",
+					urb->actualLength - 2,
+					((uint8_t *)urb->buff)[0],
+					((uint8_t *)urb->buff)[1]);
+			ftdipp->iq_ptr = ftdipp->iq_buff + 2;
+			ftdipp->iq_counter = urb->actualLength - 2;
+			chThdDequeueNextI(&ftdipp->iq_waiting, Q_OK);
+			return;
+		} else {
+			udbgf("FTDI: URB IN no data, status=%02x %02x",
+					((uint8_t *)urb->buff)[0],
+					((uint8_t *)urb->buff)[1]);
+			return;
+		}
+		break;
+	case USBH_URBSTATUS_DISCONNECTED:
+		uwarn("FTDI: URB IN disconnected");
+		chThdDequeueNextI(&ftdipp->iq_waiting, Q_RESET);
+		return;
+	default:
+		uerrf("FTDI: URB IN status unexpected = %d", urb->status);
+		break;
+	}
+	_submitInI(ftdipp);
+}
+
+static size_t _read_timeout(USBHFTDIPortDriver *ftdipp, uint8_t *bp,
+		size_t n, systime_t timeout) {
+	size_t r = 0;
+
+	chDbgCheck(n > 0U);
+
+	chSysLock();
+	while (true) {
+		if (ftdipp->state != USBHFTDIP_STATE_READY) {
+			chSysUnlock();
+			return r;
+		}
+		while (ftdipp->iq_counter == 0) {
+			if (!usbhURBIsBusy(&ftdipp->iq_urb))
+				_submitInI(ftdipp);
+			if (chThdEnqueueTimeoutS(&ftdipp->iq_waiting, timeout) != Q_OK) {
+				chSysUnlock();
+				return r;
+			}
+		}
+		*bp++ = *ftdipp->iq_ptr++;
+		if (--ftdipp->iq_counter == 0) {
+			_submitInI(ftdipp);
+			chSchRescheduleS();
+		}
+		chSysUnlock();
+
+		r++;
+		if (--n == 0U)
+			return r;
+
+		chSysLock();
+	}
+}
+
+static msg_t _get_timeout(USBHFTDIPortDriver *ftdipp, systime_t timeout) {
+	uint8_t b;
+
+	chSysLock();
+	if (ftdipp->state != USBHFTDIP_STATE_READY) {
+		chSysUnlock();
+		return Q_RESET;
+	}
+	while (ftdipp->iq_counter == 0) {
+		if (!usbhURBIsBusy(&ftdipp->iq_urb))
+			_submitInI(ftdipp);
+		msg_t msg = chThdEnqueueTimeoutS(&ftdipp->iq_waiting, timeout);
+		if (msg < Q_OK) {
+			chSysUnlock();
+			return msg;
+		}
+	}
+	b = *ftdipp->iq_ptr++;
+	if (--ftdipp->iq_counter == 0) {
+		_submitInI(ftdipp);
+		chSchRescheduleS();
+	}
+	chSysUnlock();
+
+	return (msg_t)b;
+}
+
+static msg_t _get(USBHFTDIPortDriver *ftdipp) {
+	return _get_timeout(ftdipp, TIME_INFINITE);
+}
+
+static size_t _read(USBHFTDIPortDriver *ftdipp, uint8_t *bp, size_t n) {
+	return _read_timeout(ftdipp, bp, n, TIME_INFINITE);
+}
+
+static void _vt(void *p) {
+	USBHFTDIPortDriver *const ftdipp = (USBHFTDIPortDriver *)p;
+	chSysLockFromISR();
+	uint32_t len = ftdipp->oq_ptr - ftdipp->oq_buff;
+	if (len && !usbhURBIsBusy(&ftdipp->oq_urb)) {
+		_submitOutI(ftdipp, len);
+	}
+	if ((ftdipp->iq_counter == 0) && !usbhURBIsBusy(&ftdipp->iq_urb)) {
+		_submitInI(ftdipp);
+	}
+	chVTSetI(&ftdipp->vt, MS2ST(16), _vt, ftdipp);
+	chSysUnlockFromISR();
+}
+
+static const struct FTDIPortDriverVMT async_channel_vmt = {
+	(size_t (*)(void *, const uint8_t *, size_t))_write,
+	(size_t (*)(void *, uint8_t *, size_t))_read,
+	(msg_t (*)(void *, uint8_t))_put,
+	(msg_t (*)(void *))_get,
+	(msg_t (*)(void *, uint8_t, systime_t))_put_timeout,
+	(msg_t (*)(void *, systime_t))_get_timeout,
+	(size_t (*)(void *, const uint8_t *, size_t, systime_t))_write_timeout,
+	(size_t (*)(void *, uint8_t *, size_t, systime_t))_read_timeout
+};
+
+
+static void _stop(USBHFTDIPortDriver *ftdipp) {
+	osalSysLock();
+	chVTResetI(&ftdipp->vt);
+	usbhEPCloseS(&ftdipp->epin);
+	usbhEPCloseS(&ftdipp->epout);
+	chThdDequeueAllI(&ftdipp->iq_waiting, Q_RESET);
+	chThdDequeueAllI(&ftdipp->oq_waiting, Q_RESET);
+	osalOsRescheduleS();
+	ftdipp->state = USBHFTDIP_STATE_ACTIVE;
+	osalSysUnlock();
+}
+
+void usbhftdipStop(USBHFTDIPortDriver *ftdipp) {
+	osalDbgCheck((ftdipp->state == USBHFTDIP_STATE_ACTIVE)
+			|| (ftdipp->state == USBHFTDIP_STATE_READY));
+
+	if (ftdipp->state == USBHFTDIP_STATE_ACTIVE) {
+		return;
+	}
+
+	osalMutexLock(&ftdipp->ftdip->mtx);
+	_stop(ftdipp);
+	osalMutexUnlock(&ftdipp->ftdip->mtx);
+}
+
+void usbhftdipStart(USBHFTDIPortDriver *ftdipp, const USBHFTDIPortConfig *config) {
+	static const USBHFTDIPortConfig default_config = {
+		HAL_USBHFTDI_DEFAULT_SPEED,
+		HAL_USBHFTDI_DEFAULT_FRAMING,
+		HAL_USBHFTDI_DEFAULT_HANDSHAKE,
+		HAL_USBHFTDI_DEFAULT_XON,
+		HAL_USBHFTDI_DEFAULT_XOFF
+	};
+
+	osalDbgCheck((ftdipp->state == USBHFTDIP_STATE_ACTIVE)
+			|| (ftdipp->state == USBHFTDIP_STATE_READY));
+
+	if (ftdipp->state == USBHFTDIP_STATE_READY)
+		return;
+
+	osalMutexLock(&ftdipp->ftdip->mtx);
+	if (config == NULL)
+		config = &default_config;
+
+	uint16_t wValue = 0;
+	_ftdi_port_control(ftdipp, FTDI_COMMAND_RESET, FTDI_RESET_ALL, 0, 0, NULL);
+	_set_baudrate(ftdipp, config->speed);
+	_ftdi_port_control(ftdipp, FTDI_COMMAND_SETDATA, config->framing, 0, 0, NULL);
+	if (config->handshake & USBHFTDI_HANDSHAKE_XON_XOFF)
+		wValue = (config->xoff_character << 8) | config->xon_character;
+	_ftdi_port_control(ftdipp, FTDI_COMMAND_SETFLOW, wValue, config->handshake, 0, NULL);
+
+	usbhURBObjectInit(&ftdipp->oq_urb, &ftdipp->epout, _out_cb, ftdipp, ftdipp->oq_buff, 0);
+	chThdQueueObjectInit(&ftdipp->oq_waiting);
+	ftdipp->oq_counter = 64;
+	ftdipp->oq_ptr = ftdipp->oq_buff;
+	usbhEPOpen(&ftdipp->epout);
+
+	usbhURBObjectInit(&ftdipp->iq_urb, &ftdipp->epin, _in_cb, ftdipp, ftdipp->iq_buff, 64);
+	chThdQueueObjectInit(&ftdipp->iq_waiting);
+	ftdipp->iq_counter = 0;
+	ftdipp->iq_ptr = ftdipp->iq_buff;
+	usbhEPOpen(&ftdipp->epin);
+	osalSysLock();
+	usbhURBSubmitI(&ftdipp->iq_urb);
+	osalSysUnlock();
+
+	chVTObjectInit(&ftdipp->vt);
+	chVTSet(&ftdipp->vt, MS2ST(16), _vt, ftdipp);
+
+	ftdipp->state = USBHFTDIP_STATE_READY;
+	osalMutexUnlock(&ftdipp->ftdip->mtx);
+}
+
+void usbhftdiObjectInit(USBHFTDIDriver *ftdip) {
+	osalDbgCheck(ftdip != NULL);
+	memset(ftdip, 0, sizeof(*ftdip));
+	ftdip->info = &usbhftdiClassDriverInfo;
+	osalMutexObjectInit(&ftdip->mtx);
+}
+
+void usbhftdipObjectInit(USBHFTDIPortDriver *ftdipp) {
+	osalDbgCheck(ftdipp != NULL);
+	memset(ftdipp, 0, sizeof(*ftdipp));
+	ftdipp->vmt = &async_channel_vmt;
+	ftdipp->state = USBHFTDIP_STATE_STOP;
+}
+
+#endif
diff --git a/os/hal/src/usbh/usbh_hub.c b/os/hal/src/usbh/hal_usbh_hub.c
index eb585d2..7fdcef1 100644
--- a/os/hal/src/usbh/usbh_hub.c
+++ b/os/hal/src/usbh/hal_usbh_hub.c
@@ -1,302 +1,302 @@
-/*

-    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio

-              Copyright (C) 2015 Diego Ismirlian, TISA, (dismirlian (at) google's mail)

-

-    Licensed under the Apache License, Version 2.0 (the "License");

-    you may not use this file except in compliance with the License.

-    You may obtain a copy of the License at

-

-        http://www.apache.org/licenses/LICENSE-2.0

-

-    Unless required by applicable law or agreed to in writing, software

-    distributed under the License is distributed on an "AS IS" BASIS,

-    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.

-    See the License for the specific language governing permissions and

-    limitations under the License.

-*/

-

-#include "hal.h"

-#include "usbh.h"

-#include "usbh/internal.h"

-

-#if HAL_USBH_USE_HUB

-

-#if !HAL_USE_USBH

-#error "USBHHUB needs HAL_USE_USBH"

-#endif

-

-#include <string.h>

-#include "usbh/dev/hub.h"

-

-#if USBHHUB_DEBUG_ENABLE_TRACE

-#define udbgf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)

-#define udbg(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)

-#else

-#define udbgf(f, ...)  do {} while(0)

-#define udbg(f, ...)   do {} while(0)

-#endif

-

-#if USBHHUB_DEBUG_ENABLE_INFO

-#define uinfof(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)

-#define uinfo(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)

-#else

-#define uinfof(f, ...)  do {} while(0)

-#define uinfo(f, ...)   do {} while(0)

-#endif

-

-#if USBHHUB_DEBUG_ENABLE_WARNINGS

-#define uwarnf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)

-#define uwarn(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)

-#else

-#define uwarnf(f, ...)  do {} while(0)

-#define uwarn(f, ...)   do {} while(0)

-#endif

-

-#if USBHHUB_DEBUG_ENABLE_ERRORS

-#define uerrf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)

-#define uerr(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)

-#else

-#define uerrf(f, ...)  do {} while(0)

-#define uerr(f, ...)   do {} while(0)

-#endif

-

-

-USBHHubDriver USBHHUBD[HAL_USBHHUB_MAX_INSTANCES];

-usbh_port_t USBHPorts[HAL_USBHHUB_MAX_PORTS];

-

-static usbh_baseclassdriver_t *hub_load(usbh_device_t *dev, const uint8_t *descriptor, uint16_t rem);

-static void hub_unload(usbh_baseclassdriver_t *drv);

-static const usbh_classdriver_vmt_t usbhhubClassDriverVMT = {

-	hub_load,

-	hub_unload

-};

-const usbh_classdriverinfo_t usbhhubClassDriverInfo = {

-	0x09, 0x00, -1, "HUB", &usbhhubClassDriverVMT

-};

-

-

-void _usbhub_port_object_init(usbh_port_t *port, USBHDriver *usbh,

-		USBHHubDriver *hub, uint8_t number) {

-	memset(port, 0, sizeof(*port));

-	port->number = number;

-	port->device.host = usbh;

-	port->hub = hub;

-}

-

-usbh_urbstatus_t usbhhubControlRequest(USBHDriver *host, USBHHubDriver *hub,

-											uint8_t bmRequestType,

-											uint8_t bRequest,

-											uint16_t wValue,

-											uint16_t wIndex,

-											uint16_t wLength,

-											uint8_t *buf) {

-	if (hub == NULL)

-		return usbh_lld_root_hub_request(host, bmRequestType, bRequest, wValue, wIndex, wLength, buf);

-

-	return usbhControlRequest(hub->dev,

-			bmRequestType, bRequest, wValue, wIndex, wLength, buf);

-}

-

-

-static void _urb_complete(usbh_urb_t *urb) {

-

-	USBHHubDriver *const hubdp = (USBHHubDriver *)urb->userData;

-	switch (urb->status) {

-	case USBH_URBSTATUS_TIMEOUT:

-		/* the device NAKed */

-		udbg("HUB: no info");

-		hubdp->statuschange = 0;

-		break;

-	case USBH_URBSTATUS_OK: {

-		uint8_t len = hubdp->hubDesc.bNbrPorts / 8 + 1;

-		if (urb->actualLength != len) {

-			uwarnf("Expected %d status change bytes but got %d", len, urb->actualLength);

-		}

-

-		if (urb->actualLength < len)

-			len = urb->actualLength;

-

-		if (len > 4)

-			len = 4;

-

-		uint8_t *sc = (uint8_t *)&hubdp->statuschange;

-		uint8_t *r = hubdp->scbuff;

-		while (len--)

-			*sc++ |= *r++;

-

-		uinfof("HUB: change, %08x", hubdp->statuschange);

-	}	break;

-	case USBH_URBSTATUS_DISCONNECTED:

-		uwarn("HUB: URB disconnected, aborting poll");

-		return;

-	default:

-		uerrf("HUB: URB status unexpected = %d", urb->status);

-		break;

-	}

-

-	usbhURBObjectResetI(urb);

-	usbhURBSubmitI(urb);

-}

-

-static usbh_baseclassdriver_t *hub_load(usbh_device_t *dev,

-		const uint8_t *descriptor, uint16_t rem) {

-	int i;

-

-	USBHHubDriver *hubdp;

-

-	if ((rem < descriptor[0]) || (descriptor[1] != USBH_DT_DEVICE))

-		return NULL;

-

-	if (dev->devDesc.bDeviceProtocol != 0)

-		return NULL;

-

-	generic_iterator_t iep, icfg;

-	if_iterator_t iif;

-

-	cfg_iter_init(&icfg, dev->fullConfigurationDescriptor,

-			dev->basicConfigDesc.wTotalLength);

-

-	if_iter_init(&iif, &icfg);

-	if (!iif.valid)

-		return NULL;

-	const usbh_interface_descriptor_t *const ifdesc = if_get(&iif);

-	if ((ifdesc->bInterfaceClass != 0x09)

-		|| (ifdesc->bInterfaceSubClass != 0x00)

-		|| (ifdesc->bInterfaceProtocol != 0x00)) {

-		return NULL;

-	}

-

-	ep_iter_init(&iep, &iif);

-	if (!iep.valid)

-		return NULL;

-	const usbh_endpoint_descriptor_t *const epdesc = ep_get(&iep);

-	if ((epdesc->bmAttributes & 0x03) != USBH_EPTYPE_INT) {

-		return NULL;

-	}

-

-

-	/* alloc driver */

-	for (i = 0; i < HAL_USBHHUB_MAX_INSTANCES; i++) {

-		if (USBHHUBD[i].dev == NULL) {

-			hubdp = &USBHHUBD[i];

-			goto alloc_ok;

-		}

-	}

-

-	uwarn("Can't alloc HUB driver");

-

-	/* can't alloc */

-	return NULL;

-

-alloc_ok:

-	/* initialize the driver's variables */

-	hubdp->epint.status = USBH_EPSTATUS_UNINITIALIZED;

-	hubdp->dev = dev;

-	hubdp->ports = 0;

-

-	usbhEPSetName(&dev->ctrl, "HUB[CTRL]");

-

-	/* read Hub descriptor */

-	uinfo("Read Hub descriptor");

-	if (usbhhubControlRequest(dev->host, hubdp,

-			USBH_REQTYPE_IN | USBH_REQTYPE_CLASS | USBH_REQTYPE_DEVICE,

-			USBH_REQ_GET_DESCRIPTOR,

-			(USBH_DT_HUB << 8), 0, sizeof(hubdp->hubDesc),

-			(uint8_t *)&hubdp->hubDesc) != USBH_URBSTATUS_OK) {

-		hubdp->dev = NULL;

-		return NULL;

-	}

-

-	const usbh_hub_descriptor_t *const hubdesc = &hubdp->hubDesc;

-

-	uinfof("Hub descriptor loaded; %d ports, wHubCharacteristics=%04x, bPwrOn2PwrGood=%d, bHubContrCurrent=%d",

-			hubdesc->bNbrPorts,

-			hubdesc->wHubCharacteristics,

-			hubdesc->bPwrOn2PwrGood,

-			hubdesc->bHubContrCurrent);

-

-	/* Alloc ports */

-	uint8_t ports = hubdesc->bNbrPorts;

-	for (i = 0; (ports > 0) && (i < HAL_USBHHUB_MAX_PORTS); i++) {

-		if (USBHPorts[i].hub == NULL) {

-			uinfof("Alloc port %d", ports);

-			_usbhub_port_object_init(&USBHPorts[i], dev->host, hubdp, ports);

-			USBHPorts[i].next = hubdp->ports;

-			hubdp->ports = &USBHPorts[i];

-			--ports;

-		}

-	}

-

-	if (ports) {

-		uwarn("Could not alloc all ports");

-	}

-

-	/* link hub to the host's list */

-	list_add_tail(&hubdp->node, &dev->host->hubs);

-

-	/* enable power to ports */

-	usbh_port_t *port = hubdp->ports;

-	while (port) {

-		uinfof("Enable power for port %d", port->number);

-		usbhhubSetFeaturePort(port, USBH_PORT_FEAT_POWER);

-		port = port->next;

-	}

-

-	if (hubdesc->bPwrOn2PwrGood)

-		osalThreadSleepMilliseconds(2 * hubdesc->bPwrOn2PwrGood);

-

-	/* initialize the status change endpoint and trigger the first transfer */

-	usbhEPObjectInit(&hubdp->epint, dev, epdesc);

-	usbhEPSetName(&hubdp->epint, "HUB[INT ]");

-	usbhEPOpen(&hubdp->epint);

-

-	usbhURBObjectInit(&hubdp->urb, &hubdp->epint,

-			_urb_complete, hubdp, hubdp->scbuff,

-			(hubdesc->bNbrPorts + 8) / 8);

-

-	osalSysLock();

-	usbhURBSubmitI(&hubdp->urb);

-	osalOsRescheduleS();

-	osalSysUnlock();

-

-	return (usbh_baseclassdriver_t *)hubdp;

-}

-

-static void hub_unload(usbh_baseclassdriver_t *drv) {

-	osalDbgCheck(drv != NULL);

-	USBHHubDriver *const hubdp = (USBHHubDriver *)drv;

-

-	/* close the status change endpoint (this cancels ongoing URBs) */

-	osalSysLock();

-	usbhEPCloseS(&hubdp->epint);

-	osalSysUnlock();

-

-	/* de-alloc ports and unload drivers */

-	usbh_port_t *port = hubdp->ports;

-	while (port) {

-		_usbh_port_disconnected(port);

-		port->hub = NULL;

-		port = port->next;

-	}

-

-	/* unlink the hub from the host's list */

-	list_del(&hubdp->node);

-

-}

-

-void usbhhubObjectInit(USBHHubDriver *hubdp) {

-	osalDbgCheck(hubdp != NULL);

-	memset(hubdp, 0, sizeof(*hubdp));

-	hubdp->info = &usbhhubClassDriverInfo;

-}

-#else

-

-#if HAL_USE_USBH

-void _usbhub_port_object_init(usbh_port_t *port, USBHDriver *usbh, uint8_t number) {

-	memset(port, 0, sizeof(*port));

-	port->number = number;

-	port->device.host = usbh;

-}

-#endif

-

-#endif

+/*
+    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio
+              Copyright (C) 2015 Diego Ismirlian, TISA, (dismirlian (at) google's mail)
+
+    Licensed under the Apache License, Version 2.0 (the "License");
+    you may not use this file except in compliance with the License.
+    You may obtain a copy of the License at
+
+        http://www.apache.org/licenses/LICENSE-2.0
+
+    Unless required by applicable law or agreed to in writing, software
+    distributed under the License is distributed on an "AS IS" BASIS,
+    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+    See the License for the specific language governing permissions and
+    limitations under the License.
+*/
+
+#include "hal.h"
+#include "hal_usbh.h"
+#include "usbh/internal.h"
+
+#if HAL_USBH_USE_HUB
+
+#if !HAL_USE_USBH
+#error "USBHHUB needs HAL_USE_USBH"
+#endif
+
+#include <string.h>
+#include "usbh/dev/hub.h"
+
+#if USBHHUB_DEBUG_ENABLE_TRACE
+#define udbgf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)
+#define udbg(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)
+#else
+#define udbgf(f, ...)  do {} while(0)
+#define udbg(f, ...)   do {} while(0)
+#endif
+
+#if USBHHUB_DEBUG_ENABLE_INFO
+#define uinfof(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)
+#define uinfo(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)
+#else
+#define uinfof(f, ...)  do {} while(0)
+#define uinfo(f, ...)   do {} while(0)
+#endif
+
+#if USBHHUB_DEBUG_ENABLE_WARNINGS
+#define uwarnf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)
+#define uwarn(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)
+#else
+#define uwarnf(f, ...)  do {} while(0)
+#define uwarn(f, ...)   do {} while(0)
+#endif
+
+#if USBHHUB_DEBUG_ENABLE_ERRORS
+#define uerrf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)
+#define uerr(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)
+#else
+#define uerrf(f, ...)  do {} while(0)
+#define uerr(f, ...)   do {} while(0)
+#endif
+
+
+USBHHubDriver USBHHUBD[HAL_USBHHUB_MAX_INSTANCES];
+usbh_port_t USBHPorts[HAL_USBHHUB_MAX_PORTS];
+
+static usbh_baseclassdriver_t *hub_load(usbh_device_t *dev, const uint8_t *descriptor, uint16_t rem);
+static void hub_unload(usbh_baseclassdriver_t *drv);
+static const usbh_classdriver_vmt_t usbhhubClassDriverVMT = {
+	hub_load,
+	hub_unload
+};
+const usbh_classdriverinfo_t usbhhubClassDriverInfo = {
+	0x09, 0x00, -1, "HUB", &usbhhubClassDriverVMT
+};
+
+
+void _usbhub_port_object_init(usbh_port_t *port, USBHDriver *usbh,
+		USBHHubDriver *hub, uint8_t number) {
+	memset(port, 0, sizeof(*port));
+	port->number = number;
+	port->device.host = usbh;
+	port->hub = hub;
+}
+
+usbh_urbstatus_t usbhhubControlRequest(USBHDriver *host, USBHHubDriver *hub,
+											uint8_t bmRequestType,
+											uint8_t bRequest,
+											uint16_t wValue,
+											uint16_t wIndex,
+											uint16_t wLength,
+											uint8_t *buf) {
+	if (hub == NULL)
+		return usbh_lld_root_hub_request(host, bmRequestType, bRequest, wValue, wIndex, wLength, buf);
+
+	return usbhControlRequest(hub->dev,
+			bmRequestType, bRequest, wValue, wIndex, wLength, buf);
+}
+
+
+static void _urb_complete(usbh_urb_t *urb) {
+
+	USBHHubDriver *const hubdp = (USBHHubDriver *)urb->userData;
+	switch (urb->status) {
+	case USBH_URBSTATUS_TIMEOUT:
+		/* the device NAKed */
+		udbg("HUB: no info");
+		hubdp->statuschange = 0;
+		break;
+	case USBH_URBSTATUS_OK: {
+		uint8_t len = hubdp->hubDesc.bNbrPorts / 8 + 1;
+		if (urb->actualLength != len) {
+			uwarnf("Expected %d status change bytes but got %d", len, urb->actualLength);
+		}
+
+		if (urb->actualLength < len)
+			len = urb->actualLength;
+
+		if (len > 4)
+			len = 4;
+
+		uint8_t *sc = (uint8_t *)&hubdp->statuschange;
+		uint8_t *r = hubdp->scbuff;
+		while (len--)
+			*sc++ |= *r++;
+
+		uinfof("HUB: change, %08x", hubdp->statuschange);
+	}	break;
+	case USBH_URBSTATUS_DISCONNECTED:
+		uwarn("HUB: URB disconnected, aborting poll");
+		return;
+	default:
+		uerrf("HUB: URB status unexpected = %d", urb->status);
+		break;
+	}
+
+	usbhURBObjectResetI(urb);
+	usbhURBSubmitI(urb);
+}
+
+static usbh_baseclassdriver_t *hub_load(usbh_device_t *dev,
+		const uint8_t *descriptor, uint16_t rem) {
+	int i;
+
+	USBHHubDriver *hubdp;
+
+	if ((rem < descriptor[0]) || (descriptor[1] != USBH_DT_DEVICE))
+		return NULL;
+
+	if (dev->devDesc.bDeviceProtocol != 0)
+		return NULL;
+
+	generic_iterator_t iep, icfg;
+	if_iterator_t iif;
+
+	cfg_iter_init(&icfg, dev->fullConfigurationDescriptor,
+			dev->basicConfigDesc.wTotalLength);
+
+	if_iter_init(&iif, &icfg);
+	if (!iif.valid)
+		return NULL;
+	const usbh_interface_descriptor_t *const ifdesc = if_get(&iif);
+	if ((ifdesc->bInterfaceClass != 0x09)
+		|| (ifdesc->bInterfaceSubClass != 0x00)
+		|| (ifdesc->bInterfaceProtocol != 0x00)) {
+		return NULL;
+	}
+
+	ep_iter_init(&iep, &iif);
+	if (!iep.valid)
+		return NULL;
+	const usbh_endpoint_descriptor_t *const epdesc = ep_get(&iep);
+	if ((epdesc->bmAttributes & 0x03) != USBH_EPTYPE_INT) {
+		return NULL;
+	}
+
+
+	/* alloc driver */
+	for (i = 0; i < HAL_USBHHUB_MAX_INSTANCES; i++) {
+		if (USBHHUBD[i].dev == NULL) {
+			hubdp = &USBHHUBD[i];
+			goto alloc_ok;
+		}
+	}
+
+	uwarn("Can't alloc HUB driver");
+
+	/* can't alloc */
+	return NULL;
+
+alloc_ok:
+	/* initialize the driver's variables */
+	hubdp->epint.status = USBH_EPSTATUS_UNINITIALIZED;
+	hubdp->dev = dev;
+	hubdp->ports = 0;
+
+	usbhEPSetName(&dev->ctrl, "HUB[CTRL]");
+
+	/* read Hub descriptor */
+	uinfo("Read Hub descriptor");
+	if (usbhhubControlRequest(dev->host, hubdp,
+			USBH_REQTYPE_IN | USBH_REQTYPE_CLASS | USBH_REQTYPE_DEVICE,
+			USBH_REQ_GET_DESCRIPTOR,
+			(USBH_DT_HUB << 8), 0, sizeof(hubdp->hubDesc),
+			(uint8_t *)&hubdp->hubDesc) != USBH_URBSTATUS_OK) {
+		hubdp->dev = NULL;
+		return NULL;
+	}
+
+	const usbh_hub_descriptor_t *const hubdesc = &hubdp->hubDesc;
+
+	uinfof("Hub descriptor loaded; %d ports, wHubCharacteristics=%04x, bPwrOn2PwrGood=%d, bHubContrCurrent=%d",
+			hubdesc->bNbrPorts,
+			hubdesc->wHubCharacteristics,
+			hubdesc->bPwrOn2PwrGood,
+			hubdesc->bHubContrCurrent);
+
+	/* Alloc ports */
+	uint8_t ports = hubdesc->bNbrPorts;
+	for (i = 0; (ports > 0) && (i < HAL_USBHHUB_MAX_PORTS); i++) {
+		if (USBHPorts[i].hub == NULL) {
+			uinfof("Alloc port %d", ports);
+			_usbhub_port_object_init(&USBHPorts[i], dev->host, hubdp, ports);
+			USBHPorts[i].next = hubdp->ports;
+			hubdp->ports = &USBHPorts[i];
+			--ports;
+		}
+	}
+
+	if (ports) {
+		uwarn("Could not alloc all ports");
+	}
+
+	/* link hub to the host's list */
+	list_add_tail(&hubdp->node, &dev->host->hubs);
+
+	/* enable power to ports */
+	usbh_port_t *port = hubdp->ports;
+	while (port) {
+		uinfof("Enable power for port %d", port->number);
+		usbhhubSetFeaturePort(port, USBH_PORT_FEAT_POWER);
+		port = port->next;
+	}
+
+	if (hubdesc->bPwrOn2PwrGood)
+		osalThreadSleepMilliseconds(2 * hubdesc->bPwrOn2PwrGood);
+
+	/* initialize the status change endpoint and trigger the first transfer */
+	usbhEPObjectInit(&hubdp->epint, dev, epdesc);
+	usbhEPSetName(&hubdp->epint, "HUB[INT ]");
+	usbhEPOpen(&hubdp->epint);
+
+	usbhURBObjectInit(&hubdp->urb, &hubdp->epint,
+			_urb_complete, hubdp, hubdp->scbuff,
+			(hubdesc->bNbrPorts + 8) / 8);
+
+	osalSysLock();
+	usbhURBSubmitI(&hubdp->urb);
+	osalOsRescheduleS();
+	osalSysUnlock();
+
+	return (usbh_baseclassdriver_t *)hubdp;
+}
+
+static void hub_unload(usbh_baseclassdriver_t *drv) {
+	osalDbgCheck(drv != NULL);
+	USBHHubDriver *const hubdp = (USBHHubDriver *)drv;
+
+	/* close the status change endpoint (this cancels ongoing URBs) */
+	osalSysLock();
+	usbhEPCloseS(&hubdp->epint);
+	osalSysUnlock();
+
+	/* de-alloc ports and unload drivers */
+	usbh_port_t *port = hubdp->ports;
+	while (port) {
+		_usbh_port_disconnected(port);
+		port->hub = NULL;
+		port = port->next;
+	}
+
+	/* unlink the hub from the host's list */
+	list_del(&hubdp->node);
+
+}
+
+void usbhhubObjectInit(USBHHubDriver *hubdp) {
+	osalDbgCheck(hubdp != NULL);
+	memset(hubdp, 0, sizeof(*hubdp));
+	hubdp->info = &usbhhubClassDriverInfo;
+}
+#else
+
+#if HAL_USE_USBH
+void _usbhub_port_object_init(usbh_port_t *port, USBHDriver *usbh, uint8_t number) {
+	memset(port, 0, sizeof(*port));
+	port->number = number;
+	port->device.host = usbh;
+}
+#endif
+
+#endif
diff --git a/os/hal/src/usbh/usbh_msd.c b/os/hal/src/usbh/hal_usbh_msd.c
index 8db68a4..6869a74 100644
--- a/os/hal/src/usbh/usbh_msd.c
+++ b/os/hal/src/usbh/hal_usbh_msd.c
@@ -1,939 +1,939 @@
-/*

-    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio

-              Copyright (C) 2015 Diego Ismirlian, TISA, (dismirlian (at) google's mail)

-

-    Licensed under the Apache License, Version 2.0 (the "License");

-    you may not use this file except in compliance with the License.

-    You may obtain a copy of the License at

-

-        http://www.apache.org/licenses/LICENSE-2.0

-

-    Unless required by applicable law or agreed to in writing, software

-    distributed under the License is distributed on an "AS IS" BASIS,

-    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.

-    See the License for the specific language governing permissions and

-    limitations under the License.

-*/

-

-#include "hal.h"

-#include "usbh.h"

-

-#if HAL_USBH_USE_MSD

-

-#if !HAL_USE_USBH

-#error "USBHMSD needs USBH"

-#endif

-

-#include <string.h>

-#include "usbh/dev/msd.h"

-#include "usbh/internal.h"

-

-//#pragma GCC optimize("Og")

-

-

-#if USBHMSD_DEBUG_ENABLE_TRACE

-#define udbgf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)

-#define udbg(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)

-#else

-#define udbgf(f, ...)  do {} while(0)

-#define udbg(f, ...)   do {} while(0)

-#endif

-

-#if USBHMSD_DEBUG_ENABLE_INFO

-#define uinfof(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)

-#define uinfo(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)

-#else

-#define uinfof(f, ...)  do {} while(0)

-#define uinfo(f, ...)   do {} while(0)

-#endif

-

-#if USBHMSD_DEBUG_ENABLE_WARNINGS

-#define uwarnf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)

-#define uwarn(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)

-#else

-#define uwarnf(f, ...)  do {} while(0)

-#define uwarn(f, ...)   do {} while(0)

-#endif

-

-#if USBHMSD_DEBUG_ENABLE_ERRORS

-#define uerrf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)

-#define uerr(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)

-#else

-#define uerrf(f, ...)  do {} while(0)

-#define uerr(f, ...)   do {} while(0)

-#endif

-

-

-

-

-

-/*===========================================================================*/

-/* USB Class driver loader for MSD								 		 	 */

-/*===========================================================================*/

-

-USBHMassStorageDriver USBHMSD[HAL_USBHMSD_MAX_INSTANCES];

-

-static usbh_baseclassdriver_t *_msd_load(usbh_device_t *dev, const uint8_t *descriptor, uint16_t rem);

-static void _msd_unload(usbh_baseclassdriver_t *drv);

-

-static const usbh_classdriver_vmt_t class_driver_vmt = {

-	_msd_load,

-	_msd_unload

-};

-

-const usbh_classdriverinfo_t usbhmsdClassDriverInfo = {

-	0x08, 0x06, 0x50, "MSD", &class_driver_vmt

-};

-

-#define MSD_REQ_RESET							0xFF

-#define MSD_GET_MAX_LUN							0xFE

-

-static usbh_baseclassdriver_t *_msd_load(usbh_device_t *dev, const uint8_t *descriptor, uint16_t rem) {

-	int i;

-	USBHMassStorageDriver *msdp;

-	uint8_t luns; // should declare it here to eliminate 'control bypass initialization' warning

-	usbh_urbstatus_t stat;  // should declare it here to eliminate 'control bypass initialization' warning

-

-	if ((rem < descriptor[0]) || (descriptor[1] != USBH_DT_INTERFACE))

-		return NULL;

-

-	const usbh_interface_descriptor_t * const ifdesc = (const usbh_interface_descriptor_t *)descriptor;

-

-	if ((ifdesc->bAlternateSetting != 0)

-			|| (ifdesc->bNumEndpoints < 2)

-			|| (ifdesc->bInterfaceSubClass != 0x06)

-			|| (ifdesc->bInterfaceProtocol != 0x50)) {

-		return NULL;

-	}

-

-	/* alloc driver */

-	for (i = 0; i < HAL_USBHMSD_MAX_INSTANCES; i++) {

-		if (USBHMSD[i].dev == NULL) {

-			msdp = &USBHMSD[i];

-			goto alloc_ok;

-		}

-	}

-

-	uwarn("Can't alloc MSD driver");

-

-	/* can't alloc */

-	return NULL;

-

-alloc_ok:

-	/* initialize the driver's variables */

-	msdp->epin.status = USBH_EPSTATUS_UNINITIALIZED;

-	msdp->epout.status = USBH_EPSTATUS_UNINITIALIZED;

-	msdp->max_lun = 0;

-	msdp->tag = 0;

-	msdp->luns = 0;

-	msdp->ifnum = ifdesc->bInterfaceNumber;

-	usbhEPSetName(&dev->ctrl, "MSD[CTRL]");

-

-	/* parse the configuration descriptor */

-	if_iterator_t iif;

-	generic_iterator_t iep;

-	iif.iad = 0;

-	iif.curr = descriptor;

-	iif.rem = rem;

-	for (ep_iter_init(&iep, &iif); iep.valid; ep_iter_next(&iep)) {

-		const usbh_endpoint_descriptor_t *const epdesc = ep_get(&iep);

-		if ((epdesc->bEndpointAddress & 0x80) && (epdesc->bmAttributes == USBH_EPTYPE_BULK)) {

-			uinfof("BULK IN endpoint found: bEndpointAddress=%02x", epdesc->bEndpointAddress);

-			usbhEPObjectInit(&msdp->epin, dev, epdesc);

-			usbhEPSetName(&msdp->epin, "MSD[BIN ]");

-		} else if (((epdesc->bEndpointAddress & 0x80) == 0)

-				&& (epdesc->bmAttributes == USBH_EPTYPE_BULK)) {

-			uinfof("BULK OUT endpoint found: bEndpointAddress=%02x", epdesc->bEndpointAddress);

-			usbhEPObjectInit(&msdp->epout, dev, epdesc);

-			usbhEPSetName(&msdp->epout, "MSD[BOUT]");

-		} else {

-			uinfof("unsupported endpoint found: bEndpointAddress=%02x, bmAttributes=%02x",

-					epdesc->bEndpointAddress, epdesc->bmAttributes);

-		}

-	}

-	if ((msdp->epin.status != USBH_EPSTATUS_CLOSED) || (msdp->epout.status != USBH_EPSTATUS_CLOSED)) {

-		goto deinit;

-	}

-

-	/* read the number of LUNs */

-	uinfo("Reading Max LUN:");

-	USBH_DEFINE_BUFFER(uint8_t, buff[4]);

-	stat = usbhControlRequest(dev,

-			USBH_CLASSIN(USBH_REQTYPE_INTERFACE, MSD_GET_MAX_LUN, 0, msdp->ifnum),

-			1, buff);

-	if (stat == USBH_URBSTATUS_OK) {

-		msdp->max_lun = buff[0] + 1;

-		uinfof("\tmax_lun = %d", msdp->max_lun);

-		if (msdp->max_lun > HAL_USBHMSD_MAX_LUNS) {

-			msdp->max_lun = HAL_USBHMSD_MAX_LUNS;

-			uwarnf("\tUsing max_lun = %d", msdp->max_lun);

-		}

-	} else if (stat == USBH_URBSTATUS_STALL) {

-		uwarn("\tStall, max_lun = 1");

-		msdp->max_lun = 1;

-	} else {

-		uerr("\tError");

-		goto deinit;

-	}

-

-	/* open the bulk IN/OUT endpoints */

-	usbhEPOpen(&msdp->epin);

-	usbhEPOpen(&msdp->epout);

-

-	/* Alloc one block device per logical unit found */

-	luns = msdp->max_lun;

-	for (i = 0; (luns > 0) && (i < HAL_USBHMSD_MAX_LUNS); i++) {

-		if (MSBLKD[i].msdp == NULL) {

-			/* link the new block driver to the list */

-			MSBLKD[i].next = msdp->luns;

-			msdp->luns = &MSBLKD[i];

-			MSBLKD[i].msdp = msdp;

-

-			osalSysLock();

-			MSBLKD[i].state = BLK_ACTIVE;	/* transition directly to active, instead of BLK_STOP */

-			osalSysUnlock();

-

-			/* connect the LUN (TODO: review if it's best to leave the LUN disconnected) */

-			usbhmsdLUNConnect(&MSBLKD[i]);

-			luns--;

-		}

-	}

-

-	return (usbh_baseclassdriver_t *)msdp;

-

-deinit:

-	/* Here, the enpoints are closed, and the driver is unlinked */

-	return NULL;

-}

-

-static void _msd_unload(usbh_baseclassdriver_t *drv) {

-	osalDbgCheck(drv != NULL);

-	USBHMassStorageDriver *const msdp = (USBHMassStorageDriver *)drv;

-	USBHMassStorageLUNDriver *lunp = msdp->luns;

-

-	osalMutexLock(&msdp->mtx);

-	osalSysLock();

-	usbhEPCloseS(&msdp->epin);

-	usbhEPCloseS(&msdp->epout);

-	while (lunp) {

-		lunp->state = BLK_STOP;

-		lunp = lunp->next;

-	}

-	osalSysUnlock();

-	osalMutexUnlock(&msdp->mtx);

-

-	/* now that the LUNs are idle, deinit them */

-	lunp = msdp->luns;

-	osalSysLock();

-	while (lunp) {

-		usbhmsdLUNObjectInit(lunp);

-		lunp = lunp->next;

-	}

-	osalSysUnlock();

-}

-

-

-/*===========================================================================*/

-/* MSD Class driver operations (Bulk-Only transport)			 		 	 */

-/*===========================================================================*/

-

-

-

-/* USB Bulk Only Transport SCSI Command block wrapper */

-PACKED_STRUCT {

-	uint32_t dCBWSignature;

-	uint32_t dCBWTag;

-	uint32_t dCBWDataTransferLength;

-	uint8_t bmCBWFlags;

-	uint8_t bCBWLUN;

-	uint8_t bCBWCBLength;

-	uint8_t CBWCB[16];

-} msd_cbw_t;

-#define MSD_CBW_SIGNATURE						0x43425355

-#define MSD_CBWFLAGS_D2H						0x80

-#define MSD_CBWFLAGS_H2D						0x00

-

-

-/* USB Bulk Only Transport SCSI Command status wrapper */

-PACKED_STRUCT {

-	uint32_t dCSWSignature;

-	uint32_t dCSWTag;

-	uint32_t dCSWDataResidue;

-	uint8_t bCSWStatus;

-} msd_csw_t;

-#define MSD_CSW_SIGNATURE						0x53425355

-

-

-typedef union {

-	msd_cbw_t cbw;

-	msd_csw_t csw;

-} msd_transaction_t;

-

-typedef enum {

-	MSD_TRANSACTIONRESULT_OK,

-	MSD_TRANSACTIONRESULT_DISCONNECTED,

-	MSD_TRANSACTIONRESULT_STALL,

-	MSD_TRANSACTIONRESULT_BUS_ERROR,

-	MSD_TRANSACTIONRESULT_SYNC_ERROR

-} msd_transaction_result_t;

-

-typedef enum {

-	MSD_COMMANDRESULT_PASSED = 0,

-	MSD_COMMANDRESULT_FAILED = 1,

-	MSD_COMMANDRESULT_PHASE_ERROR = 2

-} msd_command_result_t;

-

-typedef struct {

-	msd_transaction_result_t tres;

-	msd_command_result_t cres;

-} msd_result_t;

-

-

-/* ----------------------------------------------------- */

-/* 			SCSI Commands 								*/

-/* ----------------------------------------------------- */

-

-/* Read 10 and Write 10 */

-#define SCSI_CMD_READ_10 						0x28

-#define SCSI_CMD_WRITE_10						0x2A

-

-/* Request sense */

-#define SCSI_CMD_REQUEST_SENSE 					0x03

-PACKED_STRUCT {

-	uint8_t byte[18];

-} scsi_sense_response_t;

-

-#define SCSI_SENSE_KEY_GOOD                     0x00

-#define SCSI_SENSE_KEY_RECOVERED_ERROR          0x01

-#define SCSI_SENSE_KEY_NOT_READY                0x02

-#define SCSI_SENSE_KEY_MEDIUM_ERROR             0x03

-#define SCSI_SENSE_KEY_HARDWARE_ERROR           0x04

-#define SCSI_SENSE_KEY_ILLEGAL_REQUEST          0x05

-#define SCSI_SENSE_KEY_UNIT_ATTENTION           0x06

-#define SCSI_SENSE_KEY_DATA_PROTECT             0x07

-#define SCSI_SENSE_KEY_BLANK_CHECK              0x08

-#define SCSI_SENSE_KEY_VENDOR_SPECIFIC          0x09

-#define SCSI_SENSE_KEY_COPY_ABORTED             0x0A

-#define SCSI_SENSE_KEY_ABORTED_COMMAND          0x0B

-#define SCSI_SENSE_KEY_VOLUME_OVERFLOW          0x0D

-#define SCSI_SENSE_KEY_MISCOMPARE               0x0E

-#define SCSI_ASENSE_NO_ADDITIONAL_INFORMATION   0x00

-#define SCSI_ASENSE_LOGICAL_UNIT_NOT_READY      0x04

-#define SCSI_ASENSE_INVALID_FIELD_IN_CDB        0x24

-#define SCSI_ASENSE_NOT_READY_TO_READY_CHANGE   0x28

-#define SCSI_ASENSE_WRITE_PROTECTED             0x27

-#define SCSI_ASENSE_FORMAT_ERROR                0x31

-#define SCSI_ASENSE_INVALID_COMMAND             0x20

-#define SCSI_ASENSE_LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE 0x21

-#define SCSI_ASENSE_MEDIUM_NOT_PRESENT                 0x3A

-#define SCSI_ASENSEQ_NO_QUALIFIER                      0x00

-#define SCSI_ASENSEQ_FORMAT_COMMAND_FAILED             0x01

-#define SCSI_ASENSEQ_INITIALIZING_COMMAND_REQUIRED     0x02

-#define SCSI_ASENSEQ_OPERATION_IN_PROGRESS             0x07

-

-/* Inquiry */

-#define SCSI_CMD_INQUIRY 						0x12

-PACKED_STRUCT {

-	uint8_t peripheral;

-	uint8_t removable;

-	uint8_t version;

-	uint8_t response_data_format;

-	uint8_t additional_length;

-	uint8_t sccstp;

-	uint8_t bqueetc;

-	uint8_t cmdque;

-	uint8_t vendorID[8];

-	uint8_t productID[16];

-	uint8_t productRev[4];

-} scsi_inquiry_response_t;

-

-/* Read Capacity 10 */

-#define SCSI_CMD_READ_CAPACITY_10				0x25

-PACKED_STRUCT {

-	uint32_t last_block_addr;

-	uint32_t block_size;

-} scsi_readcapacity10_response_t;

-

-/* Start/Stop Unit */

-#define SCSI_CMD_START_STOP_UNIT				0x1B

-PACKED_STRUCT {

-	uint8_t op_code;

-	uint8_t lun_immed;

-	uint8_t res1;

-	uint8_t res2;

-	uint8_t loej_start;

-	uint8_t control;

-} scsi_startstopunit_request_t;

-

-/* test unit ready */

-#define SCSI_CMD_TEST_UNIT_READY				0x00

-

-/* Other commands, TODO: use or remove them

-#define SCSI_CMD_PREVENT_ALLOW_MEDIUM_REMOVAL	0x1E

-#define SCSI_CMD_VERIFY_10						0x2F

-#define SCSI_CMD_SEND_DIAGNOSTIC				0x1D

-#define SCSI_CMD_MODE_SENSE_6                   0x1A

-*/

-

-static inline void _prepare_cbw(msd_transaction_t *tran, USBHMassStorageLUNDriver *lunp) {

-	tran->cbw.bCBWLUN = (uint8_t)(lunp - &lunp->msdp->luns[0]);

-	memset(&tran->cbw.CBWCB, 0, sizeof(tran->cbw.CBWCB));

-}

-

-static msd_transaction_result_t _msd_transaction(msd_transaction_t *tran, USBHMassStorageLUNDriver *lunp, void *data) {

-

-	uint32_t actual_len;

-	usbh_urbstatus_t status;

-

-	tran->cbw.dCBWSignature = MSD_CBW_SIGNATURE;

-	tran->cbw.dCBWTag = ++lunp->msdp->tag;

-

-	/* control phase */

-	status = usbhBulkTransfer(&lunp->msdp->epout, &tran->cbw,

-					sizeof(tran->cbw), &actual_len, MS2ST(1000));

-

-	if (status == USBH_URBSTATUS_CANCELLED) {

-		uerr("\tMSD: Control phase: USBH_URBSTATUS_CANCELLED");

-		return MSD_TRANSACTIONRESULT_DISCONNECTED;

-	} else if (status == USBH_URBSTATUS_STALL) {

-		uerr("\tMSD: Control phase: USBH_URBSTATUS_STALL");

-		return MSD_TRANSACTIONRESULT_STALL;

-	} else if (status != USBH_URBSTATUS_OK) {

-		uerrf("\tMSD: Control phase: status = %d, != OK", status);

-		return MSD_TRANSACTIONRESULT_BUS_ERROR;

-	} else if (actual_len != sizeof(tran->cbw)) {

-		uerrf("\tMSD: Control phase: wrong actual_len = %d", actual_len);

-		return MSD_TRANSACTIONRESULT_BUS_ERROR;

-	}

-

-

-	/* data phase */

-	if (tran->cbw.dCBWDataTransferLength) {

-		status = usbhBulkTransfer(

-				tran->cbw.bmCBWFlags & MSD_CBWFLAGS_D2H ? &lunp->msdp->epin : &lunp->msdp->epout,

-				data,

-				tran->cbw.dCBWDataTransferLength,

-				&actual_len, MS2ST(20000));

-

-		if (status == USBH_URBSTATUS_CANCELLED) {

-			uerr("\tMSD: Data phase: USBH_URBSTATUS_CANCELLED");

-			return MSD_TRANSACTIONRESULT_DISCONNECTED;

-		} else if (status == USBH_URBSTATUS_STALL) {

-			uerr("\tMSD: Data phase: USBH_URBSTATUS_STALL");

-			return MSD_TRANSACTIONRESULT_STALL;

-		} else if (status != USBH_URBSTATUS_OK) {

-			uerrf("\tMSD: Data phase: status = %d, != OK", status);

-			return MSD_TRANSACTIONRESULT_BUS_ERROR;

-		} else if (actual_len != tran->cbw.dCBWDataTransferLength) {

-			uerrf("\tMSD: Data phase: wrong actual_len = %d", actual_len);

-			return MSD_TRANSACTIONRESULT_BUS_ERROR;

-		}

-	}

-

-

-	/* status phase */

-	status = usbhBulkTransfer(&lunp->msdp->epin, &tran->csw,

-			sizeof(tran->csw), &actual_len, MS2ST(1000));

-

-	if (status == USBH_URBSTATUS_CANCELLED) {

-		uerr("\tMSD: Status phase: USBH_URBSTATUS_CANCELLED");

-		return MSD_TRANSACTIONRESULT_DISCONNECTED;

-	} else if (status == USBH_URBSTATUS_STALL) {

-		uerr("\tMSD: Status phase: USBH_URBSTATUS_STALL");

-		return MSD_TRANSACTIONRESULT_STALL;

-	} else if (status != USBH_URBSTATUS_OK) {

-		uerrf("\tMSD: Status phase: status = %d, != OK", status);

-		return MSD_TRANSACTIONRESULT_BUS_ERROR;

-	} else if (actual_len != sizeof(tran->csw)) {

-		uerrf("\tMSD: Status phase: wrong actual_len = %d", actual_len);

-		return MSD_TRANSACTIONRESULT_BUS_ERROR;

-	} else if (tran->csw.dCSWSignature != MSD_CSW_SIGNATURE) {

-		uerr("\tMSD: Status phase: wrong signature");

-		return MSD_TRANSACTIONRESULT_BUS_ERROR;

-	} else if (tran->csw.dCSWTag != lunp->msdp->tag) {

-		uerrf("\tMSD: Status phase: wrong tag (expected %d, got %d)",

-				lunp->msdp->tag, tran->csw.dCSWTag);

-		return MSD_TRANSACTIONRESULT_SYNC_ERROR;

-	}

-

-	if (tran->csw.dCSWDataResidue) {

-		uwarnf("\tMSD: Residue=%d", tran->csw.dCSWDataResidue);

-	}

-

-	return MSD_TRANSACTIONRESULT_OK;

-}

-

-

-static msd_result_t scsi_inquiry(USBHMassStorageLUNDriver *lunp, scsi_inquiry_response_t *resp) {

-	msd_transaction_t transaction;

-	msd_result_t res;

-

-	_prepare_cbw(&transaction, lunp);

-	transaction.cbw.dCBWDataTransferLength = sizeof(scsi_inquiry_response_t);

-	transaction.cbw.bmCBWFlags = MSD_CBWFLAGS_D2H;

-	transaction.cbw.bCBWCBLength = 6;

-	transaction.cbw.CBWCB[0] = SCSI_CMD_INQUIRY;

-	transaction.cbw.CBWCB[4] = sizeof(scsi_inquiry_response_t);

-

-	res.tres = _msd_transaction(&transaction, lunp, resp);

-	if (res.tres == MSD_TRANSACTIONRESULT_OK) {

-		res.cres = (msd_command_result_t) transaction.csw.bCSWStatus;

-	}

-	return res;

-}

-

-static msd_result_t scsi_requestsense(USBHMassStorageLUNDriver *lunp, scsi_sense_response_t *resp) {

-	msd_transaction_t transaction;

-	msd_result_t res;

-

-	_prepare_cbw(&transaction, lunp);

-	transaction.cbw.dCBWDataTransferLength = sizeof(scsi_sense_response_t);

-	transaction.cbw.bmCBWFlags = MSD_CBWFLAGS_D2H;

-	transaction.cbw.bCBWCBLength = 12;

-	transaction.cbw.CBWCB[0] = SCSI_CMD_REQUEST_SENSE;

-	transaction.cbw.CBWCB[4] = sizeof(scsi_sense_response_t);

-

-	res.tres = _msd_transaction(&transaction, lunp, resp);

-	if (res.tres == MSD_TRANSACTIONRESULT_OK) {

-		res.cres = (msd_command_result_t) transaction.csw.bCSWStatus;

-	}

-	return res;

-}

-

-static msd_result_t scsi_testunitready(USBHMassStorageLUNDriver *lunp) {

-	msd_transaction_t transaction;

-	msd_result_t res;

-

-	_prepare_cbw(&transaction, lunp);

-	transaction.cbw.dCBWDataTransferLength = 0;

-	transaction.cbw.bmCBWFlags = MSD_CBWFLAGS_D2H;

-	transaction.cbw.bCBWCBLength = 6;

-	transaction.cbw.CBWCB[0] = SCSI_CMD_TEST_UNIT_READY;

-

-	res.tres = _msd_transaction(&transaction, lunp, NULL);

-	if (res.tres == MSD_TRANSACTIONRESULT_OK) {

-		res.cres = (msd_command_result_t) transaction.csw.bCSWStatus;

-	}

-	return res;

-}

-

-static msd_result_t scsi_readcapacity10(USBHMassStorageLUNDriver *lunp, scsi_readcapacity10_response_t *resp) {

-	msd_transaction_t transaction;

-	msd_result_t res;

-

-	_prepare_cbw(&transaction, lunp);

-	transaction.cbw.dCBWDataTransferLength = sizeof(scsi_readcapacity10_response_t);

-	transaction.cbw.bmCBWFlags = MSD_CBWFLAGS_D2H;

-	transaction.cbw.bCBWCBLength = 12;

-	transaction.cbw.CBWCB[0] = SCSI_CMD_READ_CAPACITY_10;

-

-	res.tres = _msd_transaction(&transaction, lunp, resp);

-	if (res.tres == MSD_TRANSACTIONRESULT_OK) {

-		res.cres = (msd_command_result_t) transaction.csw.bCSWStatus;

-	}

-	return res;

-}

-

-

-static msd_result_t scsi_read10(USBHMassStorageLUNDriver *lunp, uint32_t lba, uint16_t n, uint8_t *data) {

-	msd_transaction_t transaction;

-	msd_result_t res;

-

-	_prepare_cbw(&transaction, lunp);

-	transaction.cbw.dCBWDataTransferLength = n * lunp->info.blk_size;

-	transaction.cbw.bmCBWFlags = MSD_CBWFLAGS_D2H;

-	transaction.cbw.bCBWCBLength = 10;

-	transaction.cbw.CBWCB[0] = SCSI_CMD_READ_10;

-	transaction.cbw.CBWCB[2] = (uint8_t)(lba >> 24);

-	transaction.cbw.CBWCB[3] = (uint8_t)(lba >> 16);

-	transaction.cbw.CBWCB[4] = (uint8_t)(lba >> 8);

-	transaction.cbw.CBWCB[5] = (uint8_t)(lba);

-	transaction.cbw.CBWCB[7] = (uint8_t)(n >> 8);

-	transaction.cbw.CBWCB[8] = (uint8_t)(n);

-

-	res.tres = _msd_transaction(&transaction, lunp, data);

-	if (res.tres == MSD_TRANSACTIONRESULT_OK) {

-		res.cres = (msd_command_result_t) transaction.csw.bCSWStatus;

-	}

-	return res;

-}

-

-static msd_result_t scsi_write10(USBHMassStorageLUNDriver *lunp, uint32_t lba, uint16_t n, const uint8_t *data) {

-	msd_transaction_t transaction;

-	msd_result_t res;

-

-	_prepare_cbw(&transaction, lunp);

-	transaction.cbw.dCBWDataTransferLength = n * lunp->info.blk_size;

-	transaction.cbw.bmCBWFlags = MSD_CBWFLAGS_H2D;

-	transaction.cbw.bCBWCBLength = 10;

-	transaction.cbw.CBWCB[0] = SCSI_CMD_WRITE_10;

-	transaction.cbw.CBWCB[2] = (uint8_t)(lba >> 24);

-	transaction.cbw.CBWCB[3] = (uint8_t)(lba >> 16);

-	transaction.cbw.CBWCB[4] = (uint8_t)(lba >> 8);

-	transaction.cbw.CBWCB[5] = (uint8_t)(lba);

-	transaction.cbw.CBWCB[7] = (uint8_t)(n >> 8);

-	transaction.cbw.CBWCB[8] = (uint8_t)(n);

-

-	res.tres = _msd_transaction(&transaction, lunp, (uint8_t *)data);

-	if (res.tres == MSD_TRANSACTIONRESULT_OK) {

-		res.cres = (msd_command_result_t) transaction.csw.bCSWStatus;

-	}

-	return res;

-}

-

-

-

-/*===========================================================================*/

-/* Block driver data/functions								 		 	 	 */

-/*===========================================================================*/

-

-USBHMassStorageLUNDriver MSBLKD[HAL_USBHMSD_MAX_LUNS];

-

-static const struct USBHMassStorageDriverVMT blk_vmt = {

-	(bool (*)(void *))usbhmsdLUNIsInserted,

-	(bool (*)(void *))usbhmsdLUNIsProtected,

-	(bool (*)(void *))usbhmsdLUNConnect,

-	(bool (*)(void *))usbhmsdLUNDisconnect,

-	(bool (*)(void *, uint32_t, uint8_t *, uint32_t))usbhmsdLUNRead,

-	(bool (*)(void *, uint32_t,	const uint8_t *, uint32_t))usbhmsdLUNWrite,

-	(bool (*)(void *))usbhmsdLUNSync,

-	(bool (*)(void *, BlockDeviceInfo *))usbhmsdLUNGetInfo

-};

-

-

-

-static uint32_t _requestsense(USBHMassStorageLUNDriver *lunp) {

-	scsi_sense_response_t sense;

-	msd_result_t res;

-

-	res = scsi_requestsense(lunp, &sense);

-	if (res.tres != MSD_TRANSACTIONRESULT_OK) {

-		uerr("\tREQUEST SENSE: Transaction error");

-		goto failed;

-	} else if (res.cres == MSD_COMMANDRESULT_FAILED) {

-		uerr("\tREQUEST SENSE: Command Failed");

-		goto failed;

-	} else if (res.cres == MSD_COMMANDRESULT_PHASE_ERROR) {

-		//TODO: Do reset, etc.

-		uerr("\tREQUEST SENSE: Command Phase Error");

-		goto failed;

-	}

-

-	uerrf("\tREQUEST SENSE: Sense key=%x, ASC=%02x, ASCQ=%02x",

-			sense.byte[2] & 0xf, sense.byte[12], sense.byte[13]);

-

-	return (sense.byte[2] & 0xf) | (sense.byte[12] << 8) | (sense.byte[13] << 16);

-

-failed:

-	return 0xffffffff;

-}

-

-void usbhmsdLUNObjectInit(USBHMassStorageLUNDriver *lunp) {

-	osalDbgCheck(lunp != NULL);

-	memset(lunp, 0, sizeof(*lunp));

-	lunp->vmt = &blk_vmt;

-	lunp->state = BLK_STOP;

-	/* Unnecessary because of the memset:

-		lunp->msdp = NULL;

-		lunp->next = NULL;

-		lunp->info.* = 0;

-	*/

-}

-

-void usbhmsdLUNStart(USBHMassStorageLUNDriver *lunp) {

-	osalDbgCheck(lunp != NULL);

-	osalSysLock();

-	osalDbgAssert((lunp->state == BLK_STOP) || (lunp->state == BLK_ACTIVE),

-			"invalid state");

-	//TODO: complete

-	//lunp->state = BLK_ACTIVE;

-	osalSysUnlock();

-}

-

-void usbhmsdLUNStop(USBHMassStorageLUNDriver *lunp) {

-	osalDbgCheck(lunp != NULL);

-	osalSysLock();

-	osalDbgAssert((lunp->state == BLK_STOP) || (lunp->state == BLK_ACTIVE),

-			"invalid state");

-	//TODO: complete

-	//lunp->state = BLK_STOP;

-	osalSysUnlock();

-}

-

-bool usbhmsdLUNConnect(USBHMassStorageLUNDriver *lunp) {

-	USBHMassStorageDriver *const msdp = lunp->msdp;

-	msd_result_t res;

-

-	osalDbgCheck(msdp != NULL);

-	osalSysLock();

-	//osalDbgAssert((lunp->state == BLK_ACTIVE) || (lunp->state == BLK_READY),

-    //            "invalid state");

-	if (lunp->state == BLK_READY) {

-	    osalSysUnlock();

-		return HAL_SUCCESS;

-	} else if (lunp->state != BLK_ACTIVE) {

-		osalSysUnlock();

-		return HAL_FAILED;

-	}

-	lunp->state = BLK_CONNECTING;

-    osalSysUnlock();

-

-    osalMutexLock(&msdp->mtx);

-

-    USBH_DEFINE_BUFFER(union {

-    		scsi_inquiry_response_t inq;

-    		scsi_readcapacity10_response_t cap;	}, u);

-

-	uinfo("INQUIRY...");

-	res = scsi_inquiry(lunp, &u.inq);

-	if (res.tres != MSD_TRANSACTIONRESULT_OK) {

-		uerr("\tINQUIRY: Transaction error");

-		goto failed;

-	} else if (res.cres == MSD_COMMANDRESULT_FAILED) {

-		uerr("\tINQUIRY: Command Failed");

-		_requestsense(lunp);

-		goto failed;

-	} else if (res.cres == MSD_COMMANDRESULT_PHASE_ERROR) {

-		//TODO: Do reset, etc.

-		uerr("\tINQUIRY: Command Phase Error");

-		goto failed;

-	}

-

-	uinfof("\tPDT=%02x", u.inq.peripheral & 0x1f);

-	if (u.inq.peripheral != 0) {

-		uerr("\tUnsupported PDT");

-		goto failed;

-	}

-

-	// Test if unit ready

-	uint8_t i;

-	for (i = 0; i < 10; i++) {

-		uinfo("TEST UNIT READY...");

-		res = scsi_testunitready(lunp);

-		if (res.tres != MSD_TRANSACTIONRESULT_OK) {

-			uerr("\tTEST UNIT READY: Transaction error");

-			goto failed;

-		} else if (res.cres == MSD_COMMANDRESULT_FAILED) {

-			uerr("\tTEST UNIT READY: Command Failed");

-			_requestsense(lunp);

-			continue;

-		} else if (res.cres == MSD_COMMANDRESULT_PHASE_ERROR) {

-			//TODO: Do reset, etc.

-			uerr("\tTEST UNIT READY: Command Phase Error");

-			goto failed;

-		}

-		uinfo("\tReady.");

-		break;

-		// osalThreadSleepMilliseconds(200); // will raise 'code is unreachable' warning

-	}

-	if (i == 10) goto failed;

-

-	// Read capacity

-	uinfo("READ CAPACITY(10)...");

-	res = scsi_readcapacity10(lunp, &u.cap);

-	if (res.tres != MSD_TRANSACTIONRESULT_OK) {

-		uerr("\tREAD CAPACITY(10): Transaction error");

-		goto failed;

-	} else if (res.cres == MSD_COMMANDRESULT_FAILED) {

-		uerr("\tREAD CAPACITY(10): Command Failed");

-		_requestsense(lunp);

-		goto failed;

-	} else if (res.cres == MSD_COMMANDRESULT_PHASE_ERROR) {

-		//TODO: Do reset, etc.

-		uerr("\tREAD CAPACITY(10): Command Phase Error");

-		goto failed;

-	}

-	lunp->info.blk_size = __REV(u.cap.block_size);

-	lunp->info.blk_num = __REV(u.cap.last_block_addr) + 1;

-	uinfof("\tBlock size=%dbytes, blocks=%u (~%u MB)", lunp->info.blk_size, lunp->info.blk_num,

-		(uint32_t)(((uint64_t)lunp->info.blk_size * lunp->info.blk_num) / (1024UL * 1024UL)));

-

-	uinfo("MSD Connected.");

-

-	osalMutexUnlock(&msdp->mtx);

-	osalSysLock();

-	lunp->state = BLK_READY;

-    osalSysUnlock();

-

-	return HAL_SUCCESS;

-

-  /* Connection failed, state reset to BLK_ACTIVE.*/

-failed:

-	osalMutexUnlock(&msdp->mtx);

-	osalSysLock();

-	lunp->state = BLK_ACTIVE;

-    osalSysUnlock();

-	return HAL_FAILED;

-}

-

-

-bool usbhmsdLUNDisconnect(USBHMassStorageLUNDriver *lunp) {

-	osalDbgCheck(lunp != NULL);

-	osalSysLock();

-	osalDbgAssert((lunp->state == BLK_ACTIVE) || (lunp->state == BLK_READY),

-				"invalid state");

-	if (lunp->state == BLK_ACTIVE) {

-		osalSysUnlock();

-		return HAL_SUCCESS;

-	}

-	lunp->state = BLK_DISCONNECTING;

-	osalSysUnlock();

-

-	//TODO: complete

-

-	osalSysLock();

-	lunp->state = BLK_ACTIVE;

-	osalSysUnlock();

-	return HAL_SUCCESS;

-}

-

-bool usbhmsdLUNRead(USBHMassStorageLUNDriver *lunp, uint32_t startblk,

-                uint8_t *buffer, uint32_t n) {

-

-	osalDbgCheck(lunp != NULL);

-	bool ret = HAL_FAILED;

-	uint16_t blocks;

-	msd_result_t res;

-

-	osalSysLock();

-	if (lunp->state != BLK_READY) {

-		osalSysUnlock();

-		return ret;

-	}

-	lunp->state = BLK_READING;

-    osalSysUnlock();

-

-	osalMutexLock(&lunp->msdp->mtx);

-	while (n) {

-		if (n > 0xffff) {

-			blocks = 0xffff;

-		} else {

-			blocks = (uint16_t)n;

-		}

-		res = scsi_read10(lunp, startblk, blocks, buffer);

-		if (res.tres != MSD_TRANSACTIONRESULT_OK) {

-			uerr("\tREAD (10): Transaction error");

-			goto exit;

-		} else if (res.cres == MSD_COMMANDRESULT_FAILED) {

-			//TODO: request sense, and act appropriately

-			uerr("\tREAD (10): Command Failed");

-			_requestsense(lunp);

-			goto exit;

-		} else if (res.cres == MSD_COMMANDRESULT_PHASE_ERROR) {

-			//TODO: Do reset, etc.

-			uerr("\tREAD (10): Command Phase Error");

-			goto exit;

-		}

-		n -= blocks;

-		startblk += blocks;

-		buffer += blocks * lunp->info.blk_size;

-	}

-

-	ret = HAL_SUCCESS;

-

-exit:

-	osalMutexUnlock(&lunp->msdp->mtx);

-	osalSysLock();

-	if (lunp->state == BLK_READING) {

-		lunp->state = BLK_READY;

-	} else {

-		osalDbgCheck(lunp->state == BLK_STOP);

-		uwarn("MSD: State = BLK_STOP");

-	}

-    osalSysUnlock();

-	return ret;

-}

-

-bool usbhmsdLUNWrite(USBHMassStorageLUNDriver *lunp, uint32_t startblk,

-                const uint8_t *buffer, uint32_t n) {

-

-	osalDbgCheck(lunp != NULL);

-	bool ret = HAL_FAILED;

-	uint16_t blocks;

-	msd_result_t res;

-

-	osalSysLock();

-	if (lunp->state != BLK_READY) {

-		osalSysUnlock();

-		return ret;

-	}

-	lunp->state = BLK_WRITING;

-    osalSysUnlock();

-

-	osalMutexLock(&lunp->msdp->mtx);

-	while (n) {

-		if (n > 0xffff) {

-			blocks = 0xffff;

-		} else {

-			blocks = (uint16_t)n;

-		}

-		res = scsi_write10(lunp, startblk, blocks, buffer);

-		if (res.tres != MSD_TRANSACTIONRESULT_OK) {

-			uerr("\tWRITE (10): Transaction error");

-			goto exit;

-		} else if (res.cres == MSD_COMMANDRESULT_FAILED) {

-			//TODO: request sense, and act appropriately

-			uerr("\tWRITE (10): Command Failed");

-			_requestsense(lunp);

-			goto exit;

-		} else if (res.cres == MSD_COMMANDRESULT_PHASE_ERROR) {

-			//TODO: Do reset, etc.

-			uerr("\tWRITE (10): Command Phase Error");

-			goto exit;

-		}

-		n -= blocks;

-		startblk += blocks;

-		buffer += blocks * lunp->info.blk_size;

-	}

-

-	ret = HAL_SUCCESS;

-

-exit:

-	osalMutexUnlock(&lunp->msdp->mtx);

-	osalSysLock();

-	if (lunp->state == BLK_WRITING) {

-		lunp->state = BLK_READY;

-	} else {

-		osalDbgCheck(lunp->state == BLK_STOP);

-		uwarn("MSD: State = BLK_STOP");

-	}

-	osalSysUnlock();

-	return ret;

-}

-

-bool usbhmsdLUNSync(USBHMassStorageLUNDriver *lunp) {

-	osalDbgCheck(lunp != NULL);

-	(void)lunp;

-	//TODO: Do SCSI Sync

-	return HAL_SUCCESS;

-}

-

-bool usbhmsdLUNGetInfo(USBHMassStorageLUNDriver *lunp, BlockDeviceInfo *bdip) {

-	osalDbgCheck(lunp != NULL);

-	osalDbgCheck(bdip != NULL);

-	*bdip = lunp->info;

-	return HAL_SUCCESS;

-}

-

-bool usbhmsdLUNIsInserted(USBHMassStorageLUNDriver *lunp) {

-	osalDbgCheck(lunp != NULL);

-	blkstate_t state;

-	osalSysLock();

-	state = lunp->state;

-	osalSysUnlock();

-	return (state >= BLK_ACTIVE);

-}

-

-bool usbhmsdLUNIsProtected(USBHMassStorageLUNDriver *lunp) {

-	osalDbgCheck(lunp != NULL);

-	return FALSE;

-}

-

-void usbhmsdObjectInit(USBHMassStorageDriver *msdp) {

-	osalDbgCheck(msdp != NULL);

-	memset(msdp, 0, sizeof(*msdp));

-	msdp->info = &usbhmsdClassDriverInfo;

-	osalMutexObjectInit(&msdp->mtx);

-}

-

-#endif

+/*
+    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio
+              Copyright (C) 2015 Diego Ismirlian, TISA, (dismirlian (at) google's mail)
+
+    Licensed under the Apache License, Version 2.0 (the "License");
+    you may not use this file except in compliance with the License.
+    You may obtain a copy of the License at
+
+        http://www.apache.org/licenses/LICENSE-2.0
+
+    Unless required by applicable law or agreed to in writing, software
+    distributed under the License is distributed on an "AS IS" BASIS,
+    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+    See the License for the specific language governing permissions and
+    limitations under the License.
+*/
+
+#include "hal.h"
+#include "hal_usbh.h"
+
+#if HAL_USBH_USE_MSD
+
+#if !HAL_USE_USBH
+#error "USBHMSD needs USBH"
+#endif
+
+#include <string.h>
+#include "usbh/dev/msd.h"
+#include "usbh/internal.h"
+
+//#pragma GCC optimize("Og")
+
+
+#if USBHMSD_DEBUG_ENABLE_TRACE
+#define udbgf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)
+#define udbg(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)
+#else
+#define udbgf(f, ...)  do {} while(0)
+#define udbg(f, ...)   do {} while(0)
+#endif
+
+#if USBHMSD_DEBUG_ENABLE_INFO
+#define uinfof(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)
+#define uinfo(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)
+#else
+#define uinfof(f, ...)  do {} while(0)
+#define uinfo(f, ...)   do {} while(0)
+#endif
+
+#if USBHMSD_DEBUG_ENABLE_WARNINGS
+#define uwarnf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)
+#define uwarn(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)
+#else
+#define uwarnf(f, ...)  do {} while(0)
+#define uwarn(f, ...)   do {} while(0)
+#endif
+
+#if USBHMSD_DEBUG_ENABLE_ERRORS
+#define uerrf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)
+#define uerr(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)
+#else
+#define uerrf(f, ...)  do {} while(0)
+#define uerr(f, ...)   do {} while(0)
+#endif
+
+
+
+
+
+/*===========================================================================*/
+/* USB Class driver loader for MSD								 		 	 */
+/*===========================================================================*/
+
+USBHMassStorageDriver USBHMSD[HAL_USBHMSD_MAX_INSTANCES];
+
+static usbh_baseclassdriver_t *_msd_load(usbh_device_t *dev, const uint8_t *descriptor, uint16_t rem);
+static void _msd_unload(usbh_baseclassdriver_t *drv);
+
+static const usbh_classdriver_vmt_t class_driver_vmt = {
+	_msd_load,
+	_msd_unload
+};
+
+const usbh_classdriverinfo_t usbhmsdClassDriverInfo = {
+	0x08, 0x06, 0x50, "MSD", &class_driver_vmt
+};
+
+#define MSD_REQ_RESET							0xFF
+#define MSD_GET_MAX_LUN							0xFE
+
+static usbh_baseclassdriver_t *_msd_load(usbh_device_t *dev, const uint8_t *descriptor, uint16_t rem) {
+	int i;
+	USBHMassStorageDriver *msdp;
+	uint8_t luns; // should declare it here to eliminate 'control bypass initialization' warning
+	usbh_urbstatus_t stat;  // should declare it here to eliminate 'control bypass initialization' warning
+
+	if ((rem < descriptor[0]) || (descriptor[1] != USBH_DT_INTERFACE))
+		return NULL;
+
+	const usbh_interface_descriptor_t * const ifdesc = (const usbh_interface_descriptor_t *)descriptor;
+
+	if ((ifdesc->bAlternateSetting != 0)
+			|| (ifdesc->bNumEndpoints < 2)
+			|| (ifdesc->bInterfaceSubClass != 0x06)
+			|| (ifdesc->bInterfaceProtocol != 0x50)) {
+		return NULL;
+	}
+
+	/* alloc driver */
+	for (i = 0; i < HAL_USBHMSD_MAX_INSTANCES; i++) {
+		if (USBHMSD[i].dev == NULL) {
+			msdp = &USBHMSD[i];
+			goto alloc_ok;
+		}
+	}
+
+	uwarn("Can't alloc MSD driver");
+
+	/* can't alloc */
+	return NULL;
+
+alloc_ok:
+	/* initialize the driver's variables */
+	msdp->epin.status = USBH_EPSTATUS_UNINITIALIZED;
+	msdp->epout.status = USBH_EPSTATUS_UNINITIALIZED;
+	msdp->max_lun = 0;
+	msdp->tag = 0;
+	msdp->luns = 0;
+	msdp->ifnum = ifdesc->bInterfaceNumber;
+	usbhEPSetName(&dev->ctrl, "MSD[CTRL]");
+
+	/* parse the configuration descriptor */
+	if_iterator_t iif;
+	generic_iterator_t iep;
+	iif.iad = 0;
+	iif.curr = descriptor;
+	iif.rem = rem;
+	for (ep_iter_init(&iep, &iif); iep.valid; ep_iter_next(&iep)) {
+		const usbh_endpoint_descriptor_t *const epdesc = ep_get(&iep);
+		if ((epdesc->bEndpointAddress & 0x80) && (epdesc->bmAttributes == USBH_EPTYPE_BULK)) {
+			uinfof("BULK IN endpoint found: bEndpointAddress=%02x", epdesc->bEndpointAddress);
+			usbhEPObjectInit(&msdp->epin, dev, epdesc);
+			usbhEPSetName(&msdp->epin, "MSD[BIN ]");
+		} else if (((epdesc->bEndpointAddress & 0x80) == 0)
+				&& (epdesc->bmAttributes == USBH_EPTYPE_BULK)) {
+			uinfof("BULK OUT endpoint found: bEndpointAddress=%02x", epdesc->bEndpointAddress);
+			usbhEPObjectInit(&msdp->epout, dev, epdesc);
+			usbhEPSetName(&msdp->epout, "MSD[BOUT]");
+		} else {
+			uinfof("unsupported endpoint found: bEndpointAddress=%02x, bmAttributes=%02x",
+					epdesc->bEndpointAddress, epdesc->bmAttributes);
+		}
+	}
+	if ((msdp->epin.status != USBH_EPSTATUS_CLOSED) || (msdp->epout.status != USBH_EPSTATUS_CLOSED)) {
+		goto deinit;
+	}
+
+	/* read the number of LUNs */
+	uinfo("Reading Max LUN:");
+	USBH_DEFINE_BUFFER(uint8_t, buff[4]);
+	stat = usbhControlRequest(dev,
+			USBH_CLASSIN(USBH_REQTYPE_INTERFACE, MSD_GET_MAX_LUN, 0, msdp->ifnum),
+			1, buff);
+	if (stat == USBH_URBSTATUS_OK) {
+		msdp->max_lun = buff[0] + 1;
+		uinfof("\tmax_lun = %d", msdp->max_lun);
+		if (msdp->max_lun > HAL_USBHMSD_MAX_LUNS) {
+			msdp->max_lun = HAL_USBHMSD_MAX_LUNS;
+			uwarnf("\tUsing max_lun = %d", msdp->max_lun);
+		}
+	} else if (stat == USBH_URBSTATUS_STALL) {
+		uwarn("\tStall, max_lun = 1");
+		msdp->max_lun = 1;
+	} else {
+		uerr("\tError");
+		goto deinit;
+	}
+
+	/* open the bulk IN/OUT endpoints */
+	usbhEPOpen(&msdp->epin);
+	usbhEPOpen(&msdp->epout);
+
+	/* Alloc one block device per logical unit found */
+	luns = msdp->max_lun;
+	for (i = 0; (luns > 0) && (i < HAL_USBHMSD_MAX_LUNS); i++) {
+		if (MSBLKD[i].msdp == NULL) {
+			/* link the new block driver to the list */
+			MSBLKD[i].next = msdp->luns;
+			msdp->luns = &MSBLKD[i];
+			MSBLKD[i].msdp = msdp;
+
+			osalSysLock();
+			MSBLKD[i].state = BLK_ACTIVE;	/* transition directly to active, instead of BLK_STOP */
+			osalSysUnlock();
+
+			/* connect the LUN (TODO: review if it's best to leave the LUN disconnected) */
+			usbhmsdLUNConnect(&MSBLKD[i]);
+			luns--;
+		}
+	}
+
+	return (usbh_baseclassdriver_t *)msdp;
+
+deinit:
+	/* Here, the enpoints are closed, and the driver is unlinked */
+	return NULL;
+}
+
+static void _msd_unload(usbh_baseclassdriver_t *drv) {
+	osalDbgCheck(drv != NULL);
+	USBHMassStorageDriver *const msdp = (USBHMassStorageDriver *)drv;
+	USBHMassStorageLUNDriver *lunp = msdp->luns;
+
+	osalMutexLock(&msdp->mtx);
+	osalSysLock();
+	usbhEPCloseS(&msdp->epin);
+	usbhEPCloseS(&msdp->epout);
+	while (lunp) {
+		lunp->state = BLK_STOP;
+		lunp = lunp->next;
+	}
+	osalSysUnlock();
+	osalMutexUnlock(&msdp->mtx);
+
+	/* now that the LUNs are idle, deinit them */
+	lunp = msdp->luns;
+	osalSysLock();
+	while (lunp) {
+		usbhmsdLUNObjectInit(lunp);
+		lunp = lunp->next;
+	}
+	osalSysUnlock();
+}
+
+
+/*===========================================================================*/
+/* MSD Class driver operations (Bulk-Only transport)			 		 	 */
+/*===========================================================================*/
+
+
+
+/* USB Bulk Only Transport SCSI Command block wrapper */
+PACKED_STRUCT {
+	uint32_t dCBWSignature;
+	uint32_t dCBWTag;
+	uint32_t dCBWDataTransferLength;
+	uint8_t bmCBWFlags;
+	uint8_t bCBWLUN;
+	uint8_t bCBWCBLength;
+	uint8_t CBWCB[16];
+} msd_cbw_t;
+#define MSD_CBW_SIGNATURE						0x43425355
+#define MSD_CBWFLAGS_D2H						0x80
+#define MSD_CBWFLAGS_H2D						0x00
+
+
+/* USB Bulk Only Transport SCSI Command status wrapper */
+PACKED_STRUCT {
+	uint32_t dCSWSignature;
+	uint32_t dCSWTag;
+	uint32_t dCSWDataResidue;
+	uint8_t bCSWStatus;
+} msd_csw_t;
+#define MSD_CSW_SIGNATURE						0x53425355
+
+
+typedef union {
+	msd_cbw_t cbw;
+	msd_csw_t csw;
+} msd_transaction_t;
+
+typedef enum {
+	MSD_TRANSACTIONRESULT_OK,
+	MSD_TRANSACTIONRESULT_DISCONNECTED,
+	MSD_TRANSACTIONRESULT_STALL,
+	MSD_TRANSACTIONRESULT_BUS_ERROR,
+	MSD_TRANSACTIONRESULT_SYNC_ERROR
+} msd_transaction_result_t;
+
+typedef enum {
+	MSD_COMMANDRESULT_PASSED = 0,
+	MSD_COMMANDRESULT_FAILED = 1,
+	MSD_COMMANDRESULT_PHASE_ERROR = 2
+} msd_command_result_t;
+
+typedef struct {
+	msd_transaction_result_t tres;
+	msd_command_result_t cres;
+} msd_result_t;
+
+
+/* ----------------------------------------------------- */
+/* 			SCSI Commands 								*/
+/* ----------------------------------------------------- */
+
+/* Read 10 and Write 10 */
+#define SCSI_CMD_READ_10 						0x28
+#define SCSI_CMD_WRITE_10						0x2A
+
+/* Request sense */
+#define SCSI_CMD_REQUEST_SENSE 					0x03
+PACKED_STRUCT {
+	uint8_t byte[18];
+} scsi_sense_response_t;
+
+#define SCSI_SENSE_KEY_GOOD                     0x00
+#define SCSI_SENSE_KEY_RECOVERED_ERROR          0x01
+#define SCSI_SENSE_KEY_NOT_READY                0x02
+#define SCSI_SENSE_KEY_MEDIUM_ERROR             0x03
+#define SCSI_SENSE_KEY_HARDWARE_ERROR           0x04
+#define SCSI_SENSE_KEY_ILLEGAL_REQUEST          0x05
+#define SCSI_SENSE_KEY_UNIT_ATTENTION           0x06
+#define SCSI_SENSE_KEY_DATA_PROTECT             0x07
+#define SCSI_SENSE_KEY_BLANK_CHECK              0x08
+#define SCSI_SENSE_KEY_VENDOR_SPECIFIC          0x09
+#define SCSI_SENSE_KEY_COPY_ABORTED             0x0A
+#define SCSI_SENSE_KEY_ABORTED_COMMAND          0x0B
+#define SCSI_SENSE_KEY_VOLUME_OVERFLOW          0x0D
+#define SCSI_SENSE_KEY_MISCOMPARE               0x0E
+#define SCSI_ASENSE_NO_ADDITIONAL_INFORMATION   0x00
+#define SCSI_ASENSE_LOGICAL_UNIT_NOT_READY      0x04
+#define SCSI_ASENSE_INVALID_FIELD_IN_CDB        0x24
+#define SCSI_ASENSE_NOT_READY_TO_READY_CHANGE   0x28
+#define SCSI_ASENSE_WRITE_PROTECTED             0x27
+#define SCSI_ASENSE_FORMAT_ERROR                0x31
+#define SCSI_ASENSE_INVALID_COMMAND             0x20
+#define SCSI_ASENSE_LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE 0x21
+#define SCSI_ASENSE_MEDIUM_NOT_PRESENT                 0x3A
+#define SCSI_ASENSEQ_NO_QUALIFIER                      0x00
+#define SCSI_ASENSEQ_FORMAT_COMMAND_FAILED             0x01
+#define SCSI_ASENSEQ_INITIALIZING_COMMAND_REQUIRED     0x02
+#define SCSI_ASENSEQ_OPERATION_IN_PROGRESS             0x07
+
+/* Inquiry */
+#define SCSI_CMD_INQUIRY 						0x12
+PACKED_STRUCT {
+	uint8_t peripheral;
+	uint8_t removable;
+	uint8_t version;
+	uint8_t response_data_format;
+	uint8_t additional_length;
+	uint8_t sccstp;
+	uint8_t bqueetc;
+	uint8_t cmdque;
+	uint8_t vendorID[8];
+	uint8_t productID[16];
+	uint8_t productRev[4];
+} scsi_inquiry_response_t;
+
+/* Read Capacity 10 */
+#define SCSI_CMD_READ_CAPACITY_10				0x25
+PACKED_STRUCT {
+	uint32_t last_block_addr;
+	uint32_t block_size;
+} scsi_readcapacity10_response_t;
+
+/* Start/Stop Unit */
+#define SCSI_CMD_START_STOP_UNIT				0x1B
+PACKED_STRUCT {
+	uint8_t op_code;
+	uint8_t lun_immed;
+	uint8_t res1;
+	uint8_t res2;
+	uint8_t loej_start;
+	uint8_t control;
+} scsi_startstopunit_request_t;
+
+/* test unit ready */
+#define SCSI_CMD_TEST_UNIT_READY				0x00
+
+/* Other commands, TODO: use or remove them
+#define SCSI_CMD_PREVENT_ALLOW_MEDIUM_REMOVAL	0x1E
+#define SCSI_CMD_VERIFY_10						0x2F
+#define SCSI_CMD_SEND_DIAGNOSTIC				0x1D
+#define SCSI_CMD_MODE_SENSE_6                   0x1A
+*/
+
+static inline void _prepare_cbw(msd_transaction_t *tran, USBHMassStorageLUNDriver *lunp) {
+	tran->cbw.bCBWLUN = (uint8_t)(lunp - &lunp->msdp->luns[0]);
+	memset(&tran->cbw.CBWCB, 0, sizeof(tran->cbw.CBWCB));
+}
+
+static msd_transaction_result_t _msd_transaction(msd_transaction_t *tran, USBHMassStorageLUNDriver *lunp, void *data) {
+
+	uint32_t actual_len;
+	usbh_urbstatus_t status;
+
+	tran->cbw.dCBWSignature = MSD_CBW_SIGNATURE;
+	tran->cbw.dCBWTag = ++lunp->msdp->tag;
+
+	/* control phase */
+	status = usbhBulkTransfer(&lunp->msdp->epout, &tran->cbw,
+					sizeof(tran->cbw), &actual_len, MS2ST(1000));
+
+	if (status == USBH_URBSTATUS_CANCELLED) {
+		uerr("\tMSD: Control phase: USBH_URBSTATUS_CANCELLED");
+		return MSD_TRANSACTIONRESULT_DISCONNECTED;
+	} else if (status == USBH_URBSTATUS_STALL) {
+		uerr("\tMSD: Control phase: USBH_URBSTATUS_STALL");
+		return MSD_TRANSACTIONRESULT_STALL;
+	} else if (status != USBH_URBSTATUS_OK) {
+		uerrf("\tMSD: Control phase: status = %d, != OK", status);
+		return MSD_TRANSACTIONRESULT_BUS_ERROR;
+	} else if (actual_len != sizeof(tran->cbw)) {
+		uerrf("\tMSD: Control phase: wrong actual_len = %d", actual_len);
+		return MSD_TRANSACTIONRESULT_BUS_ERROR;
+	}
+
+
+	/* data phase */
+	if (tran->cbw.dCBWDataTransferLength) {
+		status = usbhBulkTransfer(
+				tran->cbw.bmCBWFlags & MSD_CBWFLAGS_D2H ? &lunp->msdp->epin : &lunp->msdp->epout,
+				data,
+				tran->cbw.dCBWDataTransferLength,
+				&actual_len, MS2ST(20000));
+
+		if (status == USBH_URBSTATUS_CANCELLED) {
+			uerr("\tMSD: Data phase: USBH_URBSTATUS_CANCELLED");
+			return MSD_TRANSACTIONRESULT_DISCONNECTED;
+		} else if (status == USBH_URBSTATUS_STALL) {
+			uerr("\tMSD: Data phase: USBH_URBSTATUS_STALL");
+			return MSD_TRANSACTIONRESULT_STALL;
+		} else if (status != USBH_URBSTATUS_OK) {
+			uerrf("\tMSD: Data phase: status = %d, != OK", status);
+			return MSD_TRANSACTIONRESULT_BUS_ERROR;
+		} else if (actual_len != tran->cbw.dCBWDataTransferLength) {
+			uerrf("\tMSD: Data phase: wrong actual_len = %d", actual_len);
+			return MSD_TRANSACTIONRESULT_BUS_ERROR;
+		}
+	}
+
+
+	/* status phase */
+	status = usbhBulkTransfer(&lunp->msdp->epin, &tran->csw,
+			sizeof(tran->csw), &actual_len, MS2ST(1000));
+
+	if (status == USBH_URBSTATUS_CANCELLED) {
+		uerr("\tMSD: Status phase: USBH_URBSTATUS_CANCELLED");
+		return MSD_TRANSACTIONRESULT_DISCONNECTED;
+	} else if (status == USBH_URBSTATUS_STALL) {
+		uerr("\tMSD: Status phase: USBH_URBSTATUS_STALL");
+		return MSD_TRANSACTIONRESULT_STALL;
+	} else if (status != USBH_URBSTATUS_OK) {
+		uerrf("\tMSD: Status phase: status = %d, != OK", status);
+		return MSD_TRANSACTIONRESULT_BUS_ERROR;
+	} else if (actual_len != sizeof(tran->csw)) {
+		uerrf("\tMSD: Status phase: wrong actual_len = %d", actual_len);
+		return MSD_TRANSACTIONRESULT_BUS_ERROR;
+	} else if (tran->csw.dCSWSignature != MSD_CSW_SIGNATURE) {
+		uerr("\tMSD: Status phase: wrong signature");
+		return MSD_TRANSACTIONRESULT_BUS_ERROR;
+	} else if (tran->csw.dCSWTag != lunp->msdp->tag) {
+		uerrf("\tMSD: Status phase: wrong tag (expected %d, got %d)",
+				lunp->msdp->tag, tran->csw.dCSWTag);
+		return MSD_TRANSACTIONRESULT_SYNC_ERROR;
+	}
+
+	if (tran->csw.dCSWDataResidue) {
+		uwarnf("\tMSD: Residue=%d", tran->csw.dCSWDataResidue);
+	}
+
+	return MSD_TRANSACTIONRESULT_OK;
+}
+
+
+static msd_result_t scsi_inquiry(USBHMassStorageLUNDriver *lunp, scsi_inquiry_response_t *resp) {
+	msd_transaction_t transaction;
+	msd_result_t res;
+
+	_prepare_cbw(&transaction, lunp);
+	transaction.cbw.dCBWDataTransferLength = sizeof(scsi_inquiry_response_t);
+	transaction.cbw.bmCBWFlags = MSD_CBWFLAGS_D2H;
+	transaction.cbw.bCBWCBLength = 6;
+	transaction.cbw.CBWCB[0] = SCSI_CMD_INQUIRY;
+	transaction.cbw.CBWCB[4] = sizeof(scsi_inquiry_response_t);
+
+	res.tres = _msd_transaction(&transaction, lunp, resp);
+	if (res.tres == MSD_TRANSACTIONRESULT_OK) {
+		res.cres = (msd_command_result_t) transaction.csw.bCSWStatus;
+	}
+	return res;
+}
+
+static msd_result_t scsi_requestsense(USBHMassStorageLUNDriver *lunp, scsi_sense_response_t *resp) {
+	msd_transaction_t transaction;
+	msd_result_t res;
+
+	_prepare_cbw(&transaction, lunp);
+	transaction.cbw.dCBWDataTransferLength = sizeof(scsi_sense_response_t);
+	transaction.cbw.bmCBWFlags = MSD_CBWFLAGS_D2H;
+	transaction.cbw.bCBWCBLength = 12;
+	transaction.cbw.CBWCB[0] = SCSI_CMD_REQUEST_SENSE;
+	transaction.cbw.CBWCB[4] = sizeof(scsi_sense_response_t);
+
+	res.tres = _msd_transaction(&transaction, lunp, resp);
+	if (res.tres == MSD_TRANSACTIONRESULT_OK) {
+		res.cres = (msd_command_result_t) transaction.csw.bCSWStatus;
+	}
+	return res;
+}
+
+static msd_result_t scsi_testunitready(USBHMassStorageLUNDriver *lunp) {
+	msd_transaction_t transaction;
+	msd_result_t res;
+
+	_prepare_cbw(&transaction, lunp);
+	transaction.cbw.dCBWDataTransferLength = 0;
+	transaction.cbw.bmCBWFlags = MSD_CBWFLAGS_D2H;
+	transaction.cbw.bCBWCBLength = 6;
+	transaction.cbw.CBWCB[0] = SCSI_CMD_TEST_UNIT_READY;
+
+	res.tres = _msd_transaction(&transaction, lunp, NULL);
+	if (res.tres == MSD_TRANSACTIONRESULT_OK) {
+		res.cres = (msd_command_result_t) transaction.csw.bCSWStatus;
+	}
+	return res;
+}
+
+static msd_result_t scsi_readcapacity10(USBHMassStorageLUNDriver *lunp, scsi_readcapacity10_response_t *resp) {
+	msd_transaction_t transaction;
+	msd_result_t res;
+
+	_prepare_cbw(&transaction, lunp);
+	transaction.cbw.dCBWDataTransferLength = sizeof(scsi_readcapacity10_response_t);
+	transaction.cbw.bmCBWFlags = MSD_CBWFLAGS_D2H;
+	transaction.cbw.bCBWCBLength = 12;
+	transaction.cbw.CBWCB[0] = SCSI_CMD_READ_CAPACITY_10;
+
+	res.tres = _msd_transaction(&transaction, lunp, resp);
+	if (res.tres == MSD_TRANSACTIONRESULT_OK) {
+		res.cres = (msd_command_result_t) transaction.csw.bCSWStatus;
+	}
+	return res;
+}
+
+
+static msd_result_t scsi_read10(USBHMassStorageLUNDriver *lunp, uint32_t lba, uint16_t n, uint8_t *data) {
+	msd_transaction_t transaction;
+	msd_result_t res;
+
+	_prepare_cbw(&transaction, lunp);
+	transaction.cbw.dCBWDataTransferLength = n * lunp->info.blk_size;
+	transaction.cbw.bmCBWFlags = MSD_CBWFLAGS_D2H;
+	transaction.cbw.bCBWCBLength = 10;
+	transaction.cbw.CBWCB[0] = SCSI_CMD_READ_10;
+	transaction.cbw.CBWCB[2] = (uint8_t)(lba >> 24);
+	transaction.cbw.CBWCB[3] = (uint8_t)(lba >> 16);
+	transaction.cbw.CBWCB[4] = (uint8_t)(lba >> 8);
+	transaction.cbw.CBWCB[5] = (uint8_t)(lba);
+	transaction.cbw.CBWCB[7] = (uint8_t)(n >> 8);
+	transaction.cbw.CBWCB[8] = (uint8_t)(n);
+
+	res.tres = _msd_transaction(&transaction, lunp, data);
+	if (res.tres == MSD_TRANSACTIONRESULT_OK) {
+		res.cres = (msd_command_result_t) transaction.csw.bCSWStatus;
+	}
+	return res;
+}
+
+static msd_result_t scsi_write10(USBHMassStorageLUNDriver *lunp, uint32_t lba, uint16_t n, const uint8_t *data) {
+	msd_transaction_t transaction;
+	msd_result_t res;
+
+	_prepare_cbw(&transaction, lunp);
+	transaction.cbw.dCBWDataTransferLength = n * lunp->info.blk_size;
+	transaction.cbw.bmCBWFlags = MSD_CBWFLAGS_H2D;
+	transaction.cbw.bCBWCBLength = 10;
+	transaction.cbw.CBWCB[0] = SCSI_CMD_WRITE_10;
+	transaction.cbw.CBWCB[2] = (uint8_t)(lba >> 24);
+	transaction.cbw.CBWCB[3] = (uint8_t)(lba >> 16);
+	transaction.cbw.CBWCB[4] = (uint8_t)(lba >> 8);
+	transaction.cbw.CBWCB[5] = (uint8_t)(lba);
+	transaction.cbw.CBWCB[7] = (uint8_t)(n >> 8);
+	transaction.cbw.CBWCB[8] = (uint8_t)(n);
+
+	res.tres = _msd_transaction(&transaction, lunp, (uint8_t *)data);
+	if (res.tres == MSD_TRANSACTIONRESULT_OK) {
+		res.cres = (msd_command_result_t) transaction.csw.bCSWStatus;
+	}
+	return res;
+}
+
+
+
+/*===========================================================================*/
+/* Block driver data/functions								 		 	 	 */
+/*===========================================================================*/
+
+USBHMassStorageLUNDriver MSBLKD[HAL_USBHMSD_MAX_LUNS];
+
+static const struct USBHMassStorageDriverVMT blk_vmt = {
+	(bool (*)(void *))usbhmsdLUNIsInserted,
+	(bool (*)(void *))usbhmsdLUNIsProtected,
+	(bool (*)(void *))usbhmsdLUNConnect,
+	(bool (*)(void *))usbhmsdLUNDisconnect,
+	(bool (*)(void *, uint32_t, uint8_t *, uint32_t))usbhmsdLUNRead,
+	(bool (*)(void *, uint32_t,	const uint8_t *, uint32_t))usbhmsdLUNWrite,
+	(bool (*)(void *))usbhmsdLUNSync,
+	(bool (*)(void *, BlockDeviceInfo *))usbhmsdLUNGetInfo
+};
+
+
+
+static uint32_t _requestsense(USBHMassStorageLUNDriver *lunp) {
+	scsi_sense_response_t sense;
+	msd_result_t res;
+
+	res = scsi_requestsense(lunp, &sense);
+	if (res.tres != MSD_TRANSACTIONRESULT_OK) {
+		uerr("\tREQUEST SENSE: Transaction error");
+		goto failed;
+	} else if (res.cres == MSD_COMMANDRESULT_FAILED) {
+		uerr("\tREQUEST SENSE: Command Failed");
+		goto failed;
+	} else if (res.cres == MSD_COMMANDRESULT_PHASE_ERROR) {
+		//TODO: Do reset, etc.
+		uerr("\tREQUEST SENSE: Command Phase Error");
+		goto failed;
+	}
+
+	uerrf("\tREQUEST SENSE: Sense key=%x, ASC=%02x, ASCQ=%02x",
+			sense.byte[2] & 0xf, sense.byte[12], sense.byte[13]);
+
+	return (sense.byte[2] & 0xf) | (sense.byte[12] << 8) | (sense.byte[13] << 16);
+
+failed:
+	return 0xffffffff;
+}
+
+void usbhmsdLUNObjectInit(USBHMassStorageLUNDriver *lunp) {
+	osalDbgCheck(lunp != NULL);
+	memset(lunp, 0, sizeof(*lunp));
+	lunp->vmt = &blk_vmt;
+	lunp->state = BLK_STOP;
+	/* Unnecessary because of the memset:
+		lunp->msdp = NULL;
+		lunp->next = NULL;
+		lunp->info.* = 0;
+	*/
+}
+
+void usbhmsdLUNStart(USBHMassStorageLUNDriver *lunp) {
+	osalDbgCheck(lunp != NULL);
+	osalSysLock();
+	osalDbgAssert((lunp->state == BLK_STOP) || (lunp->state == BLK_ACTIVE),
+			"invalid state");
+	//TODO: complete
+	//lunp->state = BLK_ACTIVE;
+	osalSysUnlock();
+}
+
+void usbhmsdLUNStop(USBHMassStorageLUNDriver *lunp) {
+	osalDbgCheck(lunp != NULL);
+	osalSysLock();
+	osalDbgAssert((lunp->state == BLK_STOP) || (lunp->state == BLK_ACTIVE),
+			"invalid state");
+	//TODO: complete
+	//lunp->state = BLK_STOP;
+	osalSysUnlock();
+}
+
+bool usbhmsdLUNConnect(USBHMassStorageLUNDriver *lunp) {
+	USBHMassStorageDriver *const msdp = lunp->msdp;
+	msd_result_t res;
+
+	osalDbgCheck(msdp != NULL);
+	osalSysLock();
+	//osalDbgAssert((lunp->state == BLK_ACTIVE) || (lunp->state == BLK_READY),
+    //            "invalid state");
+	if (lunp->state == BLK_READY) {
+	    osalSysUnlock();
+		return HAL_SUCCESS;
+	} else if (lunp->state != BLK_ACTIVE) {
+		osalSysUnlock();
+		return HAL_FAILED;
+	}
+	lunp->state = BLK_CONNECTING;
+    osalSysUnlock();
+
+    osalMutexLock(&msdp->mtx);
+
+    USBH_DEFINE_BUFFER(union {
+    		scsi_inquiry_response_t inq;
+    		scsi_readcapacity10_response_t cap;	}, u);
+
+	uinfo("INQUIRY...");
+	res = scsi_inquiry(lunp, &u.inq);
+	if (res.tres != MSD_TRANSACTIONRESULT_OK) {
+		uerr("\tINQUIRY: Transaction error");
+		goto failed;
+	} else if (res.cres == MSD_COMMANDRESULT_FAILED) {
+		uerr("\tINQUIRY: Command Failed");
+		_requestsense(lunp);
+		goto failed;
+	} else if (res.cres == MSD_COMMANDRESULT_PHASE_ERROR) {
+		//TODO: Do reset, etc.
+		uerr("\tINQUIRY: Command Phase Error");
+		goto failed;
+	}
+
+	uinfof("\tPDT=%02x", u.inq.peripheral & 0x1f);
+	if (u.inq.peripheral != 0) {
+		uerr("\tUnsupported PDT");
+		goto failed;
+	}
+
+	// Test if unit ready
+	uint8_t i;
+	for (i = 0; i < 10; i++) {
+		uinfo("TEST UNIT READY...");
+		res = scsi_testunitready(lunp);
+		if (res.tres != MSD_TRANSACTIONRESULT_OK) {
+			uerr("\tTEST UNIT READY: Transaction error");
+			goto failed;
+		} else if (res.cres == MSD_COMMANDRESULT_FAILED) {
+			uerr("\tTEST UNIT READY: Command Failed");
+			_requestsense(lunp);
+			continue;
+		} else if (res.cres == MSD_COMMANDRESULT_PHASE_ERROR) {
+			//TODO: Do reset, etc.
+			uerr("\tTEST UNIT READY: Command Phase Error");
+			goto failed;
+		}
+		uinfo("\tReady.");
+		break;
+		// osalThreadSleepMilliseconds(200); // will raise 'code is unreachable' warning
+	}
+	if (i == 10) goto failed;
+
+	// Read capacity
+	uinfo("READ CAPACITY(10)...");
+	res = scsi_readcapacity10(lunp, &u.cap);
+	if (res.tres != MSD_TRANSACTIONRESULT_OK) {
+		uerr("\tREAD CAPACITY(10): Transaction error");
+		goto failed;
+	} else if (res.cres == MSD_COMMANDRESULT_FAILED) {
+		uerr("\tREAD CAPACITY(10): Command Failed");
+		_requestsense(lunp);
+		goto failed;
+	} else if (res.cres == MSD_COMMANDRESULT_PHASE_ERROR) {
+		//TODO: Do reset, etc.
+		uerr("\tREAD CAPACITY(10): Command Phase Error");
+		goto failed;
+	}
+	lunp->info.blk_size = __REV(u.cap.block_size);
+	lunp->info.blk_num = __REV(u.cap.last_block_addr) + 1;
+	uinfof("\tBlock size=%dbytes, blocks=%u (~%u MB)", lunp->info.blk_size, lunp->info.blk_num,
+		(uint32_t)(((uint64_t)lunp->info.blk_size * lunp->info.blk_num) / (1024UL * 1024UL)));
+
+	uinfo("MSD Connected.");
+
+	osalMutexUnlock(&msdp->mtx);
+	osalSysLock();
+	lunp->state = BLK_READY;
+    osalSysUnlock();
+
+	return HAL_SUCCESS;
+
+  /* Connection failed, state reset to BLK_ACTIVE.*/
+failed:
+	osalMutexUnlock(&msdp->mtx);
+	osalSysLock();
+	lunp->state = BLK_ACTIVE;
+    osalSysUnlock();
+	return HAL_FAILED;
+}
+
+
+bool usbhmsdLUNDisconnect(USBHMassStorageLUNDriver *lunp) {
+	osalDbgCheck(lunp != NULL);
+	osalSysLock();
+	osalDbgAssert((lunp->state == BLK_ACTIVE) || (lunp->state == BLK_READY),
+				"invalid state");
+	if (lunp->state == BLK_ACTIVE) {
+		osalSysUnlock();
+		return HAL_SUCCESS;
+	}
+	lunp->state = BLK_DISCONNECTING;
+	osalSysUnlock();
+
+	//TODO: complete
+
+	osalSysLock();
+	lunp->state = BLK_ACTIVE;
+	osalSysUnlock();
+	return HAL_SUCCESS;
+}
+
+bool usbhmsdLUNRead(USBHMassStorageLUNDriver *lunp, uint32_t startblk,
+                uint8_t *buffer, uint32_t n) {
+
+	osalDbgCheck(lunp != NULL);
+	bool ret = HAL_FAILED;
+	uint16_t blocks;
+	msd_result_t res;
+
+	osalSysLock();
+	if (lunp->state != BLK_READY) {
+		osalSysUnlock();
+		return ret;
+	}
+	lunp->state = BLK_READING;
+    osalSysUnlock();
+
+	osalMutexLock(&lunp->msdp->mtx);
+	while (n) {
+		if (n > 0xffff) {
+			blocks = 0xffff;
+		} else {
+			blocks = (uint16_t)n;
+		}
+		res = scsi_read10(lunp, startblk, blocks, buffer);
+		if (res.tres != MSD_TRANSACTIONRESULT_OK) {
+			uerr("\tREAD (10): Transaction error");
+			goto exit;
+		} else if (res.cres == MSD_COMMANDRESULT_FAILED) {
+			//TODO: request sense, and act appropriately
+			uerr("\tREAD (10): Command Failed");
+			_requestsense(lunp);
+			goto exit;
+		} else if (res.cres == MSD_COMMANDRESULT_PHASE_ERROR) {
+			//TODO: Do reset, etc.
+			uerr("\tREAD (10): Command Phase Error");
+			goto exit;
+		}
+		n -= blocks;
+		startblk += blocks;
+		buffer += blocks * lunp->info.blk_size;
+	}
+
+	ret = HAL_SUCCESS;
+
+exit:
+	osalMutexUnlock(&lunp->msdp->mtx);
+	osalSysLock();
+	if (lunp->state == BLK_READING) {
+		lunp->state = BLK_READY;
+	} else {
+		osalDbgCheck(lunp->state == BLK_STOP);
+		uwarn("MSD: State = BLK_STOP");
+	}
+    osalSysUnlock();
+	return ret;
+}
+
+bool usbhmsdLUNWrite(USBHMassStorageLUNDriver *lunp, uint32_t startblk,
+                const uint8_t *buffer, uint32_t n) {
+
+	osalDbgCheck(lunp != NULL);
+	bool ret = HAL_FAILED;
+	uint16_t blocks;
+	msd_result_t res;
+
+	osalSysLock();
+	if (lunp->state != BLK_READY) {
+		osalSysUnlock();
+		return ret;
+	}
+	lunp->state = BLK_WRITING;
+    osalSysUnlock();
+
+	osalMutexLock(&lunp->msdp->mtx);
+	while (n) {
+		if (n > 0xffff) {
+			blocks = 0xffff;
+		} else {
+			blocks = (uint16_t)n;
+		}
+		res = scsi_write10(lunp, startblk, blocks, buffer);
+		if (res.tres != MSD_TRANSACTIONRESULT_OK) {
+			uerr("\tWRITE (10): Transaction error");
+			goto exit;
+		} else if (res.cres == MSD_COMMANDRESULT_FAILED) {
+			//TODO: request sense, and act appropriately
+			uerr("\tWRITE (10): Command Failed");
+			_requestsense(lunp);
+			goto exit;
+		} else if (res.cres == MSD_COMMANDRESULT_PHASE_ERROR) {
+			//TODO: Do reset, etc.
+			uerr("\tWRITE (10): Command Phase Error");
+			goto exit;
+		}
+		n -= blocks;
+		startblk += blocks;
+		buffer += blocks * lunp->info.blk_size;
+	}
+
+	ret = HAL_SUCCESS;
+
+exit:
+	osalMutexUnlock(&lunp->msdp->mtx);
+	osalSysLock();
+	if (lunp->state == BLK_WRITING) {
+		lunp->state = BLK_READY;
+	} else {
+		osalDbgCheck(lunp->state == BLK_STOP);
+		uwarn("MSD: State = BLK_STOP");
+	}
+	osalSysUnlock();
+	return ret;
+}
+
+bool usbhmsdLUNSync(USBHMassStorageLUNDriver *lunp) {
+	osalDbgCheck(lunp != NULL);
+	(void)lunp;
+	//TODO: Do SCSI Sync
+	return HAL_SUCCESS;
+}
+
+bool usbhmsdLUNGetInfo(USBHMassStorageLUNDriver *lunp, BlockDeviceInfo *bdip) {
+	osalDbgCheck(lunp != NULL);
+	osalDbgCheck(bdip != NULL);
+	*bdip = lunp->info;
+	return HAL_SUCCESS;
+}
+
+bool usbhmsdLUNIsInserted(USBHMassStorageLUNDriver *lunp) {
+	osalDbgCheck(lunp != NULL);
+	blkstate_t state;
+	osalSysLock();
+	state = lunp->state;
+	osalSysUnlock();
+	return (state >= BLK_ACTIVE);
+}
+
+bool usbhmsdLUNIsProtected(USBHMassStorageLUNDriver *lunp) {
+	osalDbgCheck(lunp != NULL);
+	return FALSE;
+}
+
+void usbhmsdObjectInit(USBHMassStorageDriver *msdp) {
+	osalDbgCheck(msdp != NULL);
+	memset(msdp, 0, sizeof(*msdp));
+	msdp->info = &usbhmsdClassDriverInfo;
+	osalMutexObjectInit(&msdp->mtx);
+}
+
+#endif
diff --git a/os/hal/src/usbh/usbh_uvc.c b/os/hal/src/usbh/hal_usbh_uvc.c
index 2fb2563..09a0f1d 100644
--- a/os/hal/src/usbh/usbh_uvc.c
+++ b/os/hal/src/usbh/hal_usbh_uvc.c
@@ -1,89 +1,89 @@
-/*

-    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio

-              Copyright (C) 2015 Diego Ismirlian, TISA, (dismirlian (at) google's mail)

-

-    Licensed under the Apache License, Version 2.0 (the "License");

-    you may not use this file except in compliance with the License.

-    You may obtain a copy of the License at

-

-        http://www.apache.org/licenses/LICENSE-2.0

-

-    Unless required by applicable law or agreed to in writing, software

-    distributed under the License is distributed on an "AS IS" BASIS,

-    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.

-    See the License for the specific language governing permissions and

-    limitations under the License.

-*/

-

-#include "hal.h"

-#include "usbh.h"

-

-#if HAL_USBH_USE_UVC

-

-#if !HAL_USE_USBH

-#error "USBHUVC needs HAL_USE_USBH"

-#endif

-

-#if !HAL_USBH_USE_IAD

-#error "USBHUVC needs HAL_USBH_USE_IAD"

-#endif

-

-#if USBHUVC_DEBUG_ENABLE_TRACE

-#define udbgf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)

-#define udbg(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)

-#else

-#define udbgf(f, ...)  do {} while(0)

-#define udbg(f, ...)   do {} while(0)

-#endif

-

-#if USBHUVC_DEBUG_ENABLE_INFO

-#define uinfof(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)

-#define uinfo(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)

-#else

-#define uinfof(f, ...)  do {} while(0)

-#define uinfo(f, ...)   do {} while(0)

-#endif

-

-#if USBHUVC_DEBUG_ENABLE_WARNINGS

-#define uwarnf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)

-#define uwarn(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)

-#else

-#define uwarnf(f, ...)  do {} while(0)

-#define uwarn(f, ...)   do {} while(0)

-#endif

-

-#if USBHUVC_DEBUG_ENABLE_ERRORS

-#define uerrf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)

-#define uerr(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)

-#else

-#define uerrf(f, ...)  do {} while(0)

-#define uerr(f, ...)   do {} while(0)

-#endif

-

-

-static usbh_baseclassdriver_t *uvc_load(usbh_device_t *dev,

-		const uint8_t *descriptor, uint16_t rem);

-static void uvc_unload(usbh_baseclassdriver_t *drv);

-

-static const usbh_classdriver_vmt_t class_driver_vmt = {

-	uvc_load,

-	uvc_unload

-};

-const usbh_classdriverinfo_t usbhuvcClassDriverInfo = {

-	0x0e, 0x03, 0x00, "UVC", &class_driver_vmt

-};

-

-

-static usbh_baseclassdriver_t *uvc_load(usbh_device_t *dev, const uint8_t *descriptor, uint16_t rem) {

-	(void)dev;

-	(void)descriptor;

-	(void)rem;

-	return NULL;

-}

-

-static void uvc_unload(usbh_baseclassdriver_t *drv) {

-	(void)drv;

-}

-

-#endif

-

+/*
+    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio
+              Copyright (C) 2015 Diego Ismirlian, TISA, (dismirlian (at) google's mail)
+
+    Licensed under the Apache License, Version 2.0 (the "License");
+    you may not use this file except in compliance with the License.
+    You may obtain a copy of the License at
+
+        http://www.apache.org/licenses/LICENSE-2.0
+
+    Unless required by applicable law or agreed to in writing, software
+    distributed under the License is distributed on an "AS IS" BASIS,
+    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+    See the License for the specific language governing permissions and
+    limitations under the License.
+*/
+
+#include "hal.h"
+#include "hal_usbh.h"
+
+#if HAL_USBH_USE_UVC
+
+#if !HAL_USE_USBH
+#error "USBHUVC needs HAL_USE_USBH"
+#endif
+
+#if !HAL_USBH_USE_IAD
+#error "USBHUVC needs HAL_USBH_USE_IAD"
+#endif
+
+#if USBHUVC_DEBUG_ENABLE_TRACE
+#define udbgf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)
+#define udbg(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)
+#else
+#define udbgf(f, ...)  do {} while(0)
+#define udbg(f, ...)   do {} while(0)
+#endif
+
+#if USBHUVC_DEBUG_ENABLE_INFO
+#define uinfof(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)
+#define uinfo(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)
+#else
+#define uinfof(f, ...)  do {} while(0)
+#define uinfo(f, ...)   do {} while(0)
+#endif
+
+#if USBHUVC_DEBUG_ENABLE_WARNINGS
+#define uwarnf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)
+#define uwarn(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)
+#else
+#define uwarnf(f, ...)  do {} while(0)
+#define uwarn(f, ...)   do {} while(0)
+#endif
+
+#if USBHUVC_DEBUG_ENABLE_ERRORS
+#define uerrf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)
+#define uerr(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)
+#else
+#define uerrf(f, ...)  do {} while(0)
+#define uerr(f, ...)   do {} while(0)
+#endif
+
+
+static usbh_baseclassdriver_t *uvc_load(usbh_device_t *dev,
+		const uint8_t *descriptor, uint16_t rem);
+static void uvc_unload(usbh_baseclassdriver_t *drv);
+
+static const usbh_classdriver_vmt_t class_driver_vmt = {
+	uvc_load,
+	uvc_unload
+};
+const usbh_classdriverinfo_t usbhuvcClassDriverInfo = {
+	0x0e, 0x03, 0x00, "UVC", &class_driver_vmt
+};
+
+
+static usbh_baseclassdriver_t *uvc_load(usbh_device_t *dev, const uint8_t *descriptor, uint16_t rem) {
+	(void)dev;
+	(void)descriptor;
+	(void)rem;
+	return NULL;
+}
+
+static void uvc_unload(usbh_baseclassdriver_t *drv) {
+	(void)drv;
+}
+
+#endif
+