1 files changed, 63 insertions, 0 deletions
diff --git a/drivers/mmc/core/mmc_ops.c b/drivers/mmc/core/mmc_ops.c
index 28bed97ae1a5..c85c58aca3e2 100644
--- a/drivers/mmc/core/mmc_ops.c
+++ b/drivers/mmc/core/mmc_ops.c
@@ -617,3 +617,66 @@ int mmc_send_bk_ops_cmd(struct mmc_card *card, bool is_synchronous)
 
 	return 0;
 }
+
+int mmc_gen_cmd(struct mmc_card *card, void *buf, u8 index, u8 arg1, u8 arg2, u8 mode)
+{
+	struct mmc_request mrq;
+	struct mmc_command cmd;
+	struct mmc_data data;
+	struct mmc_command stop;
+	struct scatterlist sg;
+	void *data_buf;
+
+	mmc_set_blocklen(card, 512);
+
+	data_buf = kmalloc(512, GFP_KERNEL);
+	if (data_buf == NULL)
+		return -ENOMEM;
+
+	memset(&mrq, 0, sizeof(struct mmc_request));
+	memset(&cmd, 0, sizeof(struct mmc_command));
+	memset(&data, 0, sizeof(struct mmc_data));
+	memset(&stop, 0, sizeof(struct mmc_command));
+
+	mrq.cmd = &cmd;
+	mrq.data = &data;
+	mrq.stop = &stop;
+
+	cmd.opcode = MMC_GEN_CMD;
+	cmd.arg = (arg2 << 16) |
+		  (arg1 << 8) |
+		  (index << 1) |
+		  mode;
+
+	cmd.flags = MMC_RSP_SPI_R1 | MMC_RSP_R1 | MMC_CMD_ADTC;
+
+	data.blksz = 512;
+	data.blocks = 1;
+	data.flags = MMC_DATA_READ;
+	data.sg = &sg;
+	data.sg_len = 1;
+
+	stop.opcode = MMC_STOP_TRANSMISSION;
+	stop.arg = 0;
+	stop.flags = MMC_RSP_SPI_R1B | MMC_RSP_R1B | MMC_CMD_AC;
+
+	sg_init_one(&sg, data_buf, 512);
+
+	mmc_set_data_timeout(&data, card);
+
+	mmc_claim_host(card->host);
+	mmc_wait_for_req(card->host, &mrq);
+	mmc_release_host(card->host);
+
+	memcpy(buf, data_buf, 512);
+	kfree(data_buf);
+
+	if (cmd.error)
+		return cmd.error;
+	if (data.error)
+		return data.error;
+	if (stop.error)
+		return stop.error;
+
+	return 0;
+}