1 files changed, 104 insertions, 34 deletions

diff --git a/drivers/usb/gadget/f_mass_storage.c b/drivers/usb/gadget/f_mass_storage.c
index f4911c09022e..7d05a0be5c60 100644
--- a/drivers/usb/gadget/f_mass_storage.c
+++ b/drivers/usb/gadget/f_mass_storage.c
@@ -163,6 +163,10 @@
  * ro setting are not allowed when the medium is loaded or if CD-ROM
  * emulation is being used.
  *
+ * When a LUN receive an "eject" SCSI request (Start/Stop Unit),
+ * if the LUN is removable, the backing file is released to simulate
+ * ejection.
+ *
  *
  * This function is heavily based on "File-backed Storage Gadget" by
  * Alan Stern which in turn is heavily based on "Gadget Zero" by David
@@ -302,7 +306,6 @@ static const char fsg_string_interface[] = "Mass Storage";
 
 
 #define FSG_NO_INTR_EP 1
-#define FSG_BUFFHD_STATIC_BUFFER 1
 #define FSG_NO_DEVICE_STRINGS    1
 #define FSG_NO_OTG               1
 #define FSG_NO_INTR_EP           1
@@ -1385,12 +1388,50 @@ static int do_mode_sense(struct fsg_common *common, struct fsg_buffhd *bh)
 
 static int do_start_stop(struct fsg_common *common)
 {
-        if (!common->curlun) {
+        struct fsg_lun  *curlun = common->curlun;
+        int             loej, start;
+
+        if (!curlun) {
                 return -EINVAL;
-        } else if (!common->curlun->removable) {
-                common->curlun->sense_data = SS_INVALID_COMMAND;
+        } else if (!curlun->removable) {
+                curlun->sense_data = SS_INVALID_COMMAND;
                 return -EINVAL;
         }
+
+        loej = common->cmnd[4] & 0x02;
+        start = common->cmnd[4] & 0x01;
+
+        /* eject code from file_storage.c:do_start_stop() */
+
+        if ((common->cmnd[1] & ~0x01) != 0 ||     /* Mask away Immed */
+                (common->cmnd[4] & ~0x03) != 0) { /* Mask LoEj, Start */
+                curlun->sense_data = SS_INVALID_FIELD_IN_CDB;
+                return -EINVAL;
+        }
+
+        if (!start) {
+                /* Are we allowed to unload the media? */
+                if (curlun->prevent_medium_removal) {
+                        LDBG(curlun, "unload attempt prevented\n");
+                        curlun->sense_data = SS_MEDIUM_REMOVAL_PREVENTED;
+                        return -EINVAL;
+                }
+                if (loej) {     /* Simulate an unload/eject */
+                        up_read(&common->filesem);
+                        down_write(&common->filesem);
+                        fsg_lun_close(curlun);
+                        up_write(&common->filesem);
+                        down_read(&common->filesem);
+                }
+        } else {
+
+                /* Our emulation doesn't support mounting; the medium is
+                 * available for use as soon as it is loaded. */
+                if (!fsg_lun_is_open(curlun)) {
+                        curlun->sense_data = SS_MEDIUM_NOT_PRESENT;
+                        return -EINVAL;
+                }
+        }
         return 0;
 }
 
@@ -2701,10 +2742,8 @@ static struct fsg_common *fsg_common_init(struct fsg_common *common,
         /* Maybe allocate device-global string IDs, and patch descriptors */
         if (fsg_strings[FSG_STRING_INTERFACE].id == 0) {
                 rc = usb_string_id(cdev);
-                if (rc < 0) {
-                        kfree(common);
-                        return ERR_PTR(rc);
-                }
+                if (unlikely(rc < 0))
+                        goto error_release;
                 fsg_strings[FSG_STRING_INTERFACE].id = rc;
                 fsg_intf_desc.iInterface = rc;
         }
@@ -2712,9 +2751,9 @@ static struct fsg_common *fsg_common_init(struct fsg_common *common,
         /* Create the LUNs, open their backing files, and register the
          * LUN devices in sysfs. */
         curlun = kzalloc(nluns * sizeof *curlun, GFP_KERNEL);
-        if (!curlun) {
-                kfree(common);
-                return ERR_PTR(-ENOMEM);
+        if (unlikely(!curlun)) {
+                rc = -ENOMEM;
+                goto error_release;
         }
         common->luns = curlun;
 
@@ -2762,13 +2801,19 @@ static struct fsg_common *fsg_common_init(struct fsg_common *common,
 
 
         /* Data buffers cyclic list */
-        /* Buffers in buffhds are static -- no need for additional
-         * allocation. */
         bh = common->buffhds;
-        i = FSG_NUM_BUFFERS - 1;
+        i = FSG_NUM_BUFFERS;
+        goto buffhds_first_it;
         do {
                 bh->next = bh + 1;
-        } while (++bh, --i);
+                ++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;
 
 
@@ -2867,10 +2912,7 @@ error_release:
 
 static void fsg_common_release(struct kref *ref)
 {
-        struct fsg_common *common =
-                container_of(ref, struct fsg_common, ref);
-        unsigned i = common->nluns;
-        struct fsg_lun *lun = common->luns;
+        struct fsg_common *common = container_of(ref, struct fsg_common, ref);
 
         /* If the thread isn't already dead, tell it to exit now */
         if (common->state != FSG_STATE_TERMINATED) {
@@ -2881,17 +2923,29 @@ static void fsg_common_release(struct kref *ref)
                 complete(&common->thread_notifier);
         }
 
-        /* Beware tempting for -> do-while optimization: when in error
-         * recovery nluns may be zero. */
+        if (likely(common->luns)) {
+                struct fsg_lun *lun = common->luns;
+                unsigned i = common->nluns;
+
+                /* In error recovery common->nluns may be zero. */
+                for (; i; --i, ++lun) {
+                        device_remove_file(&lun->dev, &dev_attr_ro);
+                        device_remove_file(&lun->dev, &dev_attr_file);
+                        fsg_lun_close(lun);
+                        device_unregister(&lun->dev);
+                }
+
+                kfree(common->luns);
+        }
 
-        for (; i; --i, ++lun) {
-                device_remove_file(&lun->dev, &dev_attr_ro);
-                device_remove_file(&lun->dev, &dev_attr_file);
-                fsg_lun_close(lun);
-                device_unregister(&lun->dev);
+        {
+                struct fsg_buffhd *bh = common->buffhds;
+                unsigned i = FSG_NUM_BUFFERS;
+                do {
+                        kfree(bh->buf);
+                } while (++bh, --i);
         }
 
-        kfree(common->luns);
         if (common->free_storage_on_release)
                 kfree(common);
 }
@@ -2906,11 +2960,13 @@ static void fsg_unbind(struct usb_configuration *c, struct usb_function *f)
 
         DBG(fsg, "unbind\n");
         fsg_common_put(fsg->common);
+        usb_free_descriptors(fsg->function.descriptors);
+        usb_free_descriptors(fsg->function.hs_descriptors);
         kfree(fsg);
 }
 
 
-static int __init fsg_bind(struct usb_configuration *c, struct usb_function *f)
+static int fsg_bind(struct usb_configuration *c, struct usb_function *f)
 {
         struct fsg_dev          *fsg = fsg_from_func(f);
         struct usb_gadget       *gadget = c->cdev->gadget;
@@ -2946,7 +3002,9 @@ static int __init fsg_bind(struct usb_configuration *c, struct usb_function *f)
                         fsg_fs_bulk_in_desc.bEndpointAddress;
                 fsg_hs_bulk_out_desc.bEndpointAddress =
                         fsg_fs_bulk_out_desc.bEndpointAddress;
-                f->hs_descriptors = fsg_hs_function;
+                f->hs_descriptors = usb_copy_descriptors(fsg_hs_function);
+                if (unlikely(!f->hs_descriptors))
+                        return -ENOMEM;
         }
 
         return 0;
@@ -2978,7 +3036,11 @@ static int fsg_add(struct usb_composite_dev *cdev,
 
         fsg->function.name        = FSG_DRIVER_DESC;
         fsg->function.strings     = fsg_strings_array;
-        fsg->function.descriptors = fsg_fs_function;
+        fsg->function.descriptors = usb_copy_descriptors(fsg_fs_function);
+        if (unlikely(!fsg->function.descriptors)) {
+                rc = -ENOMEM;
+                goto error_free_fsg;
+        }
         fsg->function.bind        = fsg_bind;
         fsg->function.unbind      = fsg_unbind;
         fsg->function.setup       = fsg_setup;
@@ -2993,11 +3055,19 @@ static int fsg_add(struct usb_composite_dev *cdev,
          * call to usb_add_function() was successful. */
 
         rc = usb_add_function(c, &fsg->function);
+        if (unlikely(rc))
+                goto error_free_all;
 
-        if (likely(rc == 0))
-                fsg_common_get(fsg->common);
-        else
-                kfree(fsg);
+        fsg_common_get(fsg->common);
+        return 0;
+
+error_free_all:
+        usb_free_descriptors(fsg->function.descriptors);
+        /* fsg_bind() might have copied those; or maybe not? who cares
+         * -- free it just in case. */
+        usb_free_descriptors(fsg->function.hs_descriptors);
+error_free_fsg:
+        kfree(fsg);
 
         return rc;
 }