Merge pull request #122 from dismirlian/usbh_devel

USBH: Various improvo

8 files changed, 352 insertions, 340 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
index 1fa49f8..85c3130 100644
--- a/os/hal/src/usbh/hal_usbh_aoa.c
+++ b/os/hal/src/usbh/hal_usbh_aoa.c
@@ -124,14 +124,16 @@ 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 = {
-	0xff, 0xff, 0xff, "AOA", &class_driver_vmt
+	"AOA", &class_driver_vmt
 };
 
 #if defined(HAL_USBHAOA_FILTER_CALLBACK)
@@ -144,7 +146,7 @@ static usbh_baseclassdriver_t *_aoa_load(usbh_device_t *dev, const uint8_t *desc
 
 	if (dev->devDesc.idVendor != AOA_GOOGLE_VID) {
 		uint16_t protocol;
-		static const USBHAOAConfig config = {
+		static USBHAOAConfig config = {
 			{
 				HAL_USBHAOA_DEFAULT_MANUFACTURER,
 				HAL_USBHAOA_DEFAULT_MODEL,
@@ -221,15 +223,9 @@ static usbh_baseclassdriver_t *_aoa_load(usbh_device_t *dev, const uint8_t *desc
 		return NULL;
 	}
 
-	if ((rem < descriptor[0]) || (descriptor[1] != USBH_DT_INTERFACE))
-		return NULL;
-
 	const usbh_interface_descriptor_t * const ifdesc = (const usbh_interface_descriptor_t *)descriptor;
-
-	if ((ifdesc->bInterfaceClass != 0xff)
-		|| (ifdesc->bInterfaceSubClass != 0xff)
-		|| (ifdesc->bInterfaceProtocol != 0x00)
-		|| (ifdesc->bNumEndpoints < 2)) {
+	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;
 	}
@@ -299,7 +295,6 @@ static void _aoa_unload(usbh_baseclassdriver_t *drv) {
 	_stop_channelS(&aoap->channel);
 	aoap->channel.state = USBHAOA_CHANNEL_STATE_STOP;
 	aoap->state = USBHAOA_STATE_STOP;
-	osalOsRescheduleS();
 	osalSysUnlock();
 }
 
@@ -341,15 +336,15 @@ static size_t _write_timeout(USBHAOAChannel *aoacp, const uint8_t *bp,
 	chDbgCheck(n > 0U);
 
 	size_t w = 0;
-	chSysLock();
+	osalSysLock();
 	while (true) {
 		if (aoacp->state != USBHAOA_CHANNEL_STATE_READY) {
-			chSysUnlock();
+			osalSysUnlock();
 			return w;
 		}
 		while (usbhURBIsBusy(&aoacp->oq_urb)) {
 			if (chThdEnqueueTimeoutS(&aoacp->oq_waiting, timeout) != Q_OK) {
-				chSysUnlock();
+				osalSysUnlock();
 				return w;
 			}
 		}
@@ -357,30 +352,30 @@ static size_t _write_timeout(USBHAOAChannel *aoacp, const uint8_t *bp,
 		*aoacp->oq_ptr++ = *bp++;
 		if (--aoacp->oq_counter == 0) {
 			_submitOutI(aoacp, 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(USBHAOAChannel *aoacp, uint8_t b, systime_t timeout) {
 
-	chSysLock();
+	osalSysLock();
 	if (aoacp->state != USBHAOA_CHANNEL_STATE_READY) {
-		chSysUnlock();
+		osalSysUnlock();
 		return Q_RESET;
 	}
 
 	while (usbhURBIsBusy(&aoacp->oq_urb)) {
 		msg_t msg = chThdEnqueueTimeoutS(&aoacp->oq_waiting, timeout);
 		if (msg < Q_OK) {
-			chSysUnlock();
+			osalSysUnlock();
 			return msg;
 		}
 	}
@@ -388,9 +383,9 @@ static msg_t _put_timeout(USBHAOAChannel *aoacp, uint8_t b, systime_t timeout) {
 	*aoacp->oq_ptr++ = b;
 	if (--aoacp->oq_counter == 0) {
 		_submitOutI(aoacp, 64);
-		chSchRescheduleS();
+		osalOsRescheduleS();
 	}
-	chSysUnlock();
+	osalSysUnlock();
 	return Q_OK;
 }
 
@@ -439,41 +434,41 @@ static size_t _read_timeout(USBHAOAChannel *aoacp, uint8_t *bp,
 
 	chDbgCheck(n > 0U);
 
-	chSysLock();
+	osalSysLock();
 	while (true) {
 		if (aoacp->state != USBHAOA_CHANNEL_STATE_READY) {
-			chSysUnlock();
+			osalSysUnlock();
 			return r;
 		}
 		while (aoacp->iq_counter == 0) {
 			if (!usbhURBIsBusy(&aoacp->iq_urb))
 				_submitInI(aoacp);
 			if (chThdEnqueueTimeoutS(&aoacp->iq_waiting, timeout) != Q_OK) {
-				chSysUnlock();
+				osalSysUnlock();
 				return r;
 			}
 		}
 		*bp++ = *aoacp->iq_ptr++;
 		if (--aoacp->iq_counter == 0) {
 			_submitInI(aoacp);
-			chSchRescheduleS();
+			osalOsRescheduleS();
 		}
-		chSysUnlock();
+		osalSysUnlock();
 
 		r++;
 		if (--n == 0U)
 			return r;
 
-		chSysLock();
+		osalSysLock();
 	}
 }
 
 static msg_t _get_timeout(USBHAOAChannel *aoacp, systime_t timeout) {
 	uint8_t b;
 
-	chSysLock();
+	osalSysLock();
 	if (aoacp->state != USBHAOA_CHANNEL_STATE_READY) {
-		chSysUnlock();
+		osalSysUnlock();
 		return Q_RESET;
 	}
 	while (aoacp->iq_counter == 0) {
@@ -481,16 +476,16 @@ static msg_t _get_timeout(USBHAOAChannel *aoacp, systime_t timeout) {
 			_submitInI(aoacp);
 		msg_t msg = chThdEnqueueTimeoutS(&aoacp->iq_waiting, timeout);
 		if (msg < Q_OK) {
-			chSysUnlock();
+			osalSysUnlock();
 			return msg;
 		}
 	}
 	b = *aoacp->iq_ptr++;
 	if (--aoacp->iq_counter == 0) {
 		_submitInI(aoacp);
-		chSchRescheduleS();
+		osalOsRescheduleS();
 	}
-	chSysUnlock();
+	osalSysUnlock();
 
 	return (msg_t)b;
 }
@@ -525,11 +520,12 @@ static void _stop_channelS(USBHAOAChannel *aoacp) {
 	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;
-	chSysLockFromISR();
+	osalSysLockFromISR();
 	uint32_t len = aoacp->oq_ptr - aoacp->oq_buff;
 	if (len && !usbhURBIsBusy(&aoacp->oq_urb)) {
 		_submitOutI(aoacp, len);
@@ -538,7 +534,7 @@ static void _vt(void *p) {
 		_submitInI(aoacp);
 	}
 	chVTSetI(&aoacp->vt, MS2ST(16), _vt, aoacp);
-	chSysUnlockFromISR();
+	osalSysUnlockFromISR();
 }
 
 void usbhaoaChannelStart(USBHAOADriver *aoap) {
@@ -566,9 +562,7 @@ void usbhaoaChannelStart(USBHAOADriver *aoap) {
 	aoacp->iq_counter = 0;
 	aoacp->iq_ptr = aoacp->iq_buff;
 	usbhEPOpen(&aoacp->epin);
-	osalSysLock();
-	usbhURBSubmitI(&aoacp->iq_urb);
-	osalSysUnlock();
+	usbhURBSubmit(&aoacp->iq_urb);
 
 	chVTObjectInit(&aoacp->vt);
 	chVTSet(&aoacp->vt, MS2ST(16), _vt, aoacp);
@@ -583,7 +577,6 @@ void usbhaoaChannelStop(USBHAOADriver *aoap) {
 			|| (aoap->channel.state == USBHAOA_CHANNEL_STATE_READY));
 	osalSysLock();
 	_stop_channelS(&aoap->channel);
-	osalOsRescheduleS();
 	osalSysUnlock();
 }
 
@@ -658,7 +651,7 @@ static bool _send_string(usbh_device_t *dev, uint8_t index, const char *string)
 	return HAL_SUCCESS;
 }
 
-void usbhaoaObjectInit(USBHAOADriver *aoap) {
+static void _object_init(USBHAOADriver *aoap) {
 	osalDbgCheck(aoap != NULL);
 	memset(aoap, 0, sizeof(*aoap));
 	aoap->info = &usbhaoaClassDriverInfo;
@@ -668,10 +661,10 @@ void usbhaoaObjectInit(USBHAOADriver *aoap) {
 	aoap->channel.state = USBHAOA_CHANNEL_STATE_STOP;
 }
 
-void usbhaoaInit(void) {
+static void _aoa_init(void) {
 	uint8_t i;
 	for (i = 0; i < HAL_USBHAOA_MAX_INSTANCES; i++) {
-		usbhaoaObjectInit(&USBHAOAD[i]);
+		_object_init(&USBHAOAD[i]);
 	}
 }
 
diff --git a/os/hal/src/usbh/hal_usbh_debug.c b/os/hal/src/usbh/hal_usbh_debug.c
index 7c4ff7d..d32f1c6 100644
--- a/os/hal/src/usbh/hal_usbh_debug.c
+++ b/os/hal/src/usbh/hal_usbh_debug.c
@@ -106,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 (iqIsFullI(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) {
@@ -341,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;
+	}
+
+	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;
+	}
+}
 
-	_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);
+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, ...)
@@ -361,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);
 }
@@ -372,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);
 }
 
@@ -429,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;
 		}
@@ -456,15 +479,15 @@ 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;
 			}
 		}
 	}
@@ -483,8 +506,9 @@ static void usb_debug_thread(void *arg) {
 			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;
 
@@ -503,10 +527,26 @@ static void usb_debug_thread(void *arg) {
 
 			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 = iqGet(&host->iq); if (c < 0) goto reset;
 				if (!c) {
 					sdPut(&USBH_DEBUG_SD, '\r');
 					sdPut(&USBH_DEBUG_SD, '\n');
@@ -514,6 +554,7 @@ static void usb_debug_thread(void *arg) {
 					break;
 				}
 				sdPut(&USBH_DEBUG_SD, (uint8_t)c);
+				c = iqGet(&host->iq); if (c < 0) goto reset;
 			}
 		}
 
diff --git a/os/hal/src/usbh/hal_usbh_ftdi.c b/os/hal/src/usbh/hal_usbh_ftdi.c
index ce96958..6e33867 100644
--- a/os/hal/src/usbh/hal_usbh_ftdi.c
+++ b/os/hal/src/usbh/hal_usbh_ftdi.c
@@ -27,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__)
@@ -62,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) {
@@ -93,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:
@@ -111,7 +109,8 @@ static usbh_baseclassdriver_t *_ftdi_load(usbh_device_t *dev, const uint8_t *des
 		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;
 
 	if (((const usbh_interface_descriptor_t *)descriptor)->bInterfaceNumber != 0) {
@@ -220,7 +219,6 @@ static void _ftdi_unload(usbh_baseclassdriver_t *drv) {
 	while (ftdipp) {
 		osalSysLock();
 		_stopS(ftdipp);
-		osalOsRescheduleS();
 		osalSysUnlock();
 		ftdipp = ftdipp->next;
 	}
@@ -229,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();
@@ -432,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;
 			}
 		}
@@ -448,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;
 		}
 	}
@@ -479,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;
 }
 
@@ -538,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) {
@@ -580,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;
 }
@@ -604,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);
@@ -613,7 +611,7 @@ static void _vt(void *p) {
 		_submitInI(ftdipp);
 	}
 	chVTSetI(&ftdipp->vt, MS2ST(16), _vt, ftdipp);
-	chSysUnlockFromISR();
+	osalSysUnlockFromISR();
 }
 
 static const struct FTDIPortDriverVMT async_channel_vmt = {
@@ -637,6 +635,7 @@ static void _stopS(USBHFTDIPortDriver *ftdipp) {
 	chThdDequeueAllI(&ftdipp->iq_waiting, Q_RESET);
 	chThdDequeueAllI(&ftdipp->oq_waiting, Q_RESET);
 	ftdipp->state = USBHFTDIP_STATE_ACTIVE;
+	osalOsRescheduleS();
 }
 
 void usbhftdipStop(USBHFTDIPortDriver *ftdipp) {
@@ -647,7 +646,6 @@ void usbhftdipStop(USBHFTDIPortDriver *ftdipp) {
 	chMtxLockS(&ftdipp->ftdip->mtx);
 	_stopS(ftdipp);
 	chMtxUnlockS(&ftdipp->ftdip->mtx);
-	osalOsRescheduleS();
 	osalSysUnlock();
 }
 
@@ -689,9 +687,7 @@ 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);
@@ -700,27 +696,27 @@ void usbhftdipStart(USBHFTDIPortDriver *ftdipp, const USBHFTDIPortConfig *config
 	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;
 }
 
-void usbhftdiInit(void) {
+static void _ftdi_init(void) {
 	uint8_t i;
 	for (i = 0; i < HAL_USBHFTDI_MAX_INSTANCES; i++) {
-		usbhftdiObjectInit(&USBHFTDID[i]);
+		_ftdi_object_init(&USBHFTDID[i]);
 	}
 	for (i = 0; i < HAL_USBHFTDI_MAX_PORTS; i++) {
-		usbhftdipObjectInit(&FTDIPD[i]);
+		_ftdip_object_init(&FTDIPD[i]);
 	}
 }
 
diff --git a/os/hal/src/usbh/hal_usbh_hid.c b/os/hal/src/usbh/hal_usbh_hid.c
index e98dff7..2b2c5ce 100644
--- a/os/hal/src/usbh/hal_usbh_hid.c
+++ b/os/hal/src/usbh/hal_usbh_hid.c
@@ -69,28 +69,32 @@
 #define USBH_HID_REQ_SET_PROTOCOL	0x0B
 
 /*===========================================================================*/
-/* USB Class driver loader for MSD								 		 	 */
+/* 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 = {
-	0x03, -1, -1, "HID", &class_driver_vmt
+	"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 ((rem < descriptor[0]) || (descriptor[1] != USBH_DT_INTERFACE))
+	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;
@@ -180,7 +184,11 @@ deinit:
 }
 
 static void _hid_unload(usbh_baseclassdriver_t *drv) {
-	(void)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) {
@@ -209,17 +217,22 @@ static void _in_cb(usbh_urb_t *urb) {
 void usbhhidStart(USBHHIDDriver *hidp, const USBHHIDConfig *cfg) {
 	osalDbgCheck(hidp && cfg);
 	osalDbgCheck(cfg->report_buffer && (cfg->protocol <= USBHHID_PROTOCOL_REPORT));
-	osalDbgCheck((hidp->state == USBHHID_STATE_ACTIVE)
-			|| (hidp->state == USBHHID_STATE_READY));
 
-	if (hidp->state == USBHHID_STATE_READY)
+	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, cfg->report_len);
+			cfg->report_buffer, report_len);
 
 	/* open the int IN/OUT endpoints */
 	usbhEPOpen(&hidp->epin);
@@ -231,29 +244,31 @@ void usbhhidStart(USBHHIDDriver *hidp, const USBHHIDConfig *cfg) {
 
 	usbhhidSetProtocol(hidp, cfg->protocol);
 
-	osalSysLock();
-	usbhURBSubmitI(&hidp->in_urb);
-	osalSysUnlock();
+	usbhURBSubmit(&hidp->in_urb);
 
 	hidp->state = USBHHID_STATE_READY;
+	chSemSignal(&hidp->sem);
 }
 
-void usbhhidStop(USBHHIDDriver *hidp) {
-	osalDbgCheck((hidp->state == USBHHID_STATE_ACTIVE)
-			|| (hidp->state == USBHHID_STATE_READY));
-
-	if (hidp->state != USBHHID_STATE_READY)
+static void _stop_locked(USBHHIDDriver *hidp) {
+	if (hidp->state == USBHHID_STATE_ACTIVE)
 		return;
 
-	osalSysLock();
-	usbhEPCloseS(&hidp->epin);
+	osalDbgCheck(hidp->state == USBHHID_STATE_READY);
+
+	usbhEPClose(&hidp->epin);
 #if HAL_USBHHID_USE_INTERRUPT_OUT
 	if (hidp->epout.status != USBH_EPSTATUS_UNINITIALIZED) {
-		usbhEPCloseS(&hidp->epout);
+		usbhEPClose(&hidp->epout);
 	}
 #endif
 	hidp->state = USBHHID_STATE_ACTIVE;
-	osalSysUnlock();
+}
+
+void usbhhidStop(USBHHIDDriver *hidp) {
+	chSemWait(&hidp->sem);
+	_stop_locked(hidp);
+	chSemSignal(&hidp->sem);
 }
 
 usbh_urbstatus_t usbhhidGetReport(USBHHIDDriver *hidp,
@@ -305,17 +320,18 @@ usbh_urbstatus_t usbhhidSetProtocol(USBHHIDDriver *hidp, uint8_t protocol) {
 			protocol, hidp->ifnum, 0, NULL);
 }
 
-void usbhhidObjectInit(USBHHIDDriver *hidp) {
+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);
 }
 
-void usbhhidInit(void) {
+static void _hid_init(void) {
 	uint8_t i;
 	for (i = 0; i < HAL_USBHHID_MAX_INSTANCES; i++) {
-		usbhhidObjectInit(&USBHHIDD[i]);
+		_hid_object_init(&USBHHIDD[i]);
 	}
 }
 
diff --git a/os/hal/src/usbh/hal_usbh_hub.c b/os/hal/src/usbh/hal_usbh_hub.c
index 3a84175..6a83c66 100644
--- a/os/hal/src/usbh/hal_usbh_hub.c
+++ b/os/hal/src/usbh/hal_usbh_hub.c
@@ -63,14 +63,17 @@
 USBHHubDriver USBHHUBD[HAL_USBHHUB_MAX_INSTANCES];
 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
 };
 
 
@@ -104,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;
@@ -137,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;
@@ -158,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)
@@ -253,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;
@@ -283,16 +278,16 @@ 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;
 }
 
-void usbhhubInit(void) {
+static void _hub_init(void) {
 	uint8_t i;
 	for (i = 0; i < HAL_USBHHUB_MAX_INSTANCES; i++) {
-		usbhhubObjectInit(&USBHHUBD[i]);
+		_object_init(&USBHHUBD[i]);
 	}
 }
 
diff --git a/os/hal/src/usbh/hal_usbh_msd.c b/os/hal/src/usbh/hal_usbh_msd.c
index b564e8e..069c47b 100644
--- a/os/hal/src/usbh/hal_usbh_msd.c
+++ b/os/hal/src/usbh/hal_usbh_msd.c
@@ -27,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__)
@@ -62,26 +59,39 @@
 #define uerr(f, ...)   do {} while(0)
 #endif
 
-
-
-
+static void _lun_object_deinit(USBHMassStorageLUNDriver *lunp);
 
 /*===========================================================================*/
 /* USB Class driver loader for MSD                                           */
 /*===========================================================================*/
 
-USBHMassStorageDriver USBHMSD[HAL_USBHMSD_MAX_INSTANCES];
+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;
+
+	USBHMassStorageLUNDriver *luns;
+};
+
+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
@@ -93,15 +103,14 @@ static usbh_baseclassdriver_t *_msd_load(usbh_device_t *dev, const uint8_t *desc
 	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;
 	}
 
@@ -187,15 +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) */
-			msdp->dev = dev;
-			usbhmsdLUNConnect(&MSBLKD[i]);
-			msdp->dev = NULL;
+			MSBLKD[i].state = BLK_ACTIVE;
 			luns--;
 		}
 	}
@@ -212,25 +213,15 @@ 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);
 }
 
 
@@ -238,8 +229,6 @@ static void _msd_unload(usbh_baseclassdriver_t *drv) {
 /* MSD Class driver operations (Bulk-Only transport)                         */
 /*===========================================================================*/
 
-
-
 /* USB Bulk Only Transport SCSI Command block wrapper */
 typedef PACKED_STRUCT {
 	uint32_t dCBWSignature;
@@ -704,11 +693,22 @@ static const struct USBHMassStorageDriverVMT blk_vmt = {
 	(bool (*)(void *, BlockDeviceInfo *))usbhmsdLUNGetInfo
 };
 
-void usbhmsdLUNObjectInit(USBHMassStorageLUNDriver *lunp) {
+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);
+}
+
+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;
@@ -716,45 +716,18 @@ 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(scsi_inquiry_response_t inq);
@@ -820,41 +793,34 @@ bool usbhmsdLUNConnect(USBHMassStorageLUNDriver *lunp) {
 		(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;
 }
 
@@ -867,15 +833,13 @@ bool usbhmsdLUNRead(USBHMassStorageLUNDriver *lunp, uint32_t startblk,
 	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;
@@ -900,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;
 }
 
@@ -921,15 +878,13 @@ bool usbhmsdLUNWrite(USBHMassStorageLUNDriver *lunp, uint32_t startblk,
 	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;
@@ -954,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;
 }
 
@@ -976,38 +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);
 }
 
-void usbhmsdInit(void) {
+static void _msd_init(void) {
 	uint8_t i;
 	for (i = 0; i < HAL_USBHMSD_MAX_INSTANCES; i++) {
-		usbhmsdObjectInit(&USBHMSD[i]);
+		_msd_object_init(&USBHMSD[i]);
 	}
 	for (i = 0; i < HAL_USBHMSD_MAX_LUNS; i++) {
-		usbhmsdLUNObjectInit(&MSBLKD[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 b0507a0..a795cd8 100644
--- a/os/hal/src/usbh/hal_usbh_uvc.c
+++ b/os/hal/src/usbh/hal_usbh_uvc.c
@@ -27,9 +27,9 @@
 #error "USBHUVC needs HAL_USBH_USE_IAD"
 #endif
 
+#include <string.h>
 #include "usbh/dev/uvc.h"
 #include "usbh/internal.h"
-#include <string.h>
 
 #if USBHUVC_DEBUG_ENABLE_TRACE
 #define udbgf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)
@@ -66,17 +66,18 @@
 
 USBHUVCDriver USBHUVCD[HAL_USBHUVC_MAX_INSTANCES];
 
-
-static usbh_baseclassdriver_t *uvc_load(usbh_device_t *dev,
+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,
@@ -360,10 +361,8 @@ bool usbhuvcStreamStart(USBHUVCDriver *uvcdp, uint16_t min_ep_sz) {
 		osalDbgCheck(msg);
 		usbhURBObjectInit(&uvcdp->urb_iso, &uvcdp->ep_iso, _cb_iso, uvcdp, msg->data, uvcdp->ep_iso.wMaxPacketSize);
 	}
-	osalSysLock();
-	usbhURBSubmitI(&uvcdp->urb_iso);
-	osalOsRescheduleS();
-	osalSysUnlock();
+
+	usbhURBSubmit(&uvcdp->urb_iso);
 
 	ret = HAL_SUCCESS;
 	goto exit;
@@ -512,22 +511,13 @@ uint32_t usbhuvcEstimateRequiredEPSize(USBHUVCDriver *uvcdp, const uint8_t *form
 	return (sz * mul) / div + 12;
 }
 
-void usbhuvcObjectInit(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 usbh_baseclassdriver_t *uvc_load(usbh_device_t *dev, const uint8_t *descriptor, uint16_t rem) {
+static usbh_baseclassdriver_t *_uvc_load(usbh_device_t *dev, const uint8_t *descriptor, uint16_t rem) {
 
 	USBHUVCDriver *uvcdp;
 	uint8_t i;
 
-	if (descriptor[1] != USBH_DT_INTERFACE_ASSOCIATION)
+	if (_usbh_match_descriptor(descriptor, rem, USBH_DT_INTERFACE_ASSOCIATION,
+			0x0e, 0x03, 0x00) != HAL_SUCCESS)
 		return NULL;
 
 	/* alloc driver */
@@ -703,14 +693,14 @@ alloc_ok:
 	osalSysLock();
 	usbhURBSubmitI(&uvcdp->urb_int);
 	uvcdp->state = USBHUVC_STATE_ACTIVE;
-	osalOsRescheduleS();
+	osalOsRescheduleS();	/* because of usbhURBSubmitI */
 	osalSysUnlock();
 
 	dev->keepFullCfgDesc++;
 	return (usbh_baseclassdriver_t *)uvcdp;
 }
 
-static void uvc_unload(usbh_baseclassdriver_t *drv) {
+static void _uvc_unload(usbh_baseclassdriver_t *drv) {
 	USBHUVCDriver *const uvcdp = (USBHUVCDriver *)drv;
 
 	usbhuvcStreamStop(uvcdp);
@@ -727,10 +717,19 @@ static void uvc_unload(usbh_baseclassdriver_t *drv) {
 	osalSysUnlock();
 }
 
-void usbhuvcInit(void) {
+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_init(void) {
 	uint8_t i;
 	for (i = 0; i < HAL_USBHUVC_MAX_INSTANCES; i++) {
-		usbhuvcObjectInit(&USBHUVCD[i]);
+		_object_init(&USBHUVCD[i]);
 	}
 }