mtd/st_smi: Add fixes for smi driver

index 7507e5d..63b10fc 100644
@@ -392,6 +392,8 @@ static int smi_write(unsigned int *src_addr, unsigned int *dst_addr,
 		return -1;
 	}
 
+	clrbits_le32(&smicntl->smi_sr, ERF1 | ERF2);
+
 	if (smi_wait_till_ready(banknum, CONFIG_SYS_FLASH_WRITE_TOUT))
 		return -EBUSY;
 

index 63b10fc..30bfa3f 100644
@@ -62,24 +62,24 @@ struct flash_device {
  * Currently the erase_cmd field is not used in this driver.
  */
 static struct flash_device flash_devices[] = {
-	FLASH_ID("st m25p16"     , 0xd8, 0x00152020, 0x100, 0x10000, 0x200000),
-	FLASH_ID("st m25p32"     , 0xd8, 0x00162020, 0x100, 0x10000, 0x400000),
-	FLASH_ID("st m25p64"     , 0xd8, 0x00172020, 0x100, 0x10000, 0x800000),
-	FLASH_ID("st m25p128"    , 0xd8, 0x00182020, 0x100, 0x40000, 0x1000000),
+	FLASH_ID("sp s25fl004"   , 0xd8, 0x00120201, 0x100, 0x10000, 0x80000),
+	FLASH_ID("sp s25fl008"   , 0xd8, 0x00130201, 0x100, 0x10000, 0x100000),
+	FLASH_ID("sp s25fl016"   , 0xd8, 0x00140201, 0x100, 0x10000, 0x200000),
+	FLASH_ID("sp s25fl032"   , 0xd8, 0x00150201, 0x100, 0x10000, 0x400000),
+	FLASH_ID("sp s25fl064"   , 0xd8, 0x00160201, 0x100, 0x10000, 0x800000),
 	FLASH_ID("st m25p05"     , 0xd8, 0x00102020, 0x80 , 0x8000 , 0x10000),
 	FLASH_ID("st m25p10"     , 0xd8, 0x00112020, 0x80 , 0x8000 , 0x20000),
+	FLASH_ID("st m25p16"     , 0xd8, 0x00152020, 0x100, 0x10000, 0x200000),
 	FLASH_ID("st m25p20"     , 0xd8, 0x00122020, 0x100, 0x10000, 0x40000),
+	FLASH_ID("st m25p32"     , 0xd8, 0x00162020, 0x100, 0x10000, 0x400000),
 	FLASH_ID("st m25p40"     , 0xd8, 0x00132020, 0x100, 0x10000, 0x80000),
+	FLASH_ID("st m25p64"     , 0xd8, 0x00172020, 0x100, 0x10000, 0x800000),
 	FLASH_ID("st m25p80"     , 0xd8, 0x00142020, 0x100, 0x10000, 0x100000),
+	FLASH_ID("st m25p128"    , 0xd8, 0x00182020, 0x100, 0x40000, 0x1000000),
 	FLASH_ID("st m45pe10"    , 0xd8, 0x00114020, 0x100, 0x10000, 0x20000),
 	FLASH_ID("st m45pe20"    , 0xd8, 0x00124020, 0x100, 0x10000, 0x40000),
 	FLASH_ID("st m45pe40"    , 0xd8, 0x00134020, 0x100, 0x10000, 0x80000),
 	FLASH_ID("st m45pe80"    , 0xd8, 0x00144020, 0x100, 0x10000, 0x100000),
-	FLASH_ID("sp s25fl004"   , 0xd8, 0x00120201, 0x100, 0x10000, 0x80000),
-	FLASH_ID("sp s25fl008"   , 0xd8, 0x00130201, 0x100, 0x10000, 0x100000),
-	FLASH_ID("sp s25fl016"   , 0xd8, 0x00140201, 0x100, 0x10000, 0x200000),
-	FLASH_ID("sp s25fl032"   , 0xd8, 0x00150201, 0x100, 0x10000, 0x400000),
-	FLASH_ID("sp s25fl064"   , 0xd8, 0x00160201, 0x100, 0x10000, 0x800000),
 	FLASH_ID("mac 25l512"    , 0xd8, 0x001020C2, 0x010, 0x10000, 0x10000),
 	FLASH_ID("mac 25l1005"   , 0xd8, 0x001120C2, 0x010, 0x10000, 0x20000),
 	FLASH_ID("mac 25l2005"   , 0xd8, 0x001220C2, 0x010, 0x10000, 0x40000),

@@ -91,7 +91,7 @@ static struct flash_device flash_devices[] = {
 	FLASH_ID("mac 25l3205"   , 0xd8, 0x001620C2, 0x100, 0x10000, 0x400000),
 	FLASH_ID("mac 25l3205a"  , 0xd8, 0x001620C2, 0x100, 0x10000, 0x400000),
 	FLASH_ID("mac 25l6405"   , 0xd8, 0x001720C2, 0x100, 0x10000, 0x800000),
-	FLASH_ID("wbd w25q128" , 0xd8, 0x001840EF, 0x100, 0x10000, 0x1000000),
+	FLASH_ID("wbd w25q128"   , 0xd8, 0x001840EF, 0x100, 0x10000, 0x1000000),
 };
 
 /*

index 30bfa3f..99b9576 100644
@@ -91,6 +91,7 @@ static struct flash_device flash_devices[] = {
 	FLASH_ID("mac 25l3205"   , 0xd8, 0x001620C2, 0x100, 0x10000, 0x400000),
 	FLASH_ID("mac 25l3205a"  , 0xd8, 0x001620C2, 0x100, 0x10000, 0x400000),
 	FLASH_ID("mac 25l6405"   , 0xd8, 0x001720C2, 0x100, 0x10000, 0x800000),
+	FLASH_ID("mcr n25q128"   , 0xd8, 0x0018BA20, 0x100, 0x10000, 0x1000000),
 	FLASH_ID("wbd w25q128"   , 0xd8, 0x001840EF, 0x100, 0x10000, 0x1000000),
 };
 

index 99b9576..6c3e594 100644
@@ -374,7 +374,7 @@ static int smi_write(unsigned int *src_addr, unsigned int *dst_addr,
 	u8 *src_addr8 = (u8 *)src_addr;
 	u8 *dst_addr8 = (u8 *)dst_addr;
 	int banknum;
-	int i;
+	int i, issue_we;
 
 	switch (bank_addr) {
 	case SMIBANK0_BASE:

@@ -394,19 +394,16 @@ static int smi_write(unsigned int *src_addr, unsigned int *dst_addr,
 	}
 
 	clrbits_le32(&smicntl->smi_sr, ERF1 | ERF2);
-
-	if (smi_wait_till_ready(banknum, CONFIG_SYS_FLASH_WRITE_TOUT))
-		return -EBUSY;
+	issue_we = 1;
 
 	/* Set SMI in Hardware Mode */
 	writel(readl(&smicntl->smi_cr1) & ~SW_MODE, &smicntl->smi_cr1);
 
-	if (smi_write_enable(banknum))
-		return -EIO;
-
 	/* Perform the write command */
 	for (i = 0; i < length; i += 4) {
-		if (((ulong) (dst_addr) % SFLASH_PAGE_SIZE) == 0) {
+		if (issue_we || (((ulong)(dst_addr) % SFLASH_PAGE_SIZE) == 0)) {
+			issue_we = 0;
+
 			if (smi_wait_till_ready(banknum,
 						CONFIG_SYS_FLASH_WRITE_TOUT))
 				return -EBUSY;

index 6c3e594..5f67807 100644
@@ -368,13 +368,11 @@ static int smi_sector_erase(flash_info_t *info, unsigned int sector)
  *
  * Write to SMI flash
  */
-static int smi_write(unsigned int *src_addr, unsigned int *dst_addr,
+static int smi_write(unsigned char *src_addr, unsigned char *dst_addr,
 		     unsigned int length, ulong bank_addr)
 {
-	u8 *src_addr8 = (u8 *)src_addr;
-	u8 *dst_addr8 = (u8 *)dst_addr;
 	int banknum;
-	int i, issue_we;
+	int issue_we;
 
 	switch (bank_addr) {
 	case SMIBANK0_BASE:

@@ -400,7 +398,10 @@ static int smi_write(unsigned int *src_addr, unsigned int *dst_addr,
 	writel(readl(&smicntl->smi_cr1) & ~SW_MODE, &smicntl->smi_cr1);
 
 	/* Perform the write command */
-	for (i = 0; i < length; i += 4) {
+	while (length) {
+		int k;
+		unsigned int wlen = min(SFLASH_PAGE_SIZE, length);
+
 		if (issue_we || (((ulong)(dst_addr) % SFLASH_PAGE_SIZE) == 0)) {
 			issue_we = 0;
 

@@ -412,19 +413,14 @@ static int smi_write(unsigned int *src_addr, unsigned int *dst_addr,
 				return -EIO;
 		}
 
-		if (length < 4) {
-			int k;
+		setbits_le32(&smicntl->smi_cr1, WB_MODE);
 
-			/*
-			 * Handle special case, where length < 4 (redundant env)
-			 */
-			for (k = 0; k < length; k++)
-				*dst_addr8++ = *src_addr8++;
-		} else {
-			/* Normal 32bit write */
+		for (k = 0; k < wlen; k++)
 			*dst_addr++ = *src_addr++;
-		}
 
+		clrbits_le32(&smicntl->smi_cr1, WB_MODE);
+
+		length -= wlen;
 		if ((readl(&smicntl->smi_sr) & (ERF1 | ERF2)))
 			return -EIO;
 	}

@@ -448,8 +444,8 @@ static int smi_write(unsigned int *src_addr, unsigned int *dst_addr,
  */
 int write_buff(flash_info_t *info, uchar *src, ulong dest_addr, ulong length)
 {
-	return smi_write((unsigned int *)src, (unsigned int *)dest_addr,
-			 length, info->start[0]);
+	return smi_write(src, (unsigned char *)dest_addr, length,
+			info->start[0]);
 }
 
 /*

index 5f67807..0ed6c0d 100644
@@ -96,6 +96,25 @@ static struct flash_device flash_devices[] = {
 };
 
 /*
+ * get_flash_device - Return flash_device pointer for a particular device id
+ * @id:	 Device id
+ *
+ * Return flash_device pointer for a particular device id
+ */
+static struct flash_device *get_flash_device(u32 id)
+{
+	struct flash_device *flash_dev_p = &flash_devices[0];
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(flash_devices); i++, flash_dev_p++) {
+		if (flash_dev_p->device_id == id)
+			return flash_dev_p;
+	}
+
+	return NULL;
+}
+
+/*
  * smi_wait_xfer_finish - Wait until TFF is set in status register
  * @timeout:	 timeout in milliseconds
  *

@@ -361,20 +380,27 @@ static int smi_sector_erase(flash_info_t *info, unsigned int sector)
 
 /*
  * smi_write - Write to SMI flash
+ * @info:	 flash info structure
  * @src_addr:	 source buffer
  * @dst_addr:	 destination buffer
  * @length:	 length to write in bytes
- * @bank:	 bank base address
  *
  * Write to SMI flash
  */
-static int smi_write(unsigned char *src_addr, unsigned char *dst_addr,
-		     unsigned int length, ulong bank_addr)
+static int smi_write(flash_info_t *info, unsigned char *src_addr,
+		unsigned char *dst_addr, unsigned int length)
 {
+	struct flash_device *flash_device_p = get_flash_device(info->flash_id);
+	u32 page_size;
 	int banknum;
 	int issue_we;
 
-	switch (bank_addr) {
+	if (!flash_device_p)
+		return -EIO;
+
+	page_size = flash_device_p->pagesize;
+
+	switch (info->start[0]) {
 	case SMIBANK0_BASE:
 		banknum = BANK0;
 		break;

@@ -400,9 +426,9 @@ static int smi_write(unsigned char *src_addr, unsigned char *dst_addr,
 	/* Perform the write command */
 	while (length) {
 		int k;
-		unsigned int wlen = min(SFLASH_PAGE_SIZE, length);
+		unsigned int wlen = min(page_size, length);
 
-		if (issue_we || (((ulong)(dst_addr) % SFLASH_PAGE_SIZE) == 0)) {
+		if (issue_we || (((ulong)(dst_addr) % page_size) == 0)) {
 			issue_we = 0;
 
 			if (smi_wait_till_ready(banknum,

@@ -444,8 +470,7 @@ static int smi_write(unsigned char *src_addr, unsigned char *dst_addr,
  */
 int write_buff(flash_info_t *info, uchar *src, ulong dest_addr, ulong length)
 {
-	return smi_write(src, (unsigned char *)dest_addr, length,
-			info->start[0]);
+	return smi_write(info, src, (unsigned char *)dest_addr, length);
 }
 
 /*

index 04f81ea..5837493 100644
@@ -108,7 +108,6 @@ struct flash_dev {
 	ushort sector_count;
 };
 
-#define SFLASH_PAGE_SIZE	0x100	/* flash page size */
 #define XFER_FINISH_TOUT	15	/* xfer finish timeout(in ms) */
 #define WMODE_TOUT		15	/* write enable timeout(in ms) */
 

index 0ed6c0d..c4780c3 100644
@@ -24,6 +24,7 @@
 #include <common.h>
 #include <flash.h>
 #include <linux/err.h>
+#include <linux/mtd/mtd.h>
 #include <linux/mtd/st_smi.h>
 
 #include <asm/io.h>

@@ -37,6 +38,14 @@ static ulong bank_base[CONFIG_SYS_MAX_FLASH_BANKS] =
     CONFIG_SYS_FLASH_ADDR_BASE;
 flash_info_t flash_info[CONFIG_SYS_MAX_FLASH_BANKS];
 
+#if defined(CONFIG_MTD_DEVICE)
+/* MTD interface for SMI devices */
+static struct mtd_info smi_mtd_info[CONFIG_SYS_MAX_FLASH_BANKS];
+static char smi_mtd_names[CONFIG_SYS_MAX_FLASH_BANKS][16];
+#endif
+
+static int smi_mtd_init(void);
+
 /* data structure to maintain flash ids from different vendors */
 struct flash_device {
 	char *name;

@@ -482,6 +491,7 @@ unsigned long flash_init(void)
 {
 	unsigned long size = 0;
 	int i, j;
+	int error;
 
 	smi_init();
 

@@ -498,6 +508,10 @@ unsigned long flash_init(void)
 
 	}
 
+	error = smi_mtd_init();
+	if (error < 0)
+		return 0;
+
 	return size;
 }
 

@@ -599,4 +613,155 @@ int flash_erase(flash_info_t *info, int s_first, int s_last)
 	puts(" done\n");
 	return rcode;
 }
-#endif
+
+#if defined(CONFIG_MTD_DEVICE)
+static int smi_mtd_erase(struct mtd_info *mtd, struct erase_info *instr)
+{
+	flash_info_t *fi = mtd->priv;
+	size_t a_start = fi->start[0] + instr->addr;
+	size_t a_end = a_start + instr->len;
+	int s_first = -1;
+	int s_last = -1;
+	int error, sect;
+
+	for (sect = 0; sect < fi->sector_count; sect++) {
+		if (a_start == fi->start[sect])
+			s_first = sect;
+
+		if (sect < fi->sector_count - 1) {
+			if (a_end == fi->start[sect + 1]) {
+				s_last = sect;
+				break;
+			}
+		} else {
+			s_last = sect;
+			break;
+		}
+	}
+
+	if (s_first >= 0 && s_first <= s_last) {
+		instr->state = MTD_ERASING;
+
+		error = flash_erase(fi, s_first, s_last);
+		if (error) {
+			instr->state = MTD_ERASE_FAILED;
+			return -EIO;
+		}
+
+		instr->state = MTD_ERASE_DONE;
+		mtd_erase_callback(instr);
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+static int smi_mtd_read(struct mtd_info *mtd, loff_t from, size_t len,
+	size_t *retlen, u_char *buf)
+{
+	flash_info_t *fi = mtd->priv;
+	u_char *f = (u_char *)(fi->start[0]) + from;
+
+	memcpy(buf, f, len);
+	*retlen = len;
+
+	return 0;
+}
+
+static int smi_mtd_write(struct mtd_info *mtd, loff_t to, size_t len,
+	size_t *retlen, const u_char *buf)
+{
+	flash_info_t *fi = mtd->priv;
+	u_long t = fi->start[0] + to;
+	int error;
+
+	error = write_buff(fi, (u_char *)buf, t, len);
+	if (!error) {
+		*retlen = len;
+		return 0;
+	}
+
+	return -EIO;
+}
+
+static void smi_mtd_sync(struct mtd_info *mtd)
+{
+	/*
+	 * This function should wait until all pending operations
+	 * finish. However this driver is fully synchronous, so
+	 * this function returns immediately
+	 */
+}
+
+static int smi_mtd_lock(struct mtd_info *mtd, loff_t ofs, uint64_t len)
+{
+	flash_info_t *fi = mtd->priv;
+
+	flash_protect(FLAG_PROTECT_SET, fi->start[0] + ofs,
+					fi->start[0] + ofs + len - 1, fi);
+	return 0;
+}
+
+static int smi_mtd_unlock(struct mtd_info *mtd, loff_t ofs, uint64_t len)
+{
+	flash_info_t *fi = mtd->priv;
+
+	flash_protect(FLAG_PROTECT_CLEAR, fi->start[0] + ofs,
+					fi->start[0] + ofs + len - 1, fi);
+	return 0;
+}
+
+static int smi_mtd_init(void)
+{
+	struct flash_device *flash_device_p;
+	struct mtd_info *mtd;
+	flash_info_t *fi;
+	int i;
+
+	for (i = 0; i < CONFIG_SYS_MAX_FLASH_BANKS; i++) {
+		fi = &flash_info[i];
+
+		if (!fi->size)
+			continue;
+
+		flash_device_p = get_flash_device(fi->flash_id);
+		if (!flash_device_p)
+			return -EIO;
+
+		mtd = &smi_mtd_info[i];
+
+		memset(mtd, 0, sizeof(struct mtd_info));
+
+		/* Uniform erase sizes for all sectors */
+		mtd->numeraseregions = 0;
+		mtd->erasesize = flash_device_p->sectorsize;
+
+		sprintf(smi_mtd_names[i], "nor%d", i);
+		mtd->name		= smi_mtd_names[i];
+		mtd->type		= MTD_NORFLASH;
+		mtd->flags		= MTD_CAP_NORFLASH;
+		mtd->size		= fi->size;
+		mtd->writesize		= 1;
+
+		mtd->erase		= smi_mtd_erase;
+		mtd->read		= smi_mtd_read;
+		mtd->write		= smi_mtd_write;
+		mtd->sync		= smi_mtd_sync;
+		mtd->lock		= smi_mtd_lock;
+		mtd->unlock		= smi_mtd_unlock;
+		mtd->priv		= fi;
+
+		if (add_mtd_device(mtd))
+			return -ENOMEM;
+	}
+
+	return 0;
+}
+#else
+static int smi_mtd_init(void)
+{
+	return 0;
+}
+#endif /* CONFIG_MTD_DEVICE */
+
+#endif /* CONFIG_SYS_NO_FLASH */