1 files changed, 73 insertions, 121 deletions

diff --git a/drivers/usb/gadget/f_mass_storage.c b/drivers/usb/gadget/f_mass_storage.c
index 7d05a0be5c60..4ce899c9b165 100644
--- a/drivers/usb/gadget/f_mass_storage.c
+++ b/drivers/usb/gadget/f_mass_storage.c
@@ -321,8 +321,8 @@ struct fsg_dev;
 /* Data shared by all the FSG instances. */
 struct fsg_common {
         struct usb_gadget       *gadget;
-        struct fsg_dev          *fsg;
-        struct fsg_dev          *prev_fsg;
+        struct fsg_dev          *fsg, *new_fsg;
+        wait_queue_head_t       fsg_wait;
 
         /* filesem protects: backing files in use */
         struct rw_semaphore     filesem;
@@ -351,7 +351,6 @@ struct fsg_common {
         enum fsg_state          state;          /* For exception handling */
         unsigned int            exception_req_tag;
 
-        u8                      config, new_config;
         enum data_direction     data_dir;
         u32                     data_size;
         u32                     data_size_from_cmnd;
@@ -595,7 +594,7 @@ static int fsg_setup(struct usb_function *f,
         u16                     w_value = le16_to_cpu(ctrl->wValue);
         u16                     w_length = le16_to_cpu(ctrl->wLength);
 
-        if (!fsg->common->config)
+        if (!fsg_is_set(fsg->common))
                 return -EOPNOTSUPP;
 
         switch (ctrl->bRequest) {
@@ -2303,24 +2302,20 @@ static int alloc_request(struct fsg_common *common, struct usb_ep *ep,
         return -ENOMEM;
 }
 
-/*
- * Reset interface setting and re-init endpoint state (toggle etc).
- * Call with altsetting < 0 to disable the interface.  The only other
- * available altsetting is 0, which enables the interface.
- */
-static int do_set_interface(struct fsg_common *common, int altsetting)
+/* Reset interface setting and re-init endpoint state (toggle etc). */
+static int do_set_interface(struct fsg_common *common, struct fsg_dev *new_fsg)
 {
-        int     rc = 0;
-        int     i;
-        const struct usb_endpoint_descriptor    *d;
+        const struct usb_endpoint_descriptor *d;
+        struct fsg_dev *fsg;
+        int i, rc = 0;
 
         if (common->running)
                 DBG(common, "reset interface\n");
 
 reset:
         /* Deallocate the requests */
-        if (common->prev_fsg) {
-                struct fsg_dev *fsg = common->prev_fsg;
+        if (common->fsg) {
+                fsg = common->fsg;
 
                 for (i = 0; i < FSG_NUM_BUFFERS; ++i) {
                         struct fsg_buffhd *bh = &common->buffhds[i];
@@ -2345,88 +2340,53 @@ reset:
                         fsg->bulk_out_enabled = 0;
                 }
 
-                common->prev_fsg = 0;
+                common->fsg = NULL;
+                wake_up(&common->fsg_wait);
         }
 
         common->running = 0;
-        if (altsetting < 0 || rc != 0)
+        if (!new_fsg || rc)
                 return rc;
 
-        DBG(common, "set interface %d\n", altsetting);
+        common->fsg = new_fsg;
+        fsg = common->fsg;
 
-        if (fsg_is_set(common)) {
-                struct fsg_dev *fsg = common->fsg;
-                common->prev_fsg = common->fsg;
+        /* Enable the endpoints */
+        d = fsg_ep_desc(common->gadget,
+                        &fsg_fs_bulk_in_desc, &fsg_hs_bulk_in_desc);
+        rc = enable_endpoint(common, fsg->bulk_in, d);
+        if (rc)
+                goto reset;
+        fsg->bulk_in_enabled = 1;
+
+        d = fsg_ep_desc(common->gadget,
+                        &fsg_fs_bulk_out_desc, &fsg_hs_bulk_out_desc);
+        rc = enable_endpoint(common, fsg->bulk_out, d);
+        if (rc)
+                goto reset;
+        fsg->bulk_out_enabled = 1;
+        common->bulk_out_maxpacket = le16_to_cpu(d->wMaxPacketSize);
+        clear_bit(IGNORE_BULK_OUT, &fsg->atomic_bitflags);
 
-                /* Enable the endpoints */
-                d = fsg_ep_desc(common->gadget,
-                                &fsg_fs_bulk_in_desc, &fsg_hs_bulk_in_desc);
-                rc = enable_endpoint(common, fsg->bulk_in, d);
+        /* Allocate the requests */
+        for (i = 0; i < FSG_NUM_BUFFERS; ++i) {
+                struct fsg_buffhd       *bh = &common->buffhds[i];
+
+                rc = alloc_request(common, fsg->bulk_in, &bh->inreq);
                 if (rc)
                         goto reset;
-                fsg->bulk_in_enabled = 1;
-
-                d = fsg_ep_desc(common->gadget,
-                                &fsg_fs_bulk_out_desc, &fsg_hs_bulk_out_desc);
-                rc = enable_endpoint(common, fsg->bulk_out, d);
+                rc = alloc_request(common, fsg->bulk_out, &bh->outreq);
                 if (rc)
                         goto reset;
-                fsg->bulk_out_enabled = 1;
-                common->bulk_out_maxpacket = le16_to_cpu(d->wMaxPacketSize);
-                clear_bit(IGNORE_BULK_OUT, &fsg->atomic_bitflags);
-
-                /* Allocate the requests */
-                for (i = 0; i < FSG_NUM_BUFFERS; ++i) {
-                        struct fsg_buffhd       *bh = &common->buffhds[i];
-
-                        rc = alloc_request(common, fsg->bulk_in, &bh->inreq);
-                        if (rc)
-                                goto reset;
-                        rc = alloc_request(common, fsg->bulk_out, &bh->outreq);
-                        if (rc)
-                                goto reset;
-                        bh->inreq->buf = bh->outreq->buf = bh->buf;
-                        bh->inreq->context = bh->outreq->context = bh;
-                        bh->inreq->complete = bulk_in_complete;
-                        bh->outreq->complete = bulk_out_complete;
-                }
-
-                common->running = 1;
-                for (i = 0; i < common->nluns; ++i)
-                        common->luns[i].unit_attention_data = SS_RESET_OCCURRED;
-                return rc;
-        } else {
-                return -EIO;
-        }
-}
-
-
-/*
- * Change our operational configuration.  This code must agree with the code
- * that returns config descriptors, and with interface altsetting code.
- *
- * It's also responsible for power management interactions.  Some
- * configurations might not work with our current power sources.
- * For now we just assume the gadget is always self-powered.
- */
-static int do_set_config(struct fsg_common *common, u8 new_config)
-{
-        int     rc = 0;
-
-        /* Disable the single interface */
-        if (common->config != 0) {
-                DBG(common, "reset config\n");
-                common->config = 0;
-                rc = do_set_interface(common, -1);
+                bh->inreq->buf = bh->outreq->buf = bh->buf;
+                bh->inreq->context = bh->outreq->context = bh;
+                bh->inreq->complete = bulk_in_complete;
+                bh->outreq->complete = bulk_out_complete;
         }
 
-        /* Enable the interface */
-        if (new_config != 0) {
-                common->config = new_config;
-                rc = do_set_interface(common, 0);
-                if (rc != 0)
-                        common->config = 0;     /* Reset on errors */
-        }
+        common->running = 1;
+        for (i = 0; i < common->nluns; ++i)
+                common->luns[i].unit_attention_data = SS_RESET_OCCURRED;
         return rc;
 }
 
@@ -2437,9 +2397,7 @@ static int do_set_config(struct fsg_common *common, u8 new_config)
 static int fsg_set_alt(struct usb_function *f, unsigned intf, unsigned alt)
 {
         struct fsg_dev *fsg = fsg_from_func(f);
-        fsg->common->prev_fsg = fsg->common->fsg;
-        fsg->common->fsg = fsg;
-        fsg->common->new_config = 1;
+        fsg->common->new_fsg = fsg;
         raise_exception(fsg->common, FSG_STATE_CONFIG_CHANGE);
         return 0;
 }
@@ -2447,9 +2405,7 @@ static int fsg_set_alt(struct usb_function *f, unsigned intf, unsigned alt)
 static void fsg_disable(struct usb_function *f)
 {
         struct fsg_dev *fsg = fsg_from_func(f);
-        fsg->common->prev_fsg = fsg->common->fsg;
-        fsg->common->fsg = fsg;
-        fsg->common->new_config = 0;
+        fsg->common->new_fsg = NULL;
         raise_exception(fsg->common, FSG_STATE_CONFIG_CHANGE);
 }
 
@@ -2459,19 +2415,17 @@ static void fsg_disable(struct usb_function *f)
 static void handle_exception(struct fsg_common *common)
 {
         siginfo_t               info;
-        int                     sig;
         int                     i;
         struct fsg_buffhd       *bh;
         enum fsg_state          old_state;
-        u8                      new_config;
         struct fsg_lun          *curlun;
         unsigned int            exception_req_tag;
-        int                     rc;
 
         /* Clear the existing signals.  Anything but SIGUSR1 is converted
          * into a high-priority EXIT exception. */
         for (;;) {
-                sig = dequeue_signal_lock(current, &current->blocked, &info);
+                int sig =
+                        dequeue_signal_lock(current, &current->blocked, &info);
                 if (!sig)
                         break;
                 if (sig != SIGUSR1) {
@@ -2482,7 +2436,7 @@ static void handle_exception(struct fsg_common *common)
         }
 
         /* Cancel all the pending transfers */
-        if (fsg_is_set(common)) {
+        if (likely(common->fsg)) {
                 for (i = 0; i < FSG_NUM_BUFFERS; ++i) {
                         bh = &common->buffhds[i];
                         if (bh->inreq_busy)
@@ -2523,7 +2477,6 @@ static void handle_exception(struct fsg_common *common)
         common->next_buffhd_to_fill = &common->buffhds[0];
         common->next_buffhd_to_drain = &common->buffhds[0];
         exception_req_tag = common->exception_req_tag;
-        new_config = common->new_config;
         old_state = common->state;
 
         if (old_state == FSG_STATE_ABORT_BULK_OUT)
@@ -2573,12 +2526,12 @@ static void handle_exception(struct fsg_common *common)
                 break;
 
         case FSG_STATE_CONFIG_CHANGE:
-                rc = do_set_config(common, new_config);
+                do_set_interface(common, common->new_fsg);
                 break;
 
         case FSG_STATE_EXIT:
         case FSG_STATE_TERMINATED:
-                do_set_config(common, 0);               /* Free resources */
+                do_set_interface(common, NULL);         /* Free resources */
                 spin_lock_irq(&common->lock);
                 common->state = FSG_STATE_TERMINATED;   /* Stop the thread */
                 spin_unlock_irq(&common->lock);
@@ -2863,6 +2816,7 @@ buffhds_first_it:
                 goto error_release;
         }
         init_completion(&common->thread_notifier);
+        init_waitqueue_head(&common->fsg_wait);
 #undef OR
 
 
@@ -2957,9 +2911,17 @@ static void fsg_common_release(struct kref *ref)
 static void fsg_unbind(struct usb_configuration *c, struct usb_function *f)
 {
         struct fsg_dev          *fsg = fsg_from_func(f);
+        struct fsg_common       *common = fsg->common;
 
         DBG(fsg, "unbind\n");
-        fsg_common_put(fsg->common);
+        if (fsg->common->fsg == fsg) {
+                fsg->common->new_fsg = NULL;
+                raise_exception(fsg->common, FSG_STATE_CONFIG_CHANGE);
+                /* FIXME: make interruptible or killable somehow? */
+                wait_event(common->fsg_wait, common->fsg != fsg);
+        }
+
+        fsg_common_put(common);
         usb_free_descriptors(fsg->function.descriptors);
         usb_free_descriptors(fsg->function.hs_descriptors);
         kfree(fsg);
@@ -2970,7 +2932,6 @@ 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;
-        int                     rc;
         int                     i;
         struct usb_ep           *ep;
 
@@ -2996,6 +2957,11 @@ static int fsg_bind(struct usb_configuration *c, struct usb_function *f)
         ep->driver_data = fsg->common;  /* claim the endpoint */
         fsg->bulk_out = ep;
 
+        /* Copy descriptors */
+        f->descriptors = usb_copy_descriptors(fsg_fs_function);
+        if (unlikely(!f->descriptors))
+                return -ENOMEM;
+
         if (gadget_is_dualspeed(gadget)) {
                 /* Assume endpoint addresses are the same for both speeds */
                 fsg_hs_bulk_in_desc.bEndpointAddress =
@@ -3003,16 +2969,17 @@ static int fsg_bind(struct usb_configuration *c, struct usb_function *f)
                 fsg_hs_bulk_out_desc.bEndpointAddress =
                         fsg_fs_bulk_out_desc.bEndpointAddress;
                 f->hs_descriptors = usb_copy_descriptors(fsg_hs_function);
-                if (unlikely(!f->hs_descriptors))
+                if (unlikely(!f->hs_descriptors)) {
+                        usb_free_descriptors(f->descriptors);
                         return -ENOMEM;
+                }
         }
 
         return 0;
 
 autoconf_fail:
         ERROR(fsg, "unable to autoconfigure all endpoints\n");
-        rc = -ENOTSUPP;
-        return rc;
+        return -ENOTSUPP;
 }
 
 
@@ -3036,11 +3003,6 @@ static int fsg_add(struct usb_composite_dev *cdev,
 
         fsg->function.name        = FSG_DRIVER_DESC;
         fsg->function.strings     = fsg_strings_array;
-        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;
@@ -3056,19 +3018,9 @@ static int fsg_add(struct usb_composite_dev *cdev,
 
         rc = usb_add_function(c, &fsg->function);
         if (unlikely(rc))
-                goto error_free_all;
-
-        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);
-
+                kfree(fsg);
+        else
+                fsg_common_get(fsg->common);
         return rc;
 }