2 files changed, 46 insertions, 3 deletions

diff --git a/drivers/mmc/card/block.c b/drivers/mmc/card/block.c
index 03f96e975fe5..d545f79f6000 100644
--- a/drivers/mmc/card/block.c
+++ b/drivers/mmc/card/block.c
@@ -280,6 +280,42 @@ out:
         return err ? 0 : 1;
 }
 
+static int mmc_blk_issue_secdiscard_rq(struct mmc_queue *mq,
+                                       struct request *req)
+{
+        struct mmc_blk_data *md = mq->data;
+        struct mmc_card *card = md->queue.card;
+        unsigned int from, nr, arg;
+        int err = 0;
+
+        mmc_claim_host(card->host);
+
+        if (!mmc_can_secure_erase_trim(card)) {
+                err = -EOPNOTSUPP;
+                goto out;
+        }
+
+        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;
+
+        err = mmc_erase(card, from, nr, arg);
+        if (!err && arg == MMC_SECURE_TRIM1_ARG)
+                err = mmc_erase(card, from, nr, MMC_SECURE_TRIM2_ARG);
+out:
+        spin_lock_irq(&md->lock);
+        __blk_end_request(req, err, blk_rq_bytes(req));
+        spin_unlock_irq(&md->lock);
+
+        mmc_release_host(card->host);
+
+        return err ? 0 : 1;
+}
+
 static int mmc_blk_issue_rw_rq(struct mmc_queue *mq, struct request *req)
 {
         struct mmc_blk_data *md = mq->data;
@@ -510,10 +546,14 @@ static int mmc_blk_issue_rw_rq(struct mmc_queue *mq, struct request *req)
 
 static int mmc_blk_issue_rq(struct mmc_queue *mq, struct request *req)
 {
-        if (req->cmd_flags & REQ_DISCARD)
-                return mmc_blk_issue_discard_rq(mq, req);
-        else
+        if (req->cmd_flags & REQ_DISCARD) {
+                if (req->cmd_flags & REQ_SECURE)
+                        return mmc_blk_issue_secdiscard_rq(mq, req);
+                else
+                        return mmc_blk_issue_discard_rq(mq, req);
+        } else {
                 return mmc_blk_issue_rw_rq(mq, req);
+        }
 }
 
 static inline int mmc_blk_readonly(struct mmc_card *card)
diff --git a/drivers/mmc/card/queue.c b/drivers/mmc/card/queue.c
index 3b15884e435c..e876678176be 100644
--- a/drivers/mmc/card/queue.c
+++ b/drivers/mmc/card/queue.c
@@ -141,6 +141,9 @@ int mmc_init_queue(struct mmc_queue *mq, struct mmc_card *card, spinlock_t *lock
                         mq->queue->limits.discard_alignment =
                                                         card->erase_size << 9;
                 }
+                if (mmc_can_secure_erase_trim(card))
+                        queue_flag_set_unlocked(QUEUE_FLAG_SECDISCARD,
+                                                mq->queue);
         }
 
 #ifdef CONFIG_MMC_BLOCK_BOUNCE