diff --git a/drivers/mtd/devices/m25p80.c b/drivers/mtd/devices/m25p80.c
index 2f3d2a5..3572513 100644
--- a/drivers/mtd/devices/m25p80.c
+++ b/drivers/mtd/devices/m25p80.c
@@ -41,12 +41,16 @@
 #define	OPCODE_WRSR		0x01	/* Write status register 1 byte */
 #define	OPCODE_NORM_READ	0x03	/* Read data bytes (low frequency) */
 #define	OPCODE_FAST_READ	0x0b	/* Read data bytes (high frequency) */
+#define OPCODE_QUAD_READ	0x6b	/* Quad read command */
 #define	OPCODE_PP		0x02	/* Page program (up to 256 bytes) */
+#define OPCODE_QPP		0x32	/* Quad page program */
 #define	OPCODE_BE_4K		0x20	/* Erase 4KiB block */
 #define	OPCODE_BE_32K		0x52	/* Erase 32KiB block */
 #define	OPCODE_CHIP_ERASE	0xc7	/* Erase whole flash chip */
 #define	OPCODE_SE		0xd8	/* Sector erase (usually 64KiB) */
 #define	OPCODE_RDID		0x9f	/* Read JEDEC ID */
+#define OPCODE_RDFSR		0x70	/* Read Flag Status Register */
+#define OPCODE_WREAR		0xc5	/* Write Extended Address Register */
 
 /* Used for SST flashes only. */
 #define	OPCODE_BP		0x02	/* Byte program */
@@ -59,6 +63,7 @@
 
 /* Used for Spansion flashes only. */
 #define	OPCODE_BRWR		0x17	/* Bank register write */
+#define	OPCODE_BRRD		0x16	/* Bank register read */
 
 /* Status Register bits. */
 #define	SR_WIP			1	/* Write in progress */
@@ -69,8 +74,11 @@
 #define	SR_BP2			0x10	/* Block protect 2 */
 #define	SR_SRWD			0x80	/* SR write protect */
 
+/* Flag Status Register bits. */
+#define FSR_RDY			0x80	/* Ready/Busy program erase
+					controller */
 /* Define max times to check status register before we give up. */
-#define	MAX_READY_WAIT_JIFFIES	(40 * HZ)	/* M25P16 specs 40s max chip erase */
+#define	MAX_READY_WAIT_JIFFIES	(480 * HZ) /* N25Q specs 480s max chip erase */
 #define	MAX_CMD_SIZE		5
 
 #define JEDEC_MFR(_jedec_id)	((_jedec_id) >> 16)
@@ -86,6 +94,15 @@ struct m25p {
 	u8			erase_opcode;
 	u8			*command;
 	bool			fast_read;
+	u16			curbank;
+	u32			jedec_id;
+	bool			check_fsr;
+	bool			shift;
+	bool			isparallel;
+	bool			isstacked;
+	u8			read_opcode;
+	u8			prog_opcode;
+	u8			dummycount;
 };
 
 static inline struct m25p *mtd_to_m25p(struct mtd_info *mtd)
@@ -100,21 +117,19 @@ static inline struct m25p *mtd_to_m25p(struct mtd_info *mtd)
  */
 
 /*
- * Read the status register, returning its value in the location
- * Return the status register value.
+ * Read register, returning its value in the location
  * Returns negative if error occurred.
  */
-static int read_sr(struct m25p *flash)
+static inline int read_spi_reg(struct m25p *flash, u8 code, const char *name)
 {
 	ssize_t retval;
-	u8 code = OPCODE_RDSR;
 	u8 val;
 
 	retval = spi_write_then_read(flash->spi, &code, 1, &val, 1);
 
 	if (retval < 0) {
-		dev_err(&flash->spi->dev, "error %d reading SR\n",
-				(int) retval);
+		dev_err(&flash->spi->dev, "error %d reading %s\n",
+				(int) retval, name);
 		return retval;
 	}
 
@@ -122,6 +137,26 @@ static int read_sr(struct m25p *flash)
 }
 
 /*
+ * Read flag status register, returning its value in the location
+ * Return flag status register value.
+ * Returns negative if error occurred.
+ */
+static int read_fsr(struct m25p *flash)
+{
+	return read_spi_reg(flash, OPCODE_RDFSR, "FSR");
+}
+
+/*
+ * Read the status register, returning its value in the location
+ * Return the status register value.
+ * Returns negative if error occurred.
+ */
+static int read_sr(struct m25p *flash)
+{
+	return read_spi_reg(flash, OPCODE_RDSR, "SR");
+}
+
+/*
  * Write status register 1 byte
  * Returns negative if error occurred.
  */
@@ -159,6 +194,9 @@ static inline int write_disable(struct m25p *flash)
  */
 static inline int set_4byte(struct m25p *flash, u32 jedec_id, int enable)
 {
+	int ret;
+	u8 val;
+
 	switch (JEDEC_MFR(jedec_id)) {
 	case CFI_MFR_MACRONIX:
 	case 0xEF /* winbond */:
@@ -168,7 +206,19 @@ static inline int set_4byte(struct m25p *flash, u32 jedec_id, int enable)
 		/* Spansion style */
 		flash->command[0] = OPCODE_BRWR;
 		flash->command[1] = enable << 7;
-		return spi_write(flash->spi, flash->command, 2);
+		ret = spi_write(flash->spi, flash->command, 2);
+
+		/* verify the 4 byte mode is enabled */
+		flash->command[0] = OPCODE_BRRD;
+		spi_write_then_read(flash->spi, flash->command, 1, &val, 1);
+		if (val != enable << 7) {
+			dev_warn(&flash->spi->dev,
+				 "fallback to 3-byte address mode\n");
+			dev_warn(&flash->spi->dev,
+				 "maximum accessible size is 16MB\n");
+			flash->addr_width = 3;
+		}
+		return ret;
 	}
 }
 
@@ -179,15 +229,21 @@ static inline int set_4byte(struct m25p *flash, u32 jedec_id, int enable)
 static int wait_till_ready(struct m25p *flash)
 {
 	unsigned long deadline;
-	int sr;
+	int sr, fsr;
 
 	deadline = jiffies + MAX_READY_WAIT_JIFFIES;
 
 	do {
 		if ((sr = read_sr(flash)) < 0)
 			break;
-		else if (!(sr & SR_WIP))
+		else if (!(sr & SR_WIP)) {
+			if (flash->check_fsr) {
+				fsr = read_fsr(flash);
+				if (!(fsr & FSR_RDY))
+					return 1;
+			}
 			return 0;
+		}
 
 		cond_resched();
 
@@ -197,6 +253,48 @@ static int wait_till_ready(struct m25p *flash)
 }
 
 /*
+ * Update Extended Address/bank selection Register.
+ * Call with flash->lock locked.
+ */
+static int write_ear(struct m25p *flash, u32 addr)
+{
+	u8 ear;
+	int ret;
+
+	/* Wait until finished previous write command. */
+	if (wait_till_ready(flash))
+		return 1;
+
+	if (flash->mtd.size <= (0x1000000) << flash->shift)
+		return 0;
+
+	addr = addr % (u32) flash->mtd.size;
+	ear = addr >> 24;
+
+	if ((!flash->isstacked) && (ear == flash->curbank))
+		return 0;
+
+	if (flash->isstacked && (flash->mtd.size <= 0x2000000))
+		return 0;
+
+	if (JEDEC_MFR(flash->jedec_id) == 0x01)
+		flash->command[0] = OPCODE_BRWR;
+	if (JEDEC_MFR(flash->jedec_id) == 0x20) {
+		write_enable(flash);
+		flash->command[0] = OPCODE_WREAR;
+	}
+	flash->command[1] = ear;
+
+	ret = spi_write(flash->spi, flash->command, 2);
+	if (ret)
+		return ret;
+
+	flash->curbank = ear;
+
+	return 0;
+}
+
+/*
  * Erase the whole flash memory
  *
  * Returns 0 if successful, non-zero otherwise.
@@ -210,6 +308,9 @@ static int erase_chip(struct m25p *flash)
 	if (wait_till_ready(flash))
 		return 1;
 
+	if (flash->isstacked)
+		flash->spi->master->flags &= ~SPI_MASTER_U_PAGE;
+
 	/* Send write enable, then erase commands. */
 	write_enable(flash);
 
@@ -218,16 +319,32 @@ static int erase_chip(struct m25p *flash)
 
 	spi_write(flash->spi, flash->command, 1);
 
+	if (flash->isstacked) {
+		/* Wait until finished previous write command. */
+		if (wait_till_ready(flash))
+			return 1;
+
+		flash->spi->master->flags |= SPI_MASTER_U_PAGE;
+
+		/* Send write enable, then erase commands. */
+		write_enable(flash);
+
+		/* Set up command buffer. */
+		flash->command[0] = OPCODE_CHIP_ERASE;
+
+		spi_write(flash->spi, flash->command, 1);
+	}
+
 	return 0;
 }
 
 static void m25p_addr2cmd(struct m25p *flash, unsigned int addr, u8 *cmd)
 {
+	int i;
+
 	/* opcode is in cmd[0] */
-	cmd[1] = addr >> (flash->addr_width * 8 -  8);
-	cmd[2] = addr >> (flash->addr_width * 8 - 16);
-	cmd[3] = addr >> (flash->addr_width * 8 - 24);
-	cmd[4] = addr >> (flash->addr_width * 8 - 32);
+	for (i = 1; i <= flash->addr_width; i++)
+		cmd[i] = addr >> (flash->addr_width * 8 - i * 8);
 }
 
 static int m25p_cmdsz(struct m25p *flash)
@@ -250,6 +367,10 @@ static int erase_sector(struct m25p *flash, u32 offset)
 	if (wait_till_ready(flash))
 		return 1;
 
+	/* update Extended Address Register */
+	if (write_ear(flash, offset))
+		return 1;
+
 	/* Send write enable, then erase commands. */
 	write_enable(flash);
 
@@ -275,7 +396,7 @@ static int erase_sector(struct m25p *flash, u32 offset)
 static int m25p80_erase(struct mtd_info *mtd, struct erase_info *instr)
 {
 	struct m25p *flash = mtd_to_m25p(mtd);
-	u32 addr,len;
+	u32 addr, len, offset;
 	uint32_t rem;
 
 	pr_debug("%s: %s at 0x%llx, len %lld\n", dev_name(&flash->spi->dev),
@@ -307,7 +428,19 @@ static int m25p80_erase(struct mtd_info *mtd, struct erase_info *instr)
 	/* "sector"-at-a-time erase */
 	} else {
 		while (len) {
-			if (erase_sector(flash, addr)) {
+			offset = addr;
+			if (flash->isparallel == 1)
+				offset /= 2;
+			if (flash->isstacked == 1) {
+				if (offset >= (flash->mtd.size / 2)) {
+					offset = offset - (flash->mtd.size / 2);
+					flash->spi->master->flags |=
+							SPI_MASTER_U_PAGE;
+				} else
+					flash->spi->master->flags &=
+							~SPI_MASTER_U_PAGE;
+			}
+			if (erase_sector(flash, offset)) {
 				instr->state = MTD_ERASE_FAILED;
 				mutex_unlock(&flash->lock);
 				return -EIO;
@@ -336,7 +469,6 @@ static int m25p80_read(struct mtd_info *mtd, loff_t from, size_t len,
 	struct m25p *flash = mtd_to_m25p(mtd);
 	struct spi_transfer t[2];
 	struct spi_message m;
-	uint8_t opcode;
 
 	pr_debug("%s: %s from 0x%08x, len %zd\n", dev_name(&flash->spi->dev),
 			__func__, (u32)from, len);
@@ -349,21 +481,17 @@ static int m25p80_read(struct mtd_info *mtd, loff_t from, size_t len,
 	 * Should add 1 byte DUMMY_BYTE.
 	 */
 	t[0].tx_buf = flash->command;
-	t[0].len = m25p_cmdsz(flash) + (flash->fast_read ? 1 : 0);
+	t[0].len = m25p_cmdsz(flash) + flash->dummycount;
 	spi_message_add_tail(&t[0], &m);
 
 	t[1].rx_buf = buf;
 	t[1].len = len;
 	spi_message_add_tail(&t[1], &m);
 
-	mutex_lock(&flash->lock);
-
 	/* Wait till previous write/erase is done. */
-	if (wait_till_ready(flash)) {
+	if (wait_till_ready(flash))
 		/* REVISIT status return?? */
-		mutex_unlock(&flash->lock);
 		return 1;
-	}
 
 	/* FIXME switch to OPCODE_FAST_READ.  It's required for higher
 	 * clocks; and at this writing, every chip this driver handles
@@ -371,17 +499,65 @@ static int m25p80_read(struct mtd_info *mtd, loff_t from, size_t len,
 	 */
 
 	/* Set up the write data buffer. */
-	opcode = flash->fast_read ? OPCODE_FAST_READ : OPCODE_NORM_READ;
-	flash->command[0] = opcode;
+	flash->command[0] = flash->read_opcode;
 	m25p_addr2cmd(flash, from, flash->command);
 
 	spi_sync(flash->spi, &m);
 
 	*retlen = m.actual_length - m25p_cmdsz(flash) -
-			(flash->fast_read ? 1 : 0);
+			flash->dummycount;
 
-	mutex_unlock(&flash->lock);
+	return 0;
+}
+
+static int m25p80_read_ext(struct mtd_info *mtd, loff_t from, size_t len,
+	size_t *retlen, u_char *buf)
+{
+	struct m25p *flash = mtd_to_m25p(mtd);
+	u32 addr = from;
+	u32 offset = from;
+	u32 read_len = 0;
+	u32 actual_len = 0;
+	u32 read_count = 0;
+	u32 rem_bank_len = 0;
+	u8 bank = 0;
+
+#define OFFSET_16_MB 0x1000000
+
+	mutex_lock(&flash->lock);
+
+	while (len) {
+		bank = addr / (OFFSET_16_MB << flash->shift);
+		rem_bank_len = ((OFFSET_16_MB << flash->shift) * (bank + 1)) -
+				addr;
+		offset = addr;
+		if (flash->isparallel == 1)
+			offset /= 2;
+		if (flash->isstacked == 1) {
+			if (offset >= (flash->mtd.size / 2)) {
+				offset = offset - (flash->mtd.size / 2);
+				flash->spi->master->flags |= SPI_MASTER_U_PAGE;
+			} else {
+				flash->spi->master->flags &= ~SPI_MASTER_U_PAGE;
+			}
+		}
+		write_ear(flash, offset);
+		if (len < rem_bank_len)
+			read_len = len;
+		else
+			read_len = rem_bank_len;
+
+		m25p80_read(mtd, offset, read_len, &actual_len, buf);
 
+		addr += actual_len;
+		len -= actual_len;
+		buf += actual_len;
+		read_count += actual_len;
+	}
+
+	*retlen = read_count;
+
+	mutex_unlock(&flash->lock);
 	return 0;
 }
 
@@ -411,19 +587,15 @@ static int m25p80_write(struct mtd_info *mtd, loff_t to, size_t len,
 	t[1].tx_buf = buf;
 	spi_message_add_tail(&t[1], &m);
 
-	mutex_lock(&flash->lock);
-
 	/* Wait until finished previous write command. */
-	if (wait_till_ready(flash)) {
-		mutex_unlock(&flash->lock);
+	if (wait_till_ready(flash))
 		return 1;
-	}
 
 	write_enable(flash);
 
 	/* Set up the opcode in the write buffer. */
-	flash->command[0] = OPCODE_PP;
-	m25p_addr2cmd(flash, to, flash->command);
+	flash->command[0] = flash->prog_opcode;
+	m25p_addr2cmd(flash, (to >> flash->shift), flash->command);
 
 	page_offset = to & (flash->page_size - 1);
 
@@ -452,12 +624,14 @@ static int m25p80_write(struct mtd_info *mtd, loff_t to, size_t len,
 				page_size = flash->page_size;
 
 			/* write the next page to flash */
-			m25p_addr2cmd(flash, to + i, flash->command);
+			m25p_addr2cmd(flash, ((to + i) >> flash->shift),
+					flash->command);
 
 			t[1].tx_buf = buf + i;
 			t[1].len = page_size;
 
-			wait_till_ready(flash);
+			if (wait_till_ready(flash))
+				return 1;
 
 			write_enable(flash);
 
@@ -467,8 +641,55 @@ static int m25p80_write(struct mtd_info *mtd, loff_t to, size_t len,
 		}
 	}
 
-	mutex_unlock(&flash->lock);
+	return 0;
+}
+
+static int m25p80_write_ext(struct mtd_info *mtd, loff_t to, size_t len,
+	size_t *retlen, const u_char *buf)
+{
+	struct m25p *flash = mtd_to_m25p(mtd);
+	u32 addr = to;
+	u32 offset = to;
+	u32 write_len = 0;
+	u32 actual_len = 0;
+	u32 write_count = 0;
+	u32 rem_bank_len = 0;
+	u8 bank = 0;
+
+#define OFFSET_16_MB 0x1000000
+
+	mutex_lock(&flash->lock);
+	while (len) {
+		bank = addr / (OFFSET_16_MB << flash->shift);
+		rem_bank_len = ((OFFSET_16_MB << flash->shift) * (bank + 1)) -
+				addr;
+		offset = addr;
+
+		if (flash->isstacked == 1) {
+			if (offset >= (flash->mtd.size / 2)) {
+				offset = offset - (flash->mtd.size / 2);
+				flash->spi->master->flags |= SPI_MASTER_U_PAGE;
+			} else {
+				flash->spi->master->flags &= ~SPI_MASTER_U_PAGE;
+			}
+		}
+		write_ear(flash, (offset >> flash->shift));
+		if (len < rem_bank_len)
+			write_len = len;
+		else
+			write_len = rem_bank_len;
 
+		m25p80_write(mtd, offset, write_len, &actual_len, buf);
+
+		addr += actual_len;
+		len -= actual_len;
+		buf += actual_len;
+		write_count += actual_len;
+	}
+
+	*retlen = write_count;
+
+	mutex_unlock(&flash->lock);
 	return 0;
 }
 
@@ -682,6 +903,8 @@ struct flash_info {
 #define	SECT_4K		0x01		/* OPCODE_BE_4K works uniformly */
 #define	M25P_NO_ERASE	0x02		/* No erase command needed */
 #define	SST_WRITE	0x04		/* use SST byte programming */
+#define	SECT_32K	0x10		/* OPCODE_BE_32K */
+#define E_FSR		0x08		/* Flag SR exists for flash */
 };
 
 #define INFO(_jedec_id, _ext_id, _sector_size, _n_sectors, _flags)	\
@@ -761,6 +984,15 @@ static const struct spi_device_id m25p_ids[] = {
 	{ "n25q128a11",  INFO(0x20bb18, 0, 64 * 1024, 256, 0) },
 	{ "n25q128a13",  INFO(0x20ba18, 0, 64 * 1024, 256, 0) },
 	{ "n25q256a", INFO(0x20ba19, 0, 64 * 1024, 512, SECT_4K) },
+	/* Numonyx flash n25q128 - FIXME check the name */
+	{ "n25q128",   INFO(0x20bb18, 0, 64 * 1024, 256, 0) },
+	{ "n25q128a11",  INFO(0x20bb18, 0, 64 * 1024, 256, E_FSR) },
+	{ "n25q128a13",  INFO(0x20ba18, 0, 64 * 1024, 256, E_FSR) },
+	{ "n25q256a13", INFO(0x20ba19,  0, 64 * 1024,  512, SECT_4K | E_FSR) },
+	{ "n25q256a11", INFO(0x20bb19,  0, 64 * 1024,  512, SECT_4K | E_FSR) },
+	{ "n25q512a13", INFO(0x20ba20,  0, 64 * 1024,  1024, SECT_4K | E_FSR) },
+	{ "n25q512a11", INFO(0x20bb20,  0, 64 * 1024,  1024, SECT_4K | E_FSR) },
+	{ "n25q00aa13", INFO(0x20ba21,  0, 64 * 1024,  2048, SECT_4K | E_FSR) },
 
 	/* Spansion -- single (large) sector size only, at least
 	 * for the chips listed here (without boot sectors).
@@ -781,7 +1013,11 @@ static const struct spi_device_id m25p_ids[] = {
 	{ "s25sl032a",  INFO(0x010215,      0,  64 * 1024,  64, 0) },
 	{ "s25sl064a",  INFO(0x010216,      0,  64 * 1024, 128, 0) },
 	{ "s25fl016k",  INFO(0xef4015,      0,  64 * 1024,  32, SECT_4K) },
-	{ "s25fl064k",  INFO(0xef4017,      0,  64 * 1024, 128, SECT_4K) },
+	/* s25fl064k supports 4KiB, 32KiB and 64KiB sectors erase size. */
+	/* To support JFFS2, the minimum erase size is 8KiB(>4KiB). */
+	/* And thus, the sector size of s25fl064k is set to 32KiB for */
+	/* JFFS2 support. */
+	{ "s25fl064k",  INFO(0xef4017,      0,  64 * 1024, 128, SECT_32K) },
 
 	/* SST -- large erase sizes are "overlays", "sectors" are 4K */
 	{ "sst25vf040b", INFO(0xbf258d, 0, 64 * 1024,  8, SECT_4K | SST_WRITE) },
@@ -789,10 +1025,11 @@ static const struct spi_device_id m25p_ids[] = {
 	{ "sst25vf016b", INFO(0xbf2541, 0, 64 * 1024, 32, SECT_4K | SST_WRITE) },
 	{ "sst25vf032b", INFO(0xbf254a, 0, 64 * 1024, 64, SECT_4K | SST_WRITE) },
 	{ "sst25vf064c", INFO(0xbf254b, 0, 64 * 1024, 128, SECT_4K) },
-	{ "sst25wf512",  INFO(0xbf2501, 0, 64 * 1024,  1, SECT_4K | SST_WRITE) },
-	{ "sst25wf010",  INFO(0xbf2502, 0, 64 * 1024,  2, SECT_4K | SST_WRITE) },
-	{ "sst25wf020",  INFO(0xbf2503, 0, 64 * 1024,  4, SECT_4K | SST_WRITE) },
-	{ "sst25wf040",  INFO(0xbf2504, 0, 64 * 1024,  8, SECT_4K | SST_WRITE) },
+	{ "sst25wf512", INFO(0xbf2501, 0, 64 * 1024,  1, SECT_4K | SST_WRITE) },
+	{ "sst25wf010", INFO(0xbf2502, 0, 64 * 1024,  2, SECT_4K | SST_WRITE) },
+	{ "sst25wf020", INFO(0xbf2503, 0, 64 * 1024,  4, SECT_4K | SST_WRITE) },
+	{ "sst25wf040", INFO(0xbf2504, 0, 64 * 1024,  8, SECT_4K | SST_WRITE) },
+	{ "sst25wf080", INFO(0xbf2505, 0, 64 * 1024, 16, SECT_4K | SST_WRITE) },
 
 	/* ST Microelectronics -- newer production may have feature updates */
 	{ "m25p05",  INFO(0x202010,  0,  32 * 1024,   2, 0) },
@@ -839,7 +1076,12 @@ static const struct spi_device_id m25p_ids[] = {
 	{ "w25q32", INFO(0xef4016, 0, 64 * 1024,  64, SECT_4K) },
 	{ "w25q32dw", INFO(0xef6016, 0, 64 * 1024,  64, SECT_4K) },
 	{ "w25x64", INFO(0xef3017, 0, 64 * 1024, 128, SECT_4K) },
-	{ "w25q64", INFO(0xef4017, 0, 64 * 1024, 128, SECT_4K) },
+	/* Winbond -- w25q "blocks" are 64K, "sectors" are 32KiB */
+	/* w25q64 supports 4KiB, 32KiB and 64KiB sectors erase size. */
+	/* To support JFFS2, the minimum erase size is 8KiB(>4KiB). */
+	/* And thus, the sector size of w25q64 is set to 32KiB for */
+	/* JFFS2 support. */
+	{ "w25q64", INFO(0xef4017, 0, 64 * 1024, 128, SECT_32K) },
 	{ "w25q80", INFO(0xef5014, 0, 64 * 1024,  16, SECT_4K) },
 	{ "w25q80bl", INFO(0xef4014, 0, 64 * 1024,  16, SECT_4K) },
 	{ "w25q128", INFO(0xef4018, 0, 64 * 1024, 256, SECT_4K) },
@@ -995,8 +1237,54 @@ static int m25p_probe(struct spi_device *spi)
 	flash->mtd.writesize = 1;
 	flash->mtd.flags = MTD_CAP_NORFLASH;
 	flash->mtd.size = info->sector_size * info->n_sectors;
+
+	{
+#ifdef CONFIG_OF
+		const char *comp_str;
+		u32 is_dual;
+		np = of_get_next_parent(spi->dev.of_node);
+		of_property_read_string(np, "compatible", &comp_str);
+		if (!strcmp(comp_str, "xlnx,ps7-qspi-1.00.a")) {
+			if (of_property_read_u32(np, "is-dual", &is_dual) < 0) {
+				/* Default to single if prop not defined */
+				flash->shift = 0;
+				flash->isstacked = 0;
+				flash->isparallel = 0;
+			} else {
+				if (is_dual == 1) {
+					/* dual parallel */
+					flash->shift = 1;
+					info->sector_size <<= flash->shift;
+					info->page_size <<= flash->shift;
+					flash->mtd.size <<= flash->shift;
+					flash->isparallel = 1;
+					flash->isstacked = 0;
+				} else {
+#ifdef CONFIG_SPI_XILINX_PS_QSPI_DUAL_STACKED
+					/* dual stacked */
+					flash->shift = 0;
+					flash->mtd.size <<= 1;
+					flash->isstacked = 1;
+					flash->isparallel = 0;
+#else
+					/* single */
+					flash->shift = 0;
+					flash->isstacked = 0;
+					flash->isparallel = 0;
+#endif
+				}
+			}
+		}
+#else
+		/* Default to single */
+		flash->shift = 0;
+		flash->isstacked = 0;
+		flash->isparallel = 0;
+#endif
+	}
+
 	flash->mtd._erase = m25p80_erase;
-	flash->mtd._read = m25p80_read;
+	flash->mtd._read = m25p80_read_ext;
 
 	/* flash protection support for STmicro chips */
 	if (JEDEC_MFR(info->jedec_id) == CFI_MFR_ST) {
@@ -1008,20 +1296,31 @@ static int m25p_probe(struct spi_device *spi)
 	if (info->flags & SST_WRITE)
 		flash->mtd._write = sst_write;
 	else
-		flash->mtd._write = m25p80_write;
+		flash->mtd._write = m25p80_write_ext;
 
 	/* prefer "small sector" erase if possible */
 	if (info->flags & SECT_4K) {
 		flash->erase_opcode = OPCODE_BE_4K;
-		flash->mtd.erasesize = 4096;
+		flash->mtd.erasesize = 4096 << flash->shift;
+	} else if (info->flags & SECT_32K) {
+		flash->erase_opcode = OPCODE_BE_32K;
+		flash->mtd.erasesize = 32768 << flash->shift;
 	} else {
 		flash->erase_opcode = OPCODE_SE;
 		flash->mtd.erasesize = info->sector_size;
 	}
 
+	flash->read_opcode = OPCODE_NORM_READ;
+	flash->prog_opcode = OPCODE_PP;
+	flash->dummycount = 0;
+
 	if (info->flags & M25P_NO_ERASE)
 		flash->mtd.flags |= MTD_NO_ERASE;
 
+	if (info->flags & E_FSR)
+		flash->check_fsr = 1;
+
+	flash->jedec_id = info->jedec_id;
 	ppdata.of_node = spi->dev.of_node;
 	flash->mtd.dev.parent = &spi->dev;
 	flash->page_size = info->page_size;
@@ -1036,14 +1335,36 @@ static int m25p_probe(struct spi_device *spi)
 #ifdef CONFIG_M25PXX_USE_FAST_READ
 	flash->fast_read = true;
 #endif
+	if (flash->fast_read) {
+		flash->read_opcode = OPCODE_FAST_READ;
+		flash->dummycount = 1;
+	}
+
+	if (spi->master->flags & SPI_MASTER_QUAD_MODE) {
+		flash->read_opcode = OPCODE_QUAD_READ;
+		flash->prog_opcode = OPCODE_QPP;
+		flash->dummycount = 1;
+	}
 
 	if (info->addr_width)
 		flash->addr_width = info->addr_width;
 	else {
 		/* enable 4-byte addressing if the device exceeds 16MiB */
 		if (flash->mtd.size > 0x1000000) {
-			flash->addr_width = 4;
-			set_4byte(flash, info->jedec_id, 1);
+#ifdef CONFIG_OF
+			const char *comp_str;
+			np = of_get_next_parent(spi->dev.of_node);
+			of_property_read_string(np, "compatible", &comp_str);
+			if (!strcmp(comp_str, "xlnx,ps7-qspi-1.00.a")) {
+				flash->addr_width = 3;
+				set_4byte(flash, info->jedec_id, 0);
+			} else {
+#endif
+				flash->addr_width = 4;
+				set_4byte(flash, info->jedec_id, 1);
+#ifdef CONFIG_OF
+			}
+#endif
 		} else
 			flash->addr_width = 3;
 	}
diff --git a/drivers/mtd/nand/Kconfig b/drivers/mtd/nand/Kconfig
index 50543f1..74da5e3 100644
--- a/drivers/mtd/nand/Kconfig
+++ b/drivers/mtd/nand/Kconfig
@@ -523,6 +523,13 @@ config MTD_NAND_NUC900
 	  This enables the driver for the NAND Flash on evaluation board based
 	  on w90p910 / NUC9xx.
 
+config MTD_NAND_XILINX_PS
+	tristate "Xilinx Zynq NAND flash driver"
+	depends on MTD_NAND && ARCH_ZYNQ
+	select ZYNQ_SMC
+	help
+	  This enables access to the NAND flash device on Xilinx Zynq.
+
 config MTD_NAND_JZ4740
 	tristate "Support for JZ4740 SoC NAND controller"
 	depends on MACH_JZ4740
diff --git a/drivers/mtd/nand/Makefile b/drivers/mtd/nand/Makefile
index bb81891..6c87f7b 100644
--- a/drivers/mtd/nand/Makefile
+++ b/drivers/mtd/nand/Makefile
@@ -44,6 +44,7 @@ obj-$(CONFIG_MTD_NAND_MXC)		+= mxc_nand.o
 obj-$(CONFIG_MTD_NAND_SOCRATES)		+= socrates_nand.o
 obj-$(CONFIG_MTD_NAND_TXX9NDFMC)	+= txx9ndfmc.o
 obj-$(CONFIG_MTD_NAND_NUC900)		+= nuc900_nand.o
+obj-$(CONFIG_MTD_NAND_XILINX_PS)	+= xilinx_nandps.o
 obj-$(CONFIG_MTD_NAND_MPC5121_NFC)	+= mpc5121_nfc.o
 obj-$(CONFIG_MTD_NAND_RICOH)		+= r852.o
 obj-$(CONFIG_MTD_NAND_JZ4740)		+= jz4740_nand.o
diff --git a/drivers/mtd/nand/nand_base.c b/drivers/mtd/nand/nand_base.c
index 633db88..b787dce 100644
--- a/drivers/mtd/nand/nand_base.c
+++ b/drivers/mtd/nand/nand_base.c
@@ -814,6 +814,11 @@ static int nand_wait(struct mtd_info *mtd, struct nand_chip *chip)
 	int status, state = chip->state;
 	unsigned long timeo = (state == FL_ERASING ? 400 : 20);
 
+#if defined(ARCH_ZYNQ) && (CONFIG_HZ == 20)
+		/* Xilinx Zynq NAND work around for HZ=20 */
+		timeo += 1;
+#endif
+
 	led_trigger_event(nand_led_trigger, LED_FULL);
 
 	/*
@@ -2858,11 +2863,18 @@ static int nand_flash_detect_onfi(struct mtd_info *mtd, struct nand_chip *chip,
 	int i;
 	int val;
 
+#ifdef CONFIG_MTD_NAND_XILINX_PS
+	uint8_t *buf;
+	unsigned int options;
+	int j;
+#endif
+
 	/* ONFI need to be probed in 8 bits mode, and 16 bits should be selected with NAND_BUSWIDTH_AUTO */
 	if (chip->options & NAND_BUSWIDTH_16) {
 		pr_err("Trying ONFI probe in 16 bits mode, aborting !\n");
 		return 0;
 	}
+
 	/* Try ONFI for unknown chip or LP */
 	chip->cmdfunc(mtd, NAND_CMD_READID, 0x20, -1);
 	if (chip->read_byte(mtd) != 'O' || chip->read_byte(mtd) != 'N' ||
@@ -2871,7 +2883,13 @@ static int nand_flash_detect_onfi(struct mtd_info *mtd, struct nand_chip *chip,
 
 	chip->cmdfunc(mtd, NAND_CMD_PARAM, 0, -1);
 	for (i = 0; i < 3; i++) {
+#ifdef CONFIG_MTD_NAND_XILINX_PS
+		buf = (uint8_t *)p;
+		for (j = 0; j < 256; j++)
+			buf[j] = chip->read_byte(mtd);
+#else
 		chip->read_buf(mtd, (uint8_t *)p, sizeof(*p));
+#endif
 		if (onfi_crc16(ONFI_CRC_BASE, (uint8_t *)p, 254) ==
 				le16_to_cpu(p->crc)) {
 			pr_info("ONFI param page %d valid\n", i);
@@ -2924,7 +2942,17 @@ static int nand_flash_detect_onfi(struct mtd_info *mtd, struct nand_chip *chip,
 	if (le16_to_cpu(p->features) & 1)
 		*busw = NAND_BUSWIDTH_16;
 
+#ifdef CONFIG_MTD_NAND_XILINX_PS
+	/* Read the chip options before clearing the bits */
+	options = chip->options;
+#endif
+
 	pr_info("ONFI flash detected\n");
+#ifdef CONFIG_MTD_NAND_XILINX_PS
+	/* set the bus width option */
+	if (options & NAND_BUSWIDTH_16)
+		chip->options |= NAND_BUSWIDTH_16;
+#endif
 	return 1;
 }
 
diff --git a/drivers/mtd/nand/xilinx_nandps.c b/drivers/mtd/nand/xilinx_nandps.c
new file mode 100644
index 0000000..fd833ce
--- /dev/null
+++ b/drivers/mtd/nand/xilinx_nandps.c
@@ -0,0 +1,1064 @@
+/*
+ * Xilinx Zynq NAND Flash Controller Driver
+ *
+ * Copyright (C) 2009 Xilinx, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify it under
+ * the terms of the GNU General Public License version 2 as published by the
+ * Free Software Foundation; either version 2 of the License, or (at your
+ * option) any later version.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc., 59 Temple
+ * Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+/*
+ * This driver is based on plat_nand.c and mxc_nand.c drivers
+ */
+
+#include <linux/err.h>
+#include <linux/delay.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/ioport.h>
+#include <linux/irq.h>
+#include <linux/memory/zynq-smc.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/mtd/mtd.h>
+#include <linux/mtd/nand.h>
+#include <linux/mtd/nand_ecc.h>
+#include <linux/mtd/partitions.h>
+#include <linux/of_address.h>
+#include <linux/of_device.h>
+#include <linux/of_platform.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+
+#define XNANDPS_DRIVER_NAME "xilinx_nandps"
+
+/* NAND flash driver defines */
+#define XNANDPS_CMD_PHASE	1	/* End command valid in command phase */
+#define XNANDPS_DATA_PHASE	2	/* End command valid in data phase */
+#define XNANDPS_ECC_SIZE	512	/* Size of data for ECC operation */
+
+/* Flash memory controller operating parameters */
+
+#define XNANDPS_ECC_CONFIG	((1 << 4)  |	/* ECC read at end of page */ \
+				 (0 << 5))	/* No Jumping */
+
+/* AXI Address definitions */
+#define START_CMD_SHIFT		3
+#define END_CMD_SHIFT		11
+#define END_CMD_VALID_SHIFT	20
+#define ADDR_CYCLES_SHIFT	21
+#define CLEAR_CS_SHIFT		21
+#define ECC_LAST_SHIFT		10
+#define COMMAND_PHASE		(0 << 19)
+#define DATA_PHASE		(1 << 19)
+
+#define XNANDPS_ECC_LAST	(1 << ECC_LAST_SHIFT)	/* Set ECC_Last */
+#define XNANDPS_CLEAR_CS	(1 << CLEAR_CS_SHIFT)	/* Clear chip select */
+
+#define ONDIE_ECC_FEATURE_ADDR	0x90
+
+/* Macros for the NAND controller register read/write */
+#define xnandps_write32(addr, val)	__raw_writel((val), (addr))
+
+
+/**
+ * struct xnandps_command_format - Defines NAND flash command format
+ * @start_cmd:		First cycle command (Start command)
+ * @end_cmd:		Second cycle command (Last command)
+ * @addr_cycles:	Number of address cycles required to send the address
+ * @end_cmd_valid:	The second cycle command is valid for cmd or data phase
+ **/
+struct xnandps_command_format {
+	int start_cmd;
+	int end_cmd;
+	u8 addr_cycles;
+	u8 end_cmd_valid;
+};
+
+/**
+ * struct xnandps_info - Defines the NAND flash driver instance
+ * @chip:		NAND chip information structure
+ * @mtd:		MTD information structure
+ * @parts:		Pointer	to the mtd_partition structure
+ * @nand_base:		Virtual address of the NAND flash device
+ * @end_cmd_pending:	End command is pending
+ * @end_cmd:		End command
+ **/
+struct xnandps_info {
+	struct nand_chip	chip;
+	struct mtd_info		mtd;
+	struct mtd_partition	*parts;
+	void __iomem		*nand_base;
+	unsigned long		end_cmd_pending;
+	unsigned long		end_cmd;
+};
+
+/*
+ * The NAND flash operations command format
+ */
+static const struct xnandps_command_format xnandps_commands[] = {
+	{NAND_CMD_READ0, NAND_CMD_READSTART, 5, XNANDPS_CMD_PHASE},
+	{NAND_CMD_RNDOUT, NAND_CMD_RNDOUTSTART, 2, XNANDPS_CMD_PHASE},
+	{NAND_CMD_READID, NAND_CMD_NONE, 1, NAND_CMD_NONE},
+	{NAND_CMD_STATUS, NAND_CMD_NONE, 0, NAND_CMD_NONE},
+	{NAND_CMD_SEQIN, NAND_CMD_PAGEPROG, 5, XNANDPS_DATA_PHASE},
+	{NAND_CMD_RNDIN, NAND_CMD_NONE, 2, NAND_CMD_NONE},
+	{NAND_CMD_ERASE1, NAND_CMD_ERASE2, 3, XNANDPS_CMD_PHASE},
+	{NAND_CMD_RESET, NAND_CMD_NONE, 0, NAND_CMD_NONE},
+	{NAND_CMD_PARAM, NAND_CMD_NONE, 1, NAND_CMD_NONE},
+	{NAND_CMD_GET_FEATURES, NAND_CMD_NONE, 1, NAND_CMD_NONE},
+	{NAND_CMD_SET_FEATURES, NAND_CMD_NONE, 1, NAND_CMD_NONE},
+	{NAND_CMD_NONE, NAND_CMD_NONE, 0, 0},
+	/* Add all the flash commands supported by the flash device and Linux */
+	/* The cache program command is not supported by driver because driver
+	 * cant differentiate between page program and cached page program from
+	 * start command, these commands can be differentiated through end
+	 * command, which doesn't fit in to the driver design. The cache program
+	 * command is not supported by NAND subsystem also, look at 1612 line
+	 * number (in nand_write_page function) of nand_base.c file.
+	 * {NAND_CMD_SEQIN, NAND_CMD_CACHEDPROG, 5, XNANDPS_YES}, */
+};
+
+/* Define default oob placement schemes for large and small page devices */
+static struct nand_ecclayout nand_oob_16 = {
+	.eccbytes = 3,
+	.eccpos = {0, 1, 2},
+	.oobfree = {
+		{.offset = 8,
+		 . length = 8} }
+};
+
+static struct nand_ecclayout nand_oob_64 = {
+	.eccbytes = 12,
+	.eccpos = {
+		   52, 53, 54, 55, 56, 57,
+		   58, 59, 60, 61, 62, 63},
+	.oobfree = {
+		{.offset = 2,
+		 .length = 50} }
+};
+
+static struct nand_ecclayout ondie_nand_oob_64 = {
+	.eccbytes = 32,
+
+	.eccpos = {
+		8, 9, 10, 11, 12, 13, 14, 15,
+		24, 25, 26, 27, 28, 29, 30, 31,
+		40, 41, 42, 43, 44, 45, 46, 47,
+		56, 57, 58, 59, 60, 61, 62, 63
+	},
+
+	.oobfree = {
+		{ .offset = 4, .length = 4 },
+		{ .offset = 20, .length = 4 },
+		{ .offset = 36, .length = 4 },
+		{ .offset = 52, .length = 4 }
+	}
+};
+
+/* Generic flash bbt decriptors
+*/
+static uint8_t bbt_pattern[] = {'B', 'b', 't', '0' };
+static uint8_t mirror_pattern[] = {'1', 't', 'b', 'B' };
+
+static struct nand_bbt_descr bbt_main_descr = {
+	.options = NAND_BBT_LASTBLOCK | NAND_BBT_CREATE | NAND_BBT_WRITE
+		| NAND_BBT_2BIT | NAND_BBT_VERSION | NAND_BBT_PERCHIP,
+	.offs = 4,
+	.len = 4,
+	.veroffs = 20,
+	.maxblocks = 4,
+	.pattern = bbt_pattern
+};
+
+static struct nand_bbt_descr bbt_mirror_descr = {
+	.options = NAND_BBT_LASTBLOCK | NAND_BBT_CREATE | NAND_BBT_WRITE
+		| NAND_BBT_2BIT | NAND_BBT_VERSION | NAND_BBT_PERCHIP,
+	.offs = 4,
+	.len = 4,
+	.veroffs = 20,
+	.maxblocks = 4,
+	.pattern = mirror_pattern
+};
+
+/**
+ * xnandps_calculate_hwecc - Calculate Hardware ECC
+ * @mtd:	Pointer to the mtd_info structure
+ * @data:	Pointer to the page data
+ * @ecc_code:	Pointer to the ECC buffer where ECC data needs to be stored
+ *
+ * This function retrieves the Hardware ECC data from the controller and returns
+ * ECC data back to the MTD subsystem.
+ *
+ * returns:	0 on success or error value on failure
+ **/
+static int
+xnandps_calculate_hwecc(struct mtd_info *mtd, const u8 *data, u8 *ecc_code)
+{
+	u32 ecc_value = 0;
+	u8 ecc_reg, ecc_byte;
+	u32 ecc_status;
+
+	/* Wait till the ECC operation is complete */
+	while (xsmcps_ecc_is_busy())
+		cpu_relax();
+
+	for (ecc_reg = 0; ecc_reg < 4; ecc_reg++) {
+		/* Read ECC value for each block */
+		ecc_value = xsmcps_get_ecc_val(ecc_reg);
+		ecc_status = (ecc_value >> 24) & 0xFF;
+		/* ECC value valid */
+		if (ecc_status & 0x40) {
+			for (ecc_byte = 0; ecc_byte < 3; ecc_byte++) {
+				/* Copy ECC bytes to MTD buffer */
+				*ecc_code = ecc_value & 0xFF;
+				ecc_value = ecc_value >> 8;
+				ecc_code++;
+			}
+		} else {
+			/* TO DO */
+			/* dev_warn(&pdev->dev, "pl350: ecc status failed\n");
+			* */
+		}
+	}
+	return 0;
+}
+
+/**
+ * onehot - onehot function
+ * @value:	value to check for onehot
+ *
+ * This function checks whether a value is onehot or not.
+ * onehot is if and only if onebit is set.
+ *
+ **/
+static int onehot(unsigned short value)
+{
+	return ((value & (value-1)) == 0);
+}
+
+/**
+ * xnandps_correct_data - ECC correction function
+ * @mtd:	Pointer to the mtd_info structure
+ * @buf:	Pointer to the page data
+ * @read_ecc:	Pointer to the ECC value read from spare data area
+ * @calc_ecc:	Pointer to the calculated ECC value
+ *
+ * This function corrects the ECC single bit errors & detects 2-bit errors.
+ *
+ * returns:	0 if no ECC errors found
+ *		1 if single bit error found and corrected.
+ *		-1 if multiple ECC errors found.
+ **/
+static int xnandps_correct_data(struct mtd_info *mtd, unsigned char *buf,
+			unsigned char *read_ecc, unsigned char *calc_ecc)
+{
+	unsigned char bit_addr;
+	unsigned int byte_addr;
+	unsigned short ecc_odd, ecc_even;
+	unsigned short read_ecc_lower, read_ecc_upper;
+	unsigned short calc_ecc_lower, calc_ecc_upper;
+
+	read_ecc_lower = (read_ecc[0] | (read_ecc[1] << 8)) & 0xfff;
+	read_ecc_upper = ((read_ecc[1] >> 4) | (read_ecc[2] << 4)) & 0xfff;
+
+	calc_ecc_lower = (calc_ecc[0] | (calc_ecc[1] << 8)) & 0xfff;
+	calc_ecc_upper = ((calc_ecc[1] >> 4) | (calc_ecc[2] << 4)) & 0xfff;
+
+	ecc_odd = read_ecc_lower ^ calc_ecc_lower;
+	ecc_even = read_ecc_upper ^ calc_ecc_upper;
+
+	if ((ecc_odd == 0) && (ecc_even == 0))
+		return 0;       /* no error */
+
+	if (ecc_odd == (~ecc_even & 0xfff)) {
+		/* bits [11:3] of error code is byte offset */
+		byte_addr = (ecc_odd >> 3) & 0x1ff;
+		/* bits [2:0] of error code is bit offset */
+		bit_addr = ecc_odd & 0x7;
+		/* Toggling error bit */
+		buf[byte_addr] ^= (1 << bit_addr);
+		return 1;
+	}
+
+	if (onehot(ecc_odd | ecc_even) == 1)
+		return 1; /* one error in parity */
+
+	return -1; /* Uncorrectable error */
+}
+
+/**
+ * xnandps_read_oob - [REPLACABLE] the most common OOB data read function
+ * @mtd:	mtd info structure
+ * @chip:	nand chip info structure
+ * @page:	page number to read
+ */
+static int xnandps_read_oob(struct mtd_info *mtd, struct nand_chip *chip,
+			int page)
+{
+	unsigned long data_width = 4;
+	unsigned long data_phase_addr = 0;
+	uint8_t *p;
+
+	chip->cmdfunc(mtd, NAND_CMD_READOOB, 0, page);
+
+	p = chip->oob_poi;
+	chip->read_buf(mtd, p, (mtd->oobsize - data_width));
+	p += (mtd->oobsize - data_width);
+
+	data_phase_addr = (unsigned long __force)chip->IO_ADDR_R;
+	data_phase_addr |= XNANDPS_CLEAR_CS;
+	chip->IO_ADDR_R = (void __iomem *__force)data_phase_addr;
+	chip->read_buf(mtd, p, data_width);
+
+	return 0;
+}
+
+/**
+ * xnandps_write_oob - [REPLACABLE] the most common OOB data write function
+ * @mtd:	mtd info structure
+ * @chip:	nand chip info structure
+ * @page:	page number to write
+ */
+static int xnandps_write_oob(struct mtd_info *mtd, struct nand_chip *chip,
+			int page)
+{
+	int status = 0;
+	const uint8_t *buf = chip->oob_poi;
+	unsigned long data_width = 4;
+	unsigned long data_phase_addr = 0;
+
+	chip->cmdfunc(mtd, NAND_CMD_SEQIN, mtd->writesize, page);
+
+	chip->write_buf(mtd, buf, (mtd->oobsize - data_width));
+	buf += (mtd->oobsize - data_width);
+
+	data_phase_addr = (unsigned long __force)chip->IO_ADDR_W;
+	data_phase_addr |= XNANDPS_CLEAR_CS;
+	data_phase_addr |= (1 << END_CMD_VALID_SHIFT);
+	chip->IO_ADDR_W = (void __iomem *__force)data_phase_addr;
+	chip->write_buf(mtd, buf, data_width);
+
+	/* Send command to program the OOB data */
+	chip->cmdfunc(mtd, NAND_CMD_PAGEPROG, -1, -1);
+	status = chip->waitfunc(mtd, chip);
+
+	return status & NAND_STATUS_FAIL ? -EIO : 0;
+}
+
+/**
+ * xnandps_read_page_raw - [Intern] read raw page data without ecc
+ * @mtd:        mtd info structure
+ * @chip:       nand chip info structure
+ * @buf:        buffer to store read data
+ * @page:       page number to read
+ *
+ */
+static int xnandps_read_page_raw(struct mtd_info *mtd, struct nand_chip *chip,
+			      uint8_t *buf, int oob_required, int page)
+{
+	unsigned long data_width = 4;
+	unsigned long data_phase_addr = 0;
+	uint8_t *p;
+
+	chip->read_buf(mtd, buf, mtd->writesize);
+
+	p = chip->oob_poi;
+	chip->read_buf(mtd, p, (mtd->oobsize - data_width));
+	p += (mtd->oobsize - data_width);
+
+	data_phase_addr = (unsigned long __force)chip->IO_ADDR_R;
+	data_phase_addr |= XNANDPS_CLEAR_CS;
+	chip->IO_ADDR_R = (void __iomem *__force)data_phase_addr;
+
+	chip->read_buf(mtd, p, data_width);
+	return 0;
+}
+
+/**
+ * xnandps_write_page_raw - [Intern] raw page write function
+ * @mtd:        mtd info structure
+ * @chip:       nand chip info structure
+ * @buf:        data buffer
+ *
+ */
+static int xnandps_write_page_raw(struct mtd_info *mtd, struct nand_chip *chip,
+		const uint8_t *buf, int oob_required)
+{
+	unsigned long data_width = 4;
+	unsigned long data_phase_addr = 0;
+	uint8_t *p;
+
+	chip->write_buf(mtd, buf, mtd->writesize);
+
+	p = chip->oob_poi;
+	chip->write_buf(mtd, p, (mtd->oobsize - data_width));
+	p += (mtd->oobsize - data_width);
+
+	data_phase_addr = (unsigned long __force)chip->IO_ADDR_W;
+	data_phase_addr |= XNANDPS_CLEAR_CS;
+	data_phase_addr |= (1 << END_CMD_VALID_SHIFT);
+	chip->IO_ADDR_W = (void __iomem *__force)data_phase_addr;
+
+	chip->write_buf(mtd, p, data_width);
+
+	return 0;
+}
+
+/**
+ * nand_write_page_hwecc - Hardware ECC based page write function
+ * @mtd:	Pointer to the mtd info structure
+ * @chip:	Pointer to the NAND chip info structure
+ * @buf:	Pointer to the data buffer
+ *
+ * This functions writes data and hardware generated ECC values in to the page.
+ */
+static int xnandps_write_page_hwecc(struct mtd_info *mtd,
+		struct nand_chip *chip, const uint8_t *buf, int oob_required)
+{
+	int i, eccsize = chip->ecc.size;
+	int eccsteps = chip->ecc.steps;
+	uint8_t *ecc_calc = chip->buffers->ecccalc;
+	const uint8_t *p = buf;
+	uint32_t *eccpos = chip->ecc.layout->eccpos;
+	unsigned long data_phase_addr = 0;
+	unsigned long data_width = 4;
+	uint8_t *oob_ptr;
+
+	for (; (eccsteps - 1); eccsteps--) {
+		chip->write_buf(mtd, p, eccsize);
+		p += eccsize;
+	}
+	chip->write_buf(mtd, p, (eccsize - data_width));
+	p += (eccsize - data_width);
+
+	/* Set ECC Last bit to 1 */
+	data_phase_addr = (unsigned long __force)chip->IO_ADDR_W;
+	data_phase_addr |= XNANDPS_ECC_LAST;
+	chip->IO_ADDR_W = (void __iomem *__force)data_phase_addr;
+	chip->write_buf(mtd, p, data_width);
+
+	/* Wait for ECC to be calculated and read the error values */
+	p = buf;
+	chip->ecc.calculate(mtd, p, &ecc_calc[0]);
+
+	for (i = 0; i < chip->ecc.total; i++)
+		chip->oob_poi[eccpos[i]] = ~(ecc_calc[i]);
+
+	/* Clear ECC last bit */
+	data_phase_addr = (unsigned long __force)chip->IO_ADDR_W;
+	data_phase_addr &= ~XNANDPS_ECC_LAST;
+	chip->IO_ADDR_W = (void __iomem *__force)data_phase_addr;
+
+	/* Write the spare area with ECC bytes */
+	oob_ptr = chip->oob_poi;
+	chip->write_buf(mtd, oob_ptr, (mtd->oobsize - data_width));
+
+	data_phase_addr = (unsigned long __force)chip->IO_ADDR_W;
+	data_phase_addr |= XNANDPS_CLEAR_CS;
+	data_phase_addr |= (1 << END_CMD_VALID_SHIFT);
+	chip->IO_ADDR_W = (void __iomem *__force)data_phase_addr;
+	oob_ptr += (mtd->oobsize - data_width);
+	chip->write_buf(mtd, oob_ptr, data_width);
+
+	return 0;
+}
+
+/**
+ * xnandps_write_page_swecc - [REPLACABLE] software ecc based page write function
+ * @mtd:	mtd info structure
+ * @chip:	nand chip info structure
+ * @buf:	data buffer
+ */
+static int xnandps_write_page_swecc(struct mtd_info *mtd,
+		struct nand_chip *chip, const uint8_t *buf, int oob_required)
+{
+	int i, eccsize = chip->ecc.size;
+	int eccbytes = chip->ecc.bytes;
+	int eccsteps = chip->ecc.steps;
+	uint8_t *ecc_calc = chip->buffers->ecccalc;
+	const uint8_t *p = buf;
+	uint32_t *eccpos = chip->ecc.layout->eccpos;
+
+	/* Software ecc calculation */
+	for (i = 0; eccsteps; eccsteps--, i += eccbytes, p += eccsize)
+		chip->ecc.calculate(mtd, p, &ecc_calc[i]);
+
+	for (i = 0; i < chip->ecc.total; i++)
+		chip->oob_poi[eccpos[i]] = ecc_calc[i];
+
+	chip->ecc.write_page_raw(mtd, chip, buf, 1);
+
+	return 0;
+}
+
+/**
+ * xnandps_read_page_hwecc - Hardware ECC based page read function
+ * @mtd:	Pointer to the mtd info structure
+ * @chip:	Pointer to the NAND chip info structure
+ * @buf:	Pointer to the buffer to store read data
+ * @page:	page number to read
+ *
+ * This functions reads data and checks the data integrity by comparing hardware
+ * generated ECC values and read ECC values from spare area.
+ *
+ * returns:	0 always and updates ECC operation status in to MTD structure
+ */
+static int xnandps_read_page_hwecc(struct mtd_info *mtd, struct nand_chip *chip,
+		uint8_t *buf, int oob_required, int page)
+{
+	int i, stat, eccsize = chip->ecc.size;
+	int eccbytes = chip->ecc.bytes;
+	int eccsteps = chip->ecc.steps;
+	uint8_t *p = buf;
+	uint8_t *ecc_calc = chip->buffers->ecccalc;
+	uint8_t *ecc_code = chip->buffers->ecccode;
+	uint32_t *eccpos = chip->ecc.layout->eccpos;
+	unsigned long data_phase_addr = 0;
+	unsigned long data_width = 4;
+	uint8_t *oob_ptr;
+
+	for (; (eccsteps - 1); eccsteps--) {
+		chip->read_buf(mtd, p, eccsize);
+		p += eccsize;
+	}
+	chip->read_buf(mtd, p, (eccsize - data_width));
+	p += (eccsize - data_width);
+
+	/* Set ECC Last bit to 1 */
+	data_phase_addr = (unsigned long __force)chip->IO_ADDR_R;
+	data_phase_addr |= XNANDPS_ECC_LAST;
+	chip->IO_ADDR_R = (void __iomem *__force)data_phase_addr;
+	chip->read_buf(mtd, p, data_width);
+
+	/* Read the calculated ECC value */
+	p = buf;
+	chip->ecc.calculate(mtd, p, &ecc_calc[0]);
+
+	/* Clear ECC last bit */
+	data_phase_addr = (unsigned long __force)chip->IO_ADDR_R;
+	data_phase_addr &= ~XNANDPS_ECC_LAST;
+	chip->IO_ADDR_R = (void __iomem *__force)data_phase_addr;
+
+	/* Read the stored ECC value */
+	oob_ptr = chip->oob_poi;
+	chip->read_buf(mtd, oob_ptr, (mtd->oobsize - data_width));
+
+	/* de-assert chip select */
+	data_phase_addr = (unsigned long __force)chip->IO_ADDR_R;
+	data_phase_addr |= XNANDPS_CLEAR_CS;
+	chip->IO_ADDR_R = (void __iomem *__force)data_phase_addr;
+
+	oob_ptr += (mtd->oobsize - data_width);
+	chip->read_buf(mtd, oob_ptr, data_width);
+
+	for (i = 0; i < chip->ecc.total; i++)
+		ecc_code[i] = ~(chip->oob_poi[eccpos[i]]);
+
+	eccsteps = chip->ecc.steps;
+	p = buf;
+
+	/* Check ECC error for all blocks and correct if it is correctable */
+	for (i = 0; eccsteps; eccsteps--, i += eccbytes, p += eccsize) {
+		stat = chip->ecc.correct(mtd, p, &ecc_code[i], &ecc_calc[i]);
+		if (stat < 0)
+			mtd->ecc_stats.failed++;
+		else
+			mtd->ecc_stats.corrected += stat;
+	}
+	return 0;
+}
+
+/**
+ * xnandps_read_page_swecc - [REPLACABLE] software ecc based page read function
+ * @mtd:	mtd info structure
+ * @chip:	nand chip info structure
+ * @buf:	buffer to store read data
+ * @page:	page number to read
+ */
+static int xnandps_read_page_swecc(struct mtd_info *mtd, struct nand_chip *chip,
+		uint8_t *buf,  int oob_required, int page)
+{
+	int i, eccsize = chip->ecc.size;
+	int eccbytes = chip->ecc.bytes;
+	int eccsteps = chip->ecc.steps;
+	uint8_t *p = buf;
+	uint8_t *ecc_calc = chip->buffers->ecccalc;
+	uint8_t *ecc_code = chip->buffers->ecccode;
+	uint32_t *eccpos = chip->ecc.layout->eccpos;
+
+	chip->ecc.read_page_raw(mtd, chip, buf, page, 1);
+
+	for (i = 0; eccsteps; eccsteps--, i += eccbytes, p += eccsize)
+		chip->ecc.calculate(mtd, p, &ecc_calc[i]);
+
+	for (i = 0; i < chip->ecc.total; i++)
+		ecc_code[i] = chip->oob_poi[eccpos[i]];
+
+	eccsteps = chip->ecc.steps;
+	p = buf;
+
+	for (i = 0; eccsteps; eccsteps--, i += eccbytes, p += eccsize) {
+		int stat;
+
+		stat = chip->ecc.correct(mtd, p, &ecc_code[i], &ecc_calc[i]);
+		if (stat < 0)
+			mtd->ecc_stats.failed++;
+		else
+			mtd->ecc_stats.corrected += stat;
+	}
+	return 0;
+}
+
+/**
+ * xnandps_select_chip - Select the flash device
+ * @mtd:	Pointer to the mtd_info structure
+ * @chip:	Chip number to be selected
+ *
+ * This function is empty as the NAND controller handles chip select line
+ * internally based on the chip address passed in command and data phase.
+ **/
+static void xnandps_select_chip(struct mtd_info *mtd, int chip)
+{
+	return;
+}
+
+/**
+ * xnandps_cmd_function - Send command to NAND device
+ * @mtd:	Pointer to the mtd_info structure
+ * @command:	The command to be sent to the flash device
+ * @column:	The column address for this command, -1 if none
+ * @page_addr:	The page address for this command, -1 if none
+ */
+static void xnandps_cmd_function(struct mtd_info *mtd, unsigned int command,
+			    int column, int page_addr)
+{
+	struct nand_chip *chip = mtd->priv;
+	const struct xnandps_command_format *curr_cmd = NULL;
+	struct xnandps_info *xnand =
+		container_of(mtd, struct xnandps_info, mtd);
+	void __iomem *cmd_addr;
+	unsigned long cmd_data = 0;
+	unsigned long cmd_phase_addr = 0;
+	unsigned long data_phase_addr = 0;
+	unsigned long end_cmd = 0;
+	unsigned long end_cmd_valid = 0;
+	unsigned long i;
+
+	if (xnand->end_cmd_pending) {
+		/* Check for end command if this command request is same as the
+		 * pending command then return */
+		if (xnand->end_cmd == command) {
+			xnand->end_cmd = 0;
+			xnand->end_cmd_pending = 0;
+			return;
+		}
+	}
+
+	/* Emulate NAND_CMD_READOOB for large page device */
+	if ((mtd->writesize > XNANDPS_ECC_SIZE) &&
+		(command == NAND_CMD_READOOB)) {
+		column += mtd->writesize;
+		command = NAND_CMD_READ0;
+	}
+
+	/* Get the command format */
+	for (i = 0; (xnandps_commands[i].start_cmd != NAND_CMD_NONE ||
+		xnandps_commands[i].end_cmd != NAND_CMD_NONE); i++) {
+		if (command == xnandps_commands[i].start_cmd)
+			curr_cmd = &xnandps_commands[i];
+	}
+
+	if (curr_cmd == NULL)
+		return;
+
+	/* Clear interrupt */
+	xsmcps_clr_nand_int();
+
+	/* Get the command phase address */
+	if (curr_cmd->end_cmd_valid == XNANDPS_CMD_PHASE)
+		end_cmd_valid = 1;
+
+	if (curr_cmd->end_cmd == NAND_CMD_NONE)
+		end_cmd = 0x0;
+	else
+		end_cmd = curr_cmd->end_cmd;
+
+	cmd_phase_addr = (unsigned long __force)xnand->nand_base	|
+			(curr_cmd->addr_cycles << ADDR_CYCLES_SHIFT)	|
+			(end_cmd_valid << END_CMD_VALID_SHIFT)		|
+			(COMMAND_PHASE)					|
+			(end_cmd << END_CMD_SHIFT)			|
+			(curr_cmd->start_cmd << START_CMD_SHIFT);
+
+	cmd_addr = (void __iomem * __force)cmd_phase_addr;
+
+	/* Get the data phase address */
+	end_cmd_valid = 0;
+
+	data_phase_addr = (unsigned long __force)xnand->nand_base	|
+			(0x0 << CLEAR_CS_SHIFT)				|
+			(end_cmd_valid << END_CMD_VALID_SHIFT)		|
+			(DATA_PHASE)					|
+			(end_cmd << END_CMD_SHIFT)			|
+			(0x0 << ECC_LAST_SHIFT);
+
+	chip->IO_ADDR_R = (void __iomem * __force)data_phase_addr;
+	chip->IO_ADDR_W = chip->IO_ADDR_R;
+
+	/* Command phase AXI write */
+	/* Read & Write */
+	if (column != -1 && page_addr != -1) {
+		/* Adjust columns for 16 bit bus width */
+		if (chip->options & NAND_BUSWIDTH_16)
+			column >>= 1;
+		cmd_data = column;
+		if (mtd->writesize > XNANDPS_ECC_SIZE) {
+			cmd_data |= page_addr << 16;
+			/* Another address cycle for devices > 128MiB */
+			if (chip->chipsize > (128 << 20)) {
+				xnandps_write32(cmd_addr, cmd_data);
+				cmd_data = (page_addr >> 16);
+			}
+		} else {
+			cmd_data |= page_addr << 8;
+		}
+	} else if (page_addr != -1) {
+		/* Erase */
+		cmd_data = page_addr;
+	} else if (column != -1) {
+		/* Change read/write column, read id etc */
+		/* Adjust columns for 16 bit bus width */
+		if ((chip->options & NAND_BUSWIDTH_16) &&
+			((command == NAND_CMD_READ0) ||
+			(command == NAND_CMD_SEQIN) ||
+			(command == NAND_CMD_RNDOUT) ||
+			(command == NAND_CMD_RNDIN)))
+				column >>= 1;
+		cmd_data = column;
+	}
+
+	xnandps_write32(cmd_addr, cmd_data);
+
+	if (curr_cmd->end_cmd_valid) {
+		xnand->end_cmd = curr_cmd->end_cmd;
+		xnand->end_cmd_pending = 1;
+	}
+
+	ndelay(100);
+
+	if ((command == NAND_CMD_READ0) ||
+		(command == NAND_CMD_RESET) ||
+		(command == NAND_CMD_PARAM) ||
+		(command == NAND_CMD_GET_FEATURES)) {
+
+		while (!chip->dev_ready(mtd))
+			;
+		return;
+	}
+}
+
+/**
+ * xnandps_read_buf - read chip data into buffer
+ * @mtd:        MTD device structure
+ * @buf:        buffer to store date
+ * @len:        number of bytes to read
+ *
+ */
+static void xnandps_read_buf(struct mtd_info *mtd, uint8_t *buf, int len)
+{
+	int i;
+	struct nand_chip *chip = mtd->priv;
+	unsigned long *ptr = (unsigned long *)buf;
+
+	len >>= 2;
+	for (i = 0; i < len; i++)
+		ptr[i] = readl(chip->IO_ADDR_R);
+}
+
+/**
+ * xnandps_write_buf - write buffer to chip
+ * @mtd:        MTD device structure
+ * @buf:        data buffer
+ * @len:        number of bytes to write
+ *
+ */
+static void xnandps_write_buf(struct mtd_info *mtd, const uint8_t *buf, int len)
+{
+	int i;
+	struct nand_chip *chip = mtd->priv;
+	unsigned long *ptr = (unsigned long *)buf;
+	len >>= 2;
+
+	for (i = 0; i < len; i++)
+		writel(ptr[i], chip->IO_ADDR_W);
+}
+
+/**
+ * xnandps_device_ready - Check device ready/busy line
+ * @mtd:	Pointer to the mtd_info structure
+ *
+ * returns:	0 on busy or 1 on ready state
+ **/
+static int xnandps_device_ready(struct mtd_info *mtd)
+{
+	if (xsmcps_get_nand_int_status_raw()) {
+		xsmcps_clr_nand_int();
+		return 1;
+	}
+	return 0;
+}
+
+/**
+ * xnandps_probe - Probe method for the NAND driver
+ * @pdev:	Pointer to the platform_device structure
+ *
+ * This function initializes the driver data structures and the hardware.
+ *
+ * returns:	0 on success or error value on failure
+ **/
+static int xnandps_probe(struct platform_device *pdev)
+{
+	struct xnandps_info *xnand;
+	struct mtd_info *mtd;
+	struct nand_chip *nand_chip;
+	struct resource *nand_res;
+	u8 maf_id, dev_id, i;
+	u8 get_feature;
+	u8 set_feature[4] = {0x08, 0x00, 0x00, 0x00};
+	int ondie_ecc_enabled = 0;
+	struct mtd_part_parser_data ppdata;
+	const unsigned int *prop;
+	u32 options = 0;
+
+	xnand = devm_kzalloc(&pdev->dev, sizeof(struct xnandps_info),
+					GFP_KERNEL);
+	if (!xnand)
+		return -ENOMEM;
+
+	/* Map physical address of NAND flash */
+	nand_res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	xnand->nand_base = devm_ioremap_resource(&pdev->dev, nand_res);
+	if (IS_ERR(xnand->nand_base)) {
+		dev_err(&pdev->dev, "ioremap for NAND failed\n");
+		return PTR_ERR(xnand->nand_base);
+	}
+
+	/* Get x8 or x16 mode from device tree */
+	prop = of_get_property(pdev->dev.of_node, "xlnx,nand-width", NULL);
+	if (prop) {
+		if (be32_to_cpup(prop) == 16) {
+			options |= NAND_BUSWIDTH_16;
+		} else if (be32_to_cpup(prop) == 8) {
+			options &= ~NAND_BUSWIDTH_16;
+		} else {
+			dev_info(&pdev->dev, "xlnx,nand-width not valid, using 8");
+			options &= ~NAND_BUSWIDTH_16;
+		}
+	} else {
+		dev_info(&pdev->dev, "xlnx,nand-width not in device tree, using 8");
+		options &= ~NAND_BUSWIDTH_16;
+	}
+
+	/* Link the private data with the MTD structure */
+	mtd = &xnand->mtd;
+	nand_chip = &xnand->chip;
+
+	nand_chip->priv = xnand;
+	mtd->priv = nand_chip;
+	mtd->owner = THIS_MODULE;
+	mtd->name = "xilinx_nand";
+
+	/* Set address of NAND IO lines */
+	nand_chip->IO_ADDR_R = xnand->nand_base;
+	nand_chip->IO_ADDR_W = xnand->nand_base;
+
+	/* Set the driver entry points for MTD */
+	nand_chip->cmdfunc = xnandps_cmd_function;
+	nand_chip->dev_ready = xnandps_device_ready;
+	nand_chip->select_chip = xnandps_select_chip;
+
+	/* If we don't set this delay driver sets 20us by default */
+	nand_chip->chip_delay = 30;
+
+	/* Buffer read/write routines */
+	nand_chip->read_buf = xnandps_read_buf;
+	nand_chip->write_buf = xnandps_write_buf;
+
+	/* Set the device option and flash width */
+	nand_chip->options = options;
+	nand_chip->bbt_options = NAND_BBT_USE_FLASH;
+
+	platform_set_drvdata(pdev, xnand);
+
+	/* first scan to find the device and get the page size */
+	if (nand_scan_ident(mtd, 1, NULL)) {
+		dev_err(&pdev->dev, "nand_scan_ident for NAND failed\n");
+		return -ENXIO;
+	}
+
+	/* Check if On-Die ECC flash */
+	nand_chip->cmdfunc(mtd, NAND_CMD_RESET, -1, -1);
+	nand_chip->cmdfunc(mtd, NAND_CMD_READID, 0x00, -1);
+
+	/* Read manufacturer and device IDs */
+	maf_id = nand_chip->read_byte(mtd);
+	dev_id = nand_chip->read_byte(mtd);
+
+	if ((maf_id == 0x2c) &&
+				((dev_id == 0xf1) || (dev_id == 0xa1) ||
+				(dev_id == 0xb1) ||
+				(dev_id == 0xaa) || (dev_id == 0xba) ||
+				(dev_id == 0xda) || (dev_id == 0xca) ||
+				(dev_id == 0xac) || (dev_id == 0xbc) ||
+				(dev_id == 0xdc) || (dev_id == 0xcc) ||
+				(dev_id == 0xa3) || (dev_id == 0xb3) ||
+				(dev_id == 0xd3) || (dev_id == 0xc3))) {
+
+		nand_chip->cmdfunc(mtd, NAND_CMD_GET_FEATURES,
+						ONDIE_ECC_FEATURE_ADDR, -1);
+		get_feature = nand_chip->read_byte(mtd);
+
+		if (get_feature & 0x08) {
+			ondie_ecc_enabled = 1;
+		} else {
+			nand_chip->cmdfunc(mtd, NAND_CMD_SET_FEATURES,
+						ONDIE_ECC_FEATURE_ADDR, -1);
+			for (i = 0; i < 4; i++)
+				writeb(set_feature[i], nand_chip->IO_ADDR_W);
+
+			ndelay(1000);
+
+			nand_chip->cmdfunc(mtd, NAND_CMD_GET_FEATURES,
+						ONDIE_ECC_FEATURE_ADDR, -1);
+			get_feature = nand_chip->read_byte(mtd);
+
+			if (get_feature & 0x08)
+				ondie_ecc_enabled = 1;
+		}
+	}
+
+	nand_chip->ecc.mode = NAND_ECC_HW;
+	nand_chip->ecc.read_oob = xnandps_read_oob;
+	nand_chip->ecc.read_page_raw = xnandps_read_page_raw;
+	nand_chip->ecc.strength = 1;
+	nand_chip->ecc.write_oob = xnandps_write_oob;
+	nand_chip->ecc.write_page_raw = xnandps_write_page_raw;
+	if (ondie_ecc_enabled) {
+		/* bypass the controller ECC block */
+		xsmcps_set_ecc_mode(XSMCPS_ECCMODE_BYPASS);
+
+		/* The software ECC routines won't work with the
+				SMC controller */
+		nand_chip->ecc.bytes = 0;
+		nand_chip->ecc.layout = &ondie_nand_oob_64;
+		nand_chip->ecc.read_page = xnandps_read_page_raw;
+		nand_chip->ecc.write_page = xnandps_write_page_raw;
+		nand_chip->ecc.size = mtd->writesize;
+		/* On-Die ECC spare bytes offset 8 is used for ECC codes */
+		/* Use the BBT pattern descriptors */
+		nand_chip->bbt_td = &bbt_main_descr;
+		nand_chip->bbt_md = &bbt_mirror_descr;
+	} else {
+		/* Hardware ECC generates 3 bytes ECC code for each 512 bytes */
+		nand_chip->ecc.bytes = 3;
+		nand_chip->ecc.calculate = xnandps_calculate_hwecc;
+		nand_chip->ecc.correct = xnandps_correct_data;
+		nand_chip->ecc.hwctl = NULL;
+		nand_chip->ecc.read_page = xnandps_read_page_hwecc;
+		nand_chip->ecc.size = XNANDPS_ECC_SIZE;
+		nand_chip->ecc.write_page = xnandps_write_page_hwecc;
+
+		xsmcps_set_ecc_pg_size(mtd->writesize);
+		switch (mtd->writesize) {
+		case 512:
+		case 1024:
+		case 2048:
+			xsmcps_set_ecc_mode(XSMCPS_ECCMODE_APB);
+			break;
+		default:
+			/* The software ECC routines won't work with the
+				SMC controller */
+			nand_chip->ecc.calculate = nand_calculate_ecc;
+			nand_chip->ecc.correct = nand_correct_data;
+			nand_chip->ecc.read_page = xnandps_read_page_swecc;
+			/* nand_chip->ecc.read_subpage = nand_read_subpage; */
+			nand_chip->ecc.write_page = xnandps_write_page_swecc;
+			nand_chip->ecc.size = 256;
+			break;
+		}
+
+		if (mtd->oobsize == 16)
+			nand_chip->ecc.layout = &nand_oob_16;
+		else if (mtd->oobsize == 64)
+			nand_chip->ecc.layout = &nand_oob_64;
+	}
+
+	/* second phase scan */
+	if (nand_scan_tail(mtd)) {
+		dev_err(&pdev->dev, "nand_scan_tail for NAND failed\n");
+		return -ENXIO;
+	}
+
+	ppdata.of_node = pdev->dev.of_node;
+
+	mtd_device_parse_register(&xnand->mtd, NULL, &ppdata,
+			NULL, 0);
+
+	return 0;
+}
+
+/**
+ * xnandps_remove - Remove method for the NAND driver
+ * @pdev:	Pointer to the platform_device structure
+ *
+ * This function is called if the driver module is being unloaded. It frees all
+ * resources allocated to the device.
+ *
+ * returns:	0 on success or error value on failure
+ **/
+static int xnandps_remove(struct platform_device *pdev)
+{
+	struct xnandps_info *xnand = platform_get_drvdata(pdev);
+
+	/* Release resources, unregister device */
+	nand_release(&xnand->mtd);
+	/* kfree(NULL) is safe */
+	kfree(xnand->parts);
+
+	return 0;
+}
+
+/* Match table for device tree binding */
+static const struct of_device_id xnandps_of_match[] = {
+	{ .compatible = "xlnx,ps7-nand-1.00.a" },
+	{},
+};
+MODULE_DEVICE_TABLE(of, xnandps_of_match);
+
+/*
+ * xnandps_driver - This structure defines the NAND subsystem platform driver
+ */
+static struct platform_driver xnandps_driver = {
+	.probe		= xnandps_probe,
+	.remove		= xnandps_remove,
+	.driver		= {
+		.name	= XNANDPS_DRIVER_NAME,
+		.owner	= THIS_MODULE,
+		.of_match_table = xnandps_of_match,
+	},
+};
+
+module_platform_driver(xnandps_driver);
+
+MODULE_AUTHOR("Xilinx, Inc.");
+MODULE_ALIAS("platform:" XNANDPS_DRIVER_NAME);
+MODULE_DESCRIPTION("Xilinx PS NAND Flash Driver");
+MODULE_LICENSE("GPL");
