Rework getting the currently enabled function list in the SLP gadget.

Before Change: Obtain all information of gadget from the enabled gadget.
After  Change: Obtain only necessary function list from the enabled gadget.

Change-Id: Ib6547209f443e7a58bbbdb87373e53d252cdbdb6

1 files changed, 91 insertions, 376 deletions

diff --git a/hw/usb_client_common.c b/hw/usb_client_common.c
index e8357e0..6822f32 100644
--- a/hw/usb_client_common.c
+++ b/hw/usb_client_common.c
@@ -26,10 +26,7 @@
 #include <hw/shared.h>
 #include <hw/usb_client.h>
 
-#define zalloc(amount) calloc(1, amount)
-
 #define MAX_GADGET_STR_LEN 256
-#define MAX_FUNCS 32
 
 #define LEGACY_ROOTPATH        "/sys/class/usb_mode/usb0"
 
@@ -44,11 +41,11 @@
 /* Strings */
 #define LEGACY_IMANUFACTURER_PATH   LEGACY_ROOTPATH"/iManufacturer"
 #define LEGACY_IPRODUCT_PATH        LEGACY_ROOTPATH"/iProduct"
-#define LEGACY_ISERIAL_PATH         LEGACY_ROOTPATH"/iSerial"
 
 /* Functions in each config */
 #define LEGACY_CONFIG_1_PATH        LEGACY_ROOTPATH"/funcs_fconf"
 #define LEGACY_CONFIG_2_PATH        LEGACY_ROOTPATH"/funcs_sconf"
+
 /* should be single char */
 #define LEGACY_FUNC_SEP             ","
 
@@ -57,315 +54,35 @@
 #define LEGACY_ENABLE               "1"
 #define LEGACY_DISABLE              "0"
 
-#define LEGACY_BMATTRIBUTES ((1 << 7) | (1 << 6))
-#define LEGACY_MAX_POWER 500
-
-/* +5 to be always big enough */
-#define INT_BUF_SIZE (sizeof(int)*8 + 5)
-
 #ifndef EXPORT
 #define EXPORT	__attribute__ ((visibility("default")))
 #endif
 
-static void legacy_free_gadget(struct usb_gadget *gadget);
-
-static int get_int_from_file(char *path, int *_val, int base)
-{
-	char buf[INT_BUF_SIZE];
-	char *endptr;
-	long int val;
-	int ret;
-
-	ret = sys_get_str(path, buf, sizeof(buf));
-	if (ret)
-		return ret;
-
-	val = strtol(buf, &endptr, base);
-	if (val == LONG_MIN || val == LONG_MAX ||
-	    buf[0] == '\0' || (*endptr != '\0' && *endptr != '\n')
-	    || val >= INT_MAX)
-		return -EINVAL;
-
-	*_val = (int)val;
-	return 0;
-}
-
-static int legacy_read_gadget_attrs_strs(struct usb_gadget *gadget)
-{
-	int val;
-	int ret;
-	/* We assume that values received from kernel will be valid */
-#define GET_VALUE_FROM_SYSFS(path, field, type, base)		\
-	do {							\
-		ret = get_int_from_file(path, &val, base);	\
-		if (ret)					\
-			return ret;				\
-								\
-		gadget->attrs.field = (type)val;		\
-	} while (0)
-
-	GET_VALUE_FROM_SYSFS(LEGACY_CLASS_PATH, bDeviceClass, uint8_t, 0);
-	GET_VALUE_FROM_SYSFS(LEGACY_SUBCLASS_PATH, bDeviceSubClass, uint8_t, 0);
-	GET_VALUE_FROM_SYSFS(LEGACY_PROTOCOL_PATH, bDeviceProtocol, uint8_t, 0);
-	GET_VALUE_FROM_SYSFS(LEGACY_ID_VENDOR_PATH, idVendor, uint16_t, 16);
-	GET_VALUE_FROM_SYSFS(LEGACY_ID_PRODUCT_PATH, idProduct, uint16_t, 16);
-	GET_VALUE_FROM_SYSFS(LEGACY_BCD_DEVICE_PATH, bcdDevice, uint16_t, 0);
-#undef GET_VALUE_FROM_SYSFS
-
-#define GET_STRING_FROM_SYSFS(path, field)			\
-	do {							\
-		char buf[MAX_GADGET_STR_LEN];			\
-								\
-		ret = sys_get_str(path, buf, sizeof(buf));	\
-		if (ret)					\
-			goto err_##field;			\
-								\
-		gadget->strs[0].field = strdup(buf);		\
-		if (!gadget->strs[0].field) {			\
-			ret = -ENOMEM;				\
-			goto err_##field;			\
-		}						\
-	} while (0)
-
-	GET_STRING_FROM_SYSFS(LEGACY_IMANUFACTURER_PATH, manufacturer);
-	GET_STRING_FROM_SYSFS(LEGACY_IPRODUCT_PATH, product);
-	GET_STRING_FROM_SYSFS(LEGACY_ISERIAL_PATH, serial);
-#undef GET_STRING_FROM_SYSFS
-
-	return 0;
-
-err_serial:
-	free(gadget->strs[0].product);
-err_product:
-	free(gadget->strs[0].manufacturer);
-err_manufacturer:
-	return ret;
-}
-
-static int legacy_find_func(const char *name)
+static struct usb_function *legacy_find_func(const char *name)
 {
 	int i;
 
 	for (i = 0; _available_funcs[i]; ++i)
 		if (!strcmp(name, _available_funcs[i]->name))
-			return i;
-
-	return -1;
-}
+			return _available_funcs[i];
 
-static struct usb_function *legacy_find_func_in_gadget(
-	struct usb_gadget *gadget, const char *name)
-{
-	int i;
-
-	for (i = 0; gadget->funcs[i]; ++i)
-		if (!strcmp(name, gadget->funcs[i]->name))
-			return gadget->funcs[i];
 	return NULL;
 }
 
-static int legacy_alloc_config(int n_funcs, struct usb_configuration **_config)
-{
-	struct usb_configuration *config;
-
-	config = zalloc(sizeof(*config));
-	if (!config)
-		goto out;
-
-	config->strs = calloc(1, sizeof(*config->strs));
-	if (!config->strs)
-		goto free_config;
-
-	config->funcs = calloc(n_funcs + 1, sizeof(*config->funcs));
-	if (!config->funcs)
-		goto free_strs;
-
-	/*
-	 * We cannot read correct values
-	 * so assume that they are always default
-	 */
-	config->attrs.bmAttributs = LEGACY_BMATTRIBUTES;
-	config->attrs.MaxPower = LEGACY_MAX_POWER;
-
-	*_config = config;
-
-	return 0;
-free_strs:
-	free(config->strs);
-free_config:
-	free(config);
-out:
-	return -ENOMEM;
-}
-
-static int legacy_alloc_new_func(struct usb_gadget *gadget, const char *fname,
-				 struct usb_function **_func)
-{
-	struct usb_function *func;
-	int ret;
-
-	ret = legacy_find_func(fname);
-	if (ret < 0)
-		return -ENOTSUP;
-
-	ret = _available_funcs[ret]->clone(_available_funcs[ret], &func);
-	if (ret)
-		return ret;
-
-	*_func = func;
-	return 0;
-}
-
-static int legacy_read_config(struct usb_gadget *gadget,
-			      char *cpath,
-			      struct usb_configuration **_config)
-{
-	struct usb_configuration *config;
-	char buf[MAX_GADGET_STR_LEN];
-	char *begin = buf;
-	char *fname;
-	char *sep = LEGACY_FUNC_SEP;
-	int i, f_cnt;
-	int f_idx;
-	int g_f_idx;
-	int ret;
-
-	ret = sys_get_str(cpath, buf, sizeof(buf));
-	if (ret)
-		return ret;
-
-	/* Empty */
-	if (buf[0] == '\0' || buf[0] == '\n')
-		return 0;
-
-	/* count number of functions in this config */
-	f_cnt = 1;
-	for (i = 0; buf[i] != '\0'; ++i) {
-		if (buf[i] == sep[0])
-			++f_cnt;
-		if (buf[i] == '\n')  /* buf ends with it */
-			buf[i] = 0;
-	}
-
-	ret = legacy_alloc_config(f_cnt, &config);
-	if (ret)
-		return ret;
-
-	for (g_f_idx = 0; gadget->funcs[g_f_idx]; ++g_f_idx);
-
-	f_idx = 0;
-	for (fname = strsep(&begin, sep); fname; fname = strsep(&begin, sep)) {
-		struct usb_function *func;
-
-		func = legacy_find_func_in_gadget(gadget, fname);
-		if (!func) {
-			/* new function not added yet to gadget */
-			ret = legacy_alloc_new_func(gadget, fname, &func);
-			if (ret)
-				goto free_config;
-
-			gadget->funcs[g_f_idx++] = func;
-		}
-
-		config->funcs[f_idx++] = func;
-	}
-
-	*_config = config;
-	return 0;
-free_config:
-	free(config->strs);
-	free(config->funcs);
-	free(config);
-	return ret;
-}
-
-static int legacy_get_current_gadget(struct usb_client *usb,
-				     struct usb_gadget **_gadget)
-{
-	struct usb_gadget *gadget;
-	struct usb_gadget_strings *strs;
-	struct usb_configuration **configs;
-	struct usb_function **funcs;
-	int i;
-	int ret = -ENOMEM;
-
-	gadget = zalloc(sizeof(*gadget));
-	if (!gadget)
-		goto out;
-
-	strs = calloc(2, sizeof(*strs));
-	if (!strs)
-		goto free_gadget;
-
-	strs[0].lang_code = 0x409;
-
-	gadget->strs = strs;
-
-	ret = legacy_read_gadget_attrs_strs(gadget);
-	if (ret)
-		goto free_strs;
-
-	/* There will be no more functions than bits in int */
-	funcs = calloc(MAX_FUNCS, sizeof(*funcs));
-	if (!funcs)
-		goto free_strs_with_content;
-
-	gadget->funcs = funcs;
-
-	/* slp-gadget use max 2 confiuration and NULL termination */
-	configs = calloc(3, sizeof(*configs));
-	if (!configs)
-		goto free_funcs;
-
-	gadget->configs = configs;
-
-	ret = legacy_read_config(gadget, LEGACY_CONFIG_1_PATH, configs + 0);
-	if (ret)
-		goto free_configs;
-
-	ret = legacy_read_config(gadget, LEGACY_CONFIG_2_PATH, configs + 1);
-	if (ret)
-		goto free_config_1;
-
-	*_gadget = gadget;
-	return 0;
-
-free_config_1:
-	free(configs[0]->funcs);
-	free(configs[0]->strs);
-	free(configs[0]);
-free_configs:
-	free(configs);
-	for (i = 0; gadget->funcs[i]; ++i)
-		gadget->funcs[i]->free_func(gadget->funcs[i]);
-free_funcs:
-	free(funcs);
-free_strs_with_content:
-	free(gadget->strs[0].manufacturer);
-	free(gadget->strs[0].product);
-	free(gadget->strs[0].serial);
-free_strs:
-	free(gadget->strs);
-free_gadget:
-	free(gadget);
-out:
-	return ret;
-}
 
 static bool legacy_is_function_supported(struct usb_client *usb,
 					 struct usb_function *func)
 {
-	int ret;
-
 	/*
 	 * TODO
 	 * Instead of only checking whether we know this function
 	 * we should also parse sysfs to check if it is build into
 	 * slp-gadget.
 	 */
-	ret = legacy_find_func(func->name);
-
-	return ret >= 0;
+	if (legacy_find_func(func->name))
+		return true;
+	else
+		return false;
 }
 
 static bool legacy_is_gadget_supported(struct usb_client *usb,
@@ -536,121 +253,119 @@ static int legacy_reconfigure_gadget(struct usb_client *usb,
 	return ret;
 }
 
-static int legacy_enable(struct usb_client *usb)
+static void legacy_start_stop_service_and_handler(bool start)
 {
 	int i;
 	int ret;
-	struct usb_gadget *gadget;
+	int n_func = 0;
+	char *ptr;
+	char *begin;
+	char *fname;
+	char *sep = LEGACY_FUNC_SEP;
+	char buf[MAX_GADGET_STR_LEN];
 	struct usb_function *func;
+	struct usb_function *funcs[USB_FUNCTION_IDX_MAX];
+
+	/* SLP gadget uses two USB configuration.
+	 * (/sys/class/usb_mode/usb0/funcs_fconf and /sys/class/usb_mode/usb0/funcs_sconf)
+	 *
+	 * One usb function can be included in two configurations simultaneously.
+	 * In this situation, a handler associated with function can be called twice for a usb function.
+	 * To prevent duplicate calls,
+	 * Collect all functions and remove duplicates and process them.
+	 */
 
-	ret = sys_set_str(LEGACY_ENABLE_PATH, LEGACY_ENABLE);
-	if (ret < 0)
-		return ret;
+	/* First configuration */
+	ret = sys_get_str(LEGACY_CONFIG_1_PATH, buf, sizeof(buf));
+	if (ret)
+		goto second_configuration;
 
-	ret = legacy_get_current_gadget(usb, &gadget);
-	if (ret < 0)
-		goto disable_gadget;
+	/* Caution: buf ends with '\n' */
+	ptr = strchr(buf, '\n');
+	if (ptr)
+		*ptr = 0;
+
+	begin = buf;
+	for (fname = strsep(&begin, sep); fname; fname = strsep(&begin, sep)) {
+		func = legacy_find_func(fname);
+		if (!func)
+			continue;
 
-	for (i = 0; gadget->funcs[i]; ++i) {
-		func = gadget->funcs[i];
+		for (i = 0; i < n_func; i++)
+			if (funcs[i] == func)
+				continue;
 
-		if (func->handler)
-			func->handler(1);
+		if(n_func >= USB_FUNCTION_IDX_MAX) /* What happen */
+			break;
 
-		if (func->service)
-			(void)systemd_start_unit_wait_started(func->service, ".service", -1);
+		funcs[n_func] = func;
+		n_func++;
 	}
 
-	legacy_free_gadget(gadget);
-	return 0;
-
-disable_gadget:
-	sys_set_str(LEGACY_ENABLE_PATH, LEGACY_DISABLE);
-	return ret;
-}
+	/* Second configuration */
+second_configuration:
+	ret = sys_get_str(LEGACY_CONFIG_2_PATH, buf, sizeof(buf));
+	if (ret)
+		return;
 
-static int legacy_disable(struct usb_client *usb)
-{
-	int i;
-	int ret;
-	struct usb_gadget *gadget;
-	struct usb_function *func;
+	/* Caution: buf ends with '\n' */
+	ptr = strchr(buf, '\n');
+	if (ptr)
+		*ptr = 0;
 
-	ret = legacy_get_current_gadget(usb, &gadget);
-	if (ret < 0)
-		return ret;
+	begin = buf;
+	for (fname = strsep(&begin, sep); fname; fname = strsep(&begin, sep)) {
+		func = legacy_find_func(fname);
+		if (!func)
+			continue;
 
-	for (i = 0; gadget->funcs[i]; ++i) {
-		func = gadget->funcs[i];
+		for (i = 0; i < n_func; i++)
+			if (funcs[i] == func)
+				continue;
 
-		if (func->service)
-			(void)systemd_stop_unit_wait_stopped(func->service, ".service", -1);
+		if(n_func >= USB_FUNCTION_IDX_MAX) /* What happen */
+			break;
 
-		if (func->handler)
-			func->handler(0);
+		funcs[n_func] = func;
+		n_func++;
 	}
 
-	ret = sys_set_str(LEGACY_ENABLE_PATH, LEGACY_DISABLE);
+	for (i = 0; i < n_func; i++) {
+		if (start) {
+			if (funcs[i]->handler)
+				funcs[i]->handler(1);
 
-	legacy_free_gadget(gadget);
-	return ret;
-}
-
-static void legacy_free_config(struct usb_configuration *config)
-{
-	int i;
+			if (funcs[i]->service)
+				(void)systemd_start_unit_wait_started(funcs[i]->service, ".service", -1);
+		} else {
+			if (funcs[i]->service)
+				(void)systemd_stop_unit_wait_stopped(funcs[i]->service, ".service", -1);
 
-	if (!config)
-		return;
-
-	if (config->strs) {
-		for (i = 0; config->strs[i].lang_code; ++i)
-			free(config->strs[i].config_str);
-
-		free(config->strs);
+			if (funcs[i]->handler)
+				funcs[i]->handler(0);
+		}
 	}
-
-	/*
-	 * Each function will be free later,
-	 * for now we cleanup only pointers.
-	 */
-	if (config->funcs)
-		free(config->funcs);
-
-	free(config);
 }
 
-static void legacy_free_gadget(struct usb_gadget *gadget)
+static int legacy_enable(struct usb_client *usb)
 {
-	int i;
-
-	if (!gadget)
-		return;
+	int ret;
 
-	if (gadget->strs) {
-		for (i = 0; gadget->strs[i].lang_code; ++i) {
-			free(gadget->strs[i].manufacturer);
-			free(gadget->strs[i].product);
-			free(gadget->strs[i].serial);
-		}
-		free(gadget->strs);
-	}
+	ret = sys_set_str(LEGACY_ENABLE_PATH, LEGACY_ENABLE);
+	if (ret < 0)
+		return ret;
 
-	if (gadget->configs) {
-		for (i = 0; gadget->configs[i]; ++i)
-			legacy_free_config(gadget->configs[i]);
+	legacy_start_stop_service_and_handler(true);
 
-		free(gadget->configs);
-	}
+	return 0;
+}
 
-	if (gadget->funcs) {
-		for (i = 0; gadget->funcs[i]; ++i)
-			gadget->funcs[i]->free_func(gadget->funcs[i]);
+static int legacy_disable(struct usb_client *usb)
+{
 
-		free(gadget->funcs);
-	}
+	legacy_start_stop_service_and_handler(false);
 
-	free(gadget);
+	return sys_set_str(LEGACY_ENABLE_PATH, LEGACY_DISABLE);
 }
 
 EXPORT
@@ -666,7 +381,7 @@ int hw_legacy_gadget_open(struct hw_info *info,
 	if (access("/sys/class/usb_mode/usb0/enable", F_OK))
 		return -ENOENT;
 
-	legacy = zalloc(sizeof(*legacy));
+	legacy = calloc(1, sizeof(*legacy));
 	if (!legacy)
 		return -ENOMEM;