[PATCH 4/8] usb: Extract usb_select_configuration_no() helper

[Jonas Karlman]

U-Boot only has support for one configuration for devices.

Extract handing of selecting active configuration to a helper function
to make it easier for drivers to select a different configuration.

This include a small change in behavior, errors from parsing config
descriptors are no longer ignored and is instead returned.

Signed-off-by: Jonas Karlman <jonas at kwiboo.se>
---
 common/usb.c  | 59 ++++++++++++++++++++++++++++++---------------------
 include/usb.h | 10 +++++++++
 2 files changed, 45 insertions(+), 24 deletions(-)

diff --git a/common/usb.c b/common/usb.c
index 6a4ad346f4ba..625512a1c99a 100644
--- a/common/usb.c
+++ b/common/usb.c
@@ -1149,11 +1149,38 @@ static int usb_device_is_ignored(u16 id_vendor, u16 id_product)
 	return 0;
 }
 
-int usb_select_config(struct usb_device *dev)
+int usb_select_configuration_no(struct usb_device *dev, int cfgno)
 {
 	unsigned char *tmpbuf = NULL;
 	int err;
 
+	err = usb_get_configuration_len(dev, cfgno);
+	if (err >= 0) {
+		tmpbuf = (unsigned char *)malloc_cache_aligned(err);
+		if (!tmpbuf)
+			err = -ENOMEM;
+		else
+			err = usb_get_configuration_no(dev, cfgno, tmpbuf, err);
+	}
+	if (err < 0) {
+		free(tmpbuf);
+		return err;
+	}
+
+	err = usb_parse_config(dev, tmpbuf, cfgno);
+	free(tmpbuf);
+	if (err < 0)
+		return err;
+
+	usb_set_maxpacket(dev);
+
+	return usb_set_configuration(dev, dev->config.desc.bConfigurationValue);
+}
+
+int usb_select_config(struct usb_device *dev)
+{
+	int err;
+
 	err = get_descriptor_len(dev, USB_DT_DEVICE_SIZE, USB_DT_DEVICE_SIZE);
 	if (err)
 		return err;
@@ -1193,34 +1220,18 @@ int usb_select_config(struct usb_device *dev)
 	 */
 	mdelay(1);
 
-	/* only support for one config for now */
-	err = usb_get_configuration_len(dev, 0);
-	if (err >= 0) {
-		tmpbuf = (unsigned char *)malloc_cache_aligned(err);
-		if (!tmpbuf)
-			err = -ENOMEM;
-		else
-			err = usb_get_configuration_no(dev, 0, tmpbuf, err);
-	}
-	if (err < 0) {
-		printf("usb_new_device: Cannot read configuration, " \
-		       "skipping device %04x:%04x\n",
-		       dev->descriptor.idVendor, dev->descriptor.idProduct);
-		free(tmpbuf);
-		return err;
-	}
-	usb_parse_config(dev, tmpbuf, 0);
-	free(tmpbuf);
-	usb_set_maxpacket(dev);
 	/*
-	 * we set the default configuration here
+	 * we select the default configuration here
 	 * This seems premature. If the driver wants a different configuration
 	 * it will need to select itself.
 	 */
-	err = usb_set_configuration(dev, dev->config.desc.bConfigurationValue);
+	err = usb_select_configuration_no(dev, 0);
 	if (err < 0) {
-		printf("failed to set default configuration " \
-			"len %d, status %lX\n", dev->act_len, dev->status);
+		printf("failed to select default configuration " \
+			"len %d, status %lX, " \
+			"skipping device %04x:%04x\n",
+			dev->act_len, dev->status,
+			dev->descriptor.idVendor, dev->descriptor.idProduct);
 		return err;
 	}
 
diff --git a/include/usb.h b/include/usb.h
index 44c5d1cc5999..e9d16ee6c9fd 100644
--- a/include/usb.h
+++ b/include/usb.h
@@ -900,6 +900,16 @@ int usb_scan_device(struct udevice *parent, int port,
  */
 struct udevice *usb_get_bus(struct udevice *dev);
 
+/**
+ * usb_select_configuration_no() - Select configuration to use for a device
+ *
+ * This re-reads the configuration descriptor and sets the configuration.
+ *
+ * @dev:	Device to select configuration
+ * @cfgno:	Configuration number to set
+ */
+int usb_select_configuration_no(struct usb_device *dev, int cfgno);
+
 /**
  * usb_select_config() - Set up a device ready for use
  *
-- 
2.54.0