From f33e24838dbeae17cbe80b43ca1b169d47fa19d1 Mon Sep 17 00:00:00 2001
From: Jonathan Bell <jonathan@raspberrypi.com>
Date: Tue, 26 Mar 2024 13:36:23 +0000
Subject: [PATCH] drivers: mmc: be more cautious when manipulating Command
 Queue enable

Don't attempt to turn on CQ if the other mandatory features are not
indicated as supported by the card. Also make sure that the register write
actually stuck, as some cards claim support but never report back that
the queue engine is enabled.

Signed-off-by: Jonathan Bell <jonathan@raspberrypi.com>
---
 drivers/mmc/core/sd.c     |  8 ++++++--
 drivers/mmc/core/sd_ops.c | 22 ++++++++++++++++++++++
 2 files changed, 28 insertions(+), 2 deletions(-)

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 <linux/slab.h>
 #include <linux/types.h>
 #include <linux/export.h>
+#include <linux/ktime.h>
 #include <linux/scatterlist.h>
 
 #include <linux/mmc/host.h>
@@ -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;