[usb] Select preferred USB device configuration based on driver score

Generate a score for each possible USB device configuration based on
the available driver support, and select the configuration with the
highest score.  This will allow us to prefer ECM over RNDIS (for
devices which support both) and will allow us to meaningfully select a
configuration even when we have drivers available for all functions
(e.g. when exposing unused functions via EFI_USB_IO_PROTOCOL).

Signed-off-by: Michael Brown <mcb30@ipxe.org>

1 files changed, 254 insertions, 146 deletions

diff --git a/src/drivers/bus/usb.c b/src/drivers/bus/usb.c
index cd80c320..d8d7f6b3 100644
--- a/src/drivers/bus/usb.c
+++ b/src/drivers/bus/usb.c
@@ -901,75 +901,154 @@ int usb_get_string_descriptor ( struct usb_device *usb, unsigned int index,
  */
 
 /**
+ * Get USB configuration descriptor
+ *
+ * @v usb		USB device
+ * @v index		Configuration index
+ * @ret config		Configuration descriptor
+ * @ret rc		Return status code
+ *
+ * The configuration descriptor is dynamically allocated and must
+ * eventually be freed by the caller.
+ */
+static int
+usb_config_descriptor ( struct usb_device *usb, unsigned int index,
+			struct usb_configuration_descriptor **config ) {
+	struct usb_configuration_descriptor partial;
+	size_t len;
+	int rc;
+
+	/* Read first part of configuration descriptor to get size */
+	if ( ( rc = usb_get_config_descriptor ( usb, index, &partial,
+						sizeof ( partial ) ) ) != 0 ) {
+		DBGC ( usb, "USB %s could not get configuration descriptor %d: "
+		       "%s\n", usb->name, index, strerror ( rc ) );
+		goto err_get_partial;
+	}
+	len = le16_to_cpu ( partial.len );
+	if ( len < sizeof ( partial ) ) {
+		DBGC ( usb, "USB %s underlength configuraton descriptor %d\n",
+		       usb->name, index );
+		rc = -EINVAL;
+		goto err_partial_len;
+	}
+
+	/* Allocate buffer for whole configuration descriptor */
+	*config = malloc ( len );
+	if ( ! *config ) {
+		rc = -ENOMEM;
+		goto err_alloc_config;
+	}
+
+	/* Read whole configuration descriptor */
+	if ( ( rc = usb_get_config_descriptor ( usb, index, *config,
+						len ) ) != 0 ) {
+		DBGC ( usb, "USB %s could not get configuration descriptor %d: "
+		       "%s\n", usb->name, index, strerror ( rc ) );
+		goto err_get_config_descriptor;
+	}
+	if ( (*config)->len != partial.len ) {
+		DBGC ( usb, "USB %s bad configuration descriptor %d length\n",
+		       usb->name, index );
+		rc = -EINVAL;
+		goto err_config_len;
+	}
+
+	return 0;
+
+ err_config_len:
+ err_get_config_descriptor:
+	free ( *config );
+ err_alloc_config:
+ err_partial_len:
+ err_get_partial:
+	return rc;
+}
+
+/**
  * Describe USB function
  *
- * @v func		USB function
+ * @v usb		USB device
  * @v config		Configuration descriptor
  * @v first		First interface number
+ * @v interfaces	Interface list to fill in
+ * @v desc		Function descriptor to fill in
  * @ret rc		Return status code
  */
-static int usb_function ( struct usb_function *func,
+static int usb_describe ( struct usb_device *usb,
 			  struct usb_configuration_descriptor *config,
-			  unsigned int first ) {
-	struct usb_device *usb = func->usb;
+			  unsigned int first, uint8_t *interfaces,
+			  struct usb_function_descriptor *desc ) {
 	struct usb_interface_association_descriptor *association;
 	struct usb_interface_descriptor *interface;
 	struct cdc_union_descriptor *cdc_union;
 	unsigned int i;
 
+	/* Fill in vendor and product ID */
+	desc->vendor = le16_to_cpu ( usb->device.vendor );
+	desc->product = le16_to_cpu ( usb->device.product );
+
 	/* First, look for an interface association descriptor */
 	association = usb_interface_association_descriptor ( config, first );
 	if ( association ) {
 
 		/* Sanity check */
-		if ( association->count > config->interfaces ) {
+		assert ( association->first == first );
+		if ( ( first + association->count ) > config->interfaces ) {
 			DBGC ( usb, "USB %s has invalid association [%d-%d)\n",
-			       func->name, association->first,
-			       ( association->first + association->count ) );
+			       usb->name, first, ( first + association->count));
 			return -ERANGE;
 		}
 
 		/* Describe function */
-		memcpy ( &func->class, &association->class,
-			 sizeof ( func->class ) );
-		func->count = association->count;
+		memcpy ( &desc->class, &association->class,
+			 sizeof ( desc->class ) );
+		desc->count = association->count;
 		for ( i = 0 ; i < association->count ; i++ )
-			func->interface[i] = ( association->first + i );
+			interfaces[i] = ( first + i );
 		return 0;
 	}
 
 	/* Next, look for an interface descriptor */
 	interface = usb_interface_descriptor ( config, first, 0 );
 	if ( ! interface ) {
-		DBGC ( usb, "USB %s has no interface descriptor\n",
-		       func->name );
+		DBGC ( usb, "USB %s has no descriptor for interface %d\n",
+		       usb->name, first );
 		return -ENOENT;
 	}
 
 	/* Describe function */
-	memcpy ( &func->class, &interface->class, sizeof ( func->class ) );
-	func->count = 1;
-	func->interface[0] = first;
+	memcpy ( &desc->class, &interface->class, sizeof ( desc->class ) );
+	desc->count = 1;
+	interfaces[0] = first;
 
 	/* Look for a CDC union descriptor, if applicable */
-	if ( ( func->class.class == USB_CLASS_CDC ) &&
+	if ( ( desc->class.class == USB_CLASS_CDC ) &&
 	     ( cdc_union = cdc_union_descriptor ( config, interface ) ) ) {
 
 		/* Determine interface count */
-		func->count = ( ( cdc_union->header.len -
+		desc->count = ( ( cdc_union->header.len -
 				  offsetof ( typeof ( *cdc_union ),
 					     interface[0] ) ) /
 				sizeof ( cdc_union->interface[0] ) );
-		if ( func->count > config->interfaces ) {
+		if ( desc->count > config->interfaces ) {
 			DBGC ( usb, "USB %s has invalid union functional "
 			       "descriptor with %d interfaces\n",
-			       func->name, func->count );
+			       usb->name, desc->count );
 			return -ERANGE;
 		}
 
 		/* Describe function */
-		for ( i = 0 ; i < func->count ; i++ )
-			func->interface[i] = cdc_union->interface[i];
+		for ( i = 0 ; i < desc->count ; i++ ) {
+			if ( cdc_union->interface[i] >= config->interfaces ) {
+				DBGC ( usb, "USB %s has invalid union "
+				       "functional descriptor covering "
+				       "interface %d\n", usb->name,
+				       cdc_union->interface[i] );
+				return -ERANGE;
+			}
+			interfaces[i] = cdc_union->interface[i];
+		}
 
 		return 0;
 	}
@@ -978,16 +1057,37 @@ static int usb_function ( struct usb_function *func,
 }
 
 /**
+ * Update list of used interface
+ *
+ * @v usb		USB device
+ * @v count		Number of interfaces
+ * @v interface		List of interfaces
+ * @v used		List of already-used interfaces
+ * @ret rc		Return status code
+ */
+static int usb_used ( struct usb_device *usb, unsigned int count,
+		      uint8_t *interface, uint8_t *used ) {
+	unsigned int i;
+
+	for ( i = 0 ; i < count ; i++ ) {
+		if ( used[interface[i]] ) {
+			DBGC ( usb, "USB %s interface %d already in use\n",
+			       usb->name, interface[i] );
+			return -EINVAL;
+		}
+		used[interface[i]] = 1;
+	}
+	return 0;
+}
+
+/**
  * Find USB device driver
  *
- * @v vendor		Vendor ID
- * @v product		Product ID
- * @v class		Class
+ * @v desc		Function descriptor
  * @ret id		USB device ID, or NULL
  * @ret driver		USB device driver, or NULL
  */
-struct usb_driver * usb_find_driver ( unsigned int vendor, unsigned int product,
-				      struct usb_class *class,
+struct usb_driver * usb_find_driver ( struct usb_function_descriptor *desc,
 				      struct usb_device_id **id ) {
 	struct usb_driver *driver;
 	unsigned int i;
@@ -998,13 +1098,13 @@ struct usb_driver * usb_find_driver ( unsigned int vendor, unsigned int product,
 
 			/* Check for a matching ID */
 			*id = &driver->ids[i];
-			if ( ( ( (*id)->vendor == vendor ) ||
+			if ( ( ( (*id)->vendor == desc->vendor ) ||
 			       ( (*id)->vendor == USB_ANY_ID ) ) &&
-			     ( ( (*id)->product == product ) ||
+			     ( ( (*id)->product == desc->product ) ||
 			       ( (*id)->product == USB_ANY_ID ) ) &&
-			     ( (*id)->class.class == class->class ) &&
-			     ( (*id)->class.subclass == class->subclass ) &&
-			     ( (*id)->class.protocol == class->protocol ) )
+			     ( (*id)->class.class == desc->class.class ) &&
+			     ( (*id)->class.subclass == desc->class.subclass )&&
+			     ( (*id)->class.protocol == desc->class.protocol ) )
 				return driver;
 		}
 	}
@@ -1015,6 +1115,51 @@ struct usb_driver * usb_find_driver ( unsigned int vendor, unsigned int product,
 }
 
 /**
+ * Get USB device configuration score
+ *
+ * @v usb		USB device
+ * @v config		Configuration descriptor
+ * @ret score		Device configuration score, or negative error
+ */
+static int usb_score ( struct usb_device *usb,
+		       struct usb_configuration_descriptor *config ) {
+	uint8_t used[config->interfaces];
+	uint8_t interface[config->interfaces];
+	struct usb_function_descriptor desc;
+	struct usb_driver *driver;
+	struct usb_device_id *id;
+	unsigned int first;
+	unsigned int score = 0;
+	int rc;
+
+	/* Identify each function in turn */
+	memset ( used, 0, sizeof ( used ) );
+	for ( first = 0 ; first < config->interfaces ; first++ ) {
+
+		/* Skip interfaces already used */
+		if ( used[first] )
+			continue;
+
+		/* Describe function */
+		if ( ( rc = usb_describe ( usb, config, first, interface,
+					   &desc ) ) != 0 )
+			return rc;
+
+		/* Update used interfaces */
+		if ( ( rc = usb_used ( usb, desc.count, interface,
+				       used ) ) != 0 )
+			return rc;
+
+		/* Look for a driver for this function */
+		driver = usb_find_driver ( &desc, &id );
+		if ( driver )
+			score += driver->score;
+	}
+
+	return score;
+}
+
+/**
  * Probe USB device driver
  *
  * @v func		USB function
@@ -1029,13 +1174,12 @@ static int usb_probe ( struct usb_function *func,
 	int rc;
 
 	/* Identify driver */
-	driver = usb_find_driver ( func->dev.desc.vendor, func->dev.desc.device,
-				   &func->class, &id );
+	driver = usb_find_driver ( &func->desc, &id );
 	if ( ! driver ) {
 		DBGC ( usb, "USB %s %04x:%04x class %d:%d:%d has no driver\n",
-		       func->name, func->dev.desc.vendor, func->dev.desc.device,
-		       func->class.class, func->class.subclass,
-		       func->class.protocol );
+		       func->name, func->desc.vendor, func->desc.product,
+		       func->desc.class.class, func->desc.class.subclass,
+		       func->desc.class.protocol );
 		return -ENOENT;
 	}
 
@@ -1106,28 +1250,24 @@ usb_probe_all ( struct usb_device *usb,
 		list_add_tail ( &func->list, &usb->functions );
 
 		/* Identify function */
-		if ( ( rc = usb_function ( func, config, first ) ) != 0 )
-			goto err_function;
-		assert ( func->count <= config->interfaces );
+		if ( ( rc = usb_describe ( usb, config, first, func->interface,
+					   &func->desc ) ) != 0 )
+			goto err_describe;
+		assert ( func->desc.count <= config->interfaces );
 
 		/* Mark interfaces as used */
-		for ( i = 0 ; i < func->count ; i++ ) {
-			if ( func->interface[i] >= config->interfaces ) {
-				DBGC ( usb, "USB %s has invalid interface %d\n",
-				       func->name, func->interface[i] );
-				goto err_interface;
-			}
-			used[ func->interface[i] ] = 1;
-		}
+		if ( ( rc = usb_used ( usb, func->desc.count, func->interface,
+				       used ) ) != 0 )
+			goto err_used;
 
 		/* Probe device driver */
 		if ( ( rc = usb_probe ( func, config ) ) != 0 )
 			goto err_probe;
 		DBGC ( usb, "USB %s %04x:%04x class %d:%d:%d interfaces ",
-		       func->name, func->dev.desc.vendor, func->dev.desc.device,
-		       func->class.class, func->class.subclass,
-		       func->class.protocol );
-		for ( i = 0 ; i < func->count ; i++ )
+		       func->name, func->desc.vendor, func->desc.product,
+		       func->desc.class.class, func->desc.class.subclass,
+		       func->desc.class.protocol );
+		for ( i = 0 ; i < func->desc.count ; i++ )
 			DBGC ( usb, "%s%d", ( i ? "," : "" ),
 			       func->interface[i] );
 		DBGC ( usb, " using driver %s\n", func->dev.driver_name );
@@ -1140,8 +1280,8 @@ usb_probe_all ( struct usb_device *usb,
 		list_del ( &func->dev.siblings );
 		usb_remove ( func );
 	err_probe:
-	err_interface:
-	err_function:
+	err_used:
+	err_describe:
 		list_del ( &func->list );
 		free ( func );
 	err_alloc:
@@ -1178,82 +1318,6 @@ static void usb_remove_all ( struct usb_device *usb ) {
 }
 
 /**
- * Select USB device configuration
- *
- * @v usb		USB device
- * @v index		Configuration index
- * @ret rc		Return status code
- */
-static int usb_configure ( struct usb_device *usb, unsigned int index ) {
-	struct usb_configuration_descriptor partial;
-	struct usb_configuration_descriptor *config;
-	size_t len;
-	int rc;
-
-	/* Read first part of configuration descriptor to get size */
-	if ( ( rc = usb_get_config_descriptor ( usb, index, &partial,
-						sizeof ( partial ) ) ) != 0 ) {
-		DBGC ( usb, "USB %s could not get configuration descriptor %d: "
-		       "%s\n", usb->name, index, strerror ( rc ) );
-		goto err_get_partial;
-	}
-	len = le16_to_cpu ( partial.len );
-	if ( len < sizeof ( partial ) ) {
-		DBGC ( usb, "USB %s underlength configuraton descriptor %d\n",
-		       usb->name, index );
-		rc = -EINVAL;
-		goto err_partial_len;
-	}
-
-	/* Allocate buffer for whole configuration descriptor */
-	config = malloc ( len );
-	if ( ! config ) {
-		rc = -ENOMEM;
-		goto err_alloc_config;
-	}
-
-	/* Read whole configuration descriptor */
-	if ( ( rc = usb_get_config_descriptor ( usb, index, config,
-						len ) ) != 0 ) {
-		DBGC ( usb, "USB %s could not get configuration descriptor %d: "
-		       "%s\n", usb->name, index, strerror ( rc ) );
-		goto err_get_config_descriptor;
-	}
-	if ( config->len != partial.len ) {
-		DBGC ( usb, "USB %s bad configuration descriptor %d length\n",
-		       usb->name, index );
-		rc = -EINVAL;
-		goto err_config_len;
-	}
-
-	/* Set configuration */
-	if ( ( rc = usb_set_configuration ( usb, config->config ) ) != 0){
-		DBGC ( usb, "USB %s could not set configuration %d: %s\n",
-		       usb->name, config->config, strerror ( rc ) );
-		goto err_set_configuration;
-	}
-
-	/* Probe USB device drivers */
-	usb_probe_all ( usb, config );
-
-	/* Free configuration descriptor */
-	free ( config );
-
-	return 0;
-
-	usb_remove_all ( usb );
-	usb_set_configuration ( usb, 0 );
- err_set_configuration:
- err_config_len:
- err_get_config_descriptor:
-	free ( config );
- err_alloc_config:
- err_partial_len:
- err_get_partial:
-	return rc;
-}
-
-/**
  * Clear USB device configuration
  *
  * @v usb		USB device
@@ -1275,32 +1339,76 @@ static void usb_deconfigure ( struct usb_device *usb ) {
 }
 
 /**
- * Find and select a supported USB device configuration
+ * Choose our preferred USB device configuration
  *
  * @v usb		USB device
  * @ret rc		Return status code
  */
-static int usb_configure_any ( struct usb_device *usb ) {
+static int usb_autoconfigure ( struct usb_device *usb ) {
+	struct usb_configuration_descriptor *config;
+	unsigned int preferred = 0;
 	unsigned int index;
-	int rc = -ENOENT;
+	int score;
+	int best = 0;
+	int rc;
 
-	/* Attempt all configuration indexes */
+	/* Calculate driver score for each configuration index */
 	for ( index = 0 ; index < usb->device.configurations ; index++ ) {
 
-		/* Attempt this configuration index */
-		if ( ( rc = usb_configure ( usb, index ) ) != 0 )
-			continue;
+		/* Read configuration descriptor */
+		if ( ( rc = usb_config_descriptor ( usb, index,
+						    &config ) ) != 0 )
+			goto err_config;
 
-		/* If we have no drivers, then try the next configuration */
-		if ( list_empty ( &usb->functions ) ) {
-			rc = -ENOTSUP;
-			usb_deconfigure ( usb );
-			continue;
+		/* Get score for this configuration */
+		score = usb_score ( usb, config );
+		if ( score < 0 ) {
+			rc = score;
+			goto err_score;
 		}
+		DBGC2 ( usb, "USB %s configuration %d score %d\n",
+			usb->name, config->config, score );
 
-		return 0;
+		/* Record as preferred configuration, if applicable */
+		if ( score > best ) {
+			best = score;
+			preferred = index;
+		}
+
+		/* Free configuration descriptor */
+		free ( config );
+		config = NULL;
 	}
 
+	/* Read preferred configuration descriptor */
+	if ( ( rc = usb_config_descriptor ( usb, preferred, &config ) ) != 0 )
+		goto err_preferred;
+
+	/* Set configuration */
+	if ( ( rc = usb_set_configuration ( usb, config->config ) ) != 0){
+		DBGC ( usb, "USB %s could not set configuration %d: %s\n",
+		       usb->name, config->config, strerror ( rc ) );
+		goto err_set_configuration;
+	}
+
+	/* Probe USB device drivers */
+	usb_probe_all ( usb, config );
+
+	/* Free configuration descriptor */
+	free ( config );
+
+	return 0;
+
+	usb_remove_all ( usb );
+	usb_set_configuration ( usb, 0 );
+ err_set_configuration:
+	free ( config );
+ err_preferred:
+	return rc;
+
+ err_score:
+	free ( config );
+ err_config:
 	return rc;
 }
 
@@ -1444,13 +1552,13 @@ static int register_usb ( struct usb_device *usb ) {
 	       usb_speed_name ( port->speed ), usb->control.mtu );
 
 	/* Configure device */
-	if ( ( rc = usb_configure_any ( usb ) ) != 0 )
-		goto err_configure_any;
+	if ( ( rc = usb_autoconfigure ( usb ) ) != 0 )
+		goto err_autoconfigure;
 
 	return 0;
 
 	usb_deconfigure ( usb );
- err_configure_any:
+ err_autoconfigure:
  err_get_device_descriptor:
  err_mtu:
  err_get_mtu: