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
(cherry picked from commit 25b632db82163a339c23ffafc1095be225816b25)

1 files changed, 51 insertions, 45 deletions

diff --git a/hw/usb_cfs_client_common.c b/hw/usb_cfs_client_common.c
index 39dfca2..66ca48b 100644
--- 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;
 }