From 1b1c21a0230b1c30512412f6232e298fb58724f4 Mon Sep 17 00:00:00 2001
From: Nikolay Nikolaev <evrinoma@gmail.com>
Date: Sat, 21 Dec 2019 23:05:40 +0300
Subject: [PATCH] It's added support for ST95XXX chips . The following chipsets
 have been tested for read, erase and write operations: [ STM95080 STM95160
 STM95320 STM95640 STM95128 STM95256 STM95512 STM95M01 STM95M02 ]

The chipsets(except st95xxx with literal D) can't respond with RDID instruction and for this reason was added FEATURE_IDENTITY_MISSING feature. The feature works with "force" option and disable rdid checking during read operation.
Also It's emulates erase operation for chipsets which don't support it.

Known issue: It doesn't support chipset STM95040 because memory size lower then 1K and the chipset has special instruction set for read operation.

Signed-off-by: Nikolay Nikolaev <evrinoma@gmail.com>
---
 Makefile      |   2 +-
 chipdrivers.h |   5 +-
 cli_classic.c |   5 +-
 flash.h       |   2 +
 flashchips.c  | 237 +++++++++++++++++++++++++++++++++++++++++++++++++-
 flashchips.h  |  16 +++-
 flashrom.c    |  10 ++-
 spi.h         |  29 ++++--
 spi25.c       |  92 ++++++++++++++++++--
 spi95.c       |  69 ---------------
 10 files changed, 371 insertions(+), 96 deletions(-)
 delete mode 100644 spi95.c

diff --git a/Makefile b/Makefile
index 518d41b..d345d23 100644
--- a/Makefile
+++ b/Makefile
@@ -555,7 +555,7 @@ endif
 CHIP_OBJS = jedec.o stm50.o w39.o w29ee011.o \
 	sst28sf040.o 82802ab.o \
 	sst49lfxxxc.o sst_fwhub.o edi.o flashchips.o spi.o spi25.o spi25_statusreg.o \
-	spi95.o opaque.o sfdp.o en29lv640b.o at45db.o
+	opaque.o sfdp.o en29lv640b.o at45db.o
 
 ###############################################################################
 # Library code.
diff --git a/chipdrivers.h b/chipdrivers.h
index cb1e877..2afa191 100644
--- a/chipdrivers.h
+++ b/chipdrivers.h
@@ -34,6 +34,7 @@ int probe_spi_rems(struct flashctx *flash);
 int probe_spi_res1(struct flashctx *flash);
 int probe_spi_res2(struct flashctx *flash);
 int probe_spi_res3(struct flashctx *flash);
+int probe_spi_st95(struct flashctx *flash);
 int probe_spi_at25f(struct flashctx *flash);
 int spi_write_enable(struct flashctx *flash);
 int spi_write_disable(struct flashctx *flash);
@@ -51,6 +52,7 @@ int spi_block_erase_d7(struct flashctx *flash, unsigned int addr, unsigned int b
 int spi_block_erase_d8(struct flashctx *flash, unsigned int addr, unsigned int blocklen);
 int spi_block_erase_db(struct flashctx *flash, unsigned int addr, unsigned int blocklen);
 int spi_block_erase_dc(struct flashctx *flash, unsigned int addr, unsigned int blocklen);
+int spi_block_erase_emulation(struct flashctx *flash, unsigned int addr, unsigned int blocklen);
 erasefunc_t *spi_get_erasefn_from_opcode(uint8_t opcode);
 int spi_chip_write_1(struct flashctx *flash, const uint8_t *buf, unsigned int start, unsigned int len);
 int spi_nbyte_read(struct flashctx *flash, unsigned int addr, uint8_t *bytes, unsigned int len);
@@ -202,8 +204,5 @@ int edi_chip_write(struct flashctx *flash, const uint8_t *buf, unsigned int star
 int edi_chip_read(struct flashctx *flash, uint8_t *buf, unsigned int start, unsigned int len);
 int edi_probe_kb9012(struct flashctx *flash);
 
-/* spi95.c */
-int probe_spi_st95(struct flashctx *flash);
-int spi_block_erase_emulation(struct flashctx *flash, unsigned int addr, unsigned int blocklen);
 
 #endif /* !__CHIPDRIVERS_H__ */
diff --git a/cli_classic.c b/cli_classic.c
index 73cc417..08c9316 100644
--- a/cli_classic.c
+++ b/cli_classic.c
@@ -468,7 +468,8 @@ int main(int argc, char *argv[])
 	for (j = 0; j < registered_master_count; j++) {
 		startchip = 0;
 		while (chipcount < (int)ARRAY_SIZE(flashes)) {
-			startchip = probe_flash(&registered_masters[j], startchip, &flashes[chipcount], 0);
+            startchip = probe_flash(&registered_masters[j], startchip, &flashes[chipcount], (chip_to_probe) && force );
+
 			if (startchip == -1)
 				break;
 			chipcount++;
@@ -511,7 +512,7 @@ int main(int argc, char *argv[])
 					  "chip, using the first one.\n");
 			for (j = 0; j < registered_master_count; j++) {
 				mst = &registered_masters[j];
-				startchip = probe_flash(mst, 0, &flashes[0], 1);
+				startchip = probe_flash(mst, 0, &flashes[0], 2);
 				if (startchip != -1)
 					break;
 			}
diff --git a/flash.h b/flash.h
index 1a9bd9f..94a2548 100644
--- a/flash.h
+++ b/flash.h
@@ -140,6 +140,8 @@ enum write_granularity {
  */
 #define FEATURE_ERASED_ZERO	(1 << 17)
 #define FEATURE_NO_ERASE	(1 << 18)
+#define FEATURE_3_BYTE_ADDR_LEN     (1 << 19)
+#define FEATURE_IDENTITY_MISSING	(1 << 20)
 
 #define ERASED_VALUE(flash)	(((flash)->chip->feature_bits & FEATURE_ERASED_ZERO) ? 0x00 : 0xff)
 
diff --git a/flashchips.c b/flashchips.c
index 65dda0e..09f1809 100644
--- a/flashchips.c
+++ b/flashchips.c
@@ -14808,6 +14808,33 @@ const struct flashchip flashchips[] = {
 		.voltage	= {3000, 3600}, /* Also has 12V fast program & erase */
 	},
 
+    {
+            .vendor		= "ST",
+            .name		= "M95M01",
+            .bustype	= BUS_SPI,
+            .manufacture_id	= ST_ID,
+            .model_id	= ST_M95M01,
+            .total_size	= 128,
+            .page_size	= 256,
+            .feature_bits	= FEATURE_WRSR_WREN | FEATURE_IDENTITY_MISSING,
+            .tested		= TEST_OK_PREW,
+            .probe		= probe_spi_st95,
+            .probe_timing	= TIMING_ZERO,
+            .block_erasers	=
+                    {
+                            {
+                                    .eraseblocks = { {128 * 1024, 1} },
+                                    .block_erase = spi_block_erase_emulation,
+                            }
+                    },
+
+            .printlock	= spi_prettyprint_status_register_bp1_srwd,
+            .unlock		= spi_disable_blockprotect_bp1_srwd,
+            .write		= spi_chip_write_1,
+            .read		= spi_chip_read,
+            .voltage	= {1800, 5500},
+    },
+
 	{
 		.vendor		= "ST",
 		.name		= "M95M02",
@@ -14816,7 +14843,7 @@ const struct flashchip flashchips[] = {
 		.model_id	= ST_M95M02,
 		.total_size	= 256,
 		.page_size	= 256,
-		.feature_bits	= FEATURE_WRSR_WREN | FEATURE_NO_ERASE | FEATURE_ERASED_ZERO,
+		.feature_bits	= FEATURE_WRSR_WREN | FEATURE_NO_ERASE | FEATURE_ERASED_ZERO  | FEATURE_IDENTITY_MISSING,
 		.tested		= TEST_OK_PREW,
 		.probe		= probe_spi_st95,
 		.probe_timing	= TIMING_ZERO,
@@ -15967,6 +15994,214 @@ const struct flashchip flashchips[] = {
 		.wrea_override	= 0x17,
 	},
 
+//    {
+//            .vendor		= "ST",
+//            .name		= "M95040",
+//            .bustype	= BUS_SPI,
+//            .manufacture_id	= ST_ID,
+//            .model_id	= ST_M95040,
+//            .total_size	= 512,
+//            .page_size	= 16,
+//            .feature_bits	= FEATURE_WRSR_WREN | FEATURE_3_BYTE_ADDR_LEN| FEATURE_CAPACITY_IN_BYTE | FEATURE_IDENTITY_MISSING,
+//            .tested		= TEST_UNTESTED,
+//            .probe		= probe_spi_st95,
+//            .probe_timing	= TIMING_ZERO,
+//            .block_erasers	=
+//                    {
+//                            {
+//                                    .eraseblocks = { { 512 * 1, 1 } },
+//                                    .block_erase = spi_block_erase_emulation,
+//                            }
+//                    },
+//            .printlock	= spi_prettyprint_status_register_bp1_srwd,
+//            .unlock		= spi_disable_blockprotect_bp1_srwd,
+//            .write		= spi_chip_write_1,
+//            .read		= spi_chip_read,
+//            .voltage	= {2500, 5500},
+//    },
+
+    {
+            .vendor		= "ST",
+            .name		= "M95080",
+            .bustype	= BUS_SPI,
+            .manufacture_id	= ST_ID,
+            .model_id	= ST_M95080,
+            .total_size	= 1,
+            .page_size	= 32,
+            .feature_bits	= FEATURE_WRSR_WREN | FEATURE_3_BYTE_ADDR_LEN | FEATURE_IDENTITY_MISSING,
+            .tested		= TEST_OK_PREW,
+            .probe		= probe_spi_st95,
+            .probe_timing	= TIMING_ZERO,
+            .block_erasers	=
+                    {
+                            {
+                                    .eraseblocks = { { 1 * 1024, 1 } },
+                                    .block_erase = spi_block_erase_emulation,
+                            }
+                    },
+            .printlock	= spi_prettyprint_status_register_bp1_srwd,
+            .unlock		= spi_disable_blockprotect_bp1_srwd,
+            .write		= spi_chip_write_1,
+            .read		= spi_chip_read,
+            .voltage	= {2500, 5500},
+    },
+
+    {
+            .vendor		= "ST",
+            .name		= "M95160",
+            .bustype	= BUS_SPI,
+            .manufacture_id	= ST_ID,
+            .model_id	= ST_M95160,
+            .total_size	= 2,
+            .page_size	= 32,
+            .feature_bits	= FEATURE_WRSR_WREN | FEATURE_3_BYTE_ADDR_LEN | FEATURE_IDENTITY_MISSING,
+            .tested		= TEST_OK_PREW,
+            .probe		= probe_spi_st95,
+            .probe_timing	= TIMING_ZERO,
+            .block_erasers	=
+                    {
+                            {
+                                    .eraseblocks = { { 2 * 1024, 1 } },
+                                    .block_erase = spi_block_erase_emulation,
+                            }
+                    },
+            .printlock	= spi_prettyprint_status_register_bp1_srwd,
+            .unlock		= spi_disable_blockprotect_bp1_srwd,
+            .write		= spi_chip_write_1,
+            .read		= spi_chip_read,
+            .voltage	= {2500, 5500},
+    },
+
+    {
+            .vendor		= "ST",
+            .name		= "M95320",
+            .bustype	= BUS_SPI,
+            .manufacture_id	= ST_ID,
+            .model_id	= ST_M95320,
+            .total_size	= 4,
+            .page_size	= 32,
+            .feature_bits	= FEATURE_WRSR_WREN | FEATURE_3_BYTE_ADDR_LEN | FEATURE_IDENTITY_MISSING,
+            .tested		= TEST_OK_PREW,
+            .probe		= probe_spi_st95,
+            .probe_timing	= TIMING_ZERO,
+            .block_erasers	=
+                    {
+                            {
+                                    .eraseblocks = { { 4 * 1024, 1 } },
+                                    .block_erase = spi_block_erase_emulation,
+                            }
+                    },
+            .printlock	= spi_prettyprint_status_register_bp1_srwd,
+            .unlock		= spi_disable_blockprotect_bp1_srwd,
+            .write		= spi_chip_write_1,
+            .read		= spi_chip_read,
+            .voltage	= {2500, 5500},
+    },
+
+    {
+            .vendor		= "ST",
+            .name		= "M95640",
+            .bustype	= BUS_SPI,
+            .manufacture_id	= ST_ID,
+            .model_id	= ST_M95640,
+            .total_size	= 8,
+            .page_size	= 32,
+            .feature_bits	= FEATURE_WRSR_WREN | FEATURE_3_BYTE_ADDR_LEN | FEATURE_IDENTITY_MISSING,
+            .tested		= TEST_OK_PREW,
+            .probe		= probe_spi_st95,
+            .probe_timing	= TIMING_ZERO,
+            .block_erasers	=
+                    {
+                            {
+                                    .eraseblocks = { { 8 * 1024, 1 } },
+                                    .block_erase = spi_block_erase_emulation,
+                            }
+                    },
+            .printlock	= spi_prettyprint_status_register_bp1_srwd,
+            .unlock		= spi_disable_blockprotect_bp1_srwd,
+            .write		= spi_chip_write_1,
+            .read		= spi_chip_read,
+            .voltage	= {2500, 5500},
+    },
+
+    {
+            .vendor		= "ST",
+            .name		= "M95128",
+            .bustype	= BUS_SPI,
+            .manufacture_id	= ST_ID,
+            .model_id	= ST_M95128,
+            .total_size	= 16,
+            .page_size	= 64,
+            .feature_bits	= FEATURE_WRSR_WREN | FEATURE_3_BYTE_ADDR_LEN | FEATURE_IDENTITY_MISSING,
+            .tested		= TEST_OK_PREW,
+            .probe		= probe_spi_st95,
+            .probe_timing	= TIMING_ZERO,
+            .block_erasers	=
+                    {
+                            {
+                                    .eraseblocks = { { 16 * 1024, 1 } },
+                                    .block_erase = spi_block_erase_emulation,
+                            }
+                    },
+            .printlock	= spi_prettyprint_status_register_bp1_srwd,
+            .unlock		= spi_disable_blockprotect_bp1_srwd,
+            .write		= spi_chip_write_1,
+            .read		= spi_chip_read,
+            .voltage	= {2500, 5500},
+    },
+
+    {
+            .vendor		= "ST",
+            .name		= "M95256",
+            .bustype	= BUS_SPI,
+            .manufacture_id	= ST_ID,
+            .model_id	= ST_M95256,
+            .total_size	= 32,
+            .page_size	= 64,
+            .feature_bits	= FEATURE_WRSR_WREN | FEATURE_3_BYTE_ADDR_LEN | FEATURE_IDENTITY_MISSING,
+            .tested		= TEST_OK_PREW,
+            .probe		= probe_spi_st95,
+            .probe_timing	= TIMING_ZERO,
+            .block_erasers	=
+                    {
+                            {
+                                    .eraseblocks = { { 32 * 1024, 1 } },
+                                    .block_erase = spi_block_erase_emulation,
+                            }
+                    },
+            .printlock	= spi_prettyprint_status_register_bp1_srwd,
+            .unlock		= spi_disable_blockprotect_bp1_srwd,
+            .write		= spi_chip_write_1,
+            .read		= spi_chip_read,
+            .voltage	= {2500, 5500},
+    },
+
+    {
+            .vendor		= "ST",
+            .name		= "M95512",
+            .bustype	= BUS_SPI,
+            .manufacture_id	= ST_ID,
+            .model_id	= ST_M95512,
+            .total_size	= 64,
+            .page_size	= 128,
+            .feature_bits	= FEATURE_WRSR_WREN | FEATURE_3_BYTE_ADDR_LEN | FEATURE_IDENTITY_MISSING,
+            .tested		= TEST_OK_PREW,
+            .probe		= probe_spi_st95,
+            .probe_timing	= TIMING_ZERO,
+            .block_erasers	=
+                    {
+                            {
+                                    .eraseblocks = { { 64 * 1024, 1 } },
+                                    .block_erase = spi_block_erase_emulation,
+                            }
+                    },
+            .printlock	= spi_prettyprint_status_register_bp1_srwd,
+            .unlock		= spi_disable_blockprotect_bp1_srwd,
+            .write		= spi_chip_write_1,
+            .read		= spi_chip_read,
+            .voltage	= {2500, 5500},
+    },
+
 	{
 		.vendor		= "SyncMOS/MoselVitelic",
 		.name		= "{F,S,V}29C51001B",
diff --git a/flashchips.h b/flashchips.h
index f02958c..755c3a5 100644
--- a/flashchips.h
+++ b/flashchips.h
@@ -853,9 +853,19 @@
 #define ST_M58WR032KT		0x8814
 #define ST_M58WR064KB		0x8811
 #define ST_M58WR064KT		0x8810
-
-#define ST_M95M02		0x0012	/* ST M95XXX 2Mbit (256KiB) */
-
+/* 00h Memory Density code ST_ID*/
+/* 01h SPI Family code     0x00*/
+/* 02h Memory Density code ST_M95XXX */
+#define ST_M95040           0x0009
+#define ST_M95080           0x000A
+#define ST_M95160           0x000B
+#define ST_M95320           0x000C
+#define ST_M95640           0x000D
+#define ST_M95128           0x000E
+#define ST_M95256           0x000F
+#define ST_M95512           0x0010  /* FIXME: 0x0010 it's dummy cause don't mentioned in documentation. The device is delivered with the memory array and Identification Page bits set to all 1s (each byte = FFh).*/
+#define ST_M95M01		    0x0011  /* FIXME: 0x0011 it's dummy cause don't mentioned in documentation. The device is delivered with the memory array and Identification Page bits set to all 1s (each byte = FFh).*/
+#define ST_M95M02		    0x0012	/* ST M95XXX 2Mbit (256KiB)  FIXME: 0x0012 Is this correct value? The device is delivered with the memory array and Identification Page bits set to all 1s (each byte = FFh). */
 #define ST_MT28GU01G___1	0x88B0
 #define ST_MT28GU01G___2	0x88B1
 #define ST_MT28GU256___1	0x8901
diff --git a/flashrom.c b/flashrom.c
index 2534e4a..8f5cd85 100644
--- a/flashrom.c
+++ b/flashrom.c
@@ -1153,9 +1153,13 @@ int probe_flash(struct registered_master *mst, int startchip, struct flashctx *f
 		/* We handle a forced match like a real match, we just avoid probing. Note that probe_flash()
 		 * is only called with force=1 after normal probing failed.
 		 */
-		if (force)
+		if (force == 2)
 			break;
 
+		if (force == 1 && chip->feature_bits & FEATURE_IDENTITY_MISSING) {
+            flash->flags.force = force;
+		}
+
 		if (flash->chip->probe(flash) != 1)
 			goto notfound;
 
@@ -1219,7 +1223,7 @@ notfound:
 	}
 
 	tmp = flashbuses_to_text(flash->chip->bustype);
-	msg_cinfo("%s %s flash chip \"%s\" (%d kB, %s) ", force ? "Assuming" : "Found",
+	msg_cinfo("%s %s flash chip \"%s\" (%d kB, %s) ", force == 2 ? "Assuming" : "Found",
 		  flash->chip->vendor, flash->chip->name, flash->chip->total_size, tmp);
 	free(tmp);
 #if CONFIG_INTERNAL == 1
@@ -1555,7 +1559,7 @@ static int walk_by_layout(struct flashctx *const flashctx, struct walk_info *con
 	const struct romentry *entry = NULL;
 
 	all_skipped = true;
-	msg_cinfo("Erasing and writing flash chip... ");
+	msg_cinfo("Erasing and writing flash chip...\n");
 
 	while ((entry = layout_next_included(layout, entry))) {
 		info->region_start = entry->start;
diff --git a/spi.h b/spi.h
index 3f45038..64a07e6 100644
--- a/spi.h
+++ b/spi.h
@@ -21,6 +21,7 @@
  */
 
 #define JEDEC_MAX_ADDR_LEN	0x04
+#define JEDEC_3_BYTE_ADDR_LEN	0x03
 
 /* Read Electronic ID */
 #define JEDEC_RDID		0x9f
@@ -28,12 +29,28 @@
 /* INSIZE may be 0x04 for some chips*/
 #define JEDEC_RDID_INSIZE	0x03
 
-/* Some ST M95X model */
-#define ST_M95_RDID		0x83
-#define ST_M95_RDID_3BA_OUTSIZE	0x04	/* 8b op, 24bit addr where size >64KiB */
-#define ST_M95_RDID_2BA_OUTSIZE	0x03	/* 8b op, 16bit addr where size <=64KiB */
-#define ST_M95_RDID_OUTSIZE_MAX 0x04	/* ST_M95_RDID_3BA_OUTSIZE */
-#define ST_M95_RDID_INSIZE	0x03
+/* Some ST M95X model  */
+#define ST_M95_RDID		        0x83
+#define ST_M95_RDID_OUTSIZE	    0x03
+#define ST_M95_RDID_INSIZE  	0x03
+#define ST_M95_RDLS		        0x83
+#define ST_M95_RDLS_OUTSIZE	    0x03
+#define ST_M95_RDLS_INSIZE  	0x01
+#define ST_M95_RDSR		        0x05
+#define ST_M95_RDSR_OUTSIZE	    0x01
+#define ST_M95_RDSR_INSIZE	    0x01
+#define ST_M95_READ		        0x03
+#define ST_M95_READ_OUTSIZE 	0x03
+#define ST_M95_READ_INSIZE  	0x03
+#define ST_M95_WREN 	        0x06
+#define ST_M95_WREN_OUTSIZE     0x01
+#define ST_M95_WREN_INSIZE      0x00
+#define ST_M95_WRITE	        0x02
+#define ST_M95_WRITE_OUTSIZE	0x06
+#define ST_M95_WRITE_INSIZE 	0x03
+#define ST_M95_WRID             0x82
+#define ST_M95_WRID_OUTSIZE 	0x06
+#define ST_M95_WRID_INSIZE  	0x00
 
 /* Some Atmel AT25F* models have bit 3 as don't care bit in commands */
 #define AT25F_RDID		0x15	/* 0x15 or 0x1d */
diff --git a/spi25.c b/spi25.c
index 2a1d492..3c0579a 100644
--- a/spi25.c
+++ b/spi25.c
@@ -21,6 +21,7 @@
 #include <stddef.h>
 #include <string.h>
 #include <stdbool.h>
+#include <stdlib.h>
 #include "flash.h"
 #include "flashchips.h"
 #include "chipdrivers.h"
@@ -262,6 +263,31 @@ int probe_spi_res3(struct flashctx *flash)
 	return 1;
 }
 
+/* Only used for some stm95 and higher chip. */
+int probe_spi_st95(struct flashctx *flash)
+{
+//
+    static const unsigned char cmd[JEDEC_RDID_OUTSIZE] = { JEDEC_RDID };
+    unsigned char readarr[JEDEC_RDID_INSIZE];
+    uint32_t manufacture_id;
+    uint32_t model_id;
+
+    spi_send_command(flash, sizeof(cmd), sizeof(readarr), cmd, readarr);
+
+    manufacture_id = readarr[0];
+    model_id = readarr[2];
+
+    msg_ginfo("RDID[%s: manID 0x%02x, modID 0x%02x, L 0x%02x, M 0x%02x, H 0x%02x]\n", __func__, flash->chip->manufacture_id,  flash->chip->model_id, readarr[0], readarr[1], readarr[2]);
+
+    if (manufacture_id == flash->chip->manufacture_id && model_id == flash->chip->model_id)
+        return 1;
+
+    if (flash->flags.force)
+        return 1;
+
+    return 0;
+}
+
 /* Only used for some Atmel chips. */
 int probe_spi_at25f(struct flashctx *flash)
 {
@@ -379,10 +405,16 @@ static int spi_prepare_address(struct flashctx *const flash, uint8_t cmd_buf[],
 				 "with this chip/programmer combination.\n", cmd_buf[0]);
 			return -1;
 		}
-		cmd_buf[1] = (addr >> 16) & 0xff;
-		cmd_buf[2] = (addr >>  8) & 0xff;
-		cmd_buf[3] = (addr >>  0) & 0xff;
-		return 3;
+		if (flash->chip->feature_bits & FEATURE_3_BYTE_ADDR_LEN) {
+            cmd_buf[1] = (addr >> 8) & 0xff;
+            cmd_buf[2] = (addr >> 0) & 0xff;
+            return 2;
+        } else  {
+            cmd_buf[1] = (addr >> 16) & 0xff;
+            cmd_buf[2] = (addr >> 8) & 0xff;
+            cmd_buf[3] = (addr >> 0) & 0xff;
+            return 3;
+        }
 	}
 }
 
@@ -405,7 +437,16 @@ static int spi_write_cmd(struct flashctx *const flash, const uint8_t op,
 			 const uint8_t *const out_bytes, const size_t out_len,
 			 const unsigned int poll_delay)
 {
-	uint8_t cmd[1 + JEDEC_MAX_ADDR_LEN + 256];
+    const unsigned int cmd_len = 1 + (flash->chip->feature_bits & FEATURE_3_BYTE_ADDR_LEN ? JEDEC_3_BYTE_ADDR_LEN : JEDEC_MAX_ADDR_LEN) + 256;
+    uint8_t *cmd = NULL;
+
+    cmd = (uint8_t*) malloc(cmd_len * sizeof(uint8_t));
+    if (!cmd) {
+        msg_cerr("Out of memory!\n");
+        return 1;
+    }
+   memset(cmd,0x00, cmd_len * sizeof(uint8_t));
+
 	struct spi_command cmds[] = {
 	{
 		.readarr = 0,
@@ -423,7 +464,7 @@ static int spi_write_cmd(struct flashctx *const flash, const uint8_t op,
 	if (addr_len < 0)
 		return 1;
 
-	if (1 + addr_len + out_len > sizeof(cmd)) {
+	if (1 + addr_len + out_len > cmd_len) {
 		msg_cerr("%s called for too long a write\n", __func__);
 		return 1;
 	}
@@ -436,6 +477,7 @@ static int spi_write_cmd(struct flashctx *const flash, const uint8_t op,
 		msg_cerr("%s failed during command execution at address 0x%x\n", __func__, addr);
 
 	const int status = spi_poll_wip(flash, poll_delay);
+    free(cmd);
 
 	return result ? result : status;
 }
@@ -577,6 +619,26 @@ int spi_block_erase_dc(struct flashctx *flash, unsigned int addr, unsigned int b
 	return spi_write_cmd(flash, 0xdc, true, addr, NULL, 0, 100 * 1000);
 }
 
+/* ST95XXX chips don't have an erase operation and erase is made as part of write command */
+int spi_block_erase_emulation(struct flashctx *flash, unsigned int addr, unsigned int blocklen)
+{
+    uint8_t *erased_contents = NULL;
+    int result = 0;
+
+    erased_contents = (uint8_t *)malloc(blocklen * sizeof(uint8_t));
+    if (!erased_contents) {
+        msg_cerr("Out of memory!\n");
+        return 1;
+    }
+    memset(erased_contents, ERASED_VALUE(flash), blocklen * sizeof(uint8_t));
+    msg_cinfo("\nIt takes too long!\n");
+
+    result = spi_write_chunked(flash, erased_contents, 0, blocklen, flash->chip->page_size);
+    free(erased_contents);
+
+    return result;
+}
+
 erasefunc_t *spi_get_erasefn_from_opcode(uint8_t opcode)
 {
 	switch(opcode){
@@ -630,14 +692,28 @@ int spi_nbyte_read(struct flashctx *flash, unsigned int address, uint8_t *bytes,
 		   unsigned int len)
 {
 	const bool native_4ba =	flash->chip->feature_bits & FEATURE_4BA_READ && spi_master_4ba(flash);
-	uint8_t cmd[1 + JEDEC_MAX_ADDR_LEN] = { native_4ba ? JEDEC_READ_4BA : JEDEC_READ, };
+    const unsigned int cmd_len = 1 + (flash->chip->feature_bits & FEATURE_3_BYTE_ADDR_LEN ? JEDEC_3_BYTE_ADDR_LEN : JEDEC_MAX_ADDR_LEN);
+    int result;
+    uint8_t *cmd = NULL;
+
+    cmd = (uint8_t*) malloc(cmd_len * sizeof(uint8_t));
+    if (!cmd) {
+        msg_cerr("Out of memory!\n");
+        return 1;
+    }
+    memset(cmd, 0x00, cmd_len * sizeof(uint8_t));
+
+	cmd[0] =  native_4ba ? JEDEC_READ_4BA : JEDEC_READ;
 
 	const int addr_len = spi_prepare_address(flash, cmd, native_4ba, address);
 	if (addr_len < 0)
 		return 1;
 
 	/* Send Read */
-	return spi_send_command(flash, 1 + addr_len, len, cmd, bytes);
+    result = spi_send_command(flash, 1 + addr_len, len, cmd, bytes);
+    free(cmd);
+
+	return result;
 }
 
 /*
diff --git a/spi95.c b/spi95.c
deleted file mode 100644
index ecb2c1d..0000000
--- a/spi95.c
+++ /dev/null
@@ -1,69 +0,0 @@
-/*
- * This file is part of the flashrom project.
- *
- * Copyright (C) 2019 Konstantin Grudnev
- * Copyright (C) 2019 Nikolay Nikolaev
- *
- * This program 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 of the License,
- * or any later version.
- * 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.
- */
-
-/*
- * Contains SPI chip driver functions related to ST95XXX series (SPI EEPROM)
- */
-#include <string.h>
-#include <stdlib.h>
-#include "flashchips.h"
-#include "chipdrivers.h"
-#include "spi.h"
-
-/* For ST95XXX chips which have RDID */
-int probe_spi_st95(struct flashctx *flash)
-{
-	/*
-	 * ST_M95_RDID_OUTSIZE depends on size of the flash and
-	 * not all ST_M95XXX have RDID.
-	 */
-	static const unsigned char cmd[ST_M95_RDID_OUTSIZE_MAX] = { ST_M95_RDID };
-	unsigned char readarr[ST_M95_RDID_INSIZE];
-	uint32_t id1, id2;
-
-	uint32_t rdid_outsize = ST_M95_RDID_2BA_OUTSIZE; // 16 bit address
-	if (flash->chip->total_size * KiB > 64 * KiB)
-		rdid_outsize = ST_M95_RDID_3BA_OUTSIZE; // 24 bit address
-
-	spi_send_command(flash, rdid_outsize, sizeof(readarr), cmd, readarr);
-
-	id1 = readarr[0]; // manufacture id
-	id2 = (readarr[1] << 8) | readarr[2]; // SPI family code + model id
-
-	msg_cdbg("%s: id1 0x%02x, id2 0x%02x\n", __func__, id1, id2);
-
-	if (id1 == flash->chip->manufacture_id && id2 == flash->chip->model_id)
-		return 1;
-
-	return 0;
-}
-
-/* ST95XXX chips don't have erase operation and erase is made as part of write command */
-int spi_block_erase_emulation(struct flashctx *flash, unsigned int addr, unsigned int blocklen)
-{
-	uint8_t *erased_contents = NULL;
-	int result = 0;
-
-	erased_contents = (uint8_t *)malloc(blocklen * sizeof(uint8_t));
-	if (!erased_contents) {
-		msg_cerr("Out of memory!\n");
-		return 1;
-	}
-	memset(erased_contents, ERASED_VALUE(flash), blocklen * sizeof(uint8_t));
-	result = spi_write_chunked(flash, erased_contents, 0, blocklen, flash->chip->page_size);
-	free(erased_contents);
-	return result;
-}
-- 
2.24.0

