usb: gadget: f_mass_storage: create fsg_common_set_num_buffers for use in fsg_common_init

fsg_common_init is a lengthy function. Factor a portion of it out.

Signed-off-by: Andrzej Pietrasiewicz <andrzej.p@samsung.com>
Signed-off-by: Kyungmin Park <kyungmin.park@samsung.com>
Acked-by: Michal Nazarewicz <mina86@mina86.com>
Signed-off-by: Felipe Balbi <balbi@ti.com>

2 files changed, 48 insertions, 26 deletions

diff --git a/drivers/usb/gadget/f_mass_storage.c b/drivers/usb/gadget/f_mass_storage.c
index da87ffe34514..79d989979360 100644
--- a/drivers/usb/gadget/f_mass_storage.c
+++ b/drivers/usb/gadget/f_mass_storage.c
@@ -2681,6 +2681,49 @@ static void _fsg_common_free_buffers(struct fsg_buffhd *buffhds, unsigned n)
         }
 }
 
+int fsg_common_set_num_buffers(struct fsg_common *common, unsigned int n)
+{
+        struct fsg_buffhd *bh, *buffhds;
+        int i, rc;
+
+        rc = fsg_num_buffers_validate(n);
+        if (rc != 0)
+                return rc;
+
+        buffhds = kcalloc(n, sizeof(*buffhds), GFP_KERNEL);
+        if (!buffhds)
+                return -ENOMEM;
+
+        /* Data buffers cyclic list */
+        bh = buffhds;
+        i = n;
+        goto buffhds_first_it;
+        do {
+                bh->next = bh + 1;
+                ++bh;
+buffhds_first_it:
+                bh->buf = kmalloc(FSG_BUFLEN, GFP_KERNEL);
+                if (unlikely(!bh->buf))
+                        goto error_release;
+        } while (--i);
+        bh->next = buffhds;
+
+        _fsg_common_free_buffers(common->buffhds, common->fsg_num_buffers);
+        common->fsg_num_buffers = n;
+        common->buffhds = buffhds;
+
+        return 0;
+
+error_release:
+        /*
+         * "buf"s pointed to by heads after n - i are NULL
+         * so releasing them won't hurt
+         */
+        _fsg_common_free_buffers(buffhds, n);
+
+        return -ENOMEM;
+}
+
 static inline void fsg_common_remove_sysfs(struct fsg_lun *lun)
 {
         device_remove_file(&lun->dev, &dev_attr_nofua);
@@ -2714,17 +2757,12 @@ struct fsg_common *fsg_common_init(struct fsg_common *common,
                                    struct fsg_config *cfg)
 {
         struct usb_gadget *gadget = cdev->gadget;
-        struct fsg_buffhd *bh;
         struct fsg_lun **curlun_it;
         struct fsg_lun_config *lcfg;
         struct usb_string *us;
         int nluns, i, rc;
         char *pathbuf;
 
-        rc = fsg_num_buffers_validate(cfg->fsg_num_buffers);
-        if (rc != 0)
-                return ERR_PTR(rc);
-
         /* Find out how many LUNs there should be */
         nluns = cfg->nluns;
         if (nluns < 1 || nluns > FSG_MAX_LUNS) {
@@ -2738,15 +2776,12 @@ struct fsg_common *fsg_common_init(struct fsg_common *common,
         fsg_common_set_sysfs(common, true);
         common->state = FSG_STATE_IDLE;
 
-        common->fsg_num_buffers = cfg->fsg_num_buffers;
-        common->buffhds = kcalloc(common->fsg_num_buffers,
-                                  sizeof *(common->buffhds), GFP_KERNEL);
-        if (!common->buffhds) {
+        rc = fsg_common_set_num_buffers(common, cfg->fsg_num_buffers);
+        if (rc) {
                 if (common->free_storage_on_release)
                         kfree(common);
-                return ERR_PTR(-ENOMEM);
+                return ERR_PTR(rc);
         }
-
         common->ops = cfg->ops;
         common->private_data = cfg->private_data;
 
@@ -2833,21 +2868,6 @@ struct fsg_common *fsg_common_init(struct fsg_common *common,
         }
         common->nluns = nluns;
 
-        /* Data buffers cyclic list */
-        bh = common->buffhds;
-        i = common->fsg_num_buffers;
-        goto buffhds_first_it;
-        do {
-                bh->next = bh + 1;
-                ++bh;
-buffhds_first_it:
-                bh->buf = kmalloc(FSG_BUFLEN, GFP_KERNEL);
-                if (unlikely(!bh->buf)) {
-                        rc = -ENOMEM;
-                        goto error_release;
-                }
-        } while (--i);
-        bh->next = common->buffhds;
 
         /* Prepare inquiryString */
         i = get_default_bcdDevice();
diff --git a/drivers/usb/gadget/f_mass_storage.h b/drivers/usb/gadget/f_mass_storage.h
index 1b88eae75283..a00f51a1d94b 100644
--- a/drivers/usb/gadget/f_mass_storage.h
+++ b/drivers/usb/gadget/f_mass_storage.h
@@ -104,6 +104,8 @@ struct fsg_common *fsg_common_init(struct fsg_common *common,
 
 void fsg_common_set_sysfs(struct fsg_common *common, bool sysfs);
 
+int fsg_common_set_num_buffers(struct fsg_common *common, unsigned int n);
+
 void fsg_config_from_params(struct fsg_config *cfg,
                             const struct fsg_module_parameters *params,
                             unsigned int fsg_num_buffers);