Revert "usb: workaround code for legacy usb client"

This reverts commit 35171555725ea09c7ef09df8747075e8976a7540.

Change-Id: I732aa18d6e4d3d140a88947f47814a1a54442104
Signed-off-by: Paweł Szewczyk <p.szewczyk@samsung.com>

1 files changed, 17 insertions, 19 deletions

diff --git a/hw/usb_client/usb_client.c b/hw/usb_client/usb_client.c
index bfc2d20..824e562 100644
--- a/hw/usb_client/usb_client.c
+++ b/hw/usb_client/usb_client.c
@@ -332,7 +332,7 @@ static int legacy_get_current_gadget(struct usb_client *usb,
 	struct usb_gadget_strings *strs;
 	struct usb_configuration **configs;
 	struct usb_function **funcs;
-	int i, j;
+	int i;
 	int ret = -ENOMEM;
 
 	gadget = zalloc(sizeof(*gadget));
@@ -373,13 +373,6 @@ static int legacy_get_current_gadget(struct usb_client *usb,
 	if (ret)
 		goto free_config_1;
 
-	i = 0;
-	for (j = 0; configs[0] && configs[0]->funcs[j]; i++, j++)
-		gadget->funcs[i] = configs[0]->funcs[j];
-
-	for (j = 0; configs[1] && configs[1]->funcs[j]; i++, j++)
-		gadget->funcs[i] = configs[1]->funcs[j];
-
 	*_gadget = gadget;
 	return 0;
 
@@ -608,24 +601,31 @@ static int legacy_enable(struct usb_client *usb)
 		goto disable_gadget;
 
 	for (i = 0; gadget->funcs[i]; ++i) {
-		if (gadget->funcs[i]->function_group != USB_FUNCTION_GROUP_WITH_SERVICE)
+		if (gadget->funcs[i]->function_group !=
+		    USB_FUNCTION_GROUP_WITH_SERVICE)
 			continue;
 
-		fws = container_of(gadget->funcs[i], struct usb_function_with_service, func);
+		fws = container_of(gadget->funcs[i],
+				   struct usb_function_with_service, func);
 		ret = systemd_start_service(fws->service);
 		if (ret < 0)
-			_E("start service failed: %s", fws->service);
+			goto stop_services;
 	}
 
-	// TODO. free gadget
-	// legacy_free_gadget(gadget);
 	return 0;
+stop_services:
+	while (--i >= 0) {
+		if (gadget->funcs[i]->function_group !=
+		    USB_FUNCTION_GROUP_WITH_SERVICE)
+			continue;
+
+		fws = container_of(gadget->funcs[i],
+				   struct usb_function_with_service, func);
+		systemd_stop_service(fws->service);
+	}
 
 disable_gadget:
 	sys_set_str(LEGACY_ENABLE_PATH, LEGACY_DISABLE);
-
-	// TODO. free gadget
-	// legacy_free_gadget(gadget);
 	return ret;
 }
 
@@ -647,11 +647,9 @@ static int legacy_disable(struct usb_client *usb)
 		fws = container_of(gadget->funcs[i], struct usb_function_with_service, func);
 		ret = systemd_stop_service(fws->service);
 		if (ret < 0)
-			_E("stop service failed: %s", fws->service);
+			return ret;
 	}
 
-	// TODO. free gadget
-	// legacy_free_gadget(gadget);
 	return sys_set_str(LEGACY_ENABLE_PATH, LEGACY_DISABLE);
 }