firmware/br-ext-chip-goke/board/gk7205v200/kernel/patches/00_drivers-mmc-host-sdhci.c.patch

--- linux-4.9.37/drivers/mmc/host/sdhci.c	2017-07-12 16:42:41.000000000 +0300
+++ linux-4.9.y/drivers/mmc/host/sdhci.c	2021-06-07 13:01:33.000000000 +0300
@@ -32,6 +32,7 @@
 #include <linux/mmc/slot-gpio.h>
 
 #include "sdhci.h"
+#include "sdhci-cmdq.h"
 
 #define DRIVER_NAME "sdhci"
 
@@ -76,8 +77,8 @@
 	pr_err(DRIVER_NAME ": Int enab: 0x%08x | Sig enab: 0x%08x\n",
 	       sdhci_readl(host, SDHCI_INT_ENABLE),
 	       sdhci_readl(host, SDHCI_SIGNAL_ENABLE));
-	pr_err(DRIVER_NAME ": AC12 err: 0x%08x | Slot int: 0x%08x\n",
-	       sdhci_readw(host, SDHCI_ACMD12_ERR),
+	pr_err(DRIVER_NAME ": ACMD err: 0x%08x | Slot int: 0x%08x\n",
+	       sdhci_readw(host, SDHCI_AUTO_CMD_ERR),
 	       sdhci_readw(host, SDHCI_SLOT_INT_STATUS));
 	pr_err(DRIVER_NAME ": Caps:     0x%08x | Caps_1:   0x%08x\n",
 	       sdhci_readl(host, SDHCI_CAPABILITIES),
@@ -227,7 +228,7 @@
 		    SDHCI_INT_DATA_CRC | SDHCI_INT_DATA_TIMEOUT |
 		    SDHCI_INT_INDEX | SDHCI_INT_END_BIT | SDHCI_INT_CRC |
 		    SDHCI_INT_TIMEOUT | SDHCI_INT_DATA_END |
-		    SDHCI_INT_RESPONSE;
+		    SDHCI_INT_RESPONSE | SDHCI_INT_ACMD_ERR;
 
 	if (host->tuning_mode == SDHCI_TUNING_MODE_2 ||
 	    host->tuning_mode == SDHCI_TUNING_MODE_3)
@@ -240,11 +241,17 @@
 		/* force clock reconfiguration */
 		host->clock = 0;
 		mmc->ops->set_ios(mmc, &mmc->ios);
+	} else {
+		if (host->ops->extra_init)
+			host->ops->extra_init(host);
 	}
 }
 
 static void sdhci_reinit(struct sdhci_host *host)
 {
+	if (host->ops->pre_init)
+		host->ops->pre_init(host);
+
 	sdhci_init(host, 0);
 	sdhci_enable_card_detection(host);
 }
@@ -522,6 +529,60 @@
 		dma_desc->addr_hi = cpu_to_le32((u64)addr >> 32);
 }
 
+static void sdhci_write_cmd_table(u8 *desc, u32 reg, u32 attr)
+{
+	__le32 *reg_addr = (__le32 __force *)(desc + 4);
+	__le32 *attr_addr = (__le32 __force *)desc;
+
+	attr_addr[0] = cpu_to_le32(attr);
+	reg_addr[0] = cpu_to_le32(reg);
+}
+
+static void sdhci_write_adma3_desc(struct sdhci_host *host, u8 *desc,
+		dma_addr_t addr, unsigned int attr)
+{
+	__le32 *attr_addr = (__le32 __force *)desc;
+
+	attr_addr[0] = cpu_to_le32(attr);
+
+	if (host->flags & SDHCI_USE_64_BIT_DMA) {
+		__le64 *cmd_ddr = (__le64 __force *)(desc + 4);
+		cmd_ddr[0] = cpu_to_le64(addr);
+	} else {
+		__le32 *cmd_ddr = (__le32 __force *)(desc + 4);
+		cmd_ddr[0] = cpu_to_le32(addr);
+	}
+}
+
+static void sdhci_prep_adma3_desc(struct sdhci_host *host,
+		struct mmc_command *cmd, int flags)
+{
+	struct mmc_data *data = cmd->data;
+	unsigned int ctrl_2, cmd_xfer, blksz;
+	u16 mode;
+
+	blksz = SDHCI_MAKE_BLKSZ(0, data->blksz);
+	mode = sdhci_readw(host, SDHCI_TRANSFER_MODE);
+	cmd_xfer = (SDHCI_MAKE_CMD(cmd->opcode, flags) << 16) | mode;
+
+	sdhci_write_cmd_table(host->cmd_table, data->blocks, ADMA3_CMD_VALID);
+	sdhci_write_cmd_table(host->cmd_table + 0x8, blksz, ADMA3_CMD_VALID);
+	sdhci_write_cmd_table(host->cmd_table + 0x10,
+			cmd->arg, ADMA3_CMD_VALID);
+	sdhci_write_cmd_table(host->cmd_table + 0x18,
+			cmd_xfer, ADMA3_CMD_VALID);
+	sdhci_adma_write_desc(host, host->cmd_table + 0x20,
+			host->adma_addr, 0x0, ADMA2_LINK_VALID);
+	sdhci_write_adma3_desc(host, host->adma3_table,
+			host->cmd_addr, ADMA3_END);
+
+	ctrl_2 = sdhci_readw(host, SDHCI_HOST_CONTROL2);
+	ctrl_2 |= SDHCI_CTRL_HOST_VER4_ENABLE;
+	if (host->flags & SDHCI_USE_64_BIT_DMA)
+		ctrl_2 |= SDHCI_CTRL_ADDRESSING_64BIT;
+	sdhci_writew(host, ctrl_2, SDHCI_HOST_CONTROL2);
+}
+
 static void sdhci_adma_mark_end(void *desc)
 {
 	struct sdhci_adma2_64_desc *dma_desc = desc;
@@ -589,6 +650,17 @@
 		BUG_ON(len > 65536);
 
 		if (len) {
+			/* work around for buffer across 128M boundary, split the buffer */
+			if (((addr & (SDHCI_DMA_BOUNDARY_SIZE - 1)) + len) >
+					SDHCI_DMA_BOUNDARY_SIZE) {
+				offset = SDHCI_DMA_BOUNDARY_SIZE -
+					(addr & (SDHCI_DMA_BOUNDARY_SIZE - 1));
+				sdhci_adma_write_desc(host, desc, addr, offset,
+						ADMA2_TRAN_VALID);
+				desc += host->desc_sz;
+				addr += offset;
+				len -= offset;
+			}
 			/* tran, valid */
 			sdhci_adma_write_desc(host, desc, addr, len,
 					      ADMA2_TRAN_VALID);
@@ -855,10 +927,14 @@
 		ctrl &= ~SDHCI_CTRL_DMA_MASK;
 		if ((host->flags & SDHCI_REQ_USE_DMA) &&
 			(host->flags & SDHCI_USE_ADMA)) {
-			if (host->flags & SDHCI_USE_64_BIT_DMA)
-				ctrl |= SDHCI_CTRL_ADMA64;
-			else
-				ctrl |= SDHCI_CTRL_ADMA32;
+			if (host->flags & SDHCI_USE_ADMA3) {
+					ctrl |= SDHCI_CTRL_ADMA3;
+			} else {
+				if (host->flags & SDHCI_USE_64_BIT_DMA)
+					ctrl |= SDHCI_CTRL_ADMA64;
+				else
+					ctrl |= SDHCI_CTRL_ADMA32;
+			}
 		} else {
 			ctrl |= SDHCI_CTRL_SDMA;
 		}
@@ -1121,7 +1197,8 @@
 
 	sdhci_prepare_data(host, cmd);
 
-	sdhci_writel(host, cmd->arg, SDHCI_ARGUMENT);
+	if (!(host->flags & SDHCI_USE_ADMA3) || !cmd->data)
+		sdhci_writel(host, cmd->arg, SDHCI_ARGUMENT);
 
 	sdhci_set_transfer_mode(host, cmd);
 
@@ -1152,10 +1229,29 @@
 	    cmd->opcode == MMC_SEND_TUNING_BLOCK_HS200)
 		flags |= SDHCI_CMD_DATA;
 
-	sdhci_writew(host, SDHCI_MAKE_CMD(cmd->opcode, flags), SDHCI_COMMAND);
+	if (host->flags & SDHCI_USE_ADMA3 && cmd->data) {
+		sdhci_prep_adma3_desc(host, cmd, flags);
+
+		sdhci_writel(host, (u32)host->adma3_addr,
+				SDHCI_ADMA3_ID_ADDR_LOW);
+		if (host->flags & SDHCI_USE_64_BIT_DMA)
+			sdhci_writel(host, (u32)((u64)host->adma3_addr >> 32),
+					SDHCI_ADMA3_ID_ADDR_HI);
+	} else {
+		sdhci_writew(host, SDHCI_MAKE_CMD(cmd->opcode, flags),
+				SDHCI_COMMAND);
+	}
 }
 EXPORT_SYMBOL_GPL(sdhci_send_command);
 
+#define CMD_ERRORS                          \
+	(R1_OUT_OF_RANGE |  /* Command argument out of range */ \
+	 R1_ADDRESS_ERROR | /* Misaligned address */        \
+	 R1_BLOCK_LEN_ERROR |   /* Transferred block length incorrect */\
+	 R1_WP_VIOLATION |  /* Tried to write to protected block */ \
+	 R1_CC_ERROR |      /* Card controller error */     \
+	 R1_ERROR)      /* General/unknown error */
+
 static void sdhci_finish_command(struct sdhci_host *host)
 {
 	struct mmc_command *cmd = host->cmd;
@@ -1177,6 +1273,15 @@
 		} else {
 			cmd->resp[0] = sdhci_readl(host, SDHCI_RESPONSE);
 		}
+
+		if (((cmd->flags & MMC_RSP_R1) == MMC_RSP_R1) &&
+			((cmd->flags & MMC_CMD_MASK) != MMC_CMD_BCR)) {
+			if ((cmd->resp[0] & CMD_ERRORS) && !host->is_tuning) {
+				host->error_count++;
+				cmd->mrq->cmd->error = -EACCES;
+				pr_err("The status of the card is abnormal, cmd->resp[0]: %x", cmd->resp[0]);
+			}
+		}
 	}
 
 	if (cmd->mrq->cap_cmd_during_tfr && cmd == cmd->mrq->cmd)
@@ -1430,6 +1535,12 @@
 		sdhci_writeb(host, 0, SDHCI_POWER_CONTROL);
 		if (host->quirks2 & SDHCI_QUIRK2_CARD_ON_NEEDS_BUS_ON)
 			sdhci_runtime_pm_bus_off(host);
+		/*
+		 * Controllers need an extra 100ms delay to ensure power off
+		 * completely
+		 */
+		msleep(100);
+
 	} else {
 		/*
 		 * Spec says that we should clear the power reg before setting
@@ -1568,13 +1679,9 @@
 static void sdhci_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 {
 	struct sdhci_host *host = mmc_priv(mmc);
-	unsigned long flags;
 	u8 ctrl;
 
-	spin_lock_irqsave(&host->lock, flags);
-
 	if (host->flags & SDHCI_DEVICE_DEAD) {
-		spin_unlock_irqrestore(&host->lock, flags);
 		if (!IS_ERR(mmc->supply.vmmc) &&
 		    ios->power_mode == MMC_POWER_OFF)
 			mmc_regulator_set_ocr(mmc, mmc->supply.vmmc, 0);
@@ -1710,7 +1817,9 @@
 		}
 
 		/* Re-enable SD Clock */
-		host->ops->set_clock(host, host->clock);
+		clk = sdhci_readw(host, SDHCI_CLOCK_CONTROL);
+		clk |= SDHCI_CLOCK_CARD_EN;
+		sdhci_writew(host, clk, SDHCI_CLOCK_CONTROL);
 	} else
 		sdhci_writeb(host, ctrl, SDHCI_HOST_CONTROL);
 
@@ -1723,7 +1832,6 @@
 		sdhci_do_reset(host, SDHCI_RESET_CMD | SDHCI_RESET_DATA);
 
 	mmiowb();
-	spin_unlock_irqrestore(&host->lock, flags);
 }
 
 static int sdhci_get_cd(struct mmc_host *mmc)
@@ -1846,6 +1954,9 @@
 	u16 ctrl;
 	int ret;
 
+	if (host->ops->start_signal_voltage_switch)
+		return host->ops->start_signal_voltage_switch(host, ios);
+
 	/*
 	 * Signal Voltage Switching is only applicable for Host Controllers
 	 * v3.00 and above.
@@ -2281,6 +2392,33 @@
 	spin_unlock_irqrestore(&host->lock, flags);
 }
 
+static int sdhci_card_info_save(struct mmc_host *mmc)
+{
+	struct mmc_card *card = mmc->card;
+	struct sdhci_host *host= mmc_priv(mmc);
+	struct card_info *c_info = &host->c_info;
+
+	if (!card) {
+		memset(c_info,0,sizeof(struct card_info));
+		c_info->card_connect = CARD_DISCONNECT;
+		goto out;
+	}
+
+	c_info->card_type = card->type;
+	c_info->card_state = card->state;
+
+	c_info->timing = mmc->ios.timing;
+	c_info->card_support_clock = mmc->ios.clock;
+
+	c_info->sd_bus_speed = card->sd_bus_speed;
+
+	memcpy(c_info->ssr, card->raw_ssr, ARRAY_SIZE(c_info->ssr));
+
+	c_info->card_connect = CARD_CONNECT;
+out:
+	return 0;
+}
+
 static const struct mmc_host_ops sdhci_ops = {
 	.request	= sdhci_request,
 	.post_req	= sdhci_post_req,
@@ -2296,6 +2434,7 @@
 	.select_drive_strength		= sdhci_select_drive_strength,
 	.card_event			= sdhci_card_event,
 	.card_busy	= sdhci_card_busy,
+	.card_info_save = sdhci_card_info_save,
 };
 
 /*****************************************************************************\
@@ -2370,6 +2509,9 @@
 		host->pending_reset = false;
 	}
 
+	if (mrq->data && mrq->data->error && !host->is_tuning)
+		host->error_count++;
+
 	if (!sdhci_has_requests(host))
 		sdhci_led_deactivate(host);
 
@@ -2460,17 +2602,32 @@
 		 */
 		if (host->pending_reset)
 			return;
-		pr_err("%s: Got command interrupt 0x%08x even though no command operation was in progress.\n",
+
+		/*pr_err("%s: Got command interrupt 0x%08x even though no command operation was in progress.\n",
 		       mmc_hostname(host->mmc), (unsigned)intmask);
-		sdhci_dumpregs(host);
+		sdhci_dumpregs(host);*/
+
 		return;
 	}
 
 	if (intmask & (SDHCI_INT_TIMEOUT | SDHCI_INT_CRC |
-		       SDHCI_INT_END_BIT | SDHCI_INT_INDEX)) {
+		       SDHCI_INT_END_BIT | SDHCI_INT_INDEX |
+		       SDHCI_INT_ACMD_ERR)) {
 		if (intmask & SDHCI_INT_TIMEOUT)
 			host->cmd->error = -ETIMEDOUT;
-		else
+		else if (intmask & SDHCI_INT_ACMD_ERR) {
+			u16 acmd_stat = sdhci_readw(host, SDHCI_AUTO_CMD_ERR);
+
+			if (acmd_stat & (SDHCI_AUTO_CMD12_NOT_EXEC |
+						SDHCI_AUTO_CMD_INDEX_ERR |
+						SDHCI_AUTO_CMD12_NOT_ISSUED))
+				host->cmd->error = -EIO;
+			else if (acmd_stat & SDHCI_AUTO_CMD_TIMEOUT_ERR)
+				host->cmd->error = -ETIMEDOUT;
+			else
+				host->cmd->error = -EILSEQ;
+
+		} else
 			host->cmd->error = -EILSEQ;
 
 		/*
@@ -2533,6 +2690,8 @@
 
 static void sdhci_data_irq(struct sdhci_host *host, u32 intmask)
 {
+
+#ifndef SDHCI_GOKE_EDGE_TUNING
 	u32 command;
 
 	/* CMD19 generates _only_ Buffer Read Ready interrupt */
@@ -2545,6 +2704,7 @@
 			return;
 		}
 	}
+#endif
 
 	if (!host->data) {
 		struct mmc_command *data_cmd = host->data_cmd;
@@ -2584,6 +2744,9 @@
 		if (host->pending_reset)
 			return;
 
+		if (host->is_tuning)
+			return;
+
 		pr_err("%s: Got data interrupt 0x%08x even though no data operation was in progress.\n",
 		       mmc_hostname(host->mmc), (unsigned)intmask);
 		sdhci_dumpregs(host);
@@ -2655,6 +2818,58 @@
 	}
 }
 
+#ifdef CONFIG_MMC_CQ_HCI
+static int sdhci_get_cmd_err(u32 intmask)
+{
+	if (intmask & SDHCI_INT_TIMEOUT)
+		return -ETIMEDOUT;
+	else if (intmask & (SDHCI_INT_CRC | SDHCI_INT_END_BIT |
+			    SDHCI_INT_INDEX))
+		return -EILSEQ;
+	return 0;
+}
+
+static int sdhci_get_data_err(u32 intmask)
+{
+	if (intmask & SDHCI_INT_DATA_TIMEOUT)
+		return -ETIMEDOUT;
+	else if (intmask & (SDHCI_INT_DATA_END_BIT | SDHCI_INT_DATA_CRC))
+		return -EILSEQ;
+	else if (intmask & SDHCI_INT_ADMA_ERROR)
+		return -EIO;
+	return 0;
+}
+
+static irqreturn_t sdhci_cmdq_irq(struct sdhci_host *host, u32 intmask)
+{
+	int err = 0;
+	u32 mask = 0;
+	irqreturn_t ret;
+
+	if (intmask & SDHCI_INT_CMD_MASK)
+		err = sdhci_get_cmd_err(intmask);
+	else if (intmask & SDHCI_INT_DATA_MASK)
+		err = sdhci_get_data_err(intmask);
+
+	ret = cmdq_irq(host->mmc, err);
+	if (err) {
+		/* Clear the error interrupts */
+		mask = intmask & SDHCI_INT_ERROR_MASK;
+		sdhci_writel(host, mask, SDHCI_INT_STATUS);
+	}
+	return ret;
+
+}
+
+#else
+static irqreturn_t sdhci_cmdq_irq(struct sdhci_host *host, u32 intmask)
+{
+	pr_err("%s: Received cmdq-irq when disabled !!!!\n",
+		mmc_hostname(host->mmc));
+	return IRQ_NONE;
+}
+#endif
+
 static irqreturn_t sdhci_irq(int irq, void *dev_id)
 {
 	irqreturn_t result = IRQ_NONE;
@@ -2676,6 +2891,19 @@
 	}
 
 	do {
+		if (host->mmc->card && mmc_card_cmdq(host->mmc->card) &&
+		!mmc_host_halt(host->mmc) && !mmc_host_cq_disable(host->mmc)) {
+			DBG("*** %s: cmdq intr: 0x%08x\n",
+					mmc_hostname(host->mmc),
+					intmask);
+			result = sdhci_cmdq_irq(host, intmask);
+			if (result == IRQ_HANDLED) {
+				mask = intmask & SDHCI_INT_CQE;
+				sdhci_writel(host, mask, SDHCI_INT_STATUS);
+				goto out;
+			}
+		}
+
 		/* Clear selected interrupts. */
 		mask = intmask & (SDHCI_INT_CMD_MASK | SDHCI_INT_DATA_MASK |
 				  SDHCI_INT_BUS_POWER);
@@ -3093,7 +3321,7 @@
 
 	override_timeout_clk = host->timeout_clk;
 
-	if (host->version > SDHCI_SPEC_300) {
+	if (host->version > SDHCI_SPEC_420) {
 		pr_err("%s: Unknown controller version (%d). You may experience problems.\n",
 		       mmc_hostname(mmc), host->version);
 	}
@@ -3121,6 +3349,15 @@
 		host->flags &= ~SDHCI_USE_ADMA;
 	}
 
+	if ((host->version >= SDHCI_SPEC_400) &&
+		(host->caps1 & SDHCI_CAN_DO_ADMA3))
+		host->flags |= SDHCI_USE_ADMA3 | SDHCI_HOST_VER4_ENABLE;
+
+	if ((host->quirks2 & SDHCI_QUIRK2_BROKEN_ADMA3) &&
+			(host->flags & SDHCI_USE_ADMA3)) {
+		DBG("Disabling ADMA3 as it is marked broken\n");
+		host->flags &= ~(SDHCI_USE_ADMA3 | SDHCI_HOST_VER4_ENABLE);
+	}
 	/*
 	 * It is assumed that a 64-bit capable device has set a 64-bit DMA mask
 	 * and *must* do 64-bit DMA.  A driver has the opportunity to change
@@ -3161,14 +3398,14 @@
 		 * all multipled by the descriptor size.
 		 */
 		if (host->flags & SDHCI_USE_64_BIT_DMA) {
-			host->adma_table_sz = (SDHCI_MAX_SEGS * 2 + 1) *
-					      SDHCI_ADMA2_64_DESC_SZ;
-			host->desc_sz = SDHCI_ADMA2_64_DESC_SZ;
-		} else {
-			host->adma_table_sz = (SDHCI_MAX_SEGS * 2 + 1) *
-					      SDHCI_ADMA2_32_DESC_SZ;
+			if (host->flags & SDHCI_HOST_VER4_ENABLE)
+				host->desc_sz = 16;
+			else
+				host->desc_sz = SDHCI_ADMA2_64_DESC_SZ;
+		} else
 			host->desc_sz = SDHCI_ADMA2_32_DESC_SZ;
-		}
+
+		host->adma_table_sz = (SDHCI_MAX_SEGS * 2 + 1) * host->desc_sz;
 
 		host->align_buffer_sz = SDHCI_MAX_SEGS * SDHCI_ADMA2_ALIGN;
 		buf = dma_alloc_coherent(mmc_dev(mmc), host->align_buffer_sz +
@@ -3191,6 +3428,36 @@
 			host->adma_table = buf + host->align_buffer_sz;
 			host->adma_addr = dma + host->align_buffer_sz;
 		}
+
+		if (!(host->flags & SDHCI_USE_ADMA))
+			host->flags &= ~SDHCI_USE_ADMA3;
+
+		if (host->flags & SDHCI_USE_ADMA3) {
+#define MAX_CMD_NUM	32
+#define SDHCI_CMD_DESC_SZ	16
+			if (host->flags & SDHCI_USE_64_BIT_DMA)
+				host->adma3_desc_sz = SDHCI_ADMA3_64_DESC_SZ;
+			else
+				host->adma3_desc_sz = SDHCI_ADMA3_32_DESC_SZ;
+
+			host->adma3_table_sz = MAX_CMD_NUM * host->adma3_desc_sz;
+			host->cmd_table_sz = MAX_CMD_NUM *
+						(SDHCI_CMD_DESC_SZ + 16);
+			buf = dma_alloc_coherent(mmc_dev(mmc), host->adma3_table_sz +
+					host->cmd_table_sz, &dma, GFP_KERNEL);
+			if (!buf) {
+				pr_warn("%s: Unable to allocate ADMA3 buffers - falling back to standard DMA\n",
+						mmc_hostname(mmc));
+				host->flags &= ~SDHCI_USE_ADMA3;
+			} else {
+				host->adma3_table = buf;
+				host->adma3_addr = dma;
+
+				host->cmd_table = buf + host->adma3_desc_sz;
+				host->cmd_addr = dma + host->adma3_desc_sz;
+			}
+		}
+
 	}
 
 	/*
@@ -3557,10 +3824,165 @@
 	host->adma_table = NULL;
 	host->align_buffer = NULL;
 
+	if (host->adma3_table)
+		dma_free_coherent(mmc_dev(mmc), host->adma3_table_sz +
+				  host->cmd_table_sz, host->adma3_table,
+				  host->adma3_addr);
+
+	host->adma3_table = NULL;
+	host->cmd_table = NULL;
+
 	return ret;
 }
 EXPORT_SYMBOL_GPL(sdhci_setup_host);
 
+#ifdef CONFIG_MMC_CQ_HCI
+static void sdhci_cmdq_set_transfer_params(struct mmc_host *mmc)
+{
+	struct sdhci_host *host = mmc_priv(mmc);
+	u8 ctrl;
+	u16 mode, ctrl2;
+
+	if (host->version >= SDHCI_SPEC_200) {
+
+		ctrl2 = sdhci_readw(host, SDHCI_CLOCK_CONTROL);
+		ctrl2 |= SDHCI_CLOCK_PLL_EN;
+		sdhci_writew(host, ctrl2, SDHCI_CLOCK_CONTROL);
+
+		ctrl2 = sdhci_readw(host, SDHCI_HOST_CONTROL2);
+		  ctrl2 |= SDHCI_CTRL_ADDRESSING_64BIT;
+		  ctrl2 |= SDHCI_CTRL_HOST_VER4_ENABLE;
+		  sdhci_writew(host, ctrl2, SDHCI_HOST_CONTROL2);
+
+		ctrl = sdhci_readb(host, SDHCI_HOST_CONTROL);
+		ctrl &= ~SDHCI_CTRL_DMA_MASK;
+		if (host->flags & SDHCI_USE_64_BIT_DMA)
+			ctrl |= SDHCI_CTRL_ADMA64;
+		else
+			ctrl |= SDHCI_CTRL_ADMA32;
+		sdhci_writeb(host, ctrl, SDHCI_HOST_CONTROL);
+
+		mode = sdhci_readw(host, SDHCI_TRANSFER_MODE);
+		sdhci_writew(host, mode | SDHCI_TRNS_MULTI, SDHCI_TRANSFER_MODE);
+	}
+}
+
+static void sdhci_cmdq_clear_set_irqs(struct mmc_host *mmc, bool clear)
+{
+	struct sdhci_host *host = mmc_priv(mmc);
+	u32 ier = 0;
+
+	ier &= ~SDHCI_INT_ALL_MASK;
+
+	if (clear) {
+		ier = SDHCI_INT_CQE | SDHCI_INT_ERROR_MASK;
+		sdhci_writel(host, ier, SDHCI_INT_ENABLE);
+		sdhci_writel(host, ier, SDHCI_SIGNAL_ENABLE);
+	} else {
+		ier = SDHCI_INT_BUS_POWER | SDHCI_INT_DATA_END_BIT |
+			     SDHCI_INT_DATA_CRC | SDHCI_INT_DATA_TIMEOUT |
+			     SDHCI_INT_INDEX | SDHCI_INT_END_BIT |
+			     SDHCI_INT_CRC | SDHCI_INT_TIMEOUT |
+			     SDHCI_INT_DATA_END | SDHCI_INT_RESPONSE |
+			     SDHCI_INT_ACMD_ERR;
+		sdhci_writel(host, ier, SDHCI_INT_ENABLE);
+		sdhci_writel(host, ier, SDHCI_SIGNAL_ENABLE);
+	}
+}
+
+static void sdhci_cmdq_set_data_timeout(struct mmc_host *mmc, u32 val)
+{
+	struct sdhci_host *host = mmc_priv(mmc);
+
+	val = 0xe;
+	sdhci_writeb(host, val, SDHCI_TIMEOUT_CONTROL);
+}
+
+static void sdhci_cmdq_dump_goke_regs(struct mmc_host *mmc)
+{
+	struct sdhci_host *host = mmc_priv(mmc);
+
+	sdhci_dumpregs(host);
+}
+
+static int sdhci_cmdq_init(struct sdhci_host *host, struct mmc_host *mmc,
+			   bool dma64)
+{
+	struct cmdq_host *cq_host;
+
+	cq_host = kzalloc(sizeof(*cq_host), GFP_KERNEL);
+	if (!cq_host) {
+		pr_err("failed to allocate memory for CMDQ\n");
+		host->cq_host = NULL;
+		return -ENOMEM;
+	} else {
+		cq_host->mmio = host->ioaddr + 0x180;
+		host->cq_host = cq_host;
+	}
+
+	return cmdq_init(host->cq_host, mmc, dma64);
+}
+
+static void sdhci_cmdq_set_block_size(struct mmc_host *mmc)
+{
+	struct sdhci_host *host = mmc_priv(mmc);
+
+	sdhci_writew(host, SDHCI_MAKE_BLKSZ(0, 512), SDHCI_BLOCK_SIZE);
+}
+
+static void sdhci_cmdq_post_cqe_halt(struct mmc_host *mmc)
+{
+	struct sdhci_host *host = mmc_priv(mmc);
+
+	sdhci_writel(host, sdhci_readl(host, SDHCI_INT_ENABLE) |
+			SDHCI_INT_RESPONSE, SDHCI_INT_ENABLE);
+	sdhci_writel(host, SDHCI_INT_RESPONSE, SDHCI_INT_STATUS);
+}
+#else
+static void sdhci_cmdq_set_transfer_params(struct mmc_host *mmc)
+{
+
+}
+static void sdhci_cmdq_clear_set_irqs(struct mmc_host *mmc, bool clear)
+{
+
+}
+
+static void sdhci_cmdq_set_data_timeout(struct mmc_host *mmc, u32 val)
+{
+
+}
+
+static void sdhci_cmdq_dump_goke_regs(struct mmc_host *mmc)
+{
+
+}
+
+static int sdhci_cmdq_init(struct sdhci_host *host, struct mmc_host *mmc,
+			   bool dma64)
+{
+	return -ENOSYS;
+}
+
+static void sdhci_cmdq_set_block_size(struct mmc_host *mmc)
+{
+
+}
+
+static void sdhci_cmdq_post_cqe_halt(struct mmc_host *mmc)
+{
+}
+#endif
+
+static const struct cmdq_host_ops sdhci_cmdq_ops = {
+	.clear_set_irqs = sdhci_cmdq_clear_set_irqs,
+	.set_data_timeout = sdhci_cmdq_set_data_timeout,
+	.dump_goke_regs = sdhci_cmdq_dump_goke_regs,
+	.set_block_size = sdhci_cmdq_set_block_size,
+	.post_cqe_halt = sdhci_cmdq_post_cqe_halt,
+	.set_transfer_params = sdhci_cmdq_set_transfer_params,
+};
+
 int __sdhci_add_host(struct sdhci_host *host)
 {
 	struct mmc_host *mmc = host->mmc;
@@ -3605,11 +4027,25 @@
 	if (ret)
 		goto unled;
 
-	pr_info("%s: SDHCI controller on %s [%s] using %s\n",
+	if (mmc->caps2 &  MMC_CAP2_CMD_QUEUE) {
+		bool dma64 = (host->flags & SDHCI_USE_64_BIT_DMA) ?
+			true : false;
+		ret = sdhci_cmdq_init(host, mmc, dma64);
+		if (ret)
+			pr_err("%s: CMDQ init: failed (%d)\n",
+			       mmc_hostname(host->mmc), ret);
+		else
+			host->cq_host->ops = &sdhci_cmdq_ops;
+	}
+
+	pr_info("%s: SDHCI controller on %s [%s] using %s in %s mode\n",
 		mmc_hostname(mmc), host->hw_name, dev_name(mmc_dev(mmc)),
+		(host->flags & SDHCI_USE_ADMA3) ? "ADMA3" :
 		(host->flags & SDHCI_USE_ADMA) ?
 		(host->flags & SDHCI_USE_64_BIT_DMA) ? "ADMA 64-bit" : "ADMA" :
-		(host->flags & SDHCI_USE_SDMA) ? "DMA" : "PIO");
+		((host->flags & SDHCI_USE_SDMA) ? "DMA" : "PIO"),
+		((mmc->caps2 &  MMC_CAP2_CMD_QUEUE) && !ret) ?
+		"CMDQ" : "legacy");
 
 	sdhci_enable_card_detection(host);
 
@@ -3635,6 +4071,14 @@
 	host->adma_table = NULL;
 	host->align_buffer = NULL;
 
+	if (host->adma3_table)
+		dma_free_coherent(mmc_dev(mmc), host->adma3_table_sz +
+				  host->cmd_table_sz, host->adma3_table,
+				  host->adma3_addr);
+
+	host->adma3_table = NULL;
+	host->cmd_table = NULL;
+
 	return ret;
 }
 EXPORT_SYMBOL_GPL(__sdhci_add_host);
@@ -3672,6 +4116,8 @@
 
 	sdhci_disable_card_detection(host);
 
+	free_irq(host->irq, host);
+
 	mmc_remove_host(mmc);
 
 	sdhci_led_unregister(host);
@@ -3681,7 +4127,6 @@
 
 	sdhci_writel(host, 0, SDHCI_INT_ENABLE);
 	sdhci_writel(host, 0, SDHCI_SIGNAL_ENABLE);
-	free_irq(host->irq, host);
 
 	del_timer_sync(&host->timer);
 	del_timer_sync(&host->data_timer);
@@ -3698,6 +4143,14 @@
 
 	host->adma_table = NULL;
 	host->align_buffer = NULL;
+
+	if (host->adma3_table)
+		dma_free_coherent(mmc_dev(mmc), host->adma3_table_sz +
+				  host->cmd_table_sz, host->adma3_table,
+				  host->adma3_addr);
+
+	host->adma3_table = NULL;
+	host->cmd_table = NULL;
 }
 
 EXPORT_SYMBOL_GPL(sdhci_remove_host);