Change in flashrom[master]: tree: Removing forward-declarations for spi masters

[Anastasia Klimchuk (Code Review)]

Anastasia Klimchuk has uploaded this change for review. ( https://review.coreboot.org/c/flashrom/+/50711 )

Change subject: tree: Removing forward-declarations for spi masters
......................................................................

tree: Removing forward-declarations for spi masters

Reordering functions to avoid forward-declarations. It looks like
for most of the spi masters this has already been done before, I
found 7 that still have forward-declarations and covered them all
in one patch.

BUG=b:140394053
TEST=builds

Change-Id: I23ff6b79d794876f73b327f18784ca7c04c32c84
Signed-off-by: Anastasia Klimchuk 
---
M buspirate_spi.c
M dummyflasher.c
M ft2232_spi.c
M ichspi.c
M linux_spi.c
M ni845x_spi.c
M serprog.c
7 files changed, 1,165 insertions(+), 1,224 deletions(-)

git pull ssh://review.coreboot.org:29418/flashrom refs/changes/11/50711/1

diff --git a/buspirate_spi.c b/buspirate_spi.c
index fdfc0e4..eee8daa 100644
--- a/buspirate_spi.c
+++ b/buspirate_spi.c
@@ -128,11 +128,6 @@
    return ret;
 }

-static int buspirate_spi_send_command_v1(const struct flashctx *flash, unsigned int writecnt, unsigned int readcnt,
-					 const unsigned char *writearr, unsigned char *readarr);
-static int buspirate_spi_send_command_v2(const struct flashctx *flash, unsigned int writecnt, unsigned int readcnt,
-					 const unsigned char *writearr, unsigned char *readarr);
-
 static struct spi_master spi_master_buspirate = {
    .features	= SPI_MASTER_4BA,
    .max_data_read	= MAX_DATA_UNSPECIFIED,
@@ -205,6 +200,99 @@
    return ret;
 }

+static int buspirate_spi_send_command_v1(const struct flashctx *flash, unsigned int writecnt, unsigned int readcnt,
+					 const unsigned char *writearr, unsigned char *readarr)
+{
+	unsigned int i = 0;
+	int ret = 0;
+
+	if (writecnt > 16 || readcnt > 16 || (readcnt + writecnt) > 16)
+		return SPI_INVALID_LENGTH;
+
+	/* 3 bytes extra for CS#, len, CS#. */
+	if (buspirate_commbuf_grow(writecnt + readcnt + 3))
+		return ERROR_OOM;
+
+	/* Assert CS# */
+	bp_commbuf[i++] = 0x02;
+
+	bp_commbuf[i++] = 0x10 | (writecnt + readcnt - 1);
+	memcpy(bp_commbuf + i, writearr, writecnt);
+	i += writecnt;
+	memset(bp_commbuf + i, 0, readcnt);
+
+	i += readcnt;
+	/* De-assert CS# */
+	bp_commbuf[i++] = 0x03;
+
+	ret = buspirate_sendrecv(bp_commbuf, i, i);
+
+	if (ret) {
+		msg_perr("Bus Pirate communication error!\n");
+		return SPI_GENERIC_ERROR;
+	}
+
+	if (bp_commbuf[0] != 0x01) {
+		msg_perr("Protocol error while lowering CS#!\n");
+		return SPI_GENERIC_ERROR;
+	}
+
+	if (bp_commbuf[1] != 0x01) {
+		msg_perr("Protocol error while reading/writing SPI!\n");
+		return SPI_GENERIC_ERROR;
+	}
+
+	if (bp_commbuf[i - 1] != 0x01) {
+		msg_perr("Protocol error while raising CS#!\n");
+		return SPI_GENERIC_ERROR;
+	}
+
+	/* Skip CS#, length, writearr. */
+	memcpy(readarr, bp_commbuf + 2 + writecnt, readcnt);
+
+	return ret;
+}
+
+static int buspirate_spi_send_command_v2(const struct flashctx *flash, unsigned int writecnt, unsigned int readcnt,
+					 const unsigned char *writearr, unsigned char *readarr)
+{
+	int i = 0, ret = 0;
+
+	if (writecnt > 4096 || readcnt > 4096 || (readcnt + writecnt) > 4096)
+		return SPI_INVALID_LENGTH;
+
+	/* 5 bytes extra for command, writelen, readlen.
+	 * 1 byte extra for Ack/Nack.
+	 */
+	if (buspirate_commbuf_grow(max(writecnt + 5, readcnt + 1)))
+		return ERROR_OOM;
+
+	/* Combined SPI write/read. */
+	bp_commbuf[i++] = 0x04;
+	bp_commbuf[i++] = (writecnt >> 8) & 0xff;
+	bp_commbuf[i++] = writecnt & 0xff;
+	bp_commbuf[i++] = (readcnt >> 8) & 0xff;
+	bp_commbuf[i++] = readcnt & 0xff;
+	memcpy(bp_commbuf + i, writearr, writecnt);
+
+	ret = buspirate_sendrecv(bp_commbuf, i + writecnt, 1 + readcnt);
+
+	if (ret) {
+		msg_perr("Bus Pirate communication error!\n");
+		return SPI_GENERIC_ERROR;
+	}
+
+	if (bp_commbuf[0] != 0x01) {
+		msg_perr("Protocol error while sending SPI write/read!\n");
+		return SPI_GENERIC_ERROR;
+	}
+
+	/* Skip Ack. */
+	memcpy(readarr, bp_commbuf + 1, readcnt);
+
+	return ret;
+}
+
 #define BP_FWVERSION(a,b)	((a) << 8 | (b))
 #define BP_HWVERSION(a,b)	BP_FWVERSION(a,b)

@@ -571,96 +659,3 @@

return 0;
 }
-
-static int buspirate_spi_send_command_v1(const struct flashctx *flash, unsigned int writecnt, unsigned int readcnt,
-					 const unsigned char *writearr, unsigned char *readarr)
-{
-	unsigned int i = 0;
-	int ret = 0;
-
-	if (writecnt > 16 || readcnt > 16 || (readcnt + writecnt) > 16)
-		return SPI_INVALID_LENGTH;
-
-	/* 3 bytes extra for CS#, len, CS#. */
-	if (buspirate_commbuf_grow(writecnt + readcnt + 3))
-		return ERROR_OOM;
-
-	/* Assert CS# */
-	bp_commbuf[i++] = 0x02;
-
-	bp_commbuf[i++] = 0x10 | (writecnt + readcnt - 1);
-	memcpy(bp_commbuf + i, writearr, writecnt);
-	i += writecnt;
-	memset(bp_commbuf + i, 0, readcnt);
-
-	i += readcnt;
-	/* De-assert CS# */
-	bp_commbuf[i++] = 0x03;
-
-	ret = buspirate_sendrecv(bp_commbuf, i, i);
-
-	if (ret) {
-		msg_perr("Bus Pirate communication error!\n");
-		return SPI_GENERIC_ERROR;
-	}
-
-	if (bp_commbuf[0] != 0x01) {
-		msg_perr("Protocol error while lowering CS#!\n");
-		return SPI_GENERIC_ERROR;
-	}
-
-	if (bp_commbuf[1] != 0x01) {
-		msg_perr("Protocol error while reading/writing SPI!\n");
-		return SPI_GENERIC_ERROR;
-	}
-
-	if (bp_commbuf[i - 1] != 0x01) {
-		msg_perr("Protocol error while raising CS#!\n");
-		return SPI_GENERIC_ERROR;
-	}
-
-	/* Skip CS#, length, writearr. */
-	memcpy(readarr, bp_commbuf + 2 + writecnt, readcnt);
-
-	return ret;
-}
-
-static int buspirate_spi_send_command_v2(const struct flashctx *flash, unsigned int writecnt, unsigned int readcnt,
-					 const unsigned char *writearr, unsigned char *readarr)
-{
-	int i = 0, ret = 0;
-
-	if (writecnt > 4096 || readcnt > 4096 || (readcnt + writecnt) > 4096)
-		return SPI_INVALID_LENGTH;
-
-	/* 5 bytes extra for command, writelen, readlen.
-	 * 1 byte extra for Ack/Nack.
-	 */
-	if (buspirate_commbuf_grow(max(writecnt + 5, readcnt + 1)))
-		return ERROR_OOM;
-
-	/* Combined SPI write/read. */
-	bp_commbuf[i++] = 0x04;
-	bp_commbuf[i++] = (writecnt >> 8) & 0xff;
-	bp_commbuf[i++] = writecnt & 0xff;
-	bp_commbuf[i++] = (readcnt >> 8) & 0xff;
-	bp_commbuf[i++] = readcnt & 0xff;
-	memcpy(bp_commbuf + i, writearr, writecnt);
-
-	ret = buspirate_sendrecv(bp_commbuf, i + writecnt, 1 + readcnt);
-
-	if (ret) {
-		msg_perr("Bus Pirate communication error!\n");
-		return SPI_GENERIC_ERROR;
-	}
-
-	if (bp_commbuf[0] != 0x01) {
-		msg_perr("Protocol error while sending SPI write/read!\n");
-		return SPI_GENERIC_ERROR;
-	}
-
-	/* Skip Ack. */
-	memcpy(readarr, bp_commbuf + 1, readcnt);
-
-	return ret;
-}
diff --git a/dummyflasher.c b/dummyflasher.c
index 92c30ee..5190282 100644
--- a/dummyflasher.c
+++ b/dummyflasher.c
@@ -102,19 +102,514 @@
 #endif

static unsigned int spi_write_256_chunksize = 256;
+static enum chipbustype dummy_buses_supported = BUS_NONE;

-static int dummy_spi_send_command(const struct flashctx *flash, unsigned int writecnt, unsigned int readcnt,
-				  const unsigned char *writearr, unsigned char *readarr);
-static int dummy_spi_write_256(struct flashctx *flash, const uint8_t *buf,
-			       unsigned int start, unsigned int len);
-static void dummy_chip_writeb(const struct flashctx *flash, uint8_t val, chipaddr addr);
-static void dummy_chip_writew(const struct flashctx *flash, uint16_t val, chipaddr addr);
-static void dummy_chip_writel(const struct flashctx *flash, uint32_t val, chipaddr addr);
-static void dummy_chip_writen(const struct flashctx *flash, const uint8_t *buf, chipaddr addr, size_t len);
-static uint8_t dummy_chip_readb(const struct flashctx *flash, const chipaddr addr);
-static uint16_t dummy_chip_readw(const struct flashctx *flash, const chipaddr addr);
-static uint32_t dummy_chip_readl(const struct flashctx *flash, const chipaddr addr);
-static void dummy_chip_readn(const struct flashctx *flash, uint8_t *buf, const chipaddr addr, size_t len);
+void *dummy_map(const char *descr, uintptr_t phys_addr, size_t len)
+{
+	msg_pspew("%s: Mapping %s, 0x%zx bytes at 0x%0*" PRIxPTR "\n",
+		  __func__, descr, len, PRIxPTR_WIDTH, phys_addr);
+	return (void *)phys_addr;
+}
+
+void dummy_unmap(void *virt_addr, size_t len)
+{
+	msg_pspew("%s: Unmapping 0x%zx bytes at %p\n", __func__, len, virt_addr);
+}
+
+static int dummy_spi_write_256(struct flashctx *flash, const uint8_t *buf, unsigned int start, unsigned int len)
+{
+	return spi_write_chunked(flash, buf, start, len,
+				 spi_write_256_chunksize);
+}
+
+static void dummy_chip_writeb(const struct flashctx *flash, uint8_t val, chipaddr addr)
+{
+	msg_pspew("%s: addr=0x%" PRIxPTR ", val=0x%02x\n", __func__, addr, val);
+}
+
+static void dummy_chip_writew(const struct flashctx *flash, uint16_t val, chipaddr addr)
+{
+	msg_pspew("%s: addr=0x%" PRIxPTR ", val=0x%04x\n", __func__, addr, val);
+}
+
+static void dummy_chip_writel(const struct flashctx *flash, uint32_t val, chipaddr addr)
+{
+	msg_pspew("%s: addr=0x%" PRIxPTR ", val=0x%08x\n", __func__, addr, val);
+}
+
+static void dummy_chip_writen(const struct flashctx *flash, const uint8_t *buf, chipaddr addr, size_t len)
+{
+	size_t i;
+	msg_pspew("%s: addr=0x%" PRIxPTR ", len=0x%zx, writing data (hex):", __func__, addr, len);
+	for (i = 0; i < len; i++) {
+		if ((i % 16) == 0)
+			msg_pspew("\n");
+		msg_pspew("%02x ", buf[i]);
+	}
+}
+
+static uint8_t dummy_chip_readb(const struct flashctx *flash, const chipaddr addr)
+{
+	msg_pspew("%s:  addr=0x%" PRIxPTR ", returning 0xff\n", __func__, addr);
+	return 0xff;
+}
+
+static uint16_t dummy_chip_readw(const struct flashctx *flash, const chipaddr addr)
+{
+	msg_pspew("%s:  addr=0x%" PRIxPTR ", returning 0xffff\n", __func__, addr);
+	return 0xffff;
+}
+
+static uint32_t dummy_chip_readl(const struct flashctx *flash, const chipaddr addr)
+{
+	msg_pspew("%s:  addr=0x%" PRIxPTR ", returning 0xffffffff\n", __func__, addr);
+	return 0xffffffff;
+}
+
+static void dummy_chip_readn(const struct flashctx *flash, uint8_t *buf, const chipaddr addr, size_t len)
+{
+	msg_pspew("%s:  addr=0x%" PRIxPTR ", len=0x%zx, returning array of 0xff\n", __func__, addr, len);
+	memset(buf, 0xff, len);
+	return;
+}
+
+static struct emu_data* get_data_from_context(const struct flashctx *flash)
+{
+	if (dummy_buses_supported & (BUS_PARALLEL | BUS_LPC | BUS_FWH))
+		return (struct emu_data *)flash->mst->par.data;
+	else if (dummy_buses_supported & BUS_SPI)
+		return (struct emu_data *)flash->mst->spi.data;
+
+	return NULL;	/* buses was set to BUS_NONE. */
+}
+
+#if EMULATE_SPI_CHIP
+static int emulate_spi_chip_response(unsigned int writecnt,
+				     unsigned int readcnt,
+				     const unsigned char *writearr,
+				     unsigned char *readarr,
+				     struct emu_data *data)
+{
+	unsigned int offs, i, toread;
+	static int unsigned aai_offs;
+	const unsigned char sst25vf040_rems_response[2] = {0xbf, 0x44};
+	const unsigned char sst25vf032b_rems_response[2] = {0xbf, 0x4a};
+	const unsigned char mx25l6436_rems_response[2] = {0xc2, 0x16};
+	const unsigned char w25q128fv_rems_response[2] = {0xef, 0x17};
+
+	if (writecnt == 0) {
+		msg_perr("No command sent to the chip!\n");
+		return 1;
+	}
+	/* spi_blacklist has precedence over spi_ignorelist. */
+	for (i = 0; i < data->spi_blacklist_size; i++) {
+		if (writearr[0] == data->spi_blacklist[i]) {
+			msg_pdbg("Refusing blacklisted SPI command 0x%02x\n",
+				 data->spi_blacklist[i]);
+			return SPI_INVALID_OPCODE;
+		}
+	}
+	for (i = 0; i < data->spi_ignorelist_size; i++) {
+		if (writearr[0] == data->spi_ignorelist[i]) {
+			msg_cdbg("Ignoring ignorelisted SPI command 0x%02x\n",
+				 data->spi_ignorelist[i]);
+			/* Return success because the command does not fail,
+			 * it is simply ignored.
+			 */
+			return 0;
+		}
+	}
+
+	if (data->emu_max_aai_size && (data->emu_status & SPI_SR_AAI)) {
+		if (writearr[0] != JEDEC_AAI_WORD_PROGRAM &&
+		    writearr[0] != JEDEC_WRDI &&
+		    writearr[0] != JEDEC_RDSR) {
+			msg_perr("Forbidden opcode (0x%02x) attempted during "
+				 "AAI sequence!\n", writearr[0]);
+			return 0;
+		}
+	}
+
+	switch (writearr[0]) {
+	case JEDEC_RES:
+		if (writecnt < JEDEC_RES_OUTSIZE)
+			break;
+		/* offs calculation is only needed for SST chips which treat RES like REMS. */
+		offs = writearr[1] << 16 | writearr[2] << 8 | writearr[3];
+		offs += writecnt - JEDEC_REMS_OUTSIZE;
+		switch (data->emu_chip) {
+		case EMULATE_ST_M25P10_RES:
+			if (readcnt > 0)
+				memset(readarr, 0x10, readcnt);
+			break;
+		case EMULATE_SST_SST25VF040_REMS:
+			for (i = 0; i < readcnt; i++)
+				readarr[i] = sst25vf040_rems_response[(offs + i) % 2];
+			break;
+		case EMULATE_SST_SST25VF032B:
+			for (i = 0; i < readcnt; i++)
+				readarr[i] = sst25vf032b_rems_response[(offs + i) % 2];
+			break;
+		case EMULATE_MACRONIX_MX25L6436:
+			if (readcnt > 0)
+				memset(readarr, 0x16, readcnt);
+			break;
+		case EMULATE_WINBOND_W25Q128FV:
+			if (readcnt > 0)
+				memset(readarr, 0x17, readcnt);
+			break;
+		default: /* ignore */
+			break;
+		}
+		break;
+	case JEDEC_REMS:
+		/* REMS response has wraparound and uses an address parameter. */
+		if (writecnt < JEDEC_REMS_OUTSIZE)
+			break;
+		offs = writearr[1] << 16 | writearr[2] << 8 | writearr[3];
+		offs += writecnt - JEDEC_REMS_OUTSIZE;
+		switch (data->emu_chip) {
+		case EMULATE_SST_SST25VF040_REMS:
+			for (i = 0; i < readcnt; i++)
+				readarr[i] = sst25vf040_rems_response[(offs + i) % 2];
+			break;
+		case EMULATE_SST_SST25VF032B:
+			for (i = 0; i < readcnt; i++)
+				readarr[i] = sst25vf032b_rems_response[(offs + i) % 2];
+			break;
+		case EMULATE_MACRONIX_MX25L6436:
+			for (i = 0; i < readcnt; i++)
+				readarr[i] = mx25l6436_rems_response[(offs + i) % 2];
+			break;
+		case EMULATE_WINBOND_W25Q128FV:
+			for (i = 0; i < readcnt; i++)
+				readarr[i] = w25q128fv_rems_response[(offs + i) % 2];
+			break;
+		default: /* ignore */
+			break;
+		}
+		break;
+	case JEDEC_RDID:
+		switch (data->emu_chip) {
+		case EMULATE_SST_SST25VF032B:
+			if (readcnt > 0)
+				readarr[0] = 0xbf;
+			if (readcnt > 1)
+				readarr[1] = 0x25;
+			if (readcnt > 2)
+				readarr[2] = 0x4a;
+			break;
+		case EMULATE_MACRONIX_MX25L6436:
+			if (readcnt > 0)
+				readarr[0] = 0xc2;
+			if (readcnt > 1)
+				readarr[1] = 0x20;
+			if (readcnt > 2)
+				readarr[2] = 0x17;
+			break;
+		case EMULATE_WINBOND_W25Q128FV:
+			if (readcnt > 0)
+				readarr[0] = 0xef;
+			if (readcnt > 1)
+				readarr[1] = 0x40;
+			if (readcnt > 2)
+				readarr[2] = 0x18;
+			break;
+		case EMULATE_VARIABLE_SIZE:
+			if (readcnt > 0)
+				readarr[0] = (PROGMANUF_ID >> 8) & 0xff;
+			if (readcnt > 1)
+				readarr[1] = PROGMANUF_ID & 0xff;
+			if (readcnt > 2)
+				readarr[2] = (PROGDEV_ID >> 8) & 0xff;
+			if (readcnt > 3)
+				readarr[3] = PROGDEV_ID & 0xff;
+			break;
+		default: /* ignore */
+			break;
+		}
+		break;
+	case JEDEC_RDSR:
+		memset(readarr, data->emu_status, readcnt);
+		break;
+	/* FIXME: this should be chip-specific. */
+	case JEDEC_EWSR:
+	case JEDEC_WREN:
+		data->emu_status |= SPI_SR_WEL;
+		break;
+	case JEDEC_WRSR:
+		if (!(data->emu_status & SPI_SR_WEL)) {
+			msg_perr("WRSR attempted, but WEL is 0!\n");
+			break;
+		}
+		/* FIXME: add some reasonable simulation of the busy flag */
+		data->emu_status = writearr[1] & ~SPI_SR_WIP;
+		msg_pdbg2("WRSR wrote 0x%02x.\n", data->emu_status);
+		break;
+	case JEDEC_READ:
+		offs = writearr[1] << 16 | writearr[2] << 8 | writearr[3];
+		/* Truncate to emu_chip_size. */
+		offs %= data->emu_chip_size;
+		if (readcnt > 0)
+			memcpy(readarr, flashchip_contents + offs, readcnt);
+		break;
+	case JEDEC_READ_4BA:
+		offs = writearr[1] << 24 | writearr[2] << 16 | writearr[3] << 8 | writearr[4];
+		/* Truncate to emu_chip_size. */
+		offs %= data->emu_chip_size;
+		if (readcnt > 0)
+			memcpy(readarr, flashchip_contents + offs, readcnt);
+		break;
+	case JEDEC_BYTE_PROGRAM:
+		offs = writearr[1] << 16 | writearr[2] << 8 | writearr[3];
+		/* Truncate to emu_chip_size. */
+		offs %= data->emu_chip_size;
+		if (writecnt < 5) {
+			msg_perr("BYTE PROGRAM size too short!\n");
+			return 1;
+		}
+		if (writecnt - 4 > data->emu_max_byteprogram_size) {
+			msg_perr("Max BYTE PROGRAM size exceeded!\n");
+			return 1;
+		}
+		memcpy(flashchip_contents + offs, writearr + 4, writecnt - 4);
+		data->emu_modified = 1;
+		break;
+	case JEDEC_BYTE_PROGRAM_4BA:
+		offs = writearr[1] << 24 | writearr[2] << 16 | writearr[3] << 8 | writearr[4];
+		/* Truncate to emu_chip_size. */
+		offs %= data->emu_chip_size;
+		if (writecnt < 6) {
+			msg_perr("BYTE PROGRAM size too short!\n");
+			return 1;
+		}
+		if (writecnt - 5 > data->emu_max_byteprogram_size) {
+			msg_perr("Max BYTE PROGRAM size exceeded!\n");
+			return 1;
+		}
+		memcpy(flashchip_contents + offs, writearr + 5, writecnt - 5);
+		data->emu_modified = 1;
+		break;
+	case JEDEC_AAI_WORD_PROGRAM:
+		if (!data->emu_max_aai_size)
+			break;
+		if (!(data->emu_status & SPI_SR_AAI)) {
+			if (writecnt < JEDEC_AAI_WORD_PROGRAM_OUTSIZE) {
+				msg_perr("Initial AAI WORD PROGRAM size too "
+					 "short!\n");
+				return 1;
+			}
+			if (writecnt > JEDEC_AAI_WORD_PROGRAM_OUTSIZE) {
+				msg_perr("Initial AAI WORD PROGRAM size too "
+					 "long!\n");
+				return 1;
+			}
+			data->emu_status |= SPI_SR_AAI;
+			aai_offs = writearr[1] << 16 | writearr[2] << 8 |
+				   writearr[3];
+			/* Truncate to emu_chip_size. */
+			aai_offs %= data->emu_chip_size;
+			memcpy(flashchip_contents + aai_offs, writearr + 4, 2);
+			aai_offs += 2;
+		} else {
+			if (writecnt < JEDEC_AAI_WORD_PROGRAM_CONT_OUTSIZE) {
+				msg_perr("Continuation AAI WORD PROGRAM size "
+					 "too short!\n");
+				return 1;
+			}
+			if (writecnt > JEDEC_AAI_WORD_PROGRAM_CONT_OUTSIZE) {
+				msg_perr("Continuation AAI WORD PROGRAM size "
+					 "too long!\n");
+				return 1;
+			}
+			memcpy(flashchip_contents + aai_offs, writearr + 1, 2);
+			aai_offs += 2;
+		}
+		data->emu_modified = 1;
+		break;
+	case JEDEC_WRDI:
+		if (data->emu_max_aai_size)
+			data->emu_status &= ~SPI_SR_AAI;
+		break;
+	case JEDEC_SE:
+		if (!data->emu_jedec_se_size)
+			break;
+		if (writecnt != JEDEC_SE_OUTSIZE) {
+			msg_perr("SECTOR ERASE 0x20 outsize invalid!\n");
+			return 1;
+		}
+		if (readcnt != JEDEC_SE_INSIZE) {
+			msg_perr("SECTOR ERASE 0x20 insize invalid!\n");
+			return 1;
+		}
+		offs = writearr[1] << 16 | writearr[2] << 8 | writearr[3];
+		if (offs & (data->emu_jedec_se_size - 1))
+			msg_pdbg("Unaligned SECTOR ERASE 0x20: 0x%x\n", offs);
+		offs &= ~(data->emu_jedec_se_size - 1);
+		memset(flashchip_contents + offs, 0xff, data->emu_jedec_se_size);
+		data->emu_modified = 1;
+		break;
+	case JEDEC_BE_52:
+		if (!data->emu_jedec_be_52_size)
+			break;
+		if (writecnt != JEDEC_BE_52_OUTSIZE) {
+			msg_perr("BLOCK ERASE 0x52 outsize invalid!\n");
+			return 1;
+		}
+		if (readcnt != JEDEC_BE_52_INSIZE) {
+			msg_perr("BLOCK ERASE 0x52 insize invalid!\n");
+			return 1;
+		}
+		offs = writearr[1] << 16 | writearr[2] << 8 | writearr[3];
+		if (offs & (data->emu_jedec_be_52_size - 1))
+			msg_pdbg("Unaligned BLOCK ERASE 0x52: 0x%x\n", offs);
+		offs &= ~(data->emu_jedec_be_52_size - 1);
+		memset(flashchip_contents + offs, 0xff, data->emu_jedec_be_52_size);
+		data->emu_modified = 1;
+		break;
+	case JEDEC_BE_D8:
+		if (!data->emu_jedec_be_d8_size)
+			break;
+		if (writecnt != JEDEC_BE_D8_OUTSIZE) {
+			msg_perr("BLOCK ERASE 0xd8 outsize invalid!\n");
+			return 1;
+		}
+		if (readcnt != JEDEC_BE_D8_INSIZE) {
+			msg_perr("BLOCK ERASE 0xd8 insize invalid!\n");
+			return 1;
+		}
+		offs = writearr[1] << 16 | writearr[2] << 8 | writearr[3];
+		if (offs & (data->emu_jedec_be_d8_size - 1))
+			msg_pdbg("Unaligned BLOCK ERASE 0xd8: 0x%x\n", offs);
+		offs &= ~(data->emu_jedec_be_d8_size - 1);
+		memset(flashchip_contents + offs, 0xff, data->emu_jedec_be_d8_size);
+		data->emu_modified = 1;
+		break;
+	case JEDEC_CE_60:
+		if (!data->emu_jedec_ce_60_size)
+			break;
+		if (writecnt != JEDEC_CE_60_OUTSIZE) {
+			msg_perr("CHIP ERASE 0x60 outsize invalid!\n");
+			return 1;
+		}
+		if (readcnt != JEDEC_CE_60_INSIZE) {
+			msg_perr("CHIP ERASE 0x60 insize invalid!\n");
+			return 1;
+		}
+		/* JEDEC_CE_60_OUTSIZE is 1 (no address) -> no offset. */
+		/* emu_jedec_ce_60_size is emu_chip_size. */
+		memset(flashchip_contents, 0xff, data->emu_jedec_ce_60_size);
+		data->emu_modified = 1;
+		break;
+	case JEDEC_CE_C7:
+		if (!data->emu_jedec_ce_c7_size)
+			break;
+		if (writecnt != JEDEC_CE_C7_OUTSIZE) {
+			msg_perr("CHIP ERASE 0xc7 outsize invalid!\n");
+			return 1;
+		}
+		if (readcnt != JEDEC_CE_C7_INSIZE) {
+			msg_perr("CHIP ERASE 0xc7 insize invalid!\n");
+			return 1;
+		}
+		/* JEDEC_CE_C7_OUTSIZE is 1 (no address) -> no offset. */
+		/* emu_jedec_ce_c7_size is emu_chip_size. */
+		memset(flashchip_contents, 0xff, data->emu_jedec_ce_c7_size);
+		data->emu_modified = 1;
+		break;
+	case JEDEC_SFDP:
+		if (data->emu_chip != EMULATE_MACRONIX_MX25L6436)
+			break;
+		if (writecnt < 4)
+			break;
+		offs = writearr[1] << 16 | writearr[2] << 8 | writearr[3];
+
+		/* SFDP expects one dummy byte after the address. */
+		if (writecnt == 4) {
+			/* The dummy byte was not written, make sure it is read instead.
+			 * Shifting and shortening the read array does achieve this goal.
+			 */
+			readarr++;
+			readcnt--;
+		} else {
+			/* The response is shifted if more than 5 bytes are written, because SFDP data is
+			 * already shifted out by the chip while those superfluous bytes are written. */
+			offs += writecnt - 5;
+		}
+
+		/* The SFDP spec implies that the start address of an SFDP read may be truncated to fit in the
+		 * SFDP table address space, i.e. the start address may be wrapped around at SFDP table size.
+		 * This is a reasonable implementation choice in hardware because it saves a few gates. */
+		if (offs >= sizeof(sfdp_table)) {
+			msg_pdbg("Wrapping the start address around the SFDP table boundary (using 0x%x "
+				 "instead of 0x%x).\n", (unsigned int)(offs % sizeof(sfdp_table)), offs);
+			offs %= sizeof(sfdp_table);
+		}
+		toread = min(sizeof(sfdp_table) - offs, readcnt);
+		memcpy(readarr, sfdp_table + offs, toread);
+		if (toread < readcnt)
+			msg_pdbg("Crossing the SFDP table boundary in a single "
+				 "continuous chunk produces undefined results "
+				 "after that point.\n");
+		break;
+	default:
+		/* No special response. */
+		break;
+	}
+	if (writearr[0] != JEDEC_WREN && writearr[0] != JEDEC_EWSR)
+		data->emu_status &= ~SPI_SR_WEL;
+	return 0;
+}
+#endif
+
+static int dummy_spi_send_command(const struct flashctx *flash, unsigned int writecnt,
+				  unsigned int readcnt,
+				  const unsigned char *writearr,
+				  unsigned char *readarr)
+{
+	unsigned int i;
+	struct emu_data *emu_data = get_data_from_context(flash);
+	if (!emu_data) {
+		msg_perr("No data in flash context!\n");
+		return 1;
+	}
+
+	msg_pspew("%s:", __func__);
+
+	msg_pspew(" writing %u bytes:", writecnt);
+	for (i = 0; i < writecnt; i++)
+		msg_pspew(" 0x%02x", writearr[i]);
+
+	/* Response for unknown commands and missing chip is 0xff. */
+	memset(readarr, 0xff, readcnt);
+#if EMULATE_SPI_CHIP
+	switch (emu_data->emu_chip) {
+	case EMULATE_ST_M25P10_RES:
+	case EMULATE_SST_SST25VF040_REMS:
+	case EMULATE_SST_SST25VF032B:
+	case EMULATE_MACRONIX_MX25L6436:
+	case EMULATE_WINBOND_W25Q128FV:
+	case EMULATE_VARIABLE_SIZE:
+		if (emulate_spi_chip_response(writecnt, readcnt, writearr,
+					      readarr, emu_data)) {
+			msg_pdbg("Invalid command sent to flash chip!\n");
+			return 1;
+		}
+		break;
+	default:
+		break;
+	}
+#endif
+	msg_pspew(" reading %u bytes:", readcnt);
+	for (i = 0; i < readcnt; i++)
+		msg_pspew(" 0x%02x", readarr[i]);
+	msg_pspew("\n");
+
+	programmer_delay((writecnt + readcnt) * emu_data->delay_us);
+	return 0;
+}
+
+

static struct spi_master spi_master_dummyflasher = {
    .features	= SPI_MASTER_4BA,
@@ -138,8 +633,6 @@
    	.chip_writen		= dummy_chip_writen,
 };

-static enum chipbustype dummy_buses_supported = BUS_NONE;
-
 static int dummy_shutdown(void *data)
 {
    msg_pspew("%s\n", __func__);
@@ -541,511 +1034,6 @@
    return 0;
 }

-void *dummy_map(const char *descr, uintptr_t phys_addr, size_t len)
-{
-	msg_pspew("%s: Mapping %s, 0x%zx bytes at 0x%0*" PRIxPTR "\n",
-		  __func__, descr, len, PRIxPTR_WIDTH, phys_addr);
-	return (void *)phys_addr;
-}
-
-void dummy_unmap(void *virt_addr, size_t len)
-{
-	msg_pspew("%s: Unmapping 0x%zx bytes at %p\n", __func__, len, virt_addr);
-}
-
-static void dummy_chip_writeb(const struct flashctx *flash, uint8_t val, chipaddr addr)
-{
-	msg_pspew("%s: addr=0x%" PRIxPTR ", val=0x%02x\n", __func__, addr, val);
-}
-
-static void dummy_chip_writew(const struct flashctx *flash, uint16_t val, chipaddr addr)
-{
-	msg_pspew("%s: addr=0x%" PRIxPTR ", val=0x%04x\n", __func__, addr, val);
-}
-
-static void dummy_chip_writel(const struct flashctx *flash, uint32_t val, chipaddr addr)
-{
-	msg_pspew("%s: addr=0x%" PRIxPTR ", val=0x%08x\n", __func__, addr, val);
-}
-
-static void dummy_chip_writen(const struct flashctx *flash, const uint8_t *buf, chipaddr addr, size_t len)
-{
-	size_t i;
-	msg_pspew("%s: addr=0x%" PRIxPTR ", len=0x%zx, writing data (hex):", __func__, addr, len);
-	for (i = 0; i < len; i++) {
-		if ((i % 16) == 0)
-			msg_pspew("\n");
-		msg_pspew("%02x ", buf[i]);
-	}
-}
-
-static uint8_t dummy_chip_readb(const struct flashctx *flash, const chipaddr addr)
-{
-	msg_pspew("%s:  addr=0x%" PRIxPTR ", returning 0xff\n", __func__, addr);
-	return 0xff;
-}
-
-static uint16_t dummy_chip_readw(const struct flashctx *flash, const chipaddr addr)
-{
-	msg_pspew("%s:  addr=0x%" PRIxPTR ", returning 0xffff\n", __func__, addr);
-	return 0xffff;
-}
-
-static uint32_t dummy_chip_readl(const struct flashctx *flash, const chipaddr addr)
-{
-	msg_pspew("%s:  addr=0x%" PRIxPTR ", returning 0xffffffff\n", __func__, addr);
-	return 0xffffffff;
-}
-
-static void dummy_chip_readn(const struct flashctx *flash, uint8_t *buf, const chipaddr addr, size_t len)
-{
-	msg_pspew("%s:  addr=0x%" PRIxPTR ", len=0x%zx, returning array of 0xff\n", __func__, addr, len);
-	memset(buf, 0xff, len);
-	return;
-}
-
-#if EMULATE_SPI_CHIP
-static int emulate_spi_chip_response(unsigned int writecnt,
-				     unsigned int readcnt,
-				     const unsigned char *writearr,
-				     unsigned char *readarr,
-				     struct emu_data *data)
-{
-	unsigned int offs, i, toread;
-	static int unsigned aai_offs;
-	const unsigned char sst25vf040_rems_response[2] = {0xbf, 0x44};
-	const unsigned char sst25vf032b_rems_response[2] = {0xbf, 0x4a};
-	const unsigned char mx25l6436_rems_response[2] = {0xc2, 0x16};
-	const unsigned char w25q128fv_rems_response[2] = {0xef, 0x17};
-
-	if (writecnt == 0) {
-		msg_perr("No command sent to the chip!\n");
-		return 1;
-	}
-	/* spi_blacklist has precedence over spi_ignorelist. */
-	for (i = 0; i < data->spi_blacklist_size; i++) {
-		if (writearr[0] == data->spi_blacklist[i]) {
-			msg_pdbg("Refusing blacklisted SPI command 0x%02x\n",
-				 data->spi_blacklist[i]);
-			return SPI_INVALID_OPCODE;
-		}
-	}
-	for (i = 0; i < data->spi_ignorelist_size; i++) {
-		if (writearr[0] == data->spi_ignorelist[i]) {
-			msg_cdbg("Ignoring ignorelisted SPI command 0x%02x\n",
-				 data->spi_ignorelist[i]);
-			/* Return success because the command does not fail,
-			 * it is simply ignored.
-			 */
-			return 0;
-		}
-	}
-
-	if (data->emu_max_aai_size && (data->emu_status & SPI_SR_AAI)) {
-		if (writearr[0] != JEDEC_AAI_WORD_PROGRAM &&
-		    writearr[0] != JEDEC_WRDI &&
-		    writearr[0] != JEDEC_RDSR) {
-			msg_perr("Forbidden opcode (0x%02x) attempted during "
-				 "AAI sequence!\n", writearr[0]);
-			return 0;
-		}
-	}
-
-	switch (writearr[0]) {
-	case JEDEC_RES:
-		if (writecnt < JEDEC_RES_OUTSIZE)
-			break;
-		/* offs calculation is only needed for SST chips which treat RES like REMS. */
-		offs = writearr[1] << 16 | writearr[2] << 8 | writearr[3];
-		offs += writecnt - JEDEC_REMS_OUTSIZE;
-		switch (data->emu_chip) {
-		case EMULATE_ST_M25P10_RES:
-			if (readcnt > 0)
-				memset(readarr, 0x10, readcnt);
-			break;
-		case EMULATE_SST_SST25VF040_REMS:
-			for (i = 0; i < readcnt; i++)
-				readarr[i] = sst25vf040_rems_response[(offs + i) % 2];
-			break;
-		case EMULATE_SST_SST25VF032B:
-			for (i = 0; i < readcnt; i++)
-				readarr[i] = sst25vf032b_rems_response[(offs + i) % 2];
-			break;
-		case EMULATE_MACRONIX_MX25L6436:
-			if (readcnt > 0)
-				memset(readarr, 0x16, readcnt);
-			break;
-		case EMULATE_WINBOND_W25Q128FV:
-			if (readcnt > 0)
-				memset(readarr, 0x17, readcnt);
-			break;
-		default: /* ignore */
-			break;
-		}
-		break;
-	case JEDEC_REMS:
-		/* REMS response has wraparound and uses an address parameter. */
-		if (writecnt < JEDEC_REMS_OUTSIZE)
-			break;
-		offs = writearr[1] << 16 | writearr[2] << 8 | writearr[3];
-		offs += writecnt - JEDEC_REMS_OUTSIZE;
-		switch (data->emu_chip) {
-		case EMULATE_SST_SST25VF040_REMS:
-			for (i = 0; i < readcnt; i++)
-				readarr[i] = sst25vf040_rems_response[(offs + i) % 2];
-			break;
-		case EMULATE_SST_SST25VF032B:
-			for (i = 0; i < readcnt; i++)
-				readarr[i] = sst25vf032b_rems_response[(offs + i) % 2];
-			break;
-		case EMULATE_MACRONIX_MX25L6436:
-			for (i = 0; i < readcnt; i++)
-				readarr[i] = mx25l6436_rems_response[(offs + i) % 2];
-			break;
-		case EMULATE_WINBOND_W25Q128FV:
-			for (i = 0; i < readcnt; i++)
-				readarr[i] = w25q128fv_rems_response[(offs + i) % 2];
-			break;
-		default: /* ignore */
-			break;
-		}
-		break;
-	case JEDEC_RDID:
-		switch (data->emu_chip) {
-		case EMULATE_SST_SST25VF032B:
-			if (readcnt > 0)
-				readarr[0] = 0xbf;
-			if (readcnt > 1)
-				readarr[1] = 0x25;
-			if (readcnt > 2)
-				readarr[2] = 0x4a;
-			break;
-		case EMULATE_MACRONIX_MX25L6436:
-			if (readcnt > 0)
-				readarr[0] = 0xc2;
-			if (readcnt > 1)
-				readarr[1] = 0x20;
-			if (readcnt > 2)
-				readarr[2] = 0x17;
-			break;
-		case EMULATE_WINBOND_W25Q128FV:
-			if (readcnt > 0)
-				readarr[0] = 0xef;
-			if (readcnt > 1)
-				readarr[1] = 0x40;
-			if (readcnt > 2)
-				readarr[2] = 0x18;
-			break;
-		case EMULATE_VARIABLE_SIZE:
-			if (readcnt > 0)
-				readarr[0] = (PROGMANUF_ID >> 8) & 0xff;
-			if (readcnt > 1)
-				readarr[1] = PROGMANUF_ID & 0xff;
-			if (readcnt > 2)
-				readarr[2] = (PROGDEV_ID >> 8) & 0xff;
-			if (readcnt > 3)
-				readarr[3] = PROGDEV_ID & 0xff;
-			break;
-		default: /* ignore */
-			break;
-		}
-		break;
-	case JEDEC_RDSR:
-		memset(readarr, data->emu_status, readcnt);
-		break;
-	/* FIXME: this should be chip-specific. */
-	case JEDEC_EWSR:
-	case JEDEC_WREN:
-		data->emu_status |= SPI_SR_WEL;
-		break;
-	case JEDEC_WRSR:
-		if (!(data->emu_status & SPI_SR_WEL)) {
-			msg_perr("WRSR attempted, but WEL is 0!\n");
-			break;
-		}
-		/* FIXME: add some reasonable simulation of the busy flag */
-		data->emu_status = writearr[1] & ~SPI_SR_WIP;
-		msg_pdbg2("WRSR wrote 0x%02x.\n", data->emu_status);
-		break;
-	case JEDEC_READ:
-		offs = writearr[1] << 16 | writearr[2] << 8 | writearr[3];
-		/* Truncate to emu_chip_size. */
-		offs %= data->emu_chip_size;
-		if (readcnt > 0)
-			memcpy(readarr, flashchip_contents + offs, readcnt);
-		break;
-	case JEDEC_READ_4BA:
-		offs = writearr[1] << 24 | writearr[2] << 16 | writearr[3] << 8 | writearr[4];
-		/* Truncate to emu_chip_size. */
-		offs %= data->emu_chip_size;
-		if (readcnt > 0)
-			memcpy(readarr, flashchip_contents + offs, readcnt);
-		break;
-	case JEDEC_BYTE_PROGRAM:
-		offs = writearr[1] << 16 | writearr[2] << 8 | writearr[3];
-		/* Truncate to emu_chip_size. */
-		offs %= data->emu_chip_size;
-		if (writecnt < 5) {
-			msg_perr("BYTE PROGRAM size too short!\n");
-			return 1;
-		}
-		if (writecnt - 4 > data->emu_max_byteprogram_size) {
-			msg_perr("Max BYTE PROGRAM size exceeded!\n");
-			return 1;
-		}
-		memcpy(flashchip_contents + offs, writearr + 4, writecnt - 4);
-		data->emu_modified = 1;
-		break;
-	case JEDEC_BYTE_PROGRAM_4BA:
-		offs = writearr[1] << 24 | writearr[2] << 16 | writearr[3] << 8 | writearr[4];
-		/* Truncate to emu_chip_size. */
-		offs %= data->emu_chip_size;
-		if (writecnt < 6) {
-			msg_perr("BYTE PROGRAM size too short!\n");
-			return 1;
-		}
-		if (writecnt - 5 > data->emu_max_byteprogram_size) {
-			msg_perr("Max BYTE PROGRAM size exceeded!\n");
-			return 1;
-		}
-		memcpy(flashchip_contents + offs, writearr + 5, writecnt - 5);
-		data->emu_modified = 1;
-		break;
-	case JEDEC_AAI_WORD_PROGRAM:
-		if (!data->emu_max_aai_size)
-			break;
-		if (!(data->emu_status & SPI_SR_AAI)) {
-			if (writecnt < JEDEC_AAI_WORD_PROGRAM_OUTSIZE) {
-				msg_perr("Initial AAI WORD PROGRAM size too "
-					 "short!\n");
-				return 1;
-			}
-			if (writecnt > JEDEC_AAI_WORD_PROGRAM_OUTSIZE) {
-				msg_perr("Initial AAI WORD PROGRAM size too "
-					 "long!\n");
-				return 1;
-			}
-			data->emu_status |= SPI_SR_AAI;
-			aai_offs = writearr[1] << 16 | writearr[2] << 8 |
-				   writearr[3];
-			/* Truncate to emu_chip_size. */
-			aai_offs %= data->emu_chip_size;
-			memcpy(flashchip_contents + aai_offs, writearr + 4, 2);
-			aai_offs += 2;
-		} else {
-			if (writecnt < JEDEC_AAI_WORD_PROGRAM_CONT_OUTSIZE) {
-				msg_perr("Continuation AAI WORD PROGRAM size "
-					 "too short!\n");
-				return 1;
-			}
-			if (writecnt > JEDEC_AAI_WORD_PROGRAM_CONT_OUTSIZE) {
-				msg_perr("Continuation AAI WORD PROGRAM size "
-					 "too long!\n");
-				return 1;
-			}
-			memcpy(flashchip_contents + aai_offs, writearr + 1, 2);
-			aai_offs += 2;
-		}
-		data->emu_modified = 1;
-		break;
-	case JEDEC_WRDI:
-		if (data->emu_max_aai_size)
-			data->emu_status &= ~SPI_SR_AAI;
-		break;
-	case JEDEC_SE:
-		if (!data->emu_jedec_se_size)
-			break;
-		if (writecnt != JEDEC_SE_OUTSIZE) {
-			msg_perr("SECTOR ERASE 0x20 outsize invalid!\n");
-			return 1;
-		}
-		if (readcnt != JEDEC_SE_INSIZE) {
-			msg_perr("SECTOR ERASE 0x20 insize invalid!\n");
-			return 1;
-		}
-		offs = writearr[1] << 16 | writearr[2] << 8 | writearr[3];
-		if (offs & (data->emu_jedec_se_size - 1))
-			msg_pdbg("Unaligned SECTOR ERASE 0x20: 0x%x\n", offs);
-		offs &= ~(data->emu_jedec_se_size - 1);
-		memset(flashchip_contents + offs, 0xff, data->emu_jedec_se_size);
-		data->emu_modified = 1;
-		break;
-	case JEDEC_BE_52:
-		if (!data->emu_jedec_be_52_size)
-			break;
-		if (writecnt != JEDEC_BE_52_OUTSIZE) {
-			msg_perr("BLOCK ERASE 0x52 outsize invalid!\n");
-			return 1;
-		}
-		if (readcnt != JEDEC_BE_52_INSIZE) {
-			msg_perr("BLOCK ERASE 0x52 insize invalid!\n");
-			return 1;
-		}
-		offs = writearr[1] << 16 | writearr[2] << 8 | writearr[3];
-		if (offs & (data->emu_jedec_be_52_size - 1))
-			msg_pdbg("Unaligned BLOCK ERASE 0x52: 0x%x\n", offs);
-		offs &= ~(data->emu_jedec_be_52_size - 1);
-		memset(flashchip_contents + offs, 0xff, data->emu_jedec_be_52_size);
-		data->emu_modified = 1;
-		break;
-	case JEDEC_BE_D8:
-		if (!data->emu_jedec_be_d8_size)
-			break;
-		if (writecnt != JEDEC_BE_D8_OUTSIZE) {
-			msg_perr("BLOCK ERASE 0xd8 outsize invalid!\n");
-			return 1;
-		}
-		if (readcnt != JEDEC_BE_D8_INSIZE) {
-			msg_perr("BLOCK ERASE 0xd8 insize invalid!\n");
-			return 1;
-		}
-		offs = writearr[1] << 16 | writearr[2] << 8 | writearr[3];
-		if (offs & (data->emu_jedec_be_d8_size - 1))
-			msg_pdbg("Unaligned BLOCK ERASE 0xd8: 0x%x\n", offs);
-		offs &= ~(data->emu_jedec_be_d8_size - 1);
-		memset(flashchip_contents + offs, 0xff, data->emu_jedec_be_d8_size);
-		data->emu_modified = 1;
-		break;
-	case JEDEC_CE_60:
-		if (!data->emu_jedec_ce_60_size)
-			break;
-		if (writecnt != JEDEC_CE_60_OUTSIZE) {
-			msg_perr("CHIP ERASE 0x60 outsize invalid!\n");
-			return 1;
-		}
-		if (readcnt != JEDEC_CE_60_INSIZE) {
-			msg_perr("CHIP ERASE 0x60 insize invalid!\n");
-			return 1;
-		}
-		/* JEDEC_CE_60_OUTSIZE is 1 (no address) -> no offset. */
-		/* emu_jedec_ce_60_size is emu_chip_size. */
-		memset(flashchip_contents, 0xff, data->emu_jedec_ce_60_size);
-		data->emu_modified = 1;
-		break;
-	case JEDEC_CE_C7:
-		if (!data->emu_jedec_ce_c7_size)
-			break;
-		if (writecnt != JEDEC_CE_C7_OUTSIZE) {
-			msg_perr("CHIP ERASE 0xc7 outsize invalid!\n");
-			return 1;
-		}
-		if (readcnt != JEDEC_CE_C7_INSIZE) {
-			msg_perr("CHIP ERASE 0xc7 insize invalid!\n");
-			return 1;
-		}
-		/* JEDEC_CE_C7_OUTSIZE is 1 (no address) -> no offset. */
-		/* emu_jedec_ce_c7_size is emu_chip_size. */
-		memset(flashchip_contents, 0xff, data->emu_jedec_ce_c7_size);
-		data->emu_modified = 1;
-		break;
-	case JEDEC_SFDP:
-		if (data->emu_chip != EMULATE_MACRONIX_MX25L6436)
-			break;
-		if (writecnt < 4)
-			break;
-		offs = writearr[1] << 16 | writearr[2] << 8 | writearr[3];
-
-		/* SFDP expects one dummy byte after the address. */
-		if (writecnt == 4) {
-			/* The dummy byte was not written, make sure it is read instead.
-			 * Shifting and shortening the read array does achieve this goal.
-			 */
-			readarr++;
-			readcnt--;
-		} else {
-			/* The response is shifted if more than 5 bytes are written, because SFDP data is
-			 * already shifted out by the chip while those superfluous bytes are written. */
-			offs += writecnt - 5;
-		}
-
-		/* The SFDP spec implies that the start address of an SFDP read may be truncated to fit in the
-		 * SFDP table address space, i.e. the start address may be wrapped around at SFDP table size.
-		 * This is a reasonable implementation choice in hardware because it saves a few gates. */
-		if (offs >= sizeof(sfdp_table)) {
-			msg_pdbg("Wrapping the start address around the SFDP table boundary (using 0x%x "
-				 "instead of 0x%x).\n", (unsigned int)(offs % sizeof(sfdp_table)), offs);
-			offs %= sizeof(sfdp_table);
-		}
-		toread = min(sizeof(sfdp_table) - offs, readcnt);
-		memcpy(readarr, sfdp_table + offs, toread);
-		if (toread < readcnt)
-			msg_pdbg("Crossing the SFDP table boundary in a single "
-				 "continuous chunk produces undefined results "
-				 "after that point.\n");
-		break;
-	default:
-		/* No special response. */
-		break;
-	}
-	if (writearr[0] != JEDEC_WREN && writearr[0] != JEDEC_EWSR)
-		data->emu_status &= ~SPI_SR_WEL;
-	return 0;
-}
-#endif
-
-static struct emu_data* get_data_from_context(const struct flashctx *flash)
-{
-	if (dummy_buses_supported & (BUS_PARALLEL | BUS_LPC | BUS_FWH))
-		return (struct emu_data *)flash->mst->par.data;
-	else if (dummy_buses_supported & BUS_SPI)
-		return (struct emu_data *)flash->mst->spi.data;
-
-	return NULL;	/* buses was set to BUS_NONE. */
-}
-
-static int dummy_spi_send_command(const struct flashctx *flash, unsigned int writecnt,
-				  unsigned int readcnt,
-				  const unsigned char *writearr,
-				  unsigned char *readarr)
-{
-	unsigned int i;
-	struct emu_data *emu_data = get_data_from_context(flash);
-	if (!emu_data) {
-		msg_perr("No data in flash context!\n");
-		return 1;
-	}
-
-	msg_pspew("%s:", __func__);
-
-	msg_pspew(" writing %u bytes:", writecnt);
-	for (i = 0; i < writecnt; i++)
-		msg_pspew(" 0x%02x", writearr[i]);
-
-	/* Response for unknown commands and missing chip is 0xff. */
-	memset(readarr, 0xff, readcnt);
-#if EMULATE_SPI_CHIP
-	switch (emu_data->emu_chip) {
-	case EMULATE_ST_M25P10_RES:
-	case EMULATE_SST_SST25VF040_REMS:
-	case EMULATE_SST_SST25VF032B:
-	case EMULATE_MACRONIX_MX25L6436:
-	case EMULATE_WINBOND_W25Q128FV:
-	case EMULATE_VARIABLE_SIZE:
-		if (emulate_spi_chip_response(writecnt, readcnt, writearr,
-					      readarr, emu_data)) {
-			msg_pdbg("Invalid command sent to flash chip!\n");
-			return 1;
-		}
-		break;
-	default:
-		break;
-	}
-#endif
-	msg_pspew(" reading %u bytes:", readcnt);
-	for (i = 0; i < readcnt; i++)
-		msg_pspew(" 0x%02x", readarr[i]);
-	msg_pspew("\n");
-
-	programmer_delay((writecnt + readcnt) * emu_data->delay_us);
-	return 0;
-}
-
-static int dummy_spi_write_256(struct flashctx *flash, const uint8_t *buf, unsigned int start, unsigned int len)
-{
-	return spi_write_chunked(flash, buf, start, len,
-				 spi_write_256_chunksize);
-}
-
 #if EMULATE_CHIP && EMULATE_SPI_CHIP
 int probe_variable_size(struct flashctx *flash)
 {
diff --git a/ft2232_spi.c b/ft2232_spi.c
index 9060973..1fb795b 100644
--- a/ft2232_spi.c
+++ b/ft2232_spi.c
@@ -183,10 +183,93 @@
    return 0;
 }

+/* Returns 0 upon success, a negative number upon errors. */
 static int ft2232_spi_send_command(const struct flashctx *flash,
    			   unsigned int writecnt, unsigned int readcnt,
    			   const unsigned char *writearr,
-				   unsigned char *readarr);
+				   unsigned char *readarr)
+{
+	struct ftdi_context *ftdic = &ftdic_context;
+	static unsigned char *buf = NULL;
+	/* failed is special. We use bitwise ops, but it is essentially bool. */
+	int i = 0, ret = 0, failed = 0;
+	size_t bufsize;
+	static size_t oldbufsize = 0;
+
+	if (writecnt > 65536 || readcnt > 65536)
+		return SPI_INVALID_LENGTH;
+
+	/* buf is not used for the response from the chip. */
+	bufsize = max(writecnt + 9, 260 + 9);
+	/* Never shrink. realloc() calls are expensive. */
+	if (!buf || bufsize > oldbufsize) {
+		buf = realloc(buf, bufsize);
+		if (!buf) {
+			msg_perr("Out of memory!\n");
+			/* TODO: What to do with buf? */
+			return SPI_GENERIC_ERROR;
+		}
+		oldbufsize = bufsize;
+	}
+
+	/*
+	 * Minimize USB transfers by packing as many commands as possible
+	 * together. If we're not expecting to read, we can assert CS#, write,
+	 * and deassert CS# all in one shot. If reading, we do three separate
+	 * operations.
+	 */
+	msg_pspew("Assert CS#\n");
+	buf[i++] = SET_BITS_LOW;
+	buf[i++] = ~ 0x08 & pinlvl; /* assert CS (3rd) bit only */
+	buf[i++] = pindir;
+
+	if (writecnt) {
+		buf[i++] = MPSSE_DO_WRITE | MPSSE_WRITE_NEG;
+		buf[i++] = (writecnt - 1) & 0xff;
+		buf[i++] = ((writecnt - 1) >> 8) & 0xff;
+		memcpy(buf + i, writearr, writecnt);
+		i += writecnt;
+	}
+
+	/*
+	 * Optionally terminate this batch of commands with a
+	 * read command, then do the fetch of the results.
+	 */
+	if (readcnt) {
+		buf[i++] = MPSSE_DO_READ;
+		buf[i++] = (readcnt - 1) & 0xff;
+		buf[i++] = ((readcnt - 1) >> 8) & 0xff;
+		ret = send_buf(ftdic, buf, i);
+		failed = ret;
+		/* We can't abort here, we still have to deassert CS#. */
+		if (ret)
+			msg_perr("send_buf failed before read: %i\n", ret);
+		i = 0;
+		if (ret == 0) {
+			/*
+			 * FIXME: This is unreliable. There's no guarantee that
+			 * we read the response directly after sending the read
+			 * command. We may be scheduled out etc.
+			 */
+			ret = get_buf(ftdic, readarr, readcnt);
+			failed |= ret;
+			/* We can't abort here either. */
+			if (ret)
+				msg_perr("get_buf failed: %i\n", ret);
+		}
+	}
+
+	msg_pspew("De-assert CS#\n");
+	buf[i++] = SET_BITS_LOW;
+	buf[i++] = pinlvl;
+	buf[i++] = pindir;
+	ret = send_buf(ftdic, buf, i);
+	failed |= ret;
+	if (ret)
+		msg_perr("send_buf failed at end: %i\n", ret);
+
+	return failed ? -1 : 0;
+}

static const struct spi_master spi_master_ft2232 = {
    .features	= SPI_MASTER_4BA,
@@ -532,92 +615,4 @@
    return ret;
 }

-/* Returns 0 upon success, a negative number upon errors. */
-static int ft2232_spi_send_command(const struct flashctx *flash,
-				   unsigned int writecnt, unsigned int readcnt,
-				   const unsigned char *writearr,
-				   unsigned char *readarr)
-{
-	struct ftdi_context *ftdic = &ftdic_context;
-	static unsigned char *buf = NULL;
-	/* failed is special. We use bitwise ops, but it is essentially bool. */
-	int i = 0, ret = 0, failed = 0;
-	size_t bufsize;
-	static size_t oldbufsize = 0;
-
-	if (writecnt > 65536 || readcnt > 65536)
-		return SPI_INVALID_LENGTH;
-
-	/* buf is not used for the response from the chip. */
-	bufsize = max(writecnt + 9, 260 + 9);
-	/* Never shrink. realloc() calls are expensive. */
-	if (!buf || bufsize > oldbufsize) {
-		buf = realloc(buf, bufsize);
-		if (!buf) {
-			msg_perr("Out of memory!\n");
-			/* TODO: What to do with buf? */
-			return SPI_GENERIC_ERROR;
-		}
-		oldbufsize = bufsize;
-	}
-
-	/*
-	 * Minimize USB transfers by packing as many commands as possible
-	 * together. If we're not expecting to read, we can assert CS#, write,
-	 * and deassert CS# all in one shot. If reading, we do three separate
-	 * operations.
-	 */
-	msg_pspew("Assert CS#\n");
-	buf[i++] = SET_BITS_LOW;
-	buf[i++] = ~ 0x08 & pinlvl; /* assert CS (3rd) bit only */
-	buf[i++] = pindir;
-
-	if (writecnt) {
-		buf[i++] = MPSSE_DO_WRITE | MPSSE_WRITE_NEG;
-		buf[i++] = (writecnt - 1) & 0xff;
-		buf[i++] = ((writecnt - 1) >> 8) & 0xff;
-		memcpy(buf + i, writearr, writecnt);
-		i += writecnt;
-	}
-
-	/*
-	 * Optionally terminate this batch of commands with a
-	 * read command, then do the fetch of the results.
-	 */
-	if (readcnt) {
-		buf[i++] = MPSSE_DO_READ;
-		buf[i++] = (readcnt - 1) & 0xff;
-		buf[i++] = ((readcnt - 1) >> 8) & 0xff;
-		ret = send_buf(ftdic, buf, i);
-		failed = ret;
-		/* We can't abort here, we still have to deassert CS#. */
-		if (ret)
-			msg_perr("send_buf failed before read: %i\n", ret);
-		i = 0;
-		if (ret == 0) {
-			/*
-			 * FIXME: This is unreliable. There's no guarantee that
-			 * we read the response directly after sending the read
-			 * command. We may be scheduled out etc.
-			 */
-			ret = get_buf(ftdic, readarr, readcnt);
-			failed |= ret;
-			/* We can't abort here either. */
-			if (ret)
-				msg_perr("get_buf failed: %i\n", ret);
-		}
-	}
-
-	msg_pspew("De-assert CS#\n");
-	buf[i++] = SET_BITS_LOW;
-	buf[i++] = pinlvl;
-	buf[i++] = pindir;
-	ret = send_buf(ftdic, buf, i);
-	failed |= ret;
-	if (ret)
-		msg_perr("send_buf failed at end: %i\n", ret);
-
-	return failed ? -1 : 0;
-}
-
 #endif
diff --git a/ichspi.c b/ichspi.c
index 4209d60..e45b39a 100644
--- a/ichspi.c
+++ b/ichspi.c
@@ -286,12 +286,40 @@
 #define REGWRITE8(off, val)  mmio_writeb(val, ich_spibar+(off))

/* Common SPI functions */
-static int find_opcode(OPCODES *op, uint8_t opcode);
-static int find_preop(OPCODES *op, uint8_t preop);
-static int generate_opcodes(OPCODES * op, enum ich_chipset ich_gen);
-static int program_opcodes(OPCODES *op, int enable_undo, enum ich_chipset ich_gen);
-static int run_opcode(const struct flashctx *flash, OPCODE op, uint32_t offset,
-		      uint8_t datalength, uint8_t * data);
+
+static int find_opcode(OPCODES *op, uint8_t opcode)
+{
+	int a;
+
+	if (op == NULL) {
+		msg_perr("\n%s: null OPCODES pointer!\n", __func__);
+		return -1;
+	}
+
+	for (a = 0; a < 8; a++) {
+		if (op->opcode[a].opcode == opcode)
+			return a;
+	}
+
+	return -1;
+}
+
+static int find_preop(OPCODES *op, uint8_t preop)
+{
+	int a;
+
+	if (op == NULL) {
+		msg_perr("\n%s: null OPCODES pointer!\n", __func__);
+		return -1;
+	}
+
+	for (a = 0; a < 2; a++) {
+		if (op->preop[a] == preop)
+			return a;
+	}
+
+	return -1;
+}

/* for pairing opcodes with their required preop */
 struct preop_opcode_pair {
@@ -497,131 +525,6 @@
    return 0xFF;
 }

-static int reprogram_opcode_on_the_fly(uint8_t opcode, unsigned int writecnt, unsigned int readcnt)
-{
-	uint8_t spi_type;
-
-	spi_type = lookup_spi_type(opcode);
-	if (spi_type > 3) {
-		/* Try to guess spi type from read/write sizes.
-		 * The following valid writecnt/readcnt combinations exist:
-		 * writecnt  = 4, readcnt >= 0
-		 * writecnt  = 1, readcnt >= 0
-		 * writecnt >= 4, readcnt  = 0
-		 * writecnt >= 1, readcnt  = 0
-		 * writecnt >= 1 is guaranteed for all commands.
-		 */
-		if (readcnt == 0)
-			/* if readcnt=0 and writecount >= 4, we don't know if it is WRITE_NO_ADDRESS
-			 * or WRITE_WITH_ADDRESS. But if we use WRITE_NO_ADDRESS and the first 3 data
-			 * bytes are actual the address, they go to the bus anyhow
-			 */
-			spi_type = SPI_OPCODE_TYPE_WRITE_NO_ADDRESS;
-		else if (writecnt == 1) // and readcnt is > 0
-			spi_type = SPI_OPCODE_TYPE_READ_NO_ADDRESS;
-		else if (writecnt == 4) // and readcnt is > 0
-			spi_type = SPI_OPCODE_TYPE_READ_WITH_ADDRESS;
-		else // we have an invalid case
-			return SPI_INVALID_LENGTH;
-	}
-	int oppos = 2;	// use original JEDEC_BE_D8 offset
-	curopcodes->opcode[oppos].opcode = opcode;
-	curopcodes->opcode[oppos].spi_type = spi_type;
-	program_opcodes(curopcodes, 0, ich_generation);
-	oppos = find_opcode(curopcodes, opcode);
-	msg_pdbg2("on-the-fly OPCODE (0x%02X) re-programmed, op-pos=%d\n", opcode, oppos);
-	return oppos;
-}
-
-static int find_opcode(OPCODES *op, uint8_t opcode)
-{
-	int a;
-
-	if (op == NULL) {
-		msg_perr("\n%s: null OPCODES pointer!\n", __func__);
-		return -1;
-	}
-
-	for (a = 0; a < 8; a++) {
-		if (op->opcode[a].opcode == opcode)
-			return a;
-	}
-
-	return -1;
-}
-
-static int find_preop(OPCODES *op, uint8_t preop)
-{
-	int a;
-
-	if (op == NULL) {
-		msg_perr("\n%s: null OPCODES pointer!\n", __func__);
-		return -1;
-	}
-
-	for (a = 0; a < 2; a++) {
-		if (op->preop[a] == preop)
-			return a;
-	}
-
-	return -1;
-}
-
-/* Create a struct OPCODES based on what we find in the locked down chipset. */
-static int generate_opcodes(OPCODES * op, enum ich_chipset ich_gen)
-{
-	int a;
-	uint16_t preop, optype;
-	uint32_t opmenu[2];
-
-	if (op == NULL) {
-		msg_perr("\n%s: null OPCODES pointer!\n", __func__);
-		return -1;
-	}
-
-	switch (ich_gen) {
-	case CHIPSET_ICH7:
-	case CHIPSET_TUNNEL_CREEK:
-	case CHIPSET_CENTERTON:
-		preop = REGREAD16(ICH7_REG_PREOP);
-		optype = REGREAD16(ICH7_REG_OPTYPE);
-		opmenu[0] = REGREAD32(ICH7_REG_OPMENU);
-		opmenu[1] = REGREAD32(ICH7_REG_OPMENU + 4);
-		break;
-	case CHIPSET_ICH8:
-	default:		/* Future version might behave the same */
-		preop = REGREAD16(swseq_data.reg_preop);
-		optype = REGREAD16(swseq_data.reg_optype);
-		opmenu[0] = REGREAD32(swseq_data.reg_opmenu);
-		opmenu[1] = REGREAD32(swseq_data.reg_opmenu + 4);
-		break;
-	}
-
-	op->preop[0] = (uint8_t) preop;
-	op->preop[1] = (uint8_t) (preop >> 8);
-
-	for (a = 0; a < 8; a++) {
-		op->opcode[a].spi_type = (uint8_t) (optype & 0x3);
-		optype >>= 2;
-	}
-
-	for (a = 0; a < 4; a++) {
-		op->opcode[a].opcode = (uint8_t) (opmenu[0] & 0xff);
-		opmenu[0] >>= 8;
-	}
-
-	for (a = 4; a < 8; a++) {
-		op->opcode[a].opcode = (uint8_t) (opmenu[1] & 0xff);
-		opmenu[1] >>= 8;
-	}
-
-	/* No preopcodes used by default. */
-	for (a = 0; a < 8; a++)
-		op->opcode[a].atomic = 0;
-
-	return 0;
-}
-
 static int program_opcodes(OPCODES *op, int enable_undo, enum ich_chipset ich_gen)
 {
    uint8_t a;
@@ -688,6 +591,42 @@
    return 0;
 }

+static int reprogram_opcode_on_the_fly(uint8_t opcode, unsigned int writecnt, unsigned int readcnt)
+{
+	uint8_t spi_type;
+
+	spi_type = lookup_spi_type(opcode);
+	if (spi_type > 3) {
+		/* Try to guess spi type from read/write sizes.
+		 * The following valid writecnt/readcnt combinations exist:
+		 * writecnt  = 4, readcnt >= 0
+		 * writecnt  = 1, readcnt >= 0
+		 * writecnt >= 4, readcnt  = 0
+		 * writecnt >= 1, readcnt  = 0
+		 * writecnt >= 1 is guaranteed for all commands.
+		 */
+		if (readcnt == 0)
+			/* if readcnt=0 and writecount >= 4, we don't know if it is WRITE_NO_ADDRESS
+			 * or WRITE_WITH_ADDRESS. But if we use WRITE_NO_ADDRESS and the first 3 data
+			 * bytes are actual the address, they go to the bus anyhow
+			 */
+			spi_type = SPI_OPCODE_TYPE_WRITE_NO_ADDRESS;
+		else if (writecnt == 1) // and readcnt is > 0
+			spi_type = SPI_OPCODE_TYPE_READ_NO_ADDRESS;
+		else if (writecnt == 4) // and readcnt is > 0
+			spi_type = SPI_OPCODE_TYPE_READ_WITH_ADDRESS;
+		else // we have an invalid case
+			return SPI_INVALID_LENGTH;
+	}
+	int oppos = 2;	// use original JEDEC_BE_D8 offset
+	curopcodes->opcode[oppos].opcode = opcode;
+	curopcodes->opcode[oppos].spi_type = spi_type;
+	program_opcodes(curopcodes, 0, ich_generation);
+	oppos = find_opcode(curopcodes, opcode);
+	msg_pdbg2("on-the-fly OPCODE (0x%02X) re-programmed, op-pos=%d\n", opcode, oppos);
+	return oppos;
+}
+
 /*
  * Returns -1 if at least one mandatory opcode is inaccessible, 0 otherwise.
  * FIXME: this should also check for
@@ -754,49 +693,59 @@
    		 min_addr, ichspi_bbar);
 }

-/* Read len bytes from the fdata/spid register into the data array.
- *
- * Note that using len > flash->mst->spi.max_data_read will return garbage or
- * may even crash.
- */
-static void ich_read_data(uint8_t *data, int len, int reg0_off)
+/* Create a struct OPCODES based on what we find in the locked down chipset. */
+static int generate_opcodes(OPCODES * op, enum ich_chipset ich_gen)
 {
-	int i;
-	uint32_t temp32 = 0;
+	int a;
+	uint16_t preop, optype;
+	uint32_t opmenu[2];

-	for (i = 0; i < len; i++) {
-		if ((i % 4) == 0)
-			temp32 = REGREAD32(reg0_off + i);
-
-		data[i] = (temp32 >> ((i % 4) * 8)) & 0xff;
+	if (op == NULL) {
+		msg_perr("\n%s: null OPCODES pointer!\n", __func__);
+		return -1;
    }
-}

-/* Fill len bytes from the data array into the fdata/spid registers.
- *
- * Note that using len > flash->mst->spi.max_data_write will trash the registers
- * following the data registers.
- */
-static void ich_fill_data(const uint8_t *data, int len, int reg0_off)
-{
-	uint32_t temp32 = 0;
-	int i;
-
-	if (len <= 0)
-		return;
-
-	for (i = 0; i < len; i++) {
-		if ((i % 4) == 0)
-			temp32 = 0;
-
-		temp32 |= ((uint32_t) data[i]) << ((i % 4) * 8);
-
-		if ((i % 4) == 3) /* 32 bits are full, write them to regs. */
-			REGWRITE32(reg0_off + (i - (i % 4)), temp32);
+	switch (ich_gen) {
+	case CHIPSET_ICH7:
+	case CHIPSET_TUNNEL_CREEK:
+	case CHIPSET_CENTERTON:
+		preop = REGREAD16(ICH7_REG_PREOP);
+		optype = REGREAD16(ICH7_REG_OPTYPE);
+		opmenu[0] = REGREAD32(ICH7_REG_OPMENU);
+		opmenu[1] = REGREAD32(ICH7_REG_OPMENU + 4);
+		break;
+	case CHIPSET_ICH8:
+	default:		/* Future version might behave the same */
+		preop = REGREAD16(swseq_data.reg_preop);
+		optype = REGREAD16(swseq_data.reg_optype);
+		opmenu[0] = REGREAD32(swseq_data.reg_opmenu);
+		opmenu[1] = REGREAD32(swseq_data.reg_opmenu + 4);
+		break;
    }
-	i--;
-	if ((i % 4) != 3) /* Write remaining data to regs. */
-		REGWRITE32(reg0_off + (i - (i % 4)), temp32);
+
+	op->preop[0] = (uint8_t) preop;
+	op->preop[1] = (uint8_t) (preop >> 8);
+
+	for (a = 0; a < 8; a++) {
+		op->opcode[a].spi_type = (uint8_t) (optype & 0x3);
+		optype >>= 2;
+	}
+
+	for (a = 0; a < 4; a++) {
+		op->opcode[a].opcode = (uint8_t) (opmenu[0] & 0xff);
+		opmenu[0] >>= 8;
+	}
+
+	for (a = 4; a < 8; a++) {
+		op->opcode[a].opcode = (uint8_t) (opmenu[1] & 0xff);
+		opmenu[1] >>= 8;
+	}
+
+	/* No preopcodes used by default. */
+	for (a = 0; a < 8; a++)
+		op->opcode[a].atomic = 0;
+
+	return 0;
 }

/* This function generates OPCODES from or programs OPCODES to ICH according to
@@ -834,6 +783,51 @@
    }
 }

+/* Fill len bytes from the data array into the fdata/spid registers.
+ *
+ * Note that using len > flash->mst->spi.max_data_write will trash the registers
+ * following the data registers.
+ */
+static void ich_fill_data(const uint8_t *data, int len, int reg0_off)
+{
+	uint32_t temp32 = 0;
+	int i;
+
+	if (len <= 0)
+		return;
+
+	for (i = 0; i < len; i++) {
+		if ((i % 4) == 0)
+			temp32 = 0;
+
+		temp32 |= ((uint32_t) data[i]) << ((i % 4) * 8);
+
+		if ((i % 4) == 3) /* 32 bits are full, write them to regs. */
+			REGWRITE32(reg0_off + (i - (i % 4)), temp32);
+	}
+	i--;
+	if ((i % 4) != 3) /* Write remaining data to regs. */
+		REGWRITE32(reg0_off + (i - (i % 4)), temp32);
+}
+
+/* Read len bytes from the fdata/spid register into the data array.
+ *
+ * Note that using len > flash->mst->spi.max_data_read will return garbage or
+ * may even crash.
+ */
+static void ich_read_data(uint8_t *data, int len, int reg0_off)
+{
+	int i;
+	uint32_t temp32 = 0;
+
+	for (i = 0; i < len; i++) {
+		if ((i % 4) == 0)
+			temp32 = REGREAD32(reg0_off + i);
+
+		data[i] = (temp32 >> ((i % 4) * 8)) & 0xff;
+	}
+}
+
 static int ich7_run_opcode(OPCODE op, uint32_t offset,
    		   uint8_t datalength, uint8_t * data, int maxdata)
 {
diff --git a/linux_spi.c b/linux_spi.c
index 1ef8f2b..bbd45f3 100644
--- a/linux_spi.c
+++ b/linux_spi.c
@@ -48,15 +48,65 @@
 #define BUF_SIZE_FROM_SYSFS	"/sys/module/spidev/parameters/bufsiz"
 static size_t max_kernel_buf_size;

-static int linux_spi_shutdown(void *data);
+static int linux_spi_read(struct flashctx *flash, uint8_t *buf, unsigned int start, unsigned int len)
+{
+	/* Older kernels use a single buffer for combined input and output
+	   data. So account for longest possible command + address, too. */
+	return spi_read_chunked(flash, buf, start, len, max_kernel_buf_size - 5);
+}
+
+static int linux_spi_write_256(struct flashctx *flash, const uint8_t *buf, unsigned int start, unsigned int len)
+{
+	/* 5 bytes must be reserved for longest possible command + address. */
+	return spi_write_chunked(flash, buf, start, len, max_kernel_buf_size - 5);
+}
+
+static int linux_spi_shutdown(void *data)
+{
+	if (fd != -1) {
+		close(fd);
+		fd = -1;
+	}
+	return 0;
+}
+
 static int linux_spi_send_command(const struct flashctx *flash, unsigned int writecnt,
    			  unsigned int readcnt,
    			  const unsigned char *txbuf,
-				  unsigned char *rxbuf);
-static int linux_spi_read(struct flashctx *flash, uint8_t *buf,
-			  unsigned int start, unsigned int len);
-static int linux_spi_write_256(struct flashctx *flash, const uint8_t *buf,
-			       unsigned int start, unsigned int len);
+				  unsigned char *rxbuf)
+{
+	int iocontrol_code;
+	struct spi_ioc_transfer msg[2] = {
+		{
+			.tx_buf = (uint64_t)(uintptr_t)txbuf,
+			.len = writecnt,
+		},
+		{
+			.rx_buf = (uint64_t)(uintptr_t)rxbuf,
+			.len = readcnt,
+		},
+	};
+
+	if (fd == -1)
+		return -1;
+	/* The implementation currently does not support requests that
+	   don't start with sending a command. */
+	if (writecnt == 0)
+		return SPI_INVALID_LENGTH;
+
+	/* Just submit the first (write) request in case there is nothing
+	   to read. Otherwise submit both requests. */
+	if (readcnt == 0)
+		iocontrol_code = SPI_IOC_MESSAGE(1);
+	else
+		iocontrol_code = SPI_IOC_MESSAGE(2);
+
+	if (ioctl(fd, iocontrol_code, msg) == -1) {
+		msg_cerr("%s: ioctl: %s\n", __func__, strerror(errno));
+		return -1;
+	}
+	return 0;
+}

static const struct spi_master spi_master_linux = {
    .features	= SPI_MASTER_4BA,
@@ -174,64 +224,4 @@
    return 0;
 }

-static int linux_spi_shutdown(void *data)
-{
-	if (fd != -1) {
-		close(fd);
-		fd = -1;
-	}
-	return 0;
-}
-
-static int linux_spi_send_command(const struct flashctx *flash, unsigned int writecnt,
-				  unsigned int readcnt,
-				  const unsigned char *txbuf,
-				  unsigned char *rxbuf)
-{
-	int iocontrol_code;
-	struct spi_ioc_transfer msg[2] = {
-		{
-			.tx_buf = (uint64_t)(uintptr_t)txbuf,
-			.len = writecnt,
-		},
-		{
-			.rx_buf = (uint64_t)(uintptr_t)rxbuf,
-			.len = readcnt,
-		},
-	};
-
-	if (fd == -1)
-		return -1;
-	/* The implementation currently does not support requests that
-	   don't start with sending a command. */
-	if (writecnt == 0)
-		return SPI_INVALID_LENGTH;
-
-	/* Just submit the first (write) request in case there is nothing
-	   to read. Otherwise submit both requests. */
-	if (readcnt == 0)
-		iocontrol_code = SPI_IOC_MESSAGE(1);
-	else
-		iocontrol_code = SPI_IOC_MESSAGE(2);
-
-	if (ioctl(fd, iocontrol_code, msg) == -1) {
-		msg_cerr("%s: ioctl: %s\n", __func__, strerror(errno));
-		return -1;
-	}
-	return 0;
-}
-
-static int linux_spi_read(struct flashctx *flash, uint8_t *buf, unsigned int start, unsigned int len)
-{
-	/* Older kernels use a single buffer for combined input and output
-	   data. So account for longest possible command + address, too. */
-	return spi_read_chunked(flash, buf, start, len, max_kernel_buf_size - 5);
-}
-
-static int linux_spi_write_256(struct flashctx *flash, const uint8_t *buf, unsigned int start, unsigned int len)
-{
-	/* 5 bytes must be reserved for longest possible command + address. */
-	return spi_write_chunked(flash, buf, start, len, max_kernel_buf_size - 5);
-}
-
 #endif // CONFIG_LINUX_SPI == 1
diff --git a/ni845x_spi.c b/ni845x_spi.c
index 7b2bea3..153e03e 100644
--- a/ni845x_spi.c
+++ b/ni845x_spi.c
@@ -50,10 +50,6 @@
 static uint16_t io_voltage_in_mV;
 static bool ignore_io_voltage_limits;

-static int ni845x_spi_shutdown(void *data);
-static int32 ni845x_spi_open_resource(char *resource_handle, uInt32 *opened_handle);
-static void ni845x_spi_print_available_devices(void);
-
 // USB-8452 supported voltages, keep this array in ascending order!
 static const uint8_t usb8452_io_voltages_in_100mV[5] = {
    kNi845x12Volts,
@@ -129,6 +125,28 @@
 }

/**
+ * @brief ni845x_spi_open_resource
+ * @param resource_handle the resource handle returned by the ni845xFindDevice or ni845xFindDeviceNext
+ * @param opened_handle the opened handle from the ni845xOpen
+ * @return the 0 on successful competition, negative error code on failure positive code on warning
+ */
+static int32 ni845x_spi_open_resource(char *resource_handle, uInt32 *opened_handle)
+{
+	// NI-845x driver loads the FPGA bitfile at the first time
+	// which can take couple seconds
+	if (device_pid == USB8452)
+		msg_pwarn("Opening NI-8452, this might take a while for the first time\n");
+
+	int32 tmp = ni845xOpen(resource_handle, opened_handle);
+
+	if (tmp < 0)
+		ni845x_report_error("ni845xOpen", tmp);
+	else if (tmp > 0)
+		ni845x_report_warning("ni845xOpen", tmp);
+	return tmp;
+}
+
+/**
  * @param serial a null terminated string containing the serial number of the specific device or NULL
  * @return the 0 on successful completition, negative error code on failure
  */
@@ -194,28 +212,6 @@
 }

/**
- * @brief ni845x_spi_open_resource
- * @param resource_handle the resource handle returned by the ni845xFindDevice or ni845xFindDeviceNext
- * @param opened_handle the opened handle from the ni845xOpen
- * @return the 0 on successful competition, negative error code on failure positive code on warning
- */
-static int32 ni845x_spi_open_resource(char *resource_handle, uInt32 *opened_handle)
-{
-	// NI-845x driver loads the FPGA bitfile at the first time
-	// which can take couple seconds
-	if (device_pid == USB8452)
-		msg_pwarn("Opening NI-8452, this might take a while for the first time\n");
-
-	int32 tmp = ni845xOpen(resource_handle, opened_handle);
-
-	if (tmp < 0)
-		ni845x_report_error("ni845xOpen", tmp);
-	else if (tmp > 0)
-		ni845x_report_warning("ni845xOpen", tmp);
-	return tmp;
-}
-
-/**
  * @brief usb8452_spi_set_io_voltage sets the IO voltage for the USB-8452 devices
  * @param requested_io_voltage_mV the desired IO voltage in mVolts
  * @param set_io_voltage_mV the IO voltage which was set in mVolts
@@ -389,6 +385,24 @@
    	ni845x_report_error("ni845xCloseFindDeviceHandle", tmp);
 }

+static int ni845x_spi_shutdown(void *data)
+{
+	int32 ret = 0;
+
+	if (configuration_handle != 0) {
+		ret = ni845xSpiConfigurationClose(configuration_handle);
+		if (ret)
+			ni845x_report_error("ni845xSpiConfigurationClose", ret);
+	}
+
+	if (device_handle != 0) {
+		ret = ni845xClose(device_handle);
+		if (ret)
+			ni845x_report_error("ni845xClose", ret);
+	}
+	return 0;
+}
+
 int ni845x_spi_init(void)
 {
    char *speed_str = NULL;
@@ -486,24 +500,6 @@
    return 0;
 }

-static int ni845x_spi_shutdown(void *data)
-{
-	int32 ret = 0;
-
-	if (configuration_handle != 0) {
-		ret = ni845xSpiConfigurationClose(configuration_handle);
-		if (ret)
-			ni845x_report_error("ni845xSpiConfigurationClose", ret);
-	}
-
-	if (device_handle != 0) {
-		ret = ni845xClose(device_handle);
-		if (ret)
-			ni845x_report_error("ni845xClose", ret);
-	}
-	return 0;
-}
-
 static void ni845x_warn_over_max_voltage(const struct flashctx *flash)
 {
    if (device_pid == USB8451) {
diff --git a/serprog.c b/serprog.c
index 1befce1..d583a40 100644
--- a/serprog.c
+++ b/serprog.c
@@ -42,13 +42,6 @@

#define MSGHEADER "serprog: "

-/*
- * FIXME: This prototype was added to help reduce diffs for the shutdown
- * registration patch, which shifted many lines of code to place
- * serprog_shutdown() before serprog_init(). It should be removed soon.
- */
-static int serprog_shutdown(void *data);
-
 static uint16_t sp_device_serbuf_size = 16;
 static uint16_t sp_device_opbuf_size = 300;
 /* Bitmap of supported commands */
@@ -295,10 +288,112 @@
    return 0;
 }

+/* Move an in flashrom buffer existing write-n operation to the on-device operation buffer. */
+static int sp_pass_writen(void)
+{
+	unsigned char header[7];
+	msg_pspew(MSGHEADER "Passing write-n bytes=%d addr=0x%x\n", sp_write_n_bytes, sp_write_n_addr);
+	if (sp_streamed_transmit_bytes >= (7 + sp_write_n_bytes + sp_device_serbuf_size)) {
+		if (sp_flush_stream() != 0) {
+			return 1;
+		}
+	}
+	/* In case it's just a single byte send it as a single write. */
+	if (sp_write_n_bytes == 1) {
+		sp_write_n_bytes = 0;
+		header[0] = (sp_write_n_addr >> 0) & 0xFF;
+		header[1] = (sp_write_n_addr >> 8) & 0xFF;
+		header[2] = (sp_write_n_addr >> 16) & 0xFF;
+		header[3] = sp_write_n_buf[0];
+		if (sp_stream_buffer_op(S_CMD_O_WRITEB, 4, header) != 0)
+			return 1;
+		sp_opbuf_usage += 5;
+		return 0;
+	}
+	header[0] = S_CMD_O_WRITEN;
+	header[1] = (sp_write_n_bytes >> 0) & 0xFF;
+	header[2] = (sp_write_n_bytes >> 8) & 0xFF;
+	header[3] = (sp_write_n_bytes >> 16) & 0xFF;
+	header[4] = (sp_write_n_addr >> 0) & 0xFF;
+	header[5] = (sp_write_n_addr >> 8) & 0xFF;
+	header[6] = (sp_write_n_addr >> 16) & 0xFF;
+	if (serialport_write(header, 7) != 0) {
+		msg_perr(MSGHEADER "Error: cannot write write-n command\n");
+		return 1;
+	}
+	if (serialport_write(sp_write_n_buf, sp_write_n_bytes) != 0) {
+		msg_perr(MSGHEADER "Error: cannot write write-n data");
+		return 1;
+	}
+	sp_streamed_transmit_bytes += 7 + sp_write_n_bytes;
+	sp_streamed_transmit_ops += 1;
+	sp_opbuf_usage += 7 + sp_write_n_bytes;
+	sp_write_n_bytes = 0;
+	sp_prev_was_write = 0;
+	return 0;
+}
+
+static int sp_execute_opbuf_noflush(void)
+{
+	if ((sp_max_write_n) && (sp_write_n_bytes)) {
+		if (sp_pass_writen() != 0) {
+			msg_perr("Error: could not transfer write buffer\n");
+			return 1;
+		}
+	}
+	if (sp_stream_buffer_op(S_CMD_O_EXEC, 0, NULL) != 0) {
+		msg_perr("Error: could not execute command buffer\n");
+		return 1;
+	}
+	msg_pspew(MSGHEADER "Executed operation buffer of %d bytes\n", sp_opbuf_usage);
+	sp_opbuf_usage = 0;
+	sp_prev_was_write = 0;
+	return 0;
+}
+
+static int sp_execute_opbuf(void)
+{
+	if (sp_execute_opbuf_noflush() != 0)
+		return 1;
+	if (sp_flush_stream() != 0)
+		return 1;
+
+	return 0;
+}
+
 static int serprog_spi_send_command(const struct flashctx *flash,
    			    unsigned int writecnt, unsigned int readcnt,
    			    const unsigned char *writearr,
-				    unsigned char *readarr);
+				    unsigned char *readarr)
+{
+	unsigned char *parmbuf;
+	int ret;
+	msg_pspew("%s, writecnt=%i, readcnt=%i\n", __func__, writecnt, readcnt);
+	if ((sp_opbuf_usage) || (sp_max_write_n && sp_write_n_bytes)) {
+		if (sp_execute_opbuf() != 0) {
+			msg_perr("Error: could not execute command buffer before sending SPI commands.\n");
+			return 1;
+		}
+	}
+
+	parmbuf = malloc(writecnt + 6);
+	if (!parmbuf) {
+		msg_perr("Error: could not allocate SPI send param buffer.\n");
+		return 1;
+	}
+	parmbuf[0] = (writecnt >> 0) & 0xFF;
+	parmbuf[1] = (writecnt >> 8) & 0xFF;
+	parmbuf[2] = (writecnt >> 16) & 0xFF;
+	parmbuf[3] = (readcnt >> 0) & 0xFF;
+	parmbuf[4] = (readcnt >> 8) & 0xFF;
+	parmbuf[5] = (readcnt >> 16) & 0xFF;
+	memcpy(parmbuf + 6, writearr, writecnt);
+	ret = sp_docommand(S_CMD_O_SPIOP, writecnt + 6, parmbuf, readcnt,
+			   readarr);
+	free(parmbuf);
+	return ret;
+}
+
 static struct spi_master spi_master_serprog = {
    .features	= SPI_MASTER_4BA,
    .max_data_read	= MAX_DATA_READ_UNLIMITED,
@@ -310,12 +405,108 @@
    .write_aai	= default_spi_write_aai,
 };

+static int sp_check_opbuf_usage(int bytes_to_be_added)
+{
+	if (sp_device_opbuf_size <= (sp_opbuf_usage + bytes_to_be_added)) {
+		/* If this happens in the middle of a page load the page load will probably fail. */
+		msg_pwarn(MSGHEADER "Warning: executed operation buffer due to size reasons\n");
+		if (sp_execute_opbuf() != 0)
+			return 1;
+	}
+	return 0;
+}
+
 static void serprog_chip_writeb(const struct flashctx *flash, uint8_t val,
-				chipaddr addr);
+				chipaddr addr)
+{
+	msg_pspew("%s\n", __func__);
+	if (sp_max_write_n) {
+		if ((sp_prev_was_write)
+		    && (addr == (sp_write_n_addr + sp_write_n_bytes))) {
+			sp_write_n_buf[sp_write_n_bytes++] = val;
+		} else {
+			if ((sp_prev_was_write) && (sp_write_n_bytes))
+				sp_pass_writen();
+			sp_prev_was_write = 1;
+			sp_write_n_addr = addr;
+			sp_write_n_bytes = 1;
+			sp_write_n_buf[0] = val;
+		}
+		sp_check_opbuf_usage(7 + sp_write_n_bytes);
+		if (sp_write_n_bytes >= sp_max_write_n)
+			sp_pass_writen();
+	} else {
+		/* We will have to do single writeb ops. */
+		unsigned char writeb_parm[4];
+		sp_check_opbuf_usage(6);
+		writeb_parm[0] = (addr >> 0) & 0xFF;
+		writeb_parm[1] = (addr >> 8) & 0xFF;
+		writeb_parm[2] = (addr >> 16) & 0xFF;
+		writeb_parm[3] = val;
+		sp_stream_buffer_op(S_CMD_O_WRITEB, 4, writeb_parm); // FIXME: return error
+		sp_opbuf_usage += 5;
+	}
+}
+
 static uint8_t serprog_chip_readb(const struct flashctx *flash,
-				  const chipaddr addr);
-static void serprog_chip_readn(const struct flashctx *flash, uint8_t *buf,
-			       const chipaddr addr, size_t len);
+				  const chipaddr addr)
+{
+	unsigned char c;
+	unsigned char buf[3];
+	/* Will stream the read operation - eg. add it to the stream buffer, *
+	 * then flush the buffer, then read the read answer.		     */
+	if ((sp_opbuf_usage) || (sp_max_write_n && sp_write_n_bytes))
+		sp_execute_opbuf_noflush();
+	buf[0] = ((addr >> 0) & 0xFF);
+	buf[1] = ((addr >> 8) & 0xFF);
+	buf[2] = ((addr >> 16) & 0xFF);
+	sp_stream_buffer_op(S_CMD_R_BYTE, 3, buf); // FIXME: return error
+	sp_flush_stream(); // FIXME: return error
+	if (serialport_read(&c, 1) != 0)
+		msg_perr(MSGHEADER "readb byteread");  // FIXME: return error
+	msg_pspew("%s addr=0x%" PRIxPTR " returning 0x%02X\n", __func__, addr, c);
+	return c;
+}
+
+/* Local version that really does the job, doesn't care of max_read_n. */
+static int sp_do_read_n(uint8_t * buf, const chipaddr addr, size_t len)
+{
+	unsigned char sbuf[6];
+	msg_pspew("%s: addr=0x%" PRIxPTR " len=%zu\n", __func__, addr, len);
+	/* Stream the read-n -- as above. */
+	if ((sp_opbuf_usage) || (sp_max_write_n && sp_write_n_bytes))
+		sp_execute_opbuf_noflush();
+	sbuf[0] = ((addr >> 0) & 0xFF);
+	sbuf[1] = ((addr >> 8) & 0xFF);
+	sbuf[2] = ((addr >> 16) & 0xFF);
+	sbuf[3] = ((len >> 0) & 0xFF);
+	sbuf[4] = ((len >> 8) & 0xFF);
+	sbuf[5] = ((len >> 16) & 0xFF);
+	sp_stream_buffer_op(S_CMD_R_NBYTES, 6, sbuf);
+	if (sp_flush_stream() != 0)
+		return 1;
+	if (serialport_read(buf, len) != 0) {
+		msg_perr(MSGHEADER "Error: cannot read read-n data");
+		return 1;
+	}
+	return 0;
+}
+
+/* The externally called version that makes sure that max_read_n is obeyed. */
+static void serprog_chip_readn(const struct flashctx *flash, uint8_t * buf,
+			       const chipaddr addr, size_t len)
+{
+	size_t lenm = len;
+	chipaddr addrm = addr;
+	while ((sp_max_read_n != 0) && (lenm > sp_max_read_n)) {
+		sp_do_read_n(&(buf[addrm-addr]), addrm, sp_max_read_n); // FIXME: return error
+		addrm += sp_max_read_n;
+		lenm -= sp_max_read_n;
+	}
+	if (lenm)
+		sp_do_read_n(&(buf[addrm-addr]), addrm, lenm); // FIXME: return error
+}
+
 static const struct par_master par_master_serprog = {
    	.chip_readb		= serprog_chip_readb,
    	.chip_readw		= fallback_chip_readw,
@@ -327,6 +518,25 @@
    	.chip_writen		= fallback_chip_writen,
 };

+static int serprog_shutdown(void *data)
+{
+	if ((sp_opbuf_usage) || (sp_max_write_n && sp_write_n_bytes))
+	if (sp_execute_opbuf() != 0)
+		msg_pwarn("Could not flush command buffer.\n");
+	if (sp_check_commandavail(S_CMD_S_PIN_STATE)) {
+		uint8_t dis = 0;
+		if (sp_docommand(S_CMD_S_PIN_STATE, 1, &dis, 0, NULL) == 0)
+			msg_pdbg(MSGHEADER "Output drivers disabled\n");
+		else
+			msg_pwarn(MSGHEADER "%s: Warning: could not disable output buffers\n", __func__);
+	}
+	/* FIXME: fix sockets on windows(?), especially closing */
+	serialport_shutdown(&sp_fd);
+	if (sp_max_write_n)
+		free(sp_write_n_buf);
+	return 0;
+}
+
 static enum chipbustype serprog_buses_supported = BUS_NONE;

int serprog_init(void)
@@ -679,200 +889,6 @@
    return 0;
 }

-/* Move an in flashrom buffer existing write-n operation to the on-device operation buffer. */
-static int sp_pass_writen(void)
-{
-	unsigned char header[7];
-	msg_pspew(MSGHEADER "Passing write-n bytes=%d addr=0x%x\n", sp_write_n_bytes, sp_write_n_addr);
-	if (sp_streamed_transmit_bytes >= (7 + sp_write_n_bytes + sp_device_serbuf_size)) {
-		if (sp_flush_stream() != 0) {
-			return 1;
-		}
-	}
-	/* In case it's just a single byte send it as a single write. */
-	if (sp_write_n_bytes == 1) {
-		sp_write_n_bytes = 0;
-		header[0] = (sp_write_n_addr >> 0) & 0xFF;
-		header[1] = (sp_write_n_addr >> 8) & 0xFF;
-		header[2] = (sp_write_n_addr >> 16) & 0xFF;
-		header[3] = sp_write_n_buf[0];
-		if (sp_stream_buffer_op(S_CMD_O_WRITEB, 4, header) != 0)
-			return 1;
-		sp_opbuf_usage += 5;
-		return 0;
-	}
-	header[0] = S_CMD_O_WRITEN;
-	header[1] = (sp_write_n_bytes >> 0) & 0xFF;
-	header[2] = (sp_write_n_bytes >> 8) & 0xFF;
-	header[3] = (sp_write_n_bytes >> 16) & 0xFF;
-	header[4] = (sp_write_n_addr >> 0) & 0xFF;
-	header[5] = (sp_write_n_addr >> 8) & 0xFF;
-	header[6] = (sp_write_n_addr >> 16) & 0xFF;
-	if (serialport_write(header, 7) != 0) {
-		msg_perr(MSGHEADER "Error: cannot write write-n command\n");
-		return 1;
-	}
-	if (serialport_write(sp_write_n_buf, sp_write_n_bytes) != 0) {
-		msg_perr(MSGHEADER "Error: cannot write write-n data");
-		return 1;
-	}
-	sp_streamed_transmit_bytes += 7 + sp_write_n_bytes;
-	sp_streamed_transmit_ops += 1;
-	sp_opbuf_usage += 7 + sp_write_n_bytes;
-	sp_write_n_bytes = 0;
-	sp_prev_was_write = 0;
-	return 0;
-}
-
-static int sp_execute_opbuf_noflush(void)
-{
-	if ((sp_max_write_n) && (sp_write_n_bytes)) {
-		if (sp_pass_writen() != 0) {
-			msg_perr("Error: could not transfer write buffer\n");
-			return 1;
-		}
-	}
-	if (sp_stream_buffer_op(S_CMD_O_EXEC, 0, NULL) != 0) {
-		msg_perr("Error: could not execute command buffer\n");
-		return 1;
-	}
-	msg_pspew(MSGHEADER "Executed operation buffer of %d bytes\n", sp_opbuf_usage);
-	sp_opbuf_usage = 0;
-	sp_prev_was_write = 0;
-	return 0;
-}
-
-static int sp_execute_opbuf(void)
-{
-	if (sp_execute_opbuf_noflush() != 0)
-		return 1;
-	if (sp_flush_stream() != 0)
-		return 1;
-
-	return 0;
-}
-
-static int serprog_shutdown(void *data)
-{
-	if ((sp_opbuf_usage) || (sp_max_write_n && sp_write_n_bytes))
-	if (sp_execute_opbuf() != 0)
-		msg_pwarn("Could not flush command buffer.\n");
-	if (sp_check_commandavail(S_CMD_S_PIN_STATE)) {
-		uint8_t dis = 0;
-		if (sp_docommand(S_CMD_S_PIN_STATE, 1, &dis, 0, NULL) == 0)
-			msg_pdbg(MSGHEADER "Output drivers disabled\n");
-		else
-			msg_pwarn(MSGHEADER "%s: Warning: could not disable output buffers\n", __func__);
-	}
-	/* FIXME: fix sockets on windows(?), especially closing */
-	serialport_shutdown(&sp_fd);
-	if (sp_max_write_n)
-		free(sp_write_n_buf);
-	return 0;
-}
-
-static int sp_check_opbuf_usage(int bytes_to_be_added)
-{
-	if (sp_device_opbuf_size <= (sp_opbuf_usage + bytes_to_be_added)) {
-		/* If this happens in the middle of a page load the page load will probably fail. */
-		msg_pwarn(MSGHEADER "Warning: executed operation buffer due to size reasons\n");
-		if (sp_execute_opbuf() != 0)
-			return 1;
-	}
-	return 0;
-}
-
-static void serprog_chip_writeb(const struct flashctx *flash, uint8_t val,
-				chipaddr addr)
-{
-	msg_pspew("%s\n", __func__);
-	if (sp_max_write_n) {
-		if ((sp_prev_was_write)
-		    && (addr == (sp_write_n_addr + sp_write_n_bytes))) {
-			sp_write_n_buf[sp_write_n_bytes++] = val;
-		} else {
-			if ((sp_prev_was_write) && (sp_write_n_bytes))
-				sp_pass_writen();
-			sp_prev_was_write = 1;
-			sp_write_n_addr = addr;
-			sp_write_n_bytes = 1;
-			sp_write_n_buf[0] = val;
-		}
-		sp_check_opbuf_usage(7 + sp_write_n_bytes);
-		if (sp_write_n_bytes >= sp_max_write_n)
-			sp_pass_writen();
-	} else {
-		/* We will have to do single writeb ops. */
-		unsigned char writeb_parm[4];
-		sp_check_opbuf_usage(6);
-		writeb_parm[0] = (addr >> 0) & 0xFF;
-		writeb_parm[1] = (addr >> 8) & 0xFF;
-		writeb_parm[2] = (addr >> 16) & 0xFF;
-		writeb_parm[3] = val;
-		sp_stream_buffer_op(S_CMD_O_WRITEB, 4, writeb_parm); // FIXME: return error
-		sp_opbuf_usage += 5;
-	}
-}
-
-static uint8_t serprog_chip_readb(const struct flashctx *flash,
-				  const chipaddr addr)
-{
-	unsigned char c;
-	unsigned char buf[3];
-	/* Will stream the read operation - eg. add it to the stream buffer, *
-	 * then flush the buffer, then read the read answer.		     */
-	if ((sp_opbuf_usage) || (sp_max_write_n && sp_write_n_bytes))
-		sp_execute_opbuf_noflush();
-	buf[0] = ((addr >> 0) & 0xFF);
-	buf[1] = ((addr >> 8) & 0xFF);
-	buf[2] = ((addr >> 16) & 0xFF);
-	sp_stream_buffer_op(S_CMD_R_BYTE, 3, buf); // FIXME: return error
-	sp_flush_stream(); // FIXME: return error
-	if (serialport_read(&c, 1) != 0)
-		msg_perr(MSGHEADER "readb byteread");  // FIXME: return error
-	msg_pspew("%s addr=0x%" PRIxPTR " returning 0x%02X\n", __func__, addr, c);
-	return c;
-}
-
-/* Local version that really does the job, doesn't care of max_read_n. */
-static int sp_do_read_n(uint8_t * buf, const chipaddr addr, size_t len)
-{
-	unsigned char sbuf[6];
-	msg_pspew("%s: addr=0x%" PRIxPTR " len=%zu\n", __func__, addr, len);
-	/* Stream the read-n -- as above. */
-	if ((sp_opbuf_usage) || (sp_max_write_n && sp_write_n_bytes))
-		sp_execute_opbuf_noflush();
-	sbuf[0] = ((addr >> 0) & 0xFF);
-	sbuf[1] = ((addr >> 8) & 0xFF);
-	sbuf[2] = ((addr >> 16) & 0xFF);
-	sbuf[3] = ((len >> 0) & 0xFF);
-	sbuf[4] = ((len >> 8) & 0xFF);
-	sbuf[5] = ((len >> 16) & 0xFF);
-	sp_stream_buffer_op(S_CMD_R_NBYTES, 6, sbuf);
-	if (sp_flush_stream() != 0)
-		return 1;
-	if (serialport_read(buf, len) != 0) {
-		msg_perr(MSGHEADER "Error: cannot read read-n data");
-		return 1;
-	}
-	return 0;
-}
-
-/* The externally called version that makes sure that max_read_n is obeyed. */
-static void serprog_chip_readn(const struct flashctx *flash, uint8_t * buf,
-			       const chipaddr addr, size_t len)
-{
-	size_t lenm = len;
-	chipaddr addrm = addr;
-	while ((sp_max_read_n != 0) && (lenm > sp_max_read_n)) {
-		sp_do_read_n(&(buf[addrm-addr]), addrm, sp_max_read_n); // FIXME: return error
-		addrm += sp_max_read_n;
-		lenm -= sp_max_read_n;
-	}
-	if (lenm)
-		sp_do_read_n(&(buf[addrm-addr]), addrm, lenm); // FIXME: return error
-}
-
 void serprog_delay(unsigned int usecs)
 {
    unsigned char buf[4];
@@ -894,39 +910,6 @@
    sp_prev_was_write = 0;
 }

-static int serprog_spi_send_command(const struct flashctx *flash,
-				    unsigned int writecnt, unsigned int readcnt,
-				    const unsigned char *writearr,
-				    unsigned char *readarr)
-{
-	unsigned char *parmbuf;
-	int ret;
-	msg_pspew("%s, writecnt=%i, readcnt=%i\n", __func__, writecnt, readcnt);
-	if ((sp_opbuf_usage) || (sp_max_write_n && sp_write_n_bytes)) {
-		if (sp_execute_opbuf() != 0) {
-			msg_perr("Error: could not execute command buffer before sending SPI commands.\n");
-			return 1;
-		}
-	}
-
-	parmbuf = malloc(writecnt + 6);
-	if (!parmbuf) {
-		msg_perr("Error: could not allocate SPI send param buffer.\n");
-		return 1;
-	}
-	parmbuf[0] = (writecnt >> 0) & 0xFF;
-	parmbuf[1] = (writecnt >> 8) & 0xFF;
-	parmbuf[2] = (writecnt >> 16) & 0xFF;
-	parmbuf[3] = (readcnt >> 0) & 0xFF;
-	parmbuf[4] = (readcnt >> 8) & 0xFF;
-	parmbuf[5] = (readcnt >> 16) & 0xFF;
-	memcpy(parmbuf + 6, writearr, writecnt);
-	ret = sp_docommand(S_CMD_O_SPIOP, writecnt + 6, parmbuf, readcnt,
-			   readarr);
-	free(parmbuf);
-	return ret;
-}
-
 void *serprog_map(const char *descr, uintptr_t phys_addr, size_t len)
 {
    /* Serprog transmits 24 bits only and assumes the underlying implementation handles any remaining bits