2 files changed, 41 insertions, 3 deletions

diff --git a/drivers/memstick/core/memstick.c b/drivers/memstick/core/memstick.c
index 3c7d9a79c1ea..7162f772bbfb 100644
--- a/drivers/memstick/core/memstick.c
+++ b/drivers/memstick/core/memstick.c
@@ -433,8 +433,11 @@ static void memstick_check(struct work_struct *work)
 
         dev_dbg(&host->dev, "memstick_check started\n");
         mutex_lock(&host->lock);
-        if (!host->card)
-                memstick_power_on(host);
+        if (!host->card) {
+                if (memstick_power_on(host))
+                        goto out_power_off;
+        } else
+                host->card->stop(host->card);
 
         card = memstick_alloc_card(host);
 
@@ -452,7 +455,8 @@ static void memstick_check(struct work_struct *work)
                             || !(host->card->check(host->card))) {
                                 device_unregister(&host->card->dev);
                                 host->card = NULL;
-                        }
+                        } else
+                                host->card->start(host->card);
                 }
 
                 if (!host->card) {
@@ -465,6 +469,7 @@ static void memstick_check(struct work_struct *work)
                         kfree(card);
         }
 
+out_power_off:
         if (!host->card)
                 host->set_param(host, MEMSTICK_POWER, MEMSTICK_POWER_OFF);
 
diff --git a/drivers/memstick/core/mspro_block.c b/drivers/memstick/core/mspro_block.c
index 477d0fb6e588..004ac4d176d9 100644
--- a/drivers/memstick/core/mspro_block.c
+++ b/drivers/memstick/core/mspro_block.c
@@ -752,6 +752,37 @@ static int mspro_block_has_request(struct mspro_block_data *msb)
         return rc;
 }
 
+static void mspro_block_stop(struct memstick_dev *card)
+{
+        struct mspro_block_data *msb = memstick_get_drvdata(card);
+        int rc = 0;
+        unsigned long flags;
+
+        while (1) {
+                spin_lock_irqsave(&msb->q_lock, flags);
+                if (!msb->has_request) {
+                        blk_stop_queue(msb->queue);
+                        rc = 1;
+                }
+                spin_unlock_irqrestore(&msb->q_lock, flags);
+
+                if (rc)
+                        break;
+
+                wait_for_completion(&card->mrq_complete);
+        }
+}
+
+static void mspro_block_start(struct memstick_dev *card)
+{
+        struct mspro_block_data *msb = memstick_get_drvdata(card);
+        unsigned long flags;
+
+        spin_lock_irqsave(&msb->q_lock, flags);
+        blk_start_queue(msb->queue);
+        spin_unlock_irqrestore(&msb->q_lock, flags);
+}
+
 static int mspro_block_queue_thread(void *data)
 {
         struct memstick_dev *card = data;
@@ -1272,6 +1303,8 @@ static int mspro_block_probe(struct memstick_dev *card)
         rc = mspro_block_init_disk(card);
         if (!rc) {
                 card->check = mspro_block_check_card;
+                card->stop = mspro_block_stop;
+                card->start = mspro_block_start;
                 return 0;
         }