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: