mmc: host: davinci: reimplement read/write fifo in C

On Mon, Dec 15, 2008 at 09:19:05PM +0200, Felipe Balbi wrote:
> cool, will fix :-)
>
> good catch with the missing branch, thanks

here you are

==================== cut here ====================

>From 43fbab69f7d19e39c7c270c4aa1652b0efea0a8f Mon Sep 17 00:00:00 2001
From: Felipe Balbi <felipe.balbi@nokia.com>
Date: Mon, 15 Dec 2008 18:08:36 +0200
Subject: [patch-v2.6.28 3/8] mmc: host: davinci: reimplement read/write fifo in C

It's easier to follow and to maintain.
Signed-off-by: Felipe Balbi <felipe.balbi@nokia.com>
Signed-off-by: Kevin Hilman <khilman@deeprootsystems.com>

drivers/mmc/host/davinci_mmc.c

@@ -62,59 +62,73 @@ static struct mmcsd_config_def mmcsd_cfg = {
 
 #define RSP_TYPE(x)	((x) & ~(MMC_RSP_BUSY|MMC_RSP_OPCODE))
 
-#define DAVINCI_MMCSD_READ_FIFO(pDst, pRegs, cnt) asm( \
-	"	cmp	%3,#16\n" \
-	"1:	ldrhs	r0,[%1,%2]\n" \
-	"	ldrhs	r1,[%1,%2]\n" \
-	"	ldrhs	r2,[%1,%2]\n" \
-	"	ldrhs	r3,[%1,%2]\n" \
-	"	stmhsia	%0!,{r0,r1,r2,r3}\n" \
-	"	beq	3f\n" \
-	"	subhs	%3,%3,#16\n" \
-	"	cmp	%3,#16\n" \
-	"	bhs	1b\n" \
-	"	tst	%3,#0x0c\n" \
-	"2:	ldrne	r0,[%1,%2]\n" \
-	"	strne	r0,[%0],#4\n" \
-	"	subne	%3,%3,#4\n" \
-	"	tst	%3,#0x0c\n" \
-	"	bne	2b\n" \
-	"	tst	%3,#2\n" \
-	"	ldrneh	r0,[%1,%2]\n" \
-	"	strneh	r0,[%0],#2\n" \
-	"	tst	%3,#1\n" \
-	"	ldrneb	r0,[%1,%2]\n" \
-	"	strneb	r0,[%0],#1\n" \
-	"3:\n" \
-	 : "+r"(pDst) : "r"(pRegs), "i"(DAVINCI_MMCDRR), \
-	 "r"(cnt) : "r0", "r1", "r2", "r3");
-
-#define DAVINCI_MMCSD_WRITE_FIFO(pDst, pRegs, cnt) asm( \
-	"	cmp	%3,#16\n" \
-	"1:	ldmhsia	%0!,{r0,r1,r2,r3}\n" \
-	"	strhs	r0,[%1,%2]\n" \
-	"	strhs	r1,[%1,%2]\n" \
-	"	strhs	r2,[%1,%2]\n" \
-	"	strhs	r3,[%1,%2]\n" \
-	"	beq	3f\n" \
-	"	subhs	%3,%3,#16\n" \
-	"	cmp	%3,#16\n" \
-	"	bhs	1b\n" \
-	"	tst	%3,#0x0c\n" \
-	"2:	ldrne	r0,[%0],#4\n" \
-	"	strne	r0,[%1,%2]\n" \
-	"	subne	%3,%3,#4\n" \
-	"	tst	%3,#0x0c\n" \
-	"	bne	2b\n" \
-	"	tst	%3,#2\n" \
-	"	ldrneh	r0,[%0],#2\n" \
-	"	strneh	r0,[%1,%2]\n" \
-	"	tst	%3,#1\n" \
-	"	ldrneb	r0,[%0],#1\n" \
-	"	strneb	r0,[%1,%2]\n" \
-	"3:\n" \
-	 : "+r"(pDst) : "r"(pRegs), "i"(DAVINCI_MMCDXR), \
-	 "r"(cnt) : "r0", "r1", "r2", "r3");
+static void davinci_mmc_read_fifo(struct mmc_davinci_host *host,
+		u16 len, u8 *dest)
+{
+	void __iomem *fifo = host->base + DAVINCI_MMCDRR;
+	u16 index = 0;
+
+	dev_dbg(mmc_dev(host->mmc), "RX fifo %p count %d buf %p\n",
+			fifo, len, dest);
+
+	if (likely((0x03 & (unsigned long) dest) == 0)) {
+		if (len >= 4) {
+			ioread32_rep(fifo, dest, len >> 2);
+			index = len & ~0x03;
+		}
+		if (len & 0x02) {
+			*(u16 *)&dest[index] = ioread16(fifo);
+			index += 2;
+		}
+		if (len & 0x01) {
+			dest[index] = ioread8(fifo);
+			index += 1;
+		}
+	} else if ((0x01 & (unsigned long) dest) == 0) {
+		if (len >= 2) {
+			ioread16_rep(fifo, dest, len >> 1);
+			index = len & ~0x01;
+		}
+		if (len & 0x01)
+			dest[index] = ioread8(fifo);
+	} else {
+		ioread8_rep(fifo, dest, len);
+	}
+}
+
+static void davinci_mmc_write_fifo(struct mmc_davinci_host *host,
+		u16 len, const u8 *src)
+{
+	void __iomem *fifo = host->base + DAVINCI_MMCDXR;
+	u16 index = 0;
+
+	dev_dbg(mmc_dev(host->mmc), "TX fifo %p count %d buf %p\n",
+			fifo, len, src);
+
+	if (likely((0x03 & (unsigned long) src) == 0)) {
+		if (len >= 4) {
+			iowrite32_rep(fifo, src + index, len >> 2);
+			index = len & ~0x03;
+		}
+		if (len & 0x02) {
+			iowrite16(*(u16 *)&src[index], fifo);
+			index += 2;
+		}
+		if (len & 0x01) {
+			iowrite8(src[index], fifo);
+			index += 1;
+		}
+	} else if ((0x01 & (unsigned long) src) == 0) {
+		if (len >= 2) {
+			iowrite16_rep(fifo, src + index, len >> 1);
+			index = len & ~0x01;
+		}
+		if (len & 0x01)
+			iowrite8(src[index], fifo);
+	} else {
+		iowrite8_rep(fifo, src, len);
+	}
+}
 
 /* PIO only */
 static void mmc_davinci_sg_to_buf(struct mmc_davinci_host *host)
@@ -144,11 +158,11 @@ static void davinci_fifo_data_trans(struct mmc_davinci_host *host, int n)
 	host->buffer_bytes_left -= n;
 	host->bytes_left -= n;
 
-	if (host->data_dir == DAVINCI_MMC_DATADIR_WRITE) {
-		DAVINCI_MMCSD_WRITE_FIFO(p, host->base, n);
-	} else {
-		DAVINCI_MMCSD_READ_FIFO(p, host->base, n);
-	}
+	if (host->data_dir == DAVINCI_MMC_DATADIR_WRITE)
+		davinci_mmc_write_fifo(host, n, p);
+	else
+		davinci_mmc_read_fifo(host, n, p);
+
 	host->buffer = p;
 }