USBH: Add flexibility to the enumeration process

7 files changed, 131 insertions, 133 deletions

diff --git a/os/hal/src/hal_usbh.c b/os/hal/src/hal_usbh.c
index 007ef1b..dad022f 100644
--- a/os/hal/src/hal_usbh.c
+++ b/os/hal/src/hal_usbh.c
@@ -56,8 +56,7 @@
 #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"
@@ -1205,69 +1204,68 @@ 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 = {
-	NULL,
-	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];
+
+	if ((type >= 0) && (type != dtype))
+		return HAL_FAILED;
+
+	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;
 	}
 
-	return 0;
-}
+	if (((_class < 0) || (_class == dclass))
+		&& ((subclass < 0) || (subclass == dsubclass))
+		&& ((protocol < 0) || (protocol == dprotocol)))
+		return HAL_SUCCESS;
 
-static void iad_unload(usbh_baseclassdriver_t *drv) {
-	(void)drv;
+	return HAL_FAILED;
 }
-#endif
 
-
-/*===========================================================================*/
-/* Class driver loader.                                                      */
-/*===========================================================================*/
 static const usbh_classdriverinfo_t *usbh_classdrivers_lookup[] = {
 #if HAL_USBH_USE_ADDITIONAL_CLASS_DRIVERS
 	/* user-defined out of tree class drivers */
@@ -1276,8 +1274,8 @@ static const usbh_classdriverinfo_t *usbh_classdrivers_lookup[] = {
 #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,
@@ -1291,44 +1289,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:
@@ -1369,13 +1347,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);
@@ -1385,24 +1366,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.");
 		}
 	}
 
@@ -1419,6 +1425,7 @@ void usbhInit(void) {
 			usbh_classdrivers_lookup[i]->vmt->init();
 		}
 	}
+	usbh_lld_init();
 }
 
 #endif
diff --git a/os/hal/src/usbh/hal_usbh_aoa.c b/os/hal/src/usbh/hal_usbh_aoa.c
index db348c0..1526aa3 100644
--- a/os/hal/src/usbh/hal_usbh_aoa.c
+++ b/os/hal/src/usbh/hal_usbh_aoa.c
@@ -133,7 +133,7 @@ static const usbh_classdriver_vmt_t class_driver_vmt = {
 };
 
 const usbh_classdriverinfo_t usbhaoaClassDriverInfo = {
-	0xff, 0xff, 0xff, "AOA", &class_driver_vmt
+	"AOA", &class_driver_vmt
 };
 
 #if defined(HAL_USBHAOA_FILTER_CALLBACK)
@@ -146,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,
@@ -223,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;
 	}
diff --git a/os/hal/src/usbh/hal_usbh_ftdi.c b/os/hal/src/usbh/hal_usbh_ftdi.c
index a01c566..edcf022 100644
--- a/os/hal/src/usbh/hal_usbh_ftdi.c
+++ b/os/hal/src/usbh/hal_usbh_ftdi.c
@@ -77,7 +77,7 @@ static const usbh_classdriver_vmt_t class_driver_vmt = {
 };
 
 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) {
diff --git a/os/hal/src/usbh/hal_usbh_hid.c b/os/hal/src/usbh/hal_usbh_hid.c
index 5b2823f..269b1b2 100644
--- a/os/hal/src/usbh/hal_usbh_hid.c
+++ b/os/hal/src/usbh/hal_usbh_hid.c
@@ -85,14 +85,15 @@ static const usbh_classdriver_vmt_t class_driver_vmt = {
 };
 
 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;
@@ -183,7 +184,7 @@ deinit:
 
 static void _hid_unload(usbh_baseclassdriver_t *drv) {
 	USBHHIDDriver *const hidp = (USBHHIDDriver *)drv;
-
+	(void)hidp;
 }
 
 static void _in_cb(usbh_urb_t *urb) {
diff --git a/os/hal/src/usbh/hal_usbh_hub.c b/os/hal/src/usbh/hal_usbh_hub.c
index c9bbe9b..80282ef 100644
--- a/os/hal/src/usbh/hal_usbh_hub.c
+++ b/os/hal/src/usbh/hal_usbh_hub.c
@@ -73,7 +73,7 @@ static const usbh_classdriver_vmt_t usbhhubClassDriverVMT = {
 };
 
 const usbh_classdriverinfo_t usbhhubClassDriverInfo = {
-	0x09, 0x00, -1, "HUB", &usbhhubClassDriverVMT
+	"HUB", &usbhhubClassDriverVMT
 };
 
 
@@ -146,10 +146,8 @@ static usbh_baseclassdriver_t *_hub_load(usbh_device_t *dev,
 
 	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;
@@ -161,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)
@@ -261,6 +257,7 @@ alloc_ok:
 	osalOsRescheduleS();
 	osalSysUnlock();
 
+	hubdp->dev = NULL;
 	return (usbh_baseclassdriver_t *)hubdp;
 }
 
diff --git a/os/hal/src/usbh/hal_usbh_msd.c b/os/hal/src/usbh/hal_usbh_msd.c
index f212516..abc58f3 100644
--- a/os/hal/src/usbh/hal_usbh_msd.c
+++ b/os/hal/src/usbh/hal_usbh_msd.c
@@ -91,7 +91,7 @@ static const usbh_classdriver_vmt_t class_driver_vmt = {
 };
 
 const usbh_classdriverinfo_t usbhmsdClassDriverInfo = {
-	0x08, 0x06, 0x50, "MSD", &class_driver_vmt
+	"MSD", &class_driver_vmt
 };
 
 #define MSD_REQ_RESET							0xFF
@@ -103,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;
 	}
 
diff --git a/os/hal/src/usbh/hal_usbh_uvc.c b/os/hal/src/usbh/hal_usbh_uvc.c
index 3f362a2..7777823 100644
--- a/os/hal/src/usbh/hal_usbh_uvc.c
+++ b/os/hal/src/usbh/hal_usbh_uvc.c
@@ -77,7 +77,7 @@ static const usbh_classdriver_vmt_t class_driver_vmt = {
 	_uvc_unload
 };
 const usbh_classdriverinfo_t usbhuvcClassDriverInfo = {
-	0x0e, 0x03, 0x00, "UVC", &class_driver_vmt
+	"UVC", &class_driver_vmt
 };
 
 static bool _request(USBHUVCDriver *uvcdp,
@@ -518,7 +518,8 @@ static usbh_baseclassdriver_t *_uvc_load(usbh_device_t *dev, const uint8_t *desc
 	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 */