8 files changed, 677 insertions, 20 deletions

diff --git a/drivers/mtd/devices/Kconfig b/drivers/mtd/devices/Kconfig
index 325fab92a62c..c222514bb70d 100644
--- a/drivers/mtd/devices/Kconfig
+++ b/drivers/mtd/devices/Kconfig
@@ -104,6 +104,16 @@ config M25PXX_USE_FAST_READ
         help
           This option enables FAST_READ access supported by ST M25Pxx.
 
+config MTD_SST25L
+        tristate "Support SST25L (non JEDEC) SPI Flash chips"
+        depends on SPI_MASTER
+        help
+          This enables access to the non JEDEC SST25L SPI flash chips, used
+          for program and data storage.
+
+          Set up your spi devices with the right board-specific platform data,
+          if you want to specify device partitioning.
+
 config MTD_SLRAM
         tristate "Uncached system RAM"
         help
diff --git a/drivers/mtd/devices/Makefile b/drivers/mtd/devices/Makefile
index 0993d5cf3923..ab5c9b92ac82 100644
--- a/drivers/mtd/devices/Makefile
+++ b/drivers/mtd/devices/Makefile
@@ -16,3 +16,4 @@ obj-$(CONFIG_MTD_LART)		+= lart.o
 obj-$(CONFIG_MTD_BLOCK2MTD)     += block2mtd.o
 obj-$(CONFIG_MTD_DATAFLASH)     += mtd_dataflash.o
 obj-$(CONFIG_MTD_M25P80)        += m25p80.o
+obj-$(CONFIG_MTD_SST25L)        += sst25l.o
diff --git a/drivers/mtd/devices/lart.c b/drivers/mtd/devices/lart.c
index 578de1c67bfe..f4359fe7150f 100644
--- a/drivers/mtd/devices/lart.c
+++ b/drivers/mtd/devices/lart.c
@@ -393,7 +393,8 @@ static int flash_erase (struct mtd_info *mtd,struct erase_info *instr)
         * erase range is aligned with the erase size which is in
         * effect here.
         */
-   if (instr->addr & (mtd->eraseregions[i].erasesize - 1)) return (-EINVAL);
+   if (i < 0 || (instr->addr & (mtd->eraseregions[i].erasesize - 1)))
+      return -EINVAL;
 
    /* Remember the erase region we start on */
    first = i;
@@ -409,7 +410,8 @@ static int flash_erase (struct mtd_info *mtd,struct erase_info *instr)
    i--;
 
    /* is the end aligned on a block boundary? */
-   if ((instr->addr + instr->len) & (mtd->eraseregions[i].erasesize - 1)) return (-EINVAL);
+   if (i < 0 || ((instr->addr + instr->len) & (mtd->eraseregions[i].erasesize - 1)))
+      return -EINVAL;
 
    addr = instr->addr;
    len = instr->len;
diff --git a/drivers/mtd/devices/m25p80.c b/drivers/mtd/devices/m25p80.c
index eb495d83064f..379c316f329e 100644
--- a/drivers/mtd/devices/m25p80.c
+++ b/drivers/mtd/devices/m25p80.c
@@ -44,6 +44,11 @@
 #define OPCODE_SE               0xd8    /* Sector erase (usually 64KiB) */
 #define OPCODE_RDID             0x9f    /* Read JEDEC ID */
 
+/* Used for SST flashes only. */
+#define OPCODE_BP               0x02    /* Byte program */
+#define OPCODE_WRDI             0x04    /* Write disable */
+#define OPCODE_AAI_WP           0xad    /* Auto address increment word program */
+
 /* Status Register bits. */
 #define SR_WIP                  1       /* Write in progress */
 #define SR_WEL                  2       /* Write enable latch */
@@ -132,6 +137,15 @@ static inline int write_enable(struct m25p *flash)
         return spi_write_then_read(flash->spi, &code, 1, NULL, 0);
 }
 
+/*
+ * Send write disble instruction to the chip.
+ */
+static inline int write_disable(struct m25p *flash)
+{
+        u8      code = OPCODE_WRDI;
+
+        return spi_write_then_read(flash->spi, &code, 1, NULL, 0);
+}
 
 /*
  * Service routine to read status register until ready, or timeout occurs.
@@ -454,6 +468,111 @@ static int m25p80_write(struct mtd_info *mtd, loff_t to, size_t len,
         return 0;
 }
 
+static int sst_write(struct mtd_info *mtd, loff_t to, size_t len,
+                size_t *retlen, const u_char *buf)
+{
+        struct m25p *flash = mtd_to_m25p(mtd);
+        struct spi_transfer t[2];
+        struct spi_message m;
+        size_t actual;
+        int cmd_sz, ret;
+
+        if (retlen)
+                *retlen = 0;
+
+        /* sanity checks */
+        if (!len)
+                return 0;
+
+        if (to + len > flash->mtd.size)
+                return -EINVAL;
+
+        spi_message_init(&m);
+        memset(t, 0, (sizeof t));
+
+        t[0].tx_buf = flash->command;
+        t[0].len = CMD_SIZE;
+        spi_message_add_tail(&t[0], &m);
+
+        t[1].tx_buf = buf;
+        spi_message_add_tail(&t[1], &m);
+
+        mutex_lock(&flash->lock);
+
+        /* Wait until finished previous write command. */
+        ret = wait_till_ready(flash);
+        if (ret)
+                goto time_out;
+
+        write_enable(flash);
+
+        actual = to % 2;
+        /* Start write from odd address. */
+        if (actual) {
+                flash->command[0] = OPCODE_BP;
+                flash->command[1] = to >> 16;
+                flash->command[2] = to >> 8;
+                flash->command[3] = to;
+
+                /* write one byte. */
+                t[1].len = 1;
+                spi_sync(flash->spi, &m);
+                ret = wait_till_ready(flash);
+                if (ret)
+                        goto time_out;
+                *retlen += m.actual_length - CMD_SIZE;
+        }
+        to += actual;
+
+        flash->command[0] = OPCODE_AAI_WP;
+        flash->command[1] = to >> 16;
+        flash->command[2] = to >> 8;
+        flash->command[3] = to;
+
+        /* Write out most of the data here. */
+        cmd_sz = CMD_SIZE;
+        for (; actual < len - 1; actual += 2) {
+                t[0].len = cmd_sz;
+                /* write two bytes. */
+                t[1].len = 2;
+                t[1].tx_buf = buf + actual;
+
+                spi_sync(flash->spi, &m);
+                ret = wait_till_ready(flash);
+                if (ret)
+                        goto time_out;
+                *retlen += m.actual_length - cmd_sz;
+                cmd_sz = 1;
+                to += 2;
+        }
+        write_disable(flash);
+        ret = wait_till_ready(flash);
+        if (ret)
+                goto time_out;
+
+        /* Write out trailing byte if it exists. */
+        if (actual != len) {
+                write_enable(flash);
+                flash->command[0] = OPCODE_BP;
+                flash->command[1] = to >> 16;
+                flash->command[2] = to >> 8;
+                flash->command[3] = to;
+                t[0].len = CMD_SIZE;
+                t[1].len = 1;
+                t[1].tx_buf = buf + actual;
+
+                spi_sync(flash->spi, &m);
+                ret = wait_till_ready(flash);
+                if (ret)
+                        goto time_out;
+                *retlen += m.actual_length - CMD_SIZE;
+                write_disable(flash);
+        }
+
+time_out:
+        mutex_unlock(&flash->lock);
+        return ret;
+}
 
 /****************************************************************************/
 
@@ -501,7 +620,10 @@ static struct flash_info __devinitdata m25p_data [] = {
         { "at26df321",  0x1f4701, 0, 64 * 1024, 64, SECT_4K, },
 
         /* Macronix */
+        { "mx25l3205d", 0xc22016, 0, 64 * 1024, 64, },
+        { "mx25l6405d", 0xc22017, 0, 64 * 1024, 128, },
         { "mx25l12805d", 0xc22018, 0, 64 * 1024, 256, },
+        { "mx25l12855e", 0xc22618, 0, 64 * 1024, 256, },
 
         /* Spansion -- single (large) sector size only, at least
          * for the chips listed here (without boot sectors).
@@ -511,14 +633,20 @@ static struct flash_info __devinitdata m25p_data [] = {
         { "s25sl016a", 0x010214, 0, 64 * 1024, 32, },
         { "s25sl032a", 0x010215, 0, 64 * 1024, 64, },
         { "s25sl064a", 0x010216, 0, 64 * 1024, 128, },
-        { "s25sl12800", 0x012018, 0x0300, 256 * 1024, 64, },
+        { "s25sl12800", 0x012018, 0x0300, 256 * 1024, 64, },
         { "s25sl12801", 0x012018, 0x0301, 64 * 1024, 256, },
+        { "s25fl129p0", 0x012018, 0x4d00, 256 * 1024, 64, },
+        { "s25fl129p1", 0x012018, 0x4d01, 64 * 1024, 256, },
 
         /* SST -- large erase sizes are "overlays", "sectors" are 4K */
         { "sst25vf040b", 0xbf258d, 0, 64 * 1024, 8, SECT_4K, },
         { "sst25vf080b", 0xbf258e, 0, 64 * 1024, 16, SECT_4K, },
         { "sst25vf016b", 0xbf2541, 0, 64 * 1024, 32, SECT_4K, },
         { "sst25vf032b", 0xbf254a, 0, 64 * 1024, 64, SECT_4K, },
+        { "sst25wf512",  0xbf2501, 0, 64 * 1024, 1, SECT_4K, },
+        { "sst25wf010",  0xbf2502, 0, 64 * 1024, 2, SECT_4K, },
+        { "sst25wf020",  0xbf2503, 0, 64 * 1024, 4, SECT_4K, },
+        { "sst25wf040",  0xbf2504, 0, 64 * 1024, 8, SECT_4K, },
 
         /* ST Microelectronics -- newer production may have feature updates */
         { "m25p05",  0x202010,  0, 32 * 1024, 2, },
@@ -667,7 +795,12 @@ static int __devinit m25p_probe(struct spi_device *spi)
         flash->mtd.size = info->sector_size * info->n_sectors;
         flash->mtd.erase = m25p80_erase;
         flash->mtd.read = m25p80_read;
-        flash->mtd.write = m25p80_write;
+
+        /* sst flash chips use AAI word program */
+        if (info->jedec_id >> 16 == 0xbf)
+                flash->mtd.write = sst_write;
+        else
+                flash->mtd.write = m25p80_write;
 
         /* prefer "small sector" erase if possible */
         if (info->flags & SECT_4K) {
diff --git a/drivers/mtd/devices/mtd_dataflash.c b/drivers/mtd/devices/mtd_dataflash.c
index 211c27acd01e..93e3627be74c 100644
--- a/drivers/mtd/devices/mtd_dataflash.c
+++ b/drivers/mtd/devices/mtd_dataflash.c
@@ -401,7 +401,7 @@ static int dataflash_write(struct mtd_info *mtd, loff_t to, size_t len,
                 (void) dataflash_waitready(priv->spi);
 
 
-#ifdef CONFIG_MTD_DATAFLASH_VERIFY_WRITE
+#ifdef CONFIG_MTD_DATAFLASH_WRITE_VERIFY
 
                 /* (3) Compare to Buffer1 */
                 addr = pageaddr << priv->page_offset;
@@ -430,7 +430,7 @@ static int dataflash_write(struct mtd_info *mtd, loff_t to, size_t len,
                 } else
                         status = 0;
 
-#endif  /* CONFIG_MTD_DATAFLASH_VERIFY_WRITE */
+#endif  /* CONFIG_MTD_DATAFLASH_WRITE_VERIFY */
 
                 remaining = remaining - writelen;
                 pageaddr++;
diff --git a/drivers/mtd/devices/phram.c b/drivers/mtd/devices/phram.c
index 088fbb7595b5..1696bbecaa7e 100644
--- a/drivers/mtd/devices/phram.c
+++ b/drivers/mtd/devices/phram.c
@@ -14,6 +14,9 @@
  * Example:
  *      phram=swap,64Mi,128Mi phram=test,900Mi,1Mi
  */
+
+#define pr_fmt(fmt) "phram: " fmt
+
 #include <asm/io.h>
 #include <linux/init.h>
 #include <linux/kernel.h>
@@ -23,8 +26,6 @@
 #include <linux/slab.h>
 #include <linux/mtd/mtd.h>
 
-#define ERROR(fmt, args...) printk(KERN_ERR "phram: " fmt , ## args)
-
 struct phram_mtd_list {
         struct mtd_info mtd;
         struct list_head list;
@@ -132,7 +133,7 @@ static int register_device(char *name, unsigned long start, unsigned long len)
         ret = -EIO;
         new->mtd.priv = ioremap(start, len);
         if (!new->mtd.priv) {
-                ERROR("ioremap failed\n");
+                pr_err("ioremap failed\n");
                 goto out1;
         }
 
@@ -152,7 +153,7 @@ static int register_device(char *name, unsigned long start, unsigned long len)
 
         ret = -EAGAIN;
         if (add_mtd_device(&new->mtd)) {
-                ERROR("Failed to register new device\n");
+                pr_err("Failed to register new device\n");
                 goto out2;
         }
 
@@ -227,8 +228,8 @@ static inline void kill_final_newline(char *str)
 
 
 #define parse_err(fmt, args...) do {    \
-        ERROR(fmt , ## args);   \
-        return 0;               \
+        pr_err(fmt , ## args);  \
+        return 1;               \
 } while (0)
 
 static int phram_setup(const char *val, struct kernel_param *kp)
@@ -256,12 +257,8 @@ static int phram_setup(const char *val, struct kernel_param *kp)
                 parse_err("not enough arguments\n");
 
         ret = parse_name(&name, token[0]);
-        if (ret == -ENOMEM)
-                parse_err("out of memory\n");
-        if (ret == -ENOSPC)
-                parse_err("name too long\n");
         if (ret)
-                return 0;
+                return ret;
 
         ret = parse_num32(&start, token[1]);
         if (ret) {
@@ -275,9 +272,11 @@ static int phram_setup(const char *val, struct kernel_param *kp)
                 parse_err("illegal device length\n");
         }
 
-        register_device(name, start, len);
+        ret = register_device(name, start, len);
+        if (!ret)
+                pr_info("%s device: %#x at %#x\n", name, len, start);
 
-        return 0;
+        return ret;
 }
 
 module_param_call(phram, phram_setup, NULL, NULL, 000);
diff --git a/drivers/mtd/devices/slram.c b/drivers/mtd/devices/slram.c
index 7d846e9173da..3aa05cd18ea1 100644
--- a/drivers/mtd/devices/slram.c
+++ b/drivers/mtd/devices/slram.c
@@ -341,7 +341,7 @@ static int __init init_slram(void)
 #else
         int count;
 
-        for (count = 0; (map[count]) && (count < SLRAM_MAX_DEVICES_PARAMS);
+        for (count = 0; count < SLRAM_MAX_DEVICES_PARAMS && map[count];
                         count++) {
         }
 
diff --git a/drivers/mtd/devices/sst25l.c b/drivers/mtd/devices/sst25l.c
new file mode 100644
index 000000000000..c2baf3353f84
--- /dev/null
+++ b/drivers/mtd/devices/sst25l.c
@@ -0,0 +1,512 @@
+/*
+ * sst25l.c
+ *
+ * Driver for SST25L SPI Flash chips
+ *
+ * Copyright © 2009 Bluewater Systems Ltd
+ * Author: Andre Renaud <andre@bluewatersys.com>
+ * Author: Ryan Mallon <ryan@bluewatersys.com>
+ *
+ * Based on m25p80.c
+ *
+ * This code 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.
+ *
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/mutex.h>
+#include <linux/interrupt.h>
+
+#include <linux/mtd/mtd.h>
+#include <linux/mtd/partitions.h>
+
+#include <linux/spi/spi.h>
+#include <linux/spi/flash.h>
+
+/* Erases can take up to 3 seconds! */
+#define MAX_READY_WAIT_JIFFIES  msecs_to_jiffies(3000)
+
+#define SST25L_CMD_WRSR         0x01    /* Write status register */
+#define SST25L_CMD_WRDI         0x04    /* Write disable */
+#define SST25L_CMD_RDSR         0x05    /* Read status register */
+#define SST25L_CMD_WREN         0x06    /* Write enable */
+#define SST25L_CMD_READ         0x03    /* High speed read */
+
+#define SST25L_CMD_EWSR         0x50    /* Enable write status register */
+#define SST25L_CMD_SECTOR_ERASE 0x20    /* Erase sector */
+#define SST25L_CMD_READ_ID      0x90    /* Read device ID */
+#define SST25L_CMD_AAI_PROGRAM  0xaf    /* Auto address increment */
+
+#define SST25L_STATUS_BUSY      (1 << 0)        /* Chip is busy */
+#define SST25L_STATUS_WREN      (1 << 1)        /* Write enabled */
+#define SST25L_STATUS_BP0       (1 << 2)        /* Block protection 0 */
+#define SST25L_STATUS_BP1       (1 << 3)        /* Block protection 1 */
+
+struct sst25l_flash {
+        struct spi_device       *spi;
+        struct mutex            lock;
+        struct mtd_info         mtd;
+
+        int                     partitioned;
+};
+
+struct flash_info {
+        const char              *name;
+        uint16_t                device_id;
+        unsigned                page_size;
+        unsigned                nr_pages;
+        unsigned                erase_size;
+};
+
+#define to_sst25l_flash(x) container_of(x, struct sst25l_flash, mtd)
+
+static struct flash_info __initdata sst25l_flash_info[] = {
+        {"sst25lf020a", 0xbf43, 256, 1024, 4096},
+        {"sst25lf040a", 0xbf44, 256, 2048, 4096},
+};
+
+static int sst25l_status(struct sst25l_flash *flash, int *status)
+{
+        unsigned char command, response;
+        int err;
+
+        command = SST25L_CMD_RDSR;
+        err = spi_write_then_read(flash->spi, &command, 1, &response, 1);
+        if (err < 0)
+                return err;
+
+        *status = response;
+        return 0;
+}
+
+static int sst25l_write_enable(struct sst25l_flash *flash, int enable)
+{
+        unsigned char command[2];
+        int status, err;
+
+        command[0] = enable ? SST25L_CMD_WREN : SST25L_CMD_WRDI;
+        err = spi_write(flash->spi, command, 1);
+        if (err)
+                return err;
+
+        command[0] = SST25L_CMD_EWSR;
+        err = spi_write(flash->spi, command, 1);
+        if (err)
+                return err;
+
+        command[0] = SST25L_CMD_WRSR;
+        command[1] = enable ? 0 : SST25L_STATUS_BP0 | SST25L_STATUS_BP1;
+        err = spi_write(flash->spi, command, 2);
+        if (err)
+                return err;
+
+        if (enable) {
+                err = sst25l_status(flash, &status);
+                if (err)
+                        return err;
+                if (!(status & SST25L_STATUS_WREN))
+                        return -EROFS;
+        }
+
+        return 0;
+}
+
+static int sst25l_wait_till_ready(struct sst25l_flash *flash)
+{
+        unsigned long deadline;
+        int status, err;
+
+        deadline = jiffies + MAX_READY_WAIT_JIFFIES;
+        do {
+                err = sst25l_status(flash, &status);
+                if (err)
+                        return err;
+                if (!(status & SST25L_STATUS_BUSY))
+                        return 0;
+
+                cond_resched();
+        } while (!time_after_eq(jiffies, deadline));
+
+        return -ETIMEDOUT;
+}
+
+static int sst25l_erase_sector(struct sst25l_flash *flash, uint32_t offset)
+{
+        unsigned char command[4];
+        int err;
+
+        err = sst25l_write_enable(flash, 1);
+        if (err)
+                return err;
+
+        command[0] = SST25L_CMD_SECTOR_ERASE;
+        command[1] = offset >> 16;
+        command[2] = offset >> 8;
+        command[3] = offset;
+        err = spi_write(flash->spi, command, 4);
+        if (err)
+                return err;
+
+        err = sst25l_wait_till_ready(flash);
+        if (err)
+                return err;
+
+        return sst25l_write_enable(flash, 0);
+}
+
+static int sst25l_erase(struct mtd_info *mtd, struct erase_info *instr)
+{
+        struct sst25l_flash *flash = to_sst25l_flash(mtd);
+        uint32_t addr, end;
+        int err;
+
+        /* Sanity checks */
+        if (instr->addr + instr->len > flash->mtd.size)
+                return -EINVAL;
+
+        if ((uint32_t)instr->len % mtd->erasesize)
+                return -EINVAL;
+
+        if ((uint32_t)instr->addr % mtd->erasesize)
+                return -EINVAL;
+
+        addr = instr->addr;
+        end = addr + instr->len;
+
+        mutex_lock(&flash->lock);
+
+        err = sst25l_wait_till_ready(flash);
+        if (err) {
+                mutex_unlock(&flash->lock);
+                return err;
+        }
+
+        while (addr < end) {
+                err = sst25l_erase_sector(flash, addr);
+                if (err) {
+                        mutex_unlock(&flash->lock);
+                        instr->state = MTD_ERASE_FAILED;
+                        dev_err(&flash->spi->dev, "Erase failed\n");
+                        return err;
+                }
+
+                addr += mtd->erasesize;
+        }
+
+        mutex_unlock(&flash->lock);
+
+        instr->state = MTD_ERASE_DONE;
+        mtd_erase_callback(instr);
+        return 0;
+}
+
+static int sst25l_read(struct mtd_info *mtd, loff_t from, size_t len,
+                       size_t *retlen, unsigned char *buf)
+{
+        struct sst25l_flash *flash = to_sst25l_flash(mtd);
+        struct spi_transfer transfer[2];
+        struct spi_message message;
+        unsigned char command[4];
+        int ret;
+
+        /* Sanity checking */
+        if (len == 0)
+                return 0;
+
+        if (from + len > flash->mtd.size)
+                return -EINVAL;
+
+        if (retlen)
+                *retlen = 0;
+
+        spi_message_init(&message);
+        memset(&transfer, 0, sizeof(transfer));
+
+        command[0] = SST25L_CMD_READ;
+        command[1] = from >> 16;
+        command[2] = from >> 8;
+        command[3] = from;
+
+        transfer[0].tx_buf = command;
+        transfer[0].len = sizeof(command);
+        spi_message_add_tail(&transfer[0], &message);
+
+        transfer[1].rx_buf = buf;
+        transfer[1].len = len;
+        spi_message_add_tail(&transfer[1], &message);
+
+        mutex_lock(&flash->lock);
+
+        /* Wait for previous write/erase to complete */
+        ret = sst25l_wait_till_ready(flash);
+        if (ret) {
+                mutex_unlock(&flash->lock);
+                return ret;
+        }
+
+        spi_sync(flash->spi, &message);
+
+        if (retlen && message.actual_length > sizeof(command))
+                *retlen += message.actual_length - sizeof(command);
+
+        mutex_unlock(&flash->lock);
+        return 0;
+}
+
+static int sst25l_write(struct mtd_info *mtd, loff_t to, size_t len,
+                        size_t *retlen, const unsigned char *buf)
+{
+        struct sst25l_flash *flash = to_sst25l_flash(mtd);
+        int i, j, ret, bytes, copied = 0;
+        unsigned char command[5];
+
+        /* Sanity checks */
+        if (!len)
+                return 0;
+
+        if (to + len > flash->mtd.size)
+                return -EINVAL;
+
+        if ((uint32_t)to % mtd->writesize)
+                return -EINVAL;
+
+        mutex_lock(&flash->lock);
+
+        ret = sst25l_write_enable(flash, 1);
+        if (ret)
+                goto out;
+
+        for (i = 0; i < len; i += mtd->writesize) {
+                ret = sst25l_wait_till_ready(flash);
+                if (ret)
+                        goto out;
+
+                /* Write the first byte of the page */
+                command[0] = SST25L_CMD_AAI_PROGRAM;
+                command[1] = (to + i) >> 16;
+                command[2] = (to + i) >> 8;
+                command[3] = (to + i);
+                command[4] = buf[i];
+                ret = spi_write(flash->spi, command, 5);
+                if (ret < 0)
+                        goto out;
+                copied++;
+
+                /*
+                 * Write the remaining bytes using auto address
+                 * increment mode
+                 */
+                bytes = min_t(uint32_t, mtd->writesize, len - i);
+                for (j = 1; j < bytes; j++, copied++) {
+                        ret = sst25l_wait_till_ready(flash);
+                        if (ret)
+                                goto out;
+
+                        command[1] = buf[i + j];
+                        ret = spi_write(flash->spi, command, 2);
+                        if (ret)
+                                goto out;
+                }
+        }
+
+out:
+        ret = sst25l_write_enable(flash, 0);
+
+        if (retlen)
+                *retlen = copied;
+
+        mutex_unlock(&flash->lock);
+        return ret;
+}
+
+static struct flash_info *__init sst25l_match_device(struct spi_device *spi)
+{
+        struct flash_info *flash_info = NULL;
+        unsigned char command[4], response;
+        int i, err;
+        uint16_t id;
+
+        command[0] = SST25L_CMD_READ_ID;
+        command[1] = 0;
+        command[2] = 0;
+        command[3] = 0;
+        err = spi_write_then_read(spi, command, sizeof(command), &response, 1);
+        if (err < 0) {
+                dev_err(&spi->dev, "error reading device id msb\n");
+                return NULL;
+        }
+
+        id = response << 8;
+
+        command[0] = SST25L_CMD_READ_ID;
+        command[1] = 0;
+        command[2] = 0;
+        command[3] = 1;
+        err = spi_write_then_read(spi, command, sizeof(command), &response, 1);
+        if (err < 0) {
+                dev_err(&spi->dev, "error reading device id lsb\n");
+                return NULL;
+        }
+
+        id |= response;
+
+        for (i = 0; i < ARRAY_SIZE(sst25l_flash_info); i++)
+                if (sst25l_flash_info[i].device_id == id)
+                        flash_info = &sst25l_flash_info[i];
+
+        if (!flash_info)
+                dev_err(&spi->dev, "unknown id %.4x\n", id);
+
+        return flash_info;
+}
+
+static int __init sst25l_probe(struct spi_device *spi)
+{
+        struct flash_info *flash_info;
+        struct sst25l_flash *flash;
+        struct flash_platform_data *data;
+        int ret, i;
+
+        flash_info = sst25l_match_device(spi);
+        if (!flash_info)
+                return -ENODEV;
+
+        flash = kzalloc(sizeof(struct sst25l_flash), GFP_KERNEL);
+        if (!flash)
+                return -ENOMEM;
+
+        flash->spi = spi;
+        mutex_init(&flash->lock);
+        dev_set_drvdata(&spi->dev, flash);
+
+        data = spi->dev.platform_data;
+        if (data && data->name)
+                flash->mtd.name = data->name;
+        else
+                flash->mtd.name = dev_name(&spi->dev);
+
+        flash->mtd.type         = MTD_NORFLASH;
+        flash->mtd.flags        = MTD_CAP_NORFLASH;
+        flash->mtd.erasesize    = flash_info->erase_size;
+        flash->mtd.writesize    = flash_info->page_size;
+        flash->mtd.size         = flash_info->page_size * flash_info->nr_pages;
+        flash->mtd.erase        = sst25l_erase;
+        flash->mtd.read         = sst25l_read;
+        flash->mtd.write        = sst25l_write;
+
+        dev_info(&spi->dev, "%s (%lld KiB)\n", flash_info->name,
+                 (long long)flash->mtd.size >> 10);
+
+        DEBUG(MTD_DEBUG_LEVEL2,
+              "mtd .name = %s, .size = 0x%llx (%lldMiB) "
+              ".erasesize = 0x%.8x (%uKiB) .numeraseregions = %d\n",
+              flash->mtd.name,
+              (long long)flash->mtd.size, (long long)(flash->mtd.size >> 20),
+              flash->mtd.erasesize, flash->mtd.erasesize / 1024,
+              flash->mtd.numeraseregions);
+
+        if (flash->mtd.numeraseregions)
+                for (i = 0; i < flash->mtd.numeraseregions; i++)
+                        DEBUG(MTD_DEBUG_LEVEL2,
+                              "mtd.eraseregions[%d] = { .offset = 0x%llx, "
+                              ".erasesize = 0x%.8x (%uKiB), "
+                              ".numblocks = %d }\n",
+                              i, (long long)flash->mtd.eraseregions[i].offset,
+                              flash->mtd.eraseregions[i].erasesize,
+                              flash->mtd.eraseregions[i].erasesize / 1024,
+                              flash->mtd.eraseregions[i].numblocks);
+
+        if (mtd_has_partitions()) {
+                struct mtd_partition *parts = NULL;
+                int nr_parts = 0;
+
+                if (mtd_has_cmdlinepart()) {
+                        static const char *part_probes[] =
+                                {"cmdlinepart", NULL};
+
+                        nr_parts = parse_mtd_partitions(&flash->mtd,
+                                                        part_probes,
+                                                        &parts, 0);
+                }
+
+                if (nr_parts <= 0 && data && data->parts) {
+                        parts = data->parts;
+                        nr_parts = data->nr_parts;
+                }
+
+                if (nr_parts > 0) {
+                        for (i = 0; i < nr_parts; i++) {
+                                DEBUG(MTD_DEBUG_LEVEL2, "partitions[%d] = "
+                                      "{.name = %s, .offset = 0x%llx, "
+                                      ".size = 0x%llx (%lldKiB) }\n",
+                                      i, parts[i].name,
+                                      (long long)parts[i].offset,
+                                      (long long)parts[i].size,
+                                      (long long)(parts[i].size >> 10));
+                        }
+
+                        flash->partitioned = 1;
+                        return add_mtd_partitions(&flash->mtd,
+                                                  parts, nr_parts);
+                }
+
+        } else if (data->nr_parts) {
+                dev_warn(&spi->dev, "ignoring %d default partitions on %s\n",
+                         data->nr_parts, data->name);
+        }
+
+        ret = add_mtd_device(&flash->mtd);
+        if (ret == 1) {
+                kfree(flash);
+                dev_set_drvdata(&spi->dev, NULL);
+                return -ENODEV;
+        }
+
+        return 0;
+}
+
+static int __exit sst25l_remove(struct spi_device *spi)
+{
+        struct sst25l_flash *flash = dev_get_drvdata(&spi->dev);
+        int ret;
+
+        if (mtd_has_partitions() && flash->partitioned)
+                ret = del_mtd_partitions(&flash->mtd);
+        else
+                ret = del_mtd_device(&flash->mtd);
+        if (ret == 0)
+                kfree(flash);
+        return ret;
+}
+
+static struct spi_driver sst25l_driver = {
+        .driver = {
+                .name   = "sst25l",
+                .bus    = &spi_bus_type,
+                .owner  = THIS_MODULE,
+        },
+        .probe          = sst25l_probe,
+        .remove         = __exit_p(sst25l_remove),
+};
+
+static int __init sst25l_init(void)
+{
+        return spi_register_driver(&sst25l_driver);
+}
+
+static void __exit sst25l_exit(void)
+{
+        spi_unregister_driver(&sst25l_driver);
+}
+
+module_init(sst25l_init);
+module_exit(sst25l_exit);
+
+MODULE_DESCRIPTION("MTD SPI driver for SST25L Flash chips");
+MODULE_AUTHOR("Andre Renaud <andre@bluewatersys.com>, "
+              "Ryan Mallon <ryan@bluewatersys.com>");
+MODULE_LICENSE("GPL");