1 files changed, 20 insertions, 25 deletions

diff --git a/drivers/mtd/nand/mxc_nand.c b/drivers/mtd/nand/mxc_nand.c
index 67ddc35ca7bc..1131ad126fd2 100644
--- a/drivers/mtd/nand/mxc_nand.c
+++ b/drivers/mtd/nand/mxc_nand.c
@@ -228,16 +228,25 @@ static void send_addr(struct mxc_nand_host *host, uint16_t addr, int islast)
 	wait_op_done(host, TROP_US_DELAY, islast);
 }
 
-static void send_page(struct mxc_nand_host *host, uint8_t buf_id,
-		unsigned int ops)
+static void send_page(struct mxc_nand_host *host, unsigned int ops)
 {
-	/* NANDFC buffer 0 is used for page read/write */
-	writew(buf_id, host->regs + NFC_BUF_ADDR);
+	int bufs, i;
 
-	writew(ops, host->regs + NFC_CONFIG2);
+	if (host->pagesize_2k)
+		bufs = 4;
+	else
+		bufs = 1;
 
-	/* Wait for operation to complete */
-	wait_op_done(host, TROP_US_DELAY, true);
+	for (i = 0; i < bufs; i++) {
+
+		/* NANDFC buffer 0 is used for page read/write */
+		writew(i, host->regs + NFC_BUF_ADDR);
+
+		writew(ops, host->regs + NFC_CONFIG2);
+
+		/* Wait for operation to complete */
+		wait_op_done(host, TROP_US_DELAY, true);
+	}
 }
 
 /* Request the NANDFC to perform a read of the NAND device ID. */
@@ -565,16 +574,10 @@ static void mxc_nand_command(struct mtd_info *mtd, unsigned command,
 		send_cmd(host, command, false);
 		mxc_do_addr_cycle(mtd, column, page_addr);
 
-		if (host->pagesize_2k) {
-			/* send read confirm command */
+		if (host->pagesize_2k)
 			send_cmd(host, NAND_CMD_READSTART, true);
-			/* read for each AREA */
-			send_page(host, 0, NFC_OUTPUT);
-			send_page(host, 1, NFC_OUTPUT);
-			send_page(host, 2, NFC_OUTPUT);
-			send_page(host, 3, NFC_OUTPUT);
-		} else
-			send_page(host, 0, NFC_OUTPUT);
+
+		send_page(host, NFC_OUTPUT);
 
 		memcpy(host->data_buf, host->regs + MAIN_AREA0, mtd->writesize);
 		copy_spare(mtd, true);
@@ -614,15 +617,7 @@ static void mxc_nand_command(struct mtd_info *mtd, unsigned command,
 	case NAND_CMD_PAGEPROG:
 		memcpy(host->regs + MAIN_AREA0, host->data_buf, mtd->writesize);
 		copy_spare(mtd, false);
-		send_page(host, 0, NFC_INPUT);
-
-		if (host->pagesize_2k) {
-			/* data in 4 areas datas */
-			send_page(host, 1, NFC_INPUT);
-			send_page(host, 2, NFC_INPUT);
-			send_page(host, 3, NFC_INPUT);
-		}
-
+		send_page(host, NFC_INPUT);
 		send_cmd(host, command, true);
 		mxc_do_addr_cycle(mtd, column, page_addr);
 		break;