core: adjust search for the device driver

in finding compatibility there are 3 steps how to find
out whether the provided device driver supports currently
inserted device.

1. compare fields in the driver info structure
If there is a match (all CHECK_PARTIAL_COMPATIBILITY() macros return true),
we proceed to step 2.
If all device drivers are searched, but none is compatible ->
that means no device driver is available for currently inserted device.

2. try to call driver's init function.
If it return non-null pointer to data we may proceed to the step 3.
Otherwise, we continue in the loop handling step 1.

3. call analyze descriptor for all descriptors. When it returns true,
it means success and that the driver supports current device. From now on
poll function is allowed to be called.
If all descriptors were provided to the analyze_descriptor method and
it still returns false, it means device driver is not initialized and
should not be used with this device.

Signed-off-by: Amir Hammad <amir.hammad@hotmail.com>

1 files changed, 29 insertions, 28 deletions

diff --git a/src/usbh_core.c b/src/usbh_core.c
index 60baf64..d6f6ff4 100644
--- a/src/usbh_core.c
+++ b/src/usbh_core.c
@@ -53,7 +53,7 @@ static bool enumeration(void)
 /**
  *
  */
-static const usbh_dev_driver_t *find_driver(const usbh_dev_driver_info_t * device_info)
+static bool find_driver(usbh_device_t *dev, const usbh_dev_driver_info_t * device_info)
 {
 
 #define CHECK_PARTIAL_COMPATIBILITY(what) \
@@ -63,7 +63,6 @@ static const usbh_dev_driver_t *find_driver(const usbh_dev_driver_info_t * devic
 		continue;\
 	}
 
-
 	int i = 0;
 
 	while (usbh_data.dev_drivers[i]) {
@@ -77,9 +76,16 @@ static const usbh_dev_driver_t *find_driver(const usbh_dev_driver_info_t * devic
 		CHECK_PARTIAL_COMPATIBILITY(idVendor);
 		CHECK_PARTIAL_COMPATIBILITY(idProduct);
 
-		return usbh_data.dev_drivers[i];
+		dev->drv = usbh_data.dev_drivers[i];
+		dev->drvdata = dev->drv->init(dev);
+		if (!dev->drvdata) {
+			LOG_PRINTF("Unable to initialize device driver at index %d\n", i);
+			i++;
+			continue;
+		}
+		return true;
 	}
-	return 0;
+	return false;
 #undef CHECK_PARTIAL_COMPATIBILITY
 }
 
@@ -122,14 +128,26 @@ static void device_register(void *descriptors, uint16_t descriptors_len, usbh_de
 			device_info.ifaceClass = iface->bInterfaceClass;
 			device_info.ifaceSubClass = iface->bInterfaceSubClass;
 			device_info.ifaceProtocol = iface->bInterfaceProtocol;
-			const usbh_dev_driver_t *driver = find_driver(&device_info);
-			if (driver) {
-				dev->drv = driver;
-				dev->drvdata = dev->drv->init(dev);
-				if (!dev->drvdata) {
-					LOG_PRINTF("CANT TOUCH THIS");
+			if (find_driver(dev, &device_info)) {
+				int k = 0;
+				while (k < descriptors_len) {
+					desc_len = buf[k];
+					void *drvdata = dev->drvdata;
+					LOG_PRINTF("[%d]", buf[k+1]);
+					if (dev->drv->analyze_descriptor(drvdata, &buf[k])) {
+						LOG_PRINTF("Device Initialized\n");
+						return;
+					}
+
+					if (desc_len == 0) {
+						LOG_PRINTF("Problem occured while parsing complete configuration descriptor");
+						return;
+					}
+					k += desc_len;
 				}
-				break;
+				LOG_PRINTF("Device driver isn't compatible with this device\n");
+			} else {
+				LOG_PRINTF("No compatible driver has been found for interface #%d\n", iface->bInterfaceNumber);
 			}
 		}
 			break;
@@ -142,23 +160,6 @@ static void device_register(void *descriptors, uint16_t descriptors_len, usbh_de
 			return;
 		}
 		i += desc_len;
-
-	}
-
-	if (dev->drv && dev->drvdata) {
-		// analyze descriptors
-		LOG_PRINTF("ANALYZE");
-		i = 0;
-		while (i < descriptors_len) {
-			desc_len = buf[i];
-			void *drvdata = dev->drvdata;
-			LOG_PRINTF("[%d]",buf[i+1]);
-			if (dev->drv->analyze_descriptor(drvdata, &buf[i])) {
-				LOG_PRINTF("Device Initialized\n");
-				return;
-			}
-			i += desc_len;
-		}
 	}
 	LOG_PRINTF("Device NOT Initialized\n");
 }