Merge branch 'master' into mike

9 files changed, 2361 insertions, 615 deletions

diff --git a/os/hal/src/usbh/TODO.txt b/os/hal/src/usbh/TODO.txt
new file mode 100644
index 0000000..87269be
--- /dev/null
+++ b/os/hal/src/usbh/TODO.txt
@@ -0,0 +1,21 @@
+In decreasing order of priority:
+
+Bugs:
+- Synchronization on driver unload between usbhMainLoop and driver APIs
+    - MSD: ok
+    - AOA: not done
+    - HUB: ok
+    - FTDI: not done
+    - HID: ok
+    - UVC: not done
+
+
+Enhancements:
+- Way to return error from the load() functions in order to stop the enumeration process
+- Event sources from the low-level driver, in order to know when to call usbhMainLoop (from the low-level driver and from the HUB driver status callback)
+- Possibility of internal main loop
+- Linked list for drivers for dynamic registration
+- A way to automate matching (similar to linux)
+- Hooks to override driver loading and to inform the user of problems
+- for STM32 LLD: think of a way to prevent Bulk IN NAK interrupt flood.
+- Integrate VBUS power switching functionality to the API.
diff --git a/os/hal/src/usbh/hal_usbh_aoa.c b/os/hal/src/usbh/hal_usbh_aoa.c
new file mode 100644
index 0000000..d565595
--- /dev/null
+++ b/os/hal/src/usbh/hal_usbh_aoa.c
@@ -0,0 +1,671 @@
+/*
+    ChibiOS - Copyright (C) 2006..2017 Giovanni Di Sirio
+              Copyright (C) 2015..2017 Diego Ismirlian, (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_USBH_USE_AOA
+
+#if !HAL_USE_USBH
+#error "USBHAOA needs USBH"
+#endif
+
+#include <string.h>
+#include "usbh/dev/aoa.h"
+#include "usbh/internal.h"
+
+//#pragma GCC optimize("Og")
+
+
+#if USBHAOA_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 USBHAOA_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 USBHAOA_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 USBHAOA_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
+
+
+/*===========================================================================*/
+/* Constants													 		 	 */
+/*===========================================================================*/
+
+#if !defined(HAL_USBHAOA_DEFAULT_MANUFACTURER)
+#define HAL_USBHAOA_DEFAULT_MANUFACTURER   "ChibiOS"
+#endif
+
+#if !defined(HAL_USBHAOA_DEFAULT_MODEL)
+#define HAL_USBHAOA_DEFAULT_MODEL          "USBH AOA Driver"
+#endif
+
+#if !defined(HAL_USBHAOA_DEFAULT_DESCRIPTION)
+#define HAL_USBHAOA_DEFAULT_DESCRIPTION    "ChibiOS USBH AOA Driver"
+#endif
+
+#if !defined(HAL_USBHAOA_DEFAULT_VERSION)
+#define HAL_USBHAOA_DEFAULT_VERSION        CH_KERNEL_VERSION
+#endif
+
+#if !defined(HAL_USBHAOA_DEFAULT_URI)
+#define HAL_USBHAOA_DEFAULT_URI            NULL
+#endif
+
+#if !defined(HAL_USBHAOA_DEFAULT_SERIAL)
+#define HAL_USBHAOA_DEFAULT_SERIAL         NULL
+#endif
+
+#if !defined(HAL_USBHAOA_DEFAULT_AUDIO_MODE)
+#define HAL_USBHAOA_DEFAULT_AUDIO_MODE	   USBHAOA_AUDIO_MODE_DISABLED
+#endif
+
+#define AOA_GOOGLE_VID 	        			0x18D1
+#define AOA_GOOGLE_PID_ACCESSORY    		0x2D00
+#define AOA_GOOGLE_PID_ACCESSORY_ABD    	0x2D01
+#define AOA_GOOGLE_PID_AUDIO    			0x2D02
+#define AOA_GOOGLE_PID_AUDIO_ABD    		0x2D03
+#define AOA_GOOGLE_PID_ACCESSORY_AUDIO    	0x2D04
+#define AOA_GOOGLE_PID_ACCESSORY_AUDIO_ABD  0x2D05
+
+#define AOA_ACCESSORY_GET_PROTOCOL          51
+#define AOA_ACCESSORY_SEND_STRING           52
+#define AOA_ACCESSORY_START                 53
+
+#define AOA_SET_AUDIO_MODE					58
+
+static bool _get_protocol(usbh_device_t *dev, uint16_t *protocol);
+static bool _accessory_start(usbh_device_t *dev);
+static bool _set_audio_mode(usbh_device_t *dev, uint16_t mode);
+static bool _send_string(usbh_device_t *dev, uint8_t index, const char *string);
+
+
+static void _stop_channelS(USBHAOAChannel *aoacp);
+
+/*===========================================================================*/
+/* USB Class driver loader for AOA								 		 	 */
+/*===========================================================================*/
+USBHAOADriver USBHAOAD[HAL_USBHAOA_MAX_INSTANCES];
+
+static usbh_baseclassdriver_t *_aoa_load(usbh_device_t *dev, const uint8_t *descriptor, uint16_t rem);
+static void _aoa_unload(usbh_baseclassdriver_t *drv);
+static void _aoa_init(void);
+
+static const usbh_classdriver_vmt_t class_driver_vmt = {
+	_aoa_init,
+	_aoa_load,
+	_aoa_unload
+};
+
+const usbh_classdriverinfo_t usbhaoaClassDriverInfo = {
+	"AOA", &class_driver_vmt
+};
+
+#if defined(HAL_USBHAOA_FILTER_CALLBACK)
+extern usbhaoa_filter_callback_t HAL_USBHAOA_FILTER_CALLBACK;
+#endif
+
+static usbh_baseclassdriver_t *_aoa_load(usbh_device_t *dev, const uint8_t *descriptor, uint16_t rem) {
+	int i;
+	USBHAOADriver *aoap;
+
+	if (dev->devDesc.idVendor != AOA_GOOGLE_VID) {
+		uint16_t protocol;
+		static USBHAOAConfig config = {
+			{
+				HAL_USBHAOA_DEFAULT_MANUFACTURER,
+				HAL_USBHAOA_DEFAULT_MODEL,
+				HAL_USBHAOA_DEFAULT_DESCRIPTION,
+				HAL_USBHAOA_DEFAULT_VERSION,
+				HAL_USBHAOA_DEFAULT_URI,
+				HAL_USBHAOA_DEFAULT_SERIAL
+			},
+
+			{
+				HAL_USBHAOA_DEFAULT_AUDIO_MODE,
+			}
+		};
+
+		uinfo("AOA: Unrecognized VID");
+
+#if defined(HAL_USBHAOA_FILTER_CALLBACK)
+		if (!HAL_USBHAOA_FILTER_CALLBACK(dev, descriptor, rem, &config)) {
+			return NULL;
+		}
+#endif
+
+		uinfo("AOA: Try if it's an Android device");
+		if (_get_protocol(dev, &protocol) != HAL_SUCCESS) {
+			return NULL;
+		}
+		uinfof("AOA: Possible Android device found (protocol=%d)", protocol);
+
+		if (config.channel.manufacturer != NULL) {
+			if ((_send_string(dev, USBHAOA_ACCESSORY_STRING_MANUFACTURER, config.channel.manufacturer) != HAL_SUCCESS)
+				|| (_send_string(dev, USBHAOA_ACCESSORY_STRING_MODEL, config.channel.model) != HAL_SUCCESS)
+				|| (_send_string(dev, USBHAOA_ACCESSORY_STRING_DESCRIPTION, config.channel.description) != HAL_SUCCESS)
+				|| (_send_string(dev, USBHAOA_ACCESSORY_STRING_VERSION, config.channel.version) != HAL_SUCCESS)
+				|| (_send_string(dev, USBHAOA_ACCESSORY_STRING_URI, config.channel.uri) != HAL_SUCCESS)
+				|| (_send_string(dev, USBHAOA_ACCESSORY_STRING_SERIAL, config.channel.serial) != HAL_SUCCESS)) {
+				uerr("AOA: Can't send string; abort start");
+				return NULL;
+			}
+		}
+
+		if (protocol > 1) {
+			if (_set_audio_mode(dev, (uint16_t)(config.audio.mode)) != HAL_SUCCESS) {
+				uerr("AOA: Can't set audio mode; abort channel start");
+				return NULL;
+			}
+		}
+
+		if (_accessory_start(dev) != HAL_SUCCESS) {
+			uerr("AOA: Can't start accessory; abort channel start");
+		}
+
+		return NULL;
+	}
+
+	/* AOAv2:
+		0x2D00	accessory				Provides two bulk endpoints for communicating with an Android application.
+		0x2D01	accessory + adb			For debugging purposes during accessory development. Available only if the user has enabled USB Debugging in the Android device settings.
+		0x2D02	audio					For streaming audio from an Android device to an accessory.
+		0x2D03	audio + adb
+		0x2D04	accessory + audio
+		0x2D05  accessory + audio + adb
+	*/
+
+	switch (dev->devDesc.idProduct) {
+	case AOA_GOOGLE_PID_ACCESSORY:
+	case AOA_GOOGLE_PID_ACCESSORY_ABD:
+//	case AOA_GOOGLE_PID_AUDIO:
+//	case AOA_GOOGLE_PID_AUDIO_ABD:
+	case AOA_GOOGLE_PID_ACCESSORY_AUDIO:
+	case AOA_GOOGLE_PID_ACCESSORY_AUDIO_ABD:
+		break;
+	default:
+		uerr("AOA: Unrecognized PID");
+		return NULL;
+	}
+
+	const usbh_interface_descriptor_t * const ifdesc = (const usbh_interface_descriptor_t *)descriptor;
+	if ((_usbh_match_descriptor(descriptor, rem, USBH_DT_INTERFACE, 0xFF, 0xFF, 0x00) != HAL_SUCCESS)
+			|| (ifdesc->bNumEndpoints < 2)) {
+		uerr("AOA: This IF is not the Accessory IF");
+		return NULL;
+	}
+
+	uinfof("AOA: Found Accessory Interface #%d", ifdesc->bInterfaceNumber);
+
+	for (i = 0; i < HAL_USBHAOA_MAX_INSTANCES; i++) {
+		if (USBHAOAD[i].dev == NULL) {
+			aoap = &USBHAOAD[i];
+			goto alloc_ok;
+		}
+	}
+
+	uwarn("AOA: Can't alloc driver");
+
+	/* can't alloc */
+	return NULL;
+
+alloc_ok:
+	/* initialize the driver's variables */
+	usbhEPSetName(&dev->ctrl, "AOA[CTRL]");
+	aoap->state = USBHAOA_STATE_ACTIVE;
+
+	generic_iterator_t iep;
+	if_iterator_t iif;
+	iif.iad = 0;
+	iif.curr = descriptor;
+	iif.rem = rem;
+
+	aoap->channel.epin.status = USBH_EPSTATUS_UNINITIALIZED;
+	aoap->channel.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("AOA: BULK IN endpoint found: bEndpointAddress=%02x", epdesc->bEndpointAddress);
+			usbhEPObjectInit(&aoap->channel.epin, dev, epdesc);
+			usbhEPSetName(&aoap->channel.epin, "AOA[BIN ]");
+		} else if (((epdesc->bEndpointAddress & 0x80) == 0)
+				&& (epdesc->bmAttributes == USBH_EPTYPE_BULK)) {
+			uinfof("AOA: BULK OUT endpoint found: bEndpointAddress=%02x", epdesc->bEndpointAddress);
+			usbhEPObjectInit(&aoap->channel.epout, dev, epdesc);
+			usbhEPSetName(&aoap->channel.epout, "AOA[BOUT]");
+		} else {
+			uinfof("AOA: unsupported endpoint found: bEndpointAddress=%02x, bmAttributes=%02x",
+				epdesc->bEndpointAddress, epdesc->bmAttributes);
+		}
+	}
+
+	if ((aoap->channel.epin.status != USBH_EPSTATUS_CLOSED)
+		|| (aoap->channel.epout.status != USBH_EPSTATUS_CLOSED)) {
+		uwarn("AOA: Couldn't find endpoints");
+		aoap->state = USBHAOA_STATE_STOP;
+		return NULL;
+	}
+
+	aoap->state = USBHAOA_STATE_READY;
+	aoap->channel.state = USBHAOA_CHANNEL_STATE_ACTIVE;
+	uwarn("AOA: Ready");
+	return (usbh_baseclassdriver_t *)aoap;
+}
+
+static void _aoa_unload(usbh_baseclassdriver_t *drv) {
+	osalDbgCheck(drv != NULL);
+	USBHAOADriver *const aoap = (USBHAOADriver *)drv;
+	osalSysLock();
+	_stop_channelS(&aoap->channel);
+	aoap->channel.state = USBHAOA_CHANNEL_STATE_STOP;
+	aoap->state = USBHAOA_STATE_STOP;
+	osalSysUnlock();
+}
+
+/* ------------------------------------ */
+/*      Accessory data channel          */
+/* ------------------------------------ */
+
+static void _submitOutI(USBHAOAChannel *aoacp, uint32_t len) {
+	udbgf("AOA: Submit OUT %d", len);
+	aoacp->oq_urb.requestedLength = len;
+	usbhURBObjectResetI(&aoacp->oq_urb);
+	usbhURBSubmitI(&aoacp->oq_urb);
+}
+
+static void _out_cb(usbh_urb_t *urb) {
+	USBHAOAChannel *const aoacp = (USBHAOAChannel *)urb->userData;
+	switch (urb->status) {
+	case USBH_URBSTATUS_OK:
+		aoacp->oq_ptr = aoacp->oq_buff;
+		aoacp->oq_counter = 64;
+		chThdDequeueNextI(&aoacp->oq_waiting, Q_OK);
+		chnAddFlagsI(aoacp, CHN_OUTPUT_EMPTY | CHN_TRANSMISSION_END);
+		return;
+	case USBH_URBSTATUS_DISCONNECTED:
+		uwarn("AOA: URB OUT disconnected");
+		chThdDequeueNextI(&aoacp->oq_waiting, Q_RESET);
+		chnAddFlagsI(aoacp, CHN_OUTPUT_EMPTY);
+		return;
+	default:
+		uerrf("AOA: URB OUT status unexpected = %d", urb->status);
+		break;
+	}
+	usbhURBObjectResetI(&aoacp->oq_urb);
+	usbhURBSubmitI(&aoacp->oq_urb);
+}
+
+static size_t _write_timeout(USBHAOAChannel *aoacp, const uint8_t *bp,
+		size_t n, systime_t timeout) {
+	chDbgCheck(n > 0U);
+
+	size_t w = 0;
+	osalSysLock();
+	while (true) {
+		if (aoacp->state != USBHAOA_CHANNEL_STATE_READY) {
+			osalSysUnlock();
+			return w;
+		}
+		while (usbhURBIsBusy(&aoacp->oq_urb)) {
+			if (chThdEnqueueTimeoutS(&aoacp->oq_waiting, timeout) != Q_OK) {
+				osalSysUnlock();
+				return w;
+			}
+		}
+
+		*aoacp->oq_ptr++ = *bp++;
+		if (--aoacp->oq_counter == 0) {
+			_submitOutI(aoacp, 64);
+			osalOsRescheduleS();
+		}
+		osalSysUnlock(); /* Gives a preemption chance in a controlled point.*/
+
+		w++;
+		if (--n == 0U)
+			return w;
+
+		osalSysLock();
+	}
+}
+
+static msg_t _put_timeout(USBHAOAChannel *aoacp, uint8_t b, systime_t timeout) {
+
+	osalSysLock();
+	if (aoacp->state != USBHAOA_CHANNEL_STATE_READY) {
+		osalSysUnlock();
+		return Q_RESET;
+	}
+
+	while (usbhURBIsBusy(&aoacp->oq_urb)) {
+		msg_t msg = chThdEnqueueTimeoutS(&aoacp->oq_waiting, timeout);
+		if (msg < Q_OK) {
+			osalSysUnlock();
+			return msg;
+		}
+	}
+
+	*aoacp->oq_ptr++ = b;
+	if (--aoacp->oq_counter == 0) {
+		_submitOutI(aoacp, 64);
+		osalOsRescheduleS();
+	}
+	osalSysUnlock();
+	return Q_OK;
+}
+
+static size_t _write(USBHAOAChannel *aoacp, const uint8_t *bp, size_t n) {
+	return _write_timeout(aoacp, bp, n, TIME_INFINITE);
+}
+
+static msg_t _put(USBHAOAChannel *aoacp, uint8_t b) {
+	return _put_timeout(aoacp, b, TIME_INFINITE);
+}
+
+static void _submitInI(USBHAOAChannel *aoacp) {
+	udbg("AOA: Submit IN");
+	usbhURBObjectResetI(&aoacp->iq_urb);
+	usbhURBSubmitI(&aoacp->iq_urb);
+}
+
+static void _in_cb(usbh_urb_t *urb) {
+	USBHAOAChannel *const aoacp = (USBHAOAChannel *)urb->userData;
+	switch (urb->status) {
+	case USBH_URBSTATUS_OK:
+		if (urb->actualLength == 0) {
+			udbgf("AOA: URB IN no data");
+		} else {
+			udbgf("AOA: URB IN data len=%d", urb->actualLength);
+			aoacp->iq_ptr = aoacp->iq_buff;
+			aoacp->iq_counter = urb->actualLength;
+			chThdDequeueNextI(&aoacp->iq_waiting, Q_OK);
+			chnAddFlagsI(aoacp, CHN_INPUT_AVAILABLE);
+		}
+		break;
+	case USBH_URBSTATUS_DISCONNECTED:
+		uwarn("AOA: URB IN disconnected");
+		chThdDequeueNextI(&aoacp->iq_waiting, Q_RESET);
+		break;
+	default:
+		uerrf("AOA: URB IN status unexpected = %d", urb->status);
+		_submitInI(aoacp);
+		break;
+	}
+}
+
+static size_t _read_timeout(USBHAOAChannel *aoacp, uint8_t *bp,
+		size_t n, systime_t timeout) {
+	size_t r = 0;
+
+	chDbgCheck(n > 0U);
+
+	osalSysLock();
+	while (true) {
+		if (aoacp->state != USBHAOA_CHANNEL_STATE_READY) {
+			osalSysUnlock();
+			return r;
+		}
+		while (aoacp->iq_counter == 0) {
+			if (!usbhURBIsBusy(&aoacp->iq_urb))
+				_submitInI(aoacp);
+			if (chThdEnqueueTimeoutS(&aoacp->iq_waiting, timeout) != Q_OK) {
+				osalSysUnlock();
+				return r;
+			}
+		}
+		*bp++ = *aoacp->iq_ptr++;
+		if (--aoacp->iq_counter == 0) {
+			_submitInI(aoacp);
+			osalOsRescheduleS();
+		}
+		osalSysUnlock();
+
+		r++;
+		if (--n == 0U)
+			return r;
+
+		osalSysLock();
+	}
+}
+
+static msg_t _get_timeout(USBHAOAChannel *aoacp, systime_t timeout) {
+	uint8_t b;
+
+	osalSysLock();
+	if (aoacp->state != USBHAOA_CHANNEL_STATE_READY) {
+		osalSysUnlock();
+		return Q_RESET;
+	}
+	while (aoacp->iq_counter == 0) {
+		if (!usbhURBIsBusy(&aoacp->iq_urb))
+			_submitInI(aoacp);
+		msg_t msg = chThdEnqueueTimeoutS(&aoacp->iq_waiting, timeout);
+		if (msg < Q_OK) {
+			osalSysUnlock();
+			return msg;
+		}
+	}
+	b = *aoacp->iq_ptr++;
+	if (--aoacp->iq_counter == 0) {
+		_submitInI(aoacp);
+		osalOsRescheduleS();
+	}
+	osalSysUnlock();
+
+	return (msg_t)b;
+}
+
+static msg_t _get(USBHAOAChannel *aoacp) {
+	return _get_timeout(aoacp, TIME_INFINITE);
+}
+
+static size_t _read(USBHAOAChannel *aoacp, uint8_t *bp, size_t n) {
+	return _read_timeout(aoacp, bp, n, TIME_INFINITE);
+}
+
+static const struct AOADriverVMT 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_channelS(USBHAOAChannel *aoacp) {
+	if (aoacp->state != USBHAOA_CHANNEL_STATE_READY)
+		return;
+	uwarn("AOA: Stop channel");
+	chVTResetI(&aoacp->vt);
+	usbhEPCloseS(&aoacp->epin);
+	usbhEPCloseS(&aoacp->epout);
+	chThdDequeueAllI(&aoacp->iq_waiting, Q_RESET);
+	chThdDequeueAllI(&aoacp->oq_waiting, Q_RESET);
+	chnAddFlagsI(aoacp, CHN_DISCONNECTED);
+	aoacp->state = USBHAOA_CHANNEL_STATE_ACTIVE;
+	osalOsRescheduleS();
+}
+
+static void _vt(void *p) {
+	USBHAOAChannel *const aoacp = (USBHAOAChannel *)p;
+	osalSysLockFromISR();
+	uint32_t len = aoacp->oq_ptr - aoacp->oq_buff;
+	if (len && !usbhURBIsBusy(&aoacp->oq_urb)) {
+		_submitOutI(aoacp, len);
+	}
+	if ((aoacp->iq_counter == 0) && !usbhURBIsBusy(&aoacp->iq_urb)) {
+		_submitInI(aoacp);
+	}
+	chVTSetI(&aoacp->vt, OSAL_MS2I(16), _vt, aoacp);
+	osalSysUnlockFromISR();
+}
+
+void usbhaoaChannelStart(USBHAOADriver *aoap) {
+
+	osalDbgCheck(aoap);
+
+	USBHAOAChannel *const aoacp = (USBHAOAChannel *)&aoap->channel;
+
+	osalDbgCheck(aoap->state == USBHAOA_STATE_READY);
+
+	osalDbgCheck((aoacp->state == USBHAOA_CHANNEL_STATE_ACTIVE)
+			|| (aoacp->state == USBHAOA_CHANNEL_STATE_READY));
+
+	if (aoacp->state == USBHAOA_CHANNEL_STATE_READY)
+		return;
+
+	usbhURBObjectInit(&aoacp->oq_urb, &aoacp->epout, _out_cb, aoacp, aoacp->oq_buff, 0);
+	chThdQueueObjectInit(&aoacp->oq_waiting);
+	aoacp->oq_counter = 64;
+	aoacp->oq_ptr = aoacp->oq_buff;
+	usbhEPOpen(&aoacp->epout);
+
+	usbhURBObjectInit(&aoacp->iq_urb, &aoacp->epin, _in_cb, aoacp, aoacp->iq_buff, 64);
+	chThdQueueObjectInit(&aoacp->iq_waiting);
+	aoacp->iq_counter = 0;
+	aoacp->iq_ptr = aoacp->iq_buff;
+	usbhEPOpen(&aoacp->epin);
+	usbhURBSubmit(&aoacp->iq_urb);
+
+	chVTObjectInit(&aoacp->vt);
+	chVTSet(&aoacp->vt, OSAL_MS2I(16), _vt, aoacp);
+
+	aoacp->state = USBHAOA_CHANNEL_STATE_READY;
+
+	osalEventBroadcastFlags(&aoacp->event, CHN_CONNECTED | CHN_OUTPUT_EMPTY);
+}
+
+void usbhaoaChannelStop(USBHAOADriver *aoap) {
+	osalDbgCheck((aoap->channel.state == USBHAOA_CHANNEL_STATE_ACTIVE)
+			|| (aoap->channel.state == USBHAOA_CHANNEL_STATE_READY));
+	osalSysLock();
+	_stop_channelS(&aoap->channel);
+	osalSysUnlock();
+}
+
+/* ------------------------------------ */
+/*      General AOA functions           */
+/* ------------------------------------ */
+static bool _get_protocol(usbh_device_t *dev, uint16_t *protocol) {
+	USBH_DEFINE_BUFFER(uint16_t proto);
+
+	usbh_urbstatus_t ret = usbhControlRequest(dev,
+			USBH_REQTYPE_DIR_IN | USBH_REQTYPE_TYPE_VENDOR | USBH_REQTYPE_RECIP_DEVICE,
+			AOA_ACCESSORY_GET_PROTOCOL,
+			0,
+			0,
+			2,
+			(uint8_t *)&proto);
+
+	if ((ret != USBH_URBSTATUS_OK) || (proto > 2))
+		return HAL_FAILED;
+
+	*protocol = proto;
+	return HAL_SUCCESS;
+}
+
+static bool _accessory_start(usbh_device_t *dev) {
+	usbh_urbstatus_t ret = usbhControlRequest(dev,
+			USBH_REQTYPE_DIR_OUT | USBH_REQTYPE_TYPE_VENDOR | USBH_REQTYPE_RECIP_DEVICE,
+			AOA_ACCESSORY_START,
+			0,
+			0,
+			0,
+			NULL);
+
+	if (ret != USBH_URBSTATUS_OK)
+		return HAL_FAILED;
+
+	return HAL_SUCCESS;
+}
+
+static bool _set_audio_mode(usbh_device_t *dev, uint16_t mode) {
+	usbh_urbstatus_t ret = usbhControlRequest(dev,
+			USBH_REQTYPE_DIR_OUT | USBH_REQTYPE_TYPE_VENDOR | USBH_REQTYPE_RECIP_DEVICE,
+			AOA_SET_AUDIO_MODE,
+			mode,
+			0,
+			0,
+			NULL);
+
+	if (ret != USBH_URBSTATUS_OK)
+		return HAL_FAILED;
+
+	return HAL_SUCCESS;
+}
+
+static bool _send_string(usbh_device_t *dev, uint8_t index, const char *string)
+{
+	USBH_DEFINE_BUFFER(const char nullstr[1]) = {0};
+	if (string == NULL)
+		string = nullstr;
+
+	usbh_urbstatus_t ret = usbhControlRequest(dev,
+			USBH_REQTYPE_DIR_OUT | USBH_REQTYPE_TYPE_VENDOR | USBH_REQTYPE_RECIP_DEVICE,
+			AOA_ACCESSORY_SEND_STRING,
+			0,
+			index,
+			strlen(string) + 1,
+			(uint8_t *)string);
+
+	if (ret != USBH_URBSTATUS_OK)
+		return HAL_FAILED;
+
+	return HAL_SUCCESS;
+}
+
+static void _object_init(USBHAOADriver *aoap) {
+	osalDbgCheck(aoap != NULL);
+	memset(aoap, 0, sizeof(*aoap));
+	aoap->info = &usbhaoaClassDriverInfo;
+	aoap->state = USBHAOA_STATE_STOP;
+	aoap->channel.vmt = &async_channel_vmt;
+	osalEventObjectInit(&aoap->channel.event);
+	aoap->channel.state = USBHAOA_CHANNEL_STATE_STOP;
+}
+
+static void _aoa_init(void) {
+	uint8_t i;
+	for (i = 0; i < HAL_USBHAOA_MAX_INSTANCES; i++) {
+		_object_init(&USBHAOAD[i]);
+	}
+}
+
+#endif
diff --git a/os/hal/src/usbh/hal_usbh_debug.c b/os/hal/src/usbh/hal_usbh_debug.c
index 9f17189..d32f1c6 100644
--- a/os/hal/src/usbh/hal_usbh_debug.c
+++ b/os/hal/src/usbh/hal_usbh_debug.c
@@ -1,6 +1,6 @@
 /*
-    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio
-              Copyright (C) 2015 Diego Ismirlian, TISA, (dismirlian (at) google's mail)
+    ChibiOS - Copyright (C) 2006..2017 Giovanni Di Sirio
+              Copyright (C) 2015..2017 Diego Ismirlian, (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.
@@ -17,15 +17,13 @@
 
 #include "hal.h"
 
-#if HAL_USE_USBH
+#if HAL_USE_USBH && USBH_DEBUG_ENABLE
 
 #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
@@ -108,17 +106,18 @@ static char *ftoa(char *p, double num, unsigned long precision, bool dot) {
 }
 #endif
 
-static inline void _put(char c) {
-	input_queue_t *iqp = &USBH_DEBUG_USBHD.iq;
-
-	if (chIQIsFullI(iqp))
-		return;
-
-	iqp->q_counter++;
+static inline void _wr(input_queue_t *iqp, char c) {
 	*iqp->q_wrptr++ = c;
 	if (iqp->q_wrptr >= iqp->q_top)
 		iqp->q_wrptr = iqp->q_buffer;
+}
 
+static inline void _put(char c) {
+	input_queue_t *iqp = &USBH_DEBUG_USBHD.iq;
+	if (sizeof(USBH_DEBUG_USBHD.dbg_buff) - iqp->q_counter <= 1)
+		return;
+	iqp->q_counter++;
+	_wr(iqp, c);
 }
 
 int _dbg_printf(const char *fmt, va_list ap) {
@@ -343,19 +342,39 @@ unsigned_common:
 
 }
 
-static void _print_hdr(void)
-{
+static systime_t first, last;
+static bool ena;
+static uint32_t hdr[2];
+
+static void _build_hdr(void) {
 	uint32_t hfnum = USBH_DEBUG_USBHD.otg->HFNUM;
 	uint16_t hfir = USBH_DEBUG_USBHD.otg->HFIR;
+	last = osalOsGetSystemTimeX();
+	if (ena) {
+		first = last;
+	}
 
-	_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);
+	if (((hfnum & 0x3fff) == 0x3fff) && (hfir == (hfnum >> 16))) {
+		hdr[0] = 0xfeff;
+		hdr[1] = last - first;
+		ena = FALSE;
+	} else {
+		hdr[0] = 0xffff | (hfir << 16);
+		hdr[1] = hfnum;
+		ena = TRUE;
+	}
+}
+
+static void _print_hdr(void)
+{
+	_put(hdr[0] & 0xff);
+	_put((hdr[0] >> 8) & 0xff);
+	_put((hdr[0] >> 16) & 0xff);
+	_put((hdr[0] >> 24) & 0xff);
+	_put(hdr[1] & 0xff);
+	_put((hdr[1] >> 8) & 0xff);
+	_put((hdr[1] >> 16) & 0xff);
+	_put((hdr[1] >> 24) & 0xff);
 }
 
 void usbDbgPrintf(const char *fmt, ...)
@@ -363,10 +382,16 @@ 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);
+	input_queue_t *iqp = &USBH_DEBUG_USBHD.iq;
+	int rem = sizeof(USBH_DEBUG_USBHD.dbg_buff) - iqp->q_counter;
+	if (rem >= 9) {
+		_build_hdr();
+		_print_hdr();
+		_dbg_printf(fmt, ap);
+		iqp->q_counter++;
+		_wr(iqp, 0);
+		chThdDequeueNextI(&USBH_DEBUG_USBHD.iq.q_waiting, Q_OK);
+	}
 	chSysRestoreStatusX(sts);
 	va_end(ap);
 }
@@ -374,32 +399,28 @@ void usbDbgPrintf(const char *fmt, ...)
 
 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;
+	_build_hdr();
+	uint8_t *p = (uint8_t *)hdr;
 	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;
+	if (rem >= 9) {
+		while (rem) {
+			_wr(iqp, *p);
+			if (++p == top) break;
+		}
+		rem -= 9;
+		while (rem && *s) {
+			_wr(iqp, *s);
+			rem--;
+			s++;
+		}
+		_wr(iqp, 0);
+		iqp->q_counter = sizeof(USBH_DEBUG_USBHD.dbg_buff) - rem;
+		chThdDequeueNextI(&USBH_DEBUG_USBHD.iq.q_waiting, Q_OK);
 	}
-	iqp->q_counter = sizeof(USBH_DEBUG_USBHD.dbg_buff) - rem;
-	chThdDequeueNextI(&USBH_DEBUG_USBHD.iq.q_waiting, Q_OK);
 	chSysRestoreStatusX(sts);
 }
 
@@ -407,8 +428,8 @@ 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);
+	iqResetI(&USBH_DEBUG_USBHD.iq);
+	oqResetI(&USBH_DEBUG_SD.oqueue);
 	while (*msg) {
 		*USBH_DEBUG_SD.oqueue.q_wrptr++ = *msg++;
 		USBH_DEBUG_SD.oqueue.q_counter--;
@@ -431,8 +452,8 @@ void usbDbgSystemHalted(void) {
 		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++;
+		while (!(USBH_DEBUG_SD.usart->SR & USART_SR_TXE));
+		USBH_DEBUG_SD.usart->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;
 		}
@@ -458,57 +479,74 @@ void usbDbgSystemHalted(void) {
 			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';
+					while (!(USBH_DEBUG_SD.usart->SR & USART_SR_TXE));
+					USBH_DEBUG_SD.usart->DR = '\r';
+					while (!(USBH_DEBUG_SD.usart->SR & USART_SR_TXE));
+					USBH_DEBUG_SD.usart->DR = '\n';
 					state = 0;
 					break;
 				}
-				while (!(USART1->SR & USART_SR_TXE));
-				USART1->DR = c;
+				while (!(USBH_DEBUG_SD.usart->SR & USART_SR_TXE));
+				USBH_DEBUG_SD.usart->DR = c;
 			}
 		}
 	}
 }
 
-static void usb_debug_thread(void *p) {
-	USBHDriver *host = (USBHDriver *)p;
+static void usb_debug_thread(void *arg) {
+	USBHDriver *host = (USBHDriver *)arg;
 	uint8_t state = 0;
 
 	chRegSetThreadName("USBH_DBG");
 	while (true) {
-		msg_t c = chIQGet(&host->iq);
+		msg_t c = iqGet(&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 if (c == 0xfe) state = 3;
 			else (state = 0);
-		} else {
+		} else if (state == 2) {
 			uint16_t hfir;
 			uint32_t hfnum;
 
 			hfir = c;
-			c = chIQGet(&host->iq); if (c < 0) goto reset;
+			c = iqGet(&host->iq); if (c < 0) goto reset;
 			hfir |= c << 8;
 
-			c = chIQGet(&host->iq); if (c < 0) goto reset;
+			c = iqGet(&host->iq); if (c < 0) goto reset;
 			hfnum = c;
-			c = chIQGet(&host->iq); if (c < 0) goto reset;
+			c = iqGet(&host->iq); if (c < 0) goto reset;
 			hfnum |= c << 8;
-			c = chIQGet(&host->iq); if (c < 0) goto reset;
+			c = iqGet(&host->iq); if (c < 0) goto reset;
 			hfnum |= c << 16;
-			c = chIQGet(&host->iq); if (c < 0) goto reset;
+			c = iqGet(&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);
-
+			chprintf((BaseSequentialStream *)&USBH_DEBUG_SD, "%05d.%03d ", f, p);
+			state = 4;
+		} else if (state == 3) {
+			uint32_t t;
+
+			c = iqGet(&host->iq); if (c < 0) goto reset;
+			c = iqGet(&host->iq); if (c < 0) goto reset;
+
+			t = c;
+			c = iqGet(&host->iq); if (c < 0) goto reset;
+			t |= c << 8;
+			c = iqGet(&host->iq); if (c < 0) goto reset;
+			t |= c << 16;
+			c = iqGet(&host->iq); if (c < 0) goto reset;
+			t |= c << 24;
+
+			chprintf((BaseSequentialStream *)&USBH_DEBUG_SD, "+%08d ", t);
+			state = 4;
+		} else {
 			while (true) {
-				c = chIQGet(&host->iq); if (c < 0) goto reset;
 				if (!c) {
 					sdPut(&USBH_DEBUG_SD, '\r');
 					sdPut(&USBH_DEBUG_SD, '\n');
@@ -516,6 +554,7 @@ static void usb_debug_thread(void *p) {
 					break;
 				}
 				sdPut(&USBH_DEBUG_SD, (uint8_t)c);
+				c = iqGet(&host->iq); if (c < 0) goto reset;
 			}
 		}
 
@@ -528,9 +567,8 @@ reset:
 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);
+	iqObjectInit(&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/hal_usbh_desciter.c b/os/hal/src/usbh/hal_usbh_desciter.c
index 63137d4..3695881 100644
--- a/os/hal/src/usbh/hal_usbh_desciter.c
+++ b/os/hal/src/usbh/hal_usbh_desciter.c
@@ -1,6 +1,6 @@
 /*
-    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio
-              Copyright (C) 2015 Diego Ismirlian, TISA, (dismirlian (at) google's mail)
+    ChibiOS - Copyright (C) 2006..2017 Giovanni Di Sirio
+              Copyright (C) 2015..2017 Diego Ismirlian, (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.
@@ -134,22 +134,18 @@ void cs_iter_next(generic_iterator_t *ics) {
 	if ((curr[0] < 2) || (rem < 2) || (rem < curr[0]))
 		return;
 
-	//for (;;) {
-		rem -= curr[0];
-		curr += curr[0];
+	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;
-		}
+	if ((curr[0] < 2) || (rem < 2) || (rem < curr[0]))
+		return;
 
-	//	break;
-	//}
+	if ((curr[1] == USBH_DT_INTERFACE_ASSOCIATION)
+			|| (curr[1] == USBH_DT_INTERFACE)
+			|| (curr[1] == USBH_DT_CONFIG)
+			|| (curr[1] == USBH_DT_ENDPOINT)) {
+		return;
+	}
 
 	ics->valid = 1;
 	ics->rem = rem;
diff --git a/os/hal/src/usbh/hal_usbh_ftdi.c b/os/hal/src/usbh/hal_usbh_ftdi.c
index 4bd7296..6966028 100644
--- a/os/hal/src/usbh/hal_usbh_ftdi.c
+++ b/os/hal/src/usbh/hal_usbh_ftdi.c
@@ -1,6 +1,6 @@
 /*
-    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio
-              Copyright (C) 2015 Diego Ismirlian, TISA, (dismirlian (at) google's mail)
+    ChibiOS - Copyright (C) 2006..2017 Giovanni Di Sirio
+              Copyright (C) 2015..2017 Diego Ismirlian, (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.
@@ -16,7 +16,6 @@
 */
 
 #include "hal.h"
-#include "hal_usbh.h"
 
 #if HAL_USBH_USE_FTDI
 
@@ -28,9 +27,6 @@
 #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__)
@@ -63,22 +59,25 @@
 #define uerr(f, ...)   do {} while(0)
 #endif
 
+static void _ftdip_object_init(USBHFTDIPortDriver *ftdipp);
 
 /*===========================================================================*/
 /* USB Class driver loader for FTDI								 		 	 */
 /*===========================================================================*/
 USBHFTDIDriver USBHFTDID[HAL_USBHFTDI_MAX_INSTANCES];
 
+static void _ftdi_init(void);
 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_init,
 	_ftdi_load,
 	_ftdi_unload
 };
 
 const usbh_classdriverinfo_t usbhftdiClassDriverInfo = {
-	0xff, 0xff, 0xff, "FTDI", &class_driver_vmt
+	"FTDI", &class_driver_vmt
 };
 
 static USBHFTDIPortDriver *_find_port(void) {
@@ -94,10 +93,8 @@ static usbh_baseclassdriver_t *_ftdi_load(usbh_device_t *dev, const uint8_t *des
 	int i;
 	USBHFTDIDriver *ftdip;
 
-	if (dev->devDesc.idVendor != 0x0403) {
-		uerr("FTDI: Unrecognized VID");
+	if (_usbh_match_vid_pid(dev, 0x0403, -1) != HAL_SUCCESS)
 		return NULL;
-	}
 
 	switch (dev->devDesc.idProduct) {
 	case 0x6001:
@@ -105,17 +102,18 @@ static usbh_baseclassdriver_t *_ftdi_load(usbh_device_t *dev, const uint8_t *des
 	case 0x6011:
 	case 0x6014:
 	case 0x6015:
+	case 0xE2E6:
 		break;
 	default:
 		uerr("FTDI: Unrecognized PID");
 		return NULL;
 	}
 
-	if ((rem < descriptor[0]) || (descriptor[1] != USBH_DT_INTERFACE))
+	if (_usbh_match_descriptor(descriptor, rem, USBH_DT_INTERFACE,
+			0xff, 0xff, 0xff) != HAL_SUCCESS)
 		return NULL;
 
-	const usbh_interface_descriptor_t * const ifdesc = (const usbh_interface_descriptor_t * const)descriptor;
-	if (ifdesc->bInterfaceNumber != 0) {
+	if (((const usbh_interface_descriptor_t *)descriptor)->bInterfaceNumber != 0) {
 		uwarn("FTDI: Will allocate driver along with IF #0");
 	}
 
@@ -211,7 +209,7 @@ alloc_ok:
 
 }
 
-static void _stop(USBHFTDIPortDriver *ftdipp);
+static void _stopS(USBHFTDIPortDriver *ftdipp);
 static void _ftdi_unload(usbh_baseclassdriver_t *drv) {
 	osalDbgCheck(drv != NULL);
 	USBHFTDIDriver *const ftdip = (USBHFTDIDriver *)drv;
@@ -219,7 +217,9 @@ static void _ftdi_unload(usbh_baseclassdriver_t *drv) {
 
 	osalMutexLock(&ftdip->mtx);
 	while (ftdipp) {
-		_stop(ftdipp);
+		osalSysLock();
+		_stopS(ftdipp);
+		osalSysUnlock();
 		ftdipp = ftdipp->next;
 	}
 
@@ -227,7 +227,7 @@ static void _ftdi_unload(usbh_baseclassdriver_t *drv) {
 	osalSysLock();
 	while (ftdipp) {
 		USBHFTDIPortDriver *next = ftdipp->next;
-		usbhftdipObjectInit(ftdipp);
+		_ftdip_object_init(ftdipp);
 		ftdipp = next;
 	}
 	osalSysUnlock();
@@ -314,17 +314,17 @@ static usbh_urbstatus_t _ftdi_port_control(USBHFTDIPortDriver *ftdipp,
 		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
+		USBH_REQTYPE_TYPE_VENDOR | USBH_REQTYPE_DIR_OUT | USBH_REQTYPE_RECIP_DEVICE, //0 FTDI_COMMAND_RESET
+		USBH_REQTYPE_TYPE_VENDOR | USBH_REQTYPE_DIR_OUT | USBH_REQTYPE_RECIP_DEVICE, //1 FTDI_COMMAND_MODEMCTRL
+		USBH_REQTYPE_TYPE_VENDOR | USBH_REQTYPE_DIR_OUT | USBH_REQTYPE_RECIP_DEVICE, //2 FTDI_COMMAND_SETFLOW
+		USBH_REQTYPE_TYPE_VENDOR | USBH_REQTYPE_DIR_OUT | USBH_REQTYPE_RECIP_DEVICE, //3 FTDI_COMMAND_SETBAUD
+		USBH_REQTYPE_TYPE_VENDOR | USBH_REQTYPE_DIR_OUT | USBH_REQTYPE_RECIP_DEVICE, //4 FTDI_COMMAND_SETDATA
 	};
 
 	osalDbgCheck(bRequest < sizeof_array(bmRequestType));
 	osalDbgCheck(bRequest != 1);
 
-	const USBH_DEFINE_BUFFER(usbh_control_request_t, req) = {
+	USBH_DEFINE_BUFFER(const usbh_control_request_t req) = {
 			bmRequestType[bRequest],
 			bRequest,
 			wValue,
@@ -332,7 +332,7 @@ static usbh_urbstatus_t _ftdi_port_control(USBHFTDIPortDriver *ftdipp,
 			wLength
 	};
 
-	return usbhControlRequestExtended(ftdipp->ftdip->dev, &req, buff, NULL, MS2ST(1000));
+	return usbhControlRequestExtended(ftdipp->ftdip->dev, &req, buff, NULL, OSAL_MS2I(1000));
 }
 
 static uint32_t _get_divisor(uint32_t baud, usbhftdi_type_t type) {
@@ -387,14 +387,14 @@ static usbh_urbstatus_t _set_baudrate(USBHFTDIPortDriver *ftdipp, uint32_t baudr
 	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,
+	USBH_DEFINE_BUFFER(const usbh_control_request_t req) = {
+		USBH_REQTYPE_TYPE_VENDOR | USBH_REQTYPE_DIR_OUT | USBH_REQTYPE_RECIP_DEVICE,
 		FTDI_COMMAND_SETBAUD,
 		wValue,
 		wIndex,
 		0
 	};
-	return usbhControlRequestExtended(ftdipp->ftdip->dev, &req, NULL, NULL, MS2ST(1000));
+	return usbhControlRequestExtended(ftdipp->ftdip->dev, &req, NULL, NULL, OSAL_MS2I(1000));
 }
 
 
@@ -415,7 +415,7 @@ static void _out_cb(usbh_urb_t *urb) {
 		return;
 	case USBH_URBSTATUS_DISCONNECTED:
 		uwarn("FTDI: URB OUT disconnected");
-		chThdDequeueNextI(&ftdipp->oq_waiting, Q_RESET);
+		chThdDequeueAllI(&ftdipp->oq_waiting, Q_RESET);
 		return;
 	default:
 		uerrf("FTDI: URB OUT status unexpected = %d", urb->status);
@@ -430,15 +430,15 @@ static size_t _write_timeout(USBHFTDIPortDriver *ftdipp, const uint8_t *bp,
 	chDbgCheck(n > 0U);
 
 	size_t w = 0;
-	chSysLock();
+	osalSysLock();
 	while (true) {
 		if (ftdipp->state != USBHFTDIP_STATE_READY) {
-			chSysUnlock();
+			osalSysUnlock();
 			return w;
 		}
 		while (usbhURBIsBusy(&ftdipp->oq_urb)) {
 			if (chThdEnqueueTimeoutS(&ftdipp->oq_waiting, timeout) != Q_OK) {
-				chSysUnlock();
+				osalSysUnlock();
 				return w;
 			}
 		}
@@ -446,30 +446,30 @@ static size_t _write_timeout(USBHFTDIPortDriver *ftdipp, const uint8_t *bp,
 		*ftdipp->oq_ptr++ = *bp++;
 		if (--ftdipp->oq_counter == 0) {
 			_submitOutI(ftdipp, 64);
-			chSchRescheduleS();
+			osalOsRescheduleS();
 		}
-		chSysUnlock(); /* Gives a preemption chance in a controlled point.*/
+		osalSysUnlock(); /* Gives a preemption chance in a controlled point.*/
 
 		w++;
 		if (--n == 0U)
 			return w;
 
-		chSysLock();
+		osalSysLock();
 	}
 }
 
 static msg_t _put_timeout(USBHFTDIPortDriver *ftdipp, uint8_t b, systime_t timeout) {
 
-	chSysLock();
+	osalSysLock();
 	if (ftdipp->state != USBHFTDIP_STATE_READY) {
-		chSysUnlock();
+		osalSysUnlock();
 		return Q_RESET;
 	}
 
 	while (usbhURBIsBusy(&ftdipp->oq_urb)) {
 		msg_t msg = chThdEnqueueTimeoutS(&ftdipp->oq_waiting, timeout);
 		if (msg < Q_OK) {
-			chSysUnlock();
+			osalSysUnlock();
 			return msg;
 		}
 	}
@@ -477,9 +477,9 @@ static msg_t _put_timeout(USBHFTDIPortDriver *ftdipp, uint8_t b, systime_t timeo
 	*ftdipp->oq_ptr++ = b;
 	if (--ftdipp->oq_counter == 0) {
 		_submitOutI(ftdipp, 64);
-		chSchRescheduleS();
+		osalOsRescheduleS();
 	}
-	chSysUnlock();
+	osalSysUnlock();
 	return Q_OK;
 }
 
@@ -516,12 +516,12 @@ static void _in_cb(usbh_urb_t *urb) {
 			udbgf("FTDI: URB IN no data, status=%02x %02x",
 					((uint8_t *)urb->buff)[0],
 					((uint8_t *)urb->buff)[1]);
-			return;
+		//	return;
 		}
 		break;
 	case USBH_URBSTATUS_DISCONNECTED:
 		uwarn("FTDI: URB IN disconnected");
-		chThdDequeueNextI(&ftdipp->iq_waiting, Q_RESET);
+		chThdDequeueAllI(&ftdipp->iq_waiting, Q_RESET);
 		return;
 	default:
 		uerrf("FTDI: URB IN status unexpected = %d", urb->status);
@@ -536,41 +536,41 @@ static size_t _read_timeout(USBHFTDIPortDriver *ftdipp, uint8_t *bp,
 
 	chDbgCheck(n > 0U);
 
-	chSysLock();
+	osalSysLock();
 	while (true) {
 		if (ftdipp->state != USBHFTDIP_STATE_READY) {
-			chSysUnlock();
+			osalSysUnlock();
 			return r;
 		}
 		while (ftdipp->iq_counter == 0) {
 			if (!usbhURBIsBusy(&ftdipp->iq_urb))
 				_submitInI(ftdipp);
 			if (chThdEnqueueTimeoutS(&ftdipp->iq_waiting, timeout) != Q_OK) {
-				chSysUnlock();
+				osalSysUnlock();
 				return r;
 			}
 		}
 		*bp++ = *ftdipp->iq_ptr++;
 		if (--ftdipp->iq_counter == 0) {
 			_submitInI(ftdipp);
-			chSchRescheduleS();
+			osalOsRescheduleS();
 		}
-		chSysUnlock();
+		osalSysUnlock();
 
 		r++;
 		if (--n == 0U)
 			return r;
 
-		chSysLock();
+		osalSysLock();
 	}
 }
 
 static msg_t _get_timeout(USBHFTDIPortDriver *ftdipp, systime_t timeout) {
 	uint8_t b;
 
-	chSysLock();
+	osalSysLock();
 	if (ftdipp->state != USBHFTDIP_STATE_READY) {
-		chSysUnlock();
+		osalSysUnlock();
 		return Q_RESET;
 	}
 	while (ftdipp->iq_counter == 0) {
@@ -578,16 +578,16 @@ static msg_t _get_timeout(USBHFTDIPortDriver *ftdipp, systime_t timeout) {
 			_submitInI(ftdipp);
 		msg_t msg = chThdEnqueueTimeoutS(&ftdipp->iq_waiting, timeout);
 		if (msg < Q_OK) {
-			chSysUnlock();
+			osalSysUnlock();
 			return msg;
 		}
 	}
 	b = *ftdipp->iq_ptr++;
 	if (--ftdipp->iq_counter == 0) {
 		_submitInI(ftdipp);
-		chSchRescheduleS();
+		osalOsRescheduleS();
 	}
-	chSysUnlock();
+	osalSysUnlock();
 
 	return (msg_t)b;
 }
@@ -602,7 +602,7 @@ static size_t _read(USBHFTDIPortDriver *ftdipp, uint8_t *bp, size_t n) {
 
 static void _vt(void *p) {
 	USBHFTDIPortDriver *const ftdipp = (USBHFTDIPortDriver *)p;
-	chSysLockFromISR();
+	osalSysLockFromISR();
 	uint32_t len = ftdipp->oq_ptr - ftdipp->oq_buff;
 	if (len && !usbhURBIsBusy(&ftdipp->oq_urb)) {
 		_submitOutI(ftdipp, len);
@@ -610,8 +610,8 @@ static void _vt(void *p) {
 	if ((ftdipp->iq_counter == 0) && !usbhURBIsBusy(&ftdipp->iq_urb)) {
 		_submitInI(ftdipp);
 	}
-	chVTSetI(&ftdipp->vt, MS2ST(16), _vt, ftdipp);
-	chSysUnlockFromISR();
+	chVTSetI(&ftdipp->vt, OSAL_MS2I(16), _vt, ftdipp);
+	osalSysUnlockFromISR();
 }
 
 static const struct FTDIPortDriverVMT async_channel_vmt = {
@@ -626,29 +626,27 @@ static const struct FTDIPortDriverVMT async_channel_vmt = {
 };
 
 
-static void _stop(USBHFTDIPortDriver *ftdipp) {
-	osalSysLock();
+static void _stopS(USBHFTDIPortDriver *ftdipp) {
+	if (ftdipp->state != USBHFTDIP_STATE_READY)
+		return;
 	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();
+	osalOsRescheduleS();
 }
 
 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);
+	osalSysLock();
+	chMtxLockS(&ftdipp->ftdip->mtx);
+	_stopS(ftdipp);
+	chMtxUnlockS(&ftdipp->ftdip->mtx);
+	osalSysUnlock();
 }
 
 void usbhftdipStart(USBHFTDIPortDriver *ftdipp, const USBHFTDIPortConfig *config) {
@@ -689,29 +687,37 @@ void usbhftdipStart(USBHFTDIPortDriver *ftdipp, const USBHFTDIPortConfig *config
 	ftdipp->iq_counter = 0;
 	ftdipp->iq_ptr = ftdipp->iq_buff;
 	usbhEPOpen(&ftdipp->epin);
-	osalSysLock();
-	usbhURBSubmitI(&ftdipp->iq_urb);
-	osalSysUnlock();
+	usbhURBSubmit(&ftdipp->iq_urb);
 
 	chVTObjectInit(&ftdipp->vt);
-	chVTSet(&ftdipp->vt, MS2ST(16), _vt, ftdipp);
+	chVTSet(&ftdipp->vt, OSAL_MS2I(16), _vt, ftdipp);
 
 	ftdipp->state = USBHFTDIP_STATE_READY;
 	osalMutexUnlock(&ftdipp->ftdip->mtx);
 }
 
-void usbhftdiObjectInit(USBHFTDIDriver *ftdip) {
+static void _ftdi_object_init(USBHFTDIDriver *ftdip) {
 	osalDbgCheck(ftdip != NULL);
 	memset(ftdip, 0, sizeof(*ftdip));
 	ftdip->info = &usbhftdiClassDriverInfo;
 	osalMutexObjectInit(&ftdip->mtx);
 }
 
-void usbhftdipObjectInit(USBHFTDIPortDriver *ftdipp) {
+static void _ftdip_object_init(USBHFTDIPortDriver *ftdipp) {
 	osalDbgCheck(ftdipp != NULL);
 	memset(ftdipp, 0, sizeof(*ftdipp));
 	ftdipp->vmt = &async_channel_vmt;
 	ftdipp->state = USBHFTDIP_STATE_STOP;
 }
 
+static void _ftdi_init(void) {
+	uint8_t i;
+	for (i = 0; i < HAL_USBHFTDI_MAX_INSTANCES; i++) {
+		_ftdi_object_init(&USBHFTDID[i]);
+	}
+	for (i = 0; i < HAL_USBHFTDI_MAX_PORTS; i++) {
+		_ftdip_object_init(&FTDIPD[i]);
+	}
+}
+
 #endif
diff --git a/os/hal/src/usbh/hal_usbh_hid.c b/os/hal/src/usbh/hal_usbh_hid.c
new file mode 100644
index 0000000..2b2c5ce
--- /dev/null
+++ b/os/hal/src/usbh/hal_usbh_hid.c
@@ -0,0 +1,338 @@
+/*
+    ChibiOS - Copyright (C) 2006..2017 Giovanni Di Sirio
+              Copyright (C) 2015..2017 Diego Ismirlian, (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_USBH_USE_HID
+
+#if !HAL_USE_USBH
+#error "USBHHID needs USBH"
+#endif
+
+#include <string.h>
+#include "usbh/dev/hid.h"
+#include "usbh/internal.h"
+
+#if USBHHID_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 USBHHID_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 USBHHID_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 USBHHID_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
+
+
+
+#define USBH_HID_REQ_GET_REPORT		0x01
+#define USBH_HID_REQ_GET_IDLE		0x02
+#define USBH_HID_REQ_GET_PROTOCOL	0x03
+#define USBH_HID_REQ_SET_REPORT		0x09
+#define USBH_HID_REQ_SET_IDLE		0x0A
+#define USBH_HID_REQ_SET_PROTOCOL	0x0B
+
+/*===========================================================================*/
+/* USB Class driver loader for HID								 		 	 */
+/*===========================================================================*/
+
+USBHHIDDriver USBHHIDD[HAL_USBHHID_MAX_INSTANCES];
+
+static void _hid_init(void);
+static usbh_baseclassdriver_t *_hid_load(usbh_device_t *dev, const uint8_t *descriptor, uint16_t rem);
+static void _hid_unload(usbh_baseclassdriver_t *drv);
+static void _stop_locked(USBHHIDDriver *hidp);
+
+static const usbh_classdriver_vmt_t class_driver_vmt = {
+	_hid_init,
+	_hid_load,
+	_hid_unload
+};
+
+const usbh_classdriverinfo_t usbhhidClassDriverInfo = {
+	"HID", &class_driver_vmt
+};
+
+static usbh_baseclassdriver_t *_hid_load(usbh_device_t *dev, const uint8_t *descriptor, uint16_t rem) {
+	int i;
+	USBHHIDDriver *hidp;
+
+	if (_usbh_match_descriptor(descriptor, rem, USBH_DT_INTERFACE,
+			0x03, -1, -1) != HAL_SUCCESS)
+		return NULL;
+
+	const usbh_interface_descriptor_t * const ifdesc = (const usbh_interface_descriptor_t *)descriptor;
+
+	if ((ifdesc->bAlternateSetting != 0)
+			|| (ifdesc->bNumEndpoints < 1)) {
+		return NULL;
+	}
+
+
+	/* alloc driver */
+	for (i = 0; i < HAL_USBHHID_MAX_INSTANCES; i++) {
+		if (USBHHIDD[i].dev == NULL) {
+			hidp = &USBHHIDD[i];
+			goto alloc_ok;
+		}
+	}
+
+	uwarn("Can't alloc HID driver");
+
+	/* can't alloc */
+	return NULL;
+
+alloc_ok:
+	/* initialize the driver's variables */
+	hidp->epin.status = USBH_EPSTATUS_UNINITIALIZED;
+#if HAL_USBHHID_USE_INTERRUPT_OUT
+	hidp->epout.status = USBH_EPSTATUS_UNINITIALIZED;
+#endif
+	hidp->ifnum = ifdesc->bInterfaceNumber;
+	usbhEPSetName(&dev->ctrl, "HID[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_INT)) {
+			uinfof("INT IN endpoint found: bEndpointAddress=%02x", epdesc->bEndpointAddress);
+			usbhEPObjectInit(&hidp->epin, dev, epdesc);
+			usbhEPSetName(&hidp->epin, "HID[IIN ]");
+#if HAL_USBHHID_USE_INTERRUPT_OUT
+		} else if (((epdesc->bEndpointAddress & 0x80) == 0)
+				&& (epdesc->bmAttributes == USBH_EPTYPE_INT)) {
+			uinfof("INT OUT endpoint found: bEndpointAddress=%02x", epdesc->bEndpointAddress);
+			usbhEPObjectInit(&hidp->epout, dev, epdesc);
+			usbhEPSetName(&hidp->epout, "HID[IOUT]");
+#endif
+		} else {
+			uinfof("unsupported endpoint found: bEndpointAddress=%02x, bmAttributes=%02x",
+					epdesc->bEndpointAddress, epdesc->bmAttributes);
+		}
+	}
+	if (hidp->epin.status != USBH_EPSTATUS_CLOSED) {
+		goto deinit;
+	}
+
+	if (ifdesc->bInterfaceSubClass != 0x01) {
+		hidp->type = USBHHID_DEVTYPE_GENERIC;
+		uinfof("HID: bInterfaceSubClass=%02x, generic HID", ifdesc->bInterfaceSubClass);
+		if (ifdesc->bInterfaceSubClass != 0x00) {
+			uinfof("HID: bInterfaceSubClass=%02x is an invalid bInterfaceSubClass value",
+					ifdesc->bInterfaceSubClass);
+		}
+	} else if (ifdesc->bInterfaceProtocol == 0x01) {
+		hidp->type = USBHHID_DEVTYPE_BOOT_KEYBOARD;
+		uinfo("HID: BOOT protocol keyboard found");
+	} else if (ifdesc->bInterfaceProtocol == 0x02) {
+		hidp->type = USBHHID_DEVTYPE_BOOT_MOUSE;
+		uinfo("HID: BOOT protocol mouse found");
+	} else {
+		uerrf("HID: bInterfaceProtocol=%02x is an invalid boot protocol, abort",
+				ifdesc->bInterfaceProtocol);
+		goto deinit;
+	}
+
+	hidp->state = USBHHID_STATE_ACTIVE;
+
+	return (usbh_baseclassdriver_t *)hidp;
+
+deinit:
+	/* Here, the enpoints are closed, and the driver is unlinked */
+	return NULL;
+}
+
+static void _hid_unload(usbh_baseclassdriver_t *drv) {
+	USBHHIDDriver *const hidp = (USBHHIDDriver *)drv;
+	chSemWait(&hidp->sem);
+	_stop_locked(hidp);
+	hidp->state = USBHHID_STATE_STOP;
+	chSemSignal(&hidp->sem);
+}
+
+static void _in_cb(usbh_urb_t *urb) {
+	USBHHIDDriver *const hidp = (USBHHIDDriver *)urb->userData;
+	switch (urb->status) {
+	case USBH_URBSTATUS_OK:
+		if (hidp->config->cb_report) {
+			hidp->config->cb_report(hidp, urb->actualLength);
+		}
+		break;
+	case USBH_URBSTATUS_DISCONNECTED:
+		uwarn("HID: URB IN disconnected");
+
+		return;
+	case USBH_URBSTATUS_TIMEOUT:
+		//no data
+		break;
+	default:
+		uerrf("HID: URB IN status unexpected = %d", urb->status);
+		break;
+	}
+	usbhURBObjectResetI(&hidp->in_urb);
+	usbhURBSubmitI(&hidp->in_urb);
+}
+
+void usbhhidStart(USBHHIDDriver *hidp, const USBHHIDConfig *cfg) {
+	osalDbgCheck(hidp && cfg);
+	osalDbgCheck(cfg->report_buffer && (cfg->protocol <= USBHHID_PROTOCOL_REPORT));
+
+	chSemWait(&hidp->sem);
+	if (hidp->state == USBHHID_STATE_READY) {
+		chSemSignal(&hidp->sem);
+		return;
+	}
+	osalDbgCheck(hidp->state == USBHHID_STATE_ACTIVE);
+
+	hidp->config = cfg;
+
+	/* init the URBs */
+	uint32_t report_len = hidp->epin.wMaxPacketSize;
+	if (report_len > cfg->report_len)
+		report_len = cfg->report_len;
+	usbhURBObjectInit(&hidp->in_urb, &hidp->epin, _in_cb, hidp,
+			cfg->report_buffer, report_len);
+
+	/* open the int IN/OUT endpoints */
+	usbhEPOpen(&hidp->epin);
+#if HAL_USBHHID_USE_INTERRUPT_OUT
+	if (hidp->epout.status == USBH_EPSTATUS_CLOSED) {
+		usbhEPOpen(&hidp->epout);
+	}
+#endif
+
+	usbhhidSetProtocol(hidp, cfg->protocol);
+
+	usbhURBSubmit(&hidp->in_urb);
+
+	hidp->state = USBHHID_STATE_READY;
+	chSemSignal(&hidp->sem);
+}
+
+static void _stop_locked(USBHHIDDriver *hidp) {
+	if (hidp->state == USBHHID_STATE_ACTIVE)
+		return;
+
+	osalDbgCheck(hidp->state == USBHHID_STATE_READY);
+
+	usbhEPClose(&hidp->epin);
+#if HAL_USBHHID_USE_INTERRUPT_OUT
+	if (hidp->epout.status != USBH_EPSTATUS_UNINITIALIZED) {
+		usbhEPClose(&hidp->epout);
+	}
+#endif
+	hidp->state = USBHHID_STATE_ACTIVE;
+}
+
+void usbhhidStop(USBHHIDDriver *hidp) {
+	chSemWait(&hidp->sem);
+	_stop_locked(hidp);
+	chSemSignal(&hidp->sem);
+}
+
+usbh_urbstatus_t usbhhidGetReport(USBHHIDDriver *hidp,
+		uint8_t report_id, usbhhid_reporttype_t report_type,
+		void *data, uint16_t len) {
+	osalDbgCheck(hidp);
+	osalDbgAssert((uint8_t)report_type <= USBHHID_REPORTTYPE_FEATURE, "wrong report type");
+	return usbhControlRequest(hidp->dev,
+			USBH_REQTYPE_CLASSIN(USBH_REQTYPE_RECIP_INTERFACE), USBH_HID_REQ_GET_REPORT,
+			((uint8_t)report_type << 8) | report_id, hidp->ifnum, len, data);
+}
+
+usbh_urbstatus_t usbhhidSetReport(USBHHIDDriver *hidp,
+		uint8_t report_id, usbhhid_reporttype_t report_type,
+		const void *data, uint16_t len) {
+	osalDbgCheck(hidp);
+	osalDbgAssert((uint8_t)report_type <= USBHHID_REPORTTYPE_FEATURE, "wrong report type");
+	return usbhControlRequest(hidp->dev,
+			USBH_REQTYPE_CLASSOUT(USBH_REQTYPE_RECIP_INTERFACE), USBH_HID_REQ_SET_REPORT,
+			((uint8_t)report_type << 8) | report_id, hidp->ifnum, len, (void *)data);
+}
+
+usbh_urbstatus_t usbhhidGetIdle(USBHHIDDriver *hidp, uint8_t report_id, uint8_t *duration) {
+	osalDbgCheck(hidp);
+	return usbhControlRequest(hidp->dev,
+			USBH_REQTYPE_CLASSIN(USBH_REQTYPE_RECIP_INTERFACE), USBH_HID_REQ_GET_IDLE,
+			report_id, hidp->ifnum, 1, duration);
+}
+
+usbh_urbstatus_t usbhhidSetIdle(USBHHIDDriver *hidp, uint8_t report_id, uint8_t duration) {
+	osalDbgCheck(hidp);
+	return usbhControlRequest(hidp->dev,
+			USBH_REQTYPE_CLASSOUT(USBH_REQTYPE_RECIP_INTERFACE), USBH_HID_REQ_SET_IDLE,
+			(duration << 8) | report_id, hidp->ifnum, 0, NULL);
+}
+
+usbh_urbstatus_t usbhhidGetProtocol(USBHHIDDriver *hidp, uint8_t *protocol) {
+	osalDbgCheck(hidp);
+	return usbhControlRequest(hidp->dev,
+			USBH_REQTYPE_CLASSIN(USBH_REQTYPE_RECIP_INTERFACE), USBH_HID_REQ_GET_PROTOCOL,
+			0, hidp->ifnum, 1, protocol);
+}
+
+usbh_urbstatus_t usbhhidSetProtocol(USBHHIDDriver *hidp, uint8_t protocol) {
+	osalDbgCheck(hidp);
+	osalDbgAssert(protocol <= 1, "invalid protocol");
+	return usbhControlRequest(hidp->dev,
+			USBH_REQTYPE_CLASSOUT(USBH_REQTYPE_RECIP_INTERFACE), USBH_HID_REQ_SET_PROTOCOL,
+			protocol, hidp->ifnum, 0, NULL);
+}
+
+static void _hid_object_init(USBHHIDDriver *hidp) {
+	osalDbgCheck(hidp != NULL);
+	memset(hidp, 0, sizeof(*hidp));
+	hidp->info = &usbhhidClassDriverInfo;
+	hidp->state = USBHHID_STATE_STOP;
+	chSemObjectInit(&hidp->sem, 1);
+}
+
+static void _hid_init(void) {
+	uint8_t i;
+	for (i = 0; i < HAL_USBHHID_MAX_INSTANCES; i++) {
+		_hid_object_init(&USBHHIDD[i]);
+	}
+}
+
+#endif
diff --git a/os/hal/src/usbh/hal_usbh_hub.c b/os/hal/src/usbh/hal_usbh_hub.c
index 7fdcef1..6a83c66 100644
--- a/os/hal/src/usbh/hal_usbh_hub.c
+++ b/os/hal/src/usbh/hal_usbh_hub.c
@@ -1,6 +1,6 @@
 /*
-    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio
-              Copyright (C) 2015 Diego Ismirlian, TISA, (dismirlian (at) google's mail)
+    ChibiOS - Copyright (C) 2006..2017 Giovanni Di Sirio
+              Copyright (C) 2015..2017 Diego Ismirlian, (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.
@@ -16,8 +16,6 @@
 */
 
 #include "hal.h"
-#include "hal_usbh.h"
-#include "usbh/internal.h"
 
 #if HAL_USBH_USE_HUB
 
@@ -27,6 +25,7 @@
 
 #include <string.h>
 #include "usbh/dev/hub.h"
+#include "usbh/internal.h"
 
 #if USBHHUB_DEBUG_ENABLE_TRACE
 #define udbgf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)
@@ -62,16 +61,19 @@
 
 
 USBHHubDriver USBHHUBD[HAL_USBHHUB_MAX_INSTANCES];
-usbh_port_t USBHPorts[HAL_USBHHUB_MAX_PORTS];
+static 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 void _hub_init(void);
+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
+	_hub_init,
+	_hub_load,
+	_hub_unload
 };
+
 const usbh_classdriverinfo_t usbhhubClassDriverInfo = {
-	0x09, 0x00, -1, "HUB", &usbhhubClassDriverVMT
+	"HUB", &usbhhubClassDriverVMT
 };
 
 
@@ -105,7 +107,7 @@ static void _urb_complete(usbh_urb_t *urb) {
 	case USBH_URBSTATUS_TIMEOUT:
 		/* the device NAKed */
 		udbg("HUB: no info");
-		hubdp->statuschange = 0;
+		//hubdp->statuschange = 0;
 		break;
 	case USBH_URBSTATUS_OK: {
 		uint8_t len = hubdp->hubDesc.bNbrPorts / 8 + 1;
@@ -138,16 +140,14 @@ static void _urb_complete(usbh_urb_t *urb) {
 	usbhURBSubmitI(urb);
 }
 
-static usbh_baseclassdriver_t *hub_load(usbh_device_t *dev,
+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)
+	if (_usbh_match_descriptor(descriptor, rem, USBH_DT_DEVICE,
+			0x09, 0x00, 0x00) != HAL_SUCCESS)
 		return NULL;
 
 	generic_iterator_t iep, icfg;
@@ -159,12 +159,10 @@ static usbh_baseclassdriver_t *hub_load(usbh_device_t *dev,
 	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)) {
+
+	if (_usbh_match_descriptor(iif.curr, iif.rem, USBH_DT_INTERFACE,
+			0x09, 0x00, 0x00) != HAL_SUCCESS)
 		return NULL;
-	}
 
 	ep_iter_init(&iep, &iif);
 	if (!iep.valid)
@@ -199,7 +197,7 @@ alloc_ok:
 	/* read Hub descriptor */
 	uinfo("Read Hub descriptor");
 	if (usbhhubControlRequest(dev->host, hubdp,
-			USBH_REQTYPE_IN | USBH_REQTYPE_CLASS | USBH_REQTYPE_DEVICE,
+			USBH_REQTYPE_DIR_IN | USBH_REQTYPE_TYPE_CLASS | USBH_REQTYPE_RECIP_DEVICE,
 			USBH_REQ_GET_DESCRIPTOR,
 			(USBH_DT_HUB << 8), 0, sizeof(hubdp->hubDesc),
 			(uint8_t *)&hubdp->hubDesc) != USBH_URBSTATUS_OK) {
@@ -254,22 +252,18 @@ alloc_ok:
 			_urb_complete, hubdp, hubdp->scbuff,
 			(hubdesc->bNbrPorts + 8) / 8);
 
-	osalSysLock();
-	usbhURBSubmitI(&hubdp->urb);
-	osalOsRescheduleS();
-	osalSysUnlock();
+	usbhURBSubmit(&hubdp->urb);
 
+	hubdp->dev = NULL;
 	return (usbh_baseclassdriver_t *)hubdp;
 }
 
-static void hub_unload(usbh_baseclassdriver_t *drv) {
+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();
+	usbhEPClose(&hubdp->epint);
 
 	/* de-alloc ports and unload drivers */
 	usbh_port_t *port = hubdp->ports;
@@ -284,14 +278,23 @@ static void hub_unload(usbh_baseclassdriver_t *drv) {
 
 }
 
-void usbhhubObjectInit(USBHHubDriver *hubdp) {
+static void _object_init(USBHHubDriver *hubdp) {
 	osalDbgCheck(hubdp != NULL);
 	memset(hubdp, 0, sizeof(*hubdp));
 	hubdp->info = &usbhhubClassDriverInfo;
 }
+
+static void _hub_init(void) {
+	uint8_t i;
+	for (i = 0; i < HAL_USBHHUB_MAX_INSTANCES; i++) {
+		_object_init(&USBHHUBD[i]);
+	}
+}
+
 #else
 
 #if HAL_USE_USBH
+#include <string.h>
 void _usbhub_port_object_init(usbh_port_t *port, USBHDriver *usbh, uint8_t number) {
 	memset(port, 0, sizeof(*port));
 	port->number = number;
diff --git a/os/hal/src/usbh/hal_usbh_msd.c b/os/hal/src/usbh/hal_usbh_msd.c
index 6869a74..7233a0b 100644
--- a/os/hal/src/usbh/hal_usbh_msd.c
+++ b/os/hal/src/usbh/hal_usbh_msd.c
@@ -1,6 +1,6 @@
 /*
-    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio
-              Copyright (C) 2015 Diego Ismirlian, TISA, (dismirlian (at) google's mail)
+    ChibiOS - Copyright (C) 2006..2017 Giovanni Di Sirio
+              Copyright (C) 2015..2017 Diego Ismirlian, (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.
@@ -16,7 +16,6 @@
 */
 
 #include "hal.h"
-#include "hal_usbh.h"
 
 #if HAL_USBH_USE_MSD
 
@@ -28,9 +27,6 @@
 #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__)
@@ -63,26 +59,39 @@
 #define uerr(f, ...)   do {} while(0)
 #endif
 
+static void _lun_object_deinit(USBHMassStorageLUNDriver *lunp);
 
+/*===========================================================================*/
+/* USB Class driver loader for MSD                                           */
+/*===========================================================================*/
 
+struct USBHMassStorageDriver {
+	/* inherited from abstract class driver */
+	_usbh_base_classdriver_data
 
+	usbh_ep_t epin;
+	usbh_ep_t epout;
+	uint8_t ifnum;
+	uint8_t max_lun;
+	uint32_t tag;
 
-/*===========================================================================*/
-/* USB Class driver loader for MSD								 		 	 */
-/*===========================================================================*/
+	USBHMassStorageLUNDriver *luns;
+};
 
-USBHMassStorageDriver USBHMSD[HAL_USBHMSD_MAX_INSTANCES];
+static USBHMassStorageDriver USBHMSD[HAL_USBHMSD_MAX_INSTANCES];
 
+static void _msd_init(void);
 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_init,
 	_msd_load,
 	_msd_unload
 };
 
 const usbh_classdriverinfo_t usbhmsdClassDriverInfo = {
-	0x08, 0x06, 0x50, "MSD", &class_driver_vmt
+	"MSD", &class_driver_vmt
 };
 
 #define MSD_REQ_RESET							0xFF
@@ -91,18 +100,17 @@ const usbh_classdriverinfo_t usbhmsdClassDriverInfo = {
 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
+	uint8_t luns;
+	usbh_urbstatus_t stat;
 
-	if ((rem < descriptor[0]) || (descriptor[1] != USBH_DT_INTERFACE))
+	if (_usbh_match_descriptor(descriptor, rem, USBH_DT_INTERFACE,
+			0x08, 0x06, 0x50) != HAL_SUCCESS)
 		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)) {
+			|| (ifdesc->bNumEndpoints < 2)) {
 		return NULL;
 	}
 
@@ -157,10 +165,10 @@ alloc_ok:
 
 	/* read the number of LUNs */
 	uinfo("Reading Max LUN:");
-	USBH_DEFINE_BUFFER(uint8_t, buff[4]);
+	USBH_DEFINE_BUFFER(uint8_t buff[4]);
 	stat = usbhControlRequest(dev,
-			USBH_CLASSIN(USBH_REQTYPE_INTERFACE, MSD_GET_MAX_LUN, 0, msdp->ifnum),
-			1, buff);
+			USBH_REQTYPE_CLASSIN(USBH_REQTYPE_RECIP_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);
@@ -188,13 +196,7 @@ alloc_ok:
 			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]);
+			MSBLKD[i].state = BLK_ACTIVE;
 			luns--;
 		}
 	}
@@ -211,36 +213,24 @@ static void _msd_unload(usbh_baseclassdriver_t *drv) {
 	USBHMassStorageDriver *const msdp = (USBHMassStorageDriver *)drv;
 	USBHMassStorageLUNDriver *lunp = msdp->luns;
 
-	osalMutexLock(&msdp->mtx);
-	osalSysLock();
-	usbhEPCloseS(&msdp->epin);
-	usbhEPCloseS(&msdp->epout);
+	/* disconnect all LUNs */
 	while (lunp) {
-		lunp->state = BLK_STOP;
+		usbhmsdLUNDisconnect(lunp);
+		_lun_object_deinit(lunp);
 		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();
+	usbhEPClose(&msdp->epin);
+	usbhEPClose(&msdp->epout);
 }
 
 
 /*===========================================================================*/
-/* MSD Class driver operations (Bulk-Only transport)			 		 	 */
+/* MSD Class driver operations (Bulk-Only transport)                         */
 /*===========================================================================*/
 
-
-
 /* USB Bulk Only Transport SCSI Command block wrapper */
-PACKED_STRUCT {
+typedef PACKED_STRUCT {
 	uint32_t dCBWSignature;
 	uint32_t dCBWTag;
 	uint32_t dCBWDataTransferLength;
@@ -253,9 +243,8 @@ PACKED_STRUCT {
 #define MSD_CBWFLAGS_D2H						0x80
 #define MSD_CBWFLAGS_H2D						0x00
 
-
 /* USB Bulk Only Transport SCSI Command status wrapper */
-PACKED_STRUCT {
+typedef PACKED_STRUCT {
 	uint32_t dCSWSignature;
 	uint32_t dCSWTag;
 	uint32_t dCSWDataResidue;
@@ -263,34 +252,174 @@ PACKED_STRUCT {
 } msd_csw_t;
 #define MSD_CSW_SIGNATURE						0x53425355
 
-
-typedef union {
-	msd_cbw_t cbw;
-	msd_csw_t csw;
+typedef struct {
+	msd_cbw_t *cbw;
+	uint8_t csw_status;
+	uint32_t data_processed;
 } 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;
+	MSD_BOTRESULT_OK,
+	MSD_BOTRESULT_DISCONNECTED,
+	MSD_BOTRESULT_ERROR
+} msd_bot_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_OK = MSD_BOTRESULT_OK,
+	MSD_RESULT_DISCONNECTED = MSD_BOTRESULT_DISCONNECTED,
+	MSD_RESULT_TRANSPORT_ERROR = MSD_BOTRESULT_ERROR,
+	MSD_RESULT_FAILED
 } msd_result_t;
 
 
+#define	CSW_STATUS_PASSED		0
+#define	CSW_STATUS_FAILED		1
+#define	CSW_STATUS_PHASE_ERROR	2
+
+static bool _msd_bot_reset(USBHMassStorageDriver *msdp) {
+
+	usbh_urbstatus_t res;
+	res = usbhControlRequest(msdp->dev,
+			USBH_REQTYPE_CLASSOUT(USBH_REQTYPE_RECIP_INTERFACE),
+			0xFF, 0, msdp->ifnum, 0, NULL);
+	if (res != USBH_URBSTATUS_OK) {
+		return FALSE;
+	}
+
+	osalThreadSleepMilliseconds(100);
+
+	return usbhEPReset(&msdp->epin) && usbhEPReset(&msdp->epout);
+}
+
+static msd_bot_result_t _msd_bot_transaction(msd_transaction_t *tran, USBHMassStorageLUNDriver *lunp, void *data) {
+
+	uint32_t data_actual_len, actual_len;
+	usbh_urbstatus_t status;
+	USBH_DEFINE_BUFFER(msd_csw_t csw);
+
+	tran->cbw->bCBWLUN = (uint8_t)(lunp - &lunp->msdp->luns[0]);
+	tran->cbw->dCBWSignature = MSD_CBW_SIGNATURE;
+	tran->cbw->dCBWTag = ++lunp->msdp->tag;
+	tran->data_processed = 0;
+
+	/* control phase */
+	status = usbhBulkTransfer(&lunp->msdp->epout, tran->cbw,
+					sizeof(*tran->cbw), &actual_len, OSAL_MS2I(1000));
+
+	if (status == USBH_URBSTATUS_CANCELLED) {
+		uerr("\tMSD: Control phase: USBH_URBSTATUS_CANCELLED");
+		return MSD_BOTRESULT_DISCONNECTED;
+	}
+
+	if ((status != USBH_URBSTATUS_OK) || (actual_len != sizeof(*tran->cbw))) {
+		uerrf("\tMSD: Control phase: status = %d (!= OK), actual_len = %d (expected to send %d)",
+				status, actual_len, sizeof(*tran->cbw));
+		_msd_bot_reset(lunp->msdp);
+		return MSD_BOTRESULT_ERROR;
+	}
+
+
+	/* data phase */
+	data_actual_len = 0;
+	if (tran->cbw->dCBWDataTransferLength) {
+		usbh_ep_t *const ep = tran->cbw->bmCBWFlags & MSD_CBWFLAGS_D2H ? &lunp->msdp->epin : &lunp->msdp->epout;
+		status = usbhBulkTransfer(
+				ep,
+				data,
+				tran->cbw->dCBWDataTransferLength,
+				&data_actual_len, OSAL_MS2I(20000));
+
+		if (status == USBH_URBSTATUS_CANCELLED) {
+			uerr("\tMSD: Data phase: USBH_URBSTATUS_CANCELLED");
+			return MSD_BOTRESULT_DISCONNECTED;
+		}
+
+		if (status == USBH_URBSTATUS_STALL) {
+			uerrf("\tMSD: Data phase: USBH_URBSTATUS_STALL, clear halt");
+			status = (usbhEPReset(ep) == HAL_SUCCESS) ? USBH_URBSTATUS_OK : USBH_URBSTATUS_ERROR;
+		}
+
+		if (status != USBH_URBSTATUS_OK) {
+			uerrf("\tMSD: Data phase: status = %d (!= OK), resetting", status);
+			_msd_bot_reset(lunp->msdp);
+			return MSD_BOTRESULT_ERROR;
+		}
+	}
+
+
+	/* status phase */
+	status = usbhBulkTransfer(&lunp->msdp->epin, &csw,
+				sizeof(csw), &actual_len, OSAL_MS2I(1000));
+
+	if (status == USBH_URBSTATUS_STALL) {
+		uwarn("\tMSD: Status phase: USBH_URBSTATUS_STALL, clear halt and retry");
+
+		status = (usbhEPReset(&lunp->msdp->epin) == HAL_SUCCESS) ? USBH_URBSTATUS_OK : USBH_URBSTATUS_ERROR;
+
+		if (status == USBH_URBSTATUS_OK) {
+			status = usbhBulkTransfer(&lunp->msdp->epin, &csw,
+						sizeof(csw), &actual_len, OSAL_MS2I(1000));
+		}
+	}
+
+	if (status == USBH_URBSTATUS_CANCELLED) {
+		uerr("\tMSD: Status phase: USBH_URBSTATUS_CANCELLED");
+		return MSD_BOTRESULT_DISCONNECTED;
+	}
+
+	if (status != USBH_URBSTATUS_OK) {
+		uerrf("\tMSD: Status phase: status = %d (!= OK), resetting", status);
+		_msd_bot_reset(lunp->msdp);
+		return MSD_BOTRESULT_ERROR;
+	}
+
+	/* validate CSW */
+	if ((actual_len != sizeof(csw))
+		|| (csw.dCSWSignature != MSD_CSW_SIGNATURE)
+		|| (csw.dCSWTag != lunp->msdp->tag)
+		|| (csw.bCSWStatus >= CSW_STATUS_PHASE_ERROR)) {
+		/* CSW is not valid */
+		uerrf("\tMSD: Status phase: Invalid CSW: len=%d, dCSWSignature=%x, dCSWTag=%x (expected %x), bCSWStatus=%d, resetting",
+				actual_len,
+				csw.dCSWSignature,
+				csw.dCSWTag,
+				lunp->msdp->tag,
+				csw.bCSWStatus);
+		_msd_bot_reset(lunp->msdp);
+		return MSD_BOTRESULT_ERROR;
+	}
+
+	/* check if CSW is meaningful */
+	if ((csw.bCSWStatus != CSW_STATUS_PHASE_ERROR)
+			&& (csw.dCSWDataResidue > tran->cbw->dCBWDataTransferLength)) {
+		/* CSW is not meaningful */
+		uerrf("\tMSD: Status phase: CSW not meaningful: bCSWStatus=%d, dCSWDataResidue=%u, dCBWDataTransferLength=%u, resetting",
+				csw.bCSWStatus,
+				csw.dCSWDataResidue,
+				tran->cbw->dCBWDataTransferLength);
+		_msd_bot_reset(lunp->msdp);
+		return MSD_BOTRESULT_ERROR;
+	}
+
+	if (csw.bCSWStatus == CSW_STATUS_PHASE_ERROR) {
+		uerr("\tMSD: Status phase: Phase error, resetting");
+		_msd_bot_reset(lunp->msdp);
+		return MSD_BOTRESULT_ERROR;
+	}
+
+	tran->data_processed = tran->cbw->dCBWDataTransferLength - csw.dCSWDataResidue;
+	if (data_actual_len < tran->data_processed) {
+		tran->data_processed = data_actual_len;
+	}
+
+	tran->csw_status = csw.bCSWStatus;
+
+	return MSD_BOTRESULT_OK;
+}
+
+
 /* ----------------------------------------------------- */
-/* 			SCSI Commands 								*/
+/* SCSI Commands                                         */
 /* ----------------------------------------------------- */
 
 /* Read 10 and Write 10 */
@@ -299,7 +428,7 @@ typedef struct {
 
 /* Request sense */
 #define SCSI_CMD_REQUEST_SENSE 					0x03
-PACKED_STRUCT {
+typedef PACKED_STRUCT {
 	uint8_t byte[18];
 } scsi_sense_response_t;
 
@@ -333,7 +462,7 @@ PACKED_STRUCT {
 
 /* Inquiry */
 #define SCSI_CMD_INQUIRY 						0x12
-PACKED_STRUCT {
+typedef PACKED_STRUCT {
 	uint8_t peripheral;
 	uint8_t removable;
 	uint8_t version;
@@ -349,14 +478,14 @@ PACKED_STRUCT {
 
 /* Read Capacity 10 */
 #define SCSI_CMD_READ_CAPACITY_10				0x25
-PACKED_STRUCT {
+typedef 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 {
+typedef PACKED_STRUCT {
 	uint8_t op_code;
 	uint8_t lun_immed;
 	uint8_t res1;
@@ -368,223 +497,187 @@ PACKED_STRUCT {
 /* 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 msd_result_t scsi_requestsense(USBHMassStorageLUNDriver *lunp, scsi_sense_response_t *resp);
 
-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) {
+static msd_result_t _scsi_perform_transaction(USBHMassStorageLUNDriver *lunp,
+		msd_transaction_t *transaction, 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;
+	msd_bot_result_t res;
+	res = _msd_bot_transaction(transaction, lunp, data);
+	if (res != MSD_BOTRESULT_OK) {
+		return (msd_result_t)res;
 	}
 
-
-	/* 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;
+	if (transaction->csw_status == CSW_STATUS_FAILED) {
+		if (transaction->cbw->CBWCB[0] != SCSI_CMD_REQUEST_SENSE) {
+			/* do auto-sense (except for SCSI_CMD_REQUEST_SENSE!) */
+			uwarn("\tMSD: Command failed, auto-sense");
+			USBH_DEFINE_BUFFER(scsi_sense_response_t sense);
+			if (scsi_requestsense(lunp, &sense) == MSD_RESULT_OK) {
+				uwarnf("\tMSD: REQUEST SENSE: Sense key=%x, ASC=%02x, ASCQ=%02x",
+						sense.byte[2] & 0xf, sense.byte[12], sense.byte[13]);
+			}
 		}
+		return MSD_RESULT_FAILED;
 	}
 
-
-	/* 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;
+	return MSD_RESULT_OK;
 }
 
-
 static msd_result_t scsi_inquiry(USBHMassStorageLUNDriver *lunp, scsi_inquiry_response_t *resp) {
+	USBH_DEFINE_BUFFER(msd_cbw_t cbw);
 	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;
+	memset(cbw.CBWCB, 0, sizeof(cbw.CBWCB));
+	cbw.dCBWDataTransferLength = sizeof(scsi_inquiry_response_t);
+	cbw.bmCBWFlags = MSD_CBWFLAGS_D2H;
+	cbw.bCBWCBLength = 6;
+	cbw.CBWCB[0] = SCSI_CMD_INQUIRY;
+	cbw.CBWCB[4] = sizeof(scsi_inquiry_response_t);
+	transaction.cbw = &cbw;
+
+	res = _scsi_perform_transaction(lunp, &transaction, resp);
+	if (res == MSD_RESULT_OK) {
+		//transaction is OK; check length
+		if (transaction.data_processed < cbw.dCBWDataTransferLength) {
+			res = MSD_RESULT_TRANSPORT_ERROR;
+		}
 	}
+
 	return res;
 }
 
 static msd_result_t scsi_requestsense(USBHMassStorageLUNDriver *lunp, scsi_sense_response_t *resp) {
+	USBH_DEFINE_BUFFER(msd_cbw_t cbw);
 	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;
+	memset(cbw.CBWCB, 0, sizeof(cbw.CBWCB));
+	cbw.dCBWDataTransferLength = sizeof(scsi_sense_response_t);
+	cbw.bmCBWFlags = MSD_CBWFLAGS_D2H;
+	cbw.bCBWCBLength = 12;
+	cbw.CBWCB[0] = SCSI_CMD_REQUEST_SENSE;
+	cbw.CBWCB[4] = sizeof(scsi_sense_response_t);
+	transaction.cbw = &cbw;
+
+	res = _scsi_perform_transaction(lunp, &transaction, resp);
+	if (res == MSD_RESULT_OK) {
+		//transaction is OK; check length
+		if (transaction.data_processed < cbw.dCBWDataTransferLength) {
+			res = MSD_RESULT_TRANSPORT_ERROR;
+		}
 	}
+
 	return res;
 }
 
 static msd_result_t scsi_testunitready(USBHMassStorageLUNDriver *lunp) {
+	USBH_DEFINE_BUFFER(msd_cbw_t cbw);
 	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;
+	memset(cbw.CBWCB, 0, sizeof(cbw.CBWCB));
+	cbw.dCBWDataTransferLength = 0;
+	cbw.bmCBWFlags = MSD_CBWFLAGS_D2H;
+	cbw.bCBWCBLength = 6;
+	cbw.CBWCB[0] = SCSI_CMD_TEST_UNIT_READY;
+	transaction.cbw = &cbw;
 
-	res.tres = _msd_transaction(&transaction, lunp, NULL);
-	if (res.tres == MSD_TRANSACTIONRESULT_OK) {
-		res.cres = (msd_command_result_t) transaction.csw.bCSWStatus;
-	}
-	return res;
+	return _scsi_perform_transaction(lunp, &transaction, NULL);
 }
 
 static msd_result_t scsi_readcapacity10(USBHMassStorageLUNDriver *lunp, scsi_readcapacity10_response_t *resp) {
+	USBH_DEFINE_BUFFER(msd_cbw_t cbw);
 	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;
+	memset(cbw.CBWCB, 0, sizeof(cbw.CBWCB));
+	cbw.dCBWDataTransferLength = sizeof(scsi_readcapacity10_response_t);
+	cbw.bmCBWFlags = MSD_CBWFLAGS_D2H;
+	cbw.bCBWCBLength = 12;
+	cbw.CBWCB[0] = SCSI_CMD_READ_CAPACITY_10;
+	transaction.cbw = &cbw;
+
+	res = _scsi_perform_transaction(lunp, &transaction, resp);
+	if (res == MSD_RESULT_OK) {
+		//transaction is OK; check length
+		if (transaction.data_processed < cbw.dCBWDataTransferLength) {
+			res = MSD_RESULT_TRANSPORT_ERROR;
+		}
 	}
+
 	return res;
 }
 
 
-static msd_result_t scsi_read10(USBHMassStorageLUNDriver *lunp, uint32_t lba, uint16_t n, uint8_t *data) {
+static msd_result_t scsi_read10(USBHMassStorageLUNDriver *lunp, uint32_t lba, uint16_t n, uint8_t *data, uint32_t *actual_len) {
+	USBH_DEFINE_BUFFER(msd_cbw_t cbw);
 	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;
+	memset(cbw.CBWCB, 0, sizeof(cbw.CBWCB));
+	cbw.dCBWDataTransferLength = n * lunp->info.blk_size;
+	cbw.bmCBWFlags = MSD_CBWFLAGS_D2H;
+	cbw.bCBWCBLength = 10;
+	cbw.CBWCB[0] = SCSI_CMD_READ_10;
+	cbw.CBWCB[2] = (uint8_t)(lba >> 24);
+	cbw.CBWCB[3] = (uint8_t)(lba >> 16);
+	cbw.CBWCB[4] = (uint8_t)(lba >> 8);
+	cbw.CBWCB[5] = (uint8_t)(lba);
+	cbw.CBWCB[7] = (uint8_t)(n >> 8);
+	cbw.CBWCB[8] = (uint8_t)(n);
+	transaction.cbw = &cbw;
+
+	res = _scsi_perform_transaction(lunp, &transaction, data);
+	if (actual_len) {
+		*actual_len = transaction.data_processed;
 	}
+	if (res == MSD_RESULT_OK) {
+		//transaction is OK; check length
+		if (transaction.data_processed < cbw.dCBWDataTransferLength) {
+			res = MSD_RESULT_TRANSPORT_ERROR;
+		}
+	}
+
 	return res;
 }
 
-static msd_result_t scsi_write10(USBHMassStorageLUNDriver *lunp, uint32_t lba, uint16_t n, const uint8_t *data) {
+static msd_result_t scsi_write10(USBHMassStorageLUNDriver *lunp, uint32_t lba, uint16_t n, const uint8_t *data, uint32_t *actual_len) {
+	USBH_DEFINE_BUFFER(msd_cbw_t cbw);
 	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;
+	memset(cbw.CBWCB, 0, sizeof(cbw.CBWCB));
+	cbw.dCBWDataTransferLength = n * lunp->info.blk_size;
+	cbw.bmCBWFlags = MSD_CBWFLAGS_H2D;
+	cbw.bCBWCBLength = 10;
+	cbw.CBWCB[0] = SCSI_CMD_WRITE_10;
+	cbw.CBWCB[2] = (uint8_t)(lba >> 24);
+	cbw.CBWCB[3] = (uint8_t)(lba >> 16);
+	cbw.CBWCB[4] = (uint8_t)(lba >> 8);
+	cbw.CBWCB[5] = (uint8_t)(lba);
+	cbw.CBWCB[7] = (uint8_t)(n >> 8);
+	cbw.CBWCB[8] = (uint8_t)(n);
+	transaction.cbw = &cbw;
+
+	res = _scsi_perform_transaction(lunp, &transaction, (void *)data);
+	if (actual_len) {
+		*actual_len = transaction.data_processed;
+	}
+	if (res == MSD_RESULT_OK) {
+		//transaction is OK; check length
+		if (transaction.data_processed < cbw.dCBWDataTransferLength) {
+			res = MSD_RESULT_TRANSPORT_ERROR;
+		}
 	}
+
 	return res;
 }
 
 
 
 /*===========================================================================*/
-/* Block driver data/functions								 		 	 	 */
+/* Block driver data/functions                                               */
 /*===========================================================================*/
 
 USBHMassStorageLUNDriver MSBLKD[HAL_USBHMSD_MAX_LUNS];
@@ -600,39 +693,22 @@ static const struct USBHMassStorageDriverVMT blk_vmt = {
 	(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;
+static void _lun_object_deinit(USBHMassStorageLUNDriver *lunp) {
+	osalDbgCheck(lunp != NULL);
+	chSemWait(&lunp->sem);
+	lunp->msdp = NULL;
+	lunp->next = NULL;
+	memset(&lunp->info, 0, sizeof(lunp->info));
+	lunp->state = BLK_STOP;
+	chSemSignal(&lunp->sem);
 }
 
-void usbhmsdLUNObjectInit(USBHMassStorageLUNDriver *lunp) {
+static void _lun_object_init(USBHMassStorageLUNDriver *lunp) {
 	osalDbgCheck(lunp != NULL);
 	memset(lunp, 0, sizeof(*lunp));
 	lunp->vmt = &blk_vmt;
 	lunp->state = BLK_STOP;
+	chSemObjectInit(&lunp->sem, 1);
 	/* Unnecessary because of the memset:
 		lunp->msdp = NULL;
 		lunp->next = NULL;
@@ -640,150 +716,111 @@ void usbhmsdLUNObjectInit(USBHMassStorageLUNDriver *lunp) {
 	*/
 }
 
-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;
+	osalDbgCheck(lunp != NULL);
+	osalDbgCheck(lunp->msdp != NULL);
 	msd_result_t res;
 
-	osalDbgCheck(msdp != NULL);
-	osalSysLock();
-	//osalDbgAssert((lunp->state == BLK_ACTIVE) || (lunp->state == BLK_READY),
-    //            "invalid state");
+	chSemWait(&lunp->sem);
+	osalDbgAssert((lunp->state == BLK_READY) || (lunp->state == BLK_ACTIVE), "invalid state");
 	if (lunp->state == BLK_READY) {
-	    osalSysUnlock();
+		chSemSignal(&lunp->sem);
 		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;
-	}
+    {
+		USBH_DEFINE_BUFFER(scsi_inquiry_response_t inq);
+		uinfo("INQUIRY...");
+		res = scsi_inquiry(lunp, &inq);
+		if (res == MSD_RESULT_DISCONNECTED) {
+			goto failed;
+		} else if (res == MSD_RESULT_TRANSPORT_ERROR) {
+			//retry?
+			goto failed;
+		} else if (res == MSD_RESULT_FAILED) {
+			//retry?
+			goto failed;
+		}
 
-	uinfof("\tPDT=%02x", u.inq.peripheral & 0x1f);
-	if (u.inq.peripheral != 0) {
-		uerr("\tUnsupported PDT");
-		goto failed;
+		uinfof("\tPDT=%02x", inq.peripheral & 0x1f);
+		if (inq.peripheral != 0) {
+			uerr("\tUnsupported PDT");
+			goto failed;
+		}
 	}
 
 	// Test if unit ready
-	uint8_t i;
+    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");
+		if (res == MSD_RESULT_DISCONNECTED) {
 			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");
+		} else if (res == MSD_RESULT_TRANSPORT_ERROR) {
+			//retry?
 			goto failed;
+		} else if (res == MSD_RESULT_FAILED) {
+			uinfo("\tTEST UNIT READY: Command Failed, retry");
+			osalThreadSleepMilliseconds(200);
+			continue;
 		}
 		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;
+	{
+		USBH_DEFINE_BUFFER(scsi_readcapacity10_response_t cap);
+		// Read capacity
+		uinfo("READ CAPACITY(10)...");
+		res = scsi_readcapacity10(lunp, &cap);
+		if (res == MSD_RESULT_DISCONNECTED) {
+			goto failed;
+		} else if (res == MSD_RESULT_TRANSPORT_ERROR) {
+			//retry?
+			goto failed;
+		} else if (res == MSD_RESULT_FAILED) {
+			//retry?
+			goto failed;
+		}
+
+		lunp->info.blk_size = __REV(cap.block_size);
+		lunp->info.blk_num = __REV(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();
-
+	chSemSignal(&lunp->sem);
 	return HAL_SUCCESS;
 
   /* Connection failed, state reset to BLK_ACTIVE.*/
 failed:
-	osalMutexUnlock(&msdp->mtx);
-	osalSysLock();
+	uinfo("MSD Connect failed.");
 	lunp->state = BLK_ACTIVE;
-    osalSysUnlock();
+	chSemSignal(&lunp->sem);
 	return HAL_FAILED;
 }
 
-
 bool usbhmsdLUNDisconnect(USBHMassStorageLUNDriver *lunp) {
 	osalDbgCheck(lunp != NULL);
-	osalSysLock();
-	osalDbgAssert((lunp->state == BLK_ACTIVE) || (lunp->state == BLK_READY),
-				"invalid state");
+
+	chSemWait(&lunp->sem);
+	osalDbgAssert((lunp->state == BLK_READY) || (lunp->state == BLK_ACTIVE), "invalid state");
 	if (lunp->state == BLK_ACTIVE) {
-		osalSysUnlock();
+		chSemSignal(&lunp->sem);
 		return HAL_SUCCESS;
 	}
 	lunp->state = BLK_DISCONNECTING;
-	osalSysUnlock();
 
-	//TODO: complete
+	//TODO: complete: sync, etc.
 
-	osalSysLock();
 	lunp->state = BLK_ACTIVE;
-	osalSysUnlock();
+	chSemSignal(&lunp->sem);
+
 	return HAL_SUCCESS;
 }
 
@@ -794,34 +831,29 @@ bool usbhmsdLUNRead(USBHMassStorageLUNDriver *lunp, uint32_t startblk,
 	bool ret = HAL_FAILED;
 	uint16_t blocks;
 	msd_result_t res;
+	uint32_t actual_len;
 
-	osalSysLock();
+	chSemWait(&lunp->sem);
 	if (lunp->state != BLK_READY) {
-		osalSysUnlock();
+		chSemSignal(&lunp->sem);
 		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");
+		res = scsi_read10(lunp, startblk, blocks, buffer, &actual_len);
+		if (res == MSD_RESULT_DISCONNECTED) {
 			goto exit;
-		} else if (res.cres == MSD_COMMANDRESULT_FAILED) {
-			//TODO: request sense, and act appropriately
-			uerr("\tREAD (10): Command Failed");
-			_requestsense(lunp);
+		} else if (res == MSD_RESULT_TRANSPORT_ERROR) {
+			//retry?
 			goto exit;
-		} else if (res.cres == MSD_COMMANDRESULT_PHASE_ERROR) {
-			//TODO: Do reset, etc.
-			uerr("\tREAD (10): Command Phase Error");
+		} else if (res == MSD_RESULT_FAILED) {
+			//retry?
 			goto exit;
 		}
 		n -= blocks;
@@ -832,15 +864,8 @@ bool usbhmsdLUNRead(USBHMassStorageLUNDriver *lunp, uint32_t startblk,
 	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();
+	lunp->state = BLK_READY;
+	chSemSignal(&lunp->sem);
 	return ret;
 }
 
@@ -851,34 +876,29 @@ bool usbhmsdLUNWrite(USBHMassStorageLUNDriver *lunp, uint32_t startblk,
 	bool ret = HAL_FAILED;
 	uint16_t blocks;
 	msd_result_t res;
+	uint32_t actual_len;
 
-	osalSysLock();
+	chSemWait(&lunp->sem);
 	if (lunp->state != BLK_READY) {
-		osalSysUnlock();
+		chSemSignal(&lunp->sem);
 		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");
+		res = scsi_write10(lunp, startblk, blocks, buffer, &actual_len);
+		if (res == MSD_RESULT_DISCONNECTED) {
 			goto exit;
-		} else if (res.cres == MSD_COMMANDRESULT_FAILED) {
-			//TODO: request sense, and act appropriately
-			uerr("\tWRITE (10): Command Failed");
-			_requestsense(lunp);
+		} else if (res == MSD_RESULT_TRANSPORT_ERROR) {
+			//retry?
 			goto exit;
-		} else if (res.cres == MSD_COMMANDRESULT_PHASE_ERROR) {
-			//TODO: Do reset, etc.
-			uerr("\tWRITE (10): Command Phase Error");
+		} else if (res == MSD_RESULT_FAILED) {
+			//retry?
 			goto exit;
 		}
 		n -= blocks;
@@ -889,15 +909,8 @@ bool usbhmsdLUNWrite(USBHMassStorageLUNDriver *lunp, uint32_t startblk,
 	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();
+	lunp->state = BLK_READY;
+	chSemSignal(&lunp->sem);
 	return ret;
 }
 
@@ -911,29 +924,41 @@ bool usbhmsdLUNSync(USBHMassStorageLUNDriver *lunp) {
 bool usbhmsdLUNGetInfo(USBHMassStorageLUNDriver *lunp, BlockDeviceInfo *bdip) {
 	osalDbgCheck(lunp != NULL);
 	osalDbgCheck(bdip != NULL);
-	*bdip = lunp->info;
-	return HAL_SUCCESS;
+
+	osalSysLock();
+	if (lunp->state >= BLK_READY) {
+		*bdip = lunp->info;
+		osalSysUnlock();
+		return HAL_SUCCESS;
+	}
+	osalSysUnlock();
+	return HAL_FAILED;
 }
 
 bool usbhmsdLUNIsInserted(USBHMassStorageLUNDriver *lunp) {
 	osalDbgCheck(lunp != NULL);
-	blkstate_t state;
-	osalSysLock();
-	state = lunp->state;
-	osalSysUnlock();
-	return (state >= BLK_ACTIVE);
+	return (lunp->state >= BLK_ACTIVE);
 }
 
 bool usbhmsdLUNIsProtected(USBHMassStorageLUNDriver *lunp) {
 	osalDbgCheck(lunp != NULL);
+	//TODO: Implement
 	return FALSE;
 }
 
-void usbhmsdObjectInit(USBHMassStorageDriver *msdp) {
+static void _msd_object_init(USBHMassStorageDriver *msdp) {
 	osalDbgCheck(msdp != NULL);
 	memset(msdp, 0, sizeof(*msdp));
 	msdp->info = &usbhmsdClassDriverInfo;
-	osalMutexObjectInit(&msdp->mtx);
 }
 
+static void _msd_init(void) {
+	uint8_t i;
+	for (i = 0; i < HAL_USBHMSD_MAX_INSTANCES; i++) {
+		_msd_object_init(&USBHMSD[i]);
+	}
+	for (i = 0; i < HAL_USBHMSD_MAX_LUNS; i++) {
+		_lun_object_init(&MSBLKD[i]);
+	}
+}
 #endif
diff --git a/os/hal/src/usbh/hal_usbh_uvc.c b/os/hal/src/usbh/hal_usbh_uvc.c
index 09a0f1d..a795cd8 100644
--- a/os/hal/src/usbh/hal_usbh_uvc.c
+++ b/os/hal/src/usbh/hal_usbh_uvc.c
@@ -1,6 +1,6 @@
 /*
-    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio
-              Copyright (C) 2015 Diego Ismirlian, TISA, (dismirlian (at) google's mail)
+    ChibiOS - Copyright (C) 2006..2017 Giovanni Di Sirio
+              Copyright (C) 2015..2017 Diego Ismirlian, (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.
@@ -13,10 +13,9 @@
     WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
     See the License for the specific language governing permissions and
     limitations under the License.
-*/
+ */
 
 #include "hal.h"
-#include "hal_usbh.h"
 
 #if HAL_USBH_USE_UVC
 
@@ -28,6 +27,10 @@
 #error "USBHUVC needs HAL_USBH_USE_IAD"
 #endif
 
+#include <string.h>
+#include "usbh/dev/uvc.h"
+#include "usbh/internal.h"
+
 #if USBHUVC_DEBUG_ENABLE_TRACE
 #define udbgf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)
 #define udbg(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)
@@ -61,28 +64,673 @@
 #endif
 
 
-static usbh_baseclassdriver_t *uvc_load(usbh_device_t *dev,
+USBHUVCDriver USBHUVCD[HAL_USBHUVC_MAX_INSTANCES];
+
+static void _uvc_init(void);
+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 void _uvc_unload(usbh_baseclassdriver_t *drv);
 
 static const usbh_classdriver_vmt_t class_driver_vmt = {
-	uvc_load,
-	uvc_unload
+	_uvc_init,
+	_uvc_load,
+	_uvc_unload
 };
 const usbh_classdriverinfo_t usbhuvcClassDriverInfo = {
-	0x0e, 0x03, 0x00, "UVC", &class_driver_vmt
+	"UVC", &class_driver_vmt
 };
 
+static bool _request(USBHUVCDriver *uvcdp,
+		uint8_t bRequest, uint8_t entity, uint8_t control,
+		uint16_t wLength, uint8_t *data, uint8_t interf) {
+
+	usbh_urbstatus_t res;
+
+	if (bRequest & 0x80) {
+		res = usbhControlRequest(uvcdp->dev,
+				USBH_REQTYPE_CLASSIN(USBH_REQTYPE_RECIP_INTERFACE),
+				bRequest,
+				((control) << 8),
+				(interf) | ((entity) << 8),
+				wLength, data);
+	} else {
+		res = usbhControlRequest(uvcdp->dev,
+				USBH_REQTYPE_CLASSOUT(USBH_REQTYPE_RECIP_INTERFACE),
+				bRequest,
+				((control) << 8),
+				(interf) | ((entity) << 8),
+				wLength, data);
+	}
+
+	if (res != USBH_URBSTATUS_OK)
+		return HAL_FAILED;
+
+	 return HAL_SUCCESS;
+}
+
+bool usbhuvcVCRequest(USBHUVCDriver *uvcdp,
+		uint8_t bRequest, uint8_t entity, uint8_t control,
+		uint16_t wLength, uint8_t *data) {
+	return _request(uvcdp, bRequest, entity, control, wLength, data, if_get(&uvcdp->ivc)->bInterfaceNumber);
+}
+
+bool usbhuvcVSRequest(USBHUVCDriver *uvcdp,
+		uint8_t bRequest, uint8_t control,
+		uint16_t wLength, uint8_t *data) {
+
+	return _request(uvcdp, bRequest, 0, control, wLength, data, if_get(&uvcdp->ivs)->bInterfaceNumber);
+}
+
+static bool _set_vs_alternate(USBHUVCDriver *uvcdp, uint16_t min_ep_size) {
+
+	if (min_ep_size == 0) {
+		uinfo("Selecting Alternate setting 0");
+		return usbhStdReqSetInterface(uvcdp->dev, if_get(&uvcdp->ivs)->bInterfaceNumber, 0);
+	}
+
+	if_iterator_t iif = uvcdp->ivs;
+	generic_iterator_t iep;
+	const usbh_endpoint_descriptor_t *ep = NULL;
+	uint8_t alt = 0;
+	uint16_t sz = 0xffff;
+
+	uinfof("Searching alternate setting with min_ep_size=%d", min_ep_size);
+
+	for (; iif.valid; if_iter_next(&iif)) {
+		const usbh_interface_descriptor_t *const ifdesc = if_get(&iif);
+
+		if ((ifdesc->bInterfaceClass != UVC_CC_VIDEO)
+				|| (ifdesc->bInterfaceSubClass != UVC_SC_VIDEOSTREAMING))
+			continue;
+
+		uinfof("\tScanning alternate setting=%d", ifdesc->bAlternateSetting);
+
+		if (ifdesc->bNumEndpoints == 0)
+			continue;
+
+		for (ep_iter_init(&iep, &iif); iep.valid; ep_iter_next(&iep)) {
+			const usbh_endpoint_descriptor_t *const epdesc = ep_get(&iep);
+			if (((epdesc->bmAttributes & 0x03) == USBH_EPTYPE_ISO)
+					&& ((epdesc->bEndpointAddress & 0x80) ==  USBH_EPDIR_IN)) {
+
+				uinfof("\t  Endpoint wMaxPacketSize = %d", epdesc->wMaxPacketSize);
+
+				if (epdesc->wMaxPacketSize >= min_ep_size) {
+					if (epdesc->wMaxPacketSize < sz) {
+						uinfo("\t    Found new optimal alternate setting");
+						sz = epdesc->wMaxPacketSize;
+						alt = ifdesc->bAlternateSetting;
+						ep = epdesc;
+					}
+				}
+			}
+		}
+	}
+
+	if (ep && alt) {
+		uinfof("\tSelecting Alternate setting %d", alt);
+		if (usbhStdReqSetInterface(uvcdp->dev, if_get(&uvcdp->ivs)->bInterfaceNumber, alt) == HAL_SUCCESS) {
+			usbhEPObjectInit(&uvcdp->ep_iso, uvcdp->dev, ep);
+			usbhEPSetName(&uvcdp->ep_iso, "UVC[ISO ]");
+			return HAL_SUCCESS;
+		}
+	}
+
+	return HAL_FAILED;
+}
+
+#if	USBH_DEBUG_ENABLE && USBHUVC_DEBUG_ENABLE_INFO
+void usbhuvcPrintProbeCommit(const usbh_uvc_ctrl_vs_probecommit_data_t *pc) {
+
+	//uinfof("UVC: probe/commit data:");
+	uinfof("\tbmHint=%04x", pc->bmHint);
+	uinfof("\tbFormatIndex=%d, bFrameIndex=%d, dwFrameInterval=%u",
+			pc->bFormatIndex, pc->bFrameIndex, pc->dwFrameInterval);
+	uinfof("\twKeyFrameRate=%d, wPFrameRate=%d, wCompQuality=%u, wCompWindowSize=%u",
+			pc->wKeyFrameRate, pc->wPFrameRate, pc->wCompQuality, pc->wCompWindowSize);
+	uinfof("\twDelay=%d", pc->wDelay);
+	uinfof("\tdwMaxVideoFrameSize=%u", pc->dwMaxVideoFrameSize);
+	uinfof("\tdwMaxPayloadTransferSize=%u", pc->dwMaxPayloadTransferSize);
+/*	uinfof("\tdwClockFrequency=%u", pc->dwClockFrequency);
+	uinfof("\tbmFramingInfo=%02x", pc->bmFramingInfo);
+	uinfof("\tbPreferedVersion=%d, bMinVersion=%d, bMaxVersion=%d",
+			pc->bPreferedVersion, pc->bMinVersion, pc->bMaxVersion); */
+}
+#endif
+
+static void _post(USBHUVCDriver *uvcdp, usbh_urb_t *urb, memory_pool_t *mp, uint16_t type) {
+	usbhuvc_message_base_t *const msg = (usbhuvc_message_base_t *)((uint8_t *)urb->buff - offsetof(usbhuvc_message_data_t, data));
+	msg->timestamp = osalOsGetSystemTimeX();
+
+	usbhuvc_message_base_t *const new_msg = (usbhuvc_message_base_t *)chPoolAllocI(mp);
+	if (new_msg != NULL) {
+		/* allocated the new buffer, now try to post the message to the mailbox */
+		if (chMBPostI(&uvcdp->mb, (msg_t)msg) == MSG_OK) {
+			/* everything OK, complete the missing fields */
+			msg->type = type;
+			msg->length = urb->actualLength;
+
+			/* change the URB's buffer to the newly allocated one */
+			urb->buff = ((usbhuvc_message_data_t *)new_msg)->data;
+		} else {
+			/* couldn't post the message, free the newly allocated buffer */
+			uerr("UVC: error, mailbox overrun");
+			chPoolFreeI(&uvcdp->mp_status, new_msg);
+		}
+	} else {
+		uerrf("UVC: error, %s pool overrun", mp == &uvcdp->mp_data ? "data" : "status");
+	}
+}
+
+static void _cb_int(usbh_urb_t *urb) {
+	USBHUVCDriver *uvcdp = (USBHUVCDriver *)urb->userData;
+
+	switch (urb->status) {
+	case USBH_URBSTATUS_OK:
+		if (urb->actualLength >= 2) {
+			_post(uvcdp, urb, &uvcdp->mp_status, USBHUVC_MESSAGETYPE_STATUS);
+		} else {
+			uerrf("UVC: INT IN, actualLength=%d", urb->actualLength);
+		}
+		break;
+	case USBH_URBSTATUS_TIMEOUT:	/* the device NAKed */
+		udbg("UVC: INT IN no info");
+		break;
+	case USBH_URBSTATUS_DISCONNECTED:
+	case USBH_URBSTATUS_CANCELLED:
+		uwarn("UVC: INT IN status = DISCONNECTED/CANCELLED, aborting");
+		return;
+	default:
+		uerrf("UVC: INT IN error, unexpected status = %d", urb->status);
+		break;
+	}
+
+	usbhURBObjectResetI(urb);
+	usbhURBSubmitI(urb);
+}
+
+static void _cb_iso(usbh_urb_t *urb) {
+	USBHUVCDriver *uvcdp = (USBHUVCDriver *)urb->userData;
+
+	if ((urb->status == USBH_URBSTATUS_DISCONNECTED)
+			|| (urb->status == USBH_URBSTATUS_CANCELLED)) {
+		uwarn("UVC: ISO IN status = DISCONNECTED/CANCELLED, aborting");
+		return;
+	}
+
+	if (urb->status != USBH_URBSTATUS_OK) {
+		uerrf("UVC: ISO IN error, unexpected status = %d", urb->status);
+	} else if (urb->actualLength >= 2) {
+		const uint8_t *const buff = (const uint8_t *)urb->buff;
+		if (buff[0] < 2) {
+			uerrf("UVC: ISO IN, bHeaderLength=%d", buff[0]);
+		} else if (buff[0] > urb->actualLength) {
+			uerrf("UVC: ISO IN, bHeaderLength=%d > actualLength=%d", buff[0], urb->actualLength);
+		} else {
+			udbgf("UVC: ISO IN len=%d, hdr=%d, FID=%d, EOF=%d, ERR=%d, EOH=%d",
+						urb->actualLength,
+						buff[0],
+						buff[1] & UVC_HDR_FID,
+						buff[1] & UVC_HDR_EOF,
+						buff[1] & UVC_HDR_ERR,
+						buff[1] & UVC_HDR_EOH);
+
+			if ((urb->actualLength > buff[0])
+					|| (buff[1] & (UVC_HDR_EOF | UVC_HDR_ERR))) {
+				_post(uvcdp, urb, &uvcdp->mp_data, USBHUVC_MESSAGETYPE_DATA);
+			} else {
+				udbgf("UVC: ISO IN skip: len=%d, hdr=%d, FID=%d, EOF=%d, ERR=%d, EOH=%d",
+						urb->actualLength,
+						buff[0],
+						buff[1] & UVC_HDR_FID,
+						buff[1] & UVC_HDR_EOF,
+						buff[1] & UVC_HDR_ERR,
+						buff[1] & UVC_HDR_EOH);
+			}
+		}
+	} else if (urb->actualLength > 0) {
+		uerrf("UVC: ISO IN, actualLength=%d", urb->actualLength);
+	}
+
+	usbhURBObjectResetI(urb);
+	usbhURBSubmitI(urb);
+}
+
+
+bool usbhuvcStreamStart(USBHUVCDriver *uvcdp, uint16_t min_ep_sz) {
+	bool ret = HAL_FAILED;
+
+	osalSysLock();
+	osalDbgCheck(uvcdp && (uvcdp->state != USBHUVC_STATE_UNINITIALIZED) &&
+					(uvcdp->state != USBHUVC_STATE_BUSY));
+	if (uvcdp->state == USBHUVC_STATE_STREAMING) {
+		osalSysUnlock();
+		return HAL_SUCCESS;
+	}
+	if (uvcdp->state != USBHUVC_STATE_READY) {
+		osalSysUnlock();
+		return HAL_FAILED;
+	}
+	uvcdp->state = USBHUVC_STATE_BUSY;
+	osalSysUnlock();
+
+	uint32_t workramsz;
+	const uint8_t *elem;
+	uint32_t datapackets;
+	uint32_t data_sz;
+
+	//set the alternate setting
+	if (_set_vs_alternate(uvcdp, min_ep_sz) != HAL_SUCCESS)
+		goto exit;
+
+	//reserve working RAM
+	data_sz = (uvcdp->ep_iso.wMaxPacketSize + sizeof(usbhuvc_message_data_t) + 3) & ~3;
+	datapackets = HAL_USBHUVC_WORK_RAM_SIZE / data_sz;
+	if (datapackets == 0) {
+		uerr("Not enough work RAM");
+		goto failed;
+	}
+
+	workramsz = datapackets * data_sz;
+	uinfof("Reserving %u bytes of RAM (%d data packets of %d bytes)", workramsz, datapackets, data_sz);
+	if (datapackets > (HAL_USBHUVC_MAX_MAILBOX_SZ - HAL_USBHUVC_STATUS_PACKETS_COUNT)) {
+		uwarn("Mailbox may overflow, use a larger HAL_USBHUVC_MAX_MAILBOX_SZ. UVC will under-utilize the assigned work RAM.");
+	}
+	chMBResumeX(&uvcdp->mb);
+
+	uvcdp->mp_data_buffer = chHeapAlloc(NULL, workramsz);
+	if (uvcdp->mp_data_buffer == NULL) {
+		uerr("Couldn't reserve RAM");
+		goto failed;
+	}
+
+	//initialize the mempool
+	chPoolObjectInit(&uvcdp->mp_data, data_sz, NULL);
+	elem = (const uint8_t *)uvcdp->mp_data_buffer;
+	while (datapackets--) {
+		chPoolFree(&uvcdp->mp_data, (void *)elem);
+		elem += data_sz;
+	}
+
+	//open the endpoint
+	usbhEPOpen(&uvcdp->ep_iso);
+
+	//allocate 1 buffer and submit the first transfer
+	{
+		usbhuvc_message_data_t *const msg = (usbhuvc_message_data_t *)chPoolAlloc(&uvcdp->mp_data);
+		osalDbgCheck(msg);
+		usbhURBObjectInit(&uvcdp->urb_iso, &uvcdp->ep_iso, _cb_iso, uvcdp, msg->data, uvcdp->ep_iso.wMaxPacketSize);
+	}
+
+	usbhURBSubmit(&uvcdp->urb_iso);
+
+	ret = HAL_SUCCESS;
+	goto exit;
+
+failed:
+	_set_vs_alternate(uvcdp, 0);
+	if (uvcdp->mp_data_buffer)
+		chHeapFree(uvcdp->mp_data_buffer);
+
+exit:
+	osalSysLock();
+	if (ret == HAL_SUCCESS)
+		uvcdp->state = USBHUVC_STATE_STREAMING;
+	else
+		uvcdp->state = USBHUVC_STATE_READY;
+	osalSysUnlock();
+	return ret;
+}
+
+bool usbhuvcStreamStop(USBHUVCDriver *uvcdp) {
+	osalSysLock();
+	osalDbgCheck(uvcdp && (uvcdp->state != USBHUVC_STATE_UNINITIALIZED) &&
+					(uvcdp->state != USBHUVC_STATE_BUSY));
+	if (uvcdp->state != USBHUVC_STATE_STREAMING) {
+		osalSysUnlock();
+		return HAL_SUCCESS;
+	}
+	uvcdp->state = USBHUVC_STATE_BUSY;
 
-static usbh_baseclassdriver_t *uvc_load(usbh_device_t *dev, const uint8_t *descriptor, uint16_t rem) {
-	(void)dev;
-	(void)descriptor;
-	(void)rem;
+	//close the ISO endpoint
+	usbhEPCloseS(&uvcdp->ep_iso);
+
+	//purge the mailbox
+	chMBResetI(&uvcdp->mb);		//TODO: the status messages are lost!!
+	chMtxLockS(&uvcdp->mtx);
+	osalSysUnlock();
+
+	//free the working memory
+	chHeapFree(uvcdp->mp_data_buffer);
+	uvcdp->mp_data_buffer = 0;
+
+	//set alternate setting to 0
+	_set_vs_alternate(uvcdp, 0);
+
+	osalSysLock();
+	uvcdp->state = USBHUVC_STATE_READY;
+	chMtxUnlockS(&uvcdp->mtx);
+	osalSysUnlock();
+	return HAL_SUCCESS;
+}
+
+bool usbhuvcFindVSDescriptor(USBHUVCDriver *uvcdp,
+		generic_iterator_t *ics,
+		uint8_t bDescriptorSubtype,
+		bool start) {
+
+	if (start)
+		cs_iter_init(ics, (generic_iterator_t *)&uvcdp->ivs);
+	else
+		cs_iter_next(ics);
+
+	for (; ics->valid; cs_iter_next(ics)) {
+		if (ics->curr[1] != UVC_CS_INTERFACE)
+			break;
+		if (ics->curr[2] == bDescriptorSubtype)
+			return HAL_SUCCESS;
+		if (!start)
+			break;
+	}
+	return HAL_FAILED;
+}
+
+void usbhuvcResetPC(USBHUVCDriver *uvcdp) {
+	memset(&uvcdp->pc, 0, sizeof(uvcdp->pc));
+}
+
+bool usbhuvcProbe(USBHUVCDriver *uvcdp) {
+//	memset(&uvcdp->pc_min, 0, sizeof(uvcdp->pc_min));
+//	memset(&uvcdp->pc_max, 0, sizeof(uvcdp->pc_max));
+
+	if (usbhuvcVSRequest(uvcdp, UVC_SET_CUR, UVC_CTRL_VS_PROBE_CONTROL, sizeof(uvcdp->pc), (uint8_t *)&uvcdp->pc) != HAL_SUCCESS)
+		return HAL_FAILED;
+	if (usbhuvcVSRequest(uvcdp, UVC_GET_CUR, UVC_CTRL_VS_PROBE_CONTROL, sizeof(uvcdp->pc), (uint8_t *)&uvcdp->pc) != HAL_SUCCESS)
+		return HAL_FAILED;
+	if (usbhuvcVSRequest(uvcdp, UVC_GET_MAX, UVC_CTRL_VS_PROBE_CONTROL, sizeof(uvcdp->pc_max), (uint8_t *)&uvcdp->pc_max) != HAL_SUCCESS)
+		return HAL_FAILED;
+	if (usbhuvcVSRequest(uvcdp, UVC_GET_MIN, UVC_CTRL_VS_PROBE_CONTROL, sizeof(uvcdp->pc_min), (uint8_t *)&uvcdp->pc_min) != HAL_SUCCESS)
+		return HAL_FAILED;
+	return HAL_SUCCESS;
+}
+
+bool usbhuvcCommit(USBHUVCDriver *uvcdp) {
+	if (usbhuvcVSRequest(uvcdp, UVC_SET_CUR, UVC_CTRL_VS_COMMIT_CONTROL, sizeof(uvcdp->pc), (uint8_t *)&uvcdp->pc) != HAL_SUCCESS)
+		return HAL_FAILED;
+
+	osalSysLock();
+	if (uvcdp->state == USBHUVC_STATE_ACTIVE)
+		uvcdp->state = USBHUVC_STATE_READY;
+	osalSysUnlock();
+	return HAL_SUCCESS;
+}
+
+uint32_t usbhuvcEstimateRequiredEPSize(USBHUVCDriver *uvcdp, const uint8_t *formatdesc,
+		const uint8_t *framedesc, uint32_t dwFrameInterval) {
+
+	osalDbgCheck(framedesc);
+	osalDbgCheck(framedesc[0] > 3);
+	osalDbgCheck(framedesc[1] == UVC_CS_INTERFACE);
+	osalDbgCheck(formatdesc);
+	osalDbgCheck(formatdesc[0] > 3);
+	osalDbgCheck(formatdesc[1] == UVC_CS_INTERFACE);
+
+	uint16_t w, h, div, mul;
+	uint8_t bpp;
+
+	switch (framedesc[2]) {
+	case UVC_VS_FRAME_MJPEG: {
+		const usbh_uvc_frame_mjpeg_t *frame = (const usbh_uvc_frame_mjpeg_t *)framedesc;
+		//const usbh_uvc_format_mjpeg_t *fmt = (const usbh_uvc_format_mjpeg_t *)formatdesc;
+		w = frame->wWidth;
+		h = frame->wHeight;
+		bpp = 16;	//TODO: check this!!
+		mul = 1;
+		div = 5;	//TODO: check this estimate
+	}	break;
+	case UVC_VS_FRAME_UNCOMPRESSED: {
+		const usbh_uvc_frame_uncompressed_t *frame = (const usbh_uvc_frame_uncompressed_t *)framedesc;
+		const usbh_uvc_format_uncompressed *fmt = (const usbh_uvc_format_uncompressed *)formatdesc;
+		w = frame->wWidth;
+		h = frame->wHeight;
+		bpp = fmt->bBitsPerPixel;
+		mul = div = 1;
+	}	break;
+	default:
+		uwarn("Unsupported format");
+		return 0xffffffff;
+	}
+
+	uint32_t sz = w * h / 8 * bpp;
+	sz *= 10000000UL / dwFrameInterval;
+	sz /= 1000;
+
+	if (uvcdp->dev->speed == USBH_DEVSPEED_HIGH)
+		div *= 8;
+
+	return (sz * mul) / div + 12;
+}
+
+static usbh_baseclassdriver_t *_uvc_load(usbh_device_t *dev, const uint8_t *descriptor, uint16_t rem) {
+
+	USBHUVCDriver *uvcdp;
+	uint8_t i;
+
+	if (_usbh_match_descriptor(descriptor, rem, USBH_DT_INTERFACE_ASSOCIATION,
+			0x0e, 0x03, 0x00) != HAL_SUCCESS)
+		return NULL;
+
+	/* alloc driver */
+	for (i = 0; i < HAL_USBHUVC_MAX_INSTANCES; i++) {
+		if (USBHUVCD[i].dev == NULL) {
+			uvcdp = &USBHUVCD[i];
+			goto alloc_ok;
+		}
+	}
+
+	uwarn("Can't alloc UVC driver");
+
+	/* can't alloc */
 	return NULL;
+
+alloc_ok:
+	/* initialize the driver's variables */
+	uvcdp->ivc.curr = uvcdp->ivs.curr = NULL;
+
+	usbhEPSetName(&dev->ctrl, "UVC[CTRL]");
+
+	const usbh_ia_descriptor_t *iad = (const usbh_ia_descriptor_t *)descriptor;
+	if_iterator_t iif;
+	generic_iterator_t ics;
+	generic_iterator_t iep;
+
+	iif.iad = iad;
+	iif.curr = descriptor;
+	iif.rem = rem;
+
+	for (if_iter_next(&iif); iif.valid; if_iter_next(&iif)) {
+		if (iif.iad != iad) break;
+
+		const usbh_interface_descriptor_t *const ifdesc = if_get(&iif);
+		if (ifdesc->bInterfaceClass != UVC_CC_VIDEO) {
+			uwarnf("Skipping Interface %d (class != UVC_CC_VIDEO)",
+					ifdesc->bInterfaceNumber);
+			continue;
+		}
+
+		uinfof("Interface %d, Alt=%d, Class=UVC_CC_VIDEO, Subclass=%02x",
+				ifdesc->bInterfaceNumber,
+				ifdesc->bAlternateSetting,
+				ifdesc->bInterfaceSubClass);
+
+		switch (ifdesc->bInterfaceSubClass) {
+		case UVC_SC_VIDEOCONTROL:
+			if (uvcdp->ivc.curr == NULL) {
+				uvcdp->ivc = iif;
+			}
+			for (cs_iter_init(&ics, (generic_iterator_t *)&iif); ics.valid; cs_iter_next(&ics)) {
+				if (ics.curr[1] != UVC_CS_INTERFACE) {
+					uwarnf("Unknown descriptor=%02X", ics.curr[1]);
+					continue;
+				}
+				switch (ics.curr[2]) {
+				case UVC_VC_HEADER:
+					uinfo("  VC_HEADER"); break;
+				case UVC_VC_INPUT_TERMINAL:
+					uinfof("    VC_INPUT_TERMINAL, ID=%d", ics.curr[3]); break;
+				case UVC_VC_OUTPUT_TERMINAL:
+					uinfof("    VC_OUTPUT_TERMINAL, ID=%d", ics.curr[3]); break;
+				case UVC_VC_SELECTOR_UNIT:
+					uinfof("    VC_SELECTOR_UNIT, ID=%d", ics.curr[3]); break;
+				case UVC_VC_PROCESSING_UNIT:
+					uinfof("    VC_PROCESSING_UNIT, ID=%d", ics.curr[3]); break;
+				case UVC_VC_EXTENSION_UNIT:
+					uinfof("    VC_EXTENSION_UNIT, ID=%d", ics.curr[3]); break;
+				default:
+					uwarnf("Unknown video bDescriptorSubtype=%02x", ics.curr[2]);
+					break;
+				}
+			}
+			break;
+		case UVC_SC_VIDEOSTREAMING:
+			if (uvcdp->ivs.curr == NULL) {
+				uvcdp->ivs = iif;
+			}
+			for (cs_iter_init(&ics, (generic_iterator_t *)&iif); ics.valid; cs_iter_next(&ics)) {
+				if (ics.curr[1] != UVC_CS_INTERFACE) {
+					uwarnf("Unknown descriptor=%02X", ics.curr[1]);
+					continue;
+				}
+				switch (ics.curr[2]) {
+				case UVC_VS_INPUT_HEADER:
+					uinfo("  VS_INPUT_HEADER"); break;
+				case UVC_VS_OUTPUT_HEADER:
+					uinfo("  VS_OUTPUT_HEADER"); break;
+				case UVC_VS_STILL_IMAGE_FRAME:
+					uinfo("    VS_STILL_IMAGE_FRAME"); break;
+
+				case UVC_VS_FORMAT_UNCOMPRESSED:
+					uinfof("    VS_FORMAT_UNCOMPRESSED, bFormatIndex=%d", ics.curr[3]); break;
+				case UVC_VS_FORMAT_MPEG2TS:
+					uinfof("    VS_FORMAT_MPEG2TS, bFormatIndex=%d", ics.curr[3]); break;
+				case UVC_VS_FORMAT_DV:
+					uinfof("    VS_FORMAT_DV, bFormatIndex=%d", ics.curr[3]); break;
+				case UVC_VS_FORMAT_MJPEG:
+					uinfof("    VS_FORMAT_MJPEG, bFormatIndex=%d", ics.curr[3]); break;
+				case UVC_VS_FORMAT_FRAME_BASED:
+					uinfof("    VS_FORMAT_FRAME_BASED, bFormatIndex=%d", ics.curr[3]); break;
+				case UVC_VS_FORMAT_STREAM_BASED:
+					uinfof("    VS_FORMAT_STREAM_BASED, bFormatIndex=%d", ics.curr[3]); break;
+
+				case UVC_VS_FRAME_UNCOMPRESSED:
+					uinfof("      VS_FRAME_UNCOMPRESSED, bFrameIndex=%d", ics.curr[3]); break;
+				case UVC_VS_FRAME_MJPEG:
+					uinfof("      VS_FRAME_MJPEG, bFrameIndex=%d", ics.curr[3]); break;
+				case UVC_VS_FRAME_FRAME_BASED:
+					uinfof("      VS_FRAME_FRAME_BASED, bFrameIndex=%d", ics.curr[3]); break;
+
+				case UVC_VS_COLOR_FORMAT:
+					uinfo("      VS_COLOR_FORMAT"); break;
+				default:
+					uwarnf("Unknown video bDescriptorSubtype=%02x", ics.curr[2]);
+					break;
+				}
+			}
+			break;
+		default:
+			uwarnf("Unknown video bInterfaceSubClass=%02x", ifdesc->bInterfaceSubClass);
+			break;
+		}
+
+		for (ep_iter_init(&iep, &iif); iep.valid; ep_iter_next(&iep)) {
+			const usbh_endpoint_descriptor_t *const epdesc = ep_get(&iep);
+
+			if ((ifdesc->bInterfaceSubClass == UVC_SC_VIDEOCONTROL)
+					&& ((epdesc->bmAttributes & 0x03) == USBH_EPTYPE_INT)
+					&& ((epdesc->bEndpointAddress & 0x80) ==  USBH_EPDIR_IN)) {
+				/* found VC interrupt endpoint */
+				uinfof("  VC Interrupt endpoint; %02x, bInterval=%d",
+						epdesc->bEndpointAddress, epdesc->bInterval);
+				usbhEPObjectInit(&uvcdp->ep_int, dev, epdesc);
+				usbhEPSetName(&uvcdp->ep_int, "UVC[INT ]");
+			} else if ((ifdesc->bInterfaceSubClass == UVC_SC_VIDEOSTREAMING)
+					&& ((epdesc->bmAttributes & 0x03) == USBH_EPTYPE_ISO)
+					&& ((epdesc->bEndpointAddress & 0x80) ==  USBH_EPDIR_IN)) {
+				/* found VS isochronous endpoint */
+				uinfof("  VS Isochronous endpoint; %02x, bInterval=%d, bmAttributes=%02x",
+						epdesc->bEndpointAddress, epdesc->bInterval, epdesc->bmAttributes);
+			} else {
+				/* unknown EP */
+				uwarnf("  <unknown endpoint>, bEndpointAddress=%02x, bmAttributes=%02x",
+									epdesc->bEndpointAddress, epdesc->bmAttributes);
+			}
+
+			for (cs_iter_init(&ics, &iep); ics.valid; cs_iter_next(&ics)) {
+				uinfof("    CS_ENDPOINT bLength=%d, bDescriptorType=%02X",
+						ics.curr[0], ics.curr[1]);
+			}
+		}
+	}
+
+	if ((uvcdp->ivc.curr == NULL) || (uvcdp->ivs.curr == NULL)) {
+		return NULL;
+	}
+
+//	uvcdp->dev = dev;
+
+	_set_vs_alternate(uvcdp, 0);
+
+	/* initialize the INT endpoint */
+	chPoolObjectInit(&uvcdp->mp_status, sizeof(usbhuvc_message_status_t), NULL);
+	for(i = 0; i < HAL_USBHUVC_STATUS_PACKETS_COUNT; i++)
+		chPoolFree(&uvcdp->mp_status, &uvcdp->mp_status_buffer[i]);
+
+	usbhEPOpen(&uvcdp->ep_int);
+
+	usbhuvc_message_status_t *const msg = (usbhuvc_message_status_t *)chPoolAlloc(&uvcdp->mp_status);
+	osalDbgCheck(msg);
+	usbhURBObjectInit(&uvcdp->urb_int, &uvcdp->ep_int, _cb_int, uvcdp, msg->data, USBHUVC_MAX_STATUS_PACKET_SZ);
+	osalSysLock();
+	usbhURBSubmitI(&uvcdp->urb_int);
+	uvcdp->state = USBHUVC_STATE_ACTIVE;
+	osalOsRescheduleS();	/* because of usbhURBSubmitI */
+	osalSysUnlock();
+
+	dev->keepFullCfgDesc++;
+	return (usbh_baseclassdriver_t *)uvcdp;
+}
+
+static void _uvc_unload(usbh_baseclassdriver_t *drv) {
+	USBHUVCDriver *const uvcdp = (USBHUVCDriver *)drv;
+
+	usbhuvcStreamStop(uvcdp);
+
+	usbhEPClose(&uvcdp->ep_int);
+
+	//TODO: free
+
+	if (drv->dev->keepFullCfgDesc)
+		drv->dev->keepFullCfgDesc--;
+
+	osalSysLock();
+	uvcdp->state = USBHUVC_STATE_STOP;
+	osalSysUnlock();
+}
+
+static void _object_init(USBHUVCDriver *uvcdp) {
+	osalDbgCheck(uvcdp != NULL);
+	memset(uvcdp, 0, sizeof(*uvcdp));
+	uvcdp->info = &usbhuvcClassDriverInfo;
+	chMBObjectInit(&uvcdp->mb, uvcdp->mb_buff, HAL_USBHUVC_MAX_MAILBOX_SZ);
+	chMtxObjectInit(&uvcdp->mtx);
+	uvcdp->state = USBHUVC_STATE_STOP;
 }
 
-static void uvc_unload(usbh_baseclassdriver_t *drv) {
-	(void)drv;
+static void _uvc_init(void) {
+	uint8_t i;
+	for (i = 0; i < HAL_USBHUVC_MAX_INSTANCES; i++) {
+		_object_init(&USBHUVCD[i]);
+	}
 }
 
 #endif