path: root/target/linux/storm/patches/007-mtd.patch

--- a/drivers/mtd/chips/Kconfig
+++ b/drivers/mtd/chips/Kconfig
@@ -220,6 +220,13 @@
 	  This option enables basic support for ROM chips accessed through
 	  a bus mapping driver.
 
+config MTD_SERIAL
+	tristate "Support for Serial chips in bus mapping"
+	depends on MTD
+	help
+	  This option enables basic support for Serial chips accessed through
+	  a bus mapping driver.
+
 config MTD_ABSENT
 	tristate "Support for absent chips in bus mapping"
 	help
--- a/drivers/mtd/chips/cfi_cmdset_0002.c
+++ b/drivers/mtd/chips/cfi_cmdset_0002.c
@@ -39,10 +39,15 @@
 #include <linux/mtd/cfi.h>
 #include <linux/mtd/xip.h>
 
+//****** Storlink SoC ******
 #define AMD_BOOTLOC_BUG
-#define FORCE_WORD_WRITE 0
-
-#define MAX_WORD_RETRIES 3
+//#define FORCE_WORD_WRITE 0
+#define FORCE_WORD_WRITE 1
+#define FORCE_FAST_PROG 0
+
+//#define MAX_WORD_RETRIES 3
+#define MAX_WORD_RETRIES 3 // CONFIG_MTD_CFI_AMDSTD_RETRY
+//**************************
 
 #define MANUFACTURER_AMD	0x0001
 #define MANUFACTURER_ATMEL	0x001F
@@ -322,6 +327,13 @@
 #endif
 
 		bootloc = extp->TopBottom;
+//****** Storlink SoC ******
+		if(bootloc == 5)
+		{
+			bootloc = 3;
+			extp->TopBottom = 3;
+		}
+//**************************
 		if ((bootloc != 2) && (bootloc != 3)) {
 			printk(KERN_WARNING "%s: CFI does not contain boot "
 			       "bank location. Assuming top.\n", map->name);
@@ -340,6 +352,9 @@
 				cfi->cfiq->EraseRegionInfo[j] = swap;
 			}
 		}
+#ifdef CONFIG_MTD_MAP_BANK_WIDTH_1
+		cfi->device_type = CFI_DEVICETYPE_X8;
+#endif
 		/* Set the default CFI lock/unlock addresses */
 		cfi->addr_unlock1 = 0x555;
 		cfi->addr_unlock2 = 0x2aa;
@@ -461,6 +476,7 @@
 	map_word d, t;
 
 	d = map_read(map, addr);
+	udelay(20);	//Storlink SoC
 	t = map_read(map, addr);
 
 	return map_word_equal(map, d, t);
@@ -626,7 +642,9 @@
 	default:
 		printk(KERN_ERR "MTD: put_chip() called with oldstate %d!!\n", chip->oldstate);
 	}
+//****** Storlink SoC ******
 	wake_up(&chip->wq);
+//**************************
 }
 
 #ifdef CONFIG_MTD_XIP
@@ -940,7 +958,9 @@
 	cfi_send_gen_cmd(0x90, cfi->addr_unlock1, chip->start, map, cfi, cfi->device_type, NULL);
 	cfi_send_gen_cmd(0x00, cfi->addr_unlock1, chip->start, map, cfi, cfi->device_type, NULL);
 
+//****** Storlink SoC ******
 	wake_up(&chip->wq);
+//**************************
 	spin_unlock(chip->mutex);
 
 	return 0;
@@ -1005,7 +1025,10 @@
 	 */
 	unsigned long uWriteTimeout = ( HZ / 1000 ) + 1;
 	int ret = 0;
-	map_word oldd;
+//****** Storlink SoC ******
+//	map_word oldd;
+	map_word oldd, tmp;
+//**************************
 	int retry_cnt = 0;
 
 	adr += chip->start;
@@ -1037,9 +1060,15 @@
 	ENABLE_VPP(map);
 	xip_disable(map, chip, adr);
  retry:
+//****** Storlink SoC ******
+#if FORCE_FAST_PROG  /* Unlock bypass */
+	cfi_send_gen_cmd(0xA0, cfi->addr_unlock1, chip->start, map, cfi, cfi->device_type, NULL);
+#else
 	cfi_send_gen_cmd(0xAA, cfi->addr_unlock1, chip->start, map, cfi, cfi->device_type, NULL);
 	cfi_send_gen_cmd(0x55, cfi->addr_unlock2, chip->start, map, cfi, cfi->device_type, NULL);
 	cfi_send_gen_cmd(0xA0, cfi->addr_unlock1, chip->start, map, cfi, cfi->device_type, NULL);
+#endif
+//**************************
 	map_write(map, datum, adr);
 	chip->state = FL_WRITING;
 
@@ -1072,7 +1101,13 @@
 		}
 
 		if (chip_ready(map, adr))
-			break;
+		{
+			tmp = map_read(map, adr);
+			if(map_word_equal(map, tmp, datum))
+//				goto op_done;
+                break;
+
+		}
 
 		/* Latency issues. Drop the lock, wait a while and retry */
 		UDELAY(map, chip, adr, 1);
@@ -1084,8 +1119,17 @@
 		/* FIXME - should have reset delay before continuing */
 
 		if (++retry_cnt <= MAX_WORD_RETRIES)
+		{
+//****** Storlink SoC ******
+#if FORCE_FAST_PROG
+			cfi_send_gen_cmd(0xAA, cfi->addr_unlock1, chip->start, map, cfi, cfi->device_type, NULL);
+			cfi_send_gen_cmd(0x55, cfi->addr_unlock2, chip->start, map, cfi, cfi->device_type, NULL);
+			cfi_send_gen_cmd(0x20, cfi->addr_unlock1, chip->start, map, cfi, cfi->device_type, NULL);
+		//udelay(1);
+#endif
+			udelay(1);
 			goto retry;
-
+		}
 		ret = -EIO;
 	}
 	xip_enable(map, chip, adr);
@@ -1171,7 +1215,14 @@
 				return 0;
 		}
 	}
-
+//****** Storlink SoC ******
+	map_write( map, CMD(0xF0), chipstart );
+#if FORCE_FAST_PROG
+		cfi_send_gen_cmd(0xAA, cfi->addr_unlock1, chipstart, map, cfi, cfi->device_type, NULL);
+		cfi_send_gen_cmd(0x55, cfi->addr_unlock2, chipstart, map, cfi, cfi->device_type, NULL);
+		cfi_send_gen_cmd(0x20, cfi->addr_unlock1, chipstart, map, cfi, cfi->device_type, NULL);
+#endif
+//**************************
 	/* We are now aligned, write as much as possible */
 	while(len >= map_bankwidth(map)) {
 		map_word datum;
@@ -1181,7 +1232,15 @@
 		ret = do_write_oneword(map, &cfi->chips[chipnum],
 				       ofs, datum);
 		if (ret)
+		{
+//****** Storlink SoC ******
+#if FORCE_FAST_PROG
+			/* Get out of unlock bypass mode */
+			cfi_send_gen_cmd(0x90, 0, chipstart, map, cfi, cfi->device_type, NULL);
+			cfi_send_gen_cmd(0x00, 0, chipstart, map, cfi, cfi->device_type, NULL);
+#endif
 			return ret;
+		}
 
 		ofs += map_bankwidth(map);
 		buf += map_bankwidth(map);
@@ -1189,19 +1248,38 @@
 		len -= map_bankwidth(map);
 
 		if (ofs >> cfi->chipshift) {
+//****** Storlink SoC ******
+#if FORCE_FAST_PROG
+			/* Get out of unlock bypass mode */
+			cfi_send_gen_cmd(0x90, 0, chipstart, map, cfi, cfi->device_type, NULL);
+			cfi_send_gen_cmd(0x00, 0, chipstart, map, cfi, cfi->device_type, NULL);
+#endif
 			chipnum ++;
 			ofs = 0;
 			if (chipnum == cfi->numchips)
 				return 0;
 			chipstart = cfi->chips[chipnum].start;
+#if FORCE_FAST_PROG
+			/* Go into unlock bypass mode for next set of chips */
+			cfi_send_gen_cmd(0xAA, cfi->addr_unlock1, chipstart, map, cfi, cfi->device_type, NULL);
+			cfi_send_gen_cmd(0x55, cfi->addr_unlock2, chipstart, map, cfi, cfi->device_type, NULL);
+			cfi_send_gen_cmd(0x20, cfi->addr_unlock1, chipstart, map, cfi, cfi->device_type, NULL);
+#endif
 		}
 	}
 
+#if FORCE_FAST_PROG
+	/* Get out of unlock bypass mode */
+	cfi_send_gen_cmd(0x90, 0, chipstart, map, cfi, cfi->device_type, NULL);
+	cfi_send_gen_cmd(0x00, 0, chipstart, map, cfi, cfi->device_type, NULL);
+#endif
+
 	/* Write the trailing bytes if any */
 	if (len & (map_bankwidth(map)-1)) {
 		map_word tmp_buf;
 
  retry1:
+
 		spin_lock(cfi->chips[chipnum].mutex);
 
 		if (cfi->chips[chipnum].state != FL_READY) {
@@ -1221,7 +1299,11 @@
 #endif
 			goto retry1;
 		}
-
+#if FORCE_FAST_PROG
+		cfi_send_gen_cmd(0xAA, cfi->addr_unlock1, chipstart, map, cfi, cfi->device_type, NULL);
+		cfi_send_gen_cmd(0x55, cfi->addr_unlock2, chipstart, map, cfi, cfi->device_type, NULL);
+		cfi_send_gen_cmd(0x20, cfi->addr_unlock1, chipstart, map, cfi, cfi->device_type, NULL);
+#endif
 		tmp_buf = map_read(map, ofs + chipstart);
 
 		spin_unlock(cfi->chips[chipnum].mutex);
@@ -1231,11 +1313,23 @@
 		ret = do_write_oneword(map, &cfi->chips[chipnum],
 				ofs, tmp_buf);
 		if (ret)
+		{
+#if FORCE_FAST_PROG
+	/* Get out of unlock bypass mode */
+	cfi_send_gen_cmd(0x90, 0, chipstart, map, cfi, cfi->device_type, NULL);
+	cfi_send_gen_cmd(0x00, 0, chipstart, map, cfi, cfi->device_type, NULL);
+#endif
 			return ret;
-
+		}
+#if FORCE_FAST_PROG
+	/* Get out of unlock bypass mode */
+	cfi_send_gen_cmd(0x90, 0, chipstart, map, cfi, cfi->device_type, NULL);
+	cfi_send_gen_cmd(0x00, 0, chipstart, map, cfi, cfi->device_type, NULL);
+#endif
 		(*retlen) += len;
 	}
 
+	map_write( map, CMD(0xF0), chipstart );
 	return 0;
 }
 
@@ -1275,6 +1369,7 @@
 	ENABLE_VPP(map);
 	xip_disable(map, chip, cmd_adr);
 
+	map_write( map, CMD(0xF0), chip->start );	//Storlink
 	cfi_send_gen_cmd(0xAA, cfi->addr_unlock1, chip->start, map, cfi, cfi->device_type, NULL);
 	cfi_send_gen_cmd(0x55, cfi->addr_unlock2, chip->start, map, cfi, cfi->device_type, NULL);
 	//cfi_send_gen_cmd(0xA0, cfi->addr_unlock1, chip->start, map, cfi, cfi->device_type, NULL);
@@ -1535,6 +1630,9 @@
 	DECLARE_WAITQUEUE(wait, current);
 	int ret = 0;
 
+#ifdef CONFIG_SL2312_SHARE_PIN
+	mtd_lock();				// sl2312 share pin lock
+#endif
 	adr += chip->start;
 
 	spin_lock(chip->mutex);
@@ -1613,6 +1711,9 @@
 	chip->state = FL_READY;
 	put_chip(map, chip, adr);
 	spin_unlock(chip->mutex);
+#ifdef CONFIG_SL2312_SHARE_PIN
+	mtd_unlock();				// sl2312 share pin lock
+#endif
 	return ret;
 }
 
--- /dev/null
+++ b/drivers/mtd/chips/map_serial.c
@@ -0,0 +1,188 @@
+/*
+ * Common code to handle map devices which are simple ROM
+ * (C) 2000 Red Hat. GPL'd.
+ * $Id: map_serial.c,v 1.3 2006/06/05 02:34:54 middle Exp $
+ */
+
+#include <linux/version.h>
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <asm/io.h>
+
+#include <asm/byteorder.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+
+#include <asm/hardware.h>
+#include <linux/mtd/map.h>
+#include <linux/mtd/mtd.h>
+#include <linux/init.h> //add
+#include <asm/arch/sl2312.h>
+#include <asm/arch/flash.h>
+
+static int mapserial_erase(struct mtd_info *mtd, struct erase_info *instr);
+static int mapserial_read (struct mtd_info *, loff_t, size_t, size_t *, u_char *);
+static int mapserial_write (struct mtd_info *, loff_t, size_t, size_t *, const u_char *);
+static void mapserial_nop (struct mtd_info *);
+struct mtd_info *map_serial_probe(struct map_info *map);
+
+extern int m25p80_sector_erase(__u32 address, __u32 schip_en);
+
+static struct mtd_chip_driver mapserial_chipdrv = {
+	probe: map_serial_probe,
+	name: "map_serial",
+	module: THIS_MODULE
+};
+
+struct mtd_info *map_serial_probe(struct map_info *map)
+{
+	struct mtd_info *mtd;
+
+	mtd = kmalloc(sizeof(*mtd), GFP_KERNEL);
+	if (!mtd)
+		return NULL;
+
+	memset(mtd, 0, sizeof(*mtd));
+
+	map->fldrv = &mapserial_chipdrv;
+	mtd->priv = map;
+	mtd->name = map->name;
+	mtd->type = MTD_OTHER;
+	mtd->erase = mapserial_erase;
+	mtd->size = map->size;
+	mtd->read = mapserial_read;
+	mtd->write = mapserial_write;
+	mtd->sync = mapserial_nop;
+	mtd->flags = (MTD_WRITEABLE|MTD_ERASEABLE);
+//	mtd->erasesize = 512; // page size;
+#ifdef CONFIG_MTD_SL2312_SERIAL_ST
+	mtd->erasesize = M25P80_SECTOR_SIZE; // block size;
+#else
+	mtd->erasesize = 0x1000; // block size;
+#endif
+
+	__module_get(THIS_MODULE);
+	//MOD_INC_USE_COUNT;
+	return mtd;
+}
+
+#define	FLASH_ACCESS_OFFSET	        		0x00000010
+#define	FLASH_ADDRESS_OFFSET            		0x00000014
+#define	FLASH_WRITE_DATA_OFFSET         		0x00000018
+#define	FLASH_READ_DATA_OFFSET          		0x00000018
+
+static __u32 readflash_ctrl_reg(__u32 ofs)
+{
+    __u32 *base;
+
+    base = (__u32 *)IO_ADDRESS((SL2312_FLASH_CTRL_BASE + ofs));
+    return __raw_readl(base);
+}
+
+static void writeflash_ctrl_reg(__u32 data, __u32 ofs)
+{
+    __u32 *base;
+
+    base = (__u32 *)IO_ADDRESS((SL2312_FLASH_CTRL_BASE + ofs));
+    __raw_writel(data, base);
+}
+
+static int mapserial_erase_block(struct map_info *map,unsigned int block)
+{
+
+	__u32 address;
+#ifdef CONFIG_MTD_SL2312_SERIAL_ST
+
+	if(!m25p80_sector_erase(block, 0))
+		return (MTD_ERASE_DONE);
+#else
+      __u32 opcode;
+      __u32 count=0;
+//      __u8  status;
+
+ //     printk("mapserial_erase_block : erase block %d \n",block);
+//      opcode = 0x80000000 | FLASH_ACCESS_ACTION_SHIFT_ADDRESS | cmd;
+      opcode = 0x80000000 | 0x0200 | 0x50;
+      address = (block << 13);
+      writeflash_ctrl_reg(address,FLASH_ADDRESS_OFFSET);
+      writeflash_ctrl_reg(opcode,FLASH_ACCESS_OFFSET);
+      opcode=readflash_ctrl_reg(FLASH_ACCESS_OFFSET);
+      while(opcode&0x80000000)
+      {
+          opcode = readflash_ctrl_reg(FLASH_ACCESS_OFFSET);
+          count++;
+          if (count > 10000)
+          {
+            return (MTD_ERASE_FAILED);
+          }
+      }
+      return (MTD_ERASE_DONE);
+#endif
+}
+
+static int mapserial_erase(struct mtd_info *mtd, struct erase_info *instr)
+{
+	struct map_info *map = (struct map_info *)mtd->priv;
+    unsigned int    addr;
+    int             len;
+    unsigned int    block;
+    unsigned int    ret=0;
+
+	addr = instr->addr;
+	len = instr->len;
+    while (len > 0)
+    {
+        block = addr / mtd->erasesize;
+#ifdef CONFIG_MTD_SL2312_SERIAL_ST
+        ret = mapserial_erase_block(map,addr);
+#else
+		ret = mapserial_erase_block(map,block);
+#endif
+        addr = addr + mtd->erasesize;
+        len = len - mtd->erasesize;
+    }
+    return (ret);
+}
+
+static int mapserial_read (struct mtd_info *mtd, loff_t from, size_t len, size_t *retlen, u_char *buf)
+{
+	struct map_info *map = (struct map_info *)mtd->priv;
+//        printk("mapserial_read : \n");
+	map->copy_from(map, buf, from, len);
+	*retlen = len;
+	return 0;
+}
+
+static void mapserial_nop(struct mtd_info *mtd)
+{
+	/* Nothing to see here */
+}
+
+static int mapserial_write (struct mtd_info *mtd, loff_t to, size_t len, size_t *retlen, const u_char *buf)
+{
+	struct map_info *map = (struct map_info *)mtd->priv;
+//	printk("mapserial_write : buf %x to %x len %x \n",(int)buf, (int)to, (int)len);
+	//map->copy_to(map, buf, to, len);
+	map->copy_to(map, to, buf, len);
+	*retlen = len;
+	return 0;
+}
+
+int __init map_serial_init(void)
+{
+	register_mtd_chip_driver(&mapserial_chipdrv);
+	return 0;
+}
+
+static void __exit map_serial_exit(void)
+{
+	unregister_mtd_chip_driver(&mapserial_chipdrv);
+}
+
+module_init(map_serial_init);
+module_exit(map_serial_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("David Woodhouse <dwmw2@infradead.org>");
+MODULE_DESCRIPTION("MTD chip driver for ROM chips");
--- a/drivers/mtd/maps/Kconfig
+++ b/drivers/mtd/maps/Kconfig
@@ -614,5 +614,30 @@
 
 	  This selection automatically selects the map_ram driver.
 
+#***************************************************************************************
+# Storlink parallel/Serial Flash configuration
+#***************************************************************************************
+config MTD_SL2312_CFI
+        tristate "CFI Flash device mapped on SL2312"
+        depends on MTD_CFI
+        help
+          Map driver for SL2312 demo board.
+
+config MTD_SL2312_SERIAL_ATMEL
+        tristate "ATMEL Serial Flash device mapped on SL2312"
+        depends on MTD_PARTITIONS && ARCH_SL2312
+        help
+          Map driver for SL2312 demo board.
+
+config MTD_SL2312_SERIAL_ST
+        tristate "ST Serial Flash device mapped on SL2312"
+        depends on MTD_PARTITIONS && ARCH_SL2312
+        help
+          Map driver for SL2312 demo board.
+
+config SL2312_SHARE_PIN
+        tristate "Parallel Flash share pin on SL2312 ASIC"
+        depends on SL3516_ASIC
+
 endmenu
 
--- /dev/null
+++ b/drivers/mtd/maps/sl2312-flash-atmel.c
@@ -0,0 +1,554 @@
+/*
+ * $Id: sl2312-flash-atmel.c,v 1.2 2006/06/05 02:35:57 middle Exp $
+ *
+ * Flash and EPROM on Hitachi Solution Engine and similar boards.
+ *
+ * (C) 2001 Red Hat, Inc.
+ *
+ * GPL'd
+ */
+
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/kernel.h>
+
+#include <asm/io.h>
+#include <linux/mtd/mtd.h>
+#include <linux/mtd/map.h>
+#include <linux/mtd/partitions.h>
+#include <asm/hardware.h>
+
+#include <asm/arch/sl2312.h>
+#include <asm/arch/flash.h>
+#include <linux/init.h> //add
+
+
+#define  g_page_addr  AT45DB321_PAGE_SHIFT    //321 : shift 10  ; 642 : shift 11
+#define  g_chipen     SERIAL_FLASH_CHIP0_EN   //atmel
+
+extern int parse_redboot_partitions(struct mtd_info *master, struct mtd_partition **pparts);
+
+void address_to_page(__u32 address, __u16 *page, __u16 *offset)
+{
+    *page = address / SPAGE_SIZE;
+    *offset = address % SPAGE_SIZE;
+}
+
+static __u32 read_flash_ctrl_reg(__u32 ofs)
+{
+    __u32 *base;
+
+    base = (__u32 *)IO_ADDRESS((SL2312_FLASH_CTRL_BASE + ofs));
+    return __raw_readl(base);
+}
+
+static void write_flash_ctrl_reg(__u32 ofs,__u32 data)
+{
+    __u32 *base;
+
+    base = (__u32 *)IO_ADDRESS((SL2312_FLASH_CTRL_BASE + ofs));
+    __raw_writel(data, base);
+}
+
+void atmel_read_status(__u8 cmd, __u8 *data)
+{
+      __u32 opcode;
+      __u32 value;
+
+      opcode = 0x80000000 | FLASH_ACCESS_ACTION_OPCODE_DATA | cmd | g_chipen;
+      write_flash_ctrl_reg(FLASH_ACCESS_OFFSET, opcode);
+      opcode=read_flash_ctrl_reg(FLASH_ACCESS_OFFSET);
+      while(opcode&0x80000000)
+      {
+          opcode=read_flash_ctrl_reg(FLASH_ACCESS_OFFSET);
+          flash_delay();
+          schedule();
+      }
+
+      value=read_flash_ctrl_reg(FLASH_READ_DATA_OFFSET);
+      *data = value & 0xff;
+}
+
+void main_memory_page_read(__u8 cmd, __u16 page, __u16 offset, __u8 *data)
+{
+      __u32 opcode;
+      __u32 address;
+      __u32 value;
+
+      opcode = 0x80000000 | FLASH_ACCESS_ACTION_SHIFT_ADDRESS_4X_DATA | cmd | g_chipen;
+      address = (page << g_page_addr) + offset;
+      write_flash_ctrl_reg(FLASH_ADDRESS_OFFSET, address);
+      write_flash_ctrl_reg(FLASH_ACCESS_OFFSET, opcode);
+      opcode=read_flash_ctrl_reg(FLASH_ACCESS_OFFSET);
+      while(opcode&0x80000000)
+      {
+          opcode=read_flash_ctrl_reg(FLASH_ACCESS_OFFSET);
+          flash_delay();
+          schedule();
+      }
+
+      value=read_flash_ctrl_reg(FLASH_READ_DATA_OFFSET);
+      *data = value & 0xff;
+}
+
+void buffer_to_main_memory(__u8 cmd, __u16 page)
+{
+      __u32 opcode;
+      __u32 address;
+      __u8  status;
+
+      opcode = 0x80000000 | FLASH_ACCESS_ACTION_SHIFT_ADDRESS | cmd | g_chipen;
+      address = (page << g_page_addr);
+      write_flash_ctrl_reg(FLASH_ADDRESS_OFFSET, address);
+      write_flash_ctrl_reg(FLASH_ACCESS_OFFSET, opcode);
+      opcode=read_flash_ctrl_reg(FLASH_ACCESS_OFFSET);
+      while(opcode&0x80000000)
+      {
+          opcode=read_flash_ctrl_reg(FLASH_ACCESS_OFFSET);
+          flash_delay();
+          schedule();
+      }
+      atmel_read_status(READ_STATUS_SPI, &status);
+      while(!(status&0x80))
+      {
+          atmel_read_status(READ_STATUS_SPI, &status);
+          flash_delay();
+          schedule();
+      }
+
+}
+
+
+void atmel_flash_read_page(__u32 address, __u8 *buffer, __u32 len)
+{
+    __u8  byte;
+    __u16 page, offset;
+    __u16 i;
+
+    address_to_page(address, &page, &offset);
+
+     for(i=0; i<len; i++,offset++)
+    {
+        main_memory_page_read(MAIN_MEMORY_PAGE_READ_SPI , page, offset, &byte);
+        buffer [i]= byte;
+    }
+}
+
+void atmel_flash_program_page(__u32 address, __u8 *buffer, __u32 len)
+{
+    __u8  pattern;
+    __u16 page, offset;
+    __u32 i;
+
+    address_to_page(address, &page, &offset);
+ //   printk("atmel_flash_program_page: offset %x len %x page %x \n", offset, len, page);
+
+    if(offset)
+ 	    main_memory_to_buffer(MAIN_MEMORY_TO_BUFFER1,page);
+
+    for(i=0; i<len; i++,offset++)
+    {
+        pattern = buffer[i];
+        atmel_buffer_write(BUFFER1_WRITE,offset,pattern);
+    }
+
+  //  printk("atmel_flash_program_page: offset %x \n", offset);
+    buffer_to_main_memory(BUFFER1_TO_MAIN_MEMORY, page);
+  //  printk("atmel_flash_program_page: buffer_to_main_memory %x page\n", page);
+
+}
+
+
+void main_memory_to_buffer(__u8 cmd, __u16 page)
+{
+      __u32 opcode;
+      __u32 address;
+      __u8  status;
+
+      opcode = 0x80000000 | FLASH_ACCESS_ACTION_SHIFT_ADDRESS | cmd | g_chipen;
+      address = (page << g_page_addr);
+      write_flash_ctrl_reg(FLASH_ADDRESS_OFFSET, address);
+      write_flash_ctrl_reg(FLASH_ACCESS_OFFSET, opcode);
+      opcode=read_flash_ctrl_reg(FLASH_ACCESS_OFFSET);
+      while(opcode&0x80000000)
+      {
+          opcode=read_flash_ctrl_reg(FLASH_ACCESS_OFFSET);
+          flash_delay();
+          schedule();
+      }
+      atmel_read_status(READ_STATUS_SPI, &status);
+      while(!(status&0x80))
+      {
+          atmel_read_status(READ_STATUS_SPI, &status);
+          flash_delay();
+          schedule();
+      }
+
+}
+
+void main_memory_page_program(__u8 cmd, __u16 page, __u16 offset, __u8 data)
+{
+      __u32 opcode;
+      __u32 address;
+      __u8  status;
+
+      opcode = 0x80000000 | FLASH_ACCESS_ACTION_SHIFT_ADDRESS_DATA | cmd | g_chipen;
+      address = (page << g_page_addr) + offset;
+      write_flash_ctrl_reg(FLASH_ADDRESS_OFFSET, address);
+      write_flash_ctrl_reg(FLASH_WRITE_DATA_OFFSET, data);
+      write_flash_ctrl_reg(FLASH_ACCESS_OFFSET, opcode);
+      opcode=read_flash_ctrl_reg(FLASH_ACCESS_OFFSET);
+      while(opcode&0x80000000)
+      {
+          opcode=read_flash_ctrl_reg(FLASH_ACCESS_OFFSET);
+          flash_delay();
+          schedule();
+      }
+      atmel_read_status(READ_STATUS_SPI, &status);
+      while(!(status&0x80))
+      {
+          atmel_read_status(READ_STATUS_SPI, &status);
+          flash_delay();
+          schedule();
+      }
+}
+
+void atmel_buffer_write(__u8 cmd, __u16 offset, __u8 data)
+{
+      __u32 opcode;
+      __u32 address;
+
+      opcode = 0x80000000 | FLASH_ACCESS_ACTION_SHIFT_ADDRESS_DATA | cmd  | g_chipen;
+      address = offset;
+      write_flash_ctrl_reg(FLASH_ADDRESS_OFFSET, address);
+      write_flash_ctrl_reg(FLASH_WRITE_DATA_OFFSET, data);
+      write_flash_ctrl_reg(FLASH_ACCESS_OFFSET, opcode);
+      opcode=read_flash_ctrl_reg(FLASH_ACCESS_OFFSET);
+      while(opcode&0x80000000)
+      {
+          opcode=read_flash_ctrl_reg(FLASH_ACCESS_OFFSET);
+          flash_delay();
+          schedule();
+      }
+
+}
+
+void atmel_erase_page(__u8 cmd, __u16 page)
+{
+      __u32 opcode;
+      __u32 address;
+      __u8  status;
+
+      opcode = 0x80000000 | FLASH_ACCESS_ACTION_SHIFT_ADDRESS | cmd | g_chipen;
+      address = (page << g_page_addr);
+      write_flash_ctrl_reg(FLASH_ADDRESS_OFFSET, address);
+      write_flash_ctrl_reg(FLASH_ACCESS_OFFSET, opcode);
+      opcode=read_flash_ctrl_reg(FLASH_ACCESS_OFFSET);
+      while(opcode&0x80000000)
+      {
+          opcode=read_flash_ctrl_reg(FLASH_ACCESS_OFFSET);
+          flash_delay();
+          schedule();
+      }
+      atmel_read_status(READ_STATUS_SPI, &status);
+      while(!(status&0x80))
+      {
+          atmel_read_status(READ_STATUS_SPI, &status);
+          flash_delay();
+          schedule();
+      }
+
+}
+
+void atmel_erase_block(__u8 cmd, __u16 block)
+{
+      __u32 opcode;
+      __u32 address;
+      __u8  status;
+
+      opcode = 0x80000000 | FLASH_ACCESS_ACTION_SHIFT_ADDRESS | cmd | g_chipen;
+      address = (block << 13);
+      write_flash_ctrl_reg(FLASH_ADDRESS_OFFSET, address);
+      write_flash_ctrl_reg(FLASH_ACCESS_OFFSET, opcode);
+      opcode=read_flash_ctrl_reg(FLASH_ACCESS_OFFSET);
+      while(opcode&0x80000000)
+      {
+          opcode=read_flash_ctrl_reg(FLASH_ACCESS_OFFSET);
+          flash_delay();
+          schedule();
+      }
+      atmel_read_status(READ_STATUS_SPI, &status);
+      while(!(status&0x80))
+      {
+          atmel_read_status(READ_STATUS_SPI, &status);
+          flash_delay();
+          schedule();
+      }
+
+}
+
+void flash_delay(void)
+{
+      int i;
+
+      for(i=0; i<50; i++)
+           i=i;
+}
+
+
+
+
+__u32 sl2312_read32(struct map_info *map, unsigned long ofs)
+{
+
+#if 0
+    __u16 page, offset;
+    __u32 pattern;
+    __u8  byte, i;
+
+     pattern = 0;
+     address_to_page(ofs, &page, &offset);
+     for(i=0; i<4; i++, offset++)
+    {
+        pattern = pattern << 8;
+        main_memory_page_read(MAIN_MEMORY_PAGE_READ_SPI , page, offset, &byte);
+//printk("sl2312_read32:: address = %08x  data = %c \n",ofs,byte);
+        pattern += byte;
+    }
+    return pattern;
+#else
+      return read_flash_ctrl_reg(ofs);
+#endif
+
+}
+
+__u8 sl2312_read8(struct map_info *map, unsigned long ofs)
+{
+    __u16 page, offset;
+    __u8  byte;
+
+     address_to_page(ofs, &page, &offset);
+     main_memory_page_read(MAIN_MEMORY_PAGE_READ_SPI , page, offset, &byte);
+	 //printk("sl2312_read8:: address = %08x  data = %c \n",ofs,byte);
+     return byte;
+
+}
+
+void sl2312_write32(struct map_info *map, __u32 d, unsigned long ofs)
+{
+#if 0
+    __u16 page, offset;
+    __u8  byte, i;
+
+     address_to_page(ofs, &page, &offset);
+     for(i=0; i<4; i++, offset++)
+    {
+    	byte = d & 0xff;
+        main_memory_page_program(MAIN_MEMORY_PROGRAM_BUFFER1, page, offset, byte);
+        d = d >> 8;
+//printk("sl2312_write32:: address = %08x  data = %c \n",ofs,byte);
+    }
+#else
+      write_flash_ctrl_reg(ofs, d);
+#endif
+}
+
+void sl2312_write8(struct map_info *map, __u8 d, unsigned long ofs)
+{
+     __u16 page, offset;
+
+     address_to_page(ofs, &page, &offset);
+     main_memory_page_program(MAIN_MEMORY_PROGRAM_BUFFER1, page, offset, d);
+//printk("sl2312_write8:: address = %08x  data = %c \n",ofs,d);
+
+}
+
+void sl2312_copy_from(struct map_info *map, void *buf, unsigned long ofs, ssize_t len)
+{
+     __u32 size;
+     __u8  *buffer;
+     __u32 length;//i, j,
+
+     //printk("sl2312_copy_from:: address = %08x  datalen = %d \n",ofs,len);
+
+     length = len;
+     buffer = (__u8 *)buf;
+     while(len)
+     {
+        size = SPAGE_SIZE - (ofs%SPAGE_SIZE);
+        if(size > len)
+            size = len;
+        atmel_flash_read_page(ofs, buffer, size);
+        buffer+=size;
+        ofs+=size;
+        len -= size;
+     }
+
+#if 0
+        buffer = (__u8 *)buf;
+        for(i=0; i<length; i+=16)
+       {
+          for(j=0; j<16; j++,buffer++)
+         {
+            if((i*16+j)<length)
+              printk("%x  ",(int)*buffer);
+	 }
+          printk("\n");
+       }
+
+       printk("\n");
+#endif
+
+}
+
+
+void sl2312_copy_to(struct map_info *map, unsigned long ofs, void *buf, ssize_t len)
+{
+     __u32 size;
+     __u8  *buffer;
+
+     buffer = (__u8 *)buf;
+     //printk("sl2312_copy_to:offset %x len %x \n", ofs, len);
+//     printk("sl2312_copy_to:buf is %x \n", (int)buf);
+
+     while(len)
+     {
+        size = SPAGE_SIZE - (ofs%SPAGE_SIZE);
+        if(size > len)
+            size = len;
+        atmel_flash_program_page(ofs, buffer, size);
+        buffer+=size;
+        ofs+=size;
+	len-=size;
+    }
+
+
+}
+
+
+static struct mtd_info *serial_mtd;
+
+static struct mtd_partition *parsed_parts;
+
+static struct map_info sl2312_serial_map = {
+//	name: "SL2312 serial flash",
+//	size: 4194304, //0x400000,
+//		//buswidth: 4,
+//	bankwidth: 4,
+//	phys:		 SL2312_FLASH_BASE,
+//#ifdef CONFIG_MTD_COMPLEX_MAPPINGS
+//	//read32: sl2312_read32,
+//	//read8: sl2312_read8,
+//	copy_from: sl2312_copy_from,
+//	//write8: sl2312_write8,
+//	//write32: sl2312_write32,
+//	read: sl2312_read32,
+//	write: sl2312_write32,
+//	copy_to: sl2312_copy_to
+//#endif
+	.name = "SL2312 serial flash",
+	.size = 4194304, //0x400000,
+		//buswidth: 4,
+	.bankwidth = 4,
+	.phys =		 SL2312_FLASH_BASE,
+#ifdef CONFIG_MTD_COMPLEX_MAPPINGS
+	//read32: sl2312_read32,
+	//read8: sl2312_read8,
+	.copy_from = sl2312_copy_from,
+	//write8: sl2312_write8,
+	//write32: sl2312_write32,
+	.read = sl2312_read32,
+	.write = sl2312_write32,
+	.copy_to = sl2312_copy_to
+#endif
+};
+
+
+
+static struct mtd_partition sl2312_partitions[] = {
+
+
+	///* boot code */
+	//{ name: "bootloader", offset: 0x00000000, size: 0x20000, },
+	///* kernel image */
+	//{ name: "kerel image", offset: 0x000020000, size: 0x2E0000 },
+	///* All else is writable (e.g. JFFS) */
+	//{ name: "user data", offset: 0x00300000, size: 0x00100000, },
+	/* boot code */
+	{ .name = "bootloader", .offset = 0x00000000, .size = 0x20000, },
+	/* kernel image */
+	{ .name = "kerel image", .offset = 0x000020000, .size = 0xE0000 },
+	/* All else is writable (e.g. JFFS) */
+	{ .name = "user data", .offset = 0x00100000, .size = 0x00300000, },
+
+
+};
+
+
+
+static int __init init_sl2312_maps(void)
+{
+	int nr_parts = 0;
+	struct mtd_partition *parts;
+
+	serial_mtd = kmalloc(sizeof(struct mtd_info), GFP_KERNEL);
+	if (!serial_mtd)
+		return NULL;
+
+	memset(serial_mtd, 0, sizeof(struct mtd_info));
+	//sl2312flash_map.virt = (unsigned long)ioremap(SL2312_FLASH_BASE, FLASH_SIZE);
+    //sl2312_serial_map.map_priv_1 = (unsigned long)ioremap(SL2312_FLASH_BASE, SFLASH_SIZE);//(unsigned long)FLASH_VBASE;
+    sl2312_serial_map.virt = (unsigned long)ioremap(SL2312_FLASH_BASE, SFLASH_SIZE);//(unsigned long)ioremap(FLASH_START, SFLASH_SIZE);
+    if (!sl2312_serial_map.virt) {
+		printk(" failed to ioremap \n");
+		return -EIO;
+	}
+	serial_mtd = do_map_probe("map_serial", &sl2312_serial_map);
+	if (serial_mtd) {
+		//serial_mtd->module = THIS_MODULE;
+		serial_mtd->owner = THIS_MODULE;
+
+	}
+
+#ifdef CONFIG_MTD_REDBOOT_PARTS
+	nr_parts = parse_redboot_partitions(serial_mtd, &parsed_parts);
+	if (nr_parts > 0)
+		printk(KERN_NOTICE "Found RedBoot partition table.\n");
+	else if (nr_parts < 0)
+		printk(KERN_NOTICE "Error looking for RedBoot partitions.\n");
+#else
+	parsed_parts = sl2312_partitions;
+	parts = sl2312_partitions;
+	nr_parts = sizeof(sl2312_partitions)/sizeof(*parts);
+	nr_parts = sizeof(sl2312_partitions)/sizeof(*parsed_parts);
+#endif /* CONFIG_MTD_REDBOOT_PARTS */
+
+	if (nr_parts > 0)
+	    add_mtd_partitions(serial_mtd, parsed_parts, nr_parts);
+	else
+	    add_mtd_device(serial_mtd);
+
+	return 0;
+}
+
+static void __exit cleanup_sl2312_maps(void)
+{
+	if (parsed_parts)
+	    del_mtd_partitions(serial_mtd);
+	else
+	    del_mtd_device(serial_mtd);
+
+	map_destroy(serial_mtd);
+
+
+}
+
+module_init(init_sl2312_maps);
+module_exit(cleanup_sl2312_maps);
+
+
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Plus Chen <plus@storlink.com.tw>");
+MODULE_DESCRIPTION("MTD map driver for Storlink Sword boards");
+
--- /dev/null
+++ b/drivers/mtd/maps/sl2312-flash-cfi.c
@@ -0,0 +1,370 @@
+/*======================================================================
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation; either version 2 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   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
+======================================================================*/
+
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/ioport.h>
+#include <linux/init.h>
+#include <linux/string.h>
+
+#include <linux/mtd/mtd.h>
+#include <linux/mtd/map.h>
+#include <linux/mtd/partitions.h>
+
+#include <asm/hardware.h>
+#include <asm/io.h>
+#include <asm/system.h>
+#include <asm/arch/sl2312.h>
+#include <linux/mtd/kvctl.h>
+#include "sl2312_flashmap.h"
+
+
+//extern int parse_afs_partitions(struct mtd_info *, struct mtd_partition **);
+
+/* the base address of FLASH control register */
+#define FLASH_CONTROL_BASE_ADDR	    (IO_ADDRESS(SL2312_FLASH_CTRL_BASE))
+#define SL2312_GLOBAL_BASE_ADDR     (IO_ADDRESS(SL2312_GLOBAL_BASE))
+
+/* define read/write register utility */
+#define FLASH_READ_REG(offset)			(__raw_readl(offset+FLASH_CONTROL_BASE_ADDR))
+#define FLASH_WRITE_REG(offset,val) 	(__raw_writel(val,offset+FLASH_CONTROL_BASE_ADDR))
+
+/* the offset of FLASH control register */
+enum EMAC_REGISTER {
+	FLASH_ID     	= 0x0000,
+	FLASH_STATUS 	= 0x0008,
+	FLASH_TYPE   	= 0x000c,
+	FLASH_ACCESS	= 0x0020,
+	FLASH_ADDRESS   = 0x0024,
+	FLASH_DATA		= 0x0028,
+	FLASH_TIMING    = 0x002c,
+};
+
+//#define FLASH_BASE	FLASH_CONTROL_BASE_ADDR
+//#define FLASH_SIZE	0x00800000 //INTEGRATOR_FLASH_SIZE
+
+//#define FLASH_PART_SIZE 8388608
+
+static unsigned int flash_indirect_access = 0;
+
+#ifdef CONFIG_SL2312_SHARE_PIN
+static unsigned int chip_en = 0x00000000;
+
+void sl2312flash_enable_parallel_flash(void)
+{
+    unsigned int    reg_val;
+
+    reg_val = readl(SL2312_GLOBAL_BASE_ADDR + 0x30);
+    reg_val = reg_val & 0xfffffffd;
+    writel(reg_val,SL2312_GLOBAL_BASE_ADDR + 0x30);
+    return;
+}
+
+void sl2312flash_disable_parallel_flash(void)
+{
+    unsigned int    reg_val;
+
+    reg_val = readl(SL2312_GLOBAL_BASE_ADDR + 0x30);
+    reg_val = reg_val | 0x00000002;
+    writel(reg_val,SL2312_GLOBAL_BASE_ADDR + 0x30);
+    return;
+}
+#endif
+
+
+static struct map_info sl2312flash_map =
+{
+	name:		"SL2312 CFI Flash",
+	size:       FLASH_SIZE,
+	bankwidth:   2,
+	//bankwidth:   1, //for 8 bits width
+    phys:       SL2312_FLASH_BASE,
+};
+
+static struct mtd_info *mtd;
+#if 0
+static struct mtd_partition sl2312_partitions[] = {
+	/* boot code */
+	{
+		name: "bootloader",
+		offset: 0x00000000,
+		size: 0x20000,
+//		mask_flags: MTD_WRITEABLE,
+	},
+	/* kernel image */
+	{
+		name: "kerel image",
+		offset: 0x00020000,
+		size: 0x2E0000
+	},
+	/* All else is writable (e.g. JFFS) */
+	{
+		name: "user data",
+		offset: 0x00300000,
+		size: 0x00100000,
+	}
+};
+#endif
+
+
+
+static int __init sl2312flash_init(void)
+{
+	struct mtd_partition *parts;
+	int nr_parts = 0;
+	int ret;
+#ifndef CONFIG_SL2312_SHARE_PIN
+    unsigned int    reg_val;
+#endif
+
+    printk("SL2312 MTD Driver Init.......\n");
+
+#ifndef CONFIG_SL2312_SHARE_PIN
+	/* enable flash */
+    reg_val = readl(SL2312_GLOBAL_BASE_ADDR + 0x30);
+    reg_val = reg_val & 0xfffffffd;
+    writel(reg_val,SL2312_GLOBAL_BASE_ADDR + 0x30);
+#else
+    sl2312flash_enable_parallel_flash();      /* enable Parallel FLASH */
+#endif
+    FLASH_WRITE_REG(FLASH_ACCESS,0x00004000); /* parallel flash direct access mode */
+    ret = FLASH_READ_REG(FLASH_ACCESS);
+    if (ret == 0x00004000)
+    {
+        flash_indirect_access = 0;  /* parallel flash direct access */
+    }
+    else
+    {
+        flash_indirect_access = 1;  /* parallel flash indirect access */
+    }
+
+	/*
+	 * Also, the CFI layer automatically works out what size
+	 * of chips we have, and does the necessary identification
+	 * for us automatically.
+	 */
+#ifdef CONFIG_GEMINI_IPI
+	sl2312flash_map.virt = FLASH_VBASE;//(unsigned int *)ioremap(SL2312_FLASH_BASE, FLASH_SIZE);
+#else
+	sl2312flash_map.virt = (unsigned int *)ioremap(SL2312_FLASH_BASE, FLASH_SIZE);
+#endif
+	//printk("sl2312flash_map.virt  = %08x\n",(unsigned int)sl2312flash_map.virt);
+
+//	simple_map_init(&sl2312flash_map);
+
+	mtd = do_map_probe("cfi_probe", &sl2312flash_map);
+	if (!mtd)
+	{
+#ifdef CONFIG_SL2312_SHARE_PIN
+        sl2312flash_disable_parallel_flash();      /* disable Parallel FLASH */
+#endif
+		return -ENXIO;
+	}
+	mtd->owner = THIS_MODULE;
+//    mtd->erase = flash_erase;
+//    mtd->read = flash_read;
+//    mtd->write = flash_write;
+
+    parts = sl2312_partitions;
+	nr_parts = sizeof(sl2312_partitions)/sizeof(*parts);
+	ret = add_mtd_partitions(mtd, parts, nr_parts);
+	/*If we got an error, free all resources.*/
+	if (ret < 0) {
+		del_mtd_partitions(mtd);
+		map_destroy(mtd);
+	}
+#ifdef CONFIG_SL2312_SHARE_PIN
+    sl2312flash_disable_parallel_flash();      /* disable Parallel FLASH */
+#endif
+    printk("SL2312 MTD Driver Init Success ......\n");
+	return ret;
+}
+
+static void __exit sl2312flash_exit(void)
+{
+	if (mtd) {
+		del_mtd_partitions(mtd);
+		map_destroy(mtd);
+	}
+
+	if (sl2312flash_map.virt) {
+	    iounmap((void *)sl2312flash_map.virt);
+	    sl2312flash_map.virt = 0;
+	}
+}
+
+char chrtohex(char c)
+{
+  char val;
+  if ((c >= '0') && (c <= '9'))
+  {
+    val = c - '0';
+    return val;
+  }
+  else if ((c >= 'a') && (c <= 'f'))
+  {
+    val = 10 + (c - 'a');
+    return val;
+  }
+  else if ((c >= 'A') && (c <= 'F'))
+  {
+    val = 10 + (c - 'A');
+    return val;
+  }
+  printk("<1>Error number\n");
+  return 0;
+}
+
+
+int get_vlaninfo(vlaninfo* vlan)
+{
+	vctl_mheader head;
+	vctl_entry entry;
+	struct mtd_info *mymtd=NULL;
+	int i, j, loc = 0;
+	char *payload=0, *tmp1, *tmp2, tmp3[9];
+	size_t retlen;
+
+	#ifdef CONFIG_SL2312_SHARE_PIN
+	sl2312flash_enable_parallel_flash();
+	#endif
+	for(i=0;i<MAX_MTD_DEVICES;i++)
+	{
+		mymtd=get_mtd_device(NULL,i);
+		//    printk("mymtd->name: %s\n", mymtd->name);
+		if(mymtd && !strcmp(mymtd->name,"VCTL"))
+		{
+			//      printk("%s\n", mymtd->name);
+			break;
+		}
+	}
+	if( i >= MAX_MTD_DEVICES)
+	{
+		printk("Can't find version control\n");
+		#ifdef CONFIG_SL2312_SHARE_PIN
+		sl2312flash_disable_parallel_flash();
+		#endif
+		return 0;
+	}
+
+	if (!mymtd | !mymtd->read)
+	{
+		printk("<1>Can't read Version Configuration\n");
+		#ifdef CONFIG_SL2312_SHARE_PIN
+		sl2312flash_disable_parallel_flash();
+		#endif
+		return 0;
+	}
+
+	mymtd->read(mymtd, 0, VCTL_HEAD_SIZE, &retlen, (u_char*)&head);
+	//  printk("entry header: %c%c%c%c\n", head.header[0], head.header[1], head.header[2], head.header[3]);
+	//  printk("entry number: %x\n", head.entry_num);
+	if ( strncmp(head.header, "FLFM", 4) )
+	{
+		printk("VCTL is a erase block\n");
+		#ifdef CONFIG_SL2312_SHARE_PIN
+		sl2312flash_disable_parallel_flash();
+		#endif
+		return 0;
+	}
+	loc += retlen;
+	for (i = 0; i < head.entry_num; i++)
+	{
+		mymtd->read(mymtd, loc, VCTL_ENTRY_LEN, &retlen, (u_char*)&entry);
+		//    printk("type: %x\n", entry.type);
+		//    printk("size: %x\n", entry.size);
+		strncpy(tmp3, entry.header, 4);
+		if (entry.type == VCT_VLAN)
+		{
+			for (j = 0; j < 6 ; j++)
+			{
+				vlan[0].mac[j] = 0;
+				vlan[1].mac[j] = 0;
+			}
+			vlan[0].vlanid = 1;
+			vlan[1].vlanid = 2;
+			vlan[0].vlanmap = 0x7F;
+			vlan[1].vlanmap = 0x80;
+
+			payload = (char *)kmalloc(entry.size - VCTL_ENTRY_LEN, GFP_KERNEL);
+			loc += VCTL_ENTRY_LEN;
+			mymtd->read(mymtd, loc, entry.size - VCTL_ENTRY_LEN, &retlen, payload);
+			//      printk("%s\n", payload);
+			tmp1 = strstr(payload, "MAC1:");
+			tmp2 = strstr(payload, "MAC2:");
+			if(!tmp1||!tmp2){
+				kfree(payload);
+				#ifdef CONFIG_SL2312_SHARE_PIN
+				sl2312flash_disable_parallel_flash();
+				#endif
+				printk("Error VCTL format!!\n");
+				return 0;
+			}
+			tmp1 += 7;
+			tmp2 += 7;
+
+
+			for (j = 0; j < 6; j++)
+			{
+				vlan[0].mac[j] = chrtohex(tmp1[2*j])*16 + chrtohex(tmp1[(2*j)+1]);
+				vlan[1].mac[j] = chrtohex(tmp2[2*j])*16 + chrtohex(tmp2[(2*j)+1]);
+			}
+			tmp1 = strstr(payload, "ID1:");
+			tmp2 = strstr(payload, "ID2:");
+			tmp1 += 4;
+			tmp2 += 4;
+			vlan[0].vlanid = tmp1[0] - '0';
+			vlan[1].vlanid = tmp2[0] - '0';
+			tmp1 = strstr(payload, "MAP1:");
+			tmp2 = strstr(payload, "MAP2:");
+			tmp1 += 7;
+			tmp2 += 7;
+			vlan[0].vlanmap = chrtohex(tmp1[0]) * 16 + chrtohex(tmp1[1]);
+			vlan[1].vlanmap = chrtohex(tmp2[0]) * 16 + chrtohex(tmp2[1]);
+			//  printk("Vlan1 id:%x map:%02x mac:%x%x%x%x%x%x\n", vlan[0].vlanid, vlan[0].vlanmap, vlan[0].mac[0], vlan[0].mac[1], vlan[0].mac[2], vlan[0].mac[3], vlan[0].mac[4], vlan[0].mac[5]);
+			//  printk("Vlan2 id:%x map:%02x mac:%x%x%x%x%x%x\n", vlan[1].vlanid, vlan[1].vlanmap, vlan[1].mac[0], vlan[1].mac[1], vlan[1].mac[2], vlan[1].mac[3], vlan[1].mac[4], vlan[1].mac[5]);
+			break;
+		}
+		loc += entry.size;
+	}
+	if ( entry.type == VCT_VLAN )
+	{
+		#ifdef CONFIG_SL2312_SHARE_PIN
+		sl2312flash_disable_parallel_flash();
+		#endif
+		kfree(payload);
+		return 1;
+	}
+	if (i >= head.entry_num)
+	printk("Can't find vlan information\n");
+	#ifdef CONFIG_SL2312_SHARE_PIN
+	sl2312flash_disable_parallel_flash();
+	#endif
+	return 0;
+}
+
+EXPORT_SYMBOL(get_vlaninfo);
+
+
+module_init(sl2312flash_init);
+module_exit(sl2312flash_exit);
+
+MODULE_AUTHOR("Storlink Ltd");
+MODULE_DESCRIPTION("CFI map driver");
+MODULE_LICENSE("GPL");
--- /dev/null
+++ b/drivers/mtd/maps/sl2312-flash-m25p80.c
@@ -0,0 +1,498 @@
+/*
+ * $Id: sl2312-flash-m25p80.c,v 1.2 2006/06/02 08:46:02 middle Exp $
+ *
+ * Flash and EPROM on Hitachi Solution Engine and similar boards.
+ *
+ * (C) 2001 Red Hat, Inc.
+ *
+ * GPL'd
+ */
+
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/kernel.h>
+
+#include <asm/io.h>
+#include <linux/mtd/mtd.h>
+#include <linux/mtd/map.h>
+#include <linux/mtd/partitions.h>
+#include <asm/hardware.h>
+
+#include <asm/arch/sl2312.h>
+#include <asm/arch/flash.h>
+#include <linux/init.h> //add
+#define  g_chipen     SERIAL_FLASH_CHIP0_EN   //ST
+
+//static int m25p80_page_program(__u32 address, __u8 data, __u32 schip_en);
+static void m25p80_write_cmd(__u8 cmd, __u32 schip_en);
+extern int parse_redboot_partitions(struct mtd_info *master, struct mtd_partition **pparts);
+
+
+static __u32 read_flash_ctrl_reg(__u32 ofs)
+{
+    __u32 *base;
+
+    base = (__u32 *)IO_ADDRESS((SL2312_FLASH_CTRL_BASE + ofs));
+    return __raw_readl(base);
+}
+
+static void write_flash_ctrl_reg(__u32 ofs,__u32 data)
+{
+    __u32 *base;
+
+    base = (__u32 *)IO_ADDRESS((SL2312_FLASH_CTRL_BASE + ofs));
+    __raw_writel(data, base);
+}
+
+static void m25p80_read(__u32 address, __u8 *data, __u32 schip_en)
+{
+      __u32 opcode,status;
+      __u32 value;
+
+      //opcode = 0x80000000 | FLASH_ACCESS_ACTION_OPCODE_DATA | M25P80_READ;
+      opcode = 0x80000000 | FLASH_ACCESS_ACTION_SHIFT_ADDRESS_DATA | M25P80_READ;
+      write_flash_ctrl_reg(FLASH_ADDRESS_OFFSET, address);
+
+      	opcode|=g_chipen;
+
+      write_flash_ctrl_reg(FLASH_ACCESS_OFFSET, opcode);
+      status=read_flash_ctrl_reg(FLASH_ACCESS_OFFSET);
+      while(status&0x80000000)
+      {
+          status=read_flash_ctrl_reg(FLASH_ACCESS_OFFSET);
+          flash_delay();
+          schedule();
+      }
+
+      value=read_flash_ctrl_reg(FLASH_READ_DATA_OFFSET);
+      *data = value & 0xff;
+}
+
+static int m25p80_page_program(__u32 address, __u8 *data, __u32 schip_en)
+{
+      __u32 opcode;
+      __u32  status;
+	  __u32 tmp;
+	  int res = FLASH_ERR_OK;
+	  //volatile FLASH_DATA_T* data_ptr = (volatile FLASH_DATA_T*) data;
+	  opcode = 0x80000000 | FLASH_ACCESS_ACTION_OPCODE_DATA | M25P80_READ_STATUS;
+
+      	      opcode|=g_chipen;
+
+          write_flash_ctrl_reg(FLASH_ACCESS_OFFSET, opcode);
+          tmp=read_flash_ctrl_reg(FLASH_ACCESS_OFFSET);
+      			while(tmp&0x80000000)
+      			{
+      			    tmp=read_flash_ctrl_reg(FLASH_ACCESS_OFFSET);
+      			    flash_delay();
+      			    schedule();
+      			}
+          //middle delay_ms(130);
+          status = read_flash_ctrl_reg(FLASH_READ_DATA_OFFSET);
+          if((status&0x02)==0x02)
+      	  {
+      	       //middle delay_ms(100);
+               m25p80_write_cmd(M25P80_WRITE_DISABLE, schip_en);
+          }
+
+
+      m25p80_write_cmd(M25P80_WRITE_ENABLE, schip_en);
+      ////middle delay_ms(10);
+      opcode = 0x80000000 | FLASH_ACCESS_ACTION_SHIFT_ADDRESS_DATA | M25P80_PAGE_PROGRAM;
+      write_flash_ctrl_reg(FLASH_ADDRESS_OFFSET, address);
+      write_flash_ctrl_reg(FLASH_WRITE_DATA_OFFSET, *data);
+
+      //status = read_flash_ctrl_reg(FLASH_READ_DATA_OFFSET);
+      //while(status!=data)
+      //{
+      //    status = read_flash_ctrl_reg(FLASH_READ_DATA_OFFSET);
+      //    //middle delay_ms(10);
+      //}
+
+      	opcode|=g_chipen;
+
+      write_flash_ctrl_reg(FLASH_ACCESS_OFFSET, opcode);
+      tmp=read_flash_ctrl_reg(FLASH_ACCESS_OFFSET);
+      			while(tmp&0x80000000)
+      			{
+      			    tmp=read_flash_ctrl_reg(FLASH_ACCESS_OFFSET);
+      			    flash_delay();
+      			    schedule();
+      			}
+      //opcode=read_flash_ctrl_reg(FLASH_ACCESS_OFFSET);
+
+      opcode = 0x80000000 | FLASH_ACCESS_ACTION_OPCODE_DATA | M25P80_READ_STATUS;
+
+      	opcode|=g_chipen;
+
+
+      write_flash_ctrl_reg(FLASH_ACCESS_OFFSET, opcode);
+      tmp=read_flash_ctrl_reg(FLASH_ACCESS_OFFSET);
+      			while(tmp&0x80000000)
+      			{
+      			    tmp=read_flash_ctrl_reg(FLASH_ACCESS_OFFSET);
+      			    flash_delay();
+      			    schedule();
+      			}
+      status = read_flash_ctrl_reg(FLASH_READ_DATA_OFFSET);
+      //while(status&0xfd)
+      while(status&0x01)
+      {
+      	  //if((status&0x9c)!=0)
+      	  //	printf("  m25p80_page_program	Protect Status = %x\n",status);
+      	  write_flash_ctrl_reg(FLASH_ACCESS_OFFSET, opcode);
+      	  tmp=read_flash_ctrl_reg(FLASH_ACCESS_OFFSET);
+      			while(tmp&0x80000000)
+      			{
+      			    tmp=read_flash_ctrl_reg(FLASH_ACCESS_OFFSET);
+      			    flash_delay();
+      			    schedule();
+      			}
+          status = read_flash_ctrl_reg(FLASH_READ_DATA_OFFSET);
+          flash_delay();
+          schedule();
+          //middle delay_ms(50);
+      }
+      //printf("status = %x, data = %x\n",status,data);
+      if((status&0x02)==0x02)
+      {
+	  //middle delay_ms(100);
+          m25p80_write_cmd(M25P80_WRITE_DISABLE, schip_en);
+      }
+    //};//while (len > 0)
+    return res;
+}
+
+void m25p80_copy_from(struct map_info *map, void *buf, unsigned long ofs, ssize_t len)
+{
+//     __u32 size;
+     __u8  *buffer;
+     __u32 length;//i, j,
+
+	length = len;
+     buffer = (__u8 *)buf;
+     while(len)
+     {
+        m25p80_read(ofs, buffer, g_chipen);
+        buffer++;
+        ofs++;
+        len --;
+     }	;
+
+}
+
+__u32 m25p80_read32(struct map_info *map, unsigned long ofs)
+{
+
+      return read_flash_ctrl_reg(ofs);
+
+
+}
+
+void m25p80_write32(struct map_info *map, __u32 d, unsigned long ofs)
+{
+
+      write_flash_ctrl_reg(ofs, d);
+
+}
+
+void m25p80_copy_to(struct map_info *map, unsigned long ofs, void *buf, ssize_t len)
+{
+     __u32 size, i, ret;
+
+     while(len > 0)
+     {
+        if(len >= M25P80_PAGE_SIZE)
+			size = M25P80_PAGE_SIZE;
+		else
+			size = len;
+
+        for(i=0;i<size;i++)
+	    {
+	    	ret = m25p80_page_program( (ofs+i),  (buf+i),  g_chipen);
+	    }
+        buf+=M25P80_PAGE_SIZE;
+        ofs+=M25P80_PAGE_SIZE;
+		len-=M25P80_PAGE_SIZE;
+
+    };
+
+
+}
+
+static struct mtd_info *serial_mtd;
+
+static struct mtd_partition *parsed_parts;
+
+static struct map_info m25p80_map = {
+
+	.name = "SL2312 serial flash m25p80",
+	.size = 1048576, //0x100000,
+		//buswidth: 4,
+	.bankwidth = 4,
+	.phys =		 SL2312_FLASH_BASE,
+#ifdef CONFIG_MTD_COMPLEX_MAPPINGS
+	.copy_from = m25p80_copy_from,
+	.read = m25p80_read32,
+	.write = m25p80_write32,
+	.copy_to = m25p80_copy_to
+#endif
+};
+
+
+
+static struct mtd_partition m25p80_partitions[] = {
+
+	/* boot code */
+	{ .name = "bootloader", .offset = 0x00000000, .size = 0x20000, },
+	/* kernel image */
+	{ .name = "kerel image", .offset = 0x000020000, .size = 0xC0000 },
+	/* All else is writable (e.g. JFFS) */
+	{ .name = "user data", .offset = 0x000E0000, .size = 0x00010000, },
+
+
+};
+
+void flash_delay()
+{
+	int i,j;
+	for(i=0;i<0x100;i++)
+		j=i*3+5;
+}
+
+int m25p80_sector_erase(__u32 address, __u32 schip_en)
+{
+      __u32 opcode;
+      __u32  status;
+      __u32 tmp;
+      int res = FLASH_ERR_OK;
+	//printf("\n-->m25p80_sector_erase");
+	if(address >= FLASH_START)
+		address-=FLASH_START;
+
+      m25p80_write_cmd(M25P80_WRITE_ENABLE, schip_en);
+      //printf("\n     m25p80_sector_erase : after we-en");
+      opcode = 0x80000000 | FLASH_ACCESS_ACTION_SHIFT_ADDRESS | M25P80_SECTOR_ERASE;
+      write_flash_ctrl_reg(FLASH_ADDRESS_OFFSET, address);
+      #ifdef MIDWAY_DIAG
+      	opcode|=schip_en;
+      #endif
+      write_flash_ctrl_reg(FLASH_ACCESS_OFFSET, opcode);
+      tmp=read_flash_ctrl_reg(FLASH_ACCESS_OFFSET);
+      			while(tmp&0x80000000)
+      			{
+      			    tmp=read_flash_ctrl_reg(FLASH_ACCESS_OFFSET);
+      			    flash_delay();
+      			    schedule();
+      			}
+
+      opcode = 0x80000000 | FLASH_ACCESS_ACTION_OPCODE_DATA | M25P80_READ_STATUS;
+      #ifdef MIDWAY_DIAG
+      	opcode|=schip_en;
+      #endif
+
+      write_flash_ctrl_reg(FLASH_ACCESS_OFFSET, opcode);
+      tmp=read_flash_ctrl_reg(FLASH_ACCESS_OFFSET);
+      			while(tmp&0x80000000)
+      			{
+      			    tmp=read_flash_ctrl_reg(FLASH_ACCESS_OFFSET);
+      			    flash_delay();
+      			    schedule();
+      			}
+      status = read_flash_ctrl_reg(FLASH_READ_DATA_OFFSET);
+      //while(status&0xfd)
+      while(status&0x01)
+      {
+      	  //if((status&0x9c)!=0)
+      	  //	printf("  m25p80_sector_erase	Protect Status = %x\n",status);
+      	  write_flash_ctrl_reg(FLASH_ACCESS_OFFSET, opcode);
+      	  tmp=read_flash_ctrl_reg(FLASH_ACCESS_OFFSET);
+      			while(tmp&0x80000000)
+      			{
+      			    tmp=read_flash_ctrl_reg(FLASH_ACCESS_OFFSET);
+      			    flash_delay();
+      			    schedule();
+      			}
+          status = read_flash_ctrl_reg(FLASH_READ_DATA_OFFSET);
+          flash_delay();
+          schedule();
+          //middle delay_ms(50);
+      }
+      if((status&0x02)==0x02)
+      {
+      	  //middle delay_ms(100);
+          m25p80_write_cmd(M25P80_WRITE_DISABLE, schip_en);
+      }
+      //printf("\n<--m25p80_sector_erase");
+      return res;
+}
+
+static void m25p80_write_cmd(__u8 cmd, __u32 schip_en)
+{
+      __u32 opcode,tmp;
+      __u32  status;
+
+
+
+
+      opcode = 0x80000000 | FLASH_ACCESS_ACTION_OPCODE | cmd;
+
+      	opcode|=g_chipen;
+
+      write_flash_ctrl_reg(FLASH_ACCESS_OFFSET, opcode);
+      tmp=read_flash_ctrl_reg(FLASH_ACCESS_OFFSET);
+      while(tmp&0x80000000)
+      {
+          tmp=read_flash_ctrl_reg(FLASH_ACCESS_OFFSET);
+          flash_delay();
+          schedule();
+      }
+      //////
+      opcode = 0x80000000 | FLASH_ACCESS_ACTION_OPCODE_DATA | M25P80_READ_STATUS;
+
+      	opcode|=g_chipen;
+
+      write_flash_ctrl_reg(FLASH_ACCESS_OFFSET, opcode);
+      tmp=read_flash_ctrl_reg(FLASH_ACCESS_OFFSET);
+      while(tmp&0x80000000)
+      {
+          tmp=read_flash_ctrl_reg(FLASH_ACCESS_OFFSET);
+          flash_delay();
+          schedule();
+      }
+      //middle delay_ms(130);
+      status = read_flash_ctrl_reg(FLASH_READ_DATA_OFFSET);
+      //printf("\ncmd =%x  status = %x",cmd,status);
+      if(cmd==M25P80_WRITE_ENABLE)
+      {
+      	//printf("\n**-->enable**  status = %x",status);
+      	//middle delay_ms(100);
+      	   while((status&0x03) != 2)
+      	   {
+      	   	//if((status&0x9c)!=0)
+      	  	//    printf("	M25P80_WRITE_ENABLE   Protect Status = %x\n",status);
+
+      	   	  write_flash_ctrl_reg(FLASH_ACCESS_OFFSET, opcode);
+      	   	  tmp=read_flash_ctrl_reg(FLASH_ACCESS_OFFSET);
+      			while(tmp&0x80000000)
+      			{
+      			    tmp=read_flash_ctrl_reg(FLASH_ACCESS_OFFSET);
+      			    //flash_delay();
+      			}
+      	       status = read_flash_ctrl_reg(FLASH_READ_DATA_OFFSET);
+      	       //printf("\n**enable**  status = %x",status);
+      	       flash_delay();
+      	       schedule();
+      	       //middle delay_ms(100);
+      	   }
+      }
+      else if(cmd==M25P80_WRITE_DISABLE)
+      {
+      	   //while((status&0x03) == 2)
+      	   //   printf("\n**disable**  status = %x",status);
+      	   //middle delay_ms(100);
+      	   while((status&0x03) != 0)
+      	   {
+	       //m25p80_write_status((status&0xfd),schip_en);
+      	       write_flash_ctrl_reg(FLASH_ACCESS_OFFSET, opcode);
+      	       tmp=read_flash_ctrl_reg(FLASH_ACCESS_OFFSET);
+      		while(tmp&0x80000000)
+      		{
+      		    tmp=read_flash_ctrl_reg(FLASH_ACCESS_OFFSET);
+      		    flash_delay();
+      		    schedule();
+      		}
+      	       status = read_flash_ctrl_reg(FLASH_READ_DATA_OFFSET);
+      	       //printf("\n**disable**  status = %x",status);
+      	       flash_delay();
+      	       schedule();
+      	       //middle delay_ms(50);
+      	   }
+      }
+      else
+      {
+      	   //while((status&0x01) !=0)
+      	   while((status&0x01) !=0)
+      	   {
+      	   	  write_flash_ctrl_reg(FLASH_ACCESS_OFFSET, opcode);
+      	   	  tmp=read_flash_ctrl_reg(FLASH_ACCESS_OFFSET);
+      			while(tmp&0x80000000)
+      			{
+      			    tmp=read_flash_ctrl_reg(FLASH_ACCESS_OFFSET);
+      			    flash_delay();
+      			    schedule();
+      			}
+      	       status = read_flash_ctrl_reg(FLASH_READ_DATA_OFFSET);
+      	       flash_delay();
+      	       schedule();
+      	       //middle delay_ms(50);
+      	   }
+      }
+      //////
+
+      //printf("\n<--  status = %x",status);
+}
+
+static int __init init_sl2312_m25p80(void)
+{
+	int nr_parts = 0;
+	struct mtd_partition *parts;
+
+	serial_mtd = kmalloc(sizeof(struct mtd_info), GFP_KERNEL);
+	if (!serial_mtd)
+		return NULL;
+
+	memset(serial_mtd, 0, sizeof(struct mtd_info));
+	m25p80_map.virt = (unsigned long)ioremap(SL2312_FLASH_BASE, SFLASH_SIZE);//(unsigned long)ioremap(FLASH_START, SFLASH_SIZE);
+    if (!m25p80_map.virt) {
+		printk(" failed to ioremap \n");
+		return -EIO;
+	}
+	serial_mtd = do_map_probe("map_serial", &m25p80_map);
+	if (serial_mtd) {
+		serial_mtd->owner = THIS_MODULE;
+
+	}
+
+#ifdef CONFIG_MTD_REDBOOT_PARTS
+	nr_parts = parse_redboot_partitions(serial_mtd, &parsed_parts);
+	if (nr_parts > 0)
+		printk(KERN_NOTICE "Found RedBoot partition table.\n");
+	else if (nr_parts < 0)
+		printk(KERN_NOTICE "Error looking for RedBoot partitions.\n");
+#else
+	parsed_parts = m25p80_partitions;
+	parts = m25p80_partitions;
+	nr_parts = sizeof(m25p80_partitions)/sizeof(*parts);
+	nr_parts = sizeof(m25p80_partitions)/sizeof(*parsed_parts);
+#endif /* CONFIG_MTD_REDBOOT_PARTS */
+
+	if (nr_parts > 0)
+	    add_mtd_partitions(serial_mtd, parsed_parts, nr_parts);
+	else
+	    add_mtd_device(serial_mtd);
+
+	return 0;
+}
+
+static void __exit cleanup_sl2312_m25p80(void)
+{
+	if (parsed_parts)
+	    del_mtd_partitions(serial_mtd);
+	else
+	    del_mtd_device(serial_mtd);
+
+	map_destroy(serial_mtd);
+
+
+}
+
+module_init(init_sl2312_m25p80);
+module_exit(cleanup_sl2312_m25p80);
+
+
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Plus Chen <plus@storlink.com.tw>");
+MODULE_DESCRIPTION("MTD map driver for Storlink Sword boards");
+
--- /dev/null
+++ b/drivers/mtd/maps/sl2312_flashmap.h
@@ -0,0 +1,20 @@
+/*
+ * Please note that the name are used in mkflash script. Therefore
+ * don't change them.  If you want to add different partitions, you
+ * will need to modify mkflash script as well so that the end image
+ * is what you include here!
+ *
+ * Also, the 7th item is always the size, so please don't add extra
+ * spaces in the name or other items.
+ *
+ *  - Alan
+ */
+
+static struct mtd_partition sl2312_partitions[] = {
+	{ name: "RedBoot", 	 offset: 0x00000000, size: 0x00020000, },
+	{ name: "kernel", 	 offset: 0x00020000, size: 0x00100000, },
+	{ name: "rootfs", 	 offset: 0x00120000, size: 0x006A0000, },
+	{ name: "VCTL", 	 offset: 0x007C0000, size: 0x00010000, },
+	{ name: "cfg", 	 	 offset: 0x007D0000, size: 0x00020000, },
+	{ name: "FIS directory", offset: 0x007F0000, size: 0x00010000, }
+};
--- /dev/null
+++ b/drivers/mtd/maps/sl2312_flashmap.h.16MB
@@ -0,0 +1,21 @@
+/*
+ * Please note that the name are used in mkflash script. Therefore
+ * don't change them.  If you want to add different partitions, you
+ * will need to modify mkflash script as well so that the end image
+ * is what you include here!
+ *
+ * Also, the 7th item is always the size, so please don't add extra
+ * spaces in the name or other items.
+ *
+ *  - Alan
+ */
+
+static struct mtd_partition sl2312_partitions[] = {
+	{ name: "RedBoot",     	 offset: 0x00000000, size: 0x00020000, },
+	{ name: "Kernel",      	 offset: 0x00020000, size: 0x00300000, },
+	{ name: "Ramdisk",     	 offset: 0x00320000, size: 0x00600000, },
+	{ name: "Application", 	 offset: 0x00920000, size: 0x00600000, },
+	{ name: "VCTL", 	 offset: 0x00F20000, size: 0x00020000, },
+	{ name: "CurConf", 	 offset: 0x00F40000, size: 0x000A0000, },
+	{ name: "FIS directory", offset: 0x00FE0000, size: 0x00020000, }
+};
--- /dev/null
+++ b/drivers/mtd/maps/sl2312_flashmap.h.8MB
@@ -0,0 +1,21 @@
+/*
+ * Please note that the name are used in mkflash script. Therefore
+ * don't change them.  If you want to add different partitions, you
+ * will need to modify mkflash script as well so that the end image
+ * is what you include here!
+ *
+ * Also, the 7th item is always the size, so please don't add extra
+ * spaces in the name or other items.
+ *
+ *  - Alan
+ */
+
+static struct mtd_partition sl2312_partitions[] = {
+	{ name: "RedBoot", 	 offset: 0x00000000, size: 0x00020000, },
+	{ name: "Kernel", 	 offset: 0x00020000, size: 0x00200000, },
+	{ name: "Ramdisk", 	 offset: 0x00220000, size: 0x00280000, },
+	{ name: "Application", 	 offset: 0x004A0000, size: 0x00300000, },
+	{ name: "VCTL", 	 offset: 0x007A0000, size: 0x00020000, },
+	{ name: "CurConf", 	 offset: 0x007C0000, size: 0x00020000, },
+	{ name: "FIS directory", offset: 0x007E0000, size: 0x00020000, }
+};
--- a/drivers/mtd/mtdchar.c
+++ b/drivers/mtd/mtdchar.c
@@ -59,6 +59,77 @@
 	enum mtd_file_modes mode;
 };
 
+/***********************************************************************
+/*             Storlink SoC -- flash
+/***********************************************************************/
+#ifdef CONFIG_SL2312_SHARE_PIN
+unsigned int share_pin_flag=0;		// bit0:FLASH, bit1:UART, bit2:EMAC, bit3-4:IDE
+unsigned int check_sleep_flag=0;	// bit0:FLASH, bit1:IDE
+static spinlock_t sl2312_flash_lock = SPIN_LOCK_UNLOCKED;
+EXPORT_SYMBOL(share_pin_flag);
+int dbg=0;
+DECLARE_WAIT_QUEUE_HEAD(wq);
+extern struct wait_queue_head_t *flash_wait;
+unsigned int flash_req=0;
+void mtd_lock()
+{
+	struct task_struct *tsk = current;
+	unsigned int value ;
+	unsigned long flags;
+	flash_req = 1;
+	DECLARE_WAITQUEUE(wait, tsk);
+	add_wait_queue(&wq, &wait);
+	for(;;)
+	{
+		set_task_state(tsk, TASK_INTERRUPTIBLE);
+		spin_lock_irqsave(&sl2312_flash_lock,flags);
+		if((share_pin_flag&0x1E)){//||(check_sleep_flag&0x00000002)) {
+			spin_unlock_irqrestore(&sl2312_flash_lock, flags);
+			check_sleep_flag |= 0x00000001;
+			if(dbg)
+				printk("mtd yield %x %x\n",share_pin_flag,check_sleep_flag);
+			wake_up_interruptible(&flash_wait);
+			schedule();
+		}
+		else {
+			check_sleep_flag &= ~0x01;
+			share_pin_flag |= 0x00000001 ;			// set share pin flag
+			spin_unlock_irqrestore(&sl2312_flash_lock, flags);
+			value = readl(IO_ADDRESS((SL2312_GLOBAL_BASE+GLOBAL_MISC_REG)));
+			value = value & (~PFLASH_SHARE_BIT) ;
+			writel(value,IO_ADDRESS((SL2312_GLOBAL_BASE+GLOBAL_MISC_REG)));
+			if(dbg)
+				printk("mtd Go %x %x\n",share_pin_flag,check_sleep_flag);
+			tsk->state = TASK_RUNNING;
+			remove_wait_queue(&wq, &wait);
+			return ;
+		}
+	}
+}
+
+void mtd_unlock()
+{
+	unsigned int value ;
+	unsigned long flags;
+
+	spin_lock_irqsave(&sl2312_flash_lock,flags);		// Disable IRQ
+	value = readl(IO_ADDRESS((SL2312_GLOBAL_BASE+GLOBAL_MISC_REG)));
+	value = value | PFLASH_SHARE_BIT ;				// Disable Flash PADs
+	writel(value,IO_ADDRESS((SL2312_GLOBAL_BASE+GLOBAL_MISC_REG)));
+	share_pin_flag &= ~(0x00000001);			// clear share pin flag
+	check_sleep_flag &= ~0x00000001;
+	spin_unlock_irqrestore(&sl2312_flash_lock, flags);	// Restore IRQ
+	if (check_sleep_flag & 0x00000002)
+	{
+		check_sleep_flag &= ~(0x00000002);
+		wake_up_interruptible(&flash_wait);
+	}
+	DEBUG(MTD_DEBUG_LEVEL0, "Flash Unlock...\n");
+	flash_req = 0;
+}
+#endif
+/***********************************************************************/
+
 static loff_t mtd_lseek (struct file *file, loff_t offset, int orig)
 {
 	struct mtd_file_info *mfi = file->private_data;
@@ -162,13 +233,21 @@
 	int len;
 	char *kbuf;
 
+#ifdef CONFIG_SL2312_SHARE_PIN
+	mtd_lock();				// sl2312 share pin lock
+#endif
+
 	DEBUG(MTD_DEBUG_LEVEL0,"MTD_read\n");
 
 	if (*ppos + count > mtd->size)
 		count = mtd->size - *ppos;
 
-	if (!count)
+	if (!count){
+#ifdef CONFIG_SL2312_SHARE_PIN
+	mtd_unlock();				// sl2312 share pin lock
+#endif
 		return 0;
+	}
 
 	/* FIXME: Use kiovec in 2.5 to lock down the user's buffers
 	   and pass them directly to the MTD functions */
@@ -178,8 +257,12 @@
 	else
 		kbuf=kmalloc(count, GFP_KERNEL);
 
-	if (!kbuf)
+	if (!kbuf) {
+#ifdef CONFIG_SL2312_SHARE_PIN
+	mtd_unlock();				// sl2312 share pin lock
+#endif
 		return -ENOMEM;
+	}
 
 	while (count) {
 
@@ -224,6 +307,9 @@
 			*ppos += retlen;
 			if (copy_to_user(buf, kbuf, retlen)) {
 				kfree(kbuf);
+#ifdef CONFIG_SL2312_SHARE_PIN
+				mtd_unlock();				// sl2312 share pin lock
+#endif
 				return -EFAULT;
 			}
 			else
@@ -235,13 +321,19 @@
 				count = 0;
 		}
 		else {
-			kfree(kbuf);
+	 		kfree(kbuf);
+#ifdef CONFIG_SL2312_SHARE_PIN
+			mtd_unlock();				// sl2312 share pin lock
+#endif
 			return ret;
 		}
 
 	}
 
 	kfree(kbuf);
+#ifdef CONFIG_SL2312_SHARE_PIN
+	mtd_unlock();				// sl2312 share pin lock
+#endif
 	return total_retlen;
 } /* mtd_read */
 
@@ -255,24 +347,40 @@
 	int ret=0;
 	int len;
 
+#ifdef CONFIG_SL2312_SHARE_PIN
+	mtd_lock();				// sl2312 share pin lock
+#endif
+
 	DEBUG(MTD_DEBUG_LEVEL0,"MTD_write\n");
 
-	if (*ppos == mtd->size)
+	if (*ppos == mtd->size){
+#ifdef CONFIG_SL2312_SHARE_PIN
+	mtd_unlock();				// sl2312 share pin lock
+#endif
 		return -ENOSPC;
+	}
 
 	if (*ppos + count > mtd->size)
 		count = mtd->size - *ppos;
 
-	if (!count)
+	if (!count){
+#ifdef CONFIG_SL2312_SHARE_PIN
+	mtd_unlock();				// sl2312 share pin lock
+#endif
 		return 0;
+	}
 
 	if (count > MAX_KMALLOC_SIZE)
 		kbuf=kmalloc(MAX_KMALLOC_SIZE, GFP_KERNEL);
 	else
 		kbuf=kmalloc(count, GFP_KERNEL);
 
-	if (!kbuf)
+	if (!kbuf) {
+#ifdef CONFIG_SL2312_SHARE_PIN
+		mtd_unlock();				// sl2312 share pin lock
+#endif
 		return -ENOMEM;
+	}
 
 	while (count) {
 
@@ -283,6 +391,9 @@
 
 		if (copy_from_user(kbuf, buf, len)) {
 			kfree(kbuf);
+#ifdef CONFIG_SL2312_SHARE_PIN
+			mtd_unlock();				// sl2312 share pin lock
+#endif
 			return -EFAULT;
 		}
 
@@ -323,11 +434,17 @@
 		}
 		else {
 			kfree(kbuf);
+#ifdef CONFIG_SL2312_SHARE_PIN
+			mtd_unlock();				// sl2312 share pin lock
+#endif
 			return ret;
 		}
 	}
 
 	kfree(kbuf);
+#ifdef CONFIG_SL2312_SHARE_PIN
+	mtd_unlock();				// sl2312 share pin lock
+#endif
 	return total_retlen;
 } /* mtd_write */
 
@@ -381,36 +498,67 @@
 	u_long size;
 	struct mtd_info_user info;
 
+#ifdef CONFIG_SL2312_SHARE_PIN
+	mtd_lock();				// sl2312 share pin lock
+#endif
+
 	DEBUG(MTD_DEBUG_LEVEL0, "MTD_ioctl\n");
 
 	size = (cmd & IOCSIZE_MASK) >> IOCSIZE_SHIFT;
 	if (cmd & IOC_IN) {
 		if (!access_ok(VERIFY_READ, argp, size))
+		{
+#ifdef CONFIG_SL2312_SHARE_PIN
+			mtd_unlock();				// sl2312 share pin lock
+#endif
 			return -EFAULT;
+		}
 	}
 	if (cmd & IOC_OUT) {
 		if (!access_ok(VERIFY_WRITE, argp, size))
+		{
+#ifdef CONFIG_SL2312_SHARE_PIN
+			mtd_unlock();				// sl2312 share pin lock
+#endif
 			return -EFAULT;
+		}
 	}
 
 	switch (cmd) {
 	case MEMGETREGIONCOUNT:
 		if (copy_to_user(argp, &(mtd->numeraseregions), sizeof(int)))
+		{
+#ifdef CONFIG_SL2312_SHARE_PIN
+			mtd_unlock();				// sl2312 share pin lock
+#endif
 			return -EFAULT;
+		}
 		break;
 
 	case MEMGETREGIONINFO:
 	{
 		struct region_info_user ur;
 
-		if (copy_from_user(&ur, argp, sizeof(struct region_info_user)))
+		if (copy_from_user(&ur, argp, sizeof(struct region_info_user))) {
+#ifdef CONFIG_SL2312_SHARE_PIN
+			mtd_unlock();				// sl2312 share pin lock
+#endif
 			return -EFAULT;
+		}
 
-		if (ur.regionindex >= mtd->numeraseregions)
+		if (ur.regionindex >= mtd->numeraseregions) {
+#ifdef CONFIG_SL2312_SHARE_PIN
+			mtd_unlock();				// sl2312 share pin lock
+#endif
 			return -EINVAL;
+		}
 		if (copy_to_user(argp, &(mtd->eraseregions[ur.regionindex]),
-				sizeof(struct mtd_erase_region_info)))
+				sizeof(struct mtd_erase_region_info))) {
+#ifdef CONFIG_SL2312_SHARE_PIN
+			mtd_unlock();				// sl2312 share pin lock
+#endif
 			return -EFAULT;
+		}
 		break;
 	}
 
@@ -433,7 +581,12 @@
 		struct erase_info *erase;
 
 		if(!(file->f_mode & 2))
+		{
+#ifdef CONFIG_SL2312_SHARE_PIN
+			mtd_unlock();				// sl2312 share pin lock
+#endif
 			return -EPERM;
+		}
 
 		erase=kzalloc(sizeof(struct erase_info),GFP_KERNEL);
 		if (!erase)
@@ -447,6 +600,9 @@
 			if (copy_from_user(&erase->addr, argp,
 				    sizeof(struct erase_info_user))) {
 				kfree(erase);
+#ifdef CONFIG_SL2312_SHARE_PIN
+				mtd_unlock();				// sl2312 share pin lock
+#endif
 				return -EFAULT;
 			}
 			erase->mtd = mtd;
@@ -484,14 +640,26 @@
 		struct mtd_oob_buf buf;
 		struct mtd_oob_ops ops;
 
-		if(!(file->f_mode & 2))
+		if(!(file->f_mode & 2)) {
+#ifdef CONFIG_SL2312_SHARE_PIN
+			mtd_unlock();				// sl2312 share pin lock
+#endif
 			return -EPERM;
+		}
 
-		if (copy_from_user(&buf, argp, sizeof(struct mtd_oob_buf)))
+		if (copy_from_user(&buf, argp, sizeof(struct mtd_oob_buf))) {
+#ifdef CONFIG_SL2312_SHARE_PIN
+			mtd_unlock();				// sl2312 share pin lock
+#endif
 			return -EFAULT;
+		}
 
-		if (buf.length > 4096)
+		if (buf.length > 4096) {
+#ifdef CONFIG_SL2312_SHARE_PIN
+			mtd_unlock();				// sl2312 share pin lock
+#endif
 			return -EINVAL;
+		}
 
 		if (!mtd->write_oob)
 			ret = -EOPNOTSUPP;
@@ -499,8 +667,12 @@
 			ret = access_ok(VERIFY_READ, buf.ptr,
 					buf.length) ? 0 : EFAULT;
 
-		if (ret)
+		if (ret) {
+#ifdef CONFIG_SL2312_SHARE_PIN
+			mtd_unlock();				// sl2312 share pin lock
+#endif
 			return ret;
+		}
 
 		ops.ooblen = buf.length;
 		ops.ooboffs = buf.start & (mtd->oobsize - 1);
@@ -536,19 +708,35 @@
 		struct mtd_oob_buf buf;
 		struct mtd_oob_ops ops;
 
-		if (copy_from_user(&buf, argp, sizeof(struct mtd_oob_buf)))
+		if (copy_from_user(&buf, argp, sizeof(struct mtd_oob_buf))) {
+#ifdef CONFIG_SL2312_SHARE_PIN
+			mtd_unlock();				// sl2312 share pin lock
+#endif
 			return -EFAULT;
+		}
 
-		if (buf.length > 4096)
+		if (buf.length > 4096) {
+#ifdef CONFIG_SL2312_SHARE_PIN
+			mtd_unlock();				// sl2312 share pin lock
+#endif
 			return -EINVAL;
+		}
 
-		if (!mtd->read_oob)
+		if (!mtd->read_oob) {
+#ifdef CONFIG_SL2312_SHARE_PIN
+			mtd_unlock();				// sl2312 share pin lock
+#endif
 			ret = -EOPNOTSUPP;
+		}
 		else
 			ret = access_ok(VERIFY_WRITE, buf.ptr,
 					buf.length) ? 0 : -EFAULT;
-		if (ret)
+		if (ret) {
+#ifdef CONFIG_SL2312_SHARE_PIN
+			mtd_unlock();				// sl2312 share pin lock
+#endif
 			return ret;
+		}
 
 		ops.ooblen = buf.length;
 		ops.ooboffs = buf.start & (mtd->oobsize - 1);
@@ -580,7 +768,12 @@
 		struct erase_info_user info;
 
 		if (copy_from_user(&info, argp, sizeof(info)))
+		{
+#ifdef CONFIG_SL2312_SHARE_PIN
+			mtd_unlock();				// sl2312 share pin lock
+#endif
 			return -EFAULT;
+		}
 
 		if (!mtd->lock)
 			ret = -EOPNOTSUPP;
@@ -594,7 +787,12 @@
 		struct erase_info_user info;
 
 		if (copy_from_user(&info, argp, sizeof(info)))
+		{
+#ifdef CONFIG_SL2312_SHARE_PIN
+			mtd_unlock();				// sl2312 share pin lock
+#endif
 			return -EFAULT;
+		}
 
 		if (!mtd->unlock)
 			ret = -EOPNOTSUPP;
@@ -629,11 +827,21 @@
 		loff_t offs;
 
 		if (copy_from_user(&offs, argp, sizeof(loff_t)))
+		{
+#ifdef CONFIG_SL2312_SHARE_PIN
+			mtd_unlock();				// sl2312 share pin lock
+#endif
 			return -EFAULT;
+		}
 		if (!mtd->block_isbad)
 			ret = -EOPNOTSUPP;
 		else
+		{
+#ifdef CONFIG_SL2312_SHARE_PIN
+			mtd_unlock();				// sl2312 share pin lock
+#endif
 			return mtd->block_isbad(mtd, offs);
+		}
 		break;
 	}
 
@@ -642,11 +850,21 @@
 		loff_t offs;
 
 		if (copy_from_user(&offs, argp, sizeof(loff_t)))
+		{
+#ifdef CONFIG_SL2312_SHARE_PIN
+			mtd_unlock();				// sl2312 share pin lock
+#endif
 			return -EFAULT;
+		}
 		if (!mtd->block_markbad)
 			ret = -EOPNOTSUPP;
 		else
+		{
+#ifdef CONFIG_SL2312_SHARE_PIN
+			mtd_unlock();				// sl2312 share pin lock
+#endif
 			return mtd->block_markbad(mtd, offs);
+		}
 		break;
 	}
 
@@ -654,8 +872,12 @@
 	case OTPSELECT:
 	{
 		int mode;
-		if (copy_from_user(&mode, argp, sizeof(int)))
+		if (copy_from_user(&mode, argp, sizeof(int))) {
+#ifdef CONFIG_SL2312_SHARE_PIN
+			mtd_unlock();				// sl2312 share pin lock
+#endif
 			return -EFAULT;
+		}
 
 		mfi->mode = MTD_MODE_NORMAL;
 
@@ -670,7 +892,12 @@
 	{
 		struct otp_info *buf = kmalloc(4096, GFP_KERNEL);
 		if (!buf)
+		{
+#ifdef CONFIG_SL2312_SHARE_PIN
+			mtd_unlock();				// sl2312 share pin lock
+#endif
 			return -ENOMEM;
+		}
 		ret = -EOPNOTSUPP;
 		switch (mfi->mode) {
 		case MTD_MODE_OTP_FACTORY:
@@ -701,12 +928,24 @@
 	{
 		struct otp_info info;
 
-		if (mfi->mode != MTD_MODE_OTP_USER)
+		if (mfi->mode != MTD_MODE_OTP_USER) {
+#ifdef CONFIG_SL2312_SHARE_PIN
+			mtd_unlock();				// sl2312 share pin lock
+#endif
 			return -EINVAL;
-		if (copy_from_user(&info, argp, sizeof(info)))
+		}
+		if (copy_from_user(&info, argp, sizeof(info))) {
+#ifdef CONFIG_SL2312_SHARE_PIN
+			mtd_unlock();				// sl2312 share pin lock
+#endif
 			return -EFAULT;
-		if (!mtd->lock_user_prot_reg)
+		}
+		if (!mtd->lock_user_prot_reg) {
+#ifdef CONFIG_SL2312_SHARE_PIN
+			mtd_unlock();				// sl2312 share pin lock
+#endif
 			return -EOPNOTSUPP;
+		}
 		ret = mtd->lock_user_prot_reg(mtd, info.start, info.length);
 		break;
 	}
@@ -742,8 +981,12 @@
 			break;
 
 		case MTD_MODE_RAW:
-			if (!mtd->read_oob || !mtd->write_oob)
+			if (!mtd->read_oob || !mtd->write_oob) {
+#ifdef CONFIG_SL2312_SHARE_PIN
+				mtd_unlock();				// sl2312 share pin lock
+#endif
 				return -EOPNOTSUPP;
+			}
 			mfi->mode = arg;
 
 		case MTD_MODE_NORMAL:
@@ -766,6 +1009,10 @@
 		ret = -ENOTTY;
 	}
 
+#ifdef CONFIG_SL2312_SHARE_PIN
+	mtd_unlock();				// sl2312 share pin lock
+#endif
+
 	return ret;
 } /* memory_ioctl */
 
--- a/drivers/mtd/nand/Kconfig
+++ b/drivers/mtd/nand/Kconfig
@@ -44,6 +44,13 @@
 	  This enables the driver for the autronix autcpu12 board to
 	  access the SmartMediaCard.
 
+config MTD_NAND_SL2312
+	tristate "NAND Flash device on Storlink board"
+	depends on ARM && MTD_NAND && ARCH_SL2312
+	help
+	  This enables the driver for the Storlink board to
+	  access the nand device.
+
 config MTD_NAND_EDB7312
 	tristate "Support for Cirrus Logic EBD7312 evaluation board"
 	depends on ARCH_EDB7312
--- /dev/null
+++ b/drivers/mtd/nand/sl2312-flash-nand.c
@@ -0,0 +1,2287 @@
+/*
+ *  drivers/mtd/sl2312.c
+ *
+ * $Id: sl2312-flash-nand.c,v 1.5 2006/06/15 07:02:29 middle Exp $
+ *
+ * Copyright (C) 2001 Toshiba Corporation
+ *
+ * 2003 (c) MontaVista Software, Inc. This file is licensed under
+ * the terms of the GNU General Public License version 2. This program
+ * is licensed "as is" without any warranty of any kind, whether express
+ * or implied.
+ *
+ */
+
+#include <linux/slab.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/mtd/mtd.h>
+#include <linux/mtd/nand.h>
+#include <linux/mtd/nand_ecc.h>
+#include <linux/mtd/partitions.h>
+#include <linux/delay.h>
+#include <asm/io.h>
+#include <asm/hardware.h>
+#include <asm/arch/sl2312.h>
+#include "sl2312-flash-nand.h"
+
+
+#include <linux/errno.h>
+#include <linux/sched.h>
+#include <linux/types.h>
+#include <linux/mtd/compatmac.h>
+#include <linux/interrupt.h>
+#include <linux/bitops.h>
+
+
+/*
+ * NAND low-level MTD interface functions
+ */
+static void sl2312_nand_write_buf(struct mtd_info *mtd, const u_char *buf, int len);
+static void sl2312_nand_read_buf(struct mtd_info *mtd, u_char *buf, int len);
+static int sl2312_nand_verify_buf(struct mtd_info *mtd, const u_char *buf, int len);
+
+static int sl2312_nand_read (struct mtd_info *mtd, loff_t from, size_t len, size_t * retlen, u_char * buf);
+static int sl2312_nand_read_ecc (struct mtd_info *mtd, loff_t from, size_t len, size_t * retlen, u_char * buf, u_char * eccbuf, struct nand_oobinfo *oobsel);
+static int sl2312_nand_read_oob (struct mtd_info *mtd, loff_t from, size_t len, size_t * retlen, u_char * buf);
+static int sl2312_nand_write (struct mtd_info *mtd, loff_t to, size_t len, size_t * retlen, const u_char * buf);
+static int sl2312_nand_write_ecc (struct mtd_info *mtd, loff_t to, size_t len,
+			   size_t * retlen, const u_char * buf, u_char * eccbuf, struct nand_oobinfo *oobsel);
+static int sl2312_nand_write_oob (struct mtd_info *mtd, loff_t to, size_t len, size_t * retlen, const u_char *buf);
+static int sl2312_nand_writev (struct mtd_info *mtd, const struct kvec *vecs,
+			unsigned long count, loff_t to, size_t * retlen);
+static int sl2312_nand_writev_ecc (struct mtd_info *mtd, const struct kvec *vecs,
+			unsigned long count, loff_t to, size_t * retlen, u_char *eccbuf, struct nand_oobinfo *oobsel);
+static int sl2312_nand_erase (struct mtd_info *mtd, struct erase_info *instr, int allowbbt);
+static void sl2312_nand_sync (struct mtd_info *mtd);
+static int sl2312_nand_write_page (struct mtd_info *mtd, struct nand_chip *this, int page, u_char *oob_buf,  struct nand_oobinfo *oobsel);
+static int sl2312_nand_block_checkbad (struct mtd_info *mtd, loff_t ofs, int getchip, int allowbbt);
+static int sl2312_nand_erase_block(struct mtd_info *mtd, int page);
+
+/*
+ * MTD structure for sl2312 NDFMC
+ */
+static struct mtd_info *sl2312_mtd = NULL;
+static int nand_page=0,nand_col=0;
+
+/* Define default oob placement schemes for large and small page devices */
+static struct nand_oobinfo nand_oob_8 = {
+	.useecc = MTD_NANDECC_AUTOPLACE,
+	.eccbytes = 3,
+	.eccpos = {0, 1, 2},
+	.oobfree = { {3, 2}, {6, 2} }
+};
+
+static struct nand_oobinfo nand_oob_16 = {
+	.useecc = MTD_NANDECC_AUTOPLACE,
+	.eccbytes = 6,
+	.eccpos = {0, 1, 2, 3, 6, 7},
+	.oobfree = { {8, 8} }
+};
+
+static struct nand_oobinfo nand_oob_64 = {
+	.useecc = MTD_NANDECC_AUTOPLACE,
+	.eccbytes = 24,
+	.eccpos = {
+		40, 41, 42, 43, 44, 45, 46, 47,
+		48, 49, 50, 51, 52, 53, 54, 55,
+		56, 57, 58, 59, 60, 61, 62, 63},
+	.oobfree = { {2, 38} }
+};
+
+
+/*
+ * Define partitions for flash device
+ */
+/* the base address of FLASH control register */
+#define FLASH_CONTROL_BASE_ADDR	    (IO_ADDRESS(SL2312_FLASH_CTRL_BASE))
+#define SL2312_GLOBAL_BASE_ADDR     (IO_ADDRESS(SL2312_GLOBAL_BASE))
+//#define SL2312_FLASH_BASE_ADDR      (IO_ADDRESS(SL2312_FLASH_BASE))
+#define SL2312_FLASH_BASE_ADDR       FLASH_VADDR(SL2312_FLASH_BASE)
+static unsigned int CHIP_EN;
+/* define read/write register utility */
+//#define FLASH_READ_REG(offset)			(__raw_readl(offset+FLASH_CONTROL_BASE_ADDR))
+//#define FLASH_WRITE_REG(offset,val) 	(__raw_writel(val,offset+FLASH_CONTROL_BASE_ADDR))
+//#define FLASH_READ_DATA(offset)			(__raw_readb(offset+SL2312_FLASH_BASE_ADDR))
+//#define FLASH_WRITE_DATA(offset,val) 	(__raw_writeb(val,offset+SL2312_FLASH_BASE_ADDR))
+
+unsigned int FLASH_READ_REG(unsigned int addr)
+{
+    unsigned int *base;
+    unsigned int data;
+
+    base = (unsigned int *)(FLASH_CONTROL_BASE_ADDR + addr);
+    data = *base;
+    return (data);
+}
+
+void FLASH_WRITE_REG(unsigned int addr,unsigned int data)
+{
+    unsigned int *base;
+
+    base = (unsigned int *)(FLASH_CONTROL_BASE_ADDR + addr);
+    *base = data;
+    return;
+}
+
+unsigned int FLASH_READ_DATA(unsigned int addr)
+{
+    unsigned char *base;
+    unsigned int data;
+
+    base = (unsigned char *)(SL2312_FLASH_BASE_ADDR + addr);
+    data = *base;
+    return (data);
+}
+
+void FLASH_WRITE_DATA(unsigned int addr,unsigned int data)
+{
+    unsigned char *base;
+
+    base = (unsigned char *)(SL2312_FLASH_BASE_ADDR + addr);
+    *base = data;
+    return;
+}
+
+/* the offset of FLASH control register */
+enum NFLASH_REGISTER {
+	NFLASH_ID     			= 0x0000,
+	NFLASH_STATUS 			= 0x0008,
+	NFLASH_TYPE   			= 0x000c,
+	NFLASH_ACCESS			= 0x0030,
+	NFLASH_COUNT			= 0x0034,
+	NFLASH_CMD_ADDR 		= 0x0038,
+	NFLASH_ADDRESS			= 0x003C,
+	NFLASH_DATA				= 0x0040,
+	NFLASH_TIMING   		= 0x004C,
+	NFLASH_ECC_STATUS		= 0x0050,
+	NFLASH_ECC_CONTROL		= 0x0054,
+	NFLASH_ECC_OOB			= 0x005c,
+	NFLASH_ECC_CODE_GEN0	= 0x0060,
+	NFLASH_ECC_CODE_GEN1	= 0x0064,
+	NFLASH_ECC_CODE_GEN2	= 0x0068,
+	NFLASH_ECC_CODE_GEN3	= 0x006C,
+	NFLASH_FIFO_CONTROL		= 0x0070,
+	NFLASH_FIFO_STATUS		= 0x0074,
+	NFLASH_FIFO_ADDRESS		= 0x0078,
+	NFLASH_FIFO_DATA		= 0x007c,
+};
+
+
+
+//#define FLASH_BASE	FLASH_CONTROL_BASE_ADDR
+//#define FLASH_SIZE	0x00800000 //INTEGRATOR_FLASH_SIZE
+
+//#define FLASH_PART_SIZE 8388608
+
+//static unsigned int flash_indirect_access = 0;
+
+
+#ifdef CONFIG_SL2312_SHARE_PIN
+void sl2312flash_enable_nand_flash(void)
+{
+    unsigned int    reg_val;
+
+    reg_val = readl(SL2312_GLOBAL_BASE_ADDR + 0x30);
+    reg_val = reg_val & 0xfffffffb;
+    writel(reg_val,SL2312_GLOBAL_BASE_ADDR + 0x30);
+    return;
+}
+
+void sl2312flash_disable_nand_flash(void)
+{
+    unsigned int    reg_val;
+
+    reg_val = readl(SL2312_GLOBAL_BASE_ADDR + 0x30);
+    reg_val = reg_val | 0x00000004;
+    writel(reg_val,SL2312_GLOBAL_BASE_ADDR + 0x30);
+    return;
+}
+#endif
+
+extern struct nand_oobinfo jffs2_oobinfo;
+/*
+ * Define partitions for flash devices
+ */
+
+static struct mtd_partition sl2312_partitions[] = {
+	{ name: "RedBoot", offset: 0x00000000, size: 0x0020000, },
+	{ name: "Kernel", offset: 0x00020000, size: 0x00200000, },
+	{ name: "Ramdisk", offset: 0x00220000, size: 0x00280000, },
+	{ name: "Application", offset: 0x004A0000, size: 0x00320000, },
+	{ name: "VCTL", offset: 0x007C0000, size: 0x20000, },
+	{ name: "CurConf", offset: 0x007E0000, size: 0x20000, },
+	{ name: "FIS directory", offset: 0x007e0000, size: 0x00020000, }
+
+};
+
+
+/*
+ *	hardware specific access to control-lines
+*/
+static void sl2312_hwcontrol(struct mtd_info *mtd, int cmd)
+{
+
+	return ;
+}
+
+static int sl2312_nand_scan_bbt(struct mtd_info *mtd)
+{
+	return 0;
+}
+
+/**
+ * nand_block_isbad - [MTD Interface] Check whether the block at the given offset is bad
+ * @mtd:	MTD device structure
+ * @ofs:	offset relative to mtd start
+ */
+static int sl2312_nand_block_isbad (struct mtd_info *mtd, loff_t ofs)
+{
+	/* Check for invalid offset */
+	if (ofs > mtd->size)
+		return -EINVAL;
+
+	return sl2312_nand_block_checkbad (mtd, ofs, 1, 0);
+}
+
+/**
+ * nand_block_checkbad - [GENERIC] Check if a block is marked bad
+ * @mtd:	MTD device structure
+ * @ofs:	offset from device start
+ * @getchip:	0, if the chip is already selected
+ * @allowbbt:	1, if its allowed to access the bbt area
+ *
+ * Check, if the block is bad. Either by reading the bad block table or
+ * calling of the scan function.
+ */
+
+static int sl2312_nand_erase_block(struct mtd_info *mtd, int page)
+{
+	int opcode;
+	/* Send commands to erase a page */
+		FLASH_WRITE_REG(NFLASH_ECC_CONTROL, 0x00000000); //set 31b = 0
+
+		if(mtd->oobblock > 528)
+		    FLASH_WRITE_REG(NFLASH_COUNT, 0x7f0fff21);  // 3 address & 2 command
+		else
+		    FLASH_WRITE_REG(NFLASH_COUNT, 0x7f0fff11);  // 2 address & 2 command
+
+		FLASH_WRITE_REG(NFLASH_CMD_ADDR, 0x0000d060); // write read id command
+		FLASH_WRITE_REG(NFLASH_ADDRESS, page); //write address 0x00
+
+
+
+		/* read maker code */
+		opcode = 0x80003000|DWIDTH|CHIP_EN; //set start bit & 8bits write command
+		FLASH_WRITE_REG(NFLASH_ACCESS, opcode);
+
+		while(opcode&0x80000000) //polling flash access 31b
+      	{
+           opcode=FLASH_READ_REG(NFLASH_ACCESS);
+           //sl2312_flash_delay();
+           schedule();
+           //cond_resched();
+      	}
+}
+
+void sl2312_flash_delay(void)
+{
+      int i;
+
+      for(i=0; i<50; i++)
+           i=i;
+}
+
+static int sl2312_nand_block_checkbad (struct mtd_info *mtd, loff_t ofs, int getchip, int allowbbt)
+{
+	struct nand_chip *this = mtd->priv;
+
+	if (!this->bbt)
+		return this->block_bad(mtd, ofs, getchip);
+
+	/* Return info from the table */
+	return nand_isbad_bbt (mtd, ofs, allowbbt);
+}
+
+/**
+ * nand_block_markbad - [MTD Interface] Mark the block at the given offset as bad
+ * @mtd:	MTD device structure
+ * @ofs:	offset relative to mtd start
+ */
+static int sl2312_nand_block_markbad (struct mtd_info *mtd, loff_t ofs)
+{
+	struct nand_chip *this = mtd->priv;
+	int ret;
+
+        if ((ret = sl2312_nand_block_isbad(mtd, ofs))) {
+        	/* If it was bad already, return success and do nothing. */
+		if (ret > 0)
+			return 0;
+        	return ret;
+        }
+
+	return this->block_markbad(mtd, ofs);
+}
+
+/*
+ *	Get chip for selected access
+ */
+static inline void sl2312_nand_get_chip (struct nand_chip *this, struct mtd_info *mtd, int new_state, int *erase_state)
+{
+
+	DECLARE_WAITQUEUE (wait, current);
+
+	/*
+	 * Grab the lock and see if the device is available
+	 * For erasing, we keep the spinlock until the
+	 * erase command is written.
+	*/
+retry:
+	spin_lock_bh (&this->chip_lock);
+
+	if (this->state == FL_READY) {
+		this->state = new_state;
+		if (new_state != FL_ERASING)
+			spin_unlock_bh (&this->chip_lock);
+		return;
+	}
+
+	if (this->state == FL_ERASING) {
+		if (new_state != FL_ERASING) {
+			this->state = new_state;
+			spin_unlock_bh (&this->chip_lock);
+			this->select_chip(mtd, 0);	/* select in any case */
+			this->cmdfunc(mtd, NAND_CMD_RESET, -1, -1);
+			return;
+		}
+	}
+
+	set_current_state (TASK_UNINTERRUPTIBLE);
+	add_wait_queue (&this->wq, &wait);
+	spin_unlock_bh (&this->chip_lock);
+	schedule ();
+	remove_wait_queue (&this->wq, &wait);
+	goto retry;
+}
+
+/*
+*	read device ready pin
+*/
+static int sl2312_device_ready(struct mtd_info *mtd)
+{
+	int ready;
+
+	FLASH_WRITE_REG(NFLASH_ECC_CONTROL, 0x00000000); //set 31b = 0
+	FLASH_WRITE_REG(NFLASH_COUNT, 0x7f000070); //set only command no address and two data
+
+	FLASH_WRITE_REG(NFLASH_CMD_ADDR, 0x00000070); //write read status command
+
+
+	ready = 0x80002000|DWIDTH|CHIP_EN; //set start bit & 8bits read command
+	FLASH_WRITE_REG(NFLASH_ACCESS, ready);
+
+	while(ready&0x80000000) //polling flash access 31b
+    {
+        ready=FLASH_READ_REG(NFLASH_ACCESS);
+        //sl2312_flash_delay();
+		schedule();
+    }
+    FLASH_WRITE_REG(NFLASH_ACCESS, NFLASH_DIRECT);
+      	ready=FLASH_READ_REG(NFLASH_DATA)&0xff;
+	return ready;
+}
+void sl2312_enable_hwecc(struct mtd_info *mtd, int mode)
+{
+	/* reset first */
+	FLASH_WRITE_REG(NFLASH_ECC_CONTROL, 0x80000001); //set 31b = 0
+
+}
+
+
+void sl2312_device_setup(void)
+{
+
+}
+static u_char sl2312_nand_read_byte(struct mtd_info *mtd)
+{
+
+        unsigned int    data=0, page=0, col=0, tmp, i;
+
+        printk ("**************************sl2312_nand_read_byte !! \n");
+        //page = FLASH_READ_REG(NFLASH_ADDRESS)&0xffffff00;
+        //col  = FLASH_READ_REG(NFLASH_ADDRESS)&0x000000ff;
+        page = nand_page;
+        col  = nand_col;
+        for(i=0;i<(mtd->oobblock+mtd->oobsize);i++)
+        {
+        	if(i==col)
+				data = FLASH_READ_DATA(page*mtd->oobblock +i);
+			else
+				tmp = FLASH_READ_DATA(page*mtd->oobblock +i);
+        }
+        return data&0xff;
+}
+
+static void sl2312_nand_write_byte(struct mtd_info *mtd, u_char byte)
+{
+        //struct nand_chip *this = mtd->priv;
+        unsigned int    page=0, col=0, i;
+        u_char *databuf,oobbuf[mtd->oobsize];
+        size_t  retlen;
+        retlen=0;
+		printk ("********************sl2312_nand_write_byte !! \n");
+		page = nand_page;
+        col  = nand_col;
+		databuf = kmalloc (mtd->oobsize+mtd->oobblock,GFP_KERNEL);
+
+		if (!databuf) {
+			printk ("sl2312_nand_write_byte : Unable to allocate SL2312 NAND MTD device structure.\n");
+
+		}
+
+		 for(i=0;i<(mtd->oobblock+mtd->oobsize);i++)
+           	databuf[i] = FLASH_READ_DATA(page*mtd->oobblock +i);
+
+        databuf[col] = byte;
+        sl2312_nand_write_ecc (mtd, page, mtd->oobblock, &retlen, databuf, oobbuf, NULL);
+
+}
+
+static void sl2312_nand_write_buf(struct mtd_info *mtd, const u_char *buf, int len)
+{
+	int i, page=0,col=0;
+	struct nand_chip *this = mtd->priv;
+	u_char *databuf, *oobbuf;
+        size_t  retlen;
+        retlen=0;
+
+
+		printk ("***********************sl2312_nand_write_buf !! \n");
+		databuf = &(this->data_buf[0]);
+		oobbuf = &(this->data_buf[mtd->oobblock]);
+		for (i = 0; i < mtd->oobsize; i++)
+			oobbuf[i] = 0xff;
+
+	if(len < mtd->oobblock)
+	{
+		//addr = FLASH_READ_REG(NFLASH_ADDRESS);
+		//page = FLASH_READ_REG(NFLASH_ADDRESS)&0xffffff00;
+		//col  = FLASH_READ_REG(NFLASH_ADDRESS)&0x000000ff;
+		page = nand_page;
+        col  = nand_col;
+
+		sl2312_nand_read_ecc (mtd, page, mtd->oobblock , &retlen, databuf, oobbuf, NULL);
+
+        for(i=col;i<len;i++)
+        	databuf[col+i] = buf[i];
+
+        sl2312_nand_write_ecc (mtd, page, mtd->oobblock, &retlen, databuf, oobbuf, NULL);
+
+	}
+
+}
+
+static void sl2312_nand_read_buf(struct mtd_info *mtd, u_char *buf, int len)
+{
+	int i, page=0,col=0,addr=0,tmp=0;
+	//struct nand_chip *this = mtd->priv;
+	printk ("********************sl2312_nand_read_buf !! \n");
+	if(len < mtd->oobblock)
+	{
+		//addr = FLASH_READ_REG(NFLASH_ADDRESS);
+		//page = FLASH_READ_REG(NFLASH_ADDRESS)&0xffffff00;
+		//col  = FLASH_READ_REG(NFLASH_ADDRESS)&0x000000ff;
+		page = nand_page;
+        col  = nand_col;
+		for (i=col; i<((mtd->oobblock+mtd->oobsize)-col); i++)
+		{
+			if(i<len)
+				buf[i] = FLASH_READ_DATA(addr+i);
+			else
+				tmp = FLASH_READ_DATA(addr+i);
+		}
+	}
+}
+
+static int sl2312_nand_verify_buf(struct mtd_info *mtd, const u_char *buf, int len)
+{
+	int i;
+	//struct nand_chip *this = mtd->priv;
+	u_char *datatmp, *oobtmp;
+	size_t  retlen;
+	retlen=0;
+
+	datatmp = kmalloc (mtd->oobblock,GFP_KERNEL);
+	oobtmp = kmalloc (mtd->oobsize,GFP_KERNEL);
+
+	if ((!datatmp)||(!oobtmp)) {
+		printk ("sl2312_nand_verify_buf : Unable to allocate SL2312 NAND MTD device structure.\n");
+
+	}
+	//page = nand_page;
+	for(i=0;i<mtd->oobblock;i++)
+		datatmp[i] = FLASH_READ_DATA(nand_page*mtd->oobblock +i);
+	/* read oobdata */
+	for (i = 0; i <  mtd->oobsize; i++)
+		oobtmp[i] = FLASH_READ_DATA(nand_page*mtd->oobblock + mtd->oobblock + i);
+
+	if(len==mtd->oobblock)
+	{
+		for (i=0; i<len; i++)
+		{
+			if (buf[i] != datatmp[i])
+			{
+				kfree(datatmp);
+				kfree(oobtmp);
+				printk("Data verify error -> page: %x, byte: %x \n",nand_page,i);
+				return i;
+			}
+		}
+	}
+	else if(len == mtd->oobsize)
+	{
+		for (i=0; i<len; i++)
+		{
+			if (buf[i] != oobtmp[i])
+			{
+				kfree(datatmp);
+				kfree(oobtmp);
+				printk("OOB verify error -> page: %x, byte: %x \n",nand_page,i);
+				return i;
+			}
+		}
+	}
+	else
+	{
+		printk (KERN_WARNING "sl2312_nand_verify_buf : verify length not match 0x%08x\n", len);
+		kfree(datatmp);
+		kfree(oobtmp);
+		return -1;
+	}
+
+	kfree(datatmp);
+	kfree(oobtmp);
+	return 0;
+}
+
+/*
+ * Send command to NAND device
+ */
+static void sl2312_nand_command (struct mtd_info *mtd, unsigned command, int column, int page_addr)
+{
+	register struct nand_chip *this = mtd->priv;
+	int opcode;
+
+
+	/*
+	 * program and erase have their own busy handlers
+	 * status and sequential in needs no delay
+	*/
+	switch (command) {
+
+	case NAND_CMD_PAGEPROG:
+	case NAND_CMD_ERASE1:
+	case NAND_CMD_ERASE2:
+	case NAND_CMD_SEQIN:
+	case NAND_CMD_STATUS:
+	case NAND_CMD_READ0:
+
+		/*
+		 * Write out the command to the device.
+		 */
+		if (column != -1 || page_addr != -1) {
+
+			/* Serially input address */
+			if (column != -1)
+				//FLASH_WRITE_REG(NFLASH_ADDRESS,column);
+				nand_col=column;
+
+			opcode = FLASH_READ_REG(NFLASH_ADDRESS);
+
+			if (page_addr != -1)
+				//FLASH_WRITE_REG(NFLASH_ADDRESS,opcode|(page_addr<<8));
+				nand_page = page_addr;
+
+		}
+		return;
+
+	case NAND_CMD_RESET:
+		if (this->dev_ready)
+			break;
+		FLASH_WRITE_REG(NFLASH_ECC_CONTROL, 0x00000000); //set 31b = 0
+		FLASH_WRITE_REG(NFLASH_COUNT, 0x7f0fff70); //set only command and no other data
+		FLASH_WRITE_REG(NFLASH_CMD_ADDR, NAND_CMD_RESET); //write reset command
+
+		opcode = 0x80002000|DWIDTH|CHIP_EN; //set start bit & 8bits read command
+		FLASH_WRITE_REG(NFLASH_ACCESS, opcode);
+
+		while(opcode&0x80000000) //polling flash access 31b
+      	{
+           opcode=FLASH_READ_REG(NFLASH_ACCESS);
+           //sl2312_flash_delay();
+           schedule();
+      	}
+		while ( !(sl2312_device_ready(mtd) & 0x40));
+		{
+			FLASH_WRITE_REG(NFLASH_ACCESS, NFLASH_DIRECT);
+			//sl2312_flash_delay();
+			schedule();
+			return;
+		}
+	/* This applies to read commands */
+	default:
+		/*
+		 * If we don't have access to the busy pin, we apply the given
+		 * command delay
+		*/
+		if (!this->dev_ready) {
+			udelay (this->chip_delay);
+			FLASH_WRITE_REG(NFLASH_ACCESS, NFLASH_DIRECT);
+			return;
+		}
+	}
+
+	/* wait until command is processed */
+	while (!this->dev_ready(mtd));
+
+}
+/*Add function*/
+static void nand_read_id(int chip_no, unsigned char *id)
+{
+	unsigned int opcode, i;
+
+	if(chip_no==0)
+		CHIP_EN = NFLASH_CHIP0_EN;
+	else
+		CHIP_EN = NFLASH_CHIP1_EN;
+
+	opcode = FLASH_READ_REG(NFLASH_TYPE);
+
+	FLASH_WRITE_REG(NFLASH_ECC_CONTROL, 0x00000000); //set 31b = 0
+	if((opcode&0x00000300)<=0x00000100)
+	    FLASH_WRITE_REG(NFLASH_COUNT, 0x7f000100); //set only command & address and two data
+	else
+	    FLASH_WRITE_REG(NFLASH_COUNT, 0x7f000300); //set only command & address and 4 data
+
+	FLASH_WRITE_REG(NFLASH_CMD_ADDR, 0x00000090); //write read id command
+	FLASH_WRITE_REG(NFLASH_ADDRESS, 0x00000000); //write address 0x00
+
+	/* read maker code */
+	opcode = 0x80002000|DWIDTH|CHIP_EN;//|chip0_en; //set start bit & 8bits read command
+	FLASH_WRITE_REG(NFLASH_ACCESS, opcode);
+	opcode=FLASH_READ_REG(NFLASH_ACCESS);
+		while(opcode&0x80000000) //polling flash access 31b
+      	{
+           opcode=FLASH_READ_REG(NFLASH_ACCESS);
+           //sl2312_flash_delay();
+           schedule();
+      	}
+
+    opcode = FLASH_READ_REG(NFLASH_DATA);
+    if(DWIDTH==NFLASH_WiDTH16)
+    {
+      		id[0] = opcode&0xff;
+      		id[1] = (opcode&0xff00)>>8;
+    }
+    else
+    {
+    	    id[0] = opcode&0xff;
+    	    opcode = 0x80002000|DWIDTH|CHIP_EN;//|chip0_en; //set start bit & 8bits read command
+			FLASH_WRITE_REG(NFLASH_ACCESS, opcode);
+			opcode=FLASH_READ_REG(NFLASH_ACCESS);
+			while(opcode&0x80000000) //polling flash access 31b
+    	  	{
+    	       opcode=FLASH_READ_REG(NFLASH_ACCESS);
+    	       //sl2312_flash_delay();
+    	       schedule();
+    	  	}
+    		opcode = FLASH_READ_REG(NFLASH_DATA);
+      		id[1] = (opcode&0xff00)>>8;
+
+      		opcode=FLASH_READ_REG(NFLASH_TYPE);
+      		if((opcode&0x300)>0x100)
+      		{
+      		    for(i=0;i<2;i++)
+      		    {
+      				//data cycle 3 & 4 ->not use
+      				opcode = 0x80002000|DWIDTH|CHIP_EN;//set start bit & 8bits read command
+					FLASH_WRITE_REG(NFLASH_ACCESS, opcode);
+					opcode=FLASH_READ_REG(NFLASH_ACCESS);
+      				while(opcode&0x80000000) //polling flash access 31b
+      				{
+        			   opcode=FLASH_READ_REG(NFLASH_ACCESS);
+        			   //sl2312_flash_delay();
+        			   schedule();
+      				}
+
+      				opcode=FLASH_READ_REG(NFLASH_DATA);
+      				id[2+i] = (opcode&(0xff0000<<i*8))>>(8*(2+i));
+      		    }
+      		}
+    }
+    FLASH_WRITE_REG(NFLASH_ACCESS, NFLASH_DIRECT);
+}
+
+/*
+ * NAND erase a block
+ */
+static int sl2312_nand_erase (struct mtd_info *mtd, struct erase_info *instr, int allowbbt)
+{
+	int page, len, status, pages_per_block, ret, chipnr;
+	struct nand_chip *this = mtd->priv;
+
+	DEBUG (MTD_DEBUG_LEVEL3,
+	       "nand_erase: start = 0x%08x, len = %i\n", (unsigned int) instr->addr, (unsigned int) instr->len);
+
+	/* Start address must align on block boundary */
+	if (instr->addr & ((1 << this->phys_erase_shift) - 1)) {
+		DEBUG (MTD_DEBUG_LEVEL0, "nand_erase: Unaligned address\n");
+		return -EINVAL;
+	}
+
+	/* Length must align on block boundary */
+	if (instr->len & ((1 << this->phys_erase_shift) - 1)) {
+		DEBUG (MTD_DEBUG_LEVEL0, "nand_erase: Length not block aligned\n");
+		return -EINVAL;
+	}
+
+	/* Do not allow erase past end of device */
+	if ((instr->len + instr->addr) > mtd->size) {
+		DEBUG (MTD_DEBUG_LEVEL0, "nand_erase: Erase past end of device\n");
+		return -EINVAL;
+	}
+
+	instr->fail_addr = 0xffffffff;
+
+	/* Grab the lock and see if the device is available */
+	sl2312_nand_get_chip (this, mtd, FL_ERASING, NULL);
+
+	/* Shift to get first page */
+	page = (int) (instr->addr >> this->page_shift);
+	chipnr = (int) (instr->addr >> this->chip_shift);
+
+	/* Calculate pages in each block */
+	pages_per_block = 1 << (this->phys_erase_shift - this->page_shift);
+
+	/* Select the NAND device */
+	//this->select_chip(mtd, chipnr);
+	this->select_chip(mtd, 0);
+
+	/* Check the WP bit */
+	/* Check, if it is write protected */
+	status = sl2312_device_ready(mtd);
+	if (!(status & 0x80)) {
+		DEBUG (MTD_DEBUG_LEVEL0, "nand_erase: Device is write protected!!!\n");
+		instr->state = MTD_ERASE_FAILED;
+		goto erase_exit;
+	}
+
+	/* Loop through the pages */
+	len = instr->len;
+
+	instr->state = MTD_ERASING;
+
+	while (len) {
+		/* Check if we have a bad block, we do not erase bad blocks ! */
+		if (this->block_bad(mtd, ((loff_t) page) << this->page_shift, 0)) {
+			printk (KERN_WARNING "nand_erase: attempt to erase a bad block at page 0x%08x\n", page);
+			//instr->state = MTD_ERASE_FAILED;
+			//goto erase_exit;
+		}
+
+		/* Invalidate the page cache, if we erase the block which contains
+		   the current cached page */
+		if (page <= this->pagebuf && this->pagebuf < (page + pages_per_block))
+			this->pagebuf = -1;
+		/////////
+
+		///* Send commands to erase a page */
+		//FLASH_WRITE_REG(NFLASH_ECC_CONTROL, 0x00000000); //set 31b = 0
+	    //
+		//if(mtd->oobblock > 528)
+		//    FLASH_WRITE_REG(NFLASH_COUNT, 0x7f0fff21);  // 3 address & 2 command
+		//else
+		//    FLASH_WRITE_REG(NFLASH_COUNT, 0x7f0fff11);  // 2 address & 2 command
+		//
+		//FLASH_WRITE_REG(NFLASH_CMD_ADDR, 0x0000d060); // write read id command
+		//FLASH_WRITE_REG(NFLASH_ADDRESS, page); //write address 0x00
+		//
+		//
+		//
+		///* read maker code */
+		//opcode = 0x80003000|DWIDTH|CHIP_EN; //set start bit & 8bits write command
+		//FLASH_WRITE_REG(NFLASH_ACCESS, opcode);
+		//
+		//while(opcode&0x80000000) //polling flash access 31b
+      	//{
+        //   opcode=FLASH_READ_REG(NFLASH_ACCESS);
+        //   //sl2312_flash_delay();
+        //   schedule();
+        //   //cond_resched();
+      	//}
+      	sl2312_nand_erase_block(mtd, page);
+      	//////////////
+		status = this->waitfunc (mtd, this, FL_ERASING);
+		/* See if block erase succeeded */
+		if (status & 0x01) {
+			DEBUG (MTD_DEBUG_LEVEL0, "nand_erase: " "Failed erase, page 0x%08x\n", page);
+			instr->state = MTD_ERASE_FAILED;
+			instr->fail_addr = (page << this->page_shift);
+			goto erase_exit;
+		}
+
+		/* Increment page address and decrement length */
+		len -= (1 << this->phys_erase_shift);
+		page += pages_per_block;
+
+		/* Check, if we cross a chip boundary */
+		if (len && !(page & this->pagemask)) {
+			chipnr++;
+			this->select_chip(mtd, 0);
+			this->select_chip(mtd, 0);
+		}
+		//sl2312_flash_delay();
+           schedule();
+           //cond_resched();
+	}
+	instr->state = MTD_ERASE_DONE;
+
+erase_exit:
+	/* De-select the NAND device */
+	this->select_chip(mtd, 0);
+	spin_unlock_bh (&this->chip_lock);
+
+	ret = instr->state == MTD_ERASE_DONE ? 0 : -EIO;;
+	/* Do call back function */
+	if (!ret && instr->callback)
+		instr->callback (instr);
+
+	/* The device is ready */
+	spin_lock_bh (&this->chip_lock);
+	this->state = FL_READY;
+	spin_unlock_bh (&this->chip_lock);
+	FLASH_WRITE_REG(NFLASH_ACCESS, NFLASH_DIRECT);
+	/* Return more or less happy */
+	return ret;
+}
+
+static void sl2312_nand_select_chip(struct mtd_info *mtd, int chip)
+{
+	//struct nand_chip *this = mtd->priv;
+
+	switch(chip) {
+	case -1:
+		CHIP_EN = NFLASH_CHIP0_EN;
+		break;
+	case 0:
+		CHIP_EN = NFLASH_CHIP0_EN;
+		break;
+	case 1:
+		CHIP_EN = NFLASH_CHIP1_EN;
+		break;
+	default:
+			CHIP_EN = NFLASH_CHIP0_EN;
+			break;
+	}
+}
+
+/**
+ * nand_default_block_markbad - [DEFAULT] mark a block bad
+ * @mtd:	MTD device structure
+ * @ofs:	offset from device start
+ *
+ * This is the default implementation, which can be overridden by
+ * a hardware specific driver.
+*/
+static int sl2312_nand_default_block_markbad(struct mtd_info *mtd, loff_t ofs)
+{
+	struct nand_chip *this = mtd->priv;
+	u_char buf[2] = {0, 0};
+	size_t	retlen;
+	int block;
+
+	/* Get block number */
+	block = ((int) ofs) >> this->bbt_erase_shift;
+	this->bbt[block >> 2] |= 0x01 << ((block & 0x03) << 1);
+
+	/* Do we have a flash based bad block table ? */
+	if (this->options & NAND_USE_FLASH_BBT)
+		return nand_update_bbt (mtd, ofs);
+
+	/* We write two bytes, so we dont have to mess with 16 bit access */
+	ofs += mtd->oobsize + (this->badblockpos & ~0x01);
+	return sl2312_nand_write_oob (mtd, ofs , 2, &retlen, buf);
+}
+
+/* Appropriate chip should already be selected */
+static int sl2312_nand_block_bad(struct mtd_info *mtd, loff_t ofs, int getchip)//(struct mtd_info *mtd, unsigned long page, )
+{
+	u_char *buf, *oobbuf;
+	size_t  retlen;
+	unsigned long page, chipnr;
+	struct nand_chip *this = mtd->priv;
+
+	if (getchip) {
+		page = (int)(ofs >> this->page_shift);
+		chipnr = (int)(ofs >> this->chip_shift);
+
+		/* Grab the lock and see if the device is available */
+		sl2312_nand_get_chip (this, mtd, FL_READING, NULL);
+		/* Select the NAND device */
+		this->select_chip(mtd, chipnr);
+	} else
+		page = (int) ofs;
+
+	buf = kmalloc (mtd->oobblock,GFP_KERNEL);
+	oobbuf = kmalloc (mtd->oobsize,GFP_KERNEL);
+
+	if ((!buf)||(!oobbuf)) {
+		printk ("sl2312_nand_block_bad : Unable to allocate SL2312 NAND MTD device structure.\n");
+
+	}
+
+	sl2312_nand_read_ecc (mtd, page, mtd->oobblock , &retlen, buf, oobbuf, NULL);
+
+
+	if(((mtd->oobblock < 528)&&(oobbuf[5] != 0xff))||((mtd->oobblock > 528)&&(oobbuf[0] != 0xff)))
+	{
+		kfree(buf);
+		kfree(oobbuf);
+		return 1;
+	}
+
+	kfree(buf);
+	kfree(oobbuf);
+	return 0;
+}
+
+/*
+*	Use NAND read ECC
+*/
+static int sl2312_nand_read (struct mtd_info *mtd, loff_t from, size_t len, size_t * retlen, u_char * buf)
+{
+	return sl2312_nand_read_ecc (mtd, from, len, retlen, buf, NULL, NULL);
+}
+
+/*
+ * NAND read with ECC
+ */
+static int sl2312_nand_read_ecc (struct mtd_info *mtd, loff_t from, size_t len,
+			  size_t * retlen, u_char * buf, u_char * oob_buf, struct nand_oobinfo *oobsel)
+{
+	int j, col, page, opcode, i;
+	int end=0;//, ecc=0;//, end_page=0;
+	int erase_state = 0;
+	int read = 0, oob = 0, ecc_failed = 0;//, ecc_status = 0
+	struct nand_chip *this = mtd->priv;
+	u_char *data_poi, *oob_data = oob_buf;
+	//u_char ecc_calc[6];
+	//u_char ecc_code[6];
+	int 	eccmode;
+	int	*oob_config;
+
+
+
+	// use chip default if zero
+	if (oobsel == NULL)
+		oobsel = &mtd->oobinfo;
+
+	eccmode = oobsel->useecc ? this->eccmode : NAND_ECC_NONE;
+	oob_config = oobsel->eccpos;
+
+	DEBUG (MTD_DEBUG_LEVEL3, "nand_read_ecc: from = 0x%08x, len = %i\n", (unsigned int) from, (int) len);
+
+	/* Do not allow reads past end of device */
+	if ((from + len) > mtd->size) {
+		DEBUG (MTD_DEBUG_LEVEL0, "nand_read_ecc: Attempt read beyond end of device\n");
+		*retlen = 0;
+		return -EINVAL;
+	}
+
+	/* Grab the lock and see if the device is available */
+	sl2312_nand_get_chip (this, mtd ,FL_READING, &erase_state);
+
+	/* Select the NAND device */
+	this->select_chip(mtd, 0);
+
+	/* First we calculate the starting page */
+	page = from >> this->page_shift;
+
+	//end_page = mtd->oobblock + mtd->oobsize;
+	end = mtd->oobblock;
+	//ecc = mtd->eccsize;
+	/* Get raw starting column */
+	col = (from & (mtd->oobblock - 1));
+
+
+	/* Send the read command */
+	//this->cmdfunc (mtd, NAND_CMD_READ0, 0x00, page);
+
+	/* Loop until all data read */
+	FLASH_WRITE_REG(NFLASH_ACCESS, NFLASH_DIRECT);
+	while (read < len) {
+
+		//udelay(1200);
+		/* If we have consequent page reads, apply delay or wait for ready/busy pin */
+		if (read) {
+			if (!this->dev_ready)
+				udelay (this->chip_delay);
+			else
+				while (!this->dev_ready(mtd));
+		}
+
+		/*
+		 * If the read is not page aligned, we have to read into data buffer
+		 * due to ecc, else we read into return buffer direct
+		 */
+		if (!col && (len - read) >= end)
+			data_poi = &buf[read];
+		else
+			data_poi = this->data_buf;
+
+		/* get oob area, if we have no oob buffer from fs-driver */
+		if (!oob_buf) {
+			oob_data = &this->data_buf[end];
+			oob = 0;
+		}
+
+		j = 0;
+		switch (eccmode) {
+			case NAND_ECC_NONE: {	/* No ECC, Read in a page */
+				FLASH_WRITE_REG(NFLASH_ECC_CONTROL, 0x0); //set 31b = 0
+				break;
+			}
+
+			case NAND_ECC_SOFT:	/* Software ECC 3/256: Read in a page + oob data */
+				break;
+
+			case NAND_ECC_HW3_256: /* Hardware ECC 3 byte /256 byte data: Read in first 256 byte, get ecc, */
+				break;
+
+			case NAND_ECC_HW3_512:
+			case NAND_ECC_HW6_512: /* Hardware ECC 3/6 byte / 512 byte data : Read in a page  */
+				FLASH_WRITE_REG(NFLASH_ECC_CONTROL, 0x80000001); //set 31b = 0
+				break;
+
+			default:
+				printk (KERN_WARNING "Invalid NAND_ECC_MODE %d\n", this->eccmode);
+				FLASH_WRITE_REG(NFLASH_ECC_CONTROL, 0x0);
+				//BUG();
+		}//end switch
+
+			for(i=0;i<end;i++)
+			{
+				//udelay(7);
+				data_poi[i] = FLASH_READ_DATA(page*mtd->oobblock +i);
+			}
+			/* read oobdata */
+			for (i = 0; i <  mtd->oobsize; i++)
+			{
+				//udelay(7);
+				oob_data[oob + i] = FLASH_READ_DATA(page*mtd->oobblock +end+i);
+			}
+
+		/* Skip ECC, if not active */
+			if (eccmode == NAND_ECC_NONE)
+				goto readdata;
+
+			// compare ecc and correct data
+
+				opcode=FLASH_READ_REG(NFLASH_ECC_STATUS);
+				while(!(opcode&0x80000000)) //polling flash access 31b
+      			{
+        			   opcode=FLASH_READ_REG(NFLASH_ECC_STATUS);
+        			   //sl2312_flash_delay();
+        			   schedule();
+      			}
+      			for(j=0;j<(end/512);j++)
+      			{//for 2k page
+
+					opcode = 0x00000000|oob_data[mtd->oobsize-3-4*j]<<16|oob_data[mtd->oobsize-2-4*j]<<8|oob_data[mtd->oobsize-1-4*j];
+
+					//opcode=FLASH_READ_REG(NFLASH_ECC_CODE_GEN0+(j*4));
+
+					FLASH_WRITE_REG(NFLASH_ECC_OOB, opcode);
+					opcode = 0x00000000|(j<<8); //select ECC code generation 0
+					FLASH_WRITE_REG(NFLASH_ECC_CONTROL, opcode); //???
+
+					opcode=FLASH_READ_REG(NFLASH_ECC_STATUS);
+					if((opcode&0x00000003)==0x03)
+					{
+						printk (KERN_WARNING "\nPageRead Uncorrectable error !!\n");
+						ecc_failed++;
+					}
+					else if((opcode&0x00000003)==0x01)
+					{
+						printk (KERN_WARNING "\nPageRead One bit data error !!");
+						// correct data
+						if((data_poi[(opcode&0xff80)>>7]>>((opcode&0x38)>>3))%1)
+							data_poi[(opcode&0xff80)>>7] &= ~(1<<((opcode&0x38)>>3));
+						else
+							data_poi[(opcode&0xff80)>>7] |= (1<<((opcode&0x38)>>3));
+
+					}
+					else if((opcode&0x00000003)==0x02)
+					{
+						printk (KERN_WARNING "\nPageRead One bit ECC error !!\n");
+					}
+					else if((opcode&0x00000003)==0x00)
+					{
+
+					}
+
+				}//for 2k page
+readdata:
+		if (col || (len - read) < end) {
+			for (j = col; j < end && read < len; j++)
+				buf[read++] = data_poi[j];
+		} else
+			read += mtd->oobblock;
+		/* For subsequent reads align to page boundary. */
+		col = 0;
+		/* Increment page address */
+		page++;
+		schedule();
+	}
+	/* De-select the NAND device */
+	//this->select_chip(mtd, -1);
+	FLASH_WRITE_REG(NFLASH_ECC_CONTROL, 0x0); //set 31b = 0
+	FLASH_WRITE_REG(NFLASH_ACCESS, NFLASH_INDIRECT);
+	/* Wake up anyone waiting on the device */
+	spin_lock_bh (&this->chip_lock);
+	this->state = FL_READY;
+	wake_up (&this->wq);
+	spin_unlock_bh (&this->chip_lock);
+
+	/*
+	 * Return success, if no ECC failures, else -EIO
+	 * fs driver will take care of that, because
+	 * retlen == desired len and result == -EIO
+	 */
+	*retlen = read;
+	return ecc_failed ? -EIO : 0;
+}
+
+/*
+ * Wait for command done. This applies to erase and program only
+ * Erase can take up to 400ms and program up to 20ms according to
+ * general NAND and SmartMedia specs
+ *
+*/
+static int sl2312_nand_waitfunc(struct mtd_info *mtd, struct nand_chip *this, int state)
+{
+	unsigned long	timeo = jiffies;
+	int	status, opcode;
+
+	if (state == FL_ERASING)
+		 timeo += (HZ * 400) / 1000;
+	else
+		 timeo += (HZ * 20) / 1000;
+
+	spin_lock_bh (&this->chip_lock);
+	FLASH_WRITE_REG(NFLASH_ECC_CONTROL, 0x00000000); //set 31b = 0
+	FLASH_WRITE_REG(NFLASH_COUNT, 0x007f000070); //set only command no address and two data
+
+	FLASH_WRITE_REG(NFLASH_CMD_ADDR, 0x00000070); //write read status command
+
+
+	opcode = 0x80002000|DWIDTH|CHIP_EN; //set start bit & 8bits read command
+	FLASH_WRITE_REG(NFLASH_ACCESS, opcode);
+
+	while(opcode&0x80000000) //polling flash access 31b
+    {
+        opcode=FLASH_READ_REG(NFLASH_ACCESS);
+        //sl2312_flash_delay();
+        schedule();
+    }
+
+	while (time_before(jiffies, timeo)) {
+		/* Check, if we were interrupted */
+		if (this->state != state) {
+			spin_unlock_bh (&this->chip_lock);
+			FLASH_WRITE_REG(NFLASH_ACCESS, NFLASH_DIRECT);
+			return 0;
+		}
+		if (this->dev_ready) {
+			if (this->dev_ready(mtd))
+				break;
+		}
+		if (FLASH_READ_REG(NFLASH_DATA) & 0x40)
+			break;
+
+		spin_unlock_bh (&this->chip_lock);
+		yield ();
+		spin_lock_bh (&this->chip_lock);
+	}
+	status = FLASH_READ_REG(NFLASH_DATA)&0xff;
+	spin_unlock_bh (&this->chip_lock);
+	FLASH_WRITE_REG(NFLASH_ACCESS, NFLASH_DIRECT);
+	return status;
+}
+
+static int sl2312_nand_read_oob (struct mtd_info *mtd, loff_t from, size_t len, size_t * retlen, u_char * buf)
+{
+	int i, col, page, j=0;
+	//int erase_state = 0;
+	struct nand_chip *this = mtd->priv;
+	u_char *databuf, *oobbuf;
+
+	databuf = &this->data_buf[0];
+	oobbuf = &this->data_buf[mtd->oobblock];
+		for (i = 0; i < mtd->oobsize; i++)
+			oobbuf[i] = 0xff;
+
+	DEBUG (MTD_DEBUG_LEVEL3, "nand_read_oob: from = 0x%08x, len = %i\n", (unsigned int) from, (int) len);
+
+	/* Shift to get page */
+	page = ((int) from) >> this->page_shift;
+
+	/* Mask to get column */
+	col = from & (mtd->oobsize-1);  //0x0f;
+
+	/* Initialize return length value */
+	*retlen = 0;
+	sl2312_nand_read_ecc (mtd, page, mtd->oobblock , retlen, databuf, oobbuf, NULL);
+	for(i=col,j=0;i<mtd->oobsize||i<(col+len);i++,j++)
+		buf[j] = oobbuf[i];
+
+	*retlen = j ;
+	return 0;
+}
+
+#define NOTALIGNED(x) (x & (mtd->oobblock-1)) != 0
+/*
+*	Use NAND write ECC
+*/
+static int sl2312_nand_write (struct mtd_info *mtd, loff_t to, size_t len, size_t * retlen, const u_char * buf)
+{
+	return (sl2312_nand_write_ecc (mtd, to, len, retlen, buf, NULL, NULL));
+}
+
+/*
+ * NAND write with ECC
+ */
+static int sl2312_nand_write_ecc (struct mtd_info *mtd, loff_t to, size_t len,
+			   size_t * retlen, const u_char * buf, u_char * eccbuf, struct nand_oobinfo *oobsel)
+{
+	int page, ret = 0, oob = 0, written = 0;
+	struct nand_chip *this = mtd->priv;
+
+	DEBUG (MTD_DEBUG_LEVEL3, "nand_write_ecc: to = 0x%08x, len = %i\n", (unsigned int) to, (int) len);
+
+
+	/* Do not allow write past end of device */
+	if ((to + len) > mtd->size) {
+		DEBUG (MTD_DEBUG_LEVEL0, "nand_write_ecc: Attempt to write past end of page\n");
+		return -EINVAL;
+	}
+
+	/* reject writes, which are not page aligned */
+	if (NOTALIGNED (to) || NOTALIGNED(len)) {
+		printk (KERN_NOTICE "nand_write_ecc: Attempt to write not page aligned data\n");
+		return -EINVAL;
+	}
+
+	// if oobsel is NULL, use chip defaults
+	if (oobsel == NULL)
+		oobsel = &mtd->oobinfo;
+
+	/* Shift to get page */
+	page = ((int) to) >> this->page_shift;
+
+	/* Grab the lock and see if the device is available */
+	sl2312_nand_get_chip (this, mtd, FL_WRITING, NULL);
+
+	/* Select the NAND device */
+	this->select_chip(mtd, 0);
+
+	/* Check the WP bit */
+	if (!(sl2312_device_ready(mtd) & 0x80)) {
+		DEBUG (MTD_DEBUG_LEVEL0, "nand_write_ecc: Device is write protected!!!\n");
+		ret = -EIO;
+		goto out;
+	}
+
+	/* Loop until all data is written */
+	while (written < len) {
+		//udelay(100);
+		int cnt = mtd->oobblock;
+		this->data_poi = (u_char*) &buf[written];
+		/* We use the same function for write and writev */
+		if (eccbuf) {
+			ret = sl2312_nand_write_page (mtd, this, page, &eccbuf[oob], oobsel);
+			oob += mtd->oobsize;
+		} else
+			ret = sl2312_nand_write_page (mtd, this, page, NULL, oobsel);
+
+		if (ret)
+			goto out;
+
+		/* Update written bytes count */
+		written += cnt;
+		/* Increment page address */
+		page++;
+	}
+
+out:
+	/* De-select the NAND device */
+	//this->select_chip(mtd, -1);
+
+	/* Wake up anyone waiting on the device */
+	spin_lock_bh (&this->chip_lock);
+	this->state = FL_READY;
+	wake_up (&this->wq);
+	spin_unlock_bh (&this->chip_lock);
+
+	*retlen = written;
+	return ret;
+}
+
+/*
+ *	Nand_page_program function is used for write and writev !
+ *	This function will always program a full page of data
+ *	If you call it with a non page aligned buffer, you're lost :)
+ */
+static int sl2312_nand_write_page (struct mtd_info *mtd, struct nand_chip *this, int page, u_char *oob_buf,  struct nand_oobinfo *oobsel)
+{
+	int 	i, j, status, opcode;
+	u_char	ecc_code[16], *oob_data;
+	int	eccmode = oobsel->useecc ? this->eccmode : NAND_ECC_NONE;
+	//int  	*oob_config = oobsel->eccpos;
+
+	/* pad oob area, if we have no oob buffer from fs-driver */
+	if (!oob_buf) {
+		oob_data = &this->data_buf[mtd->oobblock];
+		for (i = 0; i < mtd->oobsize; i++)
+			oob_data[i] = 0xff;
+	} else
+		oob_data = oob_buf;
+
+	/* Send command to begin auto page programming */
+
+	memset(oob_data,0xff,mtd->oobsize);
+	/* Write out complete page of data, take care of eccmode */
+	switch (eccmode) {
+	/* No ecc and software ecc 3/256, write all */
+	case NAND_ECC_NONE:
+		printk (KERN_WARNING "Writing data without ECC to NAND-FLASH is not recommended\n");
+		FLASH_WRITE_REG(NFLASH_ECC_CONTROL, 0x0); //set 31b = 0
+		break;
+	case NAND_ECC_SOFT:
+		break;
+
+	/* Hardware ecc 3 byte / 256 data, write first half, get ecc, then second, if 512 byte pagesize */
+	case NAND_ECC_HW3_256:
+		break;
+
+	/* Hardware ecc 3 byte / 512 byte data, write full page */
+	case NAND_ECC_HW3_512:
+		FLASH_WRITE_REG(NFLASH_ECC_CONTROL, 0x80000001); //set 31b = 0
+
+	/* Hardware ecc 6 byte / 512 byte data, write full page */
+	case NAND_ECC_HW6_512:
+		break;
+
+	default:
+		printk (KERN_WARNING "Invalid NAND_ECC_MODE %d\n", this->eccmode);
+		FLASH_WRITE_REG(NFLASH_ECC_CONTROL, 0x0); //set 31b = 0
+		//BUG();
+	}
+
+	FLASH_WRITE_REG(NFLASH_ACCESS, NFLASH_DIRECT);
+
+	for(i=0;i<mtd->oobblock;i++)
+	{
+		//udelay(5);
+		FLASH_WRITE_DATA((page*mtd->oobblock)+i,this->data_poi[i]);
+	}
+	///////////////
+	if(eccmode!=NAND_ECC_NONE)
+	{
+		opcode=FLASH_READ_REG(NFLASH_ECC_STATUS);
+		while(!(opcode&0x80000000)) //polling flash access 31b
+    	{
+    	 	   opcode=FLASH_READ_REG(NFLASH_ECC_STATUS);
+    	 	   //sl2312_flash_delay();
+    	 	   schedule();
+    	}
+
+
+    	for(i=0;i<(mtd->oobblock/512);i++)
+    	{
+    		opcode=FLASH_READ_REG(NFLASH_ECC_CODE_GEN0+(i*4));
+
+    		for(j=3;j>0;j--)
+    		      oob_data[(mtd->oobsize-j-(i*4))] = (opcode<<((4-j)*8)) >>24;
+
+    		for(j=0;j<4;j++)
+    		{
+    			ecc_code[15-i*4] = opcode;
+    			ecc_code[15-i*4-1] = opcode>>8;
+    			ecc_code[15-i*4-2] = opcode>>16;
+    		}
+    	}
+
+    	//disable ecc
+    	FLASH_WRITE_REG(NFLASH_ECC_CONTROL, 0x00000000);
+
+    	/* Write out OOB data */
+    	for(i=0;i<mtd->oobsize;i++)
+    	{
+    		//udelay(5);
+			FLASH_WRITE_DATA((page*mtd->oobblock)+mtd->oobblock+i,oob_data[i]);
+		}
+    }
+    else
+    {
+    	for(i=0;i<mtd->oobsize;i++)
+    	{
+    		//udelay(5);
+			FLASH_WRITE_DATA((page*mtd->oobblock)+mtd->oobblock+i,0xff);
+		}
+    }
+
+
+	/* call wait ready function */
+	status = this->waitfunc (mtd, this, FL_WRITING);
+	FLASH_WRITE_REG(NFLASH_ECC_CONTROL, 0x0); //set 31b = 0
+	/* See if device thinks it succeeded */
+	if (status & 0x01) {
+		DEBUG (MTD_DEBUG_LEVEL0, "%s: " "Failed write, page 0x%08x, ", __FUNCTION__, page);
+		FLASH_WRITE_REG(NFLASH_ECC_CONTROL, 0x0); //set 31b = 0
+		return -EIO;
+	}
+
+#ifdef CONFIG_MTD_NAND_VERIFY_WRITE
+	/*
+	 * The NAND device assumes that it is always writing to
+	 * a cleanly erased page. Hence, it performs its internal
+	 * write verification only on bits that transitioned from
+	 * 1 to 0. The device does NOT verify the whole page on a
+	 * byte by byte basis. It is possible that the page was
+	 * not completely erased or the page is becoming unusable
+	 * due to wear. The read with ECC would catch the error
+	 * later when the ECC page check fails, but we would rather
+	 * catch it early in the page write stage. Better to write
+	 * no data than invalid data.
+	 */
+
+	/* Send command to read back the page */
+	this->cmdfunc (mtd, NAND_CMD_READ0, 0, page);
+	/* Loop through and verify the data */
+	if (this->verify_buf(mtd, this->data_poi, mtd->oobblock)) {
+		DEBUG (MTD_DEBUG_LEVEL0, "%s: " "Failed write verify, page 0x%08x ", __FUNCTION__, page);
+		return -EIO;
+	}
+
+	/* check, if we have a fs-supplied oob-buffer */
+	if (oob_buf) {
+		if (this->verify_buf(mtd, oob_data, mtd->oobsize)) {
+			DEBUG (MTD_DEBUG_LEVEL0, "%s: " "Failed write verify, page 0x%08x ", __FUNCTION__, page);
+			return -EIO;
+		}
+	} else {
+		if (eccmode != NAND_ECC_NONE) {
+			int ecc_bytes = 0;
+
+			switch (this->eccmode) {
+			case NAND_ECC_SOFT:
+			case NAND_ECC_HW3_256: ecc_bytes = (mtd->oobblock == 512) ? 6 : 3; break;
+			case NAND_ECC_HW3_512: ecc_bytes = 3; break;
+			case NAND_ECC_HW6_512: ecc_bytes = 6; break;
+			}
+
+
+
+			for(i=0;i < (mtd->oobblock+mtd->oobsize);i++)
+			{
+				if(i>=mtd->oobblock)
+					oob_data[i-mtd->oobblock] = FLASH_READ_DATA((page*mtd->oobblock) +i);
+				else
+					oob_data[0] = FLASH_READ_DATA((page*mtd->oobblock) +i);
+			}
+
+			if(this->eccmode == NAND_ECC_HW3_512)
+			{
+				for(i=0;i<(mtd->oobblock/512);i++)
+    			{
+    				for(j=0;j<3;j++)
+    				{
+    				    if (oob_data[mtd->oobsize-1-j-4*i] != ecc_code[15-j-4*i]) {
+							DEBUG (MTD_DEBUG_LEVEL0,
+							       "%s: Failed ECC write "
+						       "verify, page 0x%08x, " "%6i bytes were succesful\n", __FUNCTION__, page, i);
+							return -EIO;
+						}
+    				}
+    			}
+			}
+		}//eccmode != NAND_ECC_NONE
+	}
+	/*
+	 * Terminate the read command. This is faster than sending a reset command or
+	 * applying a 20us delay before issuing the next programm sequence.
+	 * This is not a problem for all chips, but I have found a bunch of them.
+	 */
+	//this->select_chip(mtd, -1);
+	//this->select_chip(mtd, 0);
+#endif
+
+	return 0;
+}
+
+/*
+ * NAND write with iovec
+ */
+static int sl2312_nand_writev (struct mtd_info *mtd, const struct kvec *vecs, unsigned long count,
+		loff_t to, size_t * retlen)
+{
+	return (sl2312_nand_writev_ecc (mtd, vecs, count, to, retlen, NULL, 0));
+}
+
+static int sl2312_nand_writev_ecc (struct mtd_info *mtd, const struct kvec *vecs, unsigned long count,
+		loff_t to, size_t * retlen, u_char *eccbuf, struct nand_oobinfo *oobsel)
+{
+	int i, page, len, total_len, ret = 0, written = 0;
+	struct nand_chip *this = mtd->priv;
+
+	/* Calculate total length of data */
+	total_len = 0;
+	for (i = 0; i < count; i++)
+		total_len += (int) vecs[i].iov_len;
+
+	DEBUG (MTD_DEBUG_LEVEL3,
+	       "nand_writev: to = 0x%08x, len = %i, count = %ld\n", (unsigned int) to, (unsigned int) total_len, count);
+
+	/* Do not allow write past end of page */
+	if ((to + total_len) > mtd->size) {
+		DEBUG (MTD_DEBUG_LEVEL0, "nand_writev: Attempted write past end of device\n");
+		return -EINVAL;
+	}
+
+	/* reject writes, which are not page aligned */
+	if (NOTALIGNED (to) || NOTALIGNED(total_len)) {
+		printk (KERN_NOTICE "nand_write_ecc: Attempt to write not page aligned data\n");
+		return -EINVAL;
+	}
+
+	// if oobsel is NULL, use chip defaults
+	if (oobsel == NULL)
+		oobsel = &mtd->oobinfo;
+
+	/* Shift to get page */
+	page = ((int) to) >> this->page_shift;
+
+	/* Grab the lock and see if the device is available */
+	sl2312_nand_get_chip (this, mtd, FL_WRITING, NULL);
+
+	/* Select the NAND device */
+	this->select_chip(mtd, 0);
+
+	/* Check the WP bit */
+	if (!(sl2312_device_ready(mtd) & 0x80)) {
+		DEBUG (MTD_DEBUG_LEVEL0, "sl2312_nand_writev_ecc: Device is write protected!!!\n");
+		ret = -EIO;
+		goto out;
+	}
+
+	/* Loop until all iovecs' data has been written */
+	len = 0;
+	while (count) {
+		/*
+		 *  Check, if the tuple gives us not enough data for a
+		 *  full page write. Then we can use the iov direct,
+		 *  else we have to copy into data_buf.
+		 */
+		if ((vecs->iov_len - len) >= mtd->oobblock) {
+			this->data_poi = (u_char *) vecs->iov_base;
+			this->data_poi += len;
+			len += mtd->oobblock;
+			/* Check, if we have to switch to the next tuple */
+			if (len >= (int) vecs->iov_len) {
+				vecs++;
+				len = 0;
+				count--;
+			}
+		} else {
+			/*
+			 * Read data out of each tuple until we have a full page
+			 * to write or we've read all the tuples.
+		 	*/
+			int cnt = 0;
+			while ((cnt < mtd->oobblock) && count) {
+				if (vecs->iov_base != NULL && vecs->iov_len) {
+					this->data_buf[cnt++] = ((u_char *) vecs->iov_base)[len++];
+				}
+				/* Check, if we have to switch to the next tuple */
+				if (len >= (int) vecs->iov_len) {
+					vecs++;
+					len = 0;
+					count--;
+				}
+			}
+			this->data_poi = this->data_buf;
+		}
+
+		/* We use the same function for write and writev !) */
+		ret = sl2312_nand_write_page (mtd, this, page, NULL, oobsel);
+		if (ret)
+			goto out;
+
+		/* Update written bytes count */
+		written += mtd->oobblock;;
+
+		/* Increment page address */
+		page++;
+	}
+
+out:
+	/* De-select the NAND device */
+	//this->select_chip(mtd, -1);
+
+	/* Wake up anyone waiting on the device */
+	spin_lock_bh (&this->chip_lock);
+	this->state = FL_READY;
+	wake_up (&this->wq);
+	spin_unlock_bh (&this->chip_lock);
+
+	*retlen = written;
+	return ret;
+}
+
+/*
+static u_char ffchars[] = {
+	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff
+};
+*/
+/*
+ * NAND write out-of-band
+ */
+static int sl2312_nand_write_oob (struct mtd_info *mtd, loff_t to, size_t len, size_t * retlen, const u_char * buf)
+{
+	int column, page, status, ret = 0, j=0;
+	struct nand_chip *this = mtd->priv;
+	u_char *databuf, *oobbuf;
+
+
+		databuf = &this->data_buf[0];
+		oobbuf = &this->data_buf[mtd->oobblock];
+		for (j = 0; j < mtd->oobsize; j++)
+			oobbuf[j] = 0xff;
+//#ifdef CONFIG_MTD_NAND_VERIFY_WRITE
+//	int 	i;
+//#endif
+
+	DEBUG (MTD_DEBUG_LEVEL3, "nand_write_oob: to = 0x%08x, len = %i\n", (unsigned int) to, (int) len);
+
+	/* Shift to get page */
+	page = ((int) to) >> this->page_shift;
+
+	/* Mask to get column */
+	column = to & 0x1f;
+
+	/* Initialize return length value */
+	*retlen = 0;
+
+	/* Do not allow write past end of page */
+	if ((column + len) > mtd->oobsize) {
+		DEBUG (MTD_DEBUG_LEVEL0, "nand_write_oob: Attempt to write past end of page\n");
+		return -EINVAL;
+	}
+
+	/* Grab the lock and see if the device is available */
+	sl2312_nand_get_chip (this, mtd, FL_WRITING, NULL);
+
+	/* Select the NAND device */
+	this->select_chip(mtd, 0);
+
+	/* Reset the chip. Some chips (like the Toshiba TC5832DC found
+	   in one of my DiskOnChip 2000 test units) will clear the whole
+	   data page too if we don't do this. I have no clue why, but
+	   I seem to have 'fixed' it in the doc2000 driver in
+	   August 1999.  dwmw2. */
+	this->cmdfunc(mtd, NAND_CMD_RESET, -1, -1);
+
+	/* Check the WP bit */
+	if (!(sl2312_device_ready(mtd) & 0x80)) {
+		DEBUG (MTD_DEBUG_LEVEL0, "nand_write_oob: Device is write protected!!!\n");
+		ret = -EIO;
+		goto out;
+	}
+	/* Write out desired data */
+	this->cmdfunc (mtd, NAND_CMD_SEQIN, mtd->oobblock, page);
+
+	sl2312_nand_read_ecc (mtd, page, mtd->oobblock , retlen, databuf, oobbuf, NULL);
+
+    for(j=column;j<(column+len);j++)
+    	oobbuf[j] = buf[j-column];
+    sl2312_nand_write_ecc (mtd, page, mtd->oobblock, retlen, databuf, oobbuf, NULL);
+
+	status = this->waitfunc (mtd, this, FL_WRITING);
+
+	/* See if device thinks it succeeded */
+	if (status & 0x01) {
+		DEBUG (MTD_DEBUG_LEVEL0, "nand_write_oob: " "Failed write, page 0x%08x\n", page);
+		ret = -EIO;
+		goto out;
+	}
+	/* Return happy */
+	*retlen = len;
+
+
+out:
+	/* De-select the NAND device */
+	//this->select_chip(mtd, -1);
+
+	/* Wake up anyone waiting on the device */
+	spin_lock_bh (&this->chip_lock);
+	this->state = FL_READY;
+	wake_up (&this->wq);
+	spin_unlock_bh (&this->chip_lock);
+
+	return ret;
+}
+
+/*
+ * NAND sync
+ */
+static void sl2312_nand_sync (struct mtd_info *mtd)
+{
+	struct nand_chip *this = mtd->priv;
+	DECLARE_WAITQUEUE (wait, current);
+
+	DEBUG (MTD_DEBUG_LEVEL3, "nand_sync: called\n");
+
+retry:
+	/* Grab the spinlock */
+	spin_lock_bh (&this->chip_lock);
+
+	/* See what's going on */
+	switch (this->state) {
+	case FL_READY:
+	case FL_SYNCING:
+		this->state = FL_SYNCING;
+		spin_unlock_bh (&this->chip_lock);
+		break;
+
+	default:
+		/* Not an idle state */
+		add_wait_queue (&this->wq, &wait);
+		spin_unlock_bh (&this->chip_lock);
+		schedule ();
+
+		remove_wait_queue (&this->wq, &wait);
+		goto retry;
+	}
+
+	/* Lock the device */
+	spin_lock_bh (&this->chip_lock);
+
+	/* Set the device to be ready again */
+	if (this->state == FL_SYNCING) {
+		this->state = FL_READY;
+		wake_up (&this->wq);
+	}
+
+	/* Unlock the device */
+	spin_unlock_bh (&this->chip_lock);
+}
+
+
+/*
+ * Scan for the NAND device
+ */
+int sl2312_nand_scan (struct mtd_info *mtd, int maxchips)
+{
+	int i, j, nand_maf_id, nand_dev_id, busw;
+	struct nand_chip *this = mtd->priv;
+	unsigned char id[4];
+
+	/* Get buswidth to select the correct functions*/
+	busw = this->options & NAND_BUSWIDTH_16;
+
+	/* check for proper chip_delay setup, set 20us if not */
+	if (!this->chip_delay)
+		this->chip_delay = 20;
+
+	/* check, if a user supplied command function given */
+	if (this->cmdfunc == NULL)
+		this->cmdfunc = sl2312_nand_command;
+
+	/* check, if a user supplied wait function given */
+	if (this->waitfunc == NULL)
+		this->waitfunc = sl2312_nand_waitfunc;
+
+	if (!this->select_chip)
+		this->select_chip = sl2312_nand_select_chip;
+	if (!this->write_byte)
+		this->write_byte = sl2312_nand_write_byte; //busw ? nand_write_byte16 : nand_write_byte;
+	if (!this->read_byte)
+		this->read_byte = sl2312_nand_read_byte; //busw ? nand_read_byte16 : nand_read_byte;
+//	if (!this->write_word)
+//		this->write_word = nand_write_word;
+//	if (!this->read_word)
+//		this->read_word = nand_read_word;
+//	if (!this->block_bad)
+		this->block_bad = sl2312_nand_block_bad; //nand_block_bad;
+	if (!this->block_markbad)
+		this->block_markbad = sl2312_nand_default_block_markbad;
+	if (!this->write_buf)
+		this->write_buf = sl2312_nand_write_buf; //busw ? nand_write_buf16 : nand_write_buf;
+	if (!this->read_buf)
+		this->read_buf = sl2312_nand_read_buf; //busw ? nand_read_buf16 : nand_read_buf;
+	if (!this->verify_buf)
+		this->verify_buf = sl2312_nand_verify_buf; //busw ? nand_verify_buf16 : nand_verify_buf;
+	if (!this->scan_bbt)
+		this->scan_bbt = sl2312_nand_scan_bbt;
+
+	/* Select the device */
+	this->select_chip(mtd, 0);
+
+	/* Read manufacturer and device IDs */
+	nand_read_id(0,id);
+
+	nand_maf_id = id[0];
+	nand_dev_id = id[1];
+
+	/* Print and store flash device information */
+	for (i = 0; nand_flash_ids[i].name != NULL; i++) {
+
+		if (nand_dev_id != nand_flash_ids[i].id)
+			continue;
+
+		if (!mtd->name) mtd->name = nand_flash_ids[i].name;
+		this->chipsize = nand_flash_ids[i].chipsize << 20;
+
+		/* New devices have all the information in additional id bytes */
+		if (!nand_flash_ids[i].pagesize) {
+			int extid;
+
+			/* The 4th id byte is the important one */
+			extid = id[3];
+			/* Calc pagesize */
+			mtd->oobblock = 1024 << (extid & 0x3);
+			extid >>= 2;
+			/* Calc oobsize */
+			mtd->oobsize = (8 << (extid & 0x03)) * (mtd->oobblock / 512);
+			extid >>= 2;
+			/* Calc blocksize. Blocksize is multiples of 64KiB */
+			mtd->erasesize = (64 * 1024)  << (extid & 0x03);
+			extid >>= 2;
+			/* Get buswidth information */
+			busw = (extid & 0x01) ? NAND_BUSWIDTH_16 : 0;
+
+		} else {
+			/* Old devices have this data hardcoded in the
+			 * device id table */
+			mtd->erasesize = nand_flash_ids[i].erasesize;
+			mtd->oobblock = nand_flash_ids[i].pagesize;
+			mtd->oobsize = mtd->oobblock / 32;
+			busw = nand_flash_ids[i].options & NAND_BUSWIDTH_16;
+		}
+
+		/* Check, if buswidth is correct. Hardware drivers should set
+		 * this correct ! */
+		if (busw != (this->options & NAND_BUSWIDTH_16)) {
+			printk (KERN_INFO "NAND device: Manufacturer ID:"
+				" 0x%02x, Chip ID: 0x%02x (%s %s)\n", nand_maf_id, nand_dev_id,
+				nand_manuf_ids[i].name , mtd->name);
+			printk (KERN_WARNING
+				"NAND bus width %d instead %d bit\n",
+					(this->options & NAND_BUSWIDTH_16) ? 16 : 8,
+					busw ? 16 : 8);
+			this->select_chip(mtd, -1);
+			return 1;
+		}
+
+		/* Calculate the address shift from the page size */
+		this->page_shift = ffs(mtd->oobblock) - 1;
+		this->bbt_erase_shift = this->phys_erase_shift = ffs(mtd->erasesize) - 1;
+		this->chip_shift = ffs(this->chipsize) - 1;
+
+		/* Set the bad block position */
+		this->badblockpos = mtd->oobblock > 512 ?
+			NAND_LARGE_BADBLOCK_POS : NAND_SMALL_BADBLOCK_POS;
+
+		/* Get chip options, preserve non chip based options */
+		this->options &= ~NAND_CHIPOPTIONS_MSK;
+		this->options |= nand_flash_ids[i].options & NAND_CHIPOPTIONS_MSK;
+		/* Set this as a default. Board drivers can override it, if neccecary */
+		this->options |= NAND_NO_AUTOINCR;
+		/* Check if this is a not a samsung device. Do not clear the options
+		 * for chips which are not having an extended id.
+		 */
+		if (nand_maf_id != NAND_MFR_SAMSUNG && !nand_flash_ids[i].pagesize)
+			this->options &= ~NAND_SAMSUNG_LP_OPTIONS;
+
+		/* Check for AND chips with 4 page planes */
+	//	if (this->options & NAND_4PAGE_ARRAY)
+	//		this->erase_cmd = multi_erase_cmd;
+	//	else
+	//		this->erase_cmd = single_erase_cmd;
+
+		/* Do not replace user supplied command function ! */
+	//	if (mtd->oobblock > 512 && this->cmdfunc == nand_command)
+	//		this->cmdfunc = nand_command_lp;
+
+		/* Try to identify manufacturer */
+		for (j = 0; nand_manuf_ids[j].id != 0x0; j++) {
+			if (nand_manuf_ids[j].id == nand_maf_id)
+				break;
+		}
+		printk (KERN_INFO "NAND device: Manufacturer ID:"
+			" 0x%02x, Chip ID: 0x%02x (%s %s)\n", nand_maf_id, nand_dev_id,
+			nand_manuf_ids[j].name , nand_flash_ids[i].name);
+		break;
+	}
+	/////////////////////////////
+
+	for (i=1; i < maxchips; i++) {
+		this->select_chip(mtd, i);
+
+		/* Send the command for reading device ID */
+		nand_read_id(1,id);
+
+		/* Read manufacturer and device IDs */
+		if (nand_maf_id != id[0] ||
+		    nand_dev_id != id[1])
+			break;
+	}
+	if (i > 1)
+		printk(KERN_INFO "%d NAND chips detected\n", i);
+
+	/* Allocate buffers, if neccecary */
+	if (!this->oob_buf) {
+		size_t len;
+		len = mtd->oobsize << (this->phys_erase_shift - this->page_shift);
+		this->oob_buf = kmalloc (len, GFP_KERNEL);
+		if (!this->oob_buf) {
+			printk (KERN_ERR "nand_scan(): Cannot allocate oob_buf\n");
+			return -ENOMEM;
+		}
+		this->options |= NAND_OOBBUF_ALLOC;
+	}
+
+	if (!this->data_buf) {
+		size_t len;
+		len = mtd->oobblock + mtd->oobsize;
+		this->data_buf = kmalloc (len, GFP_KERNEL);
+		if (!this->data_buf) {
+			if (this->options & NAND_OOBBUF_ALLOC)
+				kfree (this->oob_buf);
+			printk (KERN_ERR "nand_scan(): Cannot allocate data_buf\n");
+			return -ENOMEM;
+		}
+		this->options |= NAND_DATABUF_ALLOC;
+	}
+
+	/* Store the number of chips and calc total size for mtd */
+	this->numchips = i;
+	mtd->size = i * this->chipsize;
+	/* Convert chipsize to number of pages per chip -1. */
+	this->pagemask = (this->chipsize >> this->page_shift) - 1;
+	/* Preset the internal oob buffer */
+	memset(this->oob_buf, 0xff, mtd->oobsize << (this->phys_erase_shift - this->page_shift));
+
+	/* If no default placement scheme is given, select an
+	 * appropriate one */
+	if (!this->autooob) {
+		/* Select the appropriate default oob placement scheme for
+		 * placement agnostic filesystems */
+		switch (mtd->oobsize) {
+		case 8:
+			this->autooob = &nand_oob_8;
+			break;
+		case 16:
+			this->autooob = &nand_oob_16;
+			break;
+		case 64:
+			this->autooob = &nand_oob_64;
+			break;
+		default:
+			printk (KERN_WARNING "No oob scheme defined for oobsize %d\n",
+				mtd->oobsize);
+			BUG();
+		}
+	}
+
+	/* The number of bytes available for the filesystem to place fs dependend
+	 * oob data */
+	if (this->options & NAND_BUSWIDTH_16) {
+		mtd->oobavail = mtd->oobsize - (this->autooob->eccbytes + 2);
+		if (this->autooob->eccbytes & 0x01)
+			mtd->oobavail--;
+	} else
+		mtd->oobavail = mtd->oobsize - (this->autooob->eccbytes + 1);
+
+
+	/*
+	 * check ECC mode, default to software
+	 * if 3byte/512byte hardware ECC is selected and we have 256 byte pagesize
+	 * fallback to software ECC
+	*/
+	this->eccsize = 256;	/* set default eccsize */
+	this->eccbytes = 3;
+
+	switch (this->eccmode) {
+	case NAND_ECC_HW12_2048:
+		if (mtd->oobblock < 2048) {
+			printk(KERN_WARNING "2048 byte HW ECC not possible on %d byte page size, fallback to SW ECC\n",
+			       mtd->oobblock);
+			this->eccmode = NAND_ECC_SOFT;
+			this->calculate_ecc = nand_calculate_ecc;
+			this->correct_data = nand_correct_data;
+		} else
+			this->eccsize = 2048;
+		break;
+
+	case NAND_ECC_HW3_512:
+	case NAND_ECC_HW6_512:
+	case NAND_ECC_HW8_512:
+		if (mtd->oobblock == 256) {
+			printk (KERN_WARNING "512 byte HW ECC not possible on 256 Byte pagesize, fallback to SW ECC \n");
+			this->eccmode = NAND_ECC_SOFT;
+			this->calculate_ecc = nand_calculate_ecc;
+			this->correct_data = nand_correct_data;
+		} else
+			this->eccsize = 512; /* set eccsize to 512 */
+		break;
+
+	case NAND_ECC_HW3_256:
+		break;
+
+	case NAND_ECC_NONE:
+		printk (KERN_WARNING "NAND_ECC_NONE selected by board driver. This is not recommended !!\n");
+		this->eccmode = NAND_ECC_NONE;
+		break;
+
+	case NAND_ECC_SOFT:
+		this->calculate_ecc = nand_calculate_ecc;
+		this->correct_data = nand_correct_data;
+		break;
+
+	default:
+		printk (KERN_WARNING "Invalid NAND_ECC_MODE %d\n", this->eccmode);
+		BUG();
+	}
+
+	/* Check hardware ecc function availability and adjust number of ecc bytes per
+	 * calculation step
+	*/
+	switch (this->eccmode) {
+	case NAND_ECC_HW12_2048:
+		this->eccbytes += 4;
+	case NAND_ECC_HW8_512:
+		this->eccbytes += 2;
+	case NAND_ECC_HW6_512:
+		this->eccbytes += 3;
+//	case NAND_ECC_HW3_512:
+	case NAND_ECC_HW3_256:
+		if (this->calculate_ecc && this->correct_data && this->enable_hwecc)
+			break;
+		printk (KERN_WARNING "No ECC functions supplied, Hardware ECC not possible\n");
+		BUG();
+	}
+
+	mtd->eccsize = this->eccsize;
+
+	/* Set the number of read / write steps for one page to ensure ECC generation */
+	switch (this->eccmode) {
+	case NAND_ECC_HW12_2048:
+		this->eccsteps = mtd->oobblock / 2048;
+		break;
+	case NAND_ECC_HW3_512:
+	case NAND_ECC_HW6_512:
+	case NAND_ECC_HW8_512:
+		this->eccsteps = mtd->oobblock / 512;
+		break;
+	case NAND_ECC_HW3_256:
+	case NAND_ECC_SOFT:
+		this->eccsteps = mtd->oobblock / 256;
+		break;
+
+	case NAND_ECC_NONE:
+		this->eccsteps = 1;
+		break;
+	}
+
+	/* Initialize state, waitqueue and spinlock */
+	this->state = FL_READY;
+	init_waitqueue_head (&this->wq);
+	spin_lock_init (&this->chip_lock);
+
+	/* De-select the device */
+	this->select_chip(mtd, 0);
+
+	/* Print warning message for no device */
+	if (!mtd->size) {
+		printk (KERN_WARNING "No NAND device found!!!\n");
+		return 1;
+	}
+
+	/* Fill in remaining MTD driver data */
+	mtd->type = MTD_NANDFLASH;
+	mtd->flags = MTD_CAP_NANDFLASH | MTD_ECC;
+	mtd->ecctype = MTD_ECC_SW;
+	mtd->erase = sl2312_nand_erase;
+	mtd->point = NULL;
+	mtd->unpoint = NULL;
+	mtd->read = sl2312_nand_read;
+	mtd->write = sl2312_nand_write;
+	mtd->read_ecc = sl2312_nand_read_ecc;
+	mtd->write_ecc = sl2312_nand_write_ecc;
+	mtd->read_oob = sl2312_nand_read_oob;
+	mtd->write_oob = sl2312_nand_write_oob;
+	mtd->readv = NULL;
+	mtd->writev = sl2312_nand_writev;
+	mtd->writev_ecc = sl2312_nand_writev_ecc;
+	mtd->sync = sl2312_nand_sync;
+	mtd->lock = NULL;
+	mtd->unlock = NULL;
+	mtd->suspend = NULL;
+	mtd->resume = NULL;
+	mtd->block_isbad = sl2312_nand_block_isbad;
+	mtd->block_markbad = sl2312_nand_block_markbad;
+
+	/* and make the autooob the default one */
+	memcpy(&mtd->oobinfo, this->autooob, sizeof(mtd->oobinfo));
+
+	mtd->owner = THIS_MODULE;
+
+	/* Build bad block table */
+	return this->scan_bbt (mtd);
+}
+
+/*End Add function*/
+
+/*
+ * Main initialization routine
+ */
+extern int nand_correct_data(struct mtd_info *mtd, u_char *dat, u_char *read_ecc, u_char *calc_ecc);
+
+int __init sl2312_mtd_init (void)
+{
+	struct nand_chip *this;
+	int err = 0;
+	struct mtd_partition *parts;
+	int nr_parts = 0;
+	int ret, data, *base;
+
+	printk("NAND MTD Driver Start Init ......\n");
+
+    	base = (unsigned int *)(IO_ADDRESS(SL2312_GLOBAL_BASE) + 0x30);
+    	data = *base;
+    	data&=0xffffffeb;
+    	data|=0x3; //disable p & s flash
+        *base = data;
+
+	/* Allocate memory for MTD device structure and private data */
+	sl2312_mtd = kmalloc(sizeof(struct mtd_info) + sizeof(struct nand_chip), GFP_KERNEL);
+	if (!sl2312_mtd) {
+		printk ("Unable to allocate SL2312 NAND MTD device structure.\n");
+		err = -ENOMEM;
+		goto out;
+	}
+
+      //  sl2312_device_setup();
+
+	/* io is indirect via a register so don't need to ioremap address */
+
+	/* Get pointer to private data */
+	this = (struct nand_chip *) (&sl2312_mtd[1]);
+
+	/* Initialize structures */
+	memset((char *) sl2312_mtd, 0, sizeof(struct mtd_info));
+	memset((char *) this, 0, sizeof(struct nand_chip));
+
+	/* Link the private data with the MTD structure */
+	sl2312_mtd->priv = this;
+	sl2312_mtd->name = "sl2312-nand";
+
+	/* Set address of NAND IO lines */
+	this->IO_ADDR_R = (void __iomem *)IO_ADDRESS((SL2312_FLASH_CTRL_BASE+NFLASH_DATA)); //(unsigned long)&(sl2312_ndfmcptr->dtr);
+	this->IO_ADDR_W = (void __iomem *)IO_ADDRESS((SL2312_FLASH_CTRL_BASE+NFLASH_DATA)); //(unsigned long)&(sl2312_ndfmcptr->dtr);
+	this->read_byte = sl2312_nand_read_byte;
+    this->write_byte = sl2312_nand_write_byte;
+    this->write_buf = sl2312_nand_write_buf;
+	this->read_buf = sl2312_nand_read_buf;
+	this->verify_buf = sl2312_nand_verify_buf;
+	this->select_chip = sl2312_nand_select_chip;
+	this->block_bad = sl2312_nand_block_bad;
+	this->hwcontrol = sl2312_hwcontrol;
+	this->dev_ready = sl2312_device_ready;
+	this->cmdfunc = sl2312_nand_command;
+	this->waitfunc = sl2312_nand_waitfunc;
+	//this->calculate_ecc = sl2312_readecc;
+	this->enable_hwecc = sl2312_enable_hwecc;
+	this->eccmode = NAND_ECC_HW3_512;
+	/*this->eccsize = 512;	*/
+	/* 20 us command delay time */
+	this->chip_delay = 20;
+
+	this->correct_data = nand_correct_data;
+//	this->scan_bbt = sl2312_nand_scan_bbt;
+
+	/* Allocate memory for internal data buffer */
+	this->data_buf = kmalloc (sizeof(u_char) * (sl2312_mtd->oobblock + sl2312_mtd->oobsize), GFP_KERNEL);
+	if (!this->data_buf) {
+		printk ("Unable to allocate NAND data buffer.\n");
+		err = -ENOMEM;
+		goto out_ior;
+	}
+
+	/* Scan to find existance of the device */
+	if (sl2312_nand_scan(sl2312_mtd, 1)) {
+		err = -ENXIO;
+		goto out_ior;
+	}
+
+	/* Register the partitions */
+	parts = sl2312_partitions;
+	nr_parts = sizeof(sl2312_partitions)/sizeof(*parts);
+
+	ret = add_mtd_partitions(sl2312_mtd, sl2312_partitions, nr_parts);
+	/*If we got an error, free all resources.*/
+	if (ret < 0) {
+		del_mtd_partitions(sl2312_mtd);
+		map_destroy(sl2312_mtd);
+	}
+	goto out;
+
+//out_buf:
+//	kfree (this->data_buf);
+out_ior:
+out:
+	printk("NAND MTD Driver Init Success ......\n");
+	return err;
+}
+
+module_init(sl2312_mtd_init);
+
+/*
+ * Clean up routine
+ */
+#ifdef MODULE
+static void __exit sl2312_cleanup (void)
+{
+	struct nand_chip *this = (struct nand_chip *) &sl2312_mtd[1];
+
+	/* Unregister partitions */
+	del_mtd_partitions(sl2312_mtd);
+
+	/* Unregister the device */
+	del_mtd_device (sl2312_mtd);
+
+	/* Free internal data buffers */
+	kfree (this->data_buf);
+
+	/* Free the MTD device structure */
+	kfree (sl2312_mtd);
+}
+module_exit(sl2312_cleanup);
+#endif
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Alice Hennessy <ahennessy@mvista.com>");
+MODULE_DESCRIPTION("Glue layer for SmartMediaCard on Toshiba RBsl2312");
--- /dev/null
+++ b/drivers/mtd/nand/sl2312-flash-nand.h
@@ -0,0 +1,24 @@
+#ifndef SL2312_FLASH_NAND_H
+#define SL2312_FLASH_NAND_H
+
+#include <linux/wait.h>
+#include <linux/spinlock.h>
+
+/*Add function*/
+static void nand_read_id(int chip_no,unsigned char *id);
+
+
+
+#define	NFLASH_WiDTH8              0x00000000
+#define	NFLASH_WiDTH16             0x00000400
+#define	NFLASH_WiDTH32             0x00000800
+#define NFLASH_CHIP0_EN            0x00000000  // 16th bit = 0
+#define NFLASH_CHIP1_EN            0x00010000  // 16th bit = 1
+#define	NFLASH_DIRECT              0x00004000
+#define	NFLASH_INDIRECT            0x00000000
+
+
+#define	DWIDTH             NFLASH_WiDTH8
+
+
+#endif /* SL2312_FLASH_NAND_H */
--- /dev/null
+++ b/include/linux/mtd/kvctl.h
@@ -0,0 +1,40 @@
+#ifndef KVCTL_H
+#define KVCTL_H
+
+#define VCTL_HEAD_SIZE	8
+#define VCTL_ENTRY_LEN	20
+
+typedef struct
+{
+  char header[4];
+  unsigned int entry_num;
+} vctl_mheader;
+
+typedef struct
+{
+  char header[4];
+  unsigned int size;
+  unsigned int type;
+  char majorver[4];
+  char minorver[4];
+  unsigned char *payload;
+} vctl_entry;
+
+typedef struct
+{
+  unsigned char mac[6];
+  unsigned char vlanid;
+  unsigned char vlanmap;
+} vlaninfo;
+
+#define VCT_VENDORSPEC		0
+#define VCT_BOOTLOADER		1
+#define VCT_KERNEL		2
+#define VCT_VERCTL		3
+#define VCT_CURRCONF		4
+#define VCT_DEFAULTCONF		5
+#define VCT_ROOTFS		6
+#define VCT_APP			7
+#define VCT_VLAN		8
+
+#endif
--- a/drivers/mtd/maps/Makefile
+++ b/drivers/mtd/maps/Makefile
@@ -71,3 +71,7 @@
 obj-$(CONFIG_MTD_OMAP_NOR)	+= omap_nor.o
 obj-$(CONFIG_MTD_MTX1)		+= mtx-1_flash.o
 obj-$(CONFIG_MTD_TQM834x)	+= tqm834x.o
+###### for Storlink Soc #######
+obj-$(CONFIG_MTD_SL2312_CFI) += sl2312-flash-cfi.o
+obj-$(CONFIG_MTD_SL2312_SERIAL_ATMEL) += sl2312-flash-atmel.o
+obj-$(CONFIG_MTD_SL2312_SERIAL_ST) += sl2312-flash-m25p80.o