2 files changed, 54 insertions, 28 deletions

diff --git a/drivers/mmc/host/s3cmci.c b/drivers/mmc/host/s3cmci.c
index a73ffb9d7b21..bb412331e3d7 100644
--- a/drivers/mmc/host/s3cmci.c
+++ b/drivers/mmc/host/s3cmci.c
@@ -190,7 +190,7 @@ static inline void clear_imask(struct s3cmci_host *host)
 }
 
 static inline int get_data_buffer(struct s3cmci_host *host,
-                                  u32 *words, u32 **pointer)
+                                  u32 *bytes, u32 **pointer)
 {
         struct scatterlist *sg;
 
@@ -207,7 +207,7 @@ static inline int get_data_buffer(struct s3cmci_host *host,
         }
         sg = &host->mrq->data->sg[host->pio_sgptr];
 
-        *words = sg->length >> 2;
+        *bytes = sg->length;
         *pointer = sg_virt(sg);
 
         host->pio_sgptr++;
@@ -223,7 +223,7 @@ static inline u32 fifo_count(struct s3cmci_host *host)
         u32 fifostat = readl(host->base + S3C2410_SDIFSTA);
 
         fifostat &= S3C2410_SDIFSTA_COUNTMASK;
-        return fifostat >> 2;
+        return fifostat;
 }
 
 static inline u32 fifo_free(struct s3cmci_host *host)
@@ -231,13 +231,14 @@ static inline u32 fifo_free(struct s3cmci_host *host)
         u32 fifostat = readl(host->base + S3C2410_SDIFSTA);
 
         fifostat &= S3C2410_SDIFSTA_COUNTMASK;
-        return (63 - fifostat) >> 2;
+        return 63 - fifostat;
 }
 
 static void do_pio_read(struct s3cmci_host *host)
 {
         int res;
         u32 fifo;
+        u32 fifo_words;
         void __iomem *from_ptr;
 
         /* write real prescaler to host, it might be set slow to fix */
@@ -246,8 +247,8 @@ static void do_pio_read(struct s3cmci_host *host)
         from_ptr = host->base + host->sdidata;
 
         while ((fifo = fifo_count(host))) {
-                if (!host->pio_words) {
-                        res = get_data_buffer(host, &host->pio_words,
+                if (!host->pio_bytes) {
+                        res = get_data_buffer(host, &host->pio_bytes,
                                               &host->pio_ptr);
                         if (res) {
                                 host->pio_active = XFER_NONE;
@@ -260,26 +261,45 @@ static void do_pio_read(struct s3cmci_host *host)
 
                         dbg(host, dbg_pio,
                             "pio_read(): new target: [%i]@[%p]\n",
-                            host->pio_words, host->pio_ptr);
+                            host->pio_bytes, host->pio_ptr);
                 }
 
                 dbg(host, dbg_pio,
                     "pio_read(): fifo:[%02i] buffer:[%03i] dcnt:[%08X]\n",
-                    fifo, host->pio_words,
+                    fifo, host->pio_bytes,
                     readl(host->base + S3C2410_SDIDCNT));
 
-                if (fifo > host->pio_words)
-                        fifo = host->pio_words;
+                /* If we have reached the end of the block, we can
+                 * read a word and get 1 to 3 bytes.  If we in the
+                 * middle of the block, we have to read full words,
+                 * otherwise we will write garbage, so round down to
+                 * an even multiple of 4. */
+                if (fifo >= host->pio_bytes)
+                        fifo = host->pio_bytes;
+                else
+                        fifo -= fifo & 3;
 
-                host->pio_words -= fifo;
+                host->pio_bytes -= fifo;
                 host->pio_count += fifo;
 
-                while (fifo--)
+                fifo_words = fifo >> 2;
+                while (fifo_words--)
                         *(host->pio_ptr++) = readl(from_ptr);
+
+                if (fifo & 3) {
+                        u32 n = fifo & 3;
+                        u32 data = readl(from_ptr);
+                        u8 *p = (u8 *)host->pio_ptr;
+
+                        while (n--) {
+                                *p++ = data;
+                                data >>= 8;
+                        }
+                }
         }
 
-        if (!host->pio_words) {
-                res = get_data_buffer(host, &host->pio_words, &host->pio_ptr);
+        if (!host->pio_bytes) {
+                res = get_data_buffer(host, &host->pio_bytes, &host->pio_ptr);
                 if (res) {
                         dbg(host, dbg_pio,
                             "pio_read(): complete (no more buffers).\n");
@@ -303,8 +323,8 @@ static void do_pio_write(struct s3cmci_host *host)
         to_ptr = host->base + host->sdidata;
 
         while ((fifo = fifo_free(host))) {
-                if (!host->pio_words) {
-                        res = get_data_buffer(host, &host->pio_words,
+                if (!host->pio_bytes) {
+                        res = get_data_buffer(host, &host->pio_bytes,
                                                         &host->pio_ptr);
                         if (res) {
                                 dbg(host, dbg_pio,
@@ -316,16 +336,23 @@ static void do_pio_write(struct s3cmci_host *host)
 
                         dbg(host, dbg_pio,
                             "pio_write(): new source: [%i]@[%p]\n",
-                            host->pio_words, host->pio_ptr);
+                            host->pio_bytes, host->pio_ptr);
 
                 }
 
-                if (fifo > host->pio_words)
-                        fifo = host->pio_words;
+                /* If we have reached the end of the block, we have to
+                 * write exactly the remaining number of bytes.  If we
+                 * in the middle of the block, we have to write full
+                 * words, so round down to an even multiple of 4. */
+                if (fifo >= host->pio_bytes)
+                        fifo = host->pio_bytes;
+                else
+                        fifo -= fifo & 3;
 
-                host->pio_words -= fifo;
+                host->pio_bytes -= fifo;
                 host->pio_count += fifo;
 
+                fifo = (fifo + 3) >> 2;
                 while (fifo--)
                         writel(*(host->pio_ptr++), to_ptr);
         }
@@ -350,9 +377,9 @@ static void pio_tasklet(unsigned long data)
                 clear_imask(host);
                 if (host->pio_active != XFER_NONE) {
                         dbg(host, dbg_err, "unfinished %s "
-                            "- pio_count:[%u] pio_words:[%u]\n",
+                            "- pio_count:[%u] pio_bytes:[%u]\n",
                             (host->pio_active == XFER_READ) ? "read" : "write",
-                            host->pio_count, host->pio_words);
+                            host->pio_count, host->pio_bytes);
 
                         if (host->mrq->data)
                                 host->mrq->data->error = -EINVAL;
@@ -813,11 +840,10 @@ static int s3cmci_setup_data(struct s3cmci_host *host, struct mmc_data *data)
                 /* We cannot deal with unaligned blocks with more than
                  * one block being transfered. */
 
-                if (data->blocks > 1)
+                if (data->blocks > 1) {
+                        pr_warning("%s: can't do non-word sized block transfers (blksz %d)\n", __func__, data->blksz);
                         return -EINVAL;
-
-                /* No support yet for non-word block transfers. */
-                return -EINVAL;
+                }
         }
 
         while (readl(host->base + S3C2410_SDIDSTA) &
@@ -897,7 +923,7 @@ static int s3cmci_prepare_pio(struct s3cmci_host *host, struct mmc_data *data)
         BUG_ON((data->flags & BOTH_DIR) == BOTH_DIR);
 
         host->pio_sgptr = 0;
-        host->pio_words = 0;
+        host->pio_bytes = 0;
         host->pio_count = 0;
         host->pio_active = rw ? XFER_WRITE : XFER_READ;
 
diff --git a/drivers/mmc/host/s3cmci.h b/drivers/mmc/host/s3cmci.h
index 7e39109587f9..ca1ba3d58cfd 100644
--- a/drivers/mmc/host/s3cmci.h
+++ b/drivers/mmc/host/s3cmci.h
@@ -51,7 +51,7 @@ struct s3cmci_host {
         int                     dma_complete;
 
         u32                     pio_sgptr;
-        u32                     pio_words;
+        u32                     pio_bytes;
         u32                     pio_count;
         u32                     *pio_ptr;
 #define XFER_NONE 0