1 files changed, 176 insertions, 24 deletions

diff --git a/drivers/usb/gadget/g_ffs.c b/drivers/usb/gadget/g_ffs.c
index a85eaf40b948..d3ace9002a6a 100644
--- a/drivers/usb/gadget/g_ffs.c
+++ b/drivers/usb/gadget/g_ffs.c
@@ -67,6 +67,15 @@ MODULE_LICENSE("GPL");
 #define GFS_VENDOR_ID   0x1d6b  /* Linux Foundation */
 #define GFS_PRODUCT_ID  0x0105  /* FunctionFS Gadget */
 
+#define GFS_MAX_DEVS    10
+
+struct gfs_ffs_obj {
+        const char *name;
+        bool mounted;
+        bool desc_ready;
+        struct ffs_data *ffs_data;
+};
+
 static struct usb_device_descriptor gfs_dev_desc = {
         .bLength                = sizeof gfs_dev_desc,
         .bDescriptorType        = USB_DT_DEVICE,
@@ -78,12 +87,17 @@ static struct usb_device_descriptor gfs_dev_desc = {
         .idProduct              = cpu_to_le16(GFS_PRODUCT_ID),
 };
 
+static char *func_names[GFS_MAX_DEVS];
+static unsigned int func_num;
+
 module_param_named(bDeviceClass,    gfs_dev_desc.bDeviceClass,    byte,   0644);
 MODULE_PARM_DESC(bDeviceClass, "USB Device class");
 module_param_named(bDeviceSubClass, gfs_dev_desc.bDeviceSubClass, byte,   0644);
 MODULE_PARM_DESC(bDeviceSubClass, "USB Device subclass");
 module_param_named(bDeviceProtocol, gfs_dev_desc.bDeviceProtocol, byte,   0644);
 MODULE_PARM_DESC(bDeviceProtocol, "USB Device protocol");
+module_param_array_named(functions, func_names, charp, &func_num, 0);
+MODULE_PARM_DESC(functions, "USB Functions list");
 
 static const struct usb_descriptor_header *gfs_otg_desc[] = {
         (const struct usb_descriptor_header *)
@@ -158,13 +172,34 @@ static struct usb_composite_driver gfs_driver = {
         .iProduct       = DRIVER_DESC,
 };
 
-static struct ffs_data *gfs_ffs_data;
-static unsigned long gfs_registered;
+static DEFINE_MUTEX(gfs_lock);
+static unsigned int missing_funcs;
+static bool gfs_ether_setup;
+static bool gfs_registered;
+static bool gfs_single_func;
+static struct gfs_ffs_obj *ffs_tab;
 
 static int __init gfs_init(void)
 {
+        int i;
+
         ENTER();
 
+        if (!func_num) {
+                gfs_single_func = true;
+                func_num = 1;
+        }
+
+        ffs_tab = kcalloc(func_num, sizeof *ffs_tab, GFP_KERNEL);
+        if (!ffs_tab)
+                return -ENOMEM;
+
+        if (!gfs_single_func)
+                for (i = 0; i < func_num; i++)
+                        ffs_tab[i].name = func_names[i];
+
+        missing_funcs = func_num;
+
         return functionfs_init();
 }
 module_init(gfs_init);
@@ -172,63 +207,165 @@ module_init(gfs_init);
 static void __exit gfs_exit(void)
 {
         ENTER();
+        mutex_lock(&gfs_lock);
 
-        if (test_and_clear_bit(0, &gfs_registered))
+        if (gfs_registered)
                 usb_composite_unregister(&gfs_driver);
+        gfs_registered = false;
 
         functionfs_cleanup();
+
+        mutex_unlock(&gfs_lock);
+        kfree(ffs_tab);
 }
 module_exit(gfs_exit);
 
+static struct gfs_ffs_obj *gfs_find_dev(const char *dev_name)
+{
+        int i;
+
+        ENTER();
+
+        if (gfs_single_func)
+                return &ffs_tab[0];
+
+        for (i = 0; i < func_num; i++)
+                if (strcmp(ffs_tab[i].name, dev_name) == 0)
+                        return &ffs_tab[i];
+
+        return NULL;
+}
+
 static int functionfs_ready_callback(struct ffs_data *ffs)
 {
+        struct gfs_ffs_obj *ffs_obj;
         int ret;
 
         ENTER();
+        mutex_lock(&gfs_lock);
+
+        ffs_obj = ffs->private_data;
+        if (!ffs_obj) {
+                ret = -EINVAL;
+                goto done;
+        }
+
+        if (WARN_ON(ffs_obj->desc_ready)) {
+                ret = -EBUSY;
+                goto done;
+        }
+        ffs_obj->desc_ready = true;
+        ffs_obj->ffs_data = ffs;
 
-        if (WARN_ON(test_and_set_bit(0, &gfs_registered)))
-                return -EBUSY;
+        if (--missing_funcs) {
+                ret = 0;
+                goto done;
+        }
+
+        if (gfs_registered) {
+                ret = -EBUSY;
+                goto done;
+        }
+        gfs_registered = true;
 
-        gfs_ffs_data = ffs;
         ret = usb_composite_probe(&gfs_driver, gfs_bind);
         if (unlikely(ret < 0))
-                clear_bit(0, &gfs_registered);
+                gfs_registered = false;
+
+done:
+        mutex_unlock(&gfs_lock);
         return ret;
 }
 
 static void functionfs_closed_callback(struct ffs_data *ffs)
 {
+        struct gfs_ffs_obj *ffs_obj;
+
         ENTER();
+        mutex_lock(&gfs_lock);
 
-        if (test_and_clear_bit(0, &gfs_registered))
+        ffs_obj = ffs->private_data;
+        if (!ffs_obj)
+                goto done;
+
+        ffs_obj->desc_ready = false;
+        missing_funcs++;
+
+        if (gfs_registered)
                 usb_composite_unregister(&gfs_driver);
+        gfs_registered = false;
+
+done:
+        mutex_unlock(&gfs_lock);
 }
 
-static int functionfs_check_dev_callback(const char *dev_name)
+static void *functionfs_acquire_dev_callback(const char *dev_name)
 {
-        return 0;
+        struct gfs_ffs_obj *ffs_dev;
+
+        ENTER();
+        mutex_lock(&gfs_lock);
+
+        ffs_dev = gfs_find_dev(dev_name);
+        if (!ffs_dev) {
+                ffs_dev = ERR_PTR(-ENODEV);
+                goto done;
+        }
+
+        if (ffs_dev->mounted) {
+                ffs_dev = ERR_PTR(-EBUSY);
+                goto done;
+        }
+        ffs_dev->mounted = true;
+
+done:
+        mutex_unlock(&gfs_lock);
+        return ffs_dev;
 }
 
+static void functionfs_release_dev_callback(struct ffs_data *ffs_data)
+{
+        struct gfs_ffs_obj *ffs_dev;
+
+        ENTER();
+        mutex_lock(&gfs_lock);
+
+        ffs_dev = ffs_data->private_data;
+        if (ffs_dev)
+                ffs_dev->mounted = false;
+
+        mutex_unlock(&gfs_lock);
+}
+
+/*
+ * It is assumed that gfs_bind is called from a context where gfs_lock is held
+ */
 static int gfs_bind(struct usb_composite_dev *cdev)
 {
         int ret, i;
 
         ENTER();
 
-        if (WARN_ON(!gfs_ffs_data))
+        if (missing_funcs)
                 return -ENODEV;
 
         ret = gether_setup(cdev->gadget, gfs_hostaddr);
         if (unlikely(ret < 0))
                 goto error_quick;
+        gfs_ether_setup = true;
 
         ret = usb_string_ids_tab(cdev, gfs_strings);
         if (unlikely(ret < 0))
                 goto error;
 
-        ret = functionfs_bind(gfs_ffs_data, cdev);
-        if (unlikely(ret < 0))
-                goto error;
+        for (i = func_num; --i; ) {
+                ret = functionfs_bind(ffs_tab[i].ffs_data, cdev);
+                if (unlikely(ret < 0)) {
+                        while (++i < func_num)
+                                functionfs_unbind(ffs_tab[i].ffs_data);
+                        goto error;
+                }
+        }
 
         for (i = 0; i < ARRAY_SIZE(gfs_configurations); ++i) {
                 struct gfs_configuration *c = gfs_configurations + i;
@@ -246,16 +383,22 @@ static int gfs_bind(struct usb_composite_dev *cdev)
         return 0;
 
 error_unbind:
-        functionfs_unbind(gfs_ffs_data);
+        for (i = 0; i < func_num; i++)
+                functionfs_unbind(ffs_tab[i].ffs_data);
 error:
         gether_cleanup();
 error_quick:
-        gfs_ffs_data = NULL;
+        gfs_ether_setup = false;
         return ret;
 }
 
+/*
+ * It is assumed that gfs_unbind is called from a context where gfs_lock is held
+ */
 static int gfs_unbind(struct usb_composite_dev *cdev)
 {
+        int i;
+
         ENTER();
 
         /*
@@ -266,22 +409,29 @@ static int gfs_unbind(struct usb_composite_dev *cdev)
          * from composite on orror recovery, but what you're gonna
          * do...?
          */
-        if (gfs_ffs_data) {
+        if (gfs_ether_setup)
                 gether_cleanup();
-                functionfs_unbind(gfs_ffs_data);
-                gfs_ffs_data = NULL;
-        }
+        gfs_ether_setup = false;
+
+        for (i = func_num; --i; )
+                if (ffs_tab[i].ffs_data)
+                        functionfs_unbind(ffs_tab[i].ffs_data);
 
         return 0;
 }
 
+/*
+ * It is assumed that gfs_do_config is called from a context where
+ * gfs_lock is held
+ */
 static int gfs_do_config(struct usb_configuration *c)
 {
         struct gfs_configuration *gc =
                 container_of(c, struct gfs_configuration, c);
+        int i;
         int ret;
 
-        if (WARN_ON(!gfs_ffs_data))
+        if (missing_funcs)
                 return -ENODEV;
 
         if (gadget_is_otg(c->cdev->gadget)) {
@@ -295,9 +445,11 @@ static int gfs_do_config(struct usb_configuration *c)
                         return ret;
         }
 
-        ret = functionfs_bind_config(c->cdev, c, gfs_ffs_data);
-        if (unlikely(ret < 0))
-                return ret;
+        for (i = 0; i < func_num; i++) {
+                ret = functionfs_bind_config(c->cdev, c, ffs_tab[i].ffs_data);
+                if (unlikely(ret < 0))
+                        return ret;
+        }
 
         /*
          * After previous do_configs there may be some invalid