1 files changed, 101 insertions, 0 deletions

diff --git a/drivers/mmc/core/mmc_ops.c b/drivers/mmc/core/mmc_ops.c
index 326447c9ede8..60842f878ded 100644
--- a/drivers/mmc/core/mmc_ops.c
+++ b/drivers/mmc/core/mmc_ops.c
@@ -462,3 +462,104 @@ int mmc_send_status(struct mmc_card *card, u32 *status)
         return 0;
 }
 
+static int
+mmc_send_bus_test(struct mmc_card *card, struct mmc_host *host, u8 opcode,
+                  u8 len)
+{
+        struct mmc_request mrq;
+        struct mmc_command cmd;
+        struct mmc_data data;
+        struct scatterlist sg;
+        u8 *data_buf;
+        u8 *test_buf;
+        int i, err;
+        static u8 testdata_8bit[8] = { 0x55, 0xaa, 0, 0, 0, 0, 0, 0 };
+        static u8 testdata_4bit[4] = { 0x5a, 0, 0, 0 };
+
+        /* dma onto stack is unsafe/nonportable, but callers to this
+         * routine normally provide temporary on-stack buffers ...
+         */
+        data_buf = kmalloc(len, GFP_KERNEL);
+        if (!data_buf)
+                return -ENOMEM;
+
+        if (len == 8)
+                test_buf = testdata_8bit;
+        else if (len == 4)
+                test_buf = testdata_4bit;
+        else {
+                printk(KERN_ERR "%s: Invalid bus_width %d\n",
+                       mmc_hostname(host), len);
+                kfree(data_buf);
+                return -EINVAL;
+        }
+
+        if (opcode == MMC_BUS_TEST_W)
+                memcpy(data_buf, test_buf, len);
+
+        memset(&mrq, 0, sizeof(struct mmc_request));
+        memset(&cmd, 0, sizeof(struct mmc_command));
+        memset(&data, 0, sizeof(struct mmc_data));
+
+        mrq.cmd = &cmd;
+        mrq.data = &data;
+        cmd.opcode = opcode;
+        cmd.arg = 0;
+
+        /* NOTE HACK:  the MMC_RSP_SPI_R1 is always correct here, but we
+         * rely on callers to never use this with "native" calls for reading
+         * CSD or CID.  Native versions of those commands use the R2 type,
+         * not R1 plus a data block.
+         */
+        cmd.flags = MMC_RSP_SPI_R1 | MMC_RSP_R1 | MMC_CMD_ADTC;
+
+        data.blksz = len;
+        data.blocks = 1;
+        if (opcode == MMC_BUS_TEST_R)
+                data.flags = MMC_DATA_READ;
+        else
+                data.flags = MMC_DATA_WRITE;
+
+        data.sg = &sg;
+        data.sg_len = 1;
+        sg_init_one(&sg, data_buf, len);
+        mmc_wait_for_req(host, &mrq);
+        err = 0;
+        if (opcode == MMC_BUS_TEST_R) {
+                for (i = 0; i < len / 4; i++)
+                        if ((test_buf[i] ^ data_buf[i]) != 0xff) {
+                                err = -EIO;
+                                break;
+                        }
+        }
+        kfree(data_buf);
+
+        if (cmd.error)
+                return cmd.error;
+        if (data.error)
+                return data.error;
+
+        return err;
+}
+
+int mmc_bus_test(struct mmc_card *card, u8 bus_width)
+{
+        int err, width;
+
+        if (bus_width == MMC_BUS_WIDTH_8)
+                width = 8;
+        else if (bus_width == MMC_BUS_WIDTH_4)
+                width = 4;
+        else if (bus_width == MMC_BUS_WIDTH_1)
+                return 0; /* no need for test */
+        else
+                return -EINVAL;
+
+        /*
+         * Ignore errors from BUS_TEST_W.  BUS_TEST_R will fail if there
+         * is a problem.  This improves chances that the test will work.
+         */
+        mmc_send_bus_test(card, card->host, MMC_BUS_TEST_W, width);
+        err = mmc_send_bus_test(card, card->host, MMC_BUS_TEST_R, width);
+        return err;
+}