[MMC] Fix sdhci PIO routines

The sdhci controllers operate with blocks, not bytes. The PIO routines must
therefore make sure that the minimum unit transfered is a complete block.

Signed-off-by: Pierre Ossman <drzeus@drzeus.cx>
Signed-off-by: Russell King <rmk+kernel@arm.linux.org.uk>

2 files changed, 101 insertions, 63 deletions

diff --git a/drivers/mmc/sdhci.c b/drivers/mmc/sdhci.c
index b9aa60aed7f0..8e480140cd28 100644
--- a/drivers/mmc/sdhci.c
+++ b/drivers/mmc/sdhci.c
@@ -8,12 +8,6 @@
  * published by the Free Software Foundation.
  */
 
- /*
-  * Note that PIO transfer is rather crappy atm. The buffer full/empty
-  * interrupts aren't reliable so we currently transfer the entire buffer
-  * directly. Patches to solve the problem are welcome.
-  */
-
 #include <linux/delay.h>
 #include <linux/highmem.h>
 #include <linux/pci.h>
@@ -128,7 +122,7 @@ static void sdhci_init(struct sdhci_host *host)
                 SDHCI_INT_DATA_CRC | SDHCI_INT_DATA_TIMEOUT | SDHCI_INT_INDEX |
                 SDHCI_INT_END_BIT | SDHCI_INT_CRC | SDHCI_INT_TIMEOUT |
                 SDHCI_INT_CARD_REMOVE | SDHCI_INT_CARD_INSERT |
-                SDHCI_INT_BUF_EMPTY | SDHCI_INT_BUF_FULL |
+                SDHCI_INT_DATA_AVAIL | SDHCI_INT_SPACE_AVAIL |
                 SDHCI_INT_DMA_END | SDHCI_INT_DATA_END | SDHCI_INT_RESPONSE;
 
         writel(intmask, host->ioaddr + SDHCI_INT_ENABLE);
@@ -189,79 +183,96 @@ static inline int sdhci_next_sg(struct sdhci_host* host)
         return host->num_sg;
 }
 
-static void sdhci_transfer_pio(struct sdhci_host *host)
+static void sdhci_read_block_pio(struct sdhci_host *host)
 {
+        int blksize, chunk_remain;
+        u32 data;
         char *buffer;
-        u32 mask;
-        int bytes, size;
-        unsigned long max_jiffies;
+        int size;
 
-        BUG_ON(!host->data);
+        DBG("PIO reading\n");
 
-        if (host->num_sg == 0)
-                return;
-
-        bytes = 0;
-        if (host->data->flags & MMC_DATA_READ)
-                mask = SDHCI_DATA_AVAILABLE;
-        else
-                mask = SDHCI_SPACE_AVAILABLE;
+        blksize = host->data->blksz;
+        chunk_remain = 0;
+        data = 0;
 
         buffer = sdhci_kmap_sg(host) + host->offset;
 
-        /* Transfer shouldn't take more than 5 s */
-        max_jiffies = jiffies + HZ * 5;
+        while (blksize) {
+                if (chunk_remain == 0) {
+                        data = readl(host->ioaddr + SDHCI_BUFFER);
+                        chunk_remain = min(blksize, 4);
+                }
 
-        while (host->size > 0) {
-                if (time_after(jiffies, max_jiffies)) {
-                        printk(KERN_ERR "%s: PIO transfer stalled. "
-                                "Please report this to "
-                                BUGMAIL ".\n", mmc_hostname(host->mmc));
-                        sdhci_dumpregs(host);
+                size = min(host->size, host->remain);
+                size = min(size, chunk_remain);
 
-                        sdhci_kunmap_sg(host);
+                chunk_remain -= size;
+                blksize -= size;
+                host->offset += size;
+                host->remain -= size;
+                host->size -= size;
+                while (size) {
+                        *buffer = data & 0xFF;
+                        buffer++;
+                        data >>= 8;
+                        size--;
+                }
 
-                        host->data->error = MMC_ERR_FAILED;
-                        sdhci_finish_data(host);
-                        return;
+                if (host->remain == 0) {
+                        sdhci_kunmap_sg(host);
+                        if (sdhci_next_sg(host) == 0) {
+                                BUG_ON(blksize != 0);
+                                return;
+                        }
+                        buffer = sdhci_kmap_sg(host);
                 }
+        }
 
-                if (!(readl(host->ioaddr + SDHCI_PRESENT_STATE) & mask))
-                        continue;
+        sdhci_kunmap_sg(host);
+}
 
-                size = min(host->size, host->remain);
+static void sdhci_write_block_pio(struct sdhci_host *host)
+{
+        int blksize, chunk_remain;
+        u32 data;
+        char *buffer;
+        int bytes, size;
 
-                if (size >= 4) {
-                        if (host->data->flags & MMC_DATA_READ)
-                                *(u32*)buffer = readl(host->ioaddr + SDHCI_BUFFER);
-                        else
-                                writel(*(u32*)buffer, host->ioaddr + SDHCI_BUFFER);
-                        size = 4;
-                } else if (size >= 2) {
-                        if (host->data->flags & MMC_DATA_READ)
-                                *(u16*)buffer = readw(host->ioaddr + SDHCI_BUFFER);
-                        else
-                                writew(*(u16*)buffer, host->ioaddr + SDHCI_BUFFER);
-                        size = 2;
-                } else {
-                        if (host->data->flags & MMC_DATA_READ)
-                                *(u8*)buffer = readb(host->ioaddr + SDHCI_BUFFER);
-                        else
-                                writeb(*(u8*)buffer, host->ioaddr + SDHCI_BUFFER);
-                        size = 1;
-                }
+        DBG("PIO writing\n");
+
+        blksize = host->data->blksz;
+        chunk_remain = 4;
+        data = 0;
+
+        bytes = 0;
+        buffer = sdhci_kmap_sg(host) + host->offset;
+
+        while (blksize) {
+                size = min(host->size, host->remain);
+                size = min(size, chunk_remain);
 
-                buffer += size;
+                chunk_remain -= size;
+                blksize -= size;
                 host->offset += size;
                 host->remain -= size;
-
-                bytes += size;
                 host->size -= size;
+                while (size) {
+                        data >>= 8;
+                        data |= (u32)*buffer << 24;
+                        buffer++;
+                        size--;
+                }
+
+                if (chunk_remain == 0) {
+                        writel(data, host->ioaddr + SDHCI_BUFFER);
+                        chunk_remain = min(blksize, 4);
+                }
 
                 if (host->remain == 0) {
                         sdhci_kunmap_sg(host);
                         if (sdhci_next_sg(host) == 0) {
-                                DBG("PIO transfer: %d bytes\n", bytes);
+                                BUG_ON(blksize != 0);
                                 return;
                         }
                         buffer = sdhci_kmap_sg(host);
@@ -269,8 +280,35 @@ static void sdhci_transfer_pio(struct sdhci_host *host)
         }
 
         sdhci_kunmap_sg(host);
+}
+
+static void sdhci_transfer_pio(struct sdhci_host *host)
+{
+        u32 mask;
+
+        BUG_ON(!host->data);
+
+        if (host->size == 0)
+                return;
+
+        if (host->data->flags & MMC_DATA_READ)
+                mask = SDHCI_DATA_AVAILABLE;
+        else
+                mask = SDHCI_SPACE_AVAILABLE;
+
+        while (readl(host->ioaddr + SDHCI_PRESENT_STATE) & mask) {
+                if (host->data->flags & MMC_DATA_READ)
+                        sdhci_read_block_pio(host);
+                else
+                        sdhci_write_block_pio(host);
+
+                if (host->size == 0)
+                        break;
+
+                BUG_ON(host->num_sg == 0);
+        }
 
-        DBG("PIO transfer: %d bytes\n", bytes);
+        DBG("PIO transfer complete.\n");
 }
 
 static void sdhci_prepare_data(struct sdhci_host *host, struct mmc_data *data)
@@ -863,7 +901,7 @@ static void sdhci_data_irq(struct sdhci_host *host, u32 intmask)
         if (host->data->error != MMC_ERR_NONE)
                 sdhci_finish_data(host);
         else {
-                if (intmask & (SDHCI_INT_BUF_FULL | SDHCI_INT_BUF_EMPTY))
+                if (intmask & (SDHCI_INT_DATA_AVAIL | SDHCI_INT_SPACE_AVAIL))
                         sdhci_transfer_pio(host);
 
                 if (intmask & SDHCI_INT_DATA_END)
diff --git a/drivers/mmc/sdhci.h b/drivers/mmc/sdhci.h
index a8f45215ef3a..f8df28f8d6dd 100644
--- a/drivers/mmc/sdhci.h
+++ b/drivers/mmc/sdhci.h
@@ -95,8 +95,8 @@
 #define  SDHCI_INT_RESPONSE     0x00000001
 #define  SDHCI_INT_DATA_END     0x00000002
 #define  SDHCI_INT_DMA_END      0x00000008
-#define  SDHCI_INT_BUF_EMPTY    0x00000010
-#define  SDHCI_INT_BUF_FULL     0x00000020
+#define  SDHCI_INT_SPACE_AVAIL  0x00000010
+#define  SDHCI_INT_DATA_AVAIL   0x00000020
 #define  SDHCI_INT_CARD_INSERT  0x00000040
 #define  SDHCI_INT_CARD_REMOVE  0x00000080
 #define  SDHCI_INT_CARD_INT     0x00000100
@@ -116,7 +116,7 @@
 #define  SDHCI_INT_CMD_MASK     (SDHCI_INT_RESPONSE | SDHCI_INT_TIMEOUT | \
                 SDHCI_INT_CRC | SDHCI_INT_END_BIT | SDHCI_INT_INDEX)
 #define  SDHCI_INT_DATA_MASK    (SDHCI_INT_DATA_END | SDHCI_INT_DMA_END | \
-                SDHCI_INT_BUF_EMPTY | SDHCI_INT_BUF_FULL | \
+                SDHCI_INT_DATA_AVAIL | SDHCI_INT_SPACE_AVAIL | \
                 SDHCI_INT_DATA_TIMEOUT | SDHCI_INT_DATA_CRC | \
                 SDHCI_INT_DATA_END_BIT)