libusbg: Add support for mass storage function

Signed-off-by: Krzysztof Opasiak <k.opasiak@samsung.com>
Reviewed-by: Pawel Szewczyk <p.szewczyk@samsung.com>

2 files changed, 428 insertions, 0 deletions

diff --git a/include/usbg/usbg.h b/include/usbg/usbg.h
index c15673d..39585fc 100644
--- a/include/usbg/usbg.h
+++ b/include/usbg/usbg.h
@@ -22,6 +22,7 @@
 #include <netinet/ether.h>
 #include <stdint.h>
 #include <limits.h>
+#include <stdbool.h>
 #include <stdio.h> /* For FILE * */
 
 #ifdef __cplusplus
@@ -180,6 +181,7 @@ typedef enum
 	F_RNDIS,
 	F_PHONET,
 	F_FFS,
+	F_MASS_STORAGE,
 	USBG_FUNCTION_TYPE_MAX,
 } usbg_function_type;
 
@@ -221,6 +223,29 @@ typedef struct {
 } usbg_f_ffs_attrs;
 
 /**
+ * @typedef usbg_f_ms_attrs
+ * @brief Attributes for mass storage functions
+ */
+typedef struct usbg_f_ms_lun_attrs {
+	int id;
+	bool cdrom;
+	bool ro;
+	bool nofua;
+	bool removable;
+	char *filename;
+} usbg_f_ms_lun_attrs;
+
+/**
+ * @typedef usbg_f_ms_attrs
+ * @brief Attributes for mass storage functions
+ */
+typedef struct {
+	bool stall;
+	int nluns;
+	usbg_f_ms_lun_attrs **luns;
+} usbg_f_ms_attrs;
+
+/**
  * @typedef attrs
  * @brief Attributes for a given function type
  */
@@ -229,6 +254,7 @@ typedef union {
 	usbg_f_net_attrs net;
 	usbg_f_phonet_attrs phonet;
 	usbg_f_ffs_attrs ffs;
+	usbg_f_ms_attrs ms;
 } usbg_f_attrs;
 
 typedef enum {
@@ -236,6 +262,7 @@ typedef enum {
 	USBG_F_ATTRS_NET,
 	USBG_F_ATTRS_PHONET,
 	USBG_F_ATTRS_FFS,
+	USBG_F_ATTRS_MS,
 } usbg_f_attrs_type;
 
 typedef struct {
diff --git a/src/usbg.c b/src/usbg.c
index 9182acb..d8b00c9 100644
--- a/src/usbg.c
+++ b/src/usbg.c
@@ -50,6 +50,7 @@ const char *function_names[] =
 	"rndis",
 	"phonet",
 	"ffs",
+	"mass_storage",
 };
 
 ARRAY_SIZE_SENTINEL(function_names, USBG_FUNCTION_TYPE_MAX);
@@ -248,6 +249,9 @@ int usbg_lookup_function_attrs_type(int f_type)
 	case F_FFS:
 		ret = USBG_F_ATTRS_PHONET;
 		break;
+	case F_MASS_STORAGE:
+		ret = USBG_F_ATTRS_MS;
+		break;
 	default:
 		ret = USBG_ERROR_NOT_SUPPORTED;
 	}
@@ -695,6 +699,8 @@ out:
 	return c;
 }
 
+static int usbg_rm_ms_function(usbg_function *f, int opts);
+
 static usbg_function *usbg_allocate_function(const char *path,
 		usbg_function_type type, const char *instance, usbg_gadget *parent)
 {
@@ -727,6 +733,9 @@ static usbg_function *usbg_allocate_function(const char *path,
 
 	/* only composed funcitons (with subdirs) require this callback */
 	switch (usbg_lookup_function_attrs_type(type)) {
+	case USBG_F_ATTRS_MS:
+		f->rm_callback = usbg_rm_ms_function;
+		break;
 	default:
 		f->rm_callback = NULL;
 		break;
@@ -885,6 +894,154 @@ out:
 	return ret;
 }
 
+static int usbg_parse_function_ms_lun_attrs(const char *path, const char *lun,
+					    usbg_f_ms_lun_attrs *lun_attrs)
+{
+	int ret;
+
+	memset(lun_attrs, 0, sizeof(*lun_attrs));
+
+	ret = sscanf(lun, "lun.%d", &lun_attrs->id);
+	if (ret != 1)
+		goto out;
+
+	ret = usbg_read_bool(path, lun, "cdrom", &(lun_attrs->cdrom));
+	if (ret != USBG_SUCCESS)
+		goto out;
+
+	ret = usbg_read_bool(path, lun, "ro", &(lun_attrs->ro));
+	if (ret != USBG_SUCCESS)
+		goto out;
+
+	ret = usbg_read_bool(path, lun, "nofua", &(lun_attrs->nofua));
+	if (ret != USBG_SUCCESS)
+		goto out;
+
+	ret = usbg_read_bool(path, lun, "removable", &(lun_attrs->removable));
+	if (ret != USBG_SUCCESS)
+		goto out;
+
+	ret = usbg_read_string_alloc(path, lun, "file",
+				     &(lun_attrs->filename));
+
+out:
+	return ret;
+}
+
+static inline int lun_select(const struct dirent *dent)
+{
+	int ret;
+	int id;
+
+	ret = file_select(dent);
+	if (!ret)
+		goto out;
+
+	ret = sscanf(dent->d_name, "lun.%d", &id);
+out:
+	return ret;
+}
+
+static inline int lun_sort(const struct dirent **d1, const struct dirent **d2)
+{
+	int ret;
+	int id1, id2;
+
+	ret = sscanf((*d1)->d_name, "lun.%d", &id1);
+	if (ret != 1)
+		goto err;
+
+	ret = sscanf((*d2)->d_name, "lun.%d", &id2);
+	if (ret != 1)
+		goto err;
+
+	if (id1 < id2)
+		ret = 1;
+
+	return id1 < id2 ? -1 : id1 > id2;
+err:
+	/*
+	 * This should not happened because dentries has been
+	 * already checked by lun_select function. This
+	 * error procedure is just in case.
+	 */
+	return -1;
+}
+
+static int usbg_parse_function_ms_attrs(usbg_function *f,
+		usbg_f_ms_attrs *f_ms_attrs)
+{
+	int ret;
+	int nmb;
+	int i = 0;
+	char fpath[USBG_MAX_PATH_LENGTH];
+	usbg_f_ms_lun_attrs *lun_attrs;
+	usbg_f_ms_lun_attrs **luns;
+	struct dirent **dent;
+
+	ret = usbg_read_bool(f->path, f->name, "stall",
+			     &(f_ms_attrs->stall));
+	if (ret != USBG_SUCCESS)
+		goto out;
+
+
+	nmb = snprintf(fpath, sizeof(fpath), "%s/%s/",
+		       f->path, f->name);
+	if (nmb >= sizeof(fpath)) {
+		ret = USBG_ERROR_PATH_TOO_LONG;
+		goto out;
+	}
+
+	nmb = scandir(fpath, &dent, lun_select, lun_sort);
+	if (nmb < 0) {
+		ret = usbg_translate_error(errno);
+		goto out;
+	}
+
+	luns = calloc(nmb + 1, sizeof(*luns));
+	if (!luns) {
+		ret = USBG_ERROR_NO_MEM;
+		goto err;
+	}
+
+	f_ms_attrs->luns = luns;
+	f_ms_attrs->nluns = nmb;
+
+	for (i = 0; i < nmb; i++) {
+		lun_attrs = malloc(sizeof(*lun_attrs));
+		if (!lun_attrs) {
+			ret = USBG_ERROR_NO_MEM;
+			goto err;
+		}
+
+		ret = usbg_parse_function_ms_lun_attrs(fpath, dent[i]->d_name,
+						       lun_attrs);
+		if (ret != USBG_SUCCESS) {
+			free(lun_attrs);
+			goto err;
+		}
+
+		luns[i] = lun_attrs;
+		free(dent[i]);
+	}
+	free(dent);
+
+	return USBG_SUCCESS;
+
+err:
+	while (i < nmb) {
+		free(dent[i]);
+		++i;
+	}
+	free(dent);
+
+	usbg_cleanup_function_attrs(
+		container_of((usbg_f_attrs *)f_ms_attrs,
+			     usbg_function_attrs, attrs));
+out:
+	return ret;
+}
+
 static int usbg_parse_function_attrs(usbg_function *f,
 		usbg_function_attrs *f_attrs)
 {
@@ -927,6 +1084,11 @@ static int usbg_parse_function_attrs(usbg_function *f,
 			ret = USBG_SUCCESS;
 		break;
 	}
+	case USBG_F_ATTRS_MS:
+		f_attrs->header.attrs_type = USBG_F_ATTRS_MS;
+		ret = usbg_parse_function_ms_attrs(f, &(f_attrs->attrs.ms));
+		break;
+
 	default:
 		ERROR("Unsupported function type\n");
 		ret = USBG_ERROR_NOT_SUPPORTED;
@@ -1616,6 +1778,45 @@ out:
 	return ret;
 }
 
+static int usbg_rm_ms_function(usbg_function *f, int opts)
+{
+	int ret;
+	int nmb;
+	int i;
+	char lpath[USBG_MAX_PATH_LENGTH];
+	struct dirent **dent;
+
+	ret = snprintf(lpath, sizeof(lpath), "%s/%s/", f->path, f->name);
+	if (ret >= sizeof(lpath)) {
+		ret = USBG_ERROR_PATH_TOO_LONG;
+		goto out;
+	}
+
+	nmb = scandir(lpath, &dent, lun_select, lun_sort);
+	if (nmb < 0) {
+		ret = usbg_translate_error(errno);
+		goto out;
+	}
+
+	for (i = nmb - 1; i > 0; --i) {
+		ret = usbg_rm_dir(lpath, dent[i]->d_name);
+		free(dent[i]);
+		if (ret)
+			goto err_free_dent_loop;
+	}
+	free(dent[0]);
+	free(dent);
+
+	return USBG_SUCCESS;
+
+err_free_dent_loop:
+	while (--i >= 0)
+		free(dent[i]);
+	free(dent[i]);
+out:
+	return ret;
+}
+
 int usbg_rm_function(usbg_function *f, int opts)
 {
 	int ret = USBG_ERROR_INVALID_PARAM;
@@ -2652,6 +2853,15 @@ int usbg_get_function_attrs(usbg_function *f, usbg_function_attrs *f_attrs)
 			: USBG_ERROR_INVALID_PARAM;
 }
 
+static void usbg_cleanup_function_ms_lun_attrs(usbg_f_ms_lun_attrs *lun_attrs)
+{
+	if (!lun_attrs)
+		return;
+
+	free(lun_attrs->filename);
+	lun_attrs->id = -1;
+}
+
 void usbg_cleanup_function_attrs(usbg_function_attrs *f_attrs)
 {
 	usbg_f_attrs *attrs;
@@ -2679,6 +2889,27 @@ void usbg_cleanup_function_attrs(usbg_function_attrs *f_attrs)
 		free(attrs->ffs.dev_name);
 		attrs->ffs.dev_name = NULL;
 		break;
+	case USBG_F_ATTRS_MS:
+	{
+		int i;
+		usbg_f_ms_attrs *ms_attrs = &attrs->ms;
+
+		if (!ms_attrs->luns)
+			goto ms_break;
+
+		for (i = 0; i < ms_attrs->nluns; ++i) {
+			if (!ms_attrs->luns[i])
+				continue;
+
+			usbg_cleanup_function_ms_lun_attrs(ms_attrs->luns[i]);
+			free(ms_attrs->luns[i]);
+		}
+		free(ms_attrs->luns);
+		ms_attrs->luns = NULL;
+		ms_attrs->nluns = -1;
+	ms_break:
+		break;
+	}
 	default:
 		ERROR("Unsupported attrs type\n");
 		break;
@@ -2713,6 +2944,173 @@ out:
 	return ret;
 }
 
+static int usbg_set_f_ms_lun_attrs(const char *path, const char *lun,
+				   usbg_f_ms_lun_attrs *lun_attrs)
+{
+	int ret;
+
+	ret = usbg_write_bool(path, lun, "cdrom", lun_attrs->cdrom);
+	if (ret != USBG_SUCCESS)
+		goto out;
+
+	ret = usbg_write_bool(path, lun, "ro", lun_attrs->ro);
+	if (ret != USBG_SUCCESS)
+		goto out;
+
+	ret = usbg_write_bool(path, lun, "nofua", lun_attrs->nofua);
+	if (ret != USBG_SUCCESS)
+		goto out;
+
+	ret = usbg_write_bool(path, lun, "removable", lun_attrs->removable);
+	if (ret != USBG_SUCCESS)
+		goto out;
+
+	ret = usbg_write_string(path, lun, "file",
+				      lun_attrs->filename);
+
+out:
+	return ret;
+}
+
+static int usbg_set_function_ms_attrs(usbg_function *f,
+				      const usbg_f_ms_attrs *f_attrs)
+{
+	int ret;
+	int i, nmb;
+	int space_left;
+	char *new_lun_mask;
+	char lpath[USBG_MAX_PATH_LENGTH];
+	char *lpath_end;
+	DIR *dir;
+	struct dirent **dent;
+
+	ret = usbg_write_bool(f->path, f->name, "stall", f_attrs->stall);
+	if (ret != USBG_SUCCESS)
+		goto out;
+
+	/* lun0 cannot be removed */
+	if (!f_attrs->luns || f_attrs->nluns <= 0)
+		goto out;
+
+	ret = snprintf(lpath, sizeof(lpath), "%s/%s/", f->path, f->name);
+	if (ret >= sizeof(lpath)) {
+		ret = USBG_ERROR_PATH_TOO_LONG;
+		goto out;
+	}
+
+	lpath_end = lpath + strlen(lpath);
+	space_left = sizeof(lpath) - (lpath_end - lpath);
+
+	new_lun_mask = calloc(f_attrs->nluns, sizeof (char));
+	if (!new_lun_mask) {
+		ret = USBG_ERROR_NO_MEM;
+		goto out;
+	}
+
+	for (i = 0; i < f_attrs->nluns; ++i) {
+		usbg_f_ms_lun_attrs *lun = f_attrs->luns[i];
+
+		/*
+		 * id may be left unset in lun attrs but
+		 * if it is set it has to be equal to position
+		 * in lun array
+		 */
+		if (lun && lun->id >= 0 && lun->id != i) {
+			ret = USBG_ERROR_INVALID_PARAM;
+			goto err_lun_loop;
+		}
+
+		ret = snprintf(lpath_end, space_left, "/lun.%d/", i);
+		if (ret >= space_left) {
+			ret = USBG_ERROR_PATH_TOO_LONG;
+			goto err_lun_loop;
+		}
+
+		/*
+		 * Check if dir exist and create it if needed
+		 */
+		dir = opendir(lpath);
+		if (dir) {
+			closedir(dir);
+		} else if (errno != ENOENT) {
+			ret = usbg_translate_error(errno);
+			goto err_lun_loop;
+		} else {
+			ret = mkdir(lpath, S_IRWXU|S_IRWXG|S_IRWXO);
+			if (!ret) {
+				/*
+				 * If we have created a new directory in
+				 * this funciton let's mark it so we can
+				 * cleanup in case of error
+				 */
+				new_lun_mask[i] = 1;
+			} else {
+				ret = usbg_translate_error(errno);
+				goto err_lun_loop;
+			}
+		}
+
+		/* if attributes has not been provided just go to next one */
+		if (!lun)
+			continue;
+
+		ret = usbg_set_f_ms_lun_attrs(lpath, "", lun);
+		if (ret != USBG_SUCCESS)
+			goto err_lun_loop;
+	}
+
+	/* Check if function has more luns and remove them */
+	*lpath_end = '\0';
+	i = 0;
+	nmb = scandir(lpath, &dent, lun_select, lun_sort);
+	if (nmb < 0) {
+		ret = usbg_translate_error(errno);
+		goto err_lun_loop;
+	}
+
+	for (i = 0; i < f_attrs->nluns; ++i)
+		free(dent[i]);
+
+	for (; i < nmb; ++i) {
+		ret = usbg_rm_dir(lpath, dent[i]->d_name);
+		free(dent[i]);
+		/* There is no good way to recover form this */
+		if (ret != USBG_SUCCESS)
+			goto err_rm_loop;
+	}
+	free(dent);
+
+	return USBG_SUCCESS;
+
+err_rm_loop:
+	while (++i < nmb)
+		free(dent[i]);
+	free(dent);
+
+	i = f_attrs->nluns;
+err_lun_loop:
+	/* array is null terminated so we may access lun[nluns] */
+	for (; i >= 0; --i) {
+		if (!new_lun_mask[i])
+			continue;
+
+		ret = snprintf(lpath_end, space_left, "/lun.%d/", i);
+		if (ret >= space_left) {
+			/*
+			 * This should not happen because if we were
+			 * able to create this directory we should be
+			 * also able to remove it.
+			 */
+			continue;
+		}
+		rmdir(lpath);
+	}
+	free(new_lun_mask);
+
+out:
+	return ret;
+}
+
 int usbg_set_function_attrs(usbg_function *f,
 			    const usbg_function_attrs *f_attrs)
 {
@@ -2755,6 +3153,9 @@ int usbg_set_function_attrs(usbg_function *f,
 		ret = f_attrs->attrs.ffs.dev_name && f_attrs->attrs.ffs.dev_name[0] ?
 			USBG_ERROR_INVALID_PARAM : USBG_SUCCESS;
 		break;
+	case USBG_F_ATTRS_MS:
+		ret = usbg_set_function_ms_attrs(f, &f_attrs->attrs.ms);
+		break;
 	default:
 		ERROR("Unsupported function type\n");
 		ret = USBG_ERROR_NOT_SUPPORTED;