1 files changed, 125 insertions, 1 deletions

diff --git a/drivers/mtd/devices/m25p80.c b/drivers/mtd/devices/m25p80.c
index 6d8d265ae91a..53de9f05ad50 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;
+}
 
 /****************************************************************************/
 
@@ -670,7 +789,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) {