1 files changed, 40 insertions, 43 deletions

diff --git a/drivers/mmc/card/block.c b/drivers/mmc/card/block.c
index eed213a5c8cb..dabec556ebb8 100644
--- a/drivers/mmc/card/block.c
+++ b/drivers/mmc/card/block.c
@@ -873,7 +873,7 @@ static int mmc_blk_issue_secdiscard_rq(struct mmc_queue *mq,
 {
         struct mmc_blk_data *md = mq->data;
         struct mmc_card *card = md->queue.card;
-        unsigned int from, nr, arg;
+        unsigned int from, nr, arg, trim_arg, erase_arg;
         int err = 0, type = MMC_BLK_SECDISCARD;
 
         if (!(mmc_can_secure_erase_trim(card) || mmc_can_sanitize(card))) {
@@ -881,20 +881,26 @@ static int mmc_blk_issue_secdiscard_rq(struct mmc_queue *mq,
                 goto out;
         }
 
+        from = blk_rq_pos(req);
+        nr = blk_rq_sectors(req);
+
         /* The sanitize operation is supported at v4.5 only */
         if (mmc_can_sanitize(card)) {
-                err = mmc_switch(card, EXT_CSD_CMD_SET_NORMAL,
-                                EXT_CSD_SANITIZE_START, 1, 0);
-                goto out;
+                erase_arg = MMC_ERASE_ARG;
+                trim_arg = MMC_TRIM_ARG;
+        } else {
+                erase_arg = MMC_SECURE_ERASE_ARG;
+                trim_arg = MMC_SECURE_TRIM1_ARG;
         }
 
-        from = blk_rq_pos(req);
-        nr = blk_rq_sectors(req);
-
-        if (mmc_can_trim(card) && !mmc_erase_group_aligned(card, from, nr))
-                arg = MMC_SECURE_TRIM1_ARG;
-        else
-                arg = MMC_SECURE_ERASE_ARG;
+        if (mmc_erase_group_aligned(card, from, nr))
+                arg = erase_arg;
+        else if (mmc_can_trim(card))
+                arg = trim_arg;
+        else {
+                err = -EINVAL;
+                goto out;
+        }
 retry:
         if (card->quirks & MMC_QUIRK_INAND_CMD38) {
                 err = mmc_switch(card, EXT_CSD_CMD_SET_NORMAL,
@@ -904,25 +910,41 @@ retry:
                                  INAND_CMD38_ARG_SECERASE,
                                  0);
                 if (err)
-                        goto out;
+                        goto out_retry;
         }
+
         err = mmc_erase(card, from, nr, arg);
-        if (!err && arg == MMC_SECURE_TRIM1_ARG) {
+        if (err == -EIO)
+                goto out_retry;
+        if (err)
+                goto out;
+
+        if (arg == MMC_SECURE_TRIM1_ARG) {
                 if (card->quirks & MMC_QUIRK_INAND_CMD38) {
                         err = mmc_switch(card, EXT_CSD_CMD_SET_NORMAL,
                                          INAND_CMD38_ARG_EXT_CSD,
                                          INAND_CMD38_ARG_SECTRIM2,
                                          0);
                         if (err)
-                                goto out;
+                                goto out_retry;
                 }
+
                 err = mmc_erase(card, from, nr, MMC_SECURE_TRIM2_ARG);
+                if (err == -EIO)
+                        goto out_retry;
+                if (err)
+                        goto out;
         }
-out:
-        if (err == -EIO && !mmc_blk_reset(md, card->host, type))
+
+        if (mmc_can_sanitize(card))
+                err = mmc_switch(card, EXT_CSD_CMD_SET_NORMAL,
+                                 EXT_CSD_SANITIZE_START, 1, 0);
+out_retry:
+        if (err && !mmc_blk_reset(md, card->host, type))
                 goto retry;
         if (!err)
                 mmc_blk_reset_success(md, type);
+out:
         spin_lock_irq(&md->lock);
         __blk_end_request(req, err, blk_rq_bytes(req));
         spin_unlock_irq(&md->lock);
@@ -1623,24 +1645,6 @@ static int mmc_blk_alloc_parts(struct mmc_card *card, struct mmc_blk_data *md)
         return ret;
 }
 
-static int
-mmc_blk_set_blksize(struct mmc_blk_data *md, struct mmc_card *card)
-{
-        int err;
-
-        mmc_claim_host(card->host);
-        err = mmc_set_blocklen(card, 512);
-        mmc_release_host(card->host);
-
-        if (err) {
-                pr_err("%s: unable to set block size to 512: %d\n",
-                        md->disk->disk_name, err);
-                return -EINVAL;
-        }
-
-        return 0;
-}
-
 static void mmc_blk_remove_req(struct mmc_blk_data *md)
 {
         struct mmc_card *card;
@@ -1768,7 +1772,6 @@ static const struct mmc_fixup blk_fixups[] =
 static int mmc_blk_probe(struct mmc_card *card)
 {
         struct mmc_blk_data *md, *part_md;
-        int err;
         char cap_str[10];
 
         /*
@@ -1781,10 +1784,6 @@ static int mmc_blk_probe(struct mmc_card *card)
         if (IS_ERR(md))
                 return PTR_ERR(md);
 
-        err = mmc_blk_set_blksize(md, card);
-        if (err)
-                goto out;
-
         string_get_size((u64)get_capacity(md->disk) << 9, STRING_UNITS_2,
                         cap_str, sizeof(cap_str));
         pr_info("%s: %s %s %s %s\n",
@@ -1809,7 +1808,7 @@ static int mmc_blk_probe(struct mmc_card *card)
  out:
         mmc_blk_remove_parts(card, md);
         mmc_blk_remove_req(md);
-        return err;
+        return 0;
 }
 
 static void mmc_blk_remove(struct mmc_card *card)
@@ -1825,7 +1824,7 @@ static void mmc_blk_remove(struct mmc_card *card)
 }
 
 #ifdef CONFIG_PM
-static int mmc_blk_suspend(struct mmc_card *card, pm_message_t state)
+static int mmc_blk_suspend(struct mmc_card *card)
 {
         struct mmc_blk_data *part_md;
         struct mmc_blk_data *md = mmc_get_drvdata(card);
@@ -1845,8 +1844,6 @@ static int mmc_blk_resume(struct mmc_card *card)
         struct mmc_blk_data *md = mmc_get_drvdata(card);
 
         if (md) {
-                mmc_blk_set_blksize(md, card);
-
                 /*
                  * Resume involves the card going into idle state,
                  * so current partition is always the main one.