Patchwork [MTD,CHIPS] stm_flash: new ST PSD4256G compatible flash chip driver

login
register
mail settings
Submitter Mike Frysinger
Date June 13, 2009, 10:59 a.m.
Message ID <1244890798-10908-1-git-send-email-vapier@gentoo.org>
Download mbox | patch
Permalink /patch/28668/
State New
Headers show

Comments

Mike Frysinger - June 13, 2009, 10:59 a.m.
The ST PSD4256G Flash In-System Programming (ISP) part is a multifunction
device.  Since all of the different pieces are memory mapped, this is a
driver for the NOR flash component.  All the other pieces can use generic
memory based drivers.  Unfortunately, this is not a CFI compliant part in
any way, so we need a dedicated chip driver for it.

Signed-off-by: Mike Frysinger <vapier@gentoo.org>
---
 drivers/mtd/chips/Kconfig     |    8 +
 drivers/mtd/chips/Makefile    |    1 +
 drivers/mtd/chips/stm_flash.c |  747 +++++++++++++++++++++++++++++++++++++++++
 3 files changed, 756 insertions(+), 0 deletions(-)
 create mode 100644 drivers/mtd/chips/stm_flash.c
David Woodhouse - Sept. 19, 2009, 8:08 p.m.
On Sat, 2009-06-13 at 06:59 -0400, Mike Frysinger wrote:
> The ST PSD4256G Flash In-System Programming (ISP) part is a multifunction
> device.  Since all of the different pieces are memory mapped, this is a
> driver for the NOR flash component.  All the other pieces can use generic
> memory based drivers.  Unfortunately, this is not a CFI compliant part in
> any way, so we need a dedicated chip driver for it.

Hrm. This looks very similar to cfi_cmdset_0002.c, which we _do_ already
use with non-CFI-compliant parts. 

CFI is just the _probe_ method. We can probe chips using the JEDEC
method too, and that works just fine -- jedec_probe.c will "invoke" the
appropriate command set driver.

Can't you do the same?
Mike Frysinger - Sept. 19, 2009, 8:28 p.m.
On Sat, Sep 19, 2009 at 16:08, David Woodhouse wrote:
> On Sat, 2009-06-13 at 06:59 -0400, Mike Frysinger wrote:
>> The ST PSD4256G Flash In-System Programming (ISP) part is a multifunction
>> device.  Since all of the different pieces are memory mapped, this is a
>> driver for the NOR flash component.  All the other pieces can use generic
>> memory based drivers.  Unfortunately, this is not a CFI compliant part in
>> any way, so we need a dedicated chip driver for it.
>
> Hrm. This looks very similar to cfi_cmdset_0002.c, which we _do_ already
> use with non-CFI-compliant parts.
>
> CFI is just the _probe_ method. We can probe chips using the JEDEC
> method too, and that works just fine -- jedec_probe.c will "invoke" the
> appropriate command set driver.
>
> Can't you do the same?

afaik, it isnt JEDEC compliant either.  you can find the data sheet here:
http://www.st.com/stonline/products/literature/ds/8116/psd4256g6v.htm

so if i created a probe function that created a fake cfi table and
then called cfi_cmdset_0002(), you think the read/write/erase
functions would work ?  i dont see any mapping drivers that do this
kind of thing.  where can i find an example ?
-mike
David Woodhouse - Sept. 19, 2009, 9:01 p.m.
On Sat, 2009-09-19 at 16:28 -0400, Mike Frysinger wrote:
> 
> > Can't you do the same?
> 
> afaik, it isnt JEDEC compliant either.  you can find the data sheet
> here:
> http://www.st.com/stonline/products/literature/ds/8116/psd4256g6v.htm
> 
> so if i created a probe function that created a fake cfi table and
> then called cfi_cmdset_0002(), you think the read/write/erase
> functions would work ?  

At first glance, it looks like that would work, yes. It basically seems
to be the same command set, at its core.

> i dont see any mapping drivers that do this
> kind of thing.  where can i find an example ?

The examples are cfi_probe.c and jedec_probe.c. We have no example of
setting it up _directly_ from a map driver, but that doesn't mean it
can't be done.
Mike Frysinger - Sept. 22, 2009, 11:05 p.m.
On Sat, Sep 19, 2009 at 17:01, David Woodhouse wrote:
> On Sat, 2009-09-19 at 16:28 -0400, Mike Frysinger wrote:
>>
>> > Can't you do the same?
>>
>> afaik, it isnt JEDEC compliant either.  you can find the data sheet
>> here:
>> http://www.st.com/stonline/products/literature/ds/8116/psd4256g6v.htm
>>
>> so if i created a probe function that created a fake cfi table and
>> then called cfi_cmdset_0002(), you think the read/write/erase
>> functions would work ?
>
> At first glance, it looks like that would work, yes. It basically seems
> to be the same command set, at its core.
>
>> i dont see any mapping drivers that do this
>> kind of thing.  where can i find an example ?
>
> The examples are cfi_probe.c and jedec_probe.c. We have no example of
> setting it up _directly_ from a map driver, but that doesn't mean it
> can't be done.

thanks, this got me poking at the jedec chip driver.  i tried adding a
new MTD_UADDR_0x0AAA_0x0554 and then defining a jedec_table entry like
so:
+    {
+       .mfr_id = 0xff20,
+       .dev_id = 0xffe9,
+       .name   = "STM PSD4256G6V",
+       .uaddr  = MTD_UADDR_0x0AAA_0x0554,
+       .devtypes   = CFI_DEVICETYPE_X16,
+       .dev_size = SIZE_1MiB,
+       .cmd_set = P_ID_AMD_STD,
+       .nr_regions = 1,
+       .regions = {
+           ERASEINFO(0x10000,16)
+       },
+   },

then i was able to read/write/erase the flash device just fine from
userspace /dev/mtd via the physmap mapping driver.

the only problem i have here is with the need to 0xff pad the mfr/dev
id's.  any idea what's up with that ?  the device is 16bit and i set
my .width in my board resources to 2, but the id field is only 8bit,
so in 16bit mode the id is padded with 0xff instead of 0x00.
-mike
David Woodhouse - Sept. 22, 2009, 11:20 p.m.
On Tue, 2009-09-22 at 19:05 -0400, Mike Frysinger wrote:
> 
> the only problem i have here is with the need to 0xff pad the mfr/dev
> id's.  any idea what's up with that ?  the device is 16bit and i set
> my .width in my board resources to 2, but the id field is only 8bit,
> so in 16bit mode the id is padded with 0xff instead of 0x00.

Hm, odd -- but I wouldn't lose sleep over it.

However, the datasheet suggests that the high byte is unspecified. Could
it be returning the value of the flash data at that location, rather
than a constant 0xFF? If so, we'd need to add a flag to the jedec match
table to make it match only the low byte?

Or maybe I'd make you do your own probe in that case, rather than
polluting jedec_probe.c... :)
Mike Frysinger - Sept. 22, 2009, 11:30 p.m.
On Tue, Sep 22, 2009 at 19:20, David Woodhouse wrote:
> On Tue, 2009-09-22 at 19:05 -0400, Mike Frysinger wrote:
>> the only problem i have here is with the need to 0xff pad the mfr/dev
>> id's.  any idea what's up with that ?  the device is 16bit and i set
>> my .width in my board resources to 2, but the id field is only 8bit,
>> so in 16bit mode the id is padded with 0xff instead of 0x00.
>
> Hm, odd -- but I wouldn't lose sleep over it.
>
> However, the datasheet suggests that the high byte is unspecified. Could
> it be returning the value of the flash data at that location, rather
> than a constant 0xFF? If so, we'd need to add a flag to the jedec match
> table to make it match only the low byte?
>
> Or maybe I'd make you do your own probe in that case, rather than
> polluting jedec_probe.c... :)

nah, none of the first 0x10 bytes are 0xff:
00000000  7f 1d bb 58 8f 04 4c 6d  f5 ee 55 d6 79 b4 31 51  |...X..Lm..U.y.1Q|

this is the only board ive seen that has this part, but i can do a
sampling of different revisions to make sure they're all 0xff.  if
you're ok with the small changes i mentioned, then i'll post a patch
for that (to replace the original proposed chip driver) and call it a
day.
-mike
David Woodhouse - Sept. 22, 2009, 11:39 p.m.
On Tue, 2009-09-22 at 19:30 -0400, Mike Frysinger wrote:
> 
> nah, none of the first 0x10 bytes are 0xff:
> 00000000  7f 1d bb 58 8f 04 4c 6d  f5 ee 55 d6 79 b4 31 51
> |...X..Lm..U.y.1Q|
> 
> this is the only board ive seen that has this part, but i can do a
> sampling of different revisions to make sure they're all 0xff.  if
> you're ok with the small changes i mentioned, then i'll post a patch
> for that (to replace the original proposed chip driver) and call it a
> day.

Sounds good. If you can get it to me in the next half hour or so, I'll
sneak it into my 2.6.32 pull request.

Patch

diff --git a/drivers/mtd/chips/Kconfig b/drivers/mtd/chips/Kconfig
index 9408099..881c6bc 100644
--- a/drivers/mtd/chips/Kconfig
+++ b/drivers/mtd/chips/Kconfig
@@ -207,6 +207,14 @@  config MTD_CFI_STAA
 	  sets which a CFI-compliant chip may claim to implement. This code
 	  provides support for one of those command sets.
 
+config MTD_PSD4256G
+	tristate "ST PSD4256G compatible flash chip support"
+	help
+	  Driver for flashes compatible with ST PSD4256G.  This is a non-CFI
+	  and non-JEDEC compliant flash and you really shouldn't buy this.
+
+	  If unsure, say N here.
+
 config MTD_CFI_UTIL
 	tristate
 
diff --git a/drivers/mtd/chips/Makefile b/drivers/mtd/chips/Makefile
index 3658241..376d78a 100644
--- a/drivers/mtd/chips/Makefile
+++ b/drivers/mtd/chips/Makefile
@@ -13,3 +13,4 @@  obj-$(CONFIG_MTD_JEDECPROBE)	+= jedec_probe.o
 obj-$(CONFIG_MTD_RAM)		+= map_ram.o
 obj-$(CONFIG_MTD_ROM)		+= map_rom.o
 obj-$(CONFIG_MTD_ABSENT)	+= map_absent.o
+obj-$(CONFIG_MTD_PSD4256G)	+= stm_flash.o
diff --git a/drivers/mtd/chips/stm_flash.c b/drivers/mtd/chips/stm_flash.c
new file mode 100644
index 0000000..cd9ed59
--- /dev/null
+++ b/drivers/mtd/chips/stm_flash.c
@@ -0,0 +1,747 @@ 
+/*
+ * Driver for ST PSD4256G flash
+ *
+ * Copyright 2004-2009 Analog Devices Inc.
+ * Licensed under the GPL-2 or later.
+ */
+
+#define pr_fmt(fmt) "stm_flash: " fmt
+
+#include <linux/delay.h>
+#include <linux/errno.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/mtd/map.h>
+#include <linux/mtd/mtd.h>
+#include <linux/mtd/flashchip.h>
+#include <linux/sched.h>
+#include <linux/types.h>
+
+/* Addresses */
+#define ADDR_MANUFACTURER		0x0000
+#define ADDR_DEVICE_ID			0x0002
+#define ADDR_UNLOCK_1			0x0AAA
+#define ADDR_UNLOCK_2			0x0554
+
+/* Commands */
+#define CMD_UNLOCK_DATA_1		0x00aa
+#define CMD_UNLOCK_DATA_2		0x0055
+#define CMD_MANUFACTURER_UNLOCK_DATA	0x0090
+#define CMD_PROGRAM_UNLOCK_DATA		0x00A0
+#define CMD_RESET_DATA			0x00F0
+#define CMD_SECTOR_ERASE_UNLOCK_DATA	0x0080
+#define CMD_SECTOR_ERASE_UNLOCK_DATA_2	0x0030
+
+#define D6_MASK 0x40
+
+struct stm_flash_private {
+	unsigned long chipshift;
+	struct flchip chip;
+};
+
+static void send_unlock(struct map_info *map, unsigned long base)
+{
+	map_word test;
+	test.x[0] = 0x00aa;
+	map_write(map, test, base + ADDR_UNLOCK_1);
+	test.x[0] = 0x0055;
+	map_write(map, test, base + ADDR_UNLOCK_2);
+}
+
+static void send_cmd(struct map_info *map, unsigned long base,
+		     unsigned long cmd)
+{
+	map_word test;
+	test.x[0] = cmd;
+	send_unlock(map, base);
+	map_write(map, test, base + ADDR_UNLOCK_1);
+}
+
+static void send_cmd_to_addr(struct map_info *map, unsigned long base,
+			     unsigned long cmd, unsigned long addr)
+{
+	map_word test;
+	test.x[0] = cmd;
+	send_unlock(map, base);
+	map_write(map, test, addr);
+}
+
+static void stm_flash_sync(struct mtd_info *mtd)
+{
+	struct map_info *map = mtd->priv;
+	struct stm_flash_private *private = map->fldrv_priv;
+	struct flchip *chip;
+	DECLARE_WAITQUEUE(wait, current);
+
+	chip = &private->chip;
+
+ retry:
+		spin_lock_bh(chip->mutex);
+
+		switch (chip->state) {
+		case FL_READY:
+		case FL_STATUS:
+		case FL_CFI_QUERY:
+		case FL_JEDEC_QUERY:
+			chip->oldstate = chip->state;
+			chip->state = FL_SYNCING;
+		case FL_SYNCING:
+			spin_unlock_bh(chip->mutex);
+			break;
+		default:
+			/* Not an idle state */
+			add_wait_queue(&chip->wq, &wait);
+
+			spin_unlock_bh(chip->mutex);
+			schedule();
+			remove_wait_queue(&chip->wq, &wait);
+
+			goto retry;
+		}
+
+	chip = &private->chip;
+
+	spin_lock_bh(chip->mutex);
+
+	if (chip->state == FL_SYNCING) {
+		chip->state = chip->oldstate;
+		wake_up(&chip->wq);
+	}
+	spin_unlock_bh(chip->mutex);
+}
+
+static int read_one_chip(struct map_info *map, struct flchip *chip,
+			loff_t addr, size_t len, unsigned char *buf)
+{
+	DECLARE_WAITQUEUE(wait, current);
+	unsigned long timeo = jiffies + HZ;
+
+ retry:
+	spin_lock_bh(chip->mutex);
+
+	if (chip->state != FL_READY) {
+		pr_info("waiting for chip to read, state = %d\n",
+			chip->state);
+		set_current_state(TASK_UNINTERRUPTIBLE);
+		add_wait_queue(&chip->wq, &wait);
+
+		spin_unlock_bh(chip->mutex);
+
+		schedule();
+		remove_wait_queue(&chip->wq, &wait);
+
+		if (signal_pending(current))
+			return -EINTR;
+
+		timeo = jiffies + HZ;
+
+		goto retry;
+	}
+
+	addr += chip->start;
+
+	chip->state = FL_READY;
+
+	map_copy_from(map, buf, addr, len);
+
+	wake_up(&chip->wq);
+	spin_unlock_bh(chip->mutex);
+
+	return 0;
+}
+
+static int stm_flash_read(struct mtd_info *mtd, loff_t from, size_t len,
+			  size_t *retlen, unsigned char *buf)
+{
+	struct map_info *map = mtd->priv;
+	struct stm_flash_private *private = map->fldrv_priv;
+	unsigned long offset;
+	int chipnum, ret = 0;
+
+	if ((from + len) > mtd->size) {
+		pr_warning("read request past end of device (0x%lx)\n",
+			   (unsigned long)from + len);
+		return -EINVAL;
+	}
+
+	/* Offset within the first chip that the first read should start. */
+	chipnum = (from >> private->chipshift);
+	offset = from - (chipnum << private->chipshift);
+
+	*retlen = 0;
+
+	while (len) {
+		unsigned long this_len;
+
+		if ((len + offset - 1) >> private->chipshift)
+			this_len = (1 << private->chipshift) - offset;
+		else
+			this_len = len;
+
+		ret = read_one_chip(map, &private->chip, offset,
+				this_len, buf);
+
+		if (ret)
+			break;
+
+		*retlen += this_len;
+		len -= this_len;
+		buf += this_len;
+
+		offset = 0;
+		chipnum++;
+	}
+
+	return ret;
+}
+
+static int flash_is_busy(struct map_info *map, unsigned long addr)
+{
+	unsigned short toggled;
+	map_word read11, read21;
+
+	read11 = map_read(map, addr);
+	read21 = map_read(map, addr);
+
+	toggled = (unsigned short)read11.x[0] ^ (unsigned short)read21.x[0];
+
+	toggled &= (((unsigned short)1) << 6);
+	return toggled;
+}
+
+static int write_one_word(struct map_info *map, struct flchip *chip,
+		unsigned long addr, unsigned long datum)
+{
+	unsigned long timeo = jiffies + HZ;
+	DECLARE_WAITQUEUE(wait, current);
+	int ret = 0;
+	int times_left;
+	map_word test;
+
+ retry:
+	spin_lock_bh(chip->mutex);
+
+	if (chip->state != FL_READY) {
+		pr_info("%s: waiting for chip to write, state = %d\n",
+			map->name, chip->state);
+		set_current_state(TASK_UNINTERRUPTIBLE);
+		add_wait_queue(&chip->wq, &wait);
+
+		spin_unlock_bh(chip->mutex);
+
+		schedule();
+		remove_wait_queue(&chip->wq, &wait);
+		pr_info("%s: woke up to write\n", map->name);
+		if (signal_pending(current))
+			return -EINTR;
+
+		timeo = jiffies + HZ;
+
+		goto retry;
+	}
+
+	chip->state = FL_WRITING;
+
+	addr += chip->start;
+
+	send_cmd(map, chip->start, CMD_PROGRAM_UNLOCK_DATA);
+
+	test.x[0] = datum;
+	map_write(map, test, addr);
+
+	times_left = 50000;
+	while (times_left-- && flash_is_busy(map, addr)) {
+		if (need_resched()) {
+			spin_unlock_bh(chip->mutex);
+			schedule();
+			spin_lock_bh(chip->mutex);
+		}
+	}
+
+	if (!times_left) {
+		pr_warning("write to 0x%lx timed out!\n", addr);
+		ret = -EIO;
+	} else {
+		unsigned long verify;
+		map_word test;
+
+		test = map_read(map, addr);
+		verify = test.x[0];
+		if (verify != datum) {
+			pr_warning("write to 0x%lx failed "
+				   "datum = %lx, verify = %lx\n",
+				   addr, datum, verify);
+			ret = -EIO;
+		}
+	}
+	chip->state = FL_READY;
+	wake_up(&chip->wq);
+	spin_unlock_bh(chip->mutex);
+
+	return ret;
+}
+
+static int stm_flash_write(struct mtd_info *mtd, loff_t to, size_t len,
+			   size_t *retlen, const unsigned char *buf)
+{
+	struct map_info *map = mtd->priv;
+	struct stm_flash_private *private = map->fldrv_priv;
+	int ret = 0;
+	int chipnum;
+	unsigned long offset;
+
+	*retlen = 0;
+	if (!len)
+		return 0;
+
+	chipnum = to >> private->chipshift;
+	offset = to - (chipnum << private->chipshift);
+
+	/* If it's not bus-aligned, do the first byte write. */
+	if (offset & (map->bankwidth - 1)) {
+		unsigned long bus_offset = offset & ~(map->bankwidth - 1);
+		int i = offset - bus_offset;
+		int n = 0;
+		unsigned char tmp_buf[4];
+		unsigned long datum;
+
+		map_copy_from(map, tmp_buf,
+			       bus_offset + private->chip.start,
+			       map->bankwidth);
+		while (len && i < map->bankwidth)
+			tmp_buf[i++] = buf[n++], len--;
+
+		if (map->bankwidth == 2)
+			datum = *(__u16 *)tmp_buf;
+		else if (map->bankwidth == 4)
+			datum = *(__u32 *)tmp_buf;
+		else
+			return -EINVAL;
+
+		ret = write_one_word(map, &private->chip, bus_offset,
+				datum);
+
+		if (ret)
+			return ret;
+
+		offset += n;
+		buf += n;
+		(*retlen) += n;
+
+		if (offset >> private->chipshift)
+			return 0;
+	}
+
+	/* We are now aligned, write as much as possible. */
+	while (len >= map->bankwidth) {
+		unsigned long datum;
+
+		if (map->bankwidth == 1)
+			datum = *(unsigned char *)buf;
+		else if (map->bankwidth == 2)
+			datum = *(unsigned short *)buf;
+		else if (map->bankwidth == 4)
+			datum = *(unsigned long *)buf;
+		else
+			return -EINVAL;
+
+		ret = write_one_word(map, &private->chip, offset,
+				datum);
+
+		if (ret)
+			return ret;
+
+		offset += map->bankwidth;
+		buf += map->bankwidth;
+		(*retlen) += map->bankwidth;
+		len -= map->bankwidth;
+
+		if (offset >> private->chipshift)
+			return 0;
+	}
+
+	if (len & (map->bankwidth - 1)) {
+		int i = 0, n = 0;
+		unsigned char tmp_buf[2];
+		unsigned long datum;
+
+		map_copy_from(map, tmp_buf,
+				offset + private->chip.start,
+				map->bankwidth);
+
+		while (len--)
+			tmp_buf[i++] = buf[n++];
+
+		if (map->bankwidth == 2)
+			datum = *(unsigned short *)tmp_buf;
+		else if (map->bankwidth == 4)
+			datum = *(unsigned long *)tmp_buf;
+		else
+			return -EINVAL;
+
+		ret = write_one_word(map, &private->chip, offset,
+				datum);
+
+		if (ret)
+			return ret;
+
+		(*retlen) += n;
+	}
+
+	return 0;
+}
+
+static int erase_one_block(struct map_info *map, struct flchip *chip,
+			   unsigned long addr, unsigned long size)
+{
+	unsigned long timeo = jiffies + HZ;
+	DECLARE_WAITQUEUE(wait, current);
+
+ retry:
+	spin_lock_bh(chip->mutex);
+
+	if (chip->state != FL_READY) {
+		set_current_state(TASK_UNINTERRUPTIBLE);
+		add_wait_queue(&chip->wq, &wait);
+
+		spin_unlock_bh(chip->mutex);
+
+		schedule();
+		remove_wait_queue(&chip->wq, &wait);
+
+		if (signal_pending(current))
+			return -EINTR;
+
+		timeo = jiffies + HZ;
+
+		goto retry;
+	}
+
+	chip->state = FL_ERASING;
+
+	addr += chip->start;
+
+	send_cmd(map, chip->start, CMD_SECTOR_ERASE_UNLOCK_DATA);
+	send_cmd_to_addr(map, chip->start, CMD_SECTOR_ERASE_UNLOCK_DATA_2, addr);
+
+	timeo = jiffies + (HZ * 20);
+
+	spin_unlock_bh(chip->mutex);
+	schedule_timeout(HZ);
+	spin_lock_bh(chip->mutex);
+
+	while (flash_is_busy(map, chip->start)) {
+		if (chip->state != FL_ERASING) {
+			/* Someone's suspended the erase. Sleep. */
+			set_current_state(TASK_UNINTERRUPTIBLE);
+			add_wait_queue(&chip->wq, &wait);
+
+			spin_unlock_bh(chip->mutex);
+			pr_info("erase suspended, sleeping\n");
+			schedule();
+			remove_wait_queue(&chip->wq, &wait);
+
+			if (signal_pending(current))
+				return -EINTR;
+
+			timeo = jiffies + (HZ*2);
+			spin_lock_bh(chip->mutex);
+			continue;
+		}
+
+		/* OK Still waiting */
+		if (time_after(jiffies, timeo)) {
+			chip->state = FL_READY;
+			spin_unlock_bh(chip->mutex);
+			pr_warning("waiting for erase to complete timed out\n");
+			return -EIO;
+		}
+
+		/* Latency issues. Drop the lock, wait a while, and retry. */
+		spin_unlock_bh(chip->mutex);
+
+		if (need_resched())
+			schedule();
+		else
+			udelay(1);
+
+		spin_lock_bh(chip->mutex);
+	}
+
+	{
+		/* Verify every single word */
+		int address;
+		int error = 0;
+		int verify;
+		map_word test;
+
+		for (address = addr; address < (addr + size); address += 2) {
+			test = map_read(map, address);
+			verify = test.x[0];
+			if (verify != 0xFFFF) {
+				error = 1;
+				break;
+			}
+		}
+
+		if (error) {
+			chip->state = FL_READY;
+			spin_unlock_bh(chip->mutex);
+			pr_warning("verify error at 0x%x, size %ld\n",
+				   address, size);
+			return -EIO;
+		}
+	}
+
+	chip->state = FL_READY;
+	wake_up(&chip->wq);
+	spin_unlock_bh(chip->mutex);
+
+	return 0;
+}
+
+static int stm_flash_erase(struct mtd_info *mtd, struct erase_info *instr)
+{
+	struct map_info *map = mtd->priv;
+	struct stm_flash_private *private = map->fldrv_priv;
+	unsigned long addr, len, shift;
+	int chipnum;
+	int ret = 0;
+	int i, first;
+	struct mtd_erase_region_info *regions = mtd->eraseregions;
+
+	if (instr->addr > mtd->size)
+		return -EINVAL;
+
+	if ((instr->len + instr->addr) > mtd->size)
+		return -EINVAL;
+
+	/*
+	 * Check that both start and end of the requested erase are aligned
+	 * with the erasesize at the appropriate addresses.
+	 */
+	i = 0;
+
+	/*
+	 * Skip all erase regions which are ended before the start of the
+	 * requested erase. Actually, to save on the calculations, we skip
+	 * to the first erase region which starts after the start of the
+	 * requested erase, and then go back one.
+	 */
+	while ((i < mtd->numeraseregions) &&
+	       (instr->addr >= regions[i].offset))
+		++i;
+	--i;
+
+	/*
+	 * OK. Now i is pointing at the erase region in which this erase
+	 * request starts. Check the start of the requested erase range
+	 * is aligned with the erase size which is in effect here.
+	 */
+	if (instr->addr & (regions[i].erasesize - 1))
+		return -EINVAL;
+
+	/*
+	 * Remember the erase region we start on.
+	 */
+	first = i;
+
+	/*
+	 * Next, theck that the end of the requested erase is aligned with
+	 * the erase region at that address.
+	 */
+	while ((i < mtd->numeraseregions) &&
+	       ((instr->addr + instr->len) >= regions[i].offset))
+		++i;
+	--i;
+
+	if ((instr->addr + instr->len) & (regions[i].erasesize - 1))
+		return -EINVAL;
+
+	chipnum = instr->addr >> private->chipshift;
+	addr = instr->addr - (chipnum << private->chipshift);
+	len = instr->len;
+
+	i = first;
+	shift = (1 << private->chipshift) - 1;
+	while (len) {
+		ret = erase_one_block(map, &private->chip, addr,
+				regions[i].erasesize);
+
+		if (ret)
+			return ret;
+
+		addr += regions[i].erasesize;
+		len -= regions[i].erasesize;
+
+		if ((addr & shift) ==
+		    ((regions[i].offset +
+		      (regions[i].erasesize * regions[i].numblocks))
+		     & shift))
+			++i;
+
+		if (addr & ~shift)
+			break;
+	}
+
+	instr->state = MTD_ERASE_DONE;
+	if (instr->callback)
+		instr->callback(instr);
+
+	return 0;
+}
+
+static void stm_flash_destroy(struct mtd_info *mtd)
+{
+	struct map_info *map = mtd->priv;
+	struct stm_flash_private *private = map->fldrv_priv;
+	kfree(private);
+}
+
+struct stm_flash_info {
+	u8 mfr_id, dev_id;
+	char *name;
+	unsigned long size;
+	int numeraseregions;
+	struct mtd_erase_region_info regions[1];
+};
+
+const struct stm_flash_info stm_data[] = {
+	{
+		.mfr_id = 0x20,
+		.dev_id = 0xe9,
+		.name   = "STM PSD4256G6V",
+		.size   = 0x100000,
+		.numeraseregions = 1,
+		.regions = {
+			{
+				.offset    = 0x000000,
+				.erasesize = 0x10000,
+				.numblocks = 16
+			},
+		},
+	},
+};
+
+static int stm_probe_new_chip(struct mtd_info *mtd)
+{
+	const struct stm_flash_info *std;
+	struct map_info *map = mtd->priv;
+	int i;
+	u8 mfr_id, dev_id;
+	map_word mfr_word, dev_word;
+
+	/* Enter autoselect mode */
+	send_cmd(map, 0, CMD_RESET_DATA);
+	send_cmd(map, 0, CMD_MANUFACTURER_UNLOCK_DATA);
+
+	mfr_word = map_read(map, ADDR_MANUFACTURER);
+	dev_word = map_read(map, ADDR_DEVICE_ID);
+	mfr_id = mfr_word.x[0] & 0xFF;
+	dev_id = dev_word.x[0] & 0xFF;
+
+	/* Exit autoselect mode */
+	send_cmd(map, 0, CMD_RESET_DATA);
+
+	for (i = 0; i < ARRAY_SIZE(stm_data); ++i) {
+		std = &stm_data[i];
+		if (mfr_id == std->mfr_id && dev_id == std->dev_id)
+			break;
+	}
+	if (i == ARRAY_SIZE(stm_data)) {
+		pr_warning("unknown mfr/dev id: 0x%02x 0x%02x\n", mfr_id, dev_id);
+		return 1;
+	}
+
+	mtd->size += std->size;
+	mtd->numeraseregions += std->numeraseregions;
+
+	mtd->eraseregions = kmalloc(sizeof(*mtd->eraseregions) *
+				    mtd->numeraseregions, GFP_KERNEL);
+	if (!mtd->eraseregions) {
+		pr_warning("failed to allocate memory for MTD erase region\n");
+		return 1;
+	}
+
+	for (i = 0; i < std->numeraseregions; ++i) {
+		mtd->eraseregions[i].offset    = std->regions[i].offset;
+		mtd->eraseregions[i].erasesize = std->regions[i].erasesize;
+		mtd->eraseregions[i].numblocks = std->regions[i].numblocks;
+		if (mtd->erasesize < mtd->eraseregions[i].erasesize)
+			mtd->erasesize = mtd->eraseregions[i].erasesize;
+	}
+
+	pr_info("found %s device at %p in %d-bit bank\n",
+	        std->name, map->virt, map->bankwidth * 8);
+
+	return 0;
+}
+
+static struct mtd_info *stm_flash_probe(struct map_info *map);
+
+static struct mtd_chip_driver stm_flash_chipdrv = {
+	.probe   = stm_flash_probe,
+	.destroy = stm_flash_destroy,
+	.name    = "stm_flash",
+	.module  = THIS_MODULE
+};
+
+static struct mtd_info *stm_flash_probe(struct map_info *map)
+{
+	struct mtd_info *mtd;
+	struct flchip *chip;
+	struct stm_flash_private *private;
+	unsigned long size;
+
+	mtd = kzalloc(sizeof(*mtd) + sizeof(*private), GFP_KERNEL);
+	if (!mtd) {
+		pr_warning("kmalloc failed for info structure\n");
+		return NULL;
+	}
+	mtd->priv = map;
+	map->fldrv_priv = private = (void *)&mtd[1];
+	map->fldrv = &stm_flash_chipdrv;
+
+	if (stm_probe_new_chip(mtd)) {
+		kfree(mtd);
+		return NULL;
+	}
+
+	chip = &private->chip;
+	init_waitqueue_head(&chip->wq);
+	spin_lock_init(&chip->_spinlock);
+	chip->start = 0;
+	chip->state = FL_READY;
+	chip->mutex = &chip->_spinlock;
+	for (size = mtd->size; size > 1; size >>= 1)
+		++private->chipshift;
+
+	mtd->type  = MTD_NORFLASH;
+	mtd->flags = MTD_CAP_NORFLASH;
+	mtd->name  = map->name;
+	mtd->erase = stm_flash_erase;
+	mtd->read  = stm_flash_read;
+	mtd->write = stm_flash_write;
+	mtd->sync  = stm_flash_sync;
+	mtd->writesize = 1;
+
+	return mtd;
+}
+
+static int __init stm_flash_init(void)
+{
+	register_mtd_chip_driver(&stm_flash_chipdrv);
+	return 0;
+}
+module_init(stm_flash_init);
+
+static void __exit stm_flash_exit(void)
+{
+	unregister_mtd_chip_driver(&stm_flash_chipdrv);
+}
+module_exit(stm_flash_exit);
+
+MODULE_DESCRIPTION("MTD chip driver for ST PSD4256G flashes");
+MODULE_LICENSE("GPL");