Merge branch 'for-linus-1' of git://git.infradead.org/mtd-2.6

* 'for-linus-1' of git://git.infradead.org/mtd-2.6: (49 commits)
  mtd: mtdswap: fix compilation warning
  mtdswap: kill strict error handling option
  mtd: nand: enable software BCH ECC in nand simulator
  mtd: nand: add software BCH ECC support
  mtd: fix printf format warnings, mostly lack of %zd for size_t, in mtdswap
  mtd: sm_rtl: check kmalloc return value
  mtd: cfi: add support for AMIC flashes (e.g. A29L160AT)
  lib: add shared BCH ECC library
  mtd: mxc_nand: fix OOB corruption when page size > 2KiB
  mtd: DaVinci: Removed header file that is not required
  mtd: pxa3xx_nand: clean the keep configure code
  mtd: pxa3xx_nand: mtd scan id process could be defined by driver itself
  mtd: pxa3xx_nand: unify prepare command
  mtd: pxa3xx_nand: discard wait_for_event,write_cmd,__readid function
  mtd: pxa3xx_nand: rework irq logic
  mtd: pxa3xx_nand: make scan procedure more clear
  mtd: speedtest: fix integer overflow
  mtd: mxc_nand: fix read past buffer end
  mtd: omap3: nand: report corrected ecc errors
  jffs2: remove a trailing white space in commentaries
  ...

38 files changed, 3056 insertions, 632 deletions

diff --git a/drivers/mtd/Kconfig b/drivers/mtd/Kconfig
index 77414702cb00..b4567c35a322 100644
--- a/drivers/mtd/Kconfig
+++ b/drivers/mtd/Kconfig
@@ -33,14 +33,6 @@ config MTD_TESTS
 	  should normally be compiled as kernel modules. The modules perform
 	  various checks and verifications when loaded.
 
-config MTD_CONCAT
-	tristate "MTD concatenating support"
-	help
-	  Support for concatenating several MTD devices into a single
-	  (virtual) one. This allows you to have -for example- a JFFS(2)
-	  file system spanning multiple physical flash chips. If unsure,
-	  say 'Y'.
-
 config MTD_PARTITIONS
 	bool "MTD partitioning support"
 	help
@@ -333,6 +325,16 @@ config MTD_OOPS
 	  To use, add console=ttyMTDx to the kernel command line,
 	  where x is the MTD device number to use.
 
+config MTD_SWAP
+	tristate "Swap on MTD device support"
+	depends on MTD && SWAP
+	select MTD_BLKDEVS
+	help
+	  Provides volatile block device driver on top of mtd partition
+          suitable for swapping.  The mapping of written blocks is not saved.
+	  The driver provides wear leveling by storing erase counter into the
+	  OOB.
+
 source "drivers/mtd/chips/Kconfig"
 
 source "drivers/mtd/maps/Kconfig"
diff --git a/drivers/mtd/Makefile b/drivers/mtd/Makefile
index d4e7f25b1ebb..d578095fb255 100644
--- a/drivers/mtd/Makefile
+++ b/drivers/mtd/Makefile
@@ -4,11 +4,10 @@
 
 # Core functionality.
 obj-$(CONFIG_MTD)		+= mtd.o
-mtd-y				:= mtdcore.o mtdsuper.o
+mtd-y				:= mtdcore.o mtdsuper.o mtdconcat.o
 mtd-$(CONFIG_MTD_PARTITIONS)	+= mtdpart.o
 mtd-$(CONFIG_MTD_OF_PARTS)	+= ofpart.o
 
-obj-$(CONFIG_MTD_CONCAT)	+= mtdconcat.o
 obj-$(CONFIG_MTD_REDBOOT_PARTS) += redboot.o
 obj-$(CONFIG_MTD_CMDLINE_PARTS) += cmdlinepart.o
 obj-$(CONFIG_MTD_AFS_PARTS)	+= afs.o
@@ -26,6 +25,7 @@ obj-$(CONFIG_RFD_FTL)		+= rfd_ftl.o
 obj-$(CONFIG_SSFDC)		+= ssfdc.o
 obj-$(CONFIG_SM_FTL)		+= sm_ftl.o
 obj-$(CONFIG_MTD_OOPS)		+= mtdoops.o
+obj-$(CONFIG_MTD_SWAP)		+= mtdswap.o
 
 nftl-objs		:= nftlcore.o nftlmount.o
 inftl-objs		:= inftlcore.o inftlmount.o
diff --git a/drivers/mtd/chips/cfi_cmdset_0001.c b/drivers/mtd/chips/cfi_cmdset_0001.c
index 4aaa88f8ab5f..092aef11120c 100644
--- a/drivers/mtd/chips/cfi_cmdset_0001.c
+++ b/drivers/mtd/chips/cfi_cmdset_0001.c
@@ -455,7 +455,7 @@ struct mtd_info *cfi_cmdset_0001(struct map_info *map, int primary)
 	mtd->flags   = MTD_CAP_NORFLASH;
 	mtd->name    = map->name;
 	mtd->writesize = 1;
-	mtd->writebufsize = 1 << cfi->cfiq->MaxBufWriteSize;
+	mtd->writebufsize = cfi_interleave(cfi) << cfi->cfiq->MaxBufWriteSize;
 
 	mtd->reboot_notifier.notifier_call = cfi_intelext_reboot;
 
diff --git a/drivers/mtd/chips/cfi_cmdset_0002.c b/drivers/mtd/chips/cfi_cmdset_0002.c
index f072fcfde04e..f9a5331e9445 100644
--- a/drivers/mtd/chips/cfi_cmdset_0002.c
+++ b/drivers/mtd/chips/cfi_cmdset_0002.c
@@ -349,6 +349,7 @@ static struct cfi_fixup cfi_fixup_table[] = {
 	{ CFI_MFR_ATMEL, CFI_ID_ANY, fixup_convert_atmel_pri },
 #ifdef AMD_BOOTLOC_BUG
 	{ CFI_MFR_AMD, CFI_ID_ANY, fixup_amd_bootblock },
+	{ CFI_MFR_AMIC, CFI_ID_ANY, fixup_amd_bootblock },
 	{ CFI_MFR_MACRONIX, CFI_ID_ANY, fixup_amd_bootblock },
 #endif
 	{ CFI_MFR_AMD, 0x0050, fixup_use_secsi },
@@ -440,7 +441,7 @@ struct mtd_info *cfi_cmdset_0002(struct map_info *map, int primary)
 	mtd->flags   = MTD_CAP_NORFLASH;
 	mtd->name    = map->name;
 	mtd->writesize = 1;
-	mtd->writebufsize = 1 << cfi->cfiq->MaxBufWriteSize;
+	mtd->writebufsize = cfi_interleave(cfi) << cfi->cfiq->MaxBufWriteSize;
 
 	DEBUG(MTD_DEBUG_LEVEL3, "MTD %s(): write buffer size %d\n",
 		__func__, mtd->writebufsize);
diff --git a/drivers/mtd/chips/cfi_cmdset_0020.c b/drivers/mtd/chips/cfi_cmdset_0020.c
index c04b7658abe9..ed56ad3884fb 100644
--- a/drivers/mtd/chips/cfi_cmdset_0020.c
+++ b/drivers/mtd/chips/cfi_cmdset_0020.c
@@ -238,7 +238,7 @@ static struct mtd_info *cfi_staa_setup(struct map_info *map)
 	mtd->resume = cfi_staa_resume;
 	mtd->flags = MTD_CAP_NORFLASH & ~MTD_BIT_WRITEABLE;
 	mtd->writesize = 8; /* FIXME: Should be 0 for STMicro flashes w/out ECC */
-	mtd->writebufsize = 1 << cfi->cfiq->MaxBufWriteSize;
+	mtd->writebufsize = cfi_interleave(cfi) << cfi->cfiq->MaxBufWriteSize;
 	map->fldrv = &cfi_staa_chipdrv;
 	__module_get(THIS_MODULE);
 	mtd->name = map->name;
diff --git a/drivers/mtd/devices/m25p80.c b/drivers/mtd/devices/m25p80.c
index e4eba6cc1b2e..3fb981d4bb51 100644
--- a/drivers/mtd/devices/m25p80.c
+++ b/drivers/mtd/devices/m25p80.c
@@ -655,7 +655,8 @@ static const struct spi_device_id m25p_ids[] = {
 	{ "at26df161a", INFO(0x1f4601, 0, 64 * 1024, 32, SECT_4K) },
 	{ "at26df321",  INFO(0x1f4700, 0, 64 * 1024, 64, SECT_4K) },
 
-	/* EON -- en25pxx */
+	/* EON -- en25xxx */
+	{ "en25f32", INFO(0x1c3116, 0, 64 * 1024,  64, SECT_4K) },
 	{ "en25p32", INFO(0x1c2016, 0, 64 * 1024,  64, 0) },
 	{ "en25p64", INFO(0x1c2017, 0, 64 * 1024, 128, 0) },
 
@@ -728,6 +729,8 @@ static const struct spi_device_id m25p_ids[] = {
 	{ "m25pe80", INFO(0x208014,  0, 64 * 1024, 16,       0) },
 	{ "m25pe16", INFO(0x208015,  0, 64 * 1024, 32, SECT_4K) },
 
+	{ "m25px64", INFO(0x207117,  0, 64 * 1024, 128, 0) },
+
 	/* Winbond -- w25x "blocks" are 64K, "sectors" are 4KiB */
 	{ "w25x10", INFO(0xef3011, 0, 64 * 1024,  2,  SECT_4K) },
 	{ "w25x20", INFO(0xef3012, 0, 64 * 1024,  4,  SECT_4K) },
diff --git a/drivers/mtd/devices/mtdram.c b/drivers/mtd/devices/mtdram.c
index 26a6e809013d..1483e18971ce 100644
--- a/drivers/mtd/devices/mtdram.c
+++ b/drivers/mtd/devices/mtdram.c
@@ -121,6 +121,7 @@ int mtdram_init_device(struct mtd_info *mtd, void *mapped_address,
 	mtd->flags = MTD_CAP_RAM;
 	mtd->size = size;
 	mtd->writesize = 1;
+	mtd->writebufsize = 64; /* Mimic CFI NOR flashes */
 	mtd->erasesize = MTDRAM_ERASE_SIZE;
 	mtd->priv = mapped_address;
 
diff --git a/drivers/mtd/devices/phram.c b/drivers/mtd/devices/phram.c
index 52393282eaf1..8d28fa02a5a2 100644
--- a/drivers/mtd/devices/phram.c
+++ b/drivers/mtd/devices/phram.c
@@ -117,6 +117,7 @@ static void unregister_devices(void)
 	list_for_each_entry_safe(this, safe, &phram_list, list) {
 		del_mtd_device(&this->mtd);
 		iounmap(this->mtd.priv);
+		kfree(this->mtd.name);
 		kfree(this);
 	}
 }
@@ -275,6 +276,8 @@ static int phram_setup(const char *val, struct kernel_param *kp)
 	ret = register_device(name, start, len);
 	if (!ret)
 		pr_info("%s device: %#x at %#x\n", name, len, start);
+	else
+		kfree(name);
 
 	return ret;
 }
diff --git a/drivers/mtd/maps/Kconfig b/drivers/mtd/maps/Kconfig
index 5d37d315fa98..44b1f46458ca 100644
--- a/drivers/mtd/maps/Kconfig
+++ b/drivers/mtd/maps/Kconfig
@@ -114,7 +114,7 @@ config MTD_SUN_UFLASH
 
 config MTD_SC520CDP
 	tristate "CFI Flash device mapped on AMD SC520 CDP"
-	depends on X86 && MTD_CFI && MTD_CONCAT
+	depends on X86 && MTD_CFI
 	help
 	  The SC520 CDP board has two banks of CFI-compliant chips and one
 	  Dual-in-line JEDEC chip. This 'mapping' driver supports that
@@ -262,7 +262,7 @@ config MTD_BCM963XX
 
 config MTD_DILNETPC
 	tristate "CFI Flash device mapped on DIL/Net PC"
-	depends on X86 && MTD_CONCAT && MTD_PARTITIONS && MTD_CFI_INTELEXT && BROKEN
+	depends on X86 && MTD_PARTITIONS && MTD_CFI_INTELEXT && BROKEN
 	help
 	  MTD map driver for SSV DIL/Net PC Boards "DNP" and "ADNP".
 	  For details, see <http://www.ssv-embedded.de/ssv/pc104/p169.htm>
@@ -552,4 +552,13 @@ config MTD_PISMO
 
 	  When built as a module, it will be called pismo.ko
 
+config MTD_LATCH_ADDR
+        tristate "Latch-assisted Flash Chip Support"
+        depends on MTD_COMPLEX_MAPPINGS
+        help
+          Map driver which allows flashes to be partially physically addressed
+          and have the upper address lines set by a board specific code.
+
+          If compiled as a module, it will be called latch-addr-flash.
+
 endmenu
diff --git a/drivers/mtd/maps/Makefile b/drivers/mtd/maps/Makefile
index c7869c7a6b18..08533bd5cba7 100644
--- a/drivers/mtd/maps/Makefile
+++ b/drivers/mtd/maps/Makefile
@@ -59,3 +59,4 @@ obj-$(CONFIG_MTD_RBTX4939)	+= rbtx4939-flash.o
 obj-$(CONFIG_MTD_VMU)		+= vmu-flash.o
 obj-$(CONFIG_MTD_GPIO_ADDR)	+= gpio-addr-flash.o
 obj-$(CONFIG_MTD_BCM963XX)	+= bcm963xx-flash.o
+obj-$(CONFIG_MTD_LATCH_ADDR)	+= latch-addr-flash.o
diff --git a/drivers/mtd/maps/ceiva.c b/drivers/mtd/maps/ceiva.c
index c09f4f57093e..e5f645b775ad 100644
--- a/drivers/mtd/maps/ceiva.c
+++ b/drivers/mtd/maps/ceiva.c
@@ -194,16 +194,10 @@ static int __init clps_setup_mtd(struct clps_info *clps, int nr, struct mtd_info
 			 * We detected multiple devices.  Concatenate
 			 * them together.
 			 */
-#ifdef CONFIG_MTD_CONCAT
 			*rmtd = mtd_concat_create(subdev, found,
 						  "clps flash");
 			if (*rmtd == NULL)
 				ret = -ENXIO;
-#else
-			printk(KERN_ERR "clps flash: multiple devices "
-			       "found but MTD concat support disabled.\n");
-			ret = -ENXIO;
-#endif
 		}
 	}
 
diff --git a/drivers/mtd/maps/integrator-flash.c b/drivers/mtd/maps/integrator-flash.c
index 2aac41bde8b3..e22ff5adbbf4 100644
--- a/drivers/mtd/maps/integrator-flash.c
+++ b/drivers/mtd/maps/integrator-flash.c
@@ -202,7 +202,6 @@ static int armflash_probe(struct platform_device *dev)
 	if (info->nr_subdev == 1)
 		info->mtd = info->subdev[0].mtd;
 	else if (info->nr_subdev > 1) {
-#ifdef CONFIG_MTD_CONCAT
 		struct mtd_info *cdev[info->nr_subdev];
 
 		/*
@@ -215,11 +214,6 @@ static int armflash_probe(struct platform_device *dev)
 					      dev_name(&dev->dev));
 		if (info->mtd == NULL)
 			err = -ENXIO;
-#else
-		printk(KERN_ERR "armflash: multiple devices found but "
-		       "MTD concat support disabled.\n");
-		err = -ENXIO;
-#endif
 	}
 
 	if (err < 0)
@@ -244,10 +238,8 @@ static int armflash_probe(struct platform_device *dev)
  cleanup:
 	if (info->mtd) {
 		del_mtd_partitions(info->mtd);
-#ifdef CONFIG_MTD_CONCAT
 		if (info->mtd != info->subdev[0].mtd)
 			mtd_concat_destroy(info->mtd);
-#endif
 	}
 	kfree(info->parts);
  subdev_err:
@@ -272,10 +264,8 @@ static int armflash_remove(struct platform_device *dev)
 	if (info) {
 		if (info->mtd) {
 			del_mtd_partitions(info->mtd);
-#ifdef CONFIG_MTD_CONCAT
 			if (info->mtd != info->subdev[0].mtd)
 				mtd_concat_destroy(info->mtd);
-#endif
 		}
 		kfree(info->parts);
 
diff --git a/drivers/mtd/maps/latch-addr-flash.c b/drivers/mtd/maps/latch-addr-flash.c
new file mode 100644
index 000000000000..ee2548085334
--- /dev/null
+++ b/drivers/mtd/maps/latch-addr-flash.c
@@ -0,0 +1,272 @@
+/*
+ * Interface for NOR flash driver whose high address lines are latched
+ *
+ * Copyright © 2000 Nicolas Pitre <nico@cam.org>
+ * Copyright © 2005-2008 Analog Devices Inc.
+ * Copyright © 2008 MontaVista Software, Inc. <source@mvista.com>
+ *
+ * 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/init.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/mtd/mtd.h>
+#include <linux/mtd/map.h>
+#include <linux/mtd/partitions.h>
+#include <linux/platform_device.h>
+#include <linux/mtd/latch-addr-flash.h>
+#include <linux/slab.h>
+
+#define DRIVER_NAME "latch-addr-flash"
+
+struct latch_addr_flash_info {
+	struct mtd_info		*mtd;
+	struct map_info		map;
+	struct resource		*res;
+
+	void			(*set_window)(unsigned long offset, void *data);
+	void			*data;
+
+	/* cache; could be found out of res */
+	unsigned long		win_mask;
+
+	int			nr_parts;
+	struct mtd_partition	*parts;
+
+	spinlock_t		lock;
+};
+
+static map_word lf_read(struct map_info *map, unsigned long ofs)
+{
+	struct latch_addr_flash_info *info;
+	map_word datum;
+
+	info = (struct latch_addr_flash_info *)map->map_priv_1;
+
+	spin_lock(&info->lock);
+
+	info->set_window(ofs, info->data);
+	datum = inline_map_read(map, info->win_mask & ofs);
+
+	spin_unlock(&info->lock);
+
+	return datum;
+}
+
+static void lf_write(struct map_info *map, map_word datum, unsigned long ofs)
+{
+	struct latch_addr_flash_info *info;
+
+	info = (struct latch_addr_flash_info *)map->map_priv_1;
+
+	spin_lock(&info->lock);
+
+	info->set_window(ofs, info->data);
+	inline_map_write(map, datum, info->win_mask & ofs);
+
+	spin_unlock(&info->lock);
+}
+
+static void lf_copy_from(struct map_info *map, void *to,
+		unsigned long from, ssize_t len)
+{
+	struct latch_addr_flash_info *info =
+		(struct latch_addr_flash_info *) map->map_priv_1;
+	unsigned n;
+
+	while (len > 0) {
+		n = info->win_mask + 1 - (from & info->win_mask);
+		if (n > len)
+			n = len;
+
+		spin_lock(&info->lock);
+
+		info->set_window(from, info->data);
+		memcpy_fromio(to, map->virt + (from & info->win_mask), n);
+
+		spin_unlock(&info->lock);
+
+		to += n;
+		from += n;
+		len -= n;
+	}
+}
+
+static char *rom_probe_types[] = { "cfi_probe", NULL };
+
+static char *part_probe_types[] = { "cmdlinepart", NULL };
+
+static int latch_addr_flash_remove(struct platform_device *dev)
+{
+	struct latch_addr_flash_info *info;
+	struct latch_addr_flash_data *latch_addr_data;
+
+	info = platform_get_drvdata(dev);
+	if (info == NULL)
+		return 0;
+	platform_set_drvdata(dev, NULL);
+
+	latch_addr_data = dev->dev.platform_data;
+
+	if (info->mtd != NULL) {
+		if (mtd_has_partitions()) {
+			if (info->nr_parts) {
+				del_mtd_partitions(info->mtd);
+				kfree(info->parts);
+			} else if (latch_addr_data->nr_parts) {
+				del_mtd_partitions(info->mtd);
+			} else {
+				del_mtd_device(info->mtd);
+			}
+		} else {
+			del_mtd_device(info->mtd);
+		}
+		map_destroy(info->mtd);
+	}
+
+	if (info->map.virt != NULL)
+		iounmap(info->map.virt);
+
+	if (info->res != NULL)
+		release_mem_region(info->res->start, resource_size(info->res));
+
+	kfree(info);
+
+	if (latch_addr_data->done)
+		latch_addr_data->done(latch_addr_data->data);
+
+	return 0;
+}
+
+static int __devinit latch_addr_flash_probe(struct platform_device *dev)
+{
+	struct latch_addr_flash_data *latch_addr_data;
+	struct latch_addr_flash_info *info;
+	resource_size_t win_base = dev->resource->start;
+	resource_size_t win_size = resource_size(dev->resource);
+	char **probe_type;
+	int chipsel;
+	int err;
+
+	latch_addr_data = dev->dev.platform_data;
+	if (latch_addr_data == NULL)
+		return -ENODEV;
+
+	pr_notice("latch-addr platform flash device: %#llx byte "
+		  "window at %#.8llx\n",
+		  (unsigned long long)win_size, (unsigned long long)win_base);
+
+	chipsel = dev->id;
+
+	if (latch_addr_data->init) {
+		err = latch_addr_data->init(latch_addr_data->data, chipsel);
+		if (err != 0)
+			return err;
+	}
+
+	info = kzalloc(sizeof(struct latch_addr_flash_info), GFP_KERNEL);
+	if (info == NULL) {
+		err = -ENOMEM;
+		goto done;
+	}
+
+	platform_set_drvdata(dev, info);
+
+	info->res = request_mem_region(win_base, win_size, DRIVER_NAME);
+	if (info->res == NULL) {
+		dev_err(&dev->dev, "Could not reserve memory region\n");
+		err = -EBUSY;
+		goto free_info;
+	}
+
+	info->map.name		= DRIVER_NAME;
+	info->map.size		= latch_addr_data->size;
+	info->map.bankwidth	= latch_addr_data->width;
+
+	info->map.phys		= NO_XIP;
+	info->map.virt		= ioremap(win_base, win_size);
+	if (!info->map.virt) {
+		err = -ENOMEM;
+		goto free_res;
+	}
+
+	info->map.map_priv_1	= (unsigned long)info;
+
+	info->map.read		= lf_read;
+	info->map.copy_from	= lf_copy_from;
+	info->map.write		= lf_write;
+	info->set_window	= latch_addr_data->set_window;
+	info->data		= latch_addr_data->data;
+	info->win_mask		= win_size - 1;
+
+	spin_lock_init(&info->lock);
+
+	for (probe_type = rom_probe_types; !info->mtd && *probe_type;
+		probe_type++)
+		info->mtd = do_map_probe(*probe_type, &info->map);
+
+	if (info->mtd == NULL) {
+		dev_err(&dev->dev, "map_probe failed\n");
+		err = -ENODEV;
+		goto iounmap;
+	}
+	info->mtd->owner = THIS_MODULE;
+
+	if (mtd_has_partitions()) {
+
+		err = parse_mtd_partitions(info->mtd,
+					   (const char **)part_probe_types,
+					   &info->parts, 0);
+		if (err > 0) {
+			add_mtd_partitions(info->mtd, info->parts, err);
+			return 0;
+		}
+		if (latch_addr_data->nr_parts) {
+			pr_notice("Using latch-addr-flash partition information\n");
+			add_mtd_partitions(info->mtd, latch_addr_data->parts,
+					latch_addr_data->nr_parts);
+			return 0;
+		}
+	}
+	add_mtd_device(info->mtd);
+	return 0;
+
+iounmap:
+	iounmap(info->map.virt);
+free_res:
+	release_mem_region(info->res->start, resource_size(info->res));
+free_info:
+	kfree(info);
+done:
+	if (latch_addr_data->done)
+		latch_addr_data->done(latch_addr_data->data);
+	return err;
+}
+
+static struct platform_driver latch_addr_flash_driver = {
+	.probe		= latch_addr_flash_probe,
+	.remove		= __devexit_p(latch_addr_flash_remove),
+	.driver		= {
+		.name	= DRIVER_NAME,
+	},
+};
+
+static int __init latch_addr_flash_init(void)
+{
+	return platform_driver_register(&latch_addr_flash_driver);
+}
+module_init(latch_addr_flash_init);
+
+static void __exit latch_addr_flash_exit(void)
+{
+	platform_driver_unregister(&latch_addr_flash_driver);
+}
+module_exit(latch_addr_flash_exit);
+
+MODULE_AUTHOR("David Griego <dgriego@mvista.com>");
+MODULE_DESCRIPTION("MTD map driver for flashes addressed physically with upper "
+		"address lines being set board specifically");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/mtd/maps/physmap.c b/drivers/mtd/maps/physmap.c
index 4c18b98a3110..7522df4f71f1 100644
--- a/drivers/mtd/maps/physmap.c
+++ b/drivers/mtd/maps/physmap.c
@@ -59,10 +59,8 @@ static int physmap_flash_remove(struct platform_device *dev)
 #else
 		del_mtd_device(info->cmtd);
 #endif
-#ifdef CONFIG_MTD_CONCAT
 		if (info->cmtd != info->mtd[0])
 			mtd_concat_destroy(info->cmtd);
-#endif
 	}
 
 	for (i = 0; i < MAX_RESOURCES; i++) {
@@ -159,15 +157,9 @@ static int physmap_flash_probe(struct platform_device *dev)
 		/*
 		 * We detected multiple devices. Concatenate them together.
 		 */
-#ifdef CONFIG_MTD_CONCAT
 		info->cmtd = mtd_concat_create(info->mtd, devices_found, dev_name(&dev->dev));
 		if (info->cmtd == NULL)
 			err = -ENXIO;
-#else
-		printk(KERN_ERR "physmap-flash: multiple devices "
-		       "found but MTD concat support disabled.\n");
-		err = -ENXIO;
-#endif
 	}
 	if (err)
 		goto err_out;
diff --git a/drivers/mtd/maps/physmap_of.c b/drivers/mtd/maps/physmap_of.c
index 3db0cb083d31..bd483f0c57e1 100644
--- a/drivers/mtd/maps/physmap_of.c
+++ b/drivers/mtd/maps/physmap_of.c
@@ -104,12 +104,10 @@ static int of_flash_remove(struct platform_device *dev)
 		return 0;
 	dev_set_drvdata(&dev->dev, NULL);
 
-#ifdef CONFIG_MTD_CONCAT
 	if (info->cmtd != info->list[0].mtd) {
 		del_mtd_device(info->cmtd);
 		mtd_concat_destroy(info->cmtd);
 	}
-#endif
 
 	if (info->cmtd) {
 		if (OF_FLASH_PARTS(info)) {
@@ -337,16 +335,10 @@ static int __devinit of_flash_probe(struct platform_device *dev)
 		/*
 		 * We detected multiple devices. Concatenate them together.
 		 */
-#ifdef CONFIG_MTD_CONCAT
 		info->cmtd = mtd_concat_create(mtd_list, info->list_size,
 					       dev_name(&dev->dev));
 		if (info->cmtd == NULL)
 			err = -ENXIO;
-#else
-		printk(KERN_ERR "physmap_of: multiple devices "
-		       "found but MTD concat support disabled.\n");
-		err = -ENXIO;
-#endif
 	}
 	if (err)
 		goto err_out;
diff --git a/drivers/mtd/maps/sa1100-flash.c b/drivers/mtd/maps/sa1100-flash.c
index f3af87e08ecd..da875908ea8e 100644
--- a/drivers/mtd/maps/sa1100-flash.c
+++ b/drivers/mtd/maps/sa1100-flash.c
@@ -232,10 +232,8 @@ static void sa1100_destroy(struct sa_info *info, struct flash_platform_data *pla
 		else
 			del_mtd_partitions(info->mtd);
 #endif
-#ifdef CONFIG_MTD_CONCAT
 		if (info->mtd != info->subdev[0].mtd)
 			mtd_concat_destroy(info->mtd);
-#endif
 	}
 
 	kfree(info->parts);
@@ -321,7 +319,6 @@ sa1100_setup_mtd(struct platform_device *pdev, struct flash_platform_data *plat)
 		info->mtd = info->subdev[0].mtd;
 		ret = 0;
 	} else if (info->num_subdev > 1) {
-#ifdef CONFIG_MTD_CONCAT
 		struct mtd_info *cdev[nr];
 		/*
 		 * We detected multiple devices.  Concatenate them together.
@@ -333,11 +330,6 @@ sa1100_setup_mtd(struct platform_device *pdev, struct flash_platform_data *plat)
 					      plat->name);
 		if (info->mtd == NULL)
 			ret = -ENXIO;
-#else
-		printk(KERN_ERR "SA1100 flash: multiple devices "
-		       "found but MTD concat support disabled.\n");
-		ret = -ENXIO;
-#endif
 	}
 
 	if (ret == 0)
diff --git a/drivers/mtd/maps/ts5500_flash.c b/drivers/mtd/maps/ts5500_flash.c
index e2147bf11c88..e02dfa9d4ddd 100644
--- a/drivers/mtd/maps/ts5500_flash.c
+++ b/drivers/mtd/maps/ts5500_flash.c
@@ -94,7 +94,6 @@ static int __init init_ts5500_map(void)
 	return 0;
 
 err1:
-	map_destroy(mymtd);
 	iounmap(ts5500_map.virt);
 err2:
 	return rc;
diff --git a/drivers/mtd/mtd_blkdevs.c b/drivers/mtd/mtd_blkdevs.c
index e0a2373bf0e2..a534e1f0c348 100644
--- a/drivers/mtd/mtd_blkdevs.c
+++ b/drivers/mtd/mtd_blkdevs.c
@@ -40,7 +40,7 @@
 static LIST_HEAD(blktrans_majors);
 static DEFINE_MUTEX(blktrans_ref_mutex);
 
-void blktrans_dev_release(struct kref *kref)
+static void blktrans_dev_release(struct kref *kref)
 {
 	struct mtd_blktrans_dev *dev =
 		container_of(kref, struct mtd_blktrans_dev, ref);
@@ -67,7 +67,7 @@ unlock:
 	return dev;
 }
 
-void blktrans_dev_put(struct mtd_blktrans_dev *dev)
+static void blktrans_dev_put(struct mtd_blktrans_dev *dev)
 {
 	mutex_lock(&blktrans_ref_mutex);
 	kref_put(&dev->ref, blktrans_dev_release);
@@ -119,18 +119,43 @@ static int do_blktrans_request(struct mtd_blktrans_ops *tr,
 	}
 }
 
+int mtd_blktrans_cease_background(struct mtd_blktrans_dev *dev)
+{
+	if (kthread_should_stop())
+		return 1;
+
+	return dev->bg_stop;
+}
+EXPORT_SYMBOL_GPL(mtd_blktrans_cease_background);
+
 static int mtd_blktrans_thread(void *arg)
 {
 	struct mtd_blktrans_dev *dev = arg;
+	struct mtd_blktrans_ops *tr = dev->tr;
 	struct request_queue *rq = dev->rq;
 	struct request *req = NULL;
+	int background_done = 0;
 
 	spin_lock_irq(rq->queue_lock);
 
 	while (!kthread_should_stop()) {
 		int res;
 
+		dev->bg_stop = false;
 		if (!req && !(req = blk_fetch_request(rq))) {
+			if (tr->background && !background_done) {
+				spin_unlock_irq(rq->queue_lock);
+				mutex_lock(&dev->lock);
+				tr->background(dev);
+				mutex_unlock(&dev->lock);
+				spin_lock_irq(rq->queue_lock);
+				/*
+				 * Do background processing just once per idle
+				 * period.
+				 */
+				background_done = !dev->bg_stop;
+				continue;
+			}
 			set_current_state(TASK_INTERRUPTIBLE);
 
 			if (kthread_should_stop())
@@ -152,6 +177,8 @@ static int mtd_blktrans_thread(void *arg)
 
 		if (!__blk_end_request_cur(req, res))
 			req = NULL;
+
+		background_done = 0;
 	}
 
 	if (req)
@@ -172,8 +199,10 @@ static void mtd_blktrans_request(struct request_queue *rq)
 	if (!dev)
 		while ((req = blk_fetch_request(rq)) != NULL)
 			__blk_end_request_all(req, -ENODEV);
-	else
+	else {
+		dev->bg_stop = true;
 		wake_up_process(dev->thread);
+	}
 }
 
 static int blktrans_open(struct block_device *bdev, fmode_t mode)
@@ -379,9 +408,10 @@ int add_mtd_blktrans_dev(struct mtd_blktrans_dev *new)
 	new->rq->queuedata = new;
 	blk_queue_logical_block_size(new->rq, tr->blksize);
 
-	if (tr->discard)
-		queue_flag_set_unlocked(QUEUE_FLAG_DISCARD,
-					new->rq);
+	if (tr->discard) {
+		queue_flag_set_unlocked(QUEUE_FLAG_DISCARD, new->rq);
+		new->rq->limits.max_discard_sectors = UINT_MAX;
+	}
 
 	gd->queue = new->rq;
 
diff --git a/drivers/mtd/mtdconcat.c b/drivers/mtd/mtdconcat.c
index 5f5777bd3f75..5060e608ea5d 100644
--- a/drivers/mtd/mtdconcat.c
+++ b/drivers/mtd/mtdconcat.c
@@ -750,6 +750,7 @@ struct mtd_info *mtd_concat_create(struct mtd_info *subdev[],	/* subdevices to c
 	struct mtd_concat *concat;
 	uint32_t max_erasesize, curr_erasesize;
 	int num_erase_region;
+	int max_writebufsize = 0;
 
 	printk(KERN_NOTICE "Concatenating MTD devices:\n");
 	for (i = 0; i < num_devs; i++)
@@ -776,7 +777,12 @@ struct mtd_info *mtd_concat_create(struct mtd_info *subdev[],	/* subdevices to c
 	concat->mtd.size = subdev[0]->size;
 	concat->mtd.erasesize = subdev[0]->erasesize;
 	concat->mtd.writesize = subdev[0]->writesize;
-	concat->mtd.writebufsize = subdev[0]->writebufsize;
+
+	for (i = 0; i < num_devs; i++)
+		if (max_writebufsize < subdev[i]->writebufsize)
+			max_writebufsize = subdev[i]->writebufsize;
+	concat->mtd.writebufsize = max_writebufsize;
+
 	concat->mtd.subpage_sft = subdev[0]->subpage_sft;
 	concat->mtd.oobsize = subdev[0]->oobsize;
 	concat->mtd.oobavail = subdev[0]->oobavail;
diff --git a/drivers/mtd/mtdcore.c b/drivers/mtd/mtdcore.c
index 527cebf58da4..da69bc8a5a7d 100644
--- a/drivers/mtd/mtdcore.c
+++ b/drivers/mtd/mtdcore.c
@@ -43,7 +43,7 @@
  * backing device capabilities for non-mappable devices (such as NAND flash)
  * - permits private mappings, copies are taken of the data
  */
-struct backing_dev_info mtd_bdi_unmappable = {
+static struct backing_dev_info mtd_bdi_unmappable = {
 	.capabilities	= BDI_CAP_MAP_COPY,
 };
 
@@ -52,7 +52,7 @@ struct backing_dev_info mtd_bdi_unmappable = {
  * - permits private mappings, copies are taken of the data
  * - permits non-writable shared mappings
  */
-struct backing_dev_info mtd_bdi_ro_mappable = {
+static struct backing_dev_info mtd_bdi_ro_mappable = {
 	.capabilities	= (BDI_CAP_MAP_COPY | BDI_CAP_MAP_DIRECT |
 			   BDI_CAP_EXEC_MAP | BDI_CAP_READ_MAP),
 };
@@ -62,7 +62,7 @@ struct backing_dev_info mtd_bdi_ro_mappable = {
  * - permits private mappings, copies are taken of the data
  * - permits non-writable shared mappings
  */
-struct backing_dev_info mtd_bdi_rw_mappable = {
+static struct backing_dev_info mtd_bdi_rw_mappable = {
 	.capabilities	= (BDI_CAP_MAP_COPY | BDI_CAP_MAP_DIRECT |
 			   BDI_CAP_EXEC_MAP | BDI_CAP_READ_MAP |
 			   BDI_CAP_WRITE_MAP),
diff --git a/drivers/mtd/mtdswap.c b/drivers/mtd/mtdswap.c
new file mode 100644
index 000000000000..237913c5c92c
--- /dev/null
+++ b/drivers/mtd/mtdswap.c
@@ -0,0 +1,1587 @@
+/*
+ * Swap block device support for MTDs
+ * Turns an MTD device into a swap device with block wear leveling
+ *
+ * Copyright © 2007,2011 Nokia Corporation. All rights reserved.
+ *
+ * Authors: Jarkko Lavinen <jarkko.lavinen@nokia.com>
+ *
+ * Based on Richard Purdie's earlier implementation in 2007. Background
+ * support and lock-less operation written by Adrian Hunter.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * 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., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/mtd/mtd.h>
+#include <linux/mtd/blktrans.h>
+#include <linux/rbtree.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/vmalloc.h>
+#include <linux/genhd.h>
+#include <linux/swap.h>
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
+#include <linux/device.h>
+#include <linux/math64.h>
+
+#define MTDSWAP_PREFIX "mtdswap"
+
+/*
+ * The number of free eraseblocks when GC should stop
+ */
+#define CLEAN_BLOCK_THRESHOLD	20
+
+/*
+ * Number of free eraseblocks below which GC can also collect low frag
+ * blocks.
+ */
+#define LOW_FRAG_GC_TRESHOLD	5
+
+/*
+ * Wear level cost amortization. We want to do wear leveling on the background
+ * without disturbing gc too much. This is made by defining max GC frequency.
+ * Frequency value 6 means 1/6 of the GC passes will pick an erase block based
+ * on the biggest wear difference rather than the biggest dirtiness.
+ *
+ * The lower freq2 should be chosen so that it makes sure the maximum erase
+ * difference will decrease even if a malicious application is deliberately
+ * trying to make erase differences large.
+ */
+#define MAX_ERASE_DIFF		4000
+#define COLLECT_NONDIRTY_BASE	MAX_ERASE_DIFF
+#define COLLECT_NONDIRTY_FREQ1	6
+#define COLLECT_NONDIRTY_FREQ2	4
+
+#define PAGE_UNDEF		UINT_MAX
+#define BLOCK_UNDEF		UINT_MAX
+#define BLOCK_ERROR		(UINT_MAX - 1)
+#define BLOCK_MAX		(UINT_MAX - 2)
+
+#define EBLOCK_BAD		(1 << 0)
+#define EBLOCK_NOMAGIC		(1 << 1)
+#define EBLOCK_BITFLIP		(1 << 2)
+#define EBLOCK_FAILED		(1 << 3)
+#define EBLOCK_READERR		(1 << 4)
+#define EBLOCK_IDX_SHIFT	5
+
+struct swap_eb {
+	struct rb_node rb;
+	struct rb_root *root;
+
+	unsigned int flags;
+	unsigned int active_count;
+	unsigned int erase_count;
+	unsigned int pad;		/* speeds up pointer decremtnt */
+};
+
+#define MTDSWAP_ECNT_MIN(rbroot) (rb_entry(rb_first(rbroot), struct swap_eb, \
+				rb)->erase_count)
+#define MTDSWAP_ECNT_MAX(rbroot) (rb_entry(rb_last(rbroot), struct swap_eb, \
+				rb)->erase_count)
+
+struct mtdswap_tree {
+	struct rb_root root;
+	unsigned int count;
+};
+
+enum {
+	MTDSWAP_CLEAN,
+	MTDSWAP_USED,
+	MTDSWAP_LOWFRAG,
+	MTDSWAP_HIFRAG,
+	MTDSWAP_DIRTY,
+	MTDSWAP_BITFLIP,
+	MTDSWAP_FAILING,
+	MTDSWAP_TREE_CNT,
+};
+
+struct mtdswap_dev {
+	struct mtd_blktrans_dev *mbd_dev;
+	struct mtd_info *mtd;
+	struct device *dev;
+
+	unsigned int *page_data;
+	unsigned int *revmap;
+
+	unsigned int eblks;
+	unsigned int spare_eblks;
+	unsigned int pages_per_eblk;
+	unsigned int max_erase_count;
+	struct swap_eb *eb_data;
+
+	struct mtdswap_tree trees[MTDSWAP_TREE_CNT];
+
+	unsigned long long sect_read_count;
+	unsigned long long sect_write_count;
+	unsigned long long mtd_write_count;
+	unsigned long long mtd_read_count;
+	unsigned long long discard_count;
+	unsigned long long discard_page_count;
+
+	unsigned int curr_write_pos;
+	struct swap_eb *curr_write;
+
+	char *page_buf;
+	char *oob_buf;
+
+	struct dentry *debugfs_root;
+};
+
+struct mtdswap_oobdata {
+	__le16 magic;
+	__le32 count;
+} __attribute__((packed));
+
+#define MTDSWAP_MAGIC_CLEAN	0x2095
+#define MTDSWAP_MAGIC_DIRTY	(MTDSWAP_MAGIC_CLEAN + 1)
+#define MTDSWAP_TYPE_CLEAN	0
+#define MTDSWAP_TYPE_DIRTY	1
+#define MTDSWAP_OOBSIZE		sizeof(struct mtdswap_oobdata)
+
+#define MTDSWAP_ERASE_RETRIES	3 /* Before marking erase block bad */
+#define MTDSWAP_IO_RETRIES	3
+
+enum {
+	MTDSWAP_SCANNED_CLEAN,
+	MTDSWAP_SCANNED_DIRTY,
+	MTDSWAP_SCANNED_BITFLIP,
+	MTDSWAP_SCANNED_BAD,
+};
+
+/*
+ * In the worst case mtdswap_writesect() has allocated the last clean
+ * page from the current block and is then pre-empted by the GC
+ * thread. The thread can consume a full erase block when moving a
+ * block.
+ */
+#define MIN_SPARE_EBLOCKS	2
+#define MIN_ERASE_BLOCKS	(MIN_SPARE_EBLOCKS + 1)
+
+#define TREE_ROOT(d, name) (&d->trees[MTDSWAP_ ## name].root)
+#define TREE_EMPTY(d, name) (TREE_ROOT(d, name)->rb_node == NULL)
+#define TREE_NONEMPTY(d, name) (!TREE_EMPTY(d, name))
+#define TREE_COUNT(d, name) (d->trees[MTDSWAP_ ## name].count)
+
+#define MTDSWAP_MBD_TO_MTDSWAP(dev) ((struct mtdswap_dev *)dev->priv)
+
+static char partitions[128] = "";
+module_param_string(partitions, partitions, sizeof(partitions), 0444);
+MODULE_PARM_DESC(partitions, "MTD partition numbers to use as swap "
+		"partitions=\"1,3,5\"");
+
+static unsigned int spare_eblocks = 10;
+module_param(spare_eblocks, uint, 0444);
+MODULE_PARM_DESC(spare_eblocks, "Percentage of spare erase blocks for "
+		"garbage collection (default 10%)");
+
+static bool header; /* false */
+module_param(header, bool, 0444);
+MODULE_PARM_DESC(header,
+		"Include builtin swap header (default 0, without header)");
+
+static int mtdswap_gc(struct mtdswap_dev *d, unsigned int background);
+
+static loff_t mtdswap_eb_offset(struct mtdswap_dev *d, struct swap_eb *eb)
+{
+	return (loff_t)(eb - d->eb_data) * d->mtd->erasesize;
+}
+
+static void mtdswap_eb_detach(struct mtdswap_dev *d, struct swap_eb *eb)
+{
+	unsigned int oldidx;
+	struct mtdswap_tree *tp;
+
+	if (eb->root) {
+		tp = container_of(eb->root, struct mtdswap_tree, root);
+		oldidx = tp - &d->trees[0];
+
+		d->trees[oldidx].count--;
+		rb_erase(&eb->rb, eb->root);
+	}
+}
+
+static void __mtdswap_rb_add(struct rb_root *root, struct swap_eb *eb)
+{
+	struct rb_node **p, *parent = NULL;
+	struct swap_eb *cur;
+
+	p = &root->rb_node;
+	while (*p) {
+		parent = *p;
+		cur = rb_entry(parent, struct swap_eb, rb);
+		if (eb->erase_count > cur->erase_count)
+			p = &(*p)->rb_right;
+		else
+			p = &(*p)->rb_left;
+	}
+
+	rb_link_node(&eb->rb, parent, p);
+	rb_insert_color(&eb->rb, root);
+}
+
+static void mtdswap_rb_add(struct mtdswap_dev *d, struct swap_eb *eb, int idx)
+{
+	struct rb_root *root;
+
+	if (eb->root == &d->trees[idx].root)
+		return;
+
+	mtdswap_eb_detach(d, eb);
+	root = &d->trees[idx].root;
+	__mtdswap_rb_add(root, eb);
+	eb->root = root;
+	d->trees[idx].count++;
+}
+
+static struct rb_node *mtdswap_rb_index(struct rb_root *root, unsigned int idx)
+{
+	struct rb_node *p;
+	unsigned int i;
+
+	p = rb_first(root);
+	i = 0;
+	while (i < idx && p) {
+		p = rb_next(p);
+		i++;
+	}
+
+	return p;
+}
+
+static int mtdswap_handle_badblock(struct mtdswap_dev *d, struct swap_eb *eb)
+{
+	int ret;
+	loff_t offset;
+
+	d->spare_eblks--;
+	eb->flags |= EBLOCK_BAD;
+	mtdswap_eb_detach(d, eb);
+	eb->root = NULL;
+
+	/* badblocks not supported */
+	if (!d->mtd->block_markbad)
+		return 1;
+
+	offset = mtdswap_eb_offset(d, eb);
+	dev_warn(d->dev, "Marking bad block at %08llx\n", offset);
+	ret = d->mtd->block_markbad(d->mtd, offset);
+
+	if (ret) {
+		dev_warn(d->dev, "Mark block bad failed for block at %08llx "
+			"error %d\n", offset, ret);
+		return ret;
+	}
+
+	return 1;
+
+}
+
+static int mtdswap_handle_write_error(struct mtdswap_dev *d, struct swap_eb *eb)
+{
+	unsigned int marked = eb->flags & EBLOCK_FAILED;
+	struct swap_eb *curr_write = d->curr_write;
+
+	eb->flags |= EBLOCK_FAILED;
+	if (curr_write == eb) {
+		d->curr_write = NULL;
+
+		if (!marked && d->curr_write_pos != 0) {
+			mtdswap_rb_add(d, eb, MTDSWAP_FAILING);
+			return 0;
+		}
+	}
+
+	return mtdswap_handle_badblock(d, eb);
+}
+
+static int mtdswap_read_oob(struct mtdswap_dev *d, loff_t from,
+			struct mtd_oob_ops *ops)
+{
+	int ret = d->mtd->read_oob(d->mtd, from, ops);
+
+	if (ret == -EUCLEAN)
+		return ret;
+
+	if (ret) {
+		dev_warn(d->dev, "Read OOB failed %d for block at %08llx\n",
+			ret, from);
+		return ret;
+	}
+
+	if (ops->oobretlen < ops->ooblen) {
+		dev_warn(d->dev, "Read OOB return short read (%zd bytes not "
+			"%zd) for block at %08llx\n",
+			ops->oobretlen, ops->ooblen, from);
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static int mtdswap_read_markers(struct mtdswap_dev *d, struct swap_eb *eb)
+{
+	struct mtdswap_oobdata *data, *data2;
+	int ret;
+	loff_t offset;
+	struct mtd_oob_ops ops;
+
+	offset = mtdswap_eb_offset(d, eb);
+
+	/* Check first if the block is bad. */
+	if (d->mtd->block_isbad && d->mtd->block_isbad(d->mtd, offset))
+		return MTDSWAP_SCANNED_BAD;
+
+	ops.ooblen = 2 * d->mtd->ecclayout->oobavail;
+	ops.oobbuf = d->oob_buf;
+	ops.ooboffs = 0;
+	ops.datbuf = NULL;
+	ops.mode = MTD_OOB_AUTO;
+
+	ret = mtdswap_read_oob(d, offset, &ops);
+
+	if (ret && ret != -EUCLEAN)
+		return ret;
+
+	data = (struct mtdswap_oobdata *)d->oob_buf;
+	data2 = (struct mtdswap_oobdata *)
+		(d->oob_buf + d->mtd->ecclayout->oobavail);
+
+	if (le16_to_cpu(data->magic) == MTDSWAP_MAGIC_CLEAN) {
+		eb->erase_count = le32_to_cpu(data->count);
+		if (ret == -EUCLEAN)
+			ret = MTDSWAP_SCANNED_BITFLIP;
+		else {
+			if (le16_to_cpu(data2->magic) == MTDSWAP_MAGIC_DIRTY)
+				ret = MTDSWAP_SCANNED_DIRTY;
+			else
+				ret = MTDSWAP_SCANNED_CLEAN;
+		}
+	} else {
+		eb->flags |= EBLOCK_NOMAGIC;
+		ret = MTDSWAP_SCANNED_DIRTY;
+	}
+
+	return ret;
+}
+
+static int mtdswap_write_marker(struct mtdswap_dev *d, struct swap_eb *eb,
+				u16 marker)
+{
+	struct mtdswap_oobdata n;
+	int ret;
+	loff_t offset;
+	struct mtd_oob_ops ops;
+
+	ops.ooboffs = 0;
+	ops.oobbuf = (uint8_t *)&n;
+	ops.mode = MTD_OOB_AUTO;
+	ops.datbuf = NULL;
+
+	if (marker == MTDSWAP_TYPE_CLEAN) {
+		n.magic = cpu_to_le16(MTDSWAP_MAGIC_CLEAN);
+		n.count = cpu_to_le32(eb->erase_count);
+		ops.ooblen = MTDSWAP_OOBSIZE;
+		offset = mtdswap_eb_offset(d, eb);
+	} else {
+		n.magic = cpu_to_le16(MTDSWAP_MAGIC_DIRTY);
+		ops.ooblen = sizeof(n.magic);
+		offset = mtdswap_eb_offset(d, eb) + d->mtd->writesize;
+	}
+
+	ret = d->mtd->write_oob(d->mtd, offset , &ops);
+
+	if (ret) {
+		dev_warn(d->dev, "Write OOB failed for block at %08llx "
+			"error %d\n", offset, ret);
+		if (ret == -EIO || ret == -EBADMSG)
+			mtdswap_handle_write_error(d, eb);
+		return ret;
+	}
+
+	if (ops.oobretlen != ops.ooblen) {
+		dev_warn(d->dev, "Short OOB write for block at %08llx: "
+			"%zd not %zd\n",
+			offset, ops.oobretlen, ops.ooblen);
+		return ret;
+	}
+
+	return 0;
+}
+
+/*
+ * Are there any erase blocks without MAGIC_CLEAN header, presumably
+ * because power was cut off after erase but before header write? We
+ * need to guestimate the erase count.
+ */
+static void mtdswap_check_counts(struct mtdswap_dev *d)
+{
+	struct rb_root hist_root = RB_ROOT;
+	struct rb_node *medrb;
+	struct swap_eb *eb;
+	unsigned int i, cnt, median;
+
+	cnt = 0;
+	for (i = 0; i < d->eblks; i++) {
+		eb = d->eb_data + i;
+
+		if (eb->flags & (EBLOCK_NOMAGIC | EBLOCK_BAD | EBLOCK_READERR))
+			continue;
+
+		__mtdswap_rb_add(&hist_root, eb);
+		cnt++;
+	}
+
+	if (cnt == 0)
+		return;
+
+	medrb = mtdswap_rb_index(&hist_root, cnt / 2);
+	median = rb_entry(medrb, struct swap_eb, rb)->erase_count;
+
+	d->max_erase_count = MTDSWAP_ECNT_MAX(&hist_root);
+
+	for (i = 0; i < d->eblks; i++) {
+		eb = d->eb_data + i;
+
+		if (eb->flags & (EBLOCK_NOMAGIC | EBLOCK_READERR))
+			eb->erase_count = median;
+
+		if (eb->flags & (EBLOCK_NOMAGIC | EBLOCK_BAD | EBLOCK_READERR))
+			continue;
+
+		rb_erase(&eb->rb, &hist_root);
+	}
+}
+
+static void mtdswap_scan_eblks(struct mtdswap_dev *d)
+{
+	int status;
+	unsigned int i, idx;
+	struct swap_eb *eb;
+
+	for (i = 0; i < d->eblks; i++) {
+		eb = d->eb_data + i;
+
+		status = mtdswap_read_markers(d, eb);
+		if (status < 0)
+			eb->flags |= EBLOCK_READERR;
+		else if (status == MTDSWAP_SCANNED_BAD) {
+			eb->flags |= EBLOCK_BAD;
+			continue;
+		}
+
+		switch (status) {
+		case MTDSWAP_SCANNED_CLEAN:
+			idx = MTDSWAP_CLEAN;
+			break;
+		case MTDSWAP_SCANNED_DIRTY:
+		case MTDSWAP_SCANNED_BITFLIP:
+			idx = MTDSWAP_DIRTY;
+			break;
+		default:
+			idx = MTDSWAP_FAILING;
+		}
+
+		eb->flags |= (idx << EBLOCK_IDX_SHIFT);
+	}
+
+	mtdswap_check_counts(d);
+
+	for (i = 0; i < d->eblks; i++) {
+		eb = d->eb_data + i;
+
+		if (eb->flags & EBLOCK_BAD)
+			continue;
+
+		idx = eb->flags >> EBLOCK_IDX_SHIFT;
+		mtdswap_rb_add(d, eb, idx);
+	}
+}
+
+/*
+ * Place eblk into a tree corresponding to its number of active blocks
+ * it contains.
+ */
+static void mtdswap_store_eb(struct mtdswap_dev *d, struct swap_eb *eb)
+{
+	unsigned int weight = eb->active_count;
+	unsigned int maxweight = d->pages_per_eblk;
+
+	if (eb == d->curr_write)
+		return;
+
+	if (eb->flags & EBLOCK_BITFLIP)
+		mtdswap_rb_add(d, eb, MTDSWAP_BITFLIP);
+	else if (eb->flags & (EBLOCK_READERR | EBLOCK_FAILED))
+		mtdswap_rb_add(d, eb, MTDSWAP_FAILING);
+	if (weight == maxweight)
+		mtdswap_rb_add(d, eb, MTDSWAP_USED);
+	else if (weight == 0)
+		mtdswap_rb_add(d, eb, MTDSWAP_DIRTY);
+	else if (weight > (maxweight/2))
+		mtdswap_rb_add(d, eb, MTDSWAP_LOWFRAG);
+	else
+		mtdswap_rb_add(d, eb, MTDSWAP_HIFRAG);
+}
+
+
+static void mtdswap_erase_callback(struct erase_info *done)
+{
+	wait_queue_head_t *wait_q = (wait_queue_head_t *)done->priv;
+	wake_up(wait_q);
+}
+
+static int mtdswap_erase_block(struct mtdswap_dev *d, struct swap_eb *eb)
+{
+	struct mtd_info *mtd = d->mtd;
+	struct erase_info erase;
+	wait_queue_head_t wq;
+	unsigned int retries = 0;
+	int ret;
+
+	eb->erase_count++;
+	if (eb->erase_count > d->max_erase_count)
+		d->max_erase_count = eb->erase_count;
+
+retry:
+	init_waitqueue_head(&wq);
+	memset(&erase, 0, sizeof(struct erase_info));
+
+	erase.mtd	= mtd;
+	erase.callback	= mtdswap_erase_callback;
+	erase.addr	= mtdswap_eb_offset(d, eb);
+	erase.len	= mtd->erasesize;
+	erase.priv	= (u_long)&wq;
+
+	ret = mtd->erase(mtd, &erase);
+	if (ret) {
+		if (retries++ < MTDSWAP_ERASE_RETRIES) {
+			dev_warn(d->dev,
+				"erase of erase block %#llx on %s failed",
+				erase.addr, mtd->name);
+			yield();
+			goto retry;
+		}
+
+		dev_err(d->dev, "Cannot erase erase block %#llx on %s\n",
+			erase.addr, mtd->name);
+
+		mtdswap_handle_badblock(d, eb);
+		return -EIO;
+	}
+
+	ret = wait_event_interruptible(wq, erase.state == MTD_ERASE_DONE ||
+					   erase.state == MTD_ERASE_FAILED);
+	if (ret) {
+		dev_err(d->dev, "Interrupted erase block %#llx erassure on %s",
+			erase.addr, mtd->name);
+		return -EINTR;
+	}
+
+	if (erase.state == MTD_ERASE_FAILED) {
+		if (retries++ < MTDSWAP_ERASE_RETRIES) {
+			dev_warn(d->dev,
+				"erase of erase block %#llx on %s failed",
+				erase.addr, mtd->name);
+			yield();
+			goto retry;
+		}
+
+		mtdswap_handle_badblock(d, eb);
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static int mtdswap_map_free_block(struct mtdswap_dev *d, unsigned int page,
+				unsigned int *block)
+{
+	int ret;
+	struct swap_eb *old_eb = d->curr_write;
+	struct rb_root *clean_root;
+	struct swap_eb *eb;
+
+	if (old_eb == NULL || d->curr_write_pos >= d->pages_per_eblk) {
+		do {
+			if (TREE_EMPTY(d, CLEAN))
+				return -ENOSPC;
+
+			clean_root = TREE_ROOT(d, CLEAN);
+			eb = rb_entry(rb_first(clean_root), struct swap_eb, rb);
+			rb_erase(&eb->rb, clean_root);
+			eb->root = NULL;
+			TREE_COUNT(d, CLEAN)--;
+
+			ret = mtdswap_write_marker(d, eb, MTDSWAP_TYPE_DIRTY);
+		} while (ret == -EIO || ret == -EBADMSG);
+
+		if (ret)
+			return ret;
+
+		d->curr_write_pos = 0;
+		d->curr_write = eb;
+		if (old_eb)
+			mtdswap_store_eb(d, old_eb);
+	}
+
+	*block = (d->curr_write - d->eb_data) * d->pages_per_eblk +
+		d->curr_write_pos;
+
+	d->curr_write->active_count++;
+	d->revmap[*block] = page;
+	d->curr_write_pos++;
+
+	return 0;
+}
+
+static unsigned int mtdswap_free_page_cnt(struct mtdswap_dev *d)
+{
+	return TREE_COUNT(d, CLEAN) * d->pages_per_eblk +
+		d->pages_per_eblk - d->curr_write_pos;
+}
+
+static unsigned int mtdswap_enough_free_pages(struct mtdswap_dev *d)
+{
+	return mtdswap_free_page_cnt(d) > d->pages_per_eblk;
+}
+
+static int mtdswap_write_block(struct mtdswap_dev *d, char *buf,
+			unsigned int page, unsigned int *bp, int gc_context)
+{
+	struct mtd_info *mtd = d->mtd;
+	struct swap_eb *eb;
+	size_t retlen;
+	loff_t writepos;
+	int ret;
+
+retry:
+	if (!gc_context)
+		while (!mtdswap_enough_free_pages(d))
+			if (mtdswap_gc(d, 0) > 0)
+				return -ENOSPC;
+
+	ret = mtdswap_map_free_block(d, page, bp);
+	eb = d->eb_data + (*bp / d->pages_per_eblk);
+
+	if (ret == -EIO || ret == -EBADMSG) {
+		d->curr_write = NULL;
+		eb->active_count--;
+		d->revmap[*bp] = PAGE_UNDEF;
+		goto retry;
+	}
+
+	if (ret < 0)
+		return ret;
+
+	writepos = (loff_t)*bp << PAGE_SHIFT;
+	ret =  mtd->write(mtd, writepos, PAGE_SIZE, &retlen, buf);
+	if (ret == -EIO || ret == -EBADMSG) {
+		d->curr_write_pos--;
+		eb->active_count--;
+		d->revmap[*bp] = PAGE_UNDEF;
+		mtdswap_handle_write_error(d, eb);
+		goto retry;
+	}
+
+	if (ret < 0) {
+		dev_err(d->dev, "Write to MTD device failed: %d (%zd written)",
+			ret, retlen);
+		goto err;
+	}
+
+	if (retlen != PAGE_SIZE) {
+		dev_err(d->dev, "Short write to MTD device: %zd written",
+			retlen);
+		ret = -EIO;
+		goto err;
+	}
+
+	return ret;
+
+err:
+	d->curr_write_pos--;
+	eb->active_count--;
+	d->revmap[*bp] = PAGE_UNDEF;
+
+	return ret;
+}
+
+static int mtdswap_move_block(struct mtdswap_dev *d, unsigned int oldblock,
+		unsigned int *newblock)
+{
+	struct mtd_info *mtd = d->mtd;
+	struct swap_eb *eb, *oldeb;
+	int ret;
+	size_t retlen;
+	unsigned int page, retries;
+	loff_t readpos;
+
+	page = d->revmap[oldblock];
+	readpos = (loff_t) oldblock << PAGE_SHIFT;
+	retries = 0;
+
+retry:
+	ret = mtd->read(mtd, readpos, PAGE_SIZE, &retlen, d->page_buf);
+
+	if (ret < 0 && ret != -EUCLEAN) {
+		oldeb = d->eb_data + oldblock / d->pages_per_eblk;
+		oldeb->flags |= EBLOCK_READERR;
+
+		dev_err(d->dev, "Read Error: %d (block %u)\n", ret,
+			oldblock);
+		retries++;
+		if (retries < MTDSWAP_IO_RETRIES)
+			goto retry;
+
+		goto read_error;
+	}
+
+	if (retlen != PAGE_SIZE) {
+		dev_err(d->dev, "Short read: %zd (block %u)\n", retlen,
+		       oldblock);
+		ret = -EIO;
+		goto read_error;
+	}
+
+	ret = mtdswap_write_block(d, d->page_buf, page, newblock, 1);
+	if (ret < 0) {
+		d->page_data[page] = BLOCK_ERROR;
+		dev_err(d->dev, "Write error: %d\n", ret);
+		return ret;
+	}
+
+	eb = d->eb_data + *newblock / d->pages_per_eblk;
+	d->page_data[page] = *newblock;
+	d->revmap[oldblock] = PAGE_UNDEF;
+	eb = d->eb_data + oldblock / d->pages_per_eblk;
+	eb->active_count--;
+
+	return 0;
+
+read_error:
+	d->page_data[page] = BLOCK_ERROR;
+	d->revmap[oldblock] = PAGE_UNDEF;
+	return ret;
+}
+
+static int mtdswap_gc_eblock(struct mtdswap_dev *d, struct swap_eb *eb)
+{
+	unsigned int i, block, eblk_base, newblock;
+	int ret, errcode;
+
+	errcode = 0;
+	eblk_base = (eb - d->eb_data) * d->pages_per_eblk;
+
+	for (i = 0; i < d->pages_per_eblk; i++) {
+		if (d->spare_eblks < MIN_SPARE_EBLOCKS)
+			return -ENOSPC;
+
+		block = eblk_base + i;
+		if (d->revmap[block] == PAGE_UNDEF)
+			continue;
+
+		ret = mtdswap_move_block(d, block, &newblock);
+		if (ret < 0 && !errcode)
+			errcode = ret;
+	}
+
+	return errcode;
+}
+
+static int __mtdswap_choose_gc_tree(struct mtdswap_dev *d)
+{
+	int idx, stopat;
+
+	if (TREE_COUNT(d, CLEAN) < LOW_FRAG_GC_TRESHOLD)
+		stopat = MTDSWAP_LOWFRAG;
+	else
+		stopat = MTDSWAP_HIFRAG;
+
+	for (idx = MTDSWAP_BITFLIP; idx >= stopat; idx--)
+		if (d->trees[idx].root.rb_node != NULL)
+			return idx;
+
+	return -1;
+}
+
+static int mtdswap_wlfreq(unsigned int maxdiff)
+{
+	unsigned int h, x, y, dist, base;
+
+	/*
+	 * Calculate linear ramp down from f1 to f2 when maxdiff goes from
+	 * MAX_ERASE_DIFF to MAX_ERASE_DIFF + COLLECT_NONDIRTY_BASE.  Similar
+	 * to triangle with height f1 - f1 and width COLLECT_NONDIRTY_BASE.
+	 */
+
+	dist = maxdiff - MAX_ERASE_DIFF;
+	if (dist > COLLECT_NONDIRTY_BASE)
+		dist = COLLECT_NONDIRTY_BASE;
+
+	/*
+	 * Modelling the slop as right angular triangle with base
+	 * COLLECT_NONDIRTY_BASE and height freq1 - freq2. The ratio y/x is
+	 * equal to the ratio h/base.
+	 */
+	h = COLLECT_NONDIRTY_FREQ1 - COLLECT_NONDIRTY_FREQ2;
+	base = COLLECT_NONDIRTY_BASE;
+
+	x = dist - base;
+	y = (x * h + base / 2) / base;
+
+	return COLLECT_NONDIRTY_FREQ2 + y;
+}
+
+static int mtdswap_choose_wl_tree(struct mtdswap_dev *d)
+{
+	static unsigned int pick_cnt;
+	unsigned int i, idx = -1, wear, max;
+	struct rb_root *root;
+
+	max = 0;
+	for (i = 0; i <= MTDSWAP_DIRTY; i++) {
+		root = &d->trees[i].root;
+		if (root->rb_node == NULL)
+			continue;
+
+		wear = d->max_erase_count - MTDSWAP_ECNT_MIN(root);
+		if (wear > max) {
+			max = wear;
+			idx = i;
+		}
+	}
+
+	if (max > MAX_ERASE_DIFF && pick_cnt >= mtdswap_wlfreq(max) - 1) {
+		pick_cnt = 0;
+		return idx;
+	}
+
+	pick_cnt++;
+	return -1;
+}
+
+static int mtdswap_choose_gc_tree(struct mtdswap_dev *d,
+				unsigned int background)
+{
+	int idx;
+
+	if (TREE_NONEMPTY(d, FAILING) &&
+		(background || (TREE_EMPTY(d, CLEAN) && TREE_EMPTY(d, DIRTY))))
+		return MTDSWAP_FAILING;
+
+	idx = mtdswap_choose_wl_tree(d);
+	if (idx >= MTDSWAP_CLEAN)
+		return idx;
+
+	return __mtdswap_choose_gc_tree(d);
+}
+
+static struct swap_eb *mtdswap_pick_gc_eblk(struct mtdswap_dev *d,
+					unsigned int background)
+{
+	struct rb_root *rp = NULL;
+	struct swap_eb *eb = NULL;
+	int idx;
+
+	if (background && TREE_COUNT(d, CLEAN) > CLEAN_BLOCK_THRESHOLD &&
+		TREE_EMPTY(d, DIRTY) && TREE_EMPTY(d, FAILING))
+		return NULL;
+
+	idx = mtdswap_choose_gc_tree(d, background);
+	if (idx < 0)
+		return NULL;
+
+	rp = &d->trees[idx].root;
+	eb = rb_entry(rb_first(rp), struct swap_eb, rb);
+
+	rb_erase(&eb->rb, rp);
+	eb->root = NULL;
+	d->trees[idx].count--;
+	return eb;
+}
+
+static unsigned int mtdswap_test_patt(unsigned int i)
+{
+	return i % 2 ? 0x55555555 : 0xAAAAAAAA;
+}
+
+static unsigned int mtdswap_eblk_passes(struct mtdswap_dev *d,
+					struct swap_eb *eb)
+{
+	struct mtd_info *mtd = d->mtd;
+	unsigned int test, i, j, patt, mtd_pages;
+	loff_t base, pos;
+	unsigned int *p1 = (unsigned int *)d->page_buf;
+	unsigned char *p2 = (unsigned char *)d->oob_buf;
+	struct mtd_oob_ops ops;
+	int ret;
+
+	ops.mode = MTD_OOB_AUTO;
+	ops.len = mtd->writesize;
+	ops.ooblen = mtd->ecclayout->oobavail;
+	ops.ooboffs = 0;
+	ops.datbuf = d->page_buf;
+	ops.oobbuf = d->oob_buf;
+	base = mtdswap_eb_offset(d, eb);
+	mtd_pages = d->pages_per_eblk * PAGE_SIZE / mtd->writesize;
+
+	for (test = 0; test < 2; test++) {
+		pos = base;
+		for (i = 0; i < mtd_pages; i++) {
+			patt = mtdswap_test_patt(test + i);
+			memset(d->page_buf, patt, mtd->writesize);
+			memset(d->oob_buf, patt, mtd->ecclayout->oobavail);
+			ret = mtd->write_oob(mtd, pos, &ops);
+			if (ret)
+				goto error;
+
+			pos += mtd->writesize;
+		}
+
+		pos = base;
+		for (i = 0; i < mtd_pages; i++) {
+			ret = mtd->read_oob(mtd, pos, &ops);
+			if (ret)
+				goto error;
+
+			patt = mtdswap_test_patt(test + i);
+			for (j = 0; j < mtd->writesize/sizeof(int); j++)
+				if (p1[j] != patt)
+					goto error;
+
+			for (j = 0; j < mtd->ecclayout->oobavail; j++)
+				if (p2[j] != (unsigned char)patt)
+					goto error;
+
+			pos += mtd->writesize;
+		}
+
+		ret = mtdswap_erase_block(d, eb);
+		if (ret)
+			goto error;
+	}
+
+	eb->flags &= ~EBLOCK_READERR;
+	return 1;
+
+error:
+	mtdswap_handle_badblock(d, eb);
+	return 0;
+}
+
+static int mtdswap_gc(struct mtdswap_dev *d, unsigned int background)
+{
+	struct swap_eb *eb;
+	int ret;
+
+	if (d->spare_eblks < MIN_SPARE_EBLOCKS)
+		return 1;
+
+	eb = mtdswap_pick_gc_eblk(d, background);
+	if (!eb)
+		return 1;
+
+	ret = mtdswap_gc_eblock(d, eb);
+	if (ret == -ENOSPC)
+		return 1;
+
+	if (eb->flags & EBLOCK_FAILED) {
+		mtdswap_handle_badblock(d, eb);
+		return 0;
+	}
+
+	eb->flags &= ~EBLOCK_BITFLIP;
+	ret = mtdswap_erase_block(d, eb);
+	if ((eb->flags & EBLOCK_READERR) &&
+		(ret || !mtdswap_eblk_passes(d, eb)))
+		return 0;
+
+	if (ret == 0)
+		ret = mtdswap_write_marker(d, eb, MTDSWAP_TYPE_CLEAN);
+
+	if (ret == 0)
+		mtdswap_rb_add(d, eb, MTDSWAP_CLEAN);
+	else if (ret != -EIO && ret != -EBADMSG)
+		mtdswap_rb_add(d, eb, MTDSWAP_DIRTY);
+
+	return 0;
+}
+
+static void mtdswap_background(struct mtd_blktrans_dev *dev)
+{
+	struct mtdswap_dev *d = MTDSWAP_MBD_TO_MTDSWAP(dev);
+	int ret;
+
+	while (1) {
+		ret = mtdswap_gc(d, 1);
+		if (ret || mtd_blktrans_cease_background(dev))
+			return;
+	}
+}
+
+static void mtdswap_cleanup(struct mtdswap_dev *d)
+{
+	vfree(d->eb_data);
+	vfree(d->revmap);
+	vfree(d->page_data);
+	kfree(d->oob_buf);
+	kfree(d->page_buf);
+}
+
+static int mtdswap_flush(struct mtd_blktrans_dev *dev)
+{
+	struct mtdswap_dev *d = MTDSWAP_MBD_TO_MTDSWAP(dev);
+
+	if (d->mtd->sync)
+		d->mtd->sync(d->mtd);
+	return 0;
+}
+
+static unsigned int mtdswap_badblocks(struct mtd_info *mtd, uint64_t size)
+{
+	loff_t offset;
+	unsigned int badcnt;
+
+	badcnt = 0;
+
+	if (mtd->block_isbad)
+		for (offset = 0; offset < size; offset += mtd->erasesize)
+			if (mtd->block_isbad(mtd, offset))
+				badcnt++;
+
+	return badcnt;
+}
+
+static int mtdswap_writesect(struct mtd_blktrans_dev *dev,
+			unsigned long page, char *buf)
+{
+	struct mtdswap_dev *d = MTDSWAP_MBD_TO_MTDSWAP(dev);
+	unsigned int newblock, mapped;
+	struct swap_eb *eb;
+	int ret;
+
+	d->sect_write_count++;
+
+	if (d->spare_eblks < MIN_SPARE_EBLOCKS)
+		return -ENOSPC;
+
+	if (header) {
+		/* Ignore writes to the header page */
+		if (unlikely(page == 0))
+			return 0;
+
+		page--;
+	}
+
+	mapped = d->page_data[page];
+	if (mapped <= BLOCK_MAX) {
+		eb = d->eb_data + (mapped / d->pages_per_eblk);
+		eb->active_count--;
+		mtdswap_store_eb(d, eb);
+		d->page_data[page] = BLOCK_UNDEF;
+		d->revmap[mapped] = PAGE_UNDEF;
+	}
+
+	ret = mtdswap_write_block(d, buf, page, &newblock, 0);
+	d->mtd_write_count++;
+
+	if (ret < 0)
+		return ret;
+
+	eb = d->eb_data + (newblock / d->pages_per_eblk);
+	d->page_data[page] = newblock;
+
+	return 0;
+}
+
+/* Provide a dummy swap header for the kernel */
+static int mtdswap_auto_header(struct mtdswap_dev *d, char *buf)
+{
+	union swap_header *hd = (union swap_header *)(buf);
+
+	memset(buf, 0, PAGE_SIZE - 10);
+
+	hd->info.version = 1;
+	hd->info.last_page = d->mbd_dev->size - 1;
+	hd->info.nr_badpages = 0;
+
+	memcpy(buf + PAGE_SIZE - 10, "SWAPSPACE2", 10);
+
+	return 0;
+}
+
+static int mtdswap_readsect(struct mtd_blktrans_dev *dev,
+			unsigned long page, char *buf)
+{
+	struct mtdswap_dev *d = MTDSWAP_MBD_TO_MTDSWAP(dev);
+	struct mtd_info *mtd = d->mtd;
+	unsigned int realblock, retries;
+	loff_t readpos;
+	struct swap_eb *eb;
+	size_t retlen;
+	int ret;
+
+	d->sect_read_count++;
+
+	if (header) {
+		if (unlikely(page == 0))
+			return mtdswap_auto_header(d, buf);
+
+		page--;
+	}
+
+	realblock = d->page_data[page];
+	if (realblock > BLOCK_MAX) {
+		memset(buf, 0x0, PAGE_SIZE);
+		if (realblock == BLOCK_UNDEF)
+			return 0;
+		else
+			return -EIO;
+	}
+
+	eb = d->eb_data + (realblock / d->pages_per_eblk);
+	BUG_ON(d->revmap[realblock] == PAGE_UNDEF);
+
+	readpos = (loff_t)realblock << PAGE_SHIFT;
+	retries = 0;
+
+retry:
+	ret = mtd->read(mtd, readpos, PAGE_SIZE, &retlen, buf);
+
+	d->mtd_read_count++;
+	if (ret == -EUCLEAN) {
+		eb->flags |= EBLOCK_BITFLIP;
+		mtdswap_rb_add(d, eb, MTDSWAP_BITFLIP);
+		ret = 0;
+	}
+
+	if (ret < 0) {
+		dev_err(d->dev, "Read error %d\n", ret);
+		eb->flags |= EBLOCK_READERR;
+		mtdswap_rb_add(d, eb, MTDSWAP_FAILING);
+		retries++;
+		if (retries < MTDSWAP_IO_RETRIES)
+			goto retry;
+
+		return ret;
+	}
+
+	if (retlen != PAGE_SIZE) {
+		dev_err(d->dev, "Short read %zd\n", retlen);
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static int mtdswap_discard(struct mtd_blktrans_dev *dev, unsigned long first,
+			unsigned nr_pages)
+{
+	struct mtdswap_dev *d = MTDSWAP_MBD_TO_MTDSWAP(dev);
+	unsigned long page;
+	struct swap_eb *eb;
+	unsigned int mapped;
+
+	d->discard_count++;
+
+	for (page = first; page < first + nr_pages; page++) {
+		mapped = d->page_data[page];
+		if (mapped <= BLOCK_MAX) {
+			eb = d->eb_data + (mapped / d->pages_per_eblk);
+			eb->active_count--;
+			mtdswap_store_eb(d, eb);
+			d->page_data[page] = BLOCK_UNDEF;
+			d->revmap[mapped] = PAGE_UNDEF;
+			d->discard_page_count++;
+		} else if (mapped == BLOCK_ERROR) {
+			d->page_data[page] = BLOCK_UNDEF;
+			d->discard_page_count++;
+		}
+	}
+
+	return 0;
+}
+
+static int mtdswap_show(struct seq_file *s, void *data)
+{
+	struct mtdswap_dev *d = (struct mtdswap_dev *) s->private;
+	unsigned long sum;
+	unsigned int count[MTDSWAP_TREE_CNT];
+	unsigned int min[MTDSWAP_TREE_CNT];
+	unsigned int max[MTDSWAP_TREE_CNT];
+	unsigned int i, cw = 0, cwp = 0, cwecount = 0, bb_cnt, mapped, pages;
+	uint64_t use_size;
+	char *name[] = {"clean", "used", "low", "high", "dirty", "bitflip",
+			"failing"};
+
+	mutex_lock(&d->mbd_dev->lock);
+
+	for (i = 0; i < MTDSWAP_TREE_CNT; i++) {
+		struct rb_root *root = &d->trees[i].root;
+
+		if (root->rb_node) {
+			count[i] = d->trees[i].count;
+			min[i] = rb_entry(rb_first(root), struct swap_eb,
+					rb)->erase_count;
+			max[i] = rb_entry(rb_last(root), struct swap_eb,
+					rb)->erase_count;
+		} else
+			count[i] = 0;
+	}
+
+	if (d->curr_write) {
+		cw = 1;
+		cwp = d->curr_write_pos;
+		cwecount = d->curr_write->erase_count;
+	}
+
+	sum = 0;
+	for (i = 0; i < d->eblks; i++)
+		sum += d->eb_data[i].erase_count;
+
+	use_size = (uint64_t)d->eblks * d->mtd->erasesize;
+	bb_cnt = mtdswap_badblocks(d->mtd, use_size);
+
+	mapped = 0;
+	pages = d->mbd_dev->size;
+	for (i = 0; i < pages; i++)
+		if (d->page_data[i] != BLOCK_UNDEF)
+			mapped++;
+
+	mutex_unlock(&d->mbd_dev->lock);
+
+	for (i = 0; i < MTDSWAP_TREE_CNT; i++) {
+		if (!count[i])
+			continue;
+
+		if (min[i] != max[i])
+			seq_printf(s, "%s:\t%5d erase blocks, erased min %d, "
+				"max %d times\n",
+				name[i], count[i], min[i], max[i]);
+		else
+			seq_printf(s, "%s:\t%5d erase blocks, all erased %d "
+				"times\n", name[i], count[i], min[i]);
+	}
+
+	if (bb_cnt)
+		seq_printf(s, "bad:\t%5u erase blocks\n", bb_cnt);
+
+	if (cw)
+		seq_printf(s, "current erase block: %u pages used, %u free, "
+			"erased %u times\n",
+			cwp, d->pages_per_eblk - cwp, cwecount);
+
+	seq_printf(s, "total erasures: %lu\n", sum);
+
+	seq_printf(s, "\n");
+
+	seq_printf(s, "mtdswap_readsect count: %llu\n", d->sect_read_count);
+	seq_printf(s, "mtdswap_writesect count: %llu\n", d->sect_write_count);
+	seq_printf(s, "mtdswap_discard count: %llu\n", d->discard_count);
+	seq_printf(s, "mtd read count: %llu\n", d->mtd_read_count);
+	seq_printf(s, "mtd write count: %llu\n", d->mtd_write_count);
+	seq_printf(s, "discarded pages count: %llu\n", d->discard_page_count);
+
+	seq_printf(s, "\n");
+	seq_printf(s, "total pages: %u\n", pages);
+	seq_printf(s, "pages mapped: %u\n", mapped);
+
+	return 0;
+}
+
+static int mtdswap_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, mtdswap_show, inode->i_private);
+}
+
+static const struct file_operations mtdswap_fops = {
+	.open		= mtdswap_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+static int mtdswap_add_debugfs(struct mtdswap_dev *d)
+{
+	struct gendisk *gd = d->mbd_dev->disk;
+	struct device *dev = disk_to_dev(gd);
+
+	struct dentry *root;
+	struct dentry *dent;
+
+	root = debugfs_create_dir(gd->disk_name, NULL);
+	if (IS_ERR(root))
+		return 0;
+
+	if (!root) {
+		dev_err(dev, "failed to initialize debugfs\n");
+		return -1;
+	}
+
+	d->debugfs_root = root;
+
+	dent = debugfs_create_file("stats", S_IRUSR, root, d,
+				&mtdswap_fops);
+	if (!dent) {
+		dev_err(d->dev, "debugfs_create_file failed\n");
+		debugfs_remove_recursive(root);
+		d->debugfs_root = NULL;
+		return -1;
+	}
+
+	return 0;
+}
+
+static int mtdswap_init(struct mtdswap_dev *d, unsigned int eblocks,
+			unsigned int spare_cnt)
+{
+	struct mtd_info *mtd = d->mbd_dev->mtd;
+	unsigned int i, eblk_bytes, pages, blocks;
+	int ret = -ENOMEM;
+
+	d->mtd = mtd;
+	d->eblks = eblocks;
+	d->spare_eblks = spare_cnt;
+	d->pages_per_eblk = mtd->erasesize >> PAGE_SHIFT;
+
+	pages = d->mbd_dev->size;
+	blocks = eblocks * d->pages_per_eblk;
+
+	for (i = 0; i < MTDSWAP_TREE_CNT; i++)
+		d->trees[i].root = RB_ROOT;
+
+	d->page_data = vmalloc(sizeof(int)*pages);
+	if (!d->page_data)
+		goto page_data_fail;
+
+	d->revmap = vmalloc(sizeof(int)*blocks);
+	if (!d->revmap)
+		goto revmap_fail;
+
+	eblk_bytes = sizeof(struct swap_eb)*d->eblks;
+	d->eb_data = vmalloc(eblk_bytes);
+	if (!d->eb_data)
+		goto eb_data_fail;
+
+	memset(d->eb_data, 0, eblk_bytes);
+	for (i = 0; i < pages; i++)
+		d->page_data[i] = BLOCK_UNDEF;
+
+	for (i = 0; i < blocks; i++)
+		d->revmap[i] = PAGE_UNDEF;
+
+	d->page_buf = kmalloc(PAGE_SIZE, GFP_KERNEL);
+	if (!d->page_buf)
+		goto page_buf_fail;
+
+	d->oob_buf = kmalloc(2 * mtd->ecclayout->oobavail, GFP_KERNEL);
+	if (!d->oob_buf)
+		goto oob_buf_fail;
+
+	mtdswap_scan_eblks(d);
+
+	return 0;
+
+oob_buf_fail:
+	kfree(d->page_buf);
+page_buf_fail:
+	vfree(d->eb_data);
+eb_data_fail:
+	vfree(d->revmap);
+revmap_fail:
+	vfree(d->page_data);
+page_data_fail:
+	printk(KERN_ERR "%s: init failed (%d)\n", MTDSWAP_PREFIX, ret);
+	return ret;
+}
+
+static void mtdswap_add_mtd(struct mtd_blktrans_ops *tr, struct mtd_info *mtd)
+{
+	struct mtdswap_dev *d;
+	struct mtd_blktrans_dev *mbd_dev;
+	char *parts;
+	char *this_opt;
+	unsigned long part;
+	unsigned int eblocks, eavailable, bad_blocks, spare_cnt;
+	uint64_t swap_size, use_size, size_limit;
+	struct nand_ecclayout *oinfo;
+	int ret;
+
+	parts = &partitions[0];
+	if (!*parts)
+		return;
+
+	while ((this_opt = strsep(&parts, ",")) != NULL) {
+		if (strict_strtoul(this_opt, 0, &part) < 0)
+			return;
+
+		if (mtd->index == part)
+			break;
+	}
+
+	if (mtd->index != part)
+		return;
+
+	if (mtd->erasesize < PAGE_SIZE || mtd->erasesize % PAGE_SIZE) {
+		printk(KERN_ERR "%s: Erase size %u not multiple of PAGE_SIZE "
+			"%lu\n", MTDSWAP_PREFIX, mtd->erasesize, PAGE_SIZE);
+		return;
+	}
+
+	if (PAGE_SIZE % mtd->writesize || mtd->writesize > PAGE_SIZE) {
+		printk(KERN_ERR "%s: PAGE_SIZE %lu not multiple of write size"
+			" %u\n", MTDSWAP_PREFIX, PAGE_SIZE, mtd->writesize);
+		return;
+	}
+
+	oinfo = mtd->ecclayout;
+	if (!mtd->oobsize || !oinfo || oinfo->oobavail < MTDSWAP_OOBSIZE) {
+		printk(KERN_ERR "%s: Not enough free bytes in OOB, "
+			"%d available, %lu needed.\n",
+			MTDSWAP_PREFIX, oinfo->oobavail, MTDSWAP_OOBSIZE);
+		return;
+	}
+
+	if (spare_eblocks > 100)
+		spare_eblocks = 100;
+
+	use_size = mtd->size;
+	size_limit = (uint64_t) BLOCK_MAX * PAGE_SIZE;
+
+	if (mtd->size > size_limit) {
+		printk(KERN_WARNING "%s: Device too large. Limiting size to "
+			"%llu bytes\n", MTDSWAP_PREFIX, size_limit);
+		use_size = size_limit;
+	}
+
+	eblocks = mtd_div_by_eb(use_size, mtd);
+	use_size = eblocks * mtd->erasesize;
+	bad_blocks = mtdswap_badblocks(mtd, use_size);
+	eavailable = eblocks - bad_blocks;
+
+	if (eavailable < MIN_ERASE_BLOCKS) {
+		printk(KERN_ERR "%s: Not enough erase blocks. %u available, "
+			"%d needed\n", MTDSWAP_PREFIX, eavailable,
+			MIN_ERASE_BLOCKS);
+		return;
+	}
+
+	spare_cnt = div_u64((uint64_t)eavailable * spare_eblocks, 100);
+
+	if (spare_cnt < MIN_SPARE_EBLOCKS)
+		spare_cnt = MIN_SPARE_EBLOCKS;
+
+	if (spare_cnt > eavailable - 1)
+		spare_cnt = eavailable - 1;
+
+	swap_size = (uint64_t)(eavailable - spare_cnt) * mtd->erasesize +
+		(header ? PAGE_SIZE : 0);
+
+	printk(KERN_INFO "%s: Enabling MTD swap on device %lu, size %llu KB, "
+		"%u spare, %u bad blocks\n",
+		MTDSWAP_PREFIX, part, swap_size / 1024, spare_cnt, bad_blocks);
+
+	d = kzalloc(sizeof(struct mtdswap_dev), GFP_KERNEL);
+	if (!d)
+		return;
+
+	mbd_dev = kzalloc(sizeof(struct mtd_blktrans_dev), GFP_KERNEL);
+	if (!mbd_dev) {
+		kfree(d);
+		return;
+	}
+
+	d->mbd_dev = mbd_dev;
+	mbd_dev->priv = d;
+
+	mbd_dev->mtd = mtd;
+	mbd_dev->devnum = mtd->index;
+	mbd_dev->size = swap_size >> PAGE_SHIFT;
+	mbd_dev->tr = tr;
+
+	if (!(mtd->flags & MTD_WRITEABLE))
+		mbd_dev->readonly = 1;
+
+	if (mtdswap_init(d, eblocks, spare_cnt) < 0)
+		goto init_failed;
+
+	if (add_mtd_blktrans_dev(mbd_dev) < 0)
+		goto cleanup;
+
+	d->dev = disk_to_dev(mbd_dev->disk);
+
+	ret = mtdswap_add_debugfs(d);
+	if (ret < 0)
+		goto debugfs_failed;
+
+	return;
+
+debugfs_failed:
+	del_mtd_blktrans_dev(mbd_dev);
+
+cleanup:
+	mtdswap_cleanup(d);
+
+init_failed:
+	kfree(mbd_dev);
+	kfree(d);
+}
+
+static void mtdswap_remove_dev(struct mtd_blktrans_dev *dev)
+{
+	struct mtdswap_dev *d = MTDSWAP_MBD_TO_MTDSWAP(dev);
+
+	debugfs_remove_recursive(d->debugfs_root);
+	del_mtd_blktrans_dev(dev);
+	mtdswap_cleanup(d);
+	kfree(d);
+}
+
+static struct mtd_blktrans_ops mtdswap_ops = {
+	.name		= "mtdswap",
+	.major		= 0,
+	.part_bits	= 0,
+	.blksize	= PAGE_SIZE,
+	.flush		= mtdswap_flush,
+	.readsect	= mtdswap_readsect,
+	.writesect	= mtdswap_writesect,
+	.discard	= mtdswap_discard,
+	.background	= mtdswap_background,
+	.add_mtd	= mtdswap_add_mtd,
+	.remove_dev	= mtdswap_remove_dev,
+	.owner		= THIS_MODULE,
+};
+
+static int __init mtdswap_modinit(void)
+{
+	return register_mtd_blktrans(&mtdswap_ops);
+}
+
+static void __exit mtdswap_modexit(void)
+{
+	deregister_mtd_blktrans(&mtdswap_ops);
+}
+
+module_init(mtdswap_modinit);
+module_exit(mtdswap_modexit);
+
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Jarkko Lavinen <jarkko.lavinen@nokia.com>");
+MODULE_DESCRIPTION("Block device access to an MTD suitable for using as "
+		"swap space");
diff --git a/drivers/mtd/nand/Kconfig b/drivers/mtd/nand/Kconfig
index 4f6c06f16328..a92054e945e1 100644
--- a/drivers/mtd/nand/Kconfig
+++ b/drivers/mtd/nand/Kconfig
@@ -31,6 +31,21 @@ config MTD_NAND_VERIFY_WRITE
 	  device thinks the write was successful, a bit could have been
 	  flipped accidentally due to device wear or something else.
 
+config MTD_NAND_BCH
+	tristate
+	select BCH
+	depends on MTD_NAND_ECC_BCH
+	default MTD_NAND
+
+config MTD_NAND_ECC_BCH
+	bool "Support software BCH ECC"
+	default n
+	help
+	  This enables support for software BCH error correction. Binary BCH
+	  codes are more powerful and cpu intensive than traditional Hamming
+	  ECC codes. They are used with NAND devices requiring more than 1 bit
+	  of error correction.
+
 config MTD_SM_COMMON
 	tristate
 	default n
diff --git a/drivers/mtd/nand/Makefile b/drivers/mtd/nand/Makefile
index 8ad6faec72cb..5745d831168e 100644
--- a/drivers/mtd/nand/Makefile
+++ b/drivers/mtd/nand/Makefile
@@ -4,6 +4,7 @@
 
 obj-$(CONFIG_MTD_NAND)			+= nand.o
 obj-$(CONFIG_MTD_NAND_ECC)		+= nand_ecc.o
+obj-$(CONFIG_MTD_NAND_BCH)		+= nand_bch.o
 obj-$(CONFIG_MTD_NAND_IDS)		+= nand_ids.o
 obj-$(CONFIG_MTD_SM_COMMON) 		+= sm_common.o
 
diff --git a/drivers/mtd/nand/atmel_nand.c b/drivers/mtd/nand/atmel_nand.c
index ccce0f03b5dc..6fae04b3fc6d 100644
--- a/drivers/mtd/nand/atmel_nand.c
+++ b/drivers/mtd/nand/atmel_nand.c
@@ -48,6 +48,9 @@
 #define no_ecc		0
 #endif
 
+static int use_dma = 1;
+module_param(use_dma, int, 0);
+
 static int on_flash_bbt = 0;
 module_param(on_flash_bbt, int, 0);
 
@@ -89,11 +92,20 @@ struct atmel_nand_host {
 	struct nand_chip	nand_chip;
 	struct mtd_info		mtd;
 	void __iomem		*io_base;
+	dma_addr_t		io_phys;
 	struct atmel_nand_data	*board;
 	struct device		*dev;
 	void __iomem		*ecc;
+
+	struct completion	comp;
+	struct dma_chan		*dma_chan;
 };
 
+static int cpu_has_dma(void)
+{
+	return cpu_is_at91sam9rl() || cpu_is_at91sam9g45();
+}
+
 /*
  * Enable NAND.
  */
@@ -150,7 +162,7 @@ static int atmel_nand_device_ready(struct mtd_info *mtd)
 /*
  * Minimal-overhead PIO for data access.
  */
-static void atmel_read_buf(struct mtd_info *mtd, u8 *buf, int len)
+static void atmel_read_buf8(struct mtd_info *mtd, u8 *buf, int len)
 {
 	struct nand_chip	*nand_chip = mtd->priv;
 
@@ -164,7 +176,7 @@ static void atmel_read_buf16(struct mtd_info *mtd, u8 *buf, int len)
 	__raw_readsw(nand_chip->IO_ADDR_R, buf, len / 2);
 }
 
-static void atmel_write_buf(struct mtd_info *mtd, const u8 *buf, int len)
+static void atmel_write_buf8(struct mtd_info *mtd, const u8 *buf, int len)
 {
 	struct nand_chip	*nand_chip = mtd->priv;
 
@@ -178,6 +190,121 @@ static void atmel_write_buf16(struct mtd_info *mtd, const u8 *buf, int len)
 	__raw_writesw(nand_chip->IO_ADDR_W, buf, len / 2);
 }
 
+static void dma_complete_func(void *completion)
+{
+	complete(completion);
+}
+
+static int atmel_nand_dma_op(struct mtd_info *mtd, void *buf, int len,
+			       int is_read)
+{
+	struct dma_device *dma_dev;
+	enum dma_ctrl_flags flags;
+	dma_addr_t dma_src_addr, dma_dst_addr, phys_addr;
+	struct dma_async_tx_descriptor *tx = NULL;
+	dma_cookie_t cookie;
+	struct nand_chip *chip = mtd->priv;
+	struct atmel_nand_host *host = chip->priv;
+	void *p = buf;
+	int err = -EIO;
+	enum dma_data_direction dir = is_read ? DMA_FROM_DEVICE : DMA_TO_DEVICE;
+
+	if (buf >= high_memory) {
+		struct page *pg;
+
+		if (((size_t)buf & PAGE_MASK) !=
+		    ((size_t)(buf + len - 1) & PAGE_MASK)) {
+			dev_warn(host->dev, "Buffer not fit in one page\n");
+			goto err_buf;
+		}
+
+		pg = vmalloc_to_page(buf);
+		if (pg == 0) {
+			dev_err(host->dev, "Failed to vmalloc_to_page\n");
+			goto err_buf;
+		}
+		p = page_address(pg) + ((size_t)buf & ~PAGE_MASK);
+	}
+
+	dma_dev = host->dma_chan->device;
+
+	flags = DMA_CTRL_ACK | DMA_PREP_INTERRUPT | DMA_COMPL_SKIP_SRC_UNMAP |
+		DMA_COMPL_SKIP_DEST_UNMAP;
+
+	phys_addr = dma_map_single(dma_dev->dev, p, len, dir);
+	if (dma_mapping_error(dma_dev->dev, phys_addr)) {
+		dev_err(host->dev, "Failed to dma_map_single\n");
+		goto err_buf;
+	}
+
+	if (is_read) {
+		dma_src_addr = host->io_phys;
+		dma_dst_addr = phys_addr;
+	} else {
+		dma_src_addr = phys_addr;
+		dma_dst_addr = host->io_phys;
+	}
+
+	tx = dma_dev->device_prep_dma_memcpy(host->dma_chan, dma_dst_addr,
+					     dma_src_addr, len, flags);
+	if (!tx) {
+		dev_err(host->dev, "Failed to prepare DMA memcpy\n");
+		goto err_dma;
+	}
+
+	init_completion(&host->comp);
+	tx->callback = dma_complete_func;
+	tx->callback_param = &host->comp;
+
+	cookie = tx->tx_submit(tx);
+	if (dma_submit_error(cookie)) {
+		dev_err(host->dev, "Failed to do DMA tx_submit\n");
+		goto err_dma;
+	}
+
+	dma_async_issue_pending(host->dma_chan);
+	wait_for_completion(&host->comp);
+
+	err = 0;
+
+err_dma:
+	dma_unmap_single(dma_dev->dev, phys_addr, len, dir);
+err_buf:
+	if (err != 0)
+		dev_warn(host->dev, "Fall back to CPU I/O\n");
+	return err;
+}
+
+static void atmel_read_buf(struct mtd_info *mtd, u8 *buf, int len)
+{
+	struct nand_chip *chip = mtd->priv;
+	struct atmel_nand_host *host = chip->priv;
+
+	if (use_dma && len >= mtd->oobsize)
+		if (atmel_nand_dma_op(mtd, buf, len, 1) == 0)
+			return;
+
+	if (host->board->bus_width_16)
+		atmel_read_buf16(mtd, buf, len);
+	else
+		atmel_read_buf8(mtd, buf, len);
+}
+
+static void atmel_write_buf(struct mtd_info *mtd, const u8 *buf, int len)
+{
+	struct nand_chip *chip = mtd->priv;
+	struct atmel_nand_host *host = chip->priv;
+
+	if (use_dma && len >= mtd->oobsize)
+		if (atmel_nand_dma_op(mtd, (void *)buf, len, 0) == 0)
+			return;
+
+	if (host->board->bus_width_16)
+		atmel_write_buf16(mtd, buf, len);
+	else
+		atmel_write_buf8(mtd, buf, len);
+}
+
 /*
  * Calculate HW ECC
  *
@@ -398,6 +525,8 @@ static int __init atmel_nand_probe(struct platform_device *pdev)
 		return -ENOMEM;
 	}
 
+	host->io_phys = (dma_addr_t)mem->start;
+
 	host->io_base = ioremap(mem->start, mem->end - mem->start + 1);
 	if (host->io_base == NULL) {
 		printk(KERN_ERR "atmel_nand: ioremap failed\n");
@@ -448,14 +577,11 @@ static int __init atmel_nand_probe(struct platform_device *pdev)
 
 	nand_chip->chip_delay = 20;		/* 20us command delay time */
 
-	if (host->board->bus_width_16) {	/* 16-bit bus width */
+	if (host->board->bus_width_16)	/* 16-bit bus width */
 		nand_chip->options |= NAND_BUSWIDTH_16;
-		nand_chip->read_buf = atmel_read_buf16;
-		nand_chip->write_buf = atmel_write_buf16;
-	} else {
-		nand_chip->read_buf = atmel_read_buf;
-		nand_chip->write_buf = atmel_write_buf;
-	}
+
+	nand_chip->read_buf = atmel_read_buf;
+	nand_chip->write_buf = atmel_write_buf;
 
 	platform_set_drvdata(pdev, host);
 	atmel_nand_enable(host);
@@ -473,6 +599,22 @@ static int __init atmel_nand_probe(struct platform_device *pdev)
 		nand_chip->options |= NAND_USE_FLASH_BBT;
 	}
 
+	if (cpu_has_dma() && use_dma) {
+		dma_cap_mask_t mask;
+
+		dma_cap_zero(mask);
+		dma_cap_set(DMA_MEMCPY, mask);
+		host->dma_chan = dma_request_channel(mask, 0, NULL);
+		if (!host->dma_chan) {
+			dev_err(host->dev, "Failed to request DMA channel\n");
+			use_dma = 0;
+		}
+	}
+	if (use_dma)
+		dev_info(host->dev, "Using DMA for NAND access.\n");
+	else
+		dev_info(host->dev, "No DMA support for NAND access.\n");
+
 	/* first scan to find the device and get the page size */
 	if (nand_scan_ident(mtd, 1, NULL)) {
 		res = -ENXIO;
@@ -555,6 +697,8 @@ err_scan_ident:
 err_no_card:
 	atmel_nand_disable(host);
 	platform_set_drvdata(pdev, NULL);
+	if (host->dma_chan)
+		dma_release_channel(host->dma_chan);
 	if (host->ecc)
 		iounmap(host->ecc);
 err_ecc_ioremap:
@@ -578,6 +722,10 @@ static int __exit atmel_nand_remove(struct platform_device *pdev)
 
 	if (host->ecc)
 		iounmap(host->ecc);
+
+	if (host->dma_chan)
+		dma_release_channel(host->dma_chan);
+
 	iounmap(host->io_base);
 	kfree(host);
 
diff --git a/drivers/mtd/nand/davinci_nand.c b/drivers/mtd/nand/davinci_nand.c
index a90fde3ede28..aff3468867ac 100644
--- a/drivers/mtd/nand/davinci_nand.c
+++ b/drivers/mtd/nand/davinci_nand.c
@@ -37,9 +37,6 @@
 #include <mach/nand.h>
 #include <mach/aemif.h>
 
-#include <asm/mach-types.h>
-
-
 /*
  * This is a device driver for the NAND flash controller found on the
  * various DaVinci family chips.  It handles up to four SoC chipselects,
diff --git a/drivers/mtd/nand/mpc5121_nfc.c b/drivers/mtd/nand/mpc5121_nfc.c
index c2f95437e5e9..0b81b5b499d1 100644
--- a/drivers/mtd/nand/mpc5121_nfc.c
+++ b/drivers/mtd/nand/mpc5121_nfc.c
@@ -29,6 +29,7 @@
 #include <linux/clk.h>
 #include <linux/gfp.h>
 #include <linux/delay.h>
+#include <linux/err.h>
 #include <linux/init.h>
 #include <linux/interrupt.h>
 #include <linux/io.h>
@@ -757,9 +758,9 @@ static int __devinit mpc5121_nfc_probe(struct platform_device *op)
 
 	/* Enable NFC clock */
 	prv->clk = clk_get(dev, "nfc_clk");
-	if (!prv->clk) {
+	if (IS_ERR(prv->clk)) {
 		dev_err(dev, "Unable to acquire NFC clock!\n");
-		retval = -ENODEV;
+		retval = PTR_ERR(prv->clk);
 		goto error;
 	}
 
diff --git a/drivers/mtd/nand/mxc_nand.c b/drivers/mtd/nand/mxc_nand.c
index 5ae1d9ee2cf1..42a95fb41504 100644
--- a/drivers/mtd/nand/mxc_nand.c
+++ b/drivers/mtd/nand/mxc_nand.c
@@ -211,6 +211,31 @@ static struct nand_ecclayout nandv2_hw_eccoob_largepage = {
 	}
 };
 
+/* OOB description for 4096 byte pages with 128 byte OOB */
+static struct nand_ecclayout nandv2_hw_eccoob_4k = {
+	.eccbytes = 8 * 9,
+	.eccpos = {
+		7,  8,  9, 10, 11, 12, 13, 14, 15,
+		23, 24, 25, 26, 27, 28, 29, 30, 31,
+		39, 40, 41, 42, 43, 44, 45, 46, 47,
+		55, 56, 57, 58, 59, 60, 61, 62, 63,
+		71, 72, 73, 74, 75, 76, 77, 78, 79,
+		87, 88, 89, 90, 91, 92, 93, 94, 95,
+		103, 104, 105, 106, 107, 108, 109, 110, 111,
+		119, 120, 121, 122, 123, 124, 125, 126, 127,
+	},
+	.oobfree = {
+		{.offset = 2, .length = 4},
+		{.offset = 16, .length = 7},
+		{.offset = 32, .length = 7},
+		{.offset = 48, .length = 7},
+		{.offset = 64, .length = 7},
+		{.offset = 80, .length = 7},
+		{.offset = 96, .length = 7},
+		{.offset = 112, .length = 7},
+	}
+};
+
 #ifdef CONFIG_MTD_PARTITIONS
 static const char *part_probes[] = { "RedBoot", "cmdlinepart", NULL };
 #endif
@@ -641,9 +666,9 @@ static void mxc_nand_read_buf(struct mtd_info *mtd, u_char *buf, int len)
 
 	n = min(n, len);
 
-	memcpy(buf, host->data_buf + col, len);
+	memcpy(buf, host->data_buf + col, n);
 
-	host->buf_start += len;
+	host->buf_start += n;
 }
 
 /* Used by the upper layer to verify the data in NAND Flash
@@ -1185,6 +1210,8 @@ static int __init mxcnd_probe(struct platform_device *pdev)
 
 	if (mtd->writesize == 2048)
 		this->ecc.layout = oob_largepage;
+	if (nfc_is_v21() && mtd->writesize == 4096)
+		this->ecc.layout = &nandv2_hw_eccoob_4k;
 
 	/* second phase scan */
 	if (nand_scan_tail(mtd)) {
diff --git a/drivers/mtd/nand/nand_base.c b/drivers/mtd/nand/nand_base.c
index a9c6ce745767..85cfc061d41c 100644
--- a/drivers/mtd/nand/nand_base.c
+++ b/drivers/mtd/nand/nand_base.c
@@ -42,6 +42,7 @@
 #include <linux/mtd/mtd.h>
 #include <linux/mtd/nand.h>
 #include <linux/mtd/nand_ecc.h>
+#include <linux/mtd/nand_bch.h>
 #include <linux/interrupt.h>
 #include <linux/bitops.h>
 #include <linux/leds.h>
@@ -2377,7 +2378,7 @@ static int nand_do_write_oob(struct mtd_info *mtd, loff_t to,
 		return -EINVAL;
 	}
 
-	/* Do not allow reads past end of device */
+	/* Do not allow write past end of device */
 	if (unlikely(to >= mtd->size ||
 		     ops->ooboffs + ops->ooblen >
 			((mtd->size >> chip->page_shift) -
@@ -3248,7 +3249,7 @@ int nand_scan_tail(struct mtd_info *mtd)
 	/*
 	 * If no default placement scheme is given, select an appropriate one
 	 */
-	if (!chip->ecc.layout) {
+	if (!chip->ecc.layout && (chip->ecc.mode != NAND_ECC_SOFT_BCH)) {
 		switch (mtd->oobsize) {
 		case 8:
 			chip->ecc.layout = &nand_oob_8;
@@ -3351,6 +3352,40 @@ int nand_scan_tail(struct mtd_info *mtd)
 		chip->ecc.bytes = 3;
 		break;
 
+	case NAND_ECC_SOFT_BCH:
+		if (!mtd_nand_has_bch()) {
+			printk(KERN_WARNING "CONFIG_MTD_ECC_BCH not enabled\n");
+			BUG();
+		}
+		chip->ecc.calculate = nand_bch_calculate_ecc;
+		chip->ecc.correct = nand_bch_correct_data;
+		chip->ecc.read_page = nand_read_page_swecc;
+		chip->ecc.read_subpage = nand_read_subpage;
+		chip->ecc.write_page = nand_write_page_swecc;
+		chip->ecc.read_page_raw = nand_read_page_raw;
+		chip->ecc.write_page_raw = nand_write_page_raw;
+		chip->ecc.read_oob = nand_read_oob_std;
+		chip->ecc.write_oob = nand_write_oob_std;
+		/*
+		 * Board driver should supply ecc.size and ecc.bytes values to
+		 * select how many bits are correctable; see nand_bch_init()
+		 * for details.
+		 * Otherwise, default to 4 bits for large page devices
+		 */
+		if (!chip->ecc.size && (mtd->oobsize >= 64)) {
+			chip->ecc.size = 512;
+			chip->ecc.bytes = 7;
+		}
+		chip->ecc.priv = nand_bch_init(mtd,
+					       chip->ecc.size,
+					       chip->ecc.bytes,
+					       &chip->ecc.layout);
+		if (!chip->ecc.priv) {
+			printk(KERN_WARNING "BCH ECC initialization failed!\n");
+			BUG();
+		}
+		break;
+
 	case NAND_ECC_NONE:
 		printk(KERN_WARNING "NAND_ECC_NONE selected by board driver. "
 		       "This is not recommended !!\n");
@@ -3501,6 +3536,9 @@ void nand_release(struct mtd_info *mtd)
 {
 	struct nand_chip *chip = mtd->priv;
 
+	if (chip->ecc.mode == NAND_ECC_SOFT_BCH)
+		nand_bch_free((struct nand_bch_control *)chip->ecc.priv);
+
 #ifdef CONFIG_MTD_PARTITIONS
 	/* Deregister partitions */
 	del_mtd_partitions(mtd);
diff --git a/drivers/mtd/nand/nand_bbt.c b/drivers/mtd/nand/nand_bbt.c
index 6ebd869993aa..a1e8b30078d9 100644
--- a/drivers/mtd/nand/nand_bbt.c
+++ b/drivers/mtd/nand/nand_bbt.c
@@ -1101,12 +1101,16 @@ static void mark_bbt_region(struct mtd_info *mtd, struct nand_bbt_descr *td)
 static void verify_bbt_descr(struct mtd_info *mtd, struct nand_bbt_descr *bd)
 {
 	struct nand_chip *this = mtd->priv;
-	u32 pattern_len = bd->len;
-	u32 bits = bd->options & NAND_BBT_NRBITS_MSK;
+	u32 pattern_len;
+	u32 bits;
 	u32 table_size;
 
 	if (!bd)
 		return;
+
+	pattern_len = bd->len;
+	bits = bd->options & NAND_BBT_NRBITS_MSK;
+
 	BUG_ON((this->options & NAND_USE_FLASH_BBT_NO_OOB) &&
 			!(this->options & NAND_USE_FLASH_BBT));
 	BUG_ON(!bits);
diff --git a/drivers/mtd/nand/nand_bch.c b/drivers/mtd/nand/nand_bch.c
new file mode 100644
index 000000000000..0f931e757116
--- /dev/null
+++ b/drivers/mtd/nand/nand_bch.c
@@ -0,0 +1,243 @@
+/*
+ * This file provides ECC correction for more than 1 bit per block of data,
+ * using binary BCH codes. It relies on the generic BCH library lib/bch.c.
+ *
+ * Copyright © 2011 Ivan Djelic <ivan.djelic@parrot.com>
+ *
+ * This file 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 or (at your option) any
+ * later version.
+ *
+ * This file 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 file; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/bitops.h>
+#include <linux/mtd/mtd.h>
+#include <linux/mtd/nand.h>
+#include <linux/mtd/nand_bch.h>
+#include <linux/bch.h>
+
+/**
+ * struct nand_bch_control - private NAND BCH control structure
+ * @bch:       BCH control structure
+ * @ecclayout: private ecc layout for this BCH configuration
+ * @errloc:    error location array
+ * @eccmask:   XOR ecc mask, allows erased pages to be decoded as valid
+ */
+struct nand_bch_control {
+	struct bch_control   *bch;
+	struct nand_ecclayout ecclayout;
+	unsigned int         *errloc;
+	unsigned char        *eccmask;
+};
+
+/**
+ * nand_bch_calculate_ecc - [NAND Interface] Calculate ECC for data block
+ * @mtd:	MTD block structure
+ * @buf:	input buffer with raw data
+ * @code:	output buffer with ECC
+ */
+int nand_bch_calculate_ecc(struct mtd_info *mtd, const unsigned char *buf,
+			   unsigned char *code)
+{
+	const struct nand_chip *chip = mtd->priv;
+	struct nand_bch_control *nbc = chip->ecc.priv;
+	unsigned int i;
+
+	memset(code, 0, chip->ecc.bytes);
+	encode_bch(nbc->bch, buf, chip->ecc.size, code);
+
+	/* apply mask so that an erased page is a valid codeword */
+	for (i = 0; i < chip->ecc.bytes; i++)
+		code[i] ^= nbc->eccmask[i];
+
+	return 0;
+}
+EXPORT_SYMBOL(nand_bch_calculate_ecc);
+
+/**
+ * nand_bch_correct_data - [NAND Interface] Detect and correct bit error(s)
+ * @mtd:	MTD block structure
+ * @buf:	raw data read from the chip
+ * @read_ecc:	ECC from the chip
+ * @calc_ecc:	the ECC calculated from raw data
+ *
+ * Detect and correct bit errors for a data byte block
+ */
+int nand_bch_correct_data(struct mtd_info *mtd, unsigned char *buf,
+			  unsigned char *read_ecc, unsigned char *calc_ecc)
+{
+	const struct nand_chip *chip = mtd->priv;
+	struct nand_bch_control *nbc = chip->ecc.priv;
+	unsigned int *errloc = nbc->errloc;
+	int i, count;
+
+	count = decode_bch(nbc->bch, NULL, chip->ecc.size, read_ecc, calc_ecc,
+			   NULL, errloc);
+	if (count > 0) {
+		for (i = 0; i < count; i++) {
+			if (errloc[i] < (chip->ecc.size*8))
+				/* error is located in data, correct it */
+				buf[errloc[i] >> 3] ^= (1 << (errloc[i] & 7));
+			/* else error in ecc, no action needed */
+
+			DEBUG(MTD_DEBUG_LEVEL0, "%s: corrected bitflip %u\n",
+			      __func__, errloc[i]);
+		}
+	} else if (count < 0) {
+		printk(KERN_ERR "ecc unrecoverable error\n");
+		count = -1;
+	}
+	return count;
+}
+EXPORT_SYMBOL(nand_bch_correct_data);
+
+/**
+ * nand_bch_init - [NAND Interface] Initialize NAND BCH error correction
+ * @mtd:	MTD block structure
+ * @eccsize:	ecc block size in bytes
+ * @eccbytes:	ecc length in bytes
+ * @ecclayout:	output default layout
+ *
+ * Returns:
+ *  a pointer to a new NAND BCH control structure, or NULL upon failure
+ *
+ * Initialize NAND BCH error correction. Parameters @eccsize and @eccbytes
+ * are used to compute BCH parameters m (Galois field order) and t (error
+ * correction capability). @eccbytes should be equal to the number of bytes
+ * required to store m*t bits, where m is such that 2^m-1 > @eccsize*8.
+ *
+ * Example: to configure 4 bit correction per 512 bytes, you should pass
+ * @eccsize = 512  (thus, m=13 is the smallest integer such that 2^m-1 > 512*8)
+ * @eccbytes = 7   (7 bytes are required to store m*t = 13*4 = 52 bits)
+ */
+struct nand_bch_control *
+nand_bch_init(struct mtd_info *mtd, unsigned int eccsize, unsigned int eccbytes,
+	      struct nand_ecclayout **ecclayout)
+{
+	unsigned int m, t, eccsteps, i;
+	struct nand_ecclayout *layout;
+	struct nand_bch_control *nbc = NULL;
+	unsigned char *erased_page;
+
+	if (!eccsize || !eccbytes) {
+		printk(KERN_WARNING "ecc parameters not supplied\n");
+		goto fail;
+	}
+
+	m = fls(1+8*eccsize);
+	t = (eccbytes*8)/m;
+
+	nbc = kzalloc(sizeof(*nbc), GFP_KERNEL);
+	if (!nbc)
+		goto fail;
+
+	nbc->bch = init_bch(m, t, 0);
+	if (!nbc->bch)
+		goto fail;
+
+	/* verify that eccbytes has the expected value */
+	if (nbc->bch->ecc_bytes != eccbytes) {
+		printk(KERN_WARNING "invalid eccbytes %u, should be %u\n",
+		       eccbytes, nbc->bch->ecc_bytes);
+		goto fail;
+	}
+
+	eccsteps = mtd->writesize/eccsize;
+
+	/* if no ecc placement scheme was provided, build one */
+	if (!*ecclayout) {
+
+		/* handle large page devices only */
+		if (mtd->oobsize < 64) {
+			printk(KERN_WARNING "must provide an oob scheme for "
+			       "oobsize %d\n", mtd->oobsize);
+			goto fail;
+		}
+
+		layout = &nbc->ecclayout;
+		layout->eccbytes = eccsteps*eccbytes;
+
+		/* reserve 2 bytes for bad block marker */
+		if (layout->eccbytes+2 > mtd->oobsize) {
+			printk(KERN_WARNING "no suitable oob scheme available "
+			       "for oobsize %d eccbytes %u\n", mtd->oobsize,
+			       eccbytes);
+			goto fail;
+		}
+		/* put ecc bytes at oob tail */
+		for (i = 0; i < layout->eccbytes; i++)
+			layout->eccpos[i] = mtd->oobsize-layout->eccbytes+i;
+
+		layout->oobfree[0].offset = 2;
+		layout->oobfree[0].length = mtd->oobsize-2-layout->eccbytes;
+
+		*ecclayout = layout;
+	}
+
+	/* sanity checks */
+	if (8*(eccsize+eccbytes) >= (1 << m)) {
+		printk(KERN_WARNING "eccsize %u is too large\n", eccsize);
+		goto fail;
+	}
+	if ((*ecclayout)->eccbytes != (eccsteps*eccbytes)) {
+		printk(KERN_WARNING "invalid ecc layout\n");
+		goto fail;
+	}
+
+	nbc->eccmask = kmalloc(eccbytes, GFP_KERNEL);
+	nbc->errloc = kmalloc(t*sizeof(*nbc->errloc), GFP_KERNEL);
+	if (!nbc->eccmask || !nbc->errloc)
+		goto fail;
+	/*
+	 * compute and store the inverted ecc of an erased ecc block
+	 */
+	erased_page = kmalloc(eccsize, GFP_KERNEL);
+	if (!erased_page)
+		goto fail;
+
+	memset(erased_page, 0xff, eccsize);
+	memset(nbc->eccmask, 0, eccbytes);
+	encode_bch(nbc->bch, erased_page, eccsize, nbc->eccmask);
+	kfree(erased_page);
+
+	for (i = 0; i < eccbytes; i++)
+		nbc->eccmask[i] ^= 0xff;
+
+	return nbc;
+fail:
+	nand_bch_free(nbc);
+	return NULL;
+}
+EXPORT_SYMBOL(nand_bch_init);
+
+/**
+ * nand_bch_free - [NAND Interface] Release NAND BCH ECC resources
+ * @nbc:	NAND BCH control structure
+ */
+void nand_bch_free(struct nand_bch_control *nbc)
+{
+	if (nbc) {
+		free_bch(nbc->bch);
+		kfree(nbc->errloc);
+		kfree(nbc->eccmask);
+		kfree(nbc);
+	}
+}
+EXPORT_SYMBOL(nand_bch_free);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Ivan Djelic <ivan.djelic@parrot.com>");
+MODULE_DESCRIPTION("NAND software BCH ECC support");
diff --git a/drivers/mtd/nand/nandsim.c b/drivers/mtd/nand/nandsim.c
index a5aa99f014ba..213181be0d9a 100644
--- a/drivers/mtd/nand/nandsim.c
+++ b/drivers/mtd/nand/nandsim.c
@@ -34,6 +34,7 @@
 #include <linux/string.h>
 #include <linux/mtd/mtd.h>
 #include <linux/mtd/nand.h>
+#include <linux/mtd/nand_bch.h>
 #include <linux/mtd/partitions.h>
 #include <linux/delay.h>
 #include <linux/list.h>
@@ -108,6 +109,7 @@ static unsigned int rptwear = 0;
 static unsigned int overridesize = 0;
 static char *cache_file = NULL;
 static unsigned int bbt;
+static unsigned int bch;
 
 module_param(first_id_byte,  uint, 0400);
 module_param(second_id_byte, uint, 0400);
@@ -132,6 +134,7 @@ module_param(rptwear,        uint, 0400);
 module_param(overridesize,   uint, 0400);
 module_param(cache_file,     charp, 0400);
 module_param(bbt,	     uint, 0400);
+module_param(bch,	     uint, 0400);
 
 MODULE_PARM_DESC(first_id_byte,  "The first byte returned by NAND Flash 'read ID' command (manufacturer ID)");
 MODULE_PARM_DESC(second_id_byte, "The second byte returned by NAND Flash 'read ID' command (chip ID)");
@@ -165,6 +168,8 @@ MODULE_PARM_DESC(overridesize,   "Specifies the NAND Flash size overriding the I
 				 " e.g. 5 means a size of 32 erase blocks");
 MODULE_PARM_DESC(cache_file,     "File to use to cache nand pages instead of memory");
 MODULE_PARM_DESC(bbt,		 "0 OOB, 1 BBT with marker in OOB, 2 BBT with marker in data area");
+MODULE_PARM_DESC(bch,		 "Enable BCH ecc and set how many bits should "
+				 "be correctable in 512-byte blocks");
 
 /* The largest possible page size */
 #define NS_LARGEST_PAGE_SIZE	4096
@@ -2309,7 +2314,43 @@ static int __init ns_init_module(void)
 	if ((retval = parse_gravepages()) != 0)
 		goto error;
 
-	if ((retval = nand_scan(nsmtd, 1)) != 0) {
+	retval = nand_scan_ident(nsmtd, 1, NULL);
+	if (retval) {
+		NS_ERR("cannot scan NAND Simulator device\n");
+		if (retval > 0)
+			retval = -ENXIO;
+		goto error;
+	}
+
+	if (bch) {
+		unsigned int eccsteps, eccbytes;
+		if (!mtd_nand_has_bch()) {
+			NS_ERR("BCH ECC support is disabled\n");
+			retval = -EINVAL;
+			goto error;
+		}
+		/* use 512-byte ecc blocks */
+		eccsteps = nsmtd->writesize/512;
+		eccbytes = (bch*13+7)/8;
+		/* do not bother supporting small page devices */
+		if ((nsmtd->oobsize < 64) || !eccsteps) {
+			NS_ERR("bch not available on small page devices\n");
+			retval = -EINVAL;
+			goto error;
+		}
+		if ((eccbytes*eccsteps+2) > nsmtd->oobsize) {
+			NS_ERR("invalid bch value %u\n", bch);
+			retval = -EINVAL;
+			goto error;
+		}
+		chip->ecc.mode = NAND_ECC_SOFT_BCH;
+		chip->ecc.size = 512;
+		chip->ecc.bytes = eccbytes;
+		NS_INFO("using %u-bit/%u bytes BCH ECC\n", bch, chip->ecc.size);
+	}
+
+	retval = nand_scan_tail(nsmtd);
+	if (retval) {
 		NS_ERR("can't register NAND Simulator\n");
 		if (retval > 0)
 			retval = -ENXIO;
diff --git a/drivers/mtd/nand/omap2.c b/drivers/mtd/nand/omap2.c
index 7b8f1fffc528..da9a351c9d79 100644
--- a/drivers/mtd/nand/omap2.c
+++ b/drivers/mtd/nand/omap2.c
@@ -668,6 +668,8 @@ static void gen_true_ecc(u8 *ecc_buf)
  *
  * This function compares two ECC's and indicates if there is an error.
  * If the error can be corrected it will be corrected to the buffer.
+ * If there is no error, %0 is returned. If there is an error but it
+ * was corrected, %1 is returned. Otherwise, %-1 is returned.
  */
 static int omap_compare_ecc(u8 *ecc_data1,	/* read from NAND memory */
 			    u8 *ecc_data2,	/* read from register */
@@ -773,7 +775,7 @@ static int omap_compare_ecc(u8 *ecc_data1,	/* read from NAND memory */
 
 		page_data[find_byte] ^= (1 << find_bit);
 
-		return 0;
+		return 1;
 	default:
 		if (isEccFF) {
 			if (ecc_data2[0] == 0 &&
@@ -794,8 +796,11 @@ static int omap_compare_ecc(u8 *ecc_data1,	/* read from NAND memory */
  * @calc_ecc: ecc read from HW ECC registers
  *
  * Compares the ecc read from nand spare area with ECC registers values
- * and if ECC's mismached, it will call 'omap_compare_ecc' for error detection
- * and correction.
+ * and if ECC's mismatched, it will call 'omap_compare_ecc' for error
+ * detection and correction. If there are no errors, %0 is returned. If
+ * there were errors and all of the errors were corrected, the number of
+ * corrected errors is returned. If uncorrectable errors exist, %-1 is
+ * returned.
  */
 static int omap_correct_data(struct mtd_info *mtd, u_char *dat,
 				u_char *read_ecc, u_char *calc_ecc)
@@ -803,6 +808,7 @@ static int omap_correct_data(struct mtd_info *mtd, u_char *dat,
 	struct omap_nand_info *info = container_of(mtd, struct omap_nand_info,
 							mtd);
 	int blockCnt = 0, i = 0, ret = 0;
+	int stat = 0;
 
 	/* Ex NAND_ECC_HW12_2048 */
 	if ((info->nand.ecc.mode == NAND_ECC_HW) &&
@@ -816,12 +822,14 @@ static int omap_correct_data(struct mtd_info *mtd, u_char *dat,
 			ret = omap_compare_ecc(read_ecc, calc_ecc, dat);
 			if (ret < 0)
 				return ret;
+			/* keep track of the number of corrected errors */
+			stat += ret;
 		}
 		read_ecc += 3;
 		calc_ecc += 3;
 		dat      += 512;
 	}
-	return 0;
+	return stat;
 }
 
 /**
diff --git a/drivers/mtd/nand/pxa3xx_nand.c b/drivers/mtd/nand/pxa3xx_nand.c
index ea2c288df3f6..ab7f4c33ced6 100644
--- a/drivers/mtd/nand/pxa3xx_nand.c
+++ b/drivers/mtd/nand/pxa3xx_nand.c
@@ -27,6 +27,8 @@
 #include <plat/pxa3xx_nand.h>
 
 #define	CHIP_DELAY_TIMEOUT	(2 * HZ/10)
+#define NAND_STOP_DELAY		(2 * HZ/50)
+#define PAGE_CHUNK_SIZE		(2048)
 
 /* registers and bit definitions */
 #define NDCR		(0x00) /* Control register */
@@ -52,16 +54,18 @@
 #define NDCR_ND_MODE		(0x3 << 21)
 #define NDCR_NAND_MODE   	(0x0)
 #define NDCR_CLR_PG_CNT		(0x1 << 20)
-#define NDCR_CLR_ECC		(0x1 << 19)
+#define NDCR_STOP_ON_UNCOR	(0x1 << 19)
 #define NDCR_RD_ID_CNT_MASK	(0x7 << 16)
 #define NDCR_RD_ID_CNT(x)	(((x) << 16) & NDCR_RD_ID_CNT_MASK)
 
 #define NDCR_RA_START		(0x1 << 15)
 #define NDCR_PG_PER_BLK		(0x1 << 14)
 #define NDCR_ND_ARB_EN		(0x1 << 12)
+#define NDCR_INT_MASK           (0xFFF)
 
 #define NDSR_MASK		(0xfff)
-#define NDSR_RDY		(0x1 << 11)
+#define NDSR_RDY                (0x1 << 12)
+#define NDSR_FLASH_RDY          (0x1 << 11)
 #define NDSR_CS0_PAGED		(0x1 << 10)
 #define NDSR_CS1_PAGED		(0x1 << 9)
 #define NDSR_CS0_CMDD		(0x1 << 8)
@@ -74,6 +78,7 @@
 #define NDSR_RDDREQ		(0x1 << 1)
 #define NDSR_WRCMDREQ		(0x1)
 
+#define NDCB0_ST_ROW_EN         (0x1 << 26)
 #define NDCB0_AUTO_RS		(0x1 << 25)
 #define NDCB0_CSEL		(0x1 << 24)
 #define NDCB0_CMD_TYPE_MASK	(0x7 << 21)
@@ -104,18 +109,21 @@ enum {
 };
 
 enum {
-	STATE_READY	= 0,
+	STATE_IDLE = 0,
 	STATE_CMD_HANDLE,
 	STATE_DMA_READING,
 	STATE_DMA_WRITING,
 	STATE_DMA_DONE,
 	STATE_PIO_READING,
 	STATE_PIO_WRITING,
+	STATE_CMD_DONE,
+	STATE_READY,
 };
 
 struct pxa3xx_nand_info {
 	struct nand_chip	nand_chip;
 
+	struct nand_hw_control	controller;
 	struct platform_device	 *pdev;
 	struct pxa3xx_nand_cmdset *cmdset;
 
@@ -126,6 +134,7 @@ struct pxa3xx_nand_info {
 	unsigned int 		buf_start;
 	unsigned int		buf_count;
 
+	struct mtd_info         *mtd;
 	/* DMA information */
 	int			drcmr_dat;
 	int			drcmr_cmd;
@@ -149,6 +158,7 @@ struct pxa3xx_nand_info {
 
 	int			use_ecc;	/* use HW ECC ? */
 	int			use_dma;	/* use DMA ? */
+	int			is_ready;
 
 	unsigned int		page_size;	/* page size of attached chip */
 	unsigned int		data_size;	/* data size in FIFO */
@@ -201,20 +211,22 @@ static struct pxa3xx_nand_timing timing[] = {
 };
 
 static struct pxa3xx_nand_flash builtin_flash_types[] = {
-	{      0,   0, 2048,  8,  8,    0, &default_cmdset, &timing[0] },
-	{ 0x46ec,  32,  512, 16, 16, 4096, &default_cmdset, &timing[1] },
-	{ 0xdaec,  64, 2048,  8,  8, 2048, &default_cmdset, &timing[1] },
-	{ 0xd7ec, 128, 4096,  8,  8, 8192, &default_cmdset, &timing[1] },
-	{ 0xa12c,  64, 2048,  8,  8, 1024, &default_cmdset, &timing[2] },
-	{ 0xb12c,  64, 2048, 16, 16, 1024, &default_cmdset, &timing[2] },
-	{ 0xdc2c,  64, 2048,  8,  8, 4096, &default_cmdset, &timing[2] },
-	{ 0xcc2c,  64, 2048, 16, 16, 4096, &default_cmdset, &timing[2] },
-	{ 0xba20,  64, 2048, 16, 16, 2048, &default_cmdset, &timing[3] },
+{ "DEFAULT FLASH",      0,   0, 2048,  8,  8,    0, &timing[0] },
+{ "64MiB 16-bit",  0x46ec,  32,  512, 16, 16, 4096, &timing[1] },
+{ "256MiB 8-bit",  0xdaec,  64, 2048,  8,  8, 2048, &timing[1] },
+{ "4GiB 8-bit",    0xd7ec, 128, 4096,  8,  8, 8192, &timing[1] },
+{ "128MiB 8-bit",  0xa12c,  64, 2048,  8,  8, 1024, &timing[2] },
+{ "128MiB 16-bit", 0xb12c,  64, 2048, 16, 16, 1024, &timing[2] },
+{ "512MiB 8-bit",  0xdc2c,  64, 2048,  8,  8, 4096, &timing[2] },
+{ "512MiB 16-bit", 0xcc2c,  64, 2048, 16, 16, 4096, &timing[2] },
+{ "256MiB 16-bit", 0xba20,  64, 2048, 16, 16, 2048, &timing[3] },
 };
 
 /* Define a default flash type setting serve as flash detecting only */
 #define DEFAULT_FLASH_TYPE (&builtin_flash_types[0])
 
+const char *mtd_names[] = {"pxa3xx_nand-0", NULL};
+
 #define NDTR0_tCH(c)	(min((c), 7) << 19)
 #define NDTR0_tCS(c)	(min((c), 7) << 16)
 #define NDTR0_tWH(c)	(min((c), 7) << 11)
@@ -252,25 +264,6 @@ static void pxa3xx_nand_set_timing(struct pxa3xx_nand_info *info,
 	nand_writel(info, NDTR1CS0, ndtr1);
 }
 
-#define WAIT_EVENT_TIMEOUT	10
-
-static int wait_for_event(struct pxa3xx_nand_info *info, uint32_t event)
-{
-	int timeout = WAIT_EVENT_TIMEOUT;
-	uint32_t ndsr;
-
-	while (timeout--) {
-		ndsr = nand_readl(info, NDSR) & NDSR_MASK;
-		if (ndsr & event) {
-			nand_writel(info, NDSR, ndsr);
-			return 0;
-		}
-		udelay(10);
-	}
-
-	return -ETIMEDOUT;
-}
-
 static void pxa3xx_set_datasize(struct pxa3xx_nand_info *info)
 {
 	int oob_enable = info->reg_ndcr & NDCR_SPARE_EN;
@@ -291,69 +284,45 @@ static void pxa3xx_set_datasize(struct pxa3xx_nand_info *info)
 	}
 }
 
-static int prepare_read_prog_cmd(struct pxa3xx_nand_info *info,
-		uint16_t cmd, int column, int page_addr)
+/**
+ * NOTE: it is a must to set ND_RUN firstly, then write
+ * command buffer, otherwise, it does not work.
+ * We enable all the interrupt at the same time, and
+ * let pxa3xx_nand_irq to handle all logic.
+ */
+static void pxa3xx_nand_start(struct pxa3xx_nand_info *info)
 {
-	const struct pxa3xx_nand_cmdset *cmdset = info->cmdset;
-	pxa3xx_set_datasize(info);
-
-	/* generate values for NDCBx registers */
-	info->ndcb0 = cmd | ((cmd & 0xff00) ? NDCB0_DBC : 0);
-	info->ndcb1 = 0;
-	info->ndcb2 = 0;
-	info->ndcb0 |= NDCB0_ADDR_CYC(info->row_addr_cycles + info->col_addr_cycles);
-
-	if (info->col_addr_cycles == 2) {
-		/* large block, 2 cycles for column address
-		 * row address starts from 3rd cycle
-		 */
-		info->ndcb1 |= page_addr << 16;
-		if (info->row_addr_cycles == 3)
-			info->ndcb2 = (page_addr >> 16) & 0xff;
-	} else
-		/* small block, 1 cycles for column address
-		 * row address starts from 2nd cycle
-		 */
-		info->ndcb1 = page_addr << 8;
-
-	if (cmd == cmdset->program)
-		info->ndcb0 |= NDCB0_CMD_TYPE(1) | NDCB0_AUTO_RS;
+	uint32_t ndcr;
 
-	return 0;
-}
+	ndcr = info->reg_ndcr;
+	ndcr |= info->use_ecc ? NDCR_ECC_EN : 0;
+	ndcr |= info->use_dma ? NDCR_DMA_EN : 0;
+	ndcr |= NDCR_ND_RUN;
 
-static int prepare_erase_cmd(struct pxa3xx_nand_info *info,
-			uint16_t cmd, int page_addr)
-{
-	info->ndcb0 = cmd | ((cmd & 0xff00) ? NDCB0_DBC : 0);
-	info->ndcb0 |= NDCB0_CMD_TYPE(2) | NDCB0_AUTO_RS | NDCB0_ADDR_CYC(3);
-	info->ndcb1 = page_addr;
-	info->ndcb2 = 0;
-	return 0;
+	/* clear status bits and run */
+	nand_writel(info, NDCR, 0);
+	nand_writel(info, NDSR, NDSR_MASK);
+	nand_writel(info, NDCR, ndcr);
 }
 
-static int prepare_other_cmd(struct pxa3xx_nand_info *info, uint16_t cmd)
+static void pxa3xx_nand_stop(struct pxa3xx_nand_info *info)
 {
-	const struct pxa3xx_nand_cmdset *cmdset = info->cmdset;
-
-	info->ndcb0 = cmd | ((cmd & 0xff00) ? NDCB0_DBC : 0);
-	info->ndcb1 = 0;
-	info->ndcb2 = 0;
+	uint32_t ndcr;
+	int timeout = NAND_STOP_DELAY;
 
-	info->oob_size = 0;
-	if (cmd == cmdset->read_id) {
-		info->ndcb0 |= NDCB0_CMD_TYPE(3);
-		info->data_size = 8;
-	} else if (cmd == cmdset->read_status) {
-		info->ndcb0 |= NDCB0_CMD_TYPE(4);
-		info->data_size = 8;
-	} else if (cmd == cmdset->reset || cmd == cmdset->lock ||
-		   cmd == cmdset->unlock) {
-		info->ndcb0 |= NDCB0_CMD_TYPE(5);
-	} else
-		return -EINVAL;
+	/* wait RUN bit in NDCR become 0 */
+	ndcr = nand_readl(info, NDCR);
+	while ((ndcr & NDCR_ND_RUN) && (timeout-- > 0)) {
+		ndcr = nand_readl(info, NDCR);
+		udelay(1);
+	}
 
-	return 0;
+	if (timeout <= 0) {
+		ndcr &= ~NDCR_ND_RUN;
+		nand_writel(info, NDCR, ndcr);
+	}
+	/* clear status bits */
+	nand_writel(info, NDSR, NDSR_MASK);
 }
 
 static void enable_int(struct pxa3xx_nand_info *info, uint32_t int_mask)
@@ -372,39 +341,8 @@ static void disable_int(struct pxa3xx_nand_info *info, uint32_t int_mask)
 	nand_writel(info, NDCR, ndcr | int_mask);
 }
 
-/* NOTE: it is a must to set ND_RUN firstly, then write command buffer
- * otherwise, it does not work
- */
-static int write_cmd(struct pxa3xx_nand_info *info)
+static void handle_data_pio(struct pxa3xx_nand_info *info)
 {
-	uint32_t ndcr;
-
-	/* clear status bits and run */
-	nand_writel(info, NDSR, NDSR_MASK);
-
-	ndcr = info->reg_ndcr;
-
-	ndcr |= info->use_ecc ? NDCR_ECC_EN : 0;
-	ndcr |= info->use_dma ? NDCR_DMA_EN : 0;
-	ndcr |= NDCR_ND_RUN;
-
-	nand_writel(info, NDCR, ndcr);
-
-	if (wait_for_event(info, NDSR_WRCMDREQ)) {
-		printk(KERN_ERR "timed out writing command\n");
-		return -ETIMEDOUT;
-	}
-
-	nand_writel(info, NDCB0, info->ndcb0);
-	nand_writel(info, NDCB0, info->ndcb1);
-	nand_writel(info, NDCB0, info->ndcb2);
-	return 0;
-}
-
-static int handle_data_pio(struct pxa3xx_nand_info *info)
-{
-	int ret, timeout = CHIP_DELAY_TIMEOUT;
-
 	switch (info->state) {
 	case STATE_PIO_WRITING:
 		__raw_writesl(info->mmio_base + NDDB, info->data_buff,
@@ -412,14 +350,6 @@ static int handle_data_pio(struct pxa3xx_nand_info *info)
 		if (info->oob_size > 0)
 			__raw_writesl(info->mmio_base + NDDB, info->oob_buff,
 					DIV_ROUND_UP(info->oob_size, 4));
-
-		enable_int(info, NDSR_CS0_BBD | NDSR_CS0_CMDD);
-
-		ret = wait_for_completion_timeout(&info->cmd_complete, timeout);
-		if (!ret) {
-			printk(KERN_ERR "program command time out\n");
-			return -1;
-		}
 		break;
 	case STATE_PIO_READING:
 		__raw_readsl(info->mmio_base + NDDB, info->data_buff,
@@ -431,14 +361,11 @@ static int handle_data_pio(struct pxa3xx_nand_info *info)
 	default:
 		printk(KERN_ERR "%s: invalid state %d\n", __func__,
 				info->state);
-		return -EINVAL;
+		BUG();
 	}
-
-	info->state = STATE_READY;
-	return 0;
 }
 
-static void start_data_dma(struct pxa3xx_nand_info *info, int dir_out)
+static void start_data_dma(struct pxa3xx_nand_info *info)
 {
 	struct pxa_dma_desc *desc = info->data_desc;
 	int dma_len = ALIGN(info->data_size + info->oob_size, 32);
@@ -446,14 +373,21 @@ static void start_data_dma(struct pxa3xx_nand_info *info, int dir_out)
 	desc->ddadr = DDADR_STOP;
 	desc->dcmd = DCMD_ENDIRQEN | DCMD_WIDTH4 | DCMD_BURST32 | dma_len;
 
-	if (dir_out) {
+	switch (info->state) {
+	case STATE_DMA_WRITING:
 		desc->dsadr = info->data_buff_phys;
 		desc->dtadr = info->mmio_phys + NDDB;
 		desc->dcmd |= DCMD_INCSRCADDR | DCMD_FLOWTRG;
-	} else {
+		break;
+	case STATE_DMA_READING:
 		desc->dtadr = info->data_buff_phys;
 		desc->dsadr = info->mmio_phys + NDDB;
 		desc->dcmd |= DCMD_INCTRGADDR | DCMD_FLOWSRC;
+		break;
+	default:
+		printk(KERN_ERR "%s: invalid state %d\n", __func__,
+				info->state);
+		BUG();
 	}
 
 	DRCMR(info->drcmr_dat) = DRCMR_MAPVLD | info->data_dma_ch;
@@ -471,93 +405,62 @@ static void pxa3xx_nand_data_dma_irq(int channel, void *data)
 
 	if (dcsr & DCSR_BUSERR) {
 		info->retcode = ERR_DMABUSERR;
-		complete(&info->cmd_complete);
 	}
 
-	if (info->state == STATE_DMA_WRITING) {
-		info->state = STATE_DMA_DONE;
-		enable_int(info, NDSR_CS0_BBD | NDSR_CS0_CMDD);
-	} else {
-		info->state = STATE_READY;
-		complete(&info->cmd_complete);
-	}
+	info->state = STATE_DMA_DONE;
+	enable_int(info, NDCR_INT_MASK);
+	nand_writel(info, NDSR, NDSR_WRDREQ | NDSR_RDDREQ);
 }
 
 static irqreturn_t pxa3xx_nand_irq(int irq, void *devid)
 {
 	struct pxa3xx_nand_info *info = devid;
-	unsigned int status;
+	unsigned int status, is_completed = 0;
 
 	status = nand_readl(info, NDSR);
 
-	if (status & (NDSR_RDDREQ | NDSR_DBERR | NDSR_SBERR)) {
-		if (status & NDSR_DBERR)
-			info->retcode = ERR_DBERR;
-		else if (status & NDSR_SBERR)
-			info->retcode = ERR_SBERR;
-
-		disable_int(info, NDSR_RDDREQ | NDSR_DBERR | NDSR_SBERR);
-
-		if (info->use_dma) {
-			info->state = STATE_DMA_READING;
-			start_data_dma(info, 0);
-		} else {
-			info->state = STATE_PIO_READING;
-			complete(&info->cmd_complete);
-		}
-	} else if (status & NDSR_WRDREQ) {
-		disable_int(info, NDSR_WRDREQ);
+	if (status & NDSR_DBERR)
+		info->retcode = ERR_DBERR;
+	if (status & NDSR_SBERR)
+		info->retcode = ERR_SBERR;
+	if (status & (NDSR_RDDREQ | NDSR_WRDREQ)) {
+		/* whether use dma to transfer data */
 		if (info->use_dma) {
-			info->state = STATE_DMA_WRITING;
-			start_data_dma(info, 1);
+			disable_int(info, NDCR_INT_MASK);
+			info->state = (status & NDSR_RDDREQ) ?
+				      STATE_DMA_READING : STATE_DMA_WRITING;
+			start_data_dma(info);
+			goto NORMAL_IRQ_EXIT;
 		} else {
-			info->state = STATE_PIO_WRITING;
-			complete(&info->cmd_complete);
+			info->state = (status & NDSR_RDDREQ) ?
+				      STATE_PIO_READING : STATE_PIO_WRITING;
+			handle_data_pio(info);
 		}
-	} else if (status & (NDSR_CS0_BBD | NDSR_CS0_CMDD)) {
-		if (status & NDSR_CS0_BBD)
-			info->retcode = ERR_BBERR;
-
-		disable_int(info, NDSR_CS0_BBD | NDSR_CS0_CMDD);
-		info->state = STATE_READY;
-		complete(&info->cmd_complete);
 	}
-	nand_writel(info, NDSR, status);
-	return IRQ_HANDLED;
-}
-
-static int pxa3xx_nand_do_cmd(struct pxa3xx_nand_info *info, uint32_t event)
-{
-	uint32_t ndcr;
-	int ret, timeout = CHIP_DELAY_TIMEOUT;
-
-	if (write_cmd(info)) {
-		info->retcode = ERR_SENDCMD;
-		goto fail_stop;
+	if (status & NDSR_CS0_CMDD) {
+		info->state = STATE_CMD_DONE;
+		is_completed = 1;
 	}
-
-	info->state = STATE_CMD_HANDLE;
-
-	enable_int(info, event);
-
-	ret = wait_for_completion_timeout(&info->cmd_complete, timeout);
-	if (!ret) {
-		printk(KERN_ERR "command execution timed out\n");
-		info->retcode = ERR_SENDCMD;
-		goto fail_stop;
+	if (status & NDSR_FLASH_RDY) {
+		info->is_ready = 1;
+		info->state = STATE_READY;
 	}
 
-	if (info->use_dma == 0 && info->data_size > 0)
-		if (handle_data_pio(info))
-			goto fail_stop;
-
-	return 0;
+	if (status & NDSR_WRCMDREQ) {
+		nand_writel(info, NDSR, NDSR_WRCMDREQ);
+		status &= ~NDSR_WRCMDREQ;
+		info->state = STATE_CMD_HANDLE;
+		nand_writel(info, NDCB0, info->ndcb0);
+		nand_writel(info, NDCB0, info->ndcb1);
+		nand_writel(info, NDCB0, info->ndcb2);
+	}
 
-fail_stop:
-	ndcr = nand_readl(info, NDCR);
-	nand_writel(info, NDCR, ndcr & ~NDCR_ND_RUN);
-	udelay(10);
-	return -ETIMEDOUT;
+	/* clear NDSR to let the controller exit the IRQ */
+	nand_writel(info, NDSR, status);
+	if (is_completed)
+		complete(&info->cmd_complete);
+NORMAL_IRQ_EXIT:
+	return IRQ_HANDLED;
 }
 
 static int pxa3xx_nand_dev_ready(struct mtd_info *mtd)
@@ -574,125 +477,218 @@ static inline int is_buf_blank(uint8_t *buf, size_t len)
 	return 1;
 }
 
-static void pxa3xx_nand_cmdfunc(struct mtd_info *mtd, unsigned command,
-				int column, int page_addr)
+static int prepare_command_pool(struct pxa3xx_nand_info *info, int command,
+		uint16_t column, int page_addr)
 {
-	struct pxa3xx_nand_info *info = mtd->priv;
-	const struct pxa3xx_nand_cmdset *cmdset = info->cmdset;
-	int ret;
+	uint16_t cmd;
+	int addr_cycle, exec_cmd, ndcb0;
+	struct mtd_info *mtd = info->mtd;
+
+	ndcb0 = 0;
+	addr_cycle = 0;
+	exec_cmd = 1;
+
+	/* reset data and oob column point to handle data */
+	info->buf_start		= 0;
+	info->buf_count		= 0;
+	info->oob_size		= 0;
+	info->use_ecc		= 0;
+	info->is_ready		= 0;
+	info->retcode		= ERR_NONE;
 
-	info->use_dma = (use_dma) ? 1 : 0;
-	info->use_ecc = 0;
-	info->data_size = 0;
-	info->state = STATE_READY;
+	switch (command) {
+	case NAND_CMD_READ0:
+	case NAND_CMD_PAGEPROG:
+		info->use_ecc = 1;
+	case NAND_CMD_READOOB:
+		pxa3xx_set_datasize(info);
+		break;
+	case NAND_CMD_SEQIN:
+		exec_cmd = 0;
+		break;
+	default:
+		info->ndcb1 = 0;
+		info->ndcb2 = 0;
+		break;
+	}
 
-	init_completion(&info->cmd_complete);
+	info->ndcb0 = ndcb0;
+	addr_cycle = NDCB0_ADDR_CYC(info->row_addr_cycles
+				    + info->col_addr_cycles);
 
 	switch (command) {
 	case NAND_CMD_READOOB:
-		/* disable HW ECC to get all the OOB data */
-		info->buf_count = mtd->writesize + mtd->oobsize;
-		info->buf_start = mtd->writesize + column;
-		memset(info->data_buff, 0xFF, info->buf_count);
+	case NAND_CMD_READ0:
+		cmd = info->cmdset->read1;
+		if (command == NAND_CMD_READOOB)
+			info->buf_start = mtd->writesize + column;
+		else
+			info->buf_start = column;
 
-		if (prepare_read_prog_cmd(info, cmdset->read1, column, page_addr))
-			break;
+		if (unlikely(info->page_size < PAGE_CHUNK_SIZE))
+			info->ndcb0 |= NDCB0_CMD_TYPE(0)
+					| addr_cycle
+					| (cmd & NDCB0_CMD1_MASK);
+		else
+			info->ndcb0 |= NDCB0_CMD_TYPE(0)
+					| NDCB0_DBC
+					| addr_cycle
+					| cmd;
 
-		pxa3xx_nand_do_cmd(info, NDSR_RDDREQ | NDSR_DBERR | NDSR_SBERR);
+	case NAND_CMD_SEQIN:
+		/* small page addr setting */
+		if (unlikely(info->page_size < PAGE_CHUNK_SIZE)) {
+			info->ndcb1 = ((page_addr & 0xFFFFFF) << 8)
+					| (column & 0xFF);
 
-		/* We only are OOB, so if the data has error, does not matter */
-		if (info->retcode == ERR_DBERR)
-			info->retcode = ERR_NONE;
-		break;
+			info->ndcb2 = 0;
+		} else {
+			info->ndcb1 = ((page_addr & 0xFFFF) << 16)
+					| (column & 0xFFFF);
+
+			if (page_addr & 0xFF0000)
+				info->ndcb2 = (page_addr & 0xFF0000) >> 16;
+			else
+				info->ndcb2 = 0;
+		}
 
-	case NAND_CMD_READ0:
-		info->use_ecc = 1;
-		info->retcode = ERR_NONE;
-		info->buf_start = column;
 		info->buf_count = mtd->writesize + mtd->oobsize;
 		memset(info->data_buff, 0xFF, info->buf_count);
 
-		if (prepare_read_prog_cmd(info, cmdset->read1, column, page_addr))
+		break;
+
+	case NAND_CMD_PAGEPROG:
+		if (is_buf_blank(info->data_buff,
+					(mtd->writesize + mtd->oobsize))) {
+			exec_cmd = 0;
 			break;
+		}
 
-		pxa3xx_nand_do_cmd(info, NDSR_RDDREQ | NDSR_DBERR | NDSR_SBERR);
+		cmd = info->cmdset->program;
+		info->ndcb0 |= NDCB0_CMD_TYPE(0x1)
+				| NDCB0_AUTO_RS
+				| NDCB0_ST_ROW_EN
+				| NDCB0_DBC
+				| cmd
+				| addr_cycle;
+		break;
 
-		if (info->retcode == ERR_DBERR) {
-			/* for blank page (all 0xff), HW will calculate its ECC as
-			 * 0, which is different from the ECC information within
-			 * OOB, ignore such double bit errors
-			 */
-			if (is_buf_blank(info->data_buff, mtd->writesize))
-				info->retcode = ERR_NONE;
-		}
+	case NAND_CMD_READID:
+		cmd = info->cmdset->read_id;
+		info->buf_count = info->read_id_bytes;
+		info->ndcb0 |= NDCB0_CMD_TYPE(3)
+				| NDCB0_ADDR_CYC(1)
+				| cmd;
+
+		info->data_size = 8;
 		break;
-	case NAND_CMD_SEQIN:
-		info->buf_start = column;
-		info->buf_count = mtd->writesize + mtd->oobsize;
-		memset(info->data_buff, 0xff, info->buf_count);
+	case NAND_CMD_STATUS:
+		cmd = info->cmdset->read_status;
+		info->buf_count = 1;
+		info->ndcb0 |= NDCB0_CMD_TYPE(4)
+				| NDCB0_ADDR_CYC(1)
+				| cmd;
 
-		/* save column/page_addr for next CMD_PAGEPROG */
-		info->seqin_column = column;
-		info->seqin_page_addr = page_addr;
+		info->data_size = 8;
 		break;
-	case NAND_CMD_PAGEPROG:
-		info->use_ecc = (info->seqin_column >= mtd->writesize) ? 0 : 1;
 
-		if (prepare_read_prog_cmd(info, cmdset->program,
-				info->seqin_column, info->seqin_page_addr))
-			break;
+	case NAND_CMD_ERASE1:
+		cmd = info->cmdset->erase;
+		info->ndcb0 |= NDCB0_CMD_TYPE(2)
+				| NDCB0_AUTO_RS
+				| NDCB0_ADDR_CYC(3)
+				| NDCB0_DBC
+				| cmd;
+		info->ndcb1 = page_addr;
+		info->ndcb2 = 0;
 
-		pxa3xx_nand_do_cmd(info, NDSR_WRDREQ);
 		break;
-	case NAND_CMD_ERASE1:
-		if (prepare_erase_cmd(info, cmdset->erase, page_addr))
-			break;
+	case NAND_CMD_RESET:
+		cmd = info->cmdset->reset;
+		info->ndcb0 |= NDCB0_CMD_TYPE(5)
+				| cmd;
 
-		pxa3xx_nand_do_cmd(info, NDSR_CS0_BBD | NDSR_CS0_CMDD);
 		break;
+
 	case NAND_CMD_ERASE2:
+		exec_cmd = 0;
 		break;
-	case NAND_CMD_READID:
-	case NAND_CMD_STATUS:
-		info->use_dma = 0;	/* force PIO read */
-		info->buf_start = 0;
-		info->buf_count = (command == NAND_CMD_READID) ?
-				info->read_id_bytes : 1;
-
-		if (prepare_other_cmd(info, (command == NAND_CMD_READID) ?
-				cmdset->read_id : cmdset->read_status))
-			break;
 
-		pxa3xx_nand_do_cmd(info, NDSR_RDDREQ);
+	default:
+		exec_cmd = 0;
+		printk(KERN_ERR "pxa3xx-nand: non-supported"
+			" command %x\n", command);
 		break;
-	case NAND_CMD_RESET:
-		if (prepare_other_cmd(info, cmdset->reset))
-			break;
+	}
 
-		ret = pxa3xx_nand_do_cmd(info, NDSR_CS0_CMDD);
-		if (ret == 0) {
-			int timeout = 2;
-			uint32_t ndcr;
+	return exec_cmd;
+}
 
-			while (timeout--) {
-				if (nand_readl(info, NDSR) & NDSR_RDY)
-					break;
-				msleep(10);
-			}
+static void pxa3xx_nand_cmdfunc(struct mtd_info *mtd, unsigned command,
+				int column, int page_addr)
+{
+	struct pxa3xx_nand_info *info = mtd->priv;
+	int ret, exec_cmd;
 
-			ndcr = nand_readl(info, NDCR);
-			nand_writel(info, NDCR, ndcr & ~NDCR_ND_RUN);
+	/*
+	 * if this is a x16 device ,then convert the input
+	 * "byte" address into a "word" address appropriate
+	 * for indexing a word-oriented device
+	 */
+	if (info->reg_ndcr & NDCR_DWIDTH_M)
+		column /= 2;
+
+	exec_cmd = prepare_command_pool(info, command, column, page_addr);
+	if (exec_cmd) {
+		init_completion(&info->cmd_complete);
+		pxa3xx_nand_start(info);
+
+		ret = wait_for_completion_timeout(&info->cmd_complete,
+				CHIP_DELAY_TIMEOUT);
+		if (!ret) {
+			printk(KERN_ERR "Wait time out!!!\n");
+			/* Stop State Machine for next command cycle */
+			pxa3xx_nand_stop(info);
 		}
-		break;
-	default:
-		printk(KERN_ERR "non-supported command.\n");
-		break;
+		info->state = STATE_IDLE;
 	}
+}
+
+static void pxa3xx_nand_write_page_hwecc(struct mtd_info *mtd,
+		struct nand_chip *chip, const uint8_t *buf)
+{
+	chip->write_buf(mtd, buf, mtd->writesize);
+	chip->write_buf(mtd, chip->oob_poi, mtd->oobsize);
+}
 
-	if (info->retcode == ERR_DBERR) {
-		printk(KERN_ERR "double bit error @ page %08x\n", page_addr);
-		info->retcode = ERR_NONE;
+static int pxa3xx_nand_read_page_hwecc(struct mtd_info *mtd,
+		struct nand_chip *chip, uint8_t *buf, int page)
+{
+	struct pxa3xx_nand_info *info = mtd->priv;
+
+	chip->read_buf(mtd, buf, mtd->writesize);
+	chip->read_buf(mtd, chip->oob_poi, mtd->oobsize);
+
+	if (info->retcode == ERR_SBERR) {
+		switch (info->use_ecc) {
+		case 1:
+			mtd->ecc_stats.corrected++;
+			break;
+		case 0:
+		default:
+			break;
+		}
+	} else if (info->retcode == ERR_DBERR) {
+		/*
+		 * for blank page (all 0xff), HW will calculate its ECC as
+		 * 0, which is different from the ECC information within
+		 * OOB, ignore such double bit errors
+		 */
+		if (is_buf_blank(buf, mtd->writesize))
+			mtd->ecc_stats.failed++;
 	}
+
+	return 0;
 }
 
 static uint8_t pxa3xx_nand_read_byte(struct mtd_info *mtd)
@@ -769,73 +765,12 @@ static int pxa3xx_nand_waitfunc(struct mtd_info *mtd, struct nand_chip *this)
 	return 0;
 }
 
-static void pxa3xx_nand_ecc_hwctl(struct mtd_info *mtd, int mode)
-{
-	return;
-}
-
-static int pxa3xx_nand_ecc_calculate(struct mtd_info *mtd,
-		const uint8_t *dat, uint8_t *ecc_code)
-{
-	return 0;
-}
-
-static int pxa3xx_nand_ecc_correct(struct mtd_info *mtd,
-		uint8_t *dat, uint8_t *read_ecc, uint8_t *calc_ecc)
-{
-	struct pxa3xx_nand_info *info = mtd->priv;
-	/*
-	 * Any error include ERR_SEND_CMD, ERR_DBERR, ERR_BUSERR, we
-	 * consider it as a ecc error which will tell the caller the
-	 * read fail We have distinguish all the errors, but the
-	 * nand_read_ecc only check this function return value
-	 *
-	 * Corrected (single-bit) errors must also be noted.
-	 */
-	if (info->retcode == ERR_SBERR)
-		return 1;
-	else if (info->retcode != ERR_NONE)
-		return -1;
-
-	return 0;
-}
-
-static int __readid(struct pxa3xx_nand_info *info, uint32_t *id)
-{
-	const struct pxa3xx_nand_cmdset *cmdset = info->cmdset;
-	uint32_t ndcr;
-	uint8_t  id_buff[8];
-
-	if (prepare_other_cmd(info, cmdset->read_id)) {
-		printk(KERN_ERR "failed to prepare command\n");
-		return -EINVAL;
-	}
-
-	/* Send command */
-	if (write_cmd(info))
-		goto fail_timeout;
-
-	/* Wait for CMDDM(command done successfully) */
-	if (wait_for_event(info, NDSR_RDDREQ))
-		goto fail_timeout;
-
-	__raw_readsl(info->mmio_base + NDDB, id_buff, 2);
-	*id = id_buff[0] | (id_buff[1] << 8);
-	return 0;
-
-fail_timeout:
-	ndcr = nand_readl(info, NDCR);
-	nand_writel(info, NDCR, ndcr & ~NDCR_ND_RUN);
-	udelay(10);
-	return -ETIMEDOUT;
-}
-
 static int pxa3xx_nand_config_flash(struct pxa3xx_nand_info *info,
 				    const struct pxa3xx_nand_flash *f)
 {
 	struct platform_device *pdev = info->pdev;
 	struct pxa3xx_nand_platform_data *pdata = pdev->dev.platform_data;
-	uint32_t ndcr = 0x00000FFF; /* disable all interrupts */
+	uint32_t ndcr = 0x0; /* enable all interrupts */
 
 	if (f->page_size != 2048 && f->page_size != 512)
 		return -EINVAL;
@@ -844,9 +779,8 @@ static int pxa3xx_nand_config_flash(struct pxa3xx_nand_info *info,
 		return -EINVAL;
 
 	/* calculate flash information */
-	info->cmdset = f->cmdset;
+	info->cmdset = &default_cmdset;
 	info->page_size = f->page_size;
-	info->oob_buff = info->data_buff + f->page_size;
 	info->read_id_bytes = (f->page_size == 2048) ? 4 : 2;
 
 	/* calculate addressing information */
@@ -876,87 +810,18 @@ static int pxa3xx_nand_config_flash(struct pxa3xx_nand_info *info,
 static int pxa3xx_nand_detect_config(struct pxa3xx_nand_info *info)
 {
 	uint32_t ndcr = nand_readl(info, NDCR);
-	struct nand_flash_dev *type = NULL;
-	uint32_t id = -1, page_per_block, num_blocks;
-	int i;
-
-	page_per_block = ndcr & NDCR_PG_PER_BLK ? 64 : 32;
 	info->page_size = ndcr & NDCR_PAGE_SZ ? 2048 : 512;
-	/* set info fields needed to __readid */
+	/* set info fields needed to read id */
 	info->read_id_bytes = (info->page_size == 2048) ? 4 : 2;
 	info->reg_ndcr = ndcr;
 	info->cmdset = &default_cmdset;
 
-	if (__readid(info, &id))
-		return -ENODEV;
-
-	/* Lookup the flash id */
-	id = (id >> 8) & 0xff;		/* device id is byte 2 */
-	for (i = 0; nand_flash_ids[i].name != NULL; i++) {
-		if (id == nand_flash_ids[i].id) {
-			type =  &nand_flash_ids[i];
-			break;
-		}
-	}
-
-	if (!type)
-		return -ENODEV;
-
-	/* fill the missing flash information */
-	i = __ffs(page_per_block * info->page_size);
-	num_blocks = type->chipsize << (20 - i);
-
-	/* calculate addressing information */
-	info->col_addr_cycles = (info->page_size == 2048) ? 2 : 1;
-
-	if (num_blocks * page_per_block > 65536)
-		info->row_addr_cycles = 3;
-	else
-		info->row_addr_cycles = 2;
-
 	info->ndtr0cs0 = nand_readl(info, NDTR0CS0);
 	info->ndtr1cs0 = nand_readl(info, NDTR1CS0);
 
 	return 0;
 }
 
-static int pxa3xx_nand_detect_flash(struct pxa3xx_nand_info *info,
-				    const struct pxa3xx_nand_platform_data *pdata)
-{
-	const struct pxa3xx_nand_flash *f;
-	uint32_t id = -1;
-	int i;
-
-	if (pdata->keep_config)
-		if (pxa3xx_nand_detect_config(info) == 0)
-			return 0;
-
-	/* we use default timing to detect id */
-	f = DEFAULT_FLASH_TYPE;
-	pxa3xx_nand_config_flash(info, f);
-	if (__readid(info, &id))
-		goto fail_detect;
-
-	for (i=0; i<ARRAY_SIZE(builtin_flash_types) + pdata->num_flash - 1; i++) {
-		/* we first choose the flash definition from platfrom */
-		if (i < pdata->num_flash)
-			f = pdata->flash + i;
-		else
-			f = &builtin_flash_types[i - pdata->num_flash + 1];
-		if (f->chip_id == id) {
-			dev_info(&info->pdev->dev, "detect chip id: 0x%x\n", id);
-			pxa3xx_nand_config_flash(info, f);
-			return 0;
-		}
-	}
-
-	dev_warn(&info->pdev->dev,
-		 "failed to detect configured nand flash; found %04x instead of\n",
-		 id);
-fail_detect:
-	return -ENODEV;
-}
-
 /* the maximum possible buffer size for large page with OOB data
  * is: 2048 + 64 = 2112 bytes, allocate a page here for both the
  * data buffer and the DMA descriptor
@@ -998,82 +863,144 @@ static int pxa3xx_nand_init_buff(struct pxa3xx_nand_info *info)
 	return 0;
 }
 
-static struct nand_ecclayout hw_smallpage_ecclayout = {
-	.eccbytes = 6,
-	.eccpos = {8, 9, 10, 11, 12, 13 },
-	.oobfree = { {2, 6} }
-};
+static int pxa3xx_nand_sensing(struct pxa3xx_nand_info *info)
+{
+	struct mtd_info *mtd = info->mtd;
+	struct nand_chip *chip = mtd->priv;
 
-static struct nand_ecclayout hw_largepage_ecclayout = {
-	.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} }
-};
+	/* use the common timing to make a try */
+	pxa3xx_nand_config_flash(info, &builtin_flash_types[0]);
+	chip->cmdfunc(mtd, NAND_CMD_RESET, 0, 0);
+	if (info->is_ready)
+		return 1;
+	else
+		return 0;
+}
 
-static void pxa3xx_nand_init_mtd(struct mtd_info *mtd,
-				 struct pxa3xx_nand_info *info)
+static int pxa3xx_nand_scan(struct mtd_info *mtd)
 {
-	struct nand_chip *this = &info->nand_chip;
-
-	this->options = (info->reg_ndcr & NDCR_DWIDTH_C) ? NAND_BUSWIDTH_16: 0;
-
-	this->waitfunc		= pxa3xx_nand_waitfunc;
-	this->select_chip	= pxa3xx_nand_select_chip;
-	this->dev_ready		= pxa3xx_nand_dev_ready;
-	this->cmdfunc		= pxa3xx_nand_cmdfunc;
-	this->read_word		= pxa3xx_nand_read_word;
-	this->read_byte		= pxa3xx_nand_read_byte;
-	this->read_buf		= pxa3xx_nand_read_buf;
-	this->write_buf		= pxa3xx_nand_write_buf;
-	this->verify_buf	= pxa3xx_nand_verify_buf;
-
-	this->ecc.mode		= NAND_ECC_HW;
-	this->ecc.hwctl		= pxa3xx_nand_ecc_hwctl;
-	this->ecc.calculate	= pxa3xx_nand_ecc_calculate;
-	this->ecc.correct	= pxa3xx_nand_ecc_correct;
-	this->ecc.size		= info->page_size;
-
-	if (info->page_size == 2048)
-		this->ecc.layout = &hw_largepage_ecclayout;
+	struct pxa3xx_nand_info *info = mtd->priv;
+	struct platform_device *pdev = info->pdev;
+	struct pxa3xx_nand_platform_data *pdata = pdev->dev.platform_data;
+	struct nand_flash_dev pxa3xx_flash_ids[2] = { {NULL,}, {NULL,} };
+	const struct pxa3xx_nand_flash *f = NULL;
+	struct nand_chip *chip = mtd->priv;
+	uint32_t id = -1;
+	uint64_t chipsize;
+	int i, ret, num;
+
+	if (pdata->keep_config && !pxa3xx_nand_detect_config(info))
+		goto KEEP_CONFIG;
+
+	ret = pxa3xx_nand_sensing(info);
+	if (!ret) {
+		kfree(mtd);
+		info->mtd = NULL;
+		printk(KERN_INFO "There is no nand chip on cs 0!\n");
+
+		return -EINVAL;
+	}
+
+	chip->cmdfunc(mtd, NAND_CMD_READID, 0, 0);
+	id = *((uint16_t *)(info->data_buff));
+	if (id != 0)
+		printk(KERN_INFO "Detect a flash id %x\n", id);
+	else {
+		kfree(mtd);
+		info->mtd = NULL;
+		printk(KERN_WARNING "Read out ID 0, potential timing set wrong!!\n");
+
+		return -EINVAL;
+	}
+
+	num = ARRAY_SIZE(builtin_flash_types) + pdata->num_flash - 1;
+	for (i = 0; i < num; i++) {
+		if (i < pdata->num_flash)
+			f = pdata->flash + i;
+		else
+			f = &builtin_flash_types[i - pdata->num_flash + 1];
+
+		/* find the chip in default list */
+		if (f->chip_id == id)
+			break;
+	}
+
+	if (i >= (ARRAY_SIZE(builtin_flash_types) + pdata->num_flash - 1)) {
+		kfree(mtd);
+		info->mtd = NULL;
+		printk(KERN_ERR "ERROR!! flash not defined!!!\n");
+
+		return -EINVAL;
+	}
+
+	pxa3xx_nand_config_flash(info, f);
+	pxa3xx_flash_ids[0].name = f->name;
+	pxa3xx_flash_ids[0].id = (f->chip_id >> 8) & 0xffff;
+	pxa3xx_flash_ids[0].pagesize = f->page_size;
+	chipsize = (uint64_t)f->num_blocks * f->page_per_block * f->page_size;
+	pxa3xx_flash_ids[0].chipsize = chipsize >> 20;
+	pxa3xx_flash_ids[0].erasesize = f->page_size * f->page_per_block;
+	if (f->flash_width == 16)
+		pxa3xx_flash_ids[0].options = NAND_BUSWIDTH_16;
+KEEP_CONFIG:
+	if (nand_scan_ident(mtd, 1, pxa3xx_flash_ids))
+		return -ENODEV;
+	/* calculate addressing information */
+	info->col_addr_cycles = (mtd->writesize >= 2048) ? 2 : 1;
+	info->oob_buff = info->data_buff + mtd->writesize;
+	if ((mtd->size >> chip->page_shift) > 65536)
+		info->row_addr_cycles = 3;
 	else
-		this->ecc.layout = &hw_smallpage_ecclayout;
+		info->row_addr_cycles = 2;
+	mtd->name = mtd_names[0];
+	chip->ecc.mode = NAND_ECC_HW;
+	chip->ecc.size = f->page_size;
+
+	chip->options = (f->flash_width == 16) ? NAND_BUSWIDTH_16 : 0;
+	chip->options |= NAND_NO_AUTOINCR;
+	chip->options |= NAND_NO_READRDY;
 
-	this->chip_delay = 25;
+	return nand_scan_tail(mtd);
 }
 
-static int pxa3xx_nand_probe(struct platform_device *pdev)
+static
+struct pxa3xx_nand_info *alloc_nand_resource(struct platform_device *pdev)
 {
-	struct pxa3xx_nand_platform_data *pdata;
 	struct pxa3xx_nand_info *info;
-	struct nand_chip *this;
+	struct nand_chip *chip;
 	struct mtd_info *mtd;
 	struct resource *r;
-	int ret = 0, irq;
-
-	pdata = pdev->dev.platform_data;
-
-	if (!pdata) {
-		dev_err(&pdev->dev, "no platform data defined\n");
-		return -ENODEV;
-	}
+	int ret, irq;
 
 	mtd = kzalloc(sizeof(struct mtd_info) + sizeof(struct pxa3xx_nand_info),
 			GFP_KERNEL);
 	if (!mtd) {
 		dev_err(&pdev->dev, "failed to allocate memory\n");
-		return -ENOMEM;
+		return NULL;
 	}
 
 	info = (struct pxa3xx_nand_info *)(&mtd[1]);
+	chip = (struct nand_chip *)(&mtd[1]);
 	info->pdev = pdev;
-
-	this = &info->nand_chip;
+	info->mtd = mtd;
 	mtd->priv = info;
 	mtd->owner = THIS_MODULE;
 
+	chip->ecc.read_page	= pxa3xx_nand_read_page_hwecc;
+	chip->ecc.write_page	= pxa3xx_nand_write_page_hwecc;
+	chip->controller        = &info->controller;
+	chip->waitfunc		= pxa3xx_nand_waitfunc;
+	chip->select_chip	= pxa3xx_nand_select_chip;
+	chip->dev_ready		= pxa3xx_nand_dev_ready;
+	chip->cmdfunc		= pxa3xx_nand_cmdfunc;
+	chip->read_word		= pxa3xx_nand_read_word;
+	chip->read_byte		= pxa3xx_nand_read_byte;
+	chip->read_buf		= pxa3xx_nand_read_buf;
+	chip->write_buf		= pxa3xx_nand_write_buf;
+	chip->verify_buf	= pxa3xx_nand_verify_buf;
+
+	spin_lock_init(&chip->controller->lock);
+	init_waitqueue_head(&chip->controller->wq);
 	info->clk = clk_get(&pdev->dev, NULL);
 	if (IS_ERR(info->clk)) {
 		dev_err(&pdev->dev, "failed to get nand clock\n");
@@ -1141,43 +1068,12 @@ static int pxa3xx_nand_probe(struct platform_device *pdev)
 		goto fail_free_buf;
 	}
 
-	ret = pxa3xx_nand_detect_flash(info, pdata);
-	if (ret) {
-		dev_err(&pdev->dev, "failed to detect flash\n");
-		ret = -ENODEV;
-		goto fail_free_irq;
-	}
-
-	pxa3xx_nand_init_mtd(mtd, info);
-
-	platform_set_drvdata(pdev, mtd);
-
-	if (nand_scan(mtd, 1)) {
-		dev_err(&pdev->dev, "failed to scan nand\n");
-		ret = -ENXIO;
-		goto fail_free_irq;
-	}
-
-#ifdef CONFIG_MTD_PARTITIONS
-	if (mtd_has_cmdlinepart()) {
-		static const char *probes[] = { "cmdlinepart", NULL };
-		struct mtd_partition *parts;
-		int nr_parts;
-
-		nr_parts = parse_mtd_partitions(mtd, probes, &parts, 0);
-
-		if (nr_parts)
-			return add_mtd_partitions(mtd, parts, nr_parts);
-	}
+	platform_set_drvdata(pdev, info);
 
-	return add_mtd_partitions(mtd, pdata->parts, pdata->nr_parts);
-#else
-	return 0;
-#endif
+	return info;
 
-fail_free_irq:
-	free_irq(irq, info);
 fail_free_buf:
+	free_irq(irq, info);
 	if (use_dma) {
 		pxa_free_dma(info->data_dma_ch);
 		dma_free_coherent(&pdev->dev, info->data_buff_size,
@@ -1193,22 +1089,18 @@ fail_put_clk:
 	clk_put(info->clk);
 fail_free_mtd:
 	kfree(mtd);
-	return ret;
+	return NULL;
 }
 
 static int pxa3xx_nand_remove(struct platform_device *pdev)
 {
-	struct mtd_info *mtd = platform_get_drvdata(pdev);
-	struct pxa3xx_nand_info *info = mtd->priv;
+	struct pxa3xx_nand_info *info = platform_get_drvdata(pdev);
+	struct mtd_info *mtd = info->mtd;
 	struct resource *r;
 	int irq;
 
 	platform_set_drvdata(pdev, NULL);
 
-	del_mtd_device(mtd);
-#ifdef CONFIG_MTD_PARTITIONS
-	del_mtd_partitions(mtd);
-#endif
 	irq = platform_get_irq(pdev, 0);
 	if (irq >= 0)
 		free_irq(irq, info);
@@ -1226,17 +1118,62 @@ static int pxa3xx_nand_remove(struct platform_device *pdev)
 	clk_disable(info->clk);
 	clk_put(info->clk);
 
-	kfree(mtd);
+	if (mtd) {
+		del_mtd_device(mtd);
+#ifdef CONFIG_MTD_PARTITIONS
+		del_mtd_partitions(mtd);
+#endif
+		kfree(mtd);
+	}
 	return 0;
 }
 
+static int pxa3xx_nand_probe(struct platform_device *pdev)
+{
+	struct pxa3xx_nand_platform_data *pdata;
+	struct pxa3xx_nand_info *info;
+
+	pdata = pdev->dev.platform_data;
+	if (!pdata) {
+		dev_err(&pdev->dev, "no platform data defined\n");
+		return -ENODEV;
+	}
+
+	info = alloc_nand_resource(pdev);
+	if (info == NULL)
+		return -ENOMEM;
+
+	if (pxa3xx_nand_scan(info->mtd)) {
+		dev_err(&pdev->dev, "failed to scan nand\n");
+		pxa3xx_nand_remove(pdev);
+		return -ENODEV;
+	}
+
+#ifdef CONFIG_MTD_PARTITIONS
+	if (mtd_has_cmdlinepart()) {
+		const char *probes[] = { "cmdlinepart", NULL };
+		struct mtd_partition *parts;
+		int nr_parts;
+
+		nr_parts = parse_mtd_partitions(info->mtd, probes, &parts, 0);
+
+		if (nr_parts)
+			return add_mtd_partitions(info->mtd, parts, nr_parts);
+	}
+
+	return add_mtd_partitions(info->mtd, pdata->parts, pdata->nr_parts);
+#else
+	return 0;
+#endif
+}
+
 #ifdef CONFIG_PM
 static int pxa3xx_nand_suspend(struct platform_device *pdev, pm_message_t state)
 {
-	struct mtd_info *mtd = (struct mtd_info *)platform_get_drvdata(pdev);
-	struct pxa3xx_nand_info *info = mtd->priv;
+	struct pxa3xx_nand_info *info = platform_get_drvdata(pdev);
+	struct mtd_info *mtd = info->mtd;
 
-	if (info->state != STATE_READY) {
+	if (info->state) {
 		dev_err(&pdev->dev, "driver busy, state = %d\n", info->state);
 		return -EAGAIN;
 	}
@@ -1246,8 +1183,8 @@ static int pxa3xx_nand_suspend(struct platform_device *pdev, pm_message_t state)
 
 static int pxa3xx_nand_resume(struct platform_device *pdev)
 {
-	struct mtd_info *mtd = (struct mtd_info *)platform_get_drvdata(pdev);
-	struct pxa3xx_nand_info *info = mtd->priv;
+	struct pxa3xx_nand_info *info = platform_get_drvdata(pdev);
+	struct mtd_info *mtd = info->mtd;
 
 	nand_writel(info, NDTR0CS0, info->ndtr0cs0);
 	nand_writel(info, NDTR1CS0, info->ndtr1cs0);
diff --git a/drivers/mtd/onenand/omap2.c b/drivers/mtd/onenand/omap2.c
index 14a49abe057e..f591f615d3f6 100644
--- a/drivers/mtd/onenand/omap2.c
+++ b/drivers/mtd/onenand/omap2.c
@@ -629,6 +629,7 @@ static int __devinit omap2_onenand_probe(struct platform_device *pdev)
 {
 	struct omap_onenand_platform_data *pdata;
 	struct omap2_onenand *c;
+	struct onenand_chip *this;
 	int r;
 
 	pdata = pdev->dev.platform_data;
@@ -726,9 +727,8 @@ static int __devinit omap2_onenand_probe(struct platform_device *pdev)
 
 	c->mtd.dev.parent = &pdev->dev;
 
+	this = &c->onenand;
 	if (c->dma_channel >= 0) {
-		struct onenand_chip *this = &c->onenand;
-
 		this->wait = omap2_onenand_wait;
 		if (cpu_is_omap34xx()) {
 			this->read_bufferram = omap3_onenand_read_bufferram;
@@ -749,6 +749,9 @@ static int __devinit omap2_onenand_probe(struct platform_device *pdev)
 		c->onenand.disable = omap2_onenand_disable;
 	}
 
+	if (pdata->skip_initial_unlocking)
+		this->options |= ONENAND_SKIP_INITIAL_UNLOCKING;
+
 	if ((r = onenand_scan(&c->mtd, 1)) < 0)
 		goto err_release_regulator;
 
diff --git a/drivers/mtd/onenand/onenand_base.c b/drivers/mtd/onenand/onenand_base.c
index bac41caa8df7..56a8b2005bda 100644
--- a/drivers/mtd/onenand/onenand_base.c
+++ b/drivers/mtd/onenand/onenand_base.c
@@ -1132,6 +1132,8 @@ static int onenand_mlc_read_ops_nolock(struct mtd_info *mtd, loff_t from,
 			onenand_update_bufferram(mtd, from, !ret);
 			if (ret == -EBADMSG)
 				ret = 0;
+			if (ret)
+				break;
 		}
 
 		this->read_bufferram(mtd, ONENAND_DATARAM, buf, column, thislen);
@@ -1646,11 +1648,10 @@ static int onenand_verify(struct mtd_info *mtd, const u_char *buf, loff_t addr,
 	int ret = 0;
 	int thislen, column;
 
+	column = addr & (this->writesize - 1);
+
 	while (len != 0) {
-		thislen = min_t(int, this->writesize, len);
-		column = addr & (this->writesize - 1);
-		if (column + thislen > this->writesize)
-			thislen = this->writesize - column;
+		thislen = min_t(int, this->writesize - column, len);
 
 		this->command(mtd, ONENAND_CMD_READ, addr, this->writesize);
 
@@ -1664,12 +1665,13 @@ static int onenand_verify(struct mtd_info *mtd, const u_char *buf, loff_t addr,
 
 		this->read_bufferram(mtd, ONENAND_DATARAM, this->verify_buf, 0, mtd->writesize);
 
-		if (memcmp(buf, this->verify_buf, thislen))
+		if (memcmp(buf, this->verify_buf + column, thislen))
 			return -EBADMSG;
 
 		len -= thislen;
 		buf += thislen;
 		addr += thislen;
+		column = 0;
 	}
 
 	return 0;
@@ -4083,7 +4085,8 @@ int onenand_scan(struct mtd_info *mtd, int maxchips)
 	mtd->writebufsize = mtd->writesize;
 
 	/* Unlock whole block */
-	this->unlock_all(mtd);
+	if (!(this->options & ONENAND_SKIP_INITIAL_UNLOCKING))
+		this->unlock_all(mtd);
 
 	ret = this->scan_bbt(mtd);
 	if ((!FLEXONENAND(this)) || ret)
diff --git a/drivers/mtd/sm_ftl.c b/drivers/mtd/sm_ftl.c
index ac0d6a8613b5..2b0daae4018d 100644
--- a/drivers/mtd/sm_ftl.c
+++ b/drivers/mtd/sm_ftl.c
@@ -64,12 +64,16 @@ struct attribute_group *sm_create_sysfs_attributes(struct sm_ftl *ftl)
 					SM_SMALL_PAGE - SM_CIS_VENDOR_OFFSET);
 
 	char *vendor = kmalloc(vendor_len, GFP_KERNEL);
+	if (!vendor)
+		goto error1;
 	memcpy(vendor, ftl->cis_buffer + SM_CIS_VENDOR_OFFSET, vendor_len);
 	vendor[vendor_len] = 0;
 
 	/* Initialize sysfs attributes */
 	vendor_attribute =
 		kzalloc(sizeof(struct sm_sysfs_attribute), GFP_KERNEL);
+	if (!vendor_attribute)
+		goto error2;
 
 	sysfs_attr_init(&vendor_attribute->dev_attr.attr);
 
@@ -83,12 +87,24 @@ struct attribute_group *sm_create_sysfs_attributes(struct sm_ftl *ftl)
 	/* Create array of pointers to the attributes */
 	attributes = kzalloc(sizeof(struct attribute *) * (NUM_ATTRIBUTES + 1),
 								GFP_KERNEL);
+	if (!attributes)
+		goto error3;
 	attributes[0] = &vendor_attribute->dev_attr.attr;
 
 	/* Finally create the attribute group */
 	attr_group = kzalloc(sizeof(struct attribute_group), GFP_KERNEL);
+	if (!attr_group)
+		goto error4;
 	attr_group->attrs = attributes;
 	return attr_group;
+error4:
+	kfree(attributes);
+error3:
+	kfree(vendor_attribute);
+error2:
+	kfree(vendor);
+error1:
+	return NULL;
 }
 
 void sm_delete_sysfs_attributes(struct sm_ftl *ftl)
@@ -1178,6 +1194,8 @@ static void sm_add_mtd(struct mtd_blktrans_ops *tr, struct mtd_info *mtd)
 	}
 
 	ftl->disk_attributes = sm_create_sysfs_attributes(ftl);
+	if (!ftl->disk_attributes)
+		goto error6;
 	trans->disk_attributes = ftl->disk_attributes;
 
 	sm_printk("Found %d MiB xD/SmartMedia FTL on mtd%d",
diff --git a/drivers/mtd/tests/mtd_speedtest.c b/drivers/mtd/tests/mtd_speedtest.c
index 161feeb7b8b9..627d4e2466a3 100644
--- a/drivers/mtd/tests/mtd_speedtest.c
+++ b/drivers/mtd/tests/mtd_speedtest.c
@@ -16,7 +16,7 @@
  *
  * Test read and write speed of a MTD device.
  *
- * Author: Adrian Hunter <ext-adrian.hunter@nokia.com>
+ * Author: Adrian Hunter <adrian.hunter@nokia.com>
  */
 
 #include <linux/init.h>
@@ -33,6 +33,11 @@ static int dev;
 module_param(dev, int, S_IRUGO);
 MODULE_PARM_DESC(dev, "MTD device number to use");
 
+static int count;
+module_param(count, int, S_IRUGO);
+MODULE_PARM_DESC(count, "Maximum number of eraseblocks to use "
+			"(0 means use all)");
+
 static struct mtd_info *mtd;
 static unsigned char *iobuf;
 static unsigned char *bbt;
@@ -89,6 +94,33 @@ static int erase_eraseblock(int ebnum)
 	return 0;
 }
 
+static int multiblock_erase(int ebnum, int blocks)
+{
+	int err;
+	struct erase_info ei;
+	loff_t addr = ebnum * mtd->erasesize;
+
+	memset(&ei, 0, sizeof(struct erase_info));
+	ei.mtd  = mtd;
+	ei.addr = addr;
+	ei.len  = mtd->erasesize * blocks;
+
+	err = mtd->erase(mtd, &ei);
+	if (err) {
+		printk(PRINT_PREF "error %d while erasing EB %d, blocks %d\n",
+		       err, ebnum, blocks);
+		return err;
+	}
+
+	if (ei.state == MTD_ERASE_FAILED) {
+		printk(PRINT_PREF "some erase error occurred at EB %d,"
+		       "blocks %d\n", ebnum, blocks);
+		return -EIO;
+	}
+
+	return 0;
+}
+
 static int erase_whole_device(void)
 {
 	int err;
@@ -282,13 +314,16 @@ static inline void stop_timing(void)
 
 static long calc_speed(void)
 {
-	long ms, k, speed;
+	uint64_t k;
+	long ms;
 
 	ms = (finish.tv_sec - start.tv_sec) * 1000 +
 	     (finish.tv_usec - start.tv_usec) / 1000;
-	k = goodebcnt * mtd->erasesize / 1024;
-	speed = (k * 1000) / ms;
-	return speed;
+	if (ms == 0)
+		return 0;
+	k = goodebcnt * (mtd->erasesize / 1024) * 1000;
+	do_div(k, ms);
+	return k;
 }
 
 static int scan_for_bad_eraseblocks(void)
@@ -320,13 +355,16 @@ out:
 
 static int __init mtd_speedtest_init(void)
 {
-	int err, i;
+	int err, i, blocks, j, k;
 	long speed;
 	uint64_t tmp;
 
 	printk(KERN_INFO "\n");
 	printk(KERN_INFO "=================================================\n");
-	printk(PRINT_PREF "MTD device: %d\n", dev);
+	if (count)
+		printk(PRINT_PREF "MTD device: %d    count: %d\n", dev, count);
+	else
+		printk(PRINT_PREF "MTD device: %d\n", dev);
 
 	mtd = get_mtd_device(NULL, dev);
 	if (IS_ERR(mtd)) {
@@ -353,6 +391,9 @@ static int __init mtd_speedtest_init(void)
 	       (unsigned long long)mtd->size, mtd->erasesize,
 	       pgsize, ebcnt, pgcnt, mtd->oobsize);
 
+	if (count > 0 && count < ebcnt)
+		ebcnt = count;
+
 	err = -ENOMEM;
 	iobuf = kmalloc(mtd->erasesize, GFP_KERNEL);
 	if (!iobuf) {
@@ -484,6 +525,31 @@ static int __init mtd_speedtest_init(void)
 	speed = calc_speed();
 	printk(PRINT_PREF "erase speed is %ld KiB/s\n", speed);
 
+	/* Multi-block erase all eraseblocks */
+	for (k = 1; k < 7; k++) {
+		blocks = 1 << k;
+		printk(PRINT_PREF "Testing %dx multi-block erase speed\n",
+		       blocks);
+		start_timing();
+		for (i = 0; i < ebcnt; ) {
+			for (j = 0; j < blocks && (i + j) < ebcnt; j++)
+				if (bbt[i + j])
+					break;
+			if (j < 1) {
+				i++;
+				continue;
+			}
+			err = multiblock_erase(i, j);
+			if (err)
+				goto out;
+			cond_resched();
+			i += j;
+		}
+		stop_timing();
+		speed = calc_speed();
+		printk(PRINT_PREF "%dx multi-block erase speed is %ld KiB/s\n",
+		       blocks, speed);
+	}
 	printk(PRINT_PREF "finished\n");
 out:
 	kfree(iobuf);
diff --git a/drivers/mtd/tests/mtd_subpagetest.c b/drivers/mtd/tests/mtd_subpagetest.c
index 11204e8aab5f..334eae53a3db 100644
--- a/drivers/mtd/tests/mtd_subpagetest.c
+++ b/drivers/mtd/tests/mtd_subpagetest.c
@@ -394,6 +394,11 @@ static int __init mtd_subpagetest_init(void)
 	}
 
 	subpgsize = mtd->writesize >> mtd->subpage_sft;
+	tmp = mtd->size;
+	do_div(tmp, mtd->erasesize);
+	ebcnt = tmp;
+	pgcnt = mtd->erasesize / mtd->writesize;
+
 	printk(PRINT_PREF "MTD device size %llu, eraseblock size %u, "
 	       "page size %u, subpage size %u, count of eraseblocks %u, "
 	       "pages per eraseblock %u, OOB size %u\n",
@@ -413,11 +418,6 @@ static int __init mtd_subpagetest_init(void)
 		goto out;
 	}
 
-	tmp = mtd->size;
-	do_div(tmp, mtd->erasesize);
-	ebcnt = tmp;
-	pgcnt = mtd->erasesize / mtd->writesize;
-
 	err = scan_for_bad_eraseblocks();
 	if (err)
 		goto out;