diff --git a/drivers/mmc/core/sd.c b/drivers/mmc/core/sd.c index 4991b987af49..bf557be17654 100644 --- a/drivers/mmc/core/sd.c +++ b/drivers/mmc/core/sd.c @@ -1107,8 +1107,12 @@ static int sd_parse_ext_reg_perf(struct mmc_card *card, u8 fno, u8 page, if ((reg_buf[4] & BIT(0)) && !mmc_card_broken_sd_cache(card)) card->ext_perf.feature_support |= SD_EXT_PERF_CACHE; - /* Command queue support indicated via queue depth bits (0 to 4). */ - if (reg_buf[6] & 0x1f) { + /* + * Command queue support indicated via queue depth bits (0 to 4). + * Qualify this with the other mandatory required features. + */ + if (reg_buf[6] & 0x1f && card->ext_power.feature_support & SD_EXT_POWER_OFF_NOTIFY && + card->ext_perf.feature_support & SD_EXT_PERF_CACHE) { card->ext_perf.feature_support |= SD_EXT_PERF_CMD_QUEUE; card->ext_csd.cmdq_depth = reg_buf[6] & 0x1f; card->ext_csd.cmdq_support = true; diff --git a/drivers/mmc/core/sd_ops.c b/drivers/mmc/core/sd_ops.c index 5bb43b542645..5d03b6b6e69b 100644 --- a/drivers/mmc/core/sd_ops.c +++ b/drivers/mmc/core/sd_ops.c @@ -8,6 +8,7 @@ #include #include #include +#include #include #include @@ -500,6 +501,8 @@ static int mmc_sd_cmdq_switch(struct mmc_card *card, bool enable) { int err; u8 reg = 0; + u8 *reg_buf = card->ext_reg_buf; + ktime_t timeout; /* * SD offers two command queueing modes - sequential (in-order) and * voluntary (out-of-order). Apps Class A2 performance is only @@ -512,6 +515,25 @@ static int mmc_sd_cmdq_switch(struct mmc_card *card, bool enable) /* Performance enhancement register byte 262 controls command queueing */ err = mmc_sd_write_ext_reg(card, card->ext_perf.fno, card->ext_perf.page, card->ext_perf.offset + 262, reg); + if (err) + goto out; + + /* Poll the register - cards may have a lazy init/deinit sequence. */ + timeout = ktime_add_ms(ktime_get(), 10); + while (1) { + err = mmc_sd_read_ext_reg(card, card->ext_perf.fno, card->ext_perf.page, + card->ext_perf.offset + 262, 1, reg_buf); + if (err) + break; + if ((reg_buf[0] & BIT(0)) == reg) + break; + if (ktime_after(ktime_get(), timeout)) { + err = -EBADMSG; + break; + } + usleep_range(100, 200); + } +out: if (!err) card->ext_csd.cmdq_en = enable;