USB-Host: Initial commit

8 files changed, 4147 insertions, 0 deletions

diff --git a/os/hal/src/hal_community.c b/os/hal/src/hal_community.c
index a24d26e..a05e70f 100644
--- a/os/hal/src/hal_community.c
+++ b/os/hal/src/hal_community.c
@@ -64,6 +64,10 @@ void halCommunityInit(void) {
 #if HAL_USE_CRC || defined(__DOXYGEN__)

   crcInit();

 #endif

+

+#if HAL_USE_USBH || defined(__DOXYGEN__)

+  usbhInit();

+#endif

 }

 

 #endif /* HAL_USE_COMMUNITY */

diff --git a/os/hal/src/usbh.c b/os/hal/src/usbh.c
new file mode 100644
index 0000000..3bdbac3
--- /dev/null
+++ b/os/hal/src/usbh.c
@@ -0,0 +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

+

diff --git a/os/hal/src/usbh/usbh_debug.c b/os/hal/src/usbh/usbh_debug.c
new file mode 100644
index 0000000..63505aa
--- /dev/null
+++ b/os/hal/src/usbh/usbh_debug.c
@@ -0,0 +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

diff --git a/os/hal/src/usbh/usbh_desciter.c b/os/hal/src/usbh/usbh_desciter.c
new file mode 100644
index 0000000..80e4728
--- /dev/null
+++ b/os/hal/src/usbh/usbh_desciter.c
@@ -0,0 +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

diff --git a/os/hal/src/usbh/usbh_ftdi.c b/os/hal/src/usbh/usbh_ftdi.c
new file mode 100644
index 0000000..cdf3410
--- /dev/null
+++ b/os/hal/src/usbh/usbh_ftdi.c
@@ -0,0 +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

diff --git a/os/hal/src/usbh/usbh_hub.c b/os/hal/src/usbh/usbh_hub.c
new file mode 100644
index 0000000..eb585d2
--- /dev/null
+++ b/os/hal/src/usbh/usbh_hub.c
@@ -0,0 +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

diff --git a/os/hal/src/usbh/usbh_msd.c b/os/hal/src/usbh/usbh_msd.c
new file mode 100644
index 0000000..8db68a4
--- /dev/null
+++ b/os/hal/src/usbh/usbh_msd.c
@@ -0,0 +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

diff --git a/os/hal/src/usbh/usbh_uvc.c b/os/hal/src/usbh/usbh_uvc.c
new file mode 100644
index 0000000..2fb2563
--- /dev/null
+++ b/os/hal/src/usbh/usbh_uvc.c
@@ -0,0 +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

+