mtd: cqspi: Simplify indirect write code

index 7786dd6..5e84144 100644
@@ -28,6 +28,7 @@
 #include <common.h>
 #include <asm/io.h>
 #include <asm/errno.h>
+#include <wait_bit.h>
 #include "cadence_qspi.h"
 
 #define CQSPI_REG_POLL_US			(1) /* 1us */

@@ -214,32 +215,6 @@ static void cadence_qspi_apb_read_fifo_data(void *dest,
 	return;
 }
 
-static void cadence_qspi_apb_write_fifo_data(const void *dest_ahb_addr,
-	const void *src, unsigned int bytes)
-{
-	unsigned int temp = 0;
-	int i;
-	int remaining = bytes;
-	unsigned int *dest_ptr = (unsigned int *)dest_ahb_addr;
-	unsigned int *src_ptr = (unsigned int *)src;
-
-	while (remaining >= CQSPI_FIFO_WIDTH) {
-		for (i = CQSPI_FIFO_WIDTH/sizeof(src_ptr) - 1; i >= 0; i--)
-			writel(*(src_ptr+i), dest_ptr+i);
-		src_ptr += CQSPI_FIFO_WIDTH/sizeof(src_ptr);
-		remaining -= CQSPI_FIFO_WIDTH;
-	}
-	if (remaining) {
-		/* dangling bytes */
-		i = remaining/sizeof(dest_ptr);
-		memcpy(&temp, src_ptr+i, remaining % sizeof(dest_ptr));
-		writel(temp, dest_ptr+i);
-		for (--i; i >= 0; i--)
-			writel(*(src_ptr+i), dest_ptr+i);
-	}
-	return;
-}
-
 /* Read from SRAM FIFO with polling SRAM fill level. */
 static int qspi_read_sram_fifo_poll(const void *reg_base, void *dest_addr,
 			const void *src_addr,  unsigned int num_bytes)

@@ -276,44 +251,6 @@ static int qspi_read_sram_fifo_poll(const void *reg_base, void *dest_addr,
 	return 0;
 }
 
-/* Write to SRAM FIFO with polling SRAM fill level. */
-static int qpsi_write_sram_fifo_push(struct cadence_spi_platdata *plat,
-				const void *src_addr, unsigned int num_bytes)
-{
-	const void *reg_base = plat->regbase;
-	void *dest_addr = plat->ahbbase;
-	unsigned int retry = CQSPI_REG_RETRY;
-	unsigned int sram_level;
-	unsigned int wr_bytes;
-	unsigned char *src = (unsigned char *)src_addr;
-	int remaining = num_bytes;
-	unsigned int page_size = plat->page_size;
-	unsigned int sram_threshold_words = CQSPI_REG_SRAM_THRESHOLD_WORDS;
-
-	while (remaining > 0) {
-		retry = CQSPI_REG_RETRY;
-		while (retry--) {
-			sram_level = CQSPI_GET_WR_SRAM_LEVEL(reg_base);
-			if (sram_level <= sram_threshold_words)
-				break;
-		}
-		if (!retry) {
-			printf("QSPI: SRAM fill level (0x%08x) not hit lower expected level (0x%08x)",
-			       sram_level, sram_threshold_words);
-			return -1;
-		}
-		/* Write a page or remaining bytes. */
-		wr_bytes = (remaining > page_size) ?
-					page_size : remaining;
-
-		cadence_qspi_apb_write_fifo_data(dest_addr, src, wr_bytes);
-		src += wr_bytes;
-		remaining -= wr_bytes;
-	}
-
-	return 0;
-}
-
 void cadence_qspi_apb_controller_enable(void *reg_base)
 {
 	unsigned int reg;

@@ -810,48 +747,45 @@ int cadence_qspi_apb_indirect_write_setup(struct cadence_spi_platdata *plat,
 }
 
 int cadence_qspi_apb_indirect_write_execute(struct cadence_spi_platdata *plat,
-	unsigned int txlen, const u8 *txbuf)
+	unsigned int n_tx, const u8 *txbuf)
 {
-	unsigned int reg = 0;
-	unsigned int retry;
+	unsigned int page_size = plat->page_size;
+	unsigned int remaining = n_tx;
+	unsigned int write_bytes;
+	int ret;
 
 	/* Configure the indirect read transfer bytes */
-	writel(txlen, plat->regbase + CQSPI_REG_INDIRECTWRBYTES);
+	writel(n_tx, plat->regbase + CQSPI_REG_INDIRECTWRBYTES);
 
 	/* Start the indirect write transfer */
 	writel(CQSPI_REG_INDIRECTWR_START_MASK,
 	       plat->regbase + CQSPI_REG_INDIRECTWR);
 
-	if (qpsi_write_sram_fifo_push(plat, (const void *)txbuf, txlen))
-		goto failwr;
-
-	/* Wait until last write is completed (FIFO empty) */
-	retry = CQSPI_REG_RETRY;
-	while (retry--) {
-		reg = CQSPI_GET_WR_SRAM_LEVEL(plat->regbase);
-		if (reg == 0)
-			break;
-
-		udelay(1);
-	}
-
-	if (reg != 0) {
-		printf("QSPI: timeout for indirect write\n");
-		goto failwr;
-	}
+	while (remaining > 0) {
+		write_bytes = remaining > page_size ? page_size : remaining;
+		/* Handle non-4-byte aligned access to avoid data abort. */
+		if (((uintptr_t)txbuf % 4) || (write_bytes % 4))
+			writesb(plat->ahbbase, txbuf, write_bytes);
+		else
+			writesl(plat->ahbbase, txbuf, write_bytes >> 2);
+
+		ret = wait_for_bit("QSPI", plat->regbase + CQSPI_REG_SDRAMLEVEL,
+				   CQSPI_REG_SDRAMLEVEL_WR_MASK <<
+				   CQSPI_REG_SDRAMLEVEL_WR_LSB, 0, 10, 0);
+		if (ret) {
+			printf("Indirect write timed out (%i)\n", ret);
+			goto failwr;
+		}
 
-	/* Check flash indirect controller status */
-	retry = CQSPI_REG_RETRY;
-	while (retry--) {
-		reg = readl(plat->regbase + CQSPI_REG_INDIRECTWR);
-		if (reg & CQSPI_REG_INDIRECTWR_DONE_MASK)
-			break;
-		udelay(1);
+		txbuf += write_bytes;
+		remaining -= write_bytes;
 	}
 
-	if (!(reg & CQSPI_REG_INDIRECTWR_DONE_MASK)) {
-		printf("QSPI: indirect completion status error with reg 0x%08x\n",
-		       reg);
+	/* Check indirect done status */
+	ret = wait_for_bit("QSPI", plat->regbase + CQSPI_REG_INDIRECTWR,
+			   CQSPI_REG_INDIRECTWR_DONE_MASK, 1, 10, 0);
+	if (ret) {
+		printf("Indirect write completion error (%i)\n", ret);
 		goto failwr;
 	}
 

@@ -864,7 +798,7 @@ failwr:
 	/* Cancel the indirect write */
 	writel(CQSPI_REG_INDIRECTWR_CANCEL_MASK,
 	       plat->regbase + CQSPI_REG_INDIRECTWR);
-	return -1;
+	return ret;
 }
 
 void cadence_qspi_apb_enter_xip(void *reg_base, char xip_dummy)

index 5e84144..a71531d 100644
@@ -193,64 +193,6 @@ static unsigned int cadence_qspi_apb_cmd2addr(const unsigned char *addr_buf,
 	return addr;
 }
 
-static void cadence_qspi_apb_read_fifo_data(void *dest,
-	const void *src_ahb_addr, unsigned int bytes)
-{
-	unsigned int temp;
-	int remaining = bytes;
-	unsigned int *dest_ptr = (unsigned int *)dest;
-	unsigned int *src_ptr = (unsigned int *)src_ahb_addr;
-
-	while (remaining >= sizeof(dest_ptr)) {
-		*dest_ptr = readl(src_ptr);
-		remaining -= sizeof(src_ptr);
-		dest_ptr++;
-	}
-	if (remaining) {
-		/* dangling bytes */
-		temp = readl(src_ptr);
-		memcpy(dest_ptr, &temp, remaining);
-	}
-
-	return;
-}
-
-/* Read from SRAM FIFO with polling SRAM fill level. */
-static int qspi_read_sram_fifo_poll(const void *reg_base, void *dest_addr,
-			const void *src_addr,  unsigned int num_bytes)
-{
-	unsigned int remaining = num_bytes;
-	unsigned int retry;
-	unsigned int sram_level = 0;
-	unsigned char *dest = (unsigned char *)dest_addr;
-
-	while (remaining > 0) {
-		retry = CQSPI_REG_RETRY;
-		while (retry--) {
-			sram_level = CQSPI_GET_RD_SRAM_LEVEL(reg_base);
-			if (sram_level)
-				break;
-			udelay(1);
-		}
-
-		if (!retry) {
-			printf("QSPI: No receive data after polling for %d times\n",
-			       CQSPI_REG_RETRY);
-			return -1;
-		}
-
-		sram_level *= CQSPI_FIFO_WIDTH;
-		sram_level = sram_level > remaining ? remaining : sram_level;
-
-		/* Read data from FIFO. */
-		cadence_qspi_apb_read_fifo_data(dest, src_addr, sram_level);
-		dest += sram_level;
-		remaining -= sram_level;
-		udelay(1);
-	}
-	return 0;
-}
-
 void cadence_qspi_apb_controller_enable(void *reg_base)
 {
 	unsigned int reg;

@@ -679,40 +621,84 @@ int cadence_qspi_apb_indirect_read_setup(struct cadence_spi_platdata *plat,
 	return 0;
 }
 
+static u32 cadence_qspi_get_rd_sram_level(struct cadence_spi_platdata *plat)
+{
+	u32 reg = readl(plat->regbase + CQSPI_REG_SDRAMLEVEL);
+	reg >>= CQSPI_REG_SDRAMLEVEL_RD_LSB;
+	return reg & CQSPI_REG_SDRAMLEVEL_RD_MASK;
+}
+
+static int cadence_qspi_wait_for_data(struct cadence_spi_platdata *plat)
+{
+	unsigned int timeout = 10000;
+	u32 reg;
+
+	while (timeout--) {
+		reg = cadence_qspi_get_rd_sram_level(plat);
+		if (reg)
+			return reg;
+		udelay(1);
+	}
+
+	return -ETIMEDOUT;
+}
+
 int cadence_qspi_apb_indirect_read_execute(struct cadence_spi_platdata *plat,
-	unsigned int rxlen, u8 *rxbuf)
+	unsigned int n_rx, u8 *rxbuf)
 {
-	unsigned int reg;
+	unsigned int remaining = n_rx;
+	unsigned int bytes_to_read = 0;
+	int ret;
 
-	writel(rxlen, plat->regbase + CQSPI_REG_INDIRECTRDBYTES);
+	writel(n_rx, plat->regbase + CQSPI_REG_INDIRECTRDBYTES);
 
 	/* Start the indirect read transfer */
 	writel(CQSPI_REG_INDIRECTRD_START_MASK,
 	       plat->regbase + CQSPI_REG_INDIRECTRD);
 
-	if (qspi_read_sram_fifo_poll(plat->regbase, (void *)rxbuf,
-				     (const void *)plat->ahbbase, rxlen))
-		goto failrd;
+	while (remaining > 0) {
+		ret = cadence_qspi_wait_for_data(plat);
+		if (ret < 0) {
+			printf("Indirect write timed out (%i)\n", ret);
+			goto failrd;
+		}
+
+		bytes_to_read = ret;
+
+		while (bytes_to_read != 0) {
+			bytes_to_read *= CQSPI_FIFO_WIDTH;
+			bytes_to_read = bytes_to_read > remaining ?
+					remaining : bytes_to_read;
+			/* Handle non-4-byte aligned access to avoid data abort. */
+			if (((uintptr_t)rxbuf % 4) || (bytes_to_read % 4))
+				readsb(plat->ahbbase, rxbuf, bytes_to_read);
+			else
+				readsl(plat->ahbbase, rxbuf, bytes_to_read >> 2);
+			rxbuf += bytes_to_read;
+			remaining -= bytes_to_read;
+			bytes_to_read = cadence_qspi_get_rd_sram_level(plat);
+		}
+	}
 
-	/* Check flash indirect controller */
-	reg = readl(plat->regbase + CQSPI_REG_INDIRECTRD);
-	if (!(reg & CQSPI_REG_INDIRECTRD_DONE_MASK)) {
-		reg = readl(plat->regbase + CQSPI_REG_INDIRECTRD);
-		printf("QSPI: indirect completion status error with reg 0x%08x\n",
-		       reg);
+	/* Check indirect done status */
+	ret = wait_for_bit("QSPI", plat->regbase + CQSPI_REG_INDIRECTRD,
+			   CQSPI_REG_INDIRECTRD_DONE_MASK, 1, 10, 0);
+	if (ret) {
+		printf("Indirect read completion error (%i)\n", ret);
 		goto failrd;
 	}
 
 	/* Clear indirect completion status */
 	writel(CQSPI_REG_INDIRECTRD_DONE_MASK,
 	       plat->regbase + CQSPI_REG_INDIRECTRD);
+
 	return 0;
 
 failrd:
 	/* Cancel the indirect read */
 	writel(CQSPI_REG_INDIRECTRD_CANCEL_MASK,
 	       plat->regbase + CQSPI_REG_INDIRECTRD);
-	return -1;
+	return ret;
 }
 
 /* Opcode + Address (3/4 bytes) */