1 files changed, 189 insertions, 188 deletions

diff --git a/os/hal/src/hal_usbh.c b/os/hal/src/hal_usbh.c
index d242086..a7ba236 100644
--- a/os/hal/src/hal_usbh.c
+++ b/os/hal/src/hal_usbh.c
@@ -20,15 +20,8 @@
 #if HAL_USE_USBH
 
 #include "usbh/internal.h"
-#include <string.h>
-
-//devices
 #include "usbh/dev/hub.h"
-#include "usbh/dev/aoa.h"
-#include "usbh/dev/ftdi.h"
-#include "usbh/dev/msd.h"
-#include "usbh/dev/hid.h"
-#include "usbh/dev/uvc.h"
+#include <string.h>
 
 #if USBH_DEBUG_ENABLE_TRACE
 #define udbgf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)
@@ -62,18 +55,15 @@
 #define uerr(f, ...)   do {} while(0)
 #endif
 
-#if STM32_USBH_USE_OTG1
-USBHDriver USBHD1;
-#endif
-#if STM32_USBH_USE_OTG2
-USBHDriver USBHD2;
-#endif
-
-
 static void _classdriver_process_device(usbh_device_t *dev);
-static bool _classdriver_load(usbh_device_t *dev, uint8_t class,
-		uint8_t subclass, uint8_t protocol, uint8_t *descbuff, uint16_t rem);
+static bool _classdriver_load(usbh_device_t *dev, uint8_t *descbuff, uint16_t rem);
 
+#if HAL_USBH_USE_ADDITIONAL_CLASS_DRIVERS
+#include "usbh_additional_class_drivers.h"
+#ifndef HAL_USBH_ADDITIONAL_CLASS_DRIVERS
+#error "Must define HAL_USBH_ADDITIONAL_CLASS_DRIVERS"
+#endif
+#endif
 
 /*===========================================================================*/
 /* Checks.                                                                   */
@@ -113,28 +103,6 @@ void usbhObjectInit(USBHDriver *usbh) {
 #endif
 }
 
-void usbhInit(void) {
-#if HAL_USBH_USE_FTDI
-	usbhftdiInit();
-#endif
-#if HAL_USBH_USE_AOA
-	usbhaoaInit();
-#endif
-#if HAL_USBH_USE_MSD
-	usbhmsdInit();
-#endif
-#if HAL_USBH_USE_HID
-	usbhhidInit();
-#endif
-#if HAL_USBH_USE_UVC
-	usbhuvcInit();
-#endif
-#if HAL_USBH_USE_HUB
-	usbhhubInit();
-#endif
-	usbh_lld_init();
-}
-
 void usbhStart(USBHDriver *usbh) {
 	usbDbgInit(usbh);
 
@@ -143,7 +111,6 @@ void usbhStart(USBHDriver *usbh) {
 				"invalid state");
 	usbh_lld_start(usbh);
 	usbh->status = USBH_STATUS_STARTED;
-	osalOsRescheduleS();
 	osalSysUnlock();
 }
 
@@ -256,6 +223,7 @@ void usbhURBObjectResetI(usbh_urb_t *urb) {
 	usbh_lld_urb_object_reset(urb);
 }
 
+/* usbhURBSubmitI may require a reschedule if called from a S-locked state */
 void usbhURBSubmitI(usbh_urb_t *urb) {
 	osalDbgCheckClassI();
 	_check_urb(urb);
@@ -277,27 +245,21 @@ void usbhURBSubmitI(usbh_urb_t *urb) {
 	usbh_lld_urb_submit(urb);
 }
 
+/* _usbh_urb_abortI may require a reschedule if called from a S-locked state */
 bool _usbh_urb_abortI(usbh_urb_t *urb, usbh_urbstatus_t status) {
 	osalDbgCheckClassI();
 	_check_urb(urb);
+	osalDbgCheck(urb->status != USBH_URBSTATUS_UNINITIALIZED);
 
-	switch (urb->status) {
-/*	case USBH_URBSTATUS_UNINITIALIZED:
- * 	case USBH_URBSTATUS_INITIALIZED:
- *	case USBH_URBSTATUS_ERROR:
- *	case USBH_URBSTATUS_TIMEOUT:
- *	case USBH_URBSTATUS_CANCELLED:
- *	case USBH_URBSTATUS_STALL:
- *	case USBH_URBSTATUS_DISCONNECTED:
- *	case USBH_URBSTATUS_OK: */
-	default:
-		/* already finished */
-		_usbh_urb_completeI(urb, status);
-		return TRUE;
-
-	case USBH_URBSTATUS_PENDING:
+	if (urb->status == USBH_URBSTATUS_PENDING) {
 		return usbh_lld_urb_abort(urb, status);
 	}
+
+	/* already finished or never submitted:
+	 * USBH_URBSTATUS_INITIALIZED, USBH_URBSTATUS_ERROR, USBH_URBSTATUS_TIMEOUT,
+	 * USBH_URBSTATUS_CANCELLED, USBH_URBSTATUS_STALL, USBH_URBSTATUS_DISCONNECTED
+	 * USBH_URBSTATUS_OK */
+	return TRUE;
 }
 
 void _usbh_urb_abort_and_waitS(usbh_urb_t *urb, usbh_urbstatus_t status) {
@@ -312,15 +274,18 @@ void _usbh_urb_abort_and_waitS(usbh_urb_t *urb, usbh_urbstatus_t status) {
 	}
 #if !(USBH_DEBUG_ENABLE && USBH_DEBUG_ENABLE_WARNINGS)
 	else {
+		/* This call is necessary because _usbh_urb_abortI may require a reschedule */
 		osalOsRescheduleS();
 	}
 #else
 	uwarn("URB aborted");
 	osalOsRescheduleS();	/* debug printing functions call I-class functions inside
-	 	 	 	 	 	 	 which may cause a priority violation without this call */
+	 	 	 	 	 	 	 which may cause a priority violation without this call
+							Also, _usbh_urb_abortI may require a reschedule */
 #endif
 }
 
+/* usbhURBCancelI may require a reschedule if called from a S-locked state */
 bool usbhURBCancelI(usbh_urb_t *urb) {
 	return _usbh_urb_abortI(urb, USBH_URBSTATUS_CANCELLED);
 }
@@ -345,7 +310,6 @@ msg_t usbhURBWaitTimeoutS(usbh_urb_t *urb, systime_t timeout) {
 
 	case USBH_URBSTATUS_OK:
 		ret = MSG_OK;
-		osalOsRescheduleS();
 		break;
 
 /*	case USBH_URBSTATUS_UNINITIALIZED:
@@ -356,7 +320,6 @@ msg_t usbhURBWaitTimeoutS(usbh_urb_t *urb, systime_t timeout) {
  *	case USBH_URBSTATUS_DISCONNECTED: */
 	default:
 		ret = MSG_RESET;
-		osalOsRescheduleS();
 		break;
 	}
 	return ret;
@@ -382,6 +345,7 @@ static inline msg_t _wakeup_message(usbh_urbstatus_t status) {
 	return MSG_RESET;
 }
 
+/* _usbh_urb_completeI may require a reschedule if called from a S-locked state */
 void _usbh_urb_completeI(usbh_urb_t *urb, usbh_urbstatus_t status) {
 	osalDbgCheckClassI();
 	_check_urb(urb);
@@ -459,7 +423,7 @@ usbh_urbstatus_t usbhControlRequest(usbh_device_t *dev,
 			wIndex,
 			wLength
 	};
-	return usbhControlRequestExtended(dev, &req, buff, NULL, MS2ST(1000));
+	return usbhControlRequestExtended(dev, &req, buff, NULL, HAL_USBH_CONTROL_REQUEST_DEFAULT_TIMEOUT);
 }
 
 /*===========================================================================*/
@@ -925,46 +889,48 @@ static void _port_process_status_change(usbh_port_t *port) {
 	_port_update_status(port);
 
 	if (port->c_status & USBH_PORTSTATUS_C_CONNECTION) {
-		/* port connected status changed */
 		port->c_status &= ~USBH_PORTSTATUS_C_CONNECTION;
 		usbhhubClearFeaturePort(port, USBH_PORT_FEAT_C_CONNECTION);
-		if ((port->status & (USBH_PORTSTATUS_CONNECTION | USBH_PORTSTATUS_ENABLE))
-				== USBH_PORTSTATUS_CONNECTION) {
-			if (port->device.status != USBH_DEVSTATUS_DISCONNECTED) {
+
+		if (port->device.status != USBH_DEVSTATUS_DISCONNECTED) {
+			if (!(port->status & USBH_PORTSTATUS_CONNECTION)) {
 				_usbh_port_disconnected(port);
 			}
+		}
+	}
 
-			/* connected, disabled */
+	if (port->device.status == USBH_DEVSTATUS_DISCONNECTED) {
+		if (port->status & USBH_PORTSTATUS_CONNECTION) {
 			_port_connected(port);
-		} else {
-			/* disconnected */
-			_usbh_port_disconnected(port);
 		}
 	}
 
 	if (port->c_status & USBH_PORTSTATUS_C_RESET) {
 		port->c_status &= ~USBH_PORTSTATUS_C_RESET;
 		usbhhubClearFeaturePort(port, USBH_PORT_FEAT_C_RESET);
+		uinfof("Port %d: reset=%d", port->number, port->status & USBH_PORTSTATUS_RESET ? 1 : 0);
 	}
 
 	if (port->c_status & USBH_PORTSTATUS_C_ENABLE) {
 		port->c_status &= ~USBH_PORTSTATUS_C_ENABLE;
 		usbhhubClearFeaturePort(port, USBH_PORT_FEAT_C_ENABLE);
+		uinfof("Port %d: enable=%d", port->number, port->status & USBH_PORTSTATUS_ENABLE ? 1 : 0);
 	}
 
 	if (port->c_status & USBH_PORTSTATUS_C_OVERCURRENT) {
 		port->c_status &= ~USBH_PORTSTATUS_C_OVERCURRENT;
 		usbhhubClearFeaturePort(port, USBH_PORT_FEAT_C_OVERCURRENT);
+		uwarnf("Port %d: overcurrent=%d", port->number, port->status & USBH_PORTSTATUS_OVERCURRENT ? 1 : 0);
 	}
 
 	if (port->c_status & USBH_PORTSTATUS_C_SUSPEND) {
 		port->c_status &= ~USBH_PORTSTATUS_C_SUSPEND;
 		usbhhubClearFeaturePort(port, USBH_PORT_FEAT_C_SUSPEND);
+		uinfof("Port %d: suspend=%d", port->number, port->status & USBH_PORTSTATUS_SUSPEND ? 1 : 0);
 	}
 
 }
 
-
 static void _port_connected(usbh_port_t *port) {
 	/* connected */
 
@@ -974,9 +940,8 @@ static void _port_connected(usbh_port_t *port) {
 	usbh_devspeed_t speed;
 	USBH_DEFINE_BUFFER(usbh_string_descriptor_t strdesc);
 
-	uinfof("Port %d connected, wait debounce...", port->number);
-
 	port->device.status = USBH_DEVSTATUS_ATTACHED;
+	uinfof("Port %d: attached, wait debounce...", port->number);
 
 	/* wait for attach de-bounce */
 	osalThreadSleepMilliseconds(HAL_USBH_PORT_DEBOUNCE_TIME);
@@ -984,16 +949,26 @@ static void _port_connected(usbh_port_t *port) {
 	/* check disconnection */
 	_port_update_status(port);
 	if (port->c_status & USBH_PORTSTATUS_C_CONNECTION) {
-		/* connection state changed; abort */
+		port->c_status &= ~USBH_PORTSTATUS_C_CONNECTION;
+		usbhhubClearFeaturePort(port, USBH_PORT_FEAT_C_CONNECTION);
+		uwarnf("Port %d: connection state changed; abort #1", port->number);
 		goto abort;
 	}
 
+	/* make sure that the device is still connected */
+	if ((port->status & USBH_PORTSTATUS_CONNECTION) == 0) {
+		uwarnf("Port %d: device is disconnected", port->number);
+		goto abort;
+	}
+
+	uinfof("Port %d: connected", port->number);
 	port->device.status = USBH_DEVSTATUS_CONNECTED;
 	retries = 3;
 
 reset:
 	for (i = 0; i < 3; i++) {
-		uinfo("Try reset...");
+		uinfof("Port %d: Try reset...", port->number);
+		/* TODO: check that port is actually disabled */
 		port->c_status &= ~(USBH_PORTSTATUS_C_RESET | USBH_PORTSTATUS_C_ENABLE);
 		_port_reset(port);
 		osalThreadSleepMilliseconds(20);	/* give it some time to reset (min. 10ms) */
@@ -1002,8 +977,12 @@ reset:
 			_port_update_status(port);
 
 			/* check for disconnection */
-			if (port->c_status & USBH_PORTSTATUS_C_CONNECTION)
+			if (port->c_status & USBH_PORTSTATUS_C_CONNECTION) {
+				port->c_status &= ~USBH_PORTSTATUS_C_CONNECTION;
+				usbhhubClearFeaturePort(port, USBH_PORT_FEAT_C_CONNECTION);
+				uwarnf("Port %d: connection state changed; abort #2", port->number);
 				goto abort;
+			}
 
 			/* check for reset completion */
 			if (port->c_status & USBH_PORTSTATUS_C_RESET) {
@@ -1017,7 +996,10 @@ reset:
 			}
 
 			/* check for timeout */
-			if (osalOsGetSystemTimeX() - start > HAL_USBH_PORT_RESET_TIMEOUT) break;
+			if (osalOsGetSystemTimeX() - start > HAL_USBH_PORT_RESET_TIMEOUT) {
+				uwarnf("Port %d: reset timeout", port->number);
+				break;
+			}
 		}
 	}
 
@@ -1025,8 +1007,7 @@ reset:
 	goto abort;
 
 reset_success:
-
-	uinfo("Reset OK, recovery...");
+	uinfof("Port %d: Reset OK, recovery...", port->number);
 
 	/* reset recovery */
 	osalThreadSleepMilliseconds(100);
@@ -1043,19 +1024,22 @@ reset_success:
 	usbhEPOpen(&port->device.ctrl);
 
 	/* device with default address (0), try enumeration */
-	if (_device_enumerate(&port->device)) {
+	if (_device_enumerate(&port->device) != HAL_SUCCESS) {
 		/* enumeration failed */
 		usbhEPClose(&port->device.ctrl);
 
-		if (!--retries)
+		if (!--retries) {
+			uwarnf("Port %d: enumeration failed; abort", port->number);
 			goto abort;
+		}
 
 		/* retry reset & enumeration */
+		uwarnf("Port %d: enumeration failed; retry reset & enumeration", port->number);
 		goto reset;
 	}
 
 	/* load the default language ID */
-	uinfo("Loading langID0...");
+	uinfof("Port %d: Loading langID0...", port->number);
 	if (!usbhStdReqGetStringDescriptor(&port->device, 0, 0,
 			USBH_DT_STRING_SIZE, (uint8_t *)&strdesc)
 		&& (strdesc.bLength >= 4)
@@ -1063,12 +1047,12 @@ reset_success:
 			4, (uint8_t *)&strdesc)) {
 
 		port->device.langID0 = strdesc.wData[0];
-		uinfof("langID0=%04x", port->device.langID0);
+		uinfof("Port %d: langID0=%04x", port->number, port->device.langID0);
 	}
 
 	/* check if the device has only one configuration */
 	if (port->device.devDesc.bNumConfigurations == 1) {
-		uinfo("Device has only one configuration");
+		uinfof("Port %d: device has only one configuration", port->number);
 		_device_configure(&port->device, 0);
 	}
 
@@ -1076,7 +1060,7 @@ reset_success:
 	return;
 
 abort:
-	uerr("Abort");
+	uerrf("Port %d: abort", port->number);
 	port->device.status = USBH_DEVSTATUS_DISCONNECTED;
 }
 
@@ -1084,14 +1068,14 @@ void _usbh_port_disconnected(usbh_port_t *port) {
 	if (port->device.status == USBH_DEVSTATUS_DISCONNECTED)
 		return;
 
-	uinfo("Port disconnected");
+	uinfof("Port %d: disconnected", port->number);
 
 	/* unload drivers */
 	while (port->device.drivers) {
 		usbh_baseclassdriver_t *drv = port->device.drivers;
 
 		/* unload */
-		uinfof("Unload driver %s", drv->info->name);
+		uinfof("Port %d: unload driver %s", port->number, drv->info->name);
 		drv->info->vmt->unload(drv);
 
 		/* unlink */
@@ -1100,9 +1084,7 @@ void _usbh_port_disconnected(usbh_port_t *port) {
 	}
 
 	/* close control endpoint */
-	osalSysLock();
-	usbhEPCloseS(&port->device.ctrl);
-	osalSysUnlock();
+	usbhEPClose(&port->device.ctrl);
 
 	/* free address */
 	if (port->device.address)
@@ -1142,7 +1124,7 @@ static void _hub_process_status_change(USBHDriver *host, USBHHubDriver *hub) {
 	uinfo("Hub status change. GET_STATUS.");
 	_hub_update_status(host, hub);
 
-	if (hub->c_status & USBH_HUBSTATUS_C_HUB_LOCAL_POWER) {
+		if (hub->c_status & USBH_HUBSTATUS_C_HUB_LOCAL_POWER) {
 		hub->c_status &= ~USBH_HUBSTATUS_C_HUB_LOCAL_POWER;
 		uinfo("Clear USBH_HUB_FEAT_C_HUB_LOCAL_POWER");
 		usbhhubClearFeatureHub(host, hub, USBH_HUB_FEAT_C_HUB_LOCAL_POWER);
@@ -1160,7 +1142,6 @@ static uint32_t _hub_get_status_change_bitmap(USBHDriver *host, USBHHubDriver *h
 		osalSysLock();
 		uint32_t ret = hub->statuschange;
 		hub->statuschange = 0;
-		osalOsRescheduleS();
 		osalSysUnlock();
 		return ret;
 	}
@@ -1226,8 +1207,8 @@ void usbhMainLoop(USBHDriver *usbh) {
 	_hub_process(usbh, NULL);
 
 	/* process connected hubs */
-	USBHHubDriver *hub;
-    list_for_each_entry(hub, USBHHubDriver, &usbh->hubs, node) {
+	USBHHubDriver *hub, *temp;
+    list_for_each_entry_safe(hub, USBHHubDriver, temp, &usbh->hubs, node) {
 		_hub_process(usbh, hub);
 	}
 #else
@@ -1236,75 +1217,78 @@ void usbhMainLoop(USBHDriver *usbh) {
 #endif
 }
 
-
 /*===========================================================================*/
-/* IAD class driver.                                                         */
+/* Class driver loader.                                                      */
 /*===========================================================================*/
-#if HAL_USBH_USE_IAD
-static usbh_baseclassdriver_t *iad_load(usbh_device_t *dev, const uint8_t *descriptor, uint16_t rem);
-static void iad_unload(usbh_baseclassdriver_t *drv);
-static const usbh_classdriver_vmt_t usbhiadClassDriverVMT = {
-	iad_load,
-	iad_unload
-};
-static const usbh_classdriverinfo_t usbhiadClassDriverInfo = {
-	0xef, 0x02, 0x01, "IAD", &usbhiadClassDriverVMT
-};
 
-static usbh_baseclassdriver_t *iad_load(usbh_device_t *dev,
-		const uint8_t *descriptor, uint16_t rem) {
-	(void)rem;
+bool _usbh_match_vid_pid(usbh_device_t *dev, int32_t vid, int32_t pid) {
+	if (((vid < 0) || (dev->devDesc.idVendor == vid))
+		&& ((pid < 0) || (dev->devDesc.idProduct == pid)))
+		return HAL_SUCCESS;
 
-	if (descriptor[1] != USBH_DT_DEVICE)
-		return 0;
+	return HAL_FAILED;
+}
 
-	uinfo("Load a driver for each IF collection.");
+bool _usbh_match_descriptor(const uint8_t *descriptor, uint16_t rem,
+		int16_t type, int16_t _class, int16_t subclass, int16_t protocol) {
 
-	generic_iterator_t icfg;
-	if_iterator_t iif;
-	const usbh_ia_descriptor_t *last_iad = 0;
+	int16_t dclass, dsubclass, dprotocol;
 
-	cfg_iter_init(&icfg, dev->fullConfigurationDescriptor,
-			dev->basicConfigDesc.wTotalLength);
-	if (!icfg.valid) {
-		uerr("Invalid configuration descriptor.");
-		return 0;
-	}
+	if ((rem < descriptor[0]) || (rem < 2))
+		return HAL_FAILED;
 
-	for (if_iter_init(&iif, &icfg); iif.valid; if_iter_next(&iif)) {
-		if (iif.iad && (iif.iad != last_iad)) {
-			last_iad = iif.iad;
-			if (_classdriver_load(dev, iif.iad->bFunctionClass,
-					iif.iad->bFunctionSubClass,
-					iif.iad->bFunctionProtocol,
-					(uint8_t *)iif.iad,
-					(uint8_t *)iif.curr - (uint8_t *)iif.iad + iif.rem) != HAL_SUCCESS) {
-				uwarnf("No drivers found for IF collection #%d:%d",
-						iif.iad->bFirstInterface,
-						iif.iad->bFirstInterface + iif.iad->bInterfaceCount - 1);
-			}
-		}
-	}
+	uint8_t dtype = descriptor[1];
 
-	return 0;
-}
+	if ((type >= 0) && (type != dtype))
+		return HAL_FAILED;
 
-static void iad_unload(usbh_baseclassdriver_t *drv) {
-	(void)drv;
-}
-#endif
+	switch (dtype) {
+	case USBH_DT_DEVICE: {
+		if (rem < USBH_DT_DEVICE_SIZE)
+			return HAL_FAILED;
+		const usbh_device_descriptor_t *const desc = (const usbh_device_descriptor_t *)descriptor;
+		dclass = desc->bDeviceClass;
+		dsubclass = desc->bDeviceSubClass;
+		dprotocol = desc->bDeviceProtocol;
+	}	break;
+	case USBH_DT_INTERFACE: {
+		if (rem < USBH_DT_INTERFACE_SIZE)
+			return HAL_FAILED;
+		const usbh_interface_descriptor_t *const desc = (const usbh_interface_descriptor_t *)descriptor;
+		dclass = desc->bInterfaceClass;
+		dsubclass = desc->bInterfaceSubClass;
+		dprotocol = desc->bInterfaceProtocol;
+	}	break;
+	case USBH_DT_INTERFACE_ASSOCIATION: {
+		if (rem < USBH_DT_INTERFACE_ASSOCIATION_SIZE)
+			return HAL_FAILED;
+		const usbh_ia_descriptor_t *const desc = (const usbh_ia_descriptor_t *)descriptor;
+		dclass = desc->bFunctionClass;
+		dsubclass = desc->bFunctionSubClass;
+		dprotocol = desc->bFunctionProtocol;
+	}	break;
+	default:
+		return HAL_FAILED;
+	}
 
+	if (((_class < 0) || (_class == dclass))
+		&& ((subclass < 0) || (subclass == dsubclass))
+		&& ((protocol < 0) || (protocol == dprotocol)))
+		return HAL_SUCCESS;
 
-/*===========================================================================*/
-/* Class driver loader.                                                      */
-/*===========================================================================*/
+	return HAL_FAILED;
+}
 
 static const usbh_classdriverinfo_t *usbh_classdrivers_lookup[] = {
+#if HAL_USBH_USE_ADDITIONAL_CLASS_DRIVERS
+	/* user-defined out of tree class drivers */
+	HAL_USBH_ADDITIONAL_CLASS_DRIVERS
+#endif
 #if HAL_USBH_USE_FTDI
 	&usbhftdiClassDriverInfo,
 #endif
-#if HAL_USBH_USE_IAD
-	&usbhiadClassDriverInfo,
+#if HAL_USBH_USE_HUB
+	&usbhhubClassDriverInfo,
 #endif
 #if HAL_USBH_USE_UVC
 	&usbhuvcClassDriverInfo,
@@ -1318,44 +1302,24 @@ static const usbh_classdriverinfo_t *usbh_classdrivers_lookup[] = {
 #if HAL_USBH_USE_UVC
 	&usbhuvcClassDriverInfo,
 #endif
-#if HAL_USBH_USE_HUB
-	&usbhhubClassDriverInfo,
-#endif
 #if HAL_USBH_USE_AOA
 	&usbhaoaClassDriverInfo,	/* Leave always last */
 #endif
 };
 
-static bool _classdriver_load(usbh_device_t *dev, uint8_t class,
-		uint8_t subclass, uint8_t protocol, uint8_t *descbuff, uint16_t rem) {
+static bool _classdriver_load(usbh_device_t *dev, uint8_t *descbuff, uint16_t rem) {
 	uint8_t i;
 	usbh_baseclassdriver_t *drv = NULL;
 	for (i = 0; i < sizeof_array(usbh_classdrivers_lookup); i++) {
 		const usbh_classdriverinfo_t *const info = usbh_classdrivers_lookup[i];
-		if (class == 0xff) {
-			/* vendor specific */
-			if (info->class == 0xff) {
-				uinfof("Try load vendor-specific driver %s", info->name);
-				drv = info->vmt->load(dev, descbuff, rem);
-				if (drv != NULL)
-					goto success;
-			}
-		} else if ((info->class < 0) || ((info->class == class)
-			&& ((info->subclass < 0) || ((info->subclass == subclass)
-			&& ((info->protocol < 0) || (info->protocol == protocol)))))) {
-			uinfof("Try load driver %s", info->name);
-			drv = info->vmt->load(dev, descbuff, rem);
 
-#if HAL_USBH_USE_IAD
-			/* special case: */
-			if (info == &usbhiadClassDriverInfo)
-				goto success; //return HAL_SUCCESS;
-#endif
+		uinfof("Try load driver %s", info->name);
+		drv = info->vmt->load(dev, descbuff, rem);
 
-			if (drv != NULL)
-				goto success;
-		}
+		if (drv != NULL)
+			goto success;
 	}
+
 	return HAL_FAILED;
 
 success:
@@ -1396,13 +1360,16 @@ static void _classdriver_process_device(usbh_device_t *dev) {
 	usbhDevicePrintConfiguration(dev->fullConfigurationDescriptor,
 			dev->basicConfigDesc.wTotalLength);
 
-	if (devdesc->bDeviceClass == 0) {
-		/* each interface defines its own device class/subclass/protocol */
-		uinfo("Load a driver for each IF.");
+#if HAL_USBH_USE_IAD
+	if (dev->devDesc.bDeviceClass == 0xef
+			&& dev->devDesc.bDeviceSubClass == 0x02
+			&& dev->devDesc.bDeviceProtocol == 0x01) {
+
+		uinfo("Load a driver for each IF collection.");
 
 		generic_iterator_t icfg;
 		if_iterator_t iif;
-		uint8_t last_if = 0xff;
+		const usbh_ia_descriptor_t *last_iad = 0;
 
 		cfg_iter_init(&icfg, dev->fullConfigurationDescriptor,
 				dev->basicConfigDesc.wTotalLength);
@@ -1412,24 +1379,49 @@ static void _classdriver_process_device(usbh_device_t *dev) {
 		}
 
 		for (if_iter_init(&iif, &icfg); iif.valid; if_iter_next(&iif)) {
-			const usbh_interface_descriptor_t *const ifdesc = if_get(&iif);
-			if (ifdesc->bInterfaceNumber != last_if) {
-				last_if = ifdesc->bInterfaceNumber;
-				if (_classdriver_load(dev, ifdesc->bInterfaceClass,
-						ifdesc->bInterfaceSubClass,
-						ifdesc->bInterfaceProtocol,
-						(uint8_t *)ifdesc, iif.rem) != HAL_SUCCESS) {
-					uwarnf("No drivers found for IF #%d", ifdesc->bInterfaceNumber);
+			if (iif.iad && (iif.iad != last_iad)) {
+				last_iad = iif.iad;
+				if (_classdriver_load(dev,
+						(uint8_t *)iif.iad,
+						(uint8_t *)iif.curr - (uint8_t *)iif.iad + iif.rem) != HAL_SUCCESS) {
+					uwarnf("No drivers found for IF collection #%d:%d",
+							iif.iad->bFirstInterface,
+							iif.iad->bFirstInterface + iif.iad->bInterfaceCount - 1);
 				}
 			}
 		}
 
-	} else {
-		if (_classdriver_load(dev, devdesc->bDeviceClass,
-				devdesc->bDeviceSubClass,
-				devdesc->bDeviceProtocol,
-				(uint8_t *)devdesc, USBH_DT_DEVICE_SIZE) != HAL_SUCCESS) {
-			uwarn("No drivers found.");
+	} else
+#endif
+	if (_classdriver_load(dev, (uint8_t *)devdesc, USBH_DT_DEVICE_SIZE) != HAL_SUCCESS) {
+		uinfo("No drivers found for device.");
+
+		if (devdesc->bDeviceClass == 0) {
+			/* each interface defines its own device class/subclass/protocol */
+			uinfo("Try load a driver for each IF.");
+
+			generic_iterator_t icfg;
+			if_iterator_t iif;
+			uint8_t last_if = 0xff;
+
+			cfg_iter_init(&icfg, dev->fullConfigurationDescriptor,
+					dev->basicConfigDesc.wTotalLength);
+			if (!icfg.valid) {
+				uerr("Invalid configuration descriptor.");
+				goto exit;
+			}
+
+			for (if_iter_init(&iif, &icfg); iif.valid; if_iter_next(&iif)) {
+				const usbh_interface_descriptor_t *const ifdesc = if_get(&iif);
+				if (ifdesc->bInterfaceNumber != last_if) {
+					last_if = ifdesc->bInterfaceNumber;
+					if (_classdriver_load(dev, (uint8_t *)ifdesc, iif.rem) != HAL_SUCCESS) {
+						uwarnf("No drivers found for IF #%d", ifdesc->bInterfaceNumber);
+					}
+				}
+			}
+		} else {
+			uwarn("Unable to load driver.");
 		}
 	}
 
@@ -1439,6 +1431,15 @@ exit:
 	}
 }
 
+void usbhInit(void) {
+	uint8_t i;
+	for (i = 0; i < sizeof_array(usbh_classdrivers_lookup); i++) {
+		if (usbh_classdrivers_lookup[i]->vmt->init) {
+			usbh_classdrivers_lookup[i]->vmt->init();
+		}
+	}
+	usbh_lld_init();
+}
 
 #endif