Fixed using wrong usb function list when usb gadget enable/disable in configfs

When usb gadget is enabled/disabled in configfs,
it retrieves the list of currently used usb functions in order to run the service handler.

Before the modification, it retrieve a list of "all functions" used even once.

So if you run "ifconfig" after changing usb mode to "sdb mode" -> "rndis mode" -> "sdb mode",
even in sdb mode, the usb0 network interface appears.

Change-Id: I83564591cd899197077823fdb2fbe25d034fcf46

diff --git a/hw/usb_cfs_client_common.c b/hw/usb_cfs_client_common.c
index 39dfca2..66ca48b 100644 (file)
--- a/hw/usb_cfs_client_common.c
+++ b/hw/usb_cfs_client_common.c
@@ -835,70 +835,83 @@ out:
        return ret;
 }
 
+static void cfs_start_stop_service_handler(usbg_gadget *gadget, bool start, bool post_stop)
+{
+       int index;
+       const char *name;
+       const char *instance;
+       usbg_config *config;
+       usbg_binding *binding;
+       usbg_function *function;
+       struct usb_function *usb_function;
+
+       /* Execute service and handler related to functions bound to configs */
+       usbg_for_each_config(config, gadget) {
+               usbg_for_each_binding(binding, config) {
+                       function = usbg_get_binding_target(binding);
+                       instance = usbg_get_function_instance(function);
+                       name = usbg_get_function_type_str(usbg_get_function_type(function));
+
+                       index = cfs_find_func(name, instance);
+                       if (index < 0)
+                               continue;
+
+                       usb_function = _available_funcs[index];
+
+                       if (start) {
+                               if (usb_function->handler)
+                                       usb_function->handler(1);
+
+                               /* functionfs service is automatically started by socket activation */
+                               if (!usb_function->is_functionfs && usb_function->service)
+                                       (void)systemd_start_unit_wait_started(usb_function->service, ".service", -1);
+                       } else {
+                               if (post_stop) {
+                                       if (usb_function->is_functionfs && usb_function->service)
+                                               (void)systemd_stop_unit_wait_stopped(usb_function->service, ".service", -1);
+                               } else {
+                                       if (!usb_function->is_functionfs && usb_function->service)
+                                               (void)systemd_stop_unit_wait_stopped(usb_function->service, ".service", -1);
+
+                                       if (usb_function->handler)
+                                               usb_function->handler(0);
+                               }
+                       }
+               }
+       }
+}
+
 static int cfs_enable(struct usb_client *usb)
 {
-       int i;
        int ret;
-       struct usb_gadget *gadget;
-       struct usb_function *func;
        struct cfs_client *cfs_client;
 
        if (!usb)
                return -EINVAL;
 
        cfs_client = container_of(usb, struct cfs_client, client);
+
        ret = usbg_enable_gadget(cfs_client->gadget, cfs_client->udc);
        if (ret)
                return ret;
 
-       ret = cfs_get_current_gadget(usb, &gadget);
-       if (ret) {
-               usbg_disable_gadget(cfs_client->gadget);
-               return ret;
-       }
-
-       for (i = 0; gadget->funcs[i]; ++i) {
-               func = gadget->funcs[i];
-
-               if (func->handler)
-                       func->handler(1);
-
-               /* functionfs service is automatically started by socket activation */
-               if (!func->is_functionfs && func->service)
-                       (void)systemd_start_unit_wait_started(func->service, ".service", -1);
-       }
-
-       cfs_free_gadget(gadget);
+       cfs_start_stop_service_handler(cfs_client->gadget, true, false);
 
        return 0;
 }
 
 static int cfs_disable(struct usb_client *usb)
 {
-       int i;
        int ret;
-       struct usb_gadget *gadget;
-       struct usb_function *func;
        struct cfs_client *cfs_client;
 
        if (!usb)
                return -EINVAL;
 
-       ret = cfs_get_current_gadget(usb, &gadget);
-       if (ret)
-               return ret;
-
-       for (i = 0; gadget->funcs[i]; ++i) {
-               func = gadget->funcs[i];
-
-               if (!func->is_functionfs && func->service)
-                       (void)systemd_stop_unit_wait_stopped(func->service, ".service", -1);
+       cfs_client = container_of(usb, struct cfs_client, client);
 
-               if (func->handler)
-                       func->handler(0);
-       }
+       cfs_start_stop_service_handler(cfs_client->gadget, false, false);
 
-       cfs_client = container_of(usb, struct cfs_client, client);
        ret = usbg_disable_gadget(cfs_client->gadget); /* ignore error checking */
 
        /*
@@ -906,14 +919,7 @@ static int cfs_disable(struct usb_client *usb)
         * If usb data may come in after stopping functionfs service and before disabling gadget,
         * functionfs service wakes up again by socket activation.
         */
-       for (i = 0; gadget->funcs[i]; ++i) {
-               func = gadget->funcs[i];
-
-               if (func->is_functionfs && func->service)
-                       (void)systemd_stop_unit_wait_stopped(func->service, ".service", -1);
-       }
-
-       cfs_free_gadget(gadget);
+       cfs_start_stop_service_handler(cfs_client->gadget, false, true);
 
        return ret;
 }