Simplify finding valid configuration, interface and endpoints and refactor to a separate function.

This function can later be used to determine active mode.

1 files changed, 87 insertions, 99 deletions

diff --git a/src/usb.c b/src/usb.c
index b0810a2..274bebc 100644
--- a/src/usb.c
+++ b/src/usb.c
@@ -390,132 +390,120 @@ static struct usb_device* find_device(int bus, int address)
         return NULL;
 }
 
-static void device_complete_initialization(struct mode_context *context, struct libusb_device_handle *handle) 
+/// @brief Finds and sets the valid configuration, interface and endpoints on the usb_device
+static int set_valid_configuration(struct libusb_device* dev, struct usb_device *usbdev, struct libusb_device_handle *handle)
 {
-        struct usb_device *usbdev = find_device(context->bus, context->address);
-        if(!usbdev) {
-                usbmuxd_log(LL_ERROR, "Device %d-%d is missing from device list, aborting initialization", context->bus, context->address);
-                return;
-        }
-        struct libusb_device *dev = context->dev;
+        int j, k, res, found = 0;
+        struct libusb_config_descriptor *config;
+        const struct libusb_interface_descriptor *intf;
         struct libusb_device_descriptor devdesc = usbdev->devdesc;
-        int bus = context->bus;
-        int address = context->address;
-        int desired_config = devdesc.bNumConfigurations;
-        int j, res;
-        struct libusb_transfer *transfer;
+        int bus = usbdev->bus;
+        int address = usbdev->address;
+        int current_config = 0;
+
+        if((res = libusb_get_configuration(handle, &current_config)) != 0) {
+                usbmuxd_log(LL_WARNING, "Could not get current configuration for device %d-%d: %s", bus, address, libusb_error_name(res));
+                return -1;
+        }
 
-        if(desired_config > 4) {
-                if(desired_config > 5) {
-                        usbmuxd_log(LL_ERROR, "Device %d-%d has more than 5 configurations, but usbmuxd doesn't support that. Choosing configuration 5 instead.", bus, address);
-                        desired_config = 5;
+        for(j = devdesc.bNumConfigurations ; j > 0  ; j--) {
+                if((res = libusb_get_config_descriptor_by_value(dev, j, &config)) != 0) {
+                        usbmuxd_log(LL_NOTICE, "Could not get configuration %i descriptor for device %i-%i: %s", j, bus, address, libusb_error_name(res));
+                        continue;
                 }
-                /* verify if the configuration 5 is actually usable */
-                do {
-                        struct libusb_config_descriptor *config;
-                        const struct libusb_interface_descriptor *intf;
-                        if (libusb_get_config_descriptor_by_value(dev, 5, &config) != 0) {
-                                usbmuxd_log(LL_WARNING, "Device %d-%d: Failed to get config descriptor for configuration 5, choosing configuration 4 instead.", bus, address);
-                                desired_config = 4;
-                                break;
-                        }
-                        // In Valeria mode, there are 3 interfaces and usbmuxd is at 2
-                        // In CDC NCM mode, there are 4 interfaces and usbmuxd is at 1
-                        // Otherwize, 0 is expected to be of a different class.
-                        int usbmux_intf_index = config->bNumInterfaces == 3 ? 2 : config->bNumInterfaces == 4 ? 1 : 0;
-                        intf = &config->interface[usbmux_intf_index].altsetting[0];
-                        if(intf->bInterfaceClass != INTERFACE_CLASS || 
-                           intf->bInterfaceSubClass != INTERFACE_SUBCLASS || 
-                           intf->bInterfaceProtocol != INTERFACE_PROTOCOL) {
-                                usbmuxd_log(LL_WARNING, "Device %d-%d: can't find usbmux interface in configuration 5, choosing configuration 4 instead.", bus, address);
-                                desired_config = 4;
+                for(k = 0 ; k < config->bNumInterfaces ; k++) {
+                        intf = &config->interface[k].altsetting[0];
+                        if(intf->bInterfaceClass == INTERFACE_CLASS || 
+                           intf->bInterfaceSubClass == INTERFACE_SUBCLASS || 
+                           intf->bInterfaceProtocol == INTERFACE_PROTOCOL) {
+                                usbmuxd_log(LL_NOTICE, "Found usbmux interface for device %i-%i: %i", bus, address, intf->bInterfaceNumber);
+                                if(intf->bNumEndpoints != 2) {
+                                        usbmuxd_log(LL_WARNING, "Endpoint count mismatch for interface %i of device %i-%i", intf->bInterfaceNumber, bus, address);
+                                        continue;
+                                }
+                                if((intf->endpoint[0].bEndpointAddress & 0x80) == LIBUSB_ENDPOINT_OUT &&
+                                   (intf->endpoint[1].bEndpointAddress & 0x80) == LIBUSB_ENDPOINT_IN) {
+                                        usbdev->interface = intf->bInterfaceNumber;
+                                        usbdev->ep_out = intf->endpoint[0].bEndpointAddress;
+                                        usbdev->ep_in = intf->endpoint[1].bEndpointAddress;
+                                        usbmuxd_log(LL_INFO, "Found interface %i with endpoints %02x/%02x for device %i-%i", usbdev->interface, usbdev->ep_out, usbdev->ep_in, bus, address);
+                                        found = 1;
+                                        break;
+                                } else if((intf->endpoint[1].bEndpointAddress & 0x80) == LIBUSB_ENDPOINT_OUT &&
+                                                  (intf->endpoint[0].bEndpointAddress & 0x80) == LIBUSB_ENDPOINT_IN) {
+                                        usbdev->interface = intf->bInterfaceNumber;
+                                        usbdev->ep_out = intf->endpoint[1].bEndpointAddress;
+                                        usbdev->ep_in = intf->endpoint[0].bEndpointAddress;
+                                        usbmuxd_log(LL_INFO, "Found interface %i with swapped endpoints %02x/%02x for device %i-%i", usbdev->interface, usbdev->ep_out, usbdev->ep_in, bus, address);
+                                        found = 1;
+                                        break;
+                                } else {
+                                        usbmuxd_log(LL_WARNING, "Endpoint type mismatch for interface %i of device %i-%i", intf->bInterfaceNumber, bus, address);
+                                }
                         }
+                }
+                if(!found) {
                         libusb_free_config_descriptor(config);
-                } while (0);
-        }
-        int current_config = 0;
-        if((res = libusb_get_configuration(handle, &current_config)) != 0) {
-                usbmuxd_log(LL_WARNING, "Could not get configuration for device %d-%d: %s", bus, address, libusb_error_name(res));
-                usbdev->alive = 0;
-                return;
-        }
-        if (current_config != desired_config) {
-                struct libusb_config_descriptor *config;
+                        continue;
+                }
+                // If set configuration is required, try to first detach all kernel drivers
                 if (current_config == 0) {
                         usbmuxd_log(LL_DEBUG, "Device %d-%d is unconfigured", bus, address);
-                } else if ((res = libusb_get_active_config_descriptor(dev, &config)) != 0) {
-                        usbmuxd_log(LL_NOTICE, "Could not get old configuration descriptor for device %d-%d: %s", bus, address, libusb_error_name(res));
-                } else {
-                        for(j=0; j<config->bNumInterfaces; j++) {
-                                const struct libusb_interface_descriptor *intf = &config->interface[j].altsetting[0];
-                                if((res = libusb_kernel_driver_active(handle, intf->bInterfaceNumber)) < 0) {
-                                        usbmuxd_log(LL_NOTICE, "Could not check kernel ownership of interface %d for device %d-%d: %s", intf->bInterfaceNumber, bus, address, libusb_error_name(res));
+                }
+                if(current_config == 0 || config->bConfigurationValue != current_config) {
+                        usbmuxd_log(LL_NOTICE, "Changing configuration of device %i-%i: %i -> %i", bus, address, config->bConfigurationValue, current_config);
+                        for(k=0 ; k < config->bNumInterfaces ; k++) {
+                                const struct libusb_interface_descriptor *intf1 = &config->interface[k].altsetting[0];
+                                if((res = libusb_kernel_driver_active(handle, intf1->bInterfaceNumber)) < 0) {
+                                        usbmuxd_log(LL_NOTICE, "Could not check kernel ownership of interface %d for device %d-%d: %s", intf1->bInterfaceNumber, bus, address, libusb_error_name(res));
                                         continue;
                                 }
                                 if(res == 1) {
-                                        usbmuxd_log(LL_INFO, "Detaching kernel driver for device %d-%d, interface %d", bus, address, intf->bInterfaceNumber);
-                                        if((res = libusb_detach_kernel_driver(handle, intf->bInterfaceNumber)) < 0) {
+                                        usbmuxd_log(LL_INFO, "Detaching kernel driver for device %d-%d, interface %d", bus, address, intf1->bInterfaceNumber);
+                                        if((res = libusb_detach_kernel_driver(handle, intf1->bInterfaceNumber)) < 0) {
                                                 usbmuxd_log(LL_WARNING, "Could not detach kernel driver, configuration change will probably fail! %s", libusb_error_name(res));
                                                 continue;
                                         }
                                 }
                         }
-                        libusb_free_config_descriptor(config);
-                }
-
-                usbmuxd_log(LL_INFO, "Setting configuration for device %d-%d, from %d to %d", bus, address, current_config, desired_config);
-                if((res = libusb_set_configuration(handle, desired_config)) != 0) {
-                        usbmuxd_log(LL_WARNING, "Could not set configuration %d for device %d-%d: %s", desired_config, bus, address, libusb_error_name(res));
-                        usbdev->alive = 0;
-                        return;
+                        if((res = libusb_set_configuration(handle, j)) != 0) {
+                                usbmuxd_log(LL_WARNING, "Could not set configuration %d for device %d-%d: %s", j, bus, address, libusb_error_name(res));
+                                libusb_free_config_descriptor(config);
+                                continue;
+                        }
                 }
+                
+                libusb_free_config_descriptor(config);
+                break;
         }
 
-        struct libusb_config_descriptor *config;
-        if((res = libusb_get_active_config_descriptor(dev, &config)) != 0) {
-                usbmuxd_log(LL_WARNING, "Could not get configuration descriptor for device %d-%d: %s", bus, address, libusb_error_name(res));
-                usbdev->alive = 0;
-                return;
+        if(!found) {
+                usbmuxd_log(LL_WARNING, "Could not find a suitable USB interface for device %i-%i", bus, address);
+                return -1;
         }
 
-        for(j=0; j<config->bNumInterfaces; j++) {
-                const struct libusb_interface_descriptor *intf = &config->interface[j].altsetting[0];
-                if(intf->bInterfaceClass != INTERFACE_CLASS ||
-                   intf->bInterfaceSubClass != INTERFACE_SUBCLASS ||
-                   intf->bInterfaceProtocol != INTERFACE_PROTOCOL)
-                        continue;
-                if(intf->bNumEndpoints != 2) {
-                        usbmuxd_log(LL_WARNING, "Endpoint count mismatch for interface %d of device %d-%d", intf->bInterfaceNumber, bus, address);
-                        continue;
-                }
-                if((intf->endpoint[0].bEndpointAddress & 0x80) == LIBUSB_ENDPOINT_OUT &&
-                   (intf->endpoint[1].bEndpointAddress & 0x80) == LIBUSB_ENDPOINT_IN) {
-                        usbdev->interface = intf->bInterfaceNumber;
-                        usbdev->ep_out = intf->endpoint[0].bEndpointAddress;
-                        usbdev->ep_in = intf->endpoint[1].bEndpointAddress;
-                        usbmuxd_log(LL_INFO, "Found interface %d with endpoints %02x/%02x for device %d-%d", usbdev->interface, usbdev->ep_out, usbdev->ep_in, bus, address);
-                        break;
-                } else if((intf->endpoint[1].bEndpointAddress & 0x80) == LIBUSB_ENDPOINT_OUT &&
-                          (intf->endpoint[0].bEndpointAddress & 0x80) == LIBUSB_ENDPOINT_IN) {
-                        usbdev->interface = intf->bInterfaceNumber;
-                        usbdev->ep_out = intf->endpoint[1].bEndpointAddress;
-                        usbdev->ep_in = intf->endpoint[0].bEndpointAddress;
-                        usbmuxd_log(LL_INFO, "Found interface %d with swapped endpoints %02x/%02x for device %d-%d", usbdev->interface, usbdev->ep_out, usbdev->ep_in, bus, address);
-                        break;
-                } else {
-                        usbmuxd_log(LL_WARNING, "Endpoint type mismatch for interface %d of device %d-%d", intf->bInterfaceNumber, bus, address);
-                }
+        return 0;
+}
+
+static void device_complete_initialization(struct mode_context *context, struct libusb_device_handle *handle) 
+{
+        struct usb_device *usbdev = find_device(context->bus, context->address);
+        if(!usbdev) {
+                usbmuxd_log(LL_ERROR, "Device %d-%d is missing from device list, aborting initialization", context->bus, context->address);
+                return;
         }
+        struct libusb_device *dev = context->dev;
+        struct libusb_device_descriptor devdesc = usbdev->devdesc;
+        int bus = context->bus;
+        int address = context->address;
+        int res;
+        struct libusb_transfer *transfer;
 
-        if(j == config->bNumInterfaces) {
-                usbmuxd_log(LL_WARNING, "Could not find a suitable USB interface for device %d-%d", bus, address);
-                libusb_free_config_descriptor(config);
+        if((res = set_valid_configuration(dev, usbdev, handle)) != 0) {
                 usbdev->alive = 0;
                 return;
         }
 
-        libusb_free_config_descriptor(config);
-
         if((res = libusb_claim_interface(handle, usbdev->interface)) != 0) {
                 usbmuxd_log(LL_WARNING, "Could not claim interface %d for device %d-%d: %s", usbdev->interface, bus, address, libusb_error_name(res));
                 usbdev->alive = 0;