+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;
+}