Flash, FRAM and EEPROM driver for STM32 QUAD-/OCTOSPI interface

- write speed up to 150 kByte/s on STM32F469I-disco (due to
  SWD clock and USB connection), up to 1 MByte/s on Nucleo-F767ZI
  with external STLink-V3 or Nucleo-G474RE with two W25Q256FV in
  dual 4-line mode or STM32H73BI-Disco in octal mode
- tested with STM32L476G-disco (64MBit flash, 3-byte addr),
  STM32F412G-Disco, STM32F469I-Disco, STM32F746G-Disco, and
  STM32L476G-Disco (all 128Mbit flash, 3-byte addr),
  STM32F723E-Disco, STM32F769I-Disco (512Mbit flash, 4-byte addr)
  STM32L4R9I-Disco, STM32L4P5G-Disco (512MBit octo-flash, DTR, 4-byte addr)
  STM32H745I-Disco, STM32H747I-Disco (two 512MBit flash, 4-byte addr)
  STM32H73BI-Disco, STM32H735G-Disco (512MBit octo-flash, DTR, 4-byte addr)
- suitable cfg for Discovery boards included
- limited parsing of SFDP data if flash device not hardcoded
  (tested only in single/quad mode as most devices either don't
  support SFDP at all or have empty(!) SFDP memory)
- 'set' command for auto detection override (e. g. for EEPROMs)
- 'cmd' command for arbitrary SPI commands (reconfiguration, testing etc.)
- makefile for creation of binary loader files
- tcl/board/stm32f469discovery.cfg superseded by stm32f469i-disco.cfg
- tcl/board/stm32f7discovery.cfg removed as name is ambiguous
  (superseded by stm32f746g-disco.cfg vs. stm32f769i-disco.cfg)
- dual 4-line mode tested on Nucleo-F767ZI, Nucleo-H743ZI and Nucleo-H7A3ZI-Q
  with two W25Q256FV, and on Nucleo-L496ZP-P and Nucleo-L4R5ZI
  with two W25Q128FV, sample cfg files included and on STM32H745I-Disco,
  STM32H747I-Disco, STM32H750B-Disco
- read/verify/erase_check uses indirect read mode to work around silicon bug in
  H7, L4+ and MP1 memory mapped mode (last bytes not readable, accessing last
  bytes causes debug interface to hang)
- octospi supported only in single/dual 1-line, 2-line, 4-line
  and single 8-line modes, (not in hyper flash mode)

Requirements:
GPIOs must be initialized appropriately, and SPI flash chip be configured
appropriately (1-line ..., QPI, 4-byte addresses ...). This is board/chip
specific, cf. included cfg files. The driver infers most parameters from
current setting in CR, CCR, ... registers.

Change-Id: I54858fbbe8758c3a5fe58812e93f5f39514704f8
Signed-off-by: Andreas Bolsch <hyphen0break@gmail.com>
Reviewed-on: http://openocd.zylin.com/4321
Tested-by: jenkins
Reviewed-by: Tarek BOCHKATI <tarek.bouchkati@gmail.com>
Reviewed-by: Tomas Vanek <vanekt@fbl.cz>
Reviewed-by: Christopher Head <chead@zaber.com>

17 files changed, 3215 insertions, 32 deletions

diff --git a/src/flash/nor/Makefile.am b/src/flash/nor/Makefile.am
index b95b003..8c9b2b7 100644
--- a/src/flash/nor/Makefile.am
+++ b/src/flash/nor/Makefile.am
@@ -52,10 +52,12 @@ NOR_DRIVERS = \
 	%D%/psoc5lp.c \
 	%D%/psoc6.c \
 	%D%/renesas_rpchf.c \
+	%D%/sfdp.c \
 	%D%/sh_qspi.c \
 	%D%/sim3x.c \
 	%D%/spi.c \
 	%D%/stmsmi.c \
+	%D%/stmqspi.c \
 	%D%/stellaris.c \
 	%D%/stm32f1x.c \
 	%D%/stm32f2x.c \
@@ -83,6 +85,8 @@ NORHEADERS = \
 	%D%/imp.h \
 	%D%/non_cfi.h \
 	%D%/ocl.h \
+	%D%/sfdp.h \
 	%D%/spi.h \
 	%D%/stm32l4x.h \
+	%D%/stmqspi.h \
 	%D%/msp432.h
diff --git a/src/flash/nor/core.c b/src/flash/nor/core.c
index 6182a5f..c162c70 100644
--- a/src/flash/nor/core.c
+++ b/src/flash/nor/core.c
@@ -94,7 +94,7 @@ int flash_driver_protect(struct flash_bank *bank, int set, unsigned int first,
 }
 
 int flash_driver_write(struct flash_bank *bank,
-	uint8_t *buffer, uint32_t offset, uint32_t count)
+	const uint8_t *buffer, uint32_t offset, uint32_t count)
 {
 	int retval;
 
@@ -135,6 +135,43 @@ int default_flash_read(struct flash_bank *bank,
 	return target_read_buffer(bank->target, offset + bank->base, count, buffer);
 }
 
+int flash_driver_verify(struct flash_bank *bank,
+	const uint8_t *buffer, uint32_t offset, uint32_t count)
+{
+	int retval;
+
+	retval = bank->driver->verify ? bank->driver->verify(bank, buffer, offset, count) :
+		default_flash_verify(bank, buffer, offset, count);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("verify failed in bank at " TARGET_ADDR_FMT " starting at 0x%8.8" PRIx32,
+			bank->base, offset);
+	}
+
+	return retval;
+}
+
+int default_flash_verify(struct flash_bank *bank,
+	const uint8_t *buffer, uint32_t offset, uint32_t count)
+{
+	uint32_t target_crc, image_crc;
+	int retval;
+
+	retval = image_calculate_checksum(buffer, count, &image_crc);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = target_checksum_memory(bank->target, offset + bank->base, count, &target_crc);
+	if (retval != ERROR_OK)
+		return retval;
+
+	LOG_DEBUG("addr " TARGET_ADDR_FMT ", len 0x%08" PRIx32 ", crc 0x%08" PRIx32 " 0x%08" PRIx32,
+		offset + bank->base, count, ~image_crc, ~target_crc);
+	if (target_crc == image_crc)
+		return ERROR_OK;
+	else
+		return ERROR_FAIL;
+}
+
 void flash_bank_add(struct flash_bank *bank)
 {
 	/* put flash bank in linked list */
@@ -697,8 +734,8 @@ static bool flash_write_check_gap(struct flash_bank *bank,
 }
 
 
-int flash_write_unlock(struct target *target, struct image *image,
-	uint32_t *written, bool erase, bool unlock)
+int flash_write_unlock_verify(struct target *target, struct image *image,
+	uint32_t *written, bool erase, bool unlock, bool write, bool verify)
 {
 	int retval = ERROR_OK;
 
@@ -932,8 +969,17 @@ int flash_write_unlock(struct target *target, struct image *image,
 		}
 
 		if (retval == ERROR_OK) {
-			/* write flash sectors */
-			retval = flash_driver_write(c, buffer, run_address - c->base, run_size);
+			if (write) {
+				/* write flash sectors */
+				retval = flash_driver_write(c, buffer, run_address - c->base, run_size);
+			}
+		}
+
+		if (retval == ERROR_OK) {
+			if (verify) {
+				/* verify flash sectors */
+				retval = flash_driver_verify(c, buffer, run_address - c->base, run_size);
+			}
 		}
 
 		free(buffer);
@@ -957,7 +1003,7 @@ done:
 int flash_write(struct target *target, struct image *image,
 	uint32_t *written, bool erase)
 {
-	return flash_write_unlock(target, image, written, erase, false);
+	return flash_write_unlock_verify(target, image, written, erase, false, true, false);
 }
 
 struct flash_sector *alloc_block_array(uint32_t offset, uint32_t size,
diff --git a/src/flash/nor/core.h b/src/flash/nor/core.h
index 163e578..107a1c5 100644
--- a/src/flash/nor/core.h
+++ b/src/flash/nor/core.h
@@ -200,6 +200,7 @@ void default_flash_free_driver_priv(struct flash_bank *bank);
 
 /** Deallocates all flash banks */
 void flash_free_all_banks(void);
+
 /**
  * Provides default read implementation for flash memory.
  * @param bank The bank to read.
@@ -210,6 +211,18 @@ void flash_free_all_banks(void);
  */
 int default_flash_read(struct flash_bank *bank,
 		uint8_t *buffer, uint32_t offset, uint32_t count);
+
+/**
+ * Provides default verify implementation for flash memory.
+ * @param bank The bank to verify.
+ * @param buffer The data bytes to verify.
+ * @param offset The offset into the chip to verify.
+ * @param count The number of bytes to verify.
+ * @returns ERROR_OK if successful; otherwise, an error code.
+ */
+int default_flash_verify(struct flash_bank *bank,
+		const uint8_t *buffer, uint32_t offset, uint32_t count);
+
 /**
  * Provides default erased-bank check handling. Checks to see if
  * the flash driver knows they are erased; if things look uncertain,
@@ -217,7 +230,6 @@ int default_flash_read(struct flash_bank *bank,
  * @returns ERROR_OK if successful; otherwise, an error code.
  */
 int default_flash_blank_check(struct flash_bank *bank);
-
 /**
  * Returns the flash bank specified by @a name, which matches the
  * driver name and a suffix (option) specify the driver-specific
diff --git a/src/flash/nor/driver.h b/src/flash/nor/driver.h
index 7f66047..e29d4f5 100644
--- a/src/flash/nor/driver.h
+++ b/src/flash/nor/driver.h
@@ -156,6 +156,20 @@ struct flash_driver {
 			uint8_t *buffer, uint32_t offset, uint32_t count);
 
 	/**
+	 * Verify data in flash.  Note CPU address will be
+	 * "bank->base + offset", while the physical address is
+	 * dependent upon current target MMU mappings.
+	 *
+	 * @param bank The bank to verify
+	 * @param buffer The data bytes to verify against.
+	 * @param offset The offset into the chip to verify.
+	 * @param count The number of bytes to verify.
+	 * @returns ERROR_OK if successful; otherwise, an error code.
+	 */
+	int (*verify)(struct flash_bank *bank,
+			const uint8_t *buffer, uint32_t offset, uint32_t count);
+
+	/**
 	 * Probe to determine what kind of flash is present.
 	 * This is invoked by the "probe" script command.
 	 *
diff --git a/src/flash/nor/drivers.c b/src/flash/nor/drivers.c
index d52e072..570861e 100644
--- a/src/flash/nor/drivers.c
+++ b/src/flash/nor/drivers.c
@@ -75,6 +75,7 @@ extern const struct flash_driver stm32f2x_flash;
 extern const struct flash_driver stm32lx_flash;
 extern const struct flash_driver stm32l4x_flash;
 extern const struct flash_driver stm32h7x_flash;
+extern const struct flash_driver stmqspi_flash;
 extern const struct flash_driver stmsmi_flash;
 extern const struct flash_driver str7x_flash;
 extern const struct flash_driver str9x_flash;
@@ -148,6 +149,7 @@ static const struct flash_driver * const flash_drivers[] = {
 	&stm32l4x_flash,
 	&stm32h7x_flash,
 	&stmsmi_flash,
+	&stmqspi_flash,
 	&str7x_flash,
 	&str9x_flash,
 	&str9xpec_flash,
diff --git a/src/flash/nor/imp.h b/src/flash/nor/imp.h
index 06fb2a2..f66cf03 100644
--- a/src/flash/nor/imp.h
+++ b/src/flash/nor/imp.h
@@ -42,12 +42,14 @@ int flash_driver_erase(struct flash_bank *bank, unsigned int first,
 int flash_driver_protect(struct flash_bank *bank, int set, unsigned int first,
 		unsigned int last);
 int flash_driver_write(struct flash_bank *bank,
-		uint8_t *buffer, uint32_t offset, uint32_t count);
+		const uint8_t *buffer, uint32_t offset, uint32_t count);
 int flash_driver_read(struct flash_bank *bank,
 		uint8_t *buffer, uint32_t offset, uint32_t count);
+int flash_driver_verify(struct flash_bank *bank,
+		const uint8_t *buffer, uint32_t offset, uint32_t count);
 
 /* write (optional verify) an image to flash memory of the given target */
-int flash_write_unlock(struct target *target, struct image *image,
-		uint32_t *written, bool erase, bool unlock);
+int flash_write_unlock_verify(struct target *target, struct image *image,
+		uint32_t *written, bool erase, bool unlock, bool write, bool verify);
 
 #endif /* OPENOCD_FLASH_NOR_IMP_H */
diff --git a/src/flash/nor/sfdp.c b/src/flash/nor/sfdp.c
new file mode 100644
index 0000000..02b4ced
--- /dev/null
+++ b/src/flash/nor/sfdp.c
@@ -0,0 +1,263 @@
+/***************************************************************************
+ *   Copyright (C) 2019 by Andreas Bolsch <andreas.bolsch@mni.thm.de	   *
+ *   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	   *
+ *   (at your option) 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.						   *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License	   *
+ *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
+ ***************************************************************************/
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "imp.h"
+#include "spi.h"
+#include "sfdp.h"
+
+#define SFDP_MAGIC			0x50444653
+#define SFDP_ACCESS_PROT	0xFF
+#define SFDP_BASIC_FLASH	0xFF00
+#define SFDP_4BYTE_ADDR		0xFF84
+
+static const char *sfdp_name = "sfdp";
+
+struct sfdp_hdr {
+	uint32_t			signature;
+	uint32_t			revision;
+};
+
+struct sfdp_phdr {
+	uint32_t			revision;
+	uint32_t			ptr;
+};
+
+struct sfdp_basic_flash_param {
+	uint32_t			fast_addr;		/* 01: fast read and 3/4 address bytes */
+	uint32_t			density;		/* 02: memory density */
+	uint32_t			fast_1x4;		/* 03: 1-1-4 and 1-4-4 fast read */
+	uint32_t			fast_1x2;		/* 04: 1-2-2 and 1-1-2 fast read */
+	uint32_t			fast_444;		/* 05: 4-4-4 and 2-2-2 fast read */
+	uint32_t			read_222;		/* 06: 2-2-2 fast read instr and dummy */
+	uint32_t			read_444;		/* 07: 4-4-4 fast read instr and dummy */
+	uint32_t			erase_t12;		/* 08: erase types 1, 2 */
+	uint32_t			erase_t34;		/* 09: erase types 3, 4 */
+	uint32_t			erase_time;		/* 10: erase times for types 1 - 4 */
+	uint32_t			chip_byte;		/* 11: chip erase time, byte prog time, page prog */
+	uint32_t			susp_time;		/* 12: suspend and resume times */
+	uint32_t			susp_instr;		/* 13: suspend and resume instr */
+	uint32_t			pwrd_instr;		/* 14: powerdown instr */
+	uint32_t			quad_req;		/* 15: quad enable requirements */
+	uint32_t			addr_reset;		/* 16: 3-/4-byte addressing and reset */
+	uint32_t			read_1x8;		/* 17: 1-1-8 and 1-8-8 fast read instr and dummy */
+	uint32_t			dtr_drive;		/* 18: dtr modes and drive strength */
+	uint32_t			octal_req;		/* 19: octal enable requirements */
+	uint32_t			speed_888;		/* 20: speed in 8-8-8 modes */
+};
+
+struct sfdp_4byte_addr_param {
+	uint32_t			flags;			/* 01: various flags */
+	uint32_t			erase_t1234;	/* 02: erase commands */
+};
+
+/* Try to get parameters from flash via SFDP */
+int spi_sfdp(struct flash_bank *bank, struct flash_device *dev,
+	read_sfdp_block_t read_sfdp_block)
+{
+	struct sfdp_hdr header;
+	struct sfdp_phdr *pheaders = NULL;
+	uint32_t *ptable = NULL;
+	unsigned int j, k, nph;
+	int retval, erase_type = 0;
+
+	memset(dev, 0, sizeof(struct flash_device));
+
+	/* retrieve SFDP header */
+	memset(&header, 0, sizeof(header));
+	retval = read_sfdp_block(bank, 0x0, sizeof(header) >> 2, (uint32_t *) &header);
+	if (retval != ERROR_OK)
+		return retval;
+	LOG_DEBUG("header 0x%08" PRIx32 " 0x%08" PRIx32, header.signature, header.revision);
+	if (header.signature != SFDP_MAGIC) {
+		LOG_INFO("no SDFP found");
+		return ERROR_FLASH_BANK_NOT_PROBED;
+	}
+	if (((header.revision >> 24) & 0xFF) != SFDP_ACCESS_PROT) {
+		LOG_ERROR("access protocol 0x%02" PRIx8 " not implemented",
+			(header.revision >> 24) & 0xFF);
+		return ERROR_FLASH_BANK_NOT_PROBED;
+	}
+
+	/* retrieve table of parameter headers */
+	nph = ((header.revision >> 16) & 0xFF) + 1;
+	LOG_DEBUG("parameter headers: %d", nph);
+	pheaders = malloc(sizeof(struct sfdp_phdr) * nph);
+	if (pheaders == NULL) {
+		LOG_ERROR("not enough memory");
+		return ERROR_FAIL;
+	}
+	memset(pheaders, 0, sizeof(struct sfdp_phdr) * nph);
+	retval = read_sfdp_block(bank, sizeof(header),
+		(sizeof(struct sfdp_phdr) >> 2) * nph, (uint32_t *) pheaders);
+	if (retval != ERROR_OK)
+		goto err;
+
+	for (k = 0; k < nph; k++) {
+		uint8_t words = (pheaders[k].revision >> 24) & 0xFF;
+		uint16_t id = (((pheaders[k].ptr) >> 16) & 0xFF00) | (pheaders[k].revision & 0xFF);
+		uint32_t ptr = pheaders[k].ptr & 0xFFFFFF;
+
+		LOG_DEBUG("pheader %d len=0x%02" PRIx8 " id=0x%04" PRIx16
+			" ptr=0x%06" PRIx32, k, words, id, ptr);
+
+		/* retrieve parameter table */
+		ptable = malloc(words << 2);
+		if (ptable == NULL) {
+			LOG_ERROR("not enough memory");
+			retval = ERROR_FAIL;
+			goto err;
+		}
+		retval = read_sfdp_block(bank, ptr, words, ptable);
+		if (retval != ERROR_OK)
+			goto err;
+
+		for (j = 0; j < words; j++)
+			LOG_DEBUG("word %02d 0x%08X", j + 1, ptable[j]);
+
+		if (id == SFDP_BASIC_FLASH) {
+			struct sfdp_basic_flash_param *table = (struct sfdp_basic_flash_param *) ptable;
+			uint16_t erase;
+
+			if (words < 9) {
+				LOG_ERROR("id=0x%04" PRIx16 " invalid length %d", id, words);
+				retval = ERROR_FLASH_BANK_NOT_PROBED;
+				goto err;
+			}
+
+			LOG_DEBUG("basic flash parameter table");
+			/* dummy device name */
+			dev->name = sfdp_name;
+
+			/* default instructions */
+			dev->read_cmd = SPIFLASH_READ;
+			dev->pprog_cmd = SPIFLASH_PAGE_PROGRAM;
+			dev->chip_erase_cmd = SPIFLASH_MASS_ERASE;
+
+			/* get device size */
+			if (table->density & (1UL << 31))
+				dev->size_in_bytes = 1UL << ((table->density & ~(1UL << 31)) - 3);
+			else
+				dev->size_in_bytes = (table->density + 1) >> 3;
+
+			/* 2-2-2 read instruction, not used */
+			if (table->fast_444 & (1UL << 0))
+				dev->qread_cmd = (table->read_222 >> 24) & 0xFF;
+
+			/* 4-4-4 read instruction */
+			if (table->fast_444 & (1UL << 4))
+				dev->qread_cmd = (table->read_444 >> 24) & 0xFF;
+
+			/* find the largest erase block size and instruction */
+			erase = (table->erase_t12 >> 0) & 0xFFFF;
+			erase_type = 1;
+			if (((table->erase_t12 >> 16) & 0xFF) > (erase & 0xFF)) {
+				erase = (table->erase_t12 >> 16) & 0xFFFF;
+				erase_type = 2;
+			}
+			if (((table->erase_t34 >> 0) & 0xFF) > (erase & 0xFF)) {
+				erase = (table->erase_t34 >> 0) & 0xFFFF;
+				erase_type = 3;
+			}
+			if (((table->erase_t34 >> 16) & 0xFF) > (erase & 0xFF)) {
+				erase = (table->erase_t34 >> 16) & 0xFFFF;
+				erase_type = 4;
+			}
+			dev->erase_cmd = (erase >> 8) & 0xFF;
+			dev->sectorsize = 1UL << (erase & 0xFF);
+
+			if ((offsetof(struct sfdp_basic_flash_param, chip_byte) >> 2) < words) {
+				/* get Program Page Size, if chip_byte present, that's optional */
+				dev->pagesize = 1UL << ((table->chip_byte >> 4) & 0x0F);
+			} else {
+				/* no explicit page size specified ... */
+				if (table->fast_addr & (1UL << 2)) {
+					/* Write Granularity = 1, use 64 bytes */
+					dev->pagesize = 1UL << 6;
+				} else {
+					/* Write Granularity = 0, use 16 bytes */
+					dev->pagesize = 1UL << 4;
+				}
+			}
+
+			if (dev->size_in_bytes > (1UL << 24)) {
+				if (((table->fast_addr >> 17) & 0x3) == 0x0)
+					LOG_ERROR("device needs paging - not implemented");
+
+				/* 4-byte addresses needed if more than 16 MBytes */
+				if (((offsetof(struct sfdp_basic_flash_param, addr_reset) >> 2) < words) &&
+					(table->addr_reset & (1UL << 29))) {
+					/* dedicated 4-byte-address instructions, hopefully these ...
+					 * this entry is unfortunately optional as well
+					 * a subsequent 4-byte address table may overwrite this */
+					dev->read_cmd = 0x13;
+					dev->pprog_cmd = 0x12;
+					dev->erase_cmd = 0xDC;
+					if (dev->qread_cmd != 0)
+						dev->qread_cmd = 0xEC;
+				} else if (((table->fast_addr >> 17) & 0x3) == 0x1)
+					LOG_INFO("device has to be switched to 4-byte addresses");
+			}
+		} else if (id == SFDP_4BYTE_ADDR) {
+			struct sfdp_4byte_addr_param *table = (struct sfdp_4byte_addr_param *) ptable;
+
+			if (words >= (offsetof(struct sfdp_4byte_addr_param, erase_t1234)
+				+ sizeof(table->erase_t1234)) >> 2) {
+				LOG_INFO("4-byte address parameter table");
+
+				/* read and page program instructions */
+				if (table->flags & (1UL << 0))
+					dev->read_cmd = 0x13;
+				if (table->flags & (1UL << 5))
+					dev->qread_cmd = 0xEC;
+				if (table->flags & (1UL << 6))
+					dev->pprog_cmd = 0x12;
+
+				/* erase instructions */
+				if ((erase_type == 1) && (table->flags & (1UL << 9)))
+					dev->erase_cmd = (table->erase_t1234 >> 0) & 0xFF;
+				else if ((erase_type == 2) && (table->flags & (1UL << 10)))
+					dev->erase_cmd = (table->erase_t1234 >> 8) & 0xFF;
+				else if ((erase_type == 3) && (table->flags & (1UL << 11)))
+					dev->erase_cmd = (table->erase_t1234 >> 16) & 0xFF;
+				else if ((erase_type == 4) && (table->flags & (1UL << 12)))
+					dev->erase_cmd = (table->erase_t1234 >> 24) & 0xFF;
+			} else
+				LOG_ERROR("parameter table id=0x%04" PRIx16 " invalid length %d", id, words);
+		} else
+			LOG_DEBUG("unimplemented parameter table id=0x%04" PRIx16, id);
+
+		free(ptable);
+		ptable = NULL;
+	}
+
+	if (erase_type != 0) {
+		LOG_INFO("valid SFDP detected");
+		retval = ERROR_OK;
+	} else {
+		LOG_ERROR("incomplete/invalid SFDP");
+		retval = ERROR_FLASH_BANK_NOT_PROBED;
+	}
+
+err:
+	free(pheaders);
+	free(ptable);
+
+	return retval;
+}
diff --git a/src/flash/nor/sfdp.h b/src/flash/nor/sfdp.h
new file mode 100644
index 0000000..f924a4e
--- /dev/null
+++ b/src/flash/nor/sfdp.h
@@ -0,0 +1,34 @@
+/***************************************************************************
+ *   Copyright (C) 2019 by Andreas Bolsch <andreas.bolsch@mni.thm.de	   *
+ *   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	   *
+ *   (at your option) 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.						   *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License	   *
+ *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
+ ***************************************************************************/
+
+#ifndef OPENOCD_FLASH_NOR_SFDP_H
+#define OPENOCD_FLASH_NOR_SFDP_H
+
+/* per JESD216D 'addr' is *byte* based but must be word aligned,
+ * 'buffer' is word based, word aligned and always little-endian encoded,
+ * in the flash, 'addr_len' is 3 or 4, 'dummy' ***usually*** 8
+ *
+ * the actual number of dummy clocks should be worked out by this function
+ * dynamically, i.e. by scanning the first few bytes for the SFDP signature
+ *
+ * buffer contents is supposed to be returned in ***host*** endianness */
+typedef int (*read_sfdp_block_t)(struct flash_bank *bank, uint32_t addr,
+	uint32_t words, uint32_t *buffer);
+
+extern int spi_sfdp(struct flash_bank *bank, struct flash_device *dev,
+	read_sfdp_block_t read_sfdp_block);
+
+#endif /* OPENOCD_FLASH_NOR_SFDP_H */
diff --git a/src/flash/nor/spi.h b/src/flash/nor/spi.h
index 11c381f..f8a0a65 100644
--- a/src/flash/nor/spi.h
+++ b/src/flash/nor/spi.h
@@ -1,5 +1,5 @@
 /***************************************************************************
- *   Copyright (C) 2018 by Andreas Bolsch                                  *
+ *   Copyright (C) 2018-2019 by Andreas Bolsch                             *
  *   andreas.bolsch@mni.thm.de                                             *
  *                                                                         *
  *   Copyright (C) 2012 by George Harris                                   *
@@ -29,7 +29,7 @@
 
 /* data structure to maintain flash ids from different vendors */
 struct flash_device {
-	char *name;
+	const char *name;
 	uint8_t read_cmd;
 	uint8_t qread_cmd;
 	uint8_t pprog_cmd;
@@ -87,6 +87,8 @@ extern const struct flash_device flash_devices[];
 #define SPIFLASH_PAGE_PROGRAM	0x02 /* Page Program */
 #define SPIFLASH_FAST_READ		0x0B /* Fast Read */
 #define SPIFLASH_READ			0x03 /* Normal Read */
+#define SPIFLASH_MASS_ERASE		0xC7 /* Mass Erase */
+#define SPIFLASH_READ_SFDP		0x5A /* Read Serial Flash Discoverable Parameters */
 
 #define SPIFLASH_DEF_PAGESIZE	256  /* default for non-page-oriented devices (FRAMs) */
 
diff --git a/src/flash/nor/stmqspi.c b/src/flash/nor/stmqspi.c
new file mode 100644
index 0000000..486ee53
--- /dev/null
+++ b/src/flash/nor/stmqspi.c
@@ -0,0 +1,2452 @@
+/***************************************************************************
+ *   Copyright (C) 2016 - 2019 by Andreas Bolsch                           *
+ *   andreas.bolsch@mni.thm.de                                             *
+ *                                                                         *
+ *   Copyright (C) 2010 by Antonio Borneo                                  *
+ *   borneo.antonio@gmail.com                                              *
+ *                                                                         *
+ *   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	   *
+ *   (at your option) 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.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
+ ***************************************************************************/
+
+/* STM QuadSPI (QSPI) and OctoSPI (OCTOSPI) controller are SPI bus controllers
+ * specifically designed for SPI memories.
+ * Two working modes are available:
+ * - indirect mode: the SPI is controlled by SW. Any custom commands can be sent
+ *   on the bus.
+ * - memory mapped mode: the SPI is under QSPI/OCTOSPI control. Memory content
+ *   is directly accessible in CPU memory space. CPU can read and execute from
+ *   memory (but not write to) */
+
+/* ATTENTION:
+ * To have flash mapped in CPU memory space, the QSPI/OCTOSPI controller
+ * has to be in "memory mapped mode". This requires following constraints:
+ * 1) The command "reset init" has to initialize QSPI/OCTOSPI controller and put
+ *    it in memory mapped mode;
+ * 2) every command in this file has to return to prompt in memory mapped mode. */
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "imp.h"
+#include <helper/time_support.h>
+#include <target/algorithm.h>
+#include <target/armv7m.h>
+#include <target/image.h>
+#include "stmqspi.h"
+#include "sfdp.h"
+
+/* deprecated */
+#undef SPIFLASH_READ
+#undef SPIFLASH_PAGE_PROGRAM
+
+#define READ_REG(a)												\
+({																\
+	uint32_t _result;											\
+																\
+	retval = target_read_u32(target, io_base + (a), &_result);	\
+	(retval == ERROR_OK) ? _result : 0x0;						\
+})
+
+/* saved mode settings */
+#define QSPI_MODE (stmqspi_info->saved_ccr & \
+	(0xF0000000U | QSPI_DCYC_MASK | QSPI_4LINE_MODE | QSPI_ALTB_MODE | QSPI_ADDR4))
+
+/* saved read mode settings but indirect read instead of memory mapped
+ * in particular, use the dummy cycle setting from this saved setting */
+#define	QSPI_CCR_READ (QSPI_READ_MODE | (stmqspi_info->saved_ccr & \
+	(0xF0000000U | QSPI_DCYC_MASK | QSPI_4LINE_MODE | QSPI_ALTB_MODE | QSPI_ADDR4 | 0xFF)))
+
+/* QSPI_CCR for various other commands, these never use dummy cycles nor alternate bytes */
+#define	QSPI_CCR_READ_STATUS \
+	((QSPI_MODE & ~QSPI_DCYC_MASK & QSPI_NO_ADDR & QSPI_NO_ALTB) | \
+	(QSPI_READ_MODE | SPIFLASH_READ_STATUS))
+
+#define	QSPI_CCR_READ_ID \
+	((QSPI_MODE & ~QSPI_DCYC_MASK & QSPI_NO_ADDR & QSPI_NO_ALTB) | \
+	(QSPI_READ_MODE | SPIFLASH_READ_ID))
+
+#define	QSPI_CCR_READ_MID \
+	((QSPI_MODE & ~QSPI_DCYC_MASK & QSPI_NO_ADDR & QSPI_NO_ALTB) | \
+	(QSPI_READ_MODE | SPIFLASH_READ_MID))
+
+/* always use 3-byte addresses for read SFDP */
+#define	QSPI_CCR_READ_SFDP \
+	((QSPI_MODE & ~QSPI_DCYC_MASK & ~QSPI_ADDR4 & QSPI_NO_ALTB) | \
+	(QSPI_READ_MODE | QSPI_ADDR3 | SPIFLASH_READ_SFDP))
+
+#define QSPI_CCR_WRITE_ENABLE \
+	((QSPI_MODE & ~QSPI_DCYC_MASK & QSPI_NO_ADDR & QSPI_NO_ALTB & QSPI_NO_DATA) | \
+	(QSPI_WRITE_MODE | SPIFLASH_WRITE_ENABLE))
+
+#define QSPI_CCR_SECTOR_ERASE \
+	((QSPI_MODE & ~QSPI_DCYC_MASK & QSPI_NO_ALTB & QSPI_NO_DATA) | \
+	(QSPI_WRITE_MODE | stmqspi_info->dev.erase_cmd))
+
+#define QSPI_CCR_MASS_ERASE \
+	((QSPI_MODE & ~QSPI_DCYC_MASK & QSPI_NO_ADDR & QSPI_NO_ALTB & QSPI_NO_DATA) | \
+	(QSPI_WRITE_MODE | stmqspi_info->dev.chip_erase_cmd))
+
+#define QSPI_CCR_PAGE_PROG \
+	((QSPI_MODE & ~QSPI_DCYC_MASK & QSPI_NO_ALTB) | \
+	(QSPI_WRITE_MODE | stmqspi_info->dev.pprog_cmd))
+
+/* saved mode settings */
+#define OCTOSPI_MODE (stmqspi_info->saved_cr & 0xCFFFFFFF)
+
+#define OPI_MODE ((stmqspi_info->saved_ccr & OCTOSPI_ISIZE_MASK) != 0)
+
+#define OCTOSPI_MODE_CCR (stmqspi_info->saved_ccr & \
+	(0xF0000000U | OCTOSPI_8LINE_MODE | OCTOSPI_ALTB_MODE | OCTOSPI_ADDR4))
+
+/* use saved ccr for read */
+#define OCTOSPI_CCR_READ OCTOSPI_MODE_CCR
+
+/* OCTOSPI_CCR for various other commands, these never use alternate bytes	*
+ * for READ_STATUS and READ_ID, 4-byte address 0							*
+ * 4 dummy cycles must sent in OPI mode when DQS is disabled. However, when	*
+ * DQS is enabled, some STM32 devices need at least 6 dummy cycles for		*
+ * proper operation, but otherwise the actual number has no effect!			*
+ * E.g. RM0432 Rev. 7 is incorrect regarding this: L4R9 works well with 4	*
+ * dummy clocks whereas L4P5 not at all.									*
+ */
+#define OPI_DUMMY \
+	((stmqspi_info->saved_ccr & OCTOSPI_DQSEN) ? 6U : 4U)
+
+#define	OCTOSPI_CCR_READ_STATUS \
+	((OCTOSPI_MODE_CCR & OCTOSPI_NO_DDTR & \
+	(OPI_MODE ? ~0U : OCTOSPI_NO_ADDR) & OCTOSPI_NO_ALTB))
+
+#define	OCTOSPI_CCR_READ_ID \
+	((OCTOSPI_MODE_CCR & OCTOSPI_NO_DDTR & \
+	(OPI_MODE ? ~0U : OCTOSPI_NO_ADDR) & OCTOSPI_NO_ALTB))
+
+#define	OCTOSPI_CCR_READ_MID OCTOSPI_CCR_READ_ID
+
+/* 4-byte address in octo mode, else 3-byte address for read SFDP */
+#define	OCTOSPI_CCR_READ_SFDP(len) \
+	((OCTOSPI_MODE_CCR & OCTOSPI_NO_DDTR & ~OCTOSPI_ADDR4 & OCTOSPI_NO_ALTB) | \
+	((len < 4) ? OCTOSPI_ADDR3 : OCTOSPI_ADDR4))
+
+#define OCTOSPI_CCR_WRITE_ENABLE \
+	((OCTOSPI_MODE_CCR & OCTOSPI_NO_ADDR & OCTOSPI_NO_ALTB & OCTOSPI_NO_DATA))
+
+#define OCTOSPI_CCR_SECTOR_ERASE \
+	((OCTOSPI_MODE_CCR & OCTOSPI_NO_ALTB & OCTOSPI_NO_DATA))
+
+#define OCTOSPI_CCR_MASS_ERASE \
+	((OCTOSPI_MODE_CCR & OCTOSPI_NO_ADDR & OCTOSPI_NO_ALTB & OCTOSPI_NO_DATA))
+
+#define OCTOSPI_CCR_PAGE_PROG \
+	((OCTOSPI_MODE_CCR & QSPI_NO_ALTB))
+
+#define SPI_ADSIZE (((stmqspi_info->saved_ccr >> SPI_ADSIZE_POS) & 0x3) + 1)
+
+#define OPI_CMD(cmd) ((OPI_MODE ? ((((uint16_t) cmd)<<8) | (~cmd & 0xFFU)) : cmd))
+
+#define OCTOSPI_CMD(mode, ccr, ir)										\
+({																		\
+	retval = target_write_u32(target, io_base + OCTOSPI_CR,				\
+		OCTOSPI_MODE | mode);											\
+	if (retval == ERROR_OK)												\
+		retval = target_write_u32(target, io_base + OCTOSPI_TCR,		\
+			(stmqspi_info->saved_tcr & ~OCTOSPI_DCYC_MASK) |			\
+			((OPI_MODE && (mode == OCTOSPI_READ_MODE)) ?				\
+			(OPI_DUMMY<<OCTOSPI_DCYC_POS) : 0));						\
+	if (retval == ERROR_OK)												\
+		retval = target_write_u32(target, io_base + OCTOSPI_CCR, ccr);	\
+	if (retval == ERROR_OK)												\
+		retval = target_write_u32(target, io_base + OCTOSPI_IR,			\
+			OPI_CMD(ir));												\
+	retval;																\
+})
+
+/* convert uint32_t into 4 uint8_t in little endian byte order */
+static inline uint32_t h_to_le_32(uint32_t val)
+{
+	uint32_t result;
+
+	h_u32_to_le((uint8_t *) &result, val);
+	return result;
+}
+
+/* Timeout in ms */
+#define SPI_CMD_TIMEOUT			(100)
+#define SPI_PROBE_TIMEOUT		(100)
+#define SPI_MAX_TIMEOUT			(2000)
+#define SPI_MASS_ERASE_TIMEOUT	(400000)
+
+struct sector_info {
+	uint32_t offset;
+	uint32_t size;
+	uint32_t result;
+};
+
+struct stmqspi_flash_bank {
+	bool probed;
+	char devname[32];
+	bool octo;
+	struct flash_device dev;
+	uint32_t io_base;
+	uint32_t saved_cr;	/* in particalar FSEL, DFM bit mask in QUADSPI_CR *AND* OCTOSPI_CR */
+	uint32_t saved_ccr; /* different meaning for QUADSPI and OCTOSPI */
+	uint32_t saved_tcr;	/* only for OCTOSPI */
+	uint32_t saved_ir;	/* only for OCTOSPI */
+	unsigned int sfdp_dummy1;	/* number of dummy bytes for SFDP read for flash1 and octo */
+	unsigned int sfdp_dummy2;	/* number of dummy bytes for SFDP read for flash2 */
+};
+
+FLASH_BANK_COMMAND_HANDLER(stmqspi_flash_bank_command)
+{
+	struct stmqspi_flash_bank *stmqspi_info;
+	uint32_t io_base;
+
+	LOG_DEBUG("%s", __func__);
+
+	if (CMD_ARGC < 7)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[6], io_base);
+
+	stmqspi_info = malloc(sizeof(struct stmqspi_flash_bank));
+	if (stmqspi_info == NULL) {
+		LOG_ERROR("not enough memory");
+		return ERROR_FAIL;
+	}
+
+	bank->driver_priv = stmqspi_info;
+	stmqspi_info->sfdp_dummy1 = 0;
+	stmqspi_info->sfdp_dummy2 = 0;
+	stmqspi_info->probed = false;
+	stmqspi_info->io_base = io_base;
+
+	return ERROR_OK;
+}
+
+/* Poll busy flag */
+/* timeout in ms */
+static int poll_busy(struct flash_bank *bank, int timeout)
+{
+	struct target *target = bank->target;
+	struct stmqspi_flash_bank *stmqspi_info = bank->driver_priv;
+	uint32_t io_base = stmqspi_info->io_base;
+	uint32_t spi_sr;
+	int retval;
+	long long endtime;
+
+	endtime = timeval_ms() + timeout;
+	do {
+		spi_sr = READ_REG(SPI_SR);
+		if ((spi_sr & (1U<<SPI_BUSY)) == 0) {
+			if (retval == ERROR_OK) {
+				/* Clear transmit finished flag */
+				retval = target_write_u32(target, io_base + SPI_FCR, (1U<<SPI_TCF));
+			}
+			return retval;
+		} else
+			LOG_DEBUG("busy: 0x%08X", spi_sr);
+		alive_sleep(1);
+	} while (timeval_ms() < endtime);
+
+	LOG_ERROR("Timeout while polling BUSY");
+	return ERROR_FLASH_OPERATION_FAILED;
+}
+
+/* Set to memory-mapped mode, e.g. after an error */
+static int set_mm_mode(struct flash_bank *bank)
+{
+	struct target *target = bank->target;
+	struct stmqspi_flash_bank *stmqspi_info = bank->driver_priv;
+	uint32_t io_base = stmqspi_info->io_base;
+	int retval;
+
+	/* Reset Address register bits 0 and 1, see various errata sheets */
+	retval = target_write_u32(target, io_base + SPI_AR, 0x0);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Abort any previous operation */
+	retval = target_write_u32(target, io_base + SPI_CR,
+		READ_REG(SPI_CR) | (1U<<SPI_ABORT));
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Wait for busy to be cleared */
+	retval = poll_busy(bank, SPI_PROBE_TIMEOUT);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Finally switch to memory mapped mode */
+	if (IS_OCTOSPI) {
+		retval = target_write_u32(target, io_base + OCTOSPI_CR,
+			OCTOSPI_MODE | OCTOSPI_MM_MODE);
+		if (retval == ERROR_OK)
+			retval = target_write_u32(target, io_base + OCTOSPI_CCR,
+				stmqspi_info->saved_ccr);
+		if (retval == ERROR_OK)
+			retval = target_write_u32(target, io_base + OCTOSPI_TCR,
+				stmqspi_info->saved_tcr);
+		if (retval == ERROR_OK)
+			retval = target_write_u32(target, io_base + OCTOSPI_IR,
+				stmqspi_info->saved_ir);
+	} else {
+		retval = target_write_u32(target, io_base + QSPI_CR,
+			stmqspi_info->saved_cr);
+		if (retval == ERROR_OK)
+			retval = target_write_u32(target, io_base + QSPI_CCR,
+				stmqspi_info->saved_ccr);
+	}
+	return retval;
+}
+
+/* Read the status register of the external SPI flash chip(s). */
+static int read_status_reg(struct flash_bank *bank, uint16_t *status)
+{
+	struct target *target = bank->target;
+	struct stmqspi_flash_bank *stmqspi_info = bank->driver_priv;
+	uint32_t io_base = stmqspi_info->io_base;
+	uint8_t data;
+	int count, retval;
+
+	/* Abort any previous operation */
+	retval = target_write_u32(target, io_base + SPI_CR,
+		READ_REG(SPI_CR) | (1U<<SPI_ABORT));
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Wait for busy to be cleared */
+	retval = poll_busy(bank, SPI_PROBE_TIMEOUT);
+	if (retval != ERROR_OK)
+		goto err;
+
+	/* Read always two (for DTR mode) bytes per chip */
+	count = 2;
+	retval = target_write_u32(target, io_base + SPI_DLR,
+		((stmqspi_info->saved_cr & (1U<<SPI_DUAL_FLASH)) ? 2 * count : count) - 1);
+	if (retval != ERROR_OK)
+		goto err;
+
+	/* Read status */
+	if (IS_OCTOSPI) {
+		retval = OCTOSPI_CMD(OCTOSPI_READ_MODE, OCTOSPI_CCR_READ_STATUS, SPIFLASH_READ_STATUS);
+		if (OPI_MODE) {
+			/* Dummy address 0, only required for 8-line mode */
+			retval = target_write_u32(target, io_base + SPI_AR, 0);
+			if (retval != ERROR_OK)
+				goto err;
+		}
+	} else
+		retval = target_write_u32(target, io_base + QSPI_CCR, QSPI_CCR_READ_STATUS);
+	if (retval != ERROR_OK)
+		goto err;
+
+	*status = 0;
+
+	/* for debugging only */
+	(void) READ_REG(SPI_SR);
+
+	for ( ; count > 0; --count) {
+		if ((stmqspi_info->saved_cr & ((1U<<SPI_DUAL_FLASH) | (1U<<SPI_FSEL_FLASH)))
+			!= (1U<<SPI_FSEL_FLASH)) {
+			/* get status of flash 1 in dual mode or flash 1 only mode */
+			retval = target_read_u8(target, io_base + SPI_DR, &data);
+			if (retval != ERROR_OK)
+				goto err;
+			*status |= data;
+		}
+
+		if ((stmqspi_info->saved_cr & ((1U<<SPI_DUAL_FLASH) | (1U<<SPI_FSEL_FLASH)))
+			!= (0U<<SPI_FSEL_FLASH)) {
+			/* get status of flash 2 in dual mode or flash 2 only mode */
+			retval = target_read_u8(target, io_base + SPI_DR, &data);
+			if (retval != ERROR_OK)
+				goto err;
+			*status |= ((uint16_t) data) << 8;
+		}
+	}
+
+	LOG_DEBUG("flash status regs: 0x%04" PRIx16, *status);
+
+err:
+	return retval;
+}
+
+/* check for WIP (write in progress) bit(s) in status register(s) */
+/* timeout in ms */
+static int wait_till_ready(struct flash_bank *bank, int timeout)
+{
+	uint16_t status;
+	int retval;
+	long long endtime;
+
+	endtime = timeval_ms() + timeout;
+	do {
+		/* Read flash status register(s) */
+		retval = read_status_reg(bank, &status);
+		if (retval != ERROR_OK)
+			return retval;
+
+		if ((status & ((SPIFLASH_BSY_BIT << 8) | SPIFLASH_BSY_BIT)) == 0)
+			return retval;
+		alive_sleep(25);
+	} while (timeval_ms() < endtime);
+
+	LOG_ERROR("timeout");
+	return ERROR_FLASH_OPERATION_FAILED;
+}
+
+/* Send "write enable" command to SPI flash chip(s). */
+static int qspi_write_enable(struct flash_bank *bank)
+{
+	struct target *target = bank->target;
+	struct stmqspi_flash_bank *stmqspi_info = bank->driver_priv;
+	uint32_t io_base = stmqspi_info->io_base;
+	uint16_t status;
+	int retval;
+
+	/* Abort any previous operation */
+	retval = target_write_u32(target, io_base + SPI_CR,
+		READ_REG(SPI_CR) | (1U<<SPI_ABORT));
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Wait for busy to be cleared */
+	retval = poll_busy(bank, SPI_PROBE_TIMEOUT);
+	if (retval != ERROR_OK)
+		goto err;
+
+	/* Send write enable command */
+	if (IS_OCTOSPI) {
+		retval = OCTOSPI_CMD(OCTOSPI_WRITE_MODE, OCTOSPI_CCR_WRITE_ENABLE, SPIFLASH_WRITE_ENABLE);
+		if (OPI_MODE) {
+			/* Dummy address 0, only required for 8-line mode */
+			retval = target_write_u32(target, io_base + SPI_AR, 0);
+			if (retval != ERROR_OK)
+				goto err;
+		}
+	 } else
+		retval = target_write_u32(target, io_base + QSPI_CCR, QSPI_CCR_WRITE_ENABLE);
+	if (retval != ERROR_OK)
+		goto err;
+
+
+	/* Wait for transmit of command completed */
+	poll_busy(bank, SPI_CMD_TIMEOUT);
+	if (retval != ERROR_OK)
+		goto err;
+
+	/* Read flash status register */
+	retval = read_status_reg(bank, &status);
+	if (retval != ERROR_OK)
+		goto err;
+
+	/* Check write enabled for flash 1 */
+	if (((stmqspi_info->saved_cr & ((1U<<SPI_DUAL_FLASH) | (1U<<SPI_FSEL_FLASH)))
+		!= (1U<<SPI_FSEL_FLASH)))
+		if ((status & (SPIFLASH_WE_BIT | SPIFLASH_BSY_BIT)) != SPIFLASH_WE_BIT) {
+			LOG_ERROR("Cannot write enable flash1. Status=0x%02" PRIx8,
+				status & 0xFF);
+			return ERROR_FLASH_OPERATION_FAILED;
+		}
+
+	/* Check write enabled for flash 2 */
+	status >>= 8;
+	if (((stmqspi_info->saved_cr & ((1U<<SPI_DUAL_FLASH) | (1U<<SPI_FSEL_FLASH)))
+		!= (0U<<SPI_FSEL_FLASH)))
+		if ((status & (SPIFLASH_WE_BIT | SPIFLASH_BSY_BIT)) != SPIFLASH_WE_BIT) {
+			LOG_ERROR("Cannot write enable flash2. Status=0x%02" PRIx8,
+				status & 0xFF);
+			return ERROR_FLASH_OPERATION_FAILED;
+		}
+
+err:
+	return retval;
+}
+
+COMMAND_HANDLER(stmqspi_handle_mass_erase_command)
+{
+	struct target *target = NULL;
+	struct flash_bank *bank;
+	struct stmqspi_flash_bank *stmqspi_info;
+	struct duration bench;
+	uint32_t io_base;
+	uint16_t status;
+	unsigned int sector;
+	int retval;
+
+	LOG_DEBUG("%s", __func__);
+
+	if (CMD_ARGC != 1)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
+	if (ERROR_OK != retval)
+		return retval;
+
+	stmqspi_info = bank->driver_priv;
+	target = bank->target;
+
+	if (target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if (!(stmqspi_info->probed)) {
+		LOG_ERROR("Flash bank not probed");
+		return ERROR_FLASH_BANK_NOT_PROBED;
+	}
+
+	if (stmqspi_info->dev.chip_erase_cmd == 0x00) {
+		LOG_ERROR("Mass erase not available for this device");
+		return ERROR_FLASH_OPER_UNSUPPORTED;
+	}
+
+	for (sector = 0; sector < bank->num_sectors; sector++) {
+		if (bank->sectors[sector].is_protected) {
+			LOG_ERROR("Flash sector %u protected", sector);
+			return ERROR_FLASH_PROTECTED;
+		}
+	}
+
+	io_base = stmqspi_info->io_base;
+	duration_start(&bench);
+
+	retval = qspi_write_enable(bank);
+	if (retval != ERROR_OK)
+		goto err;
+
+	/* Send Mass Erase command */
+	if (IS_OCTOSPI)
+		retval = OCTOSPI_CMD(OCTOSPI_WRITE_MODE, OCTOSPI_CCR_MASS_ERASE,
+			stmqspi_info->dev.chip_erase_cmd);
+	else
+		retval = target_write_u32(target, io_base + QSPI_CCR, QSPI_CCR_MASS_ERASE);
+	if (retval != ERROR_OK)
+		goto err;
+
+	/* Wait for transmit of command completed */
+	poll_busy(bank, SPI_CMD_TIMEOUT);
+	if (retval != ERROR_OK)
+		goto err;
+
+	/* Read flash status register(s) */
+	retval = read_status_reg(bank, &status);
+	if (retval != ERROR_OK)
+		goto err;
+
+	/* Check for command in progress for flash 1 */
+	if (((stmqspi_info->saved_cr & ((1U<<SPI_DUAL_FLASH) | (1U<<SPI_FSEL_FLASH)))
+		!= (1U<<SPI_FSEL_FLASH)) && ((status & SPIFLASH_BSY_BIT) == 0) &&
+		((status & SPIFLASH_WE_BIT) != 0)) {
+		LOG_ERROR("Mass erase command not accepted by flash1. Status=0x%02" PRIx8,
+			status & 0xFF);
+		retval = ERROR_FLASH_OPERATION_FAILED;
+		goto err;
+	}
+
+	/* Check for command in progress for flash 2 */
+	status >>= 8;
+	if (((stmqspi_info->saved_cr & ((1U<<SPI_DUAL_FLASH) | (1U<<SPI_FSEL_FLASH)))
+		!= (0U<<SPI_FSEL_FLASH)) && ((status & SPIFLASH_BSY_BIT) == 0) &&
+		((status & SPIFLASH_WE_BIT) != 0)) {
+		LOG_ERROR("Mass erase command not accepted by flash2. Status=0x%02" PRIx8,
+			status & 0xFF);
+		retval = ERROR_FLASH_OPERATION_FAILED;
+		goto err;
+	}
+
+	/* Poll WIP for end of self timed Sector Erase cycle */
+	retval = wait_till_ready(bank, SPI_MASS_ERASE_TIMEOUT);
+
+	duration_measure(&bench);
+	if (retval == ERROR_OK) {
+		/* set all sectors as erased */
+		for (sector = 0; sector < bank->num_sectors; sector++)
+			bank->sectors[sector].is_erased = 1;
+
+		command_print(CMD, "stmqspi mass erase completed in %fs (%0.3f KiB/s)",
+			duration_elapsed(&bench),
+			duration_kbps(&bench, bank->size));
+	} else {
+		command_print(CMD, "stmqspi mass erase not completed even after %fs",
+			duration_elapsed(&bench));
+	}
+
+err:
+	/* Switch to memory mapped mode before return to prompt */
+	set_mm_mode(bank);
+
+	return retval;
+}
+
+static int log2u(uint32_t word)
+{
+	int result;
+
+	for (result = 0; (unsigned int) result < sizeof(uint32_t) * CHAR_BIT; result++)
+		if (word == (1UL<<result))
+			return result;
+
+	return -1;
+}
+
+COMMAND_HANDLER(stmqspi_handle_set)
+{
+	struct flash_bank *bank = NULL;
+	struct target *target = NULL;
+	struct stmqspi_flash_bank *stmqspi_info = NULL;
+	struct flash_sector *sectors = NULL;
+	uint32_t io_base;
+	unsigned int index = 0, dual, fsize;
+	int retval;
+
+	LOG_DEBUG("%s", __func__);
+
+	dual = (stmqspi_info->saved_cr & (1U<<SPI_DUAL_FLASH)) ? 1 : 0;
+
+	/* chip_erase_cmd, sectorsize and erase_cmd are optional */
+	if ((CMD_ARGC < 7) || (CMD_ARGC > 10))
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	retval = CALL_COMMAND_HANDLER(flash_command_get_bank, index++, &bank);
+	if (ERROR_OK != retval)
+		return retval;
+
+	target = bank->target;
+	stmqspi_info = bank->driver_priv;
+
+	/* invalidate all old info */
+	if (stmqspi_info->probed)
+		free(bank->sectors);
+	bank->size = 0;
+	bank->num_sectors = 0;
+	bank->sectors = NULL;
+	stmqspi_info->sfdp_dummy1 = 0;
+	stmqspi_info->sfdp_dummy2 = 0;
+	stmqspi_info->probed = false;
+	memset(&stmqspi_info->dev, 0, sizeof(stmqspi_info->dev));
+	stmqspi_info->dev.name = "unknown";
+
+	strncpy(stmqspi_info->devname, CMD_ARGV[index++], sizeof(stmqspi_info->devname) - 1);
+	stmqspi_info->devname[sizeof(stmqspi_info->devname) - 1] = '\0';
+
+	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[index++], stmqspi_info->dev.size_in_bytes);
+	if (log2u(stmqspi_info->dev.size_in_bytes) < 8) {
+		command_print(CMD, "stmqspi: device size must be 2^n with n >= 8");
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[index++], stmqspi_info->dev.pagesize);
+	if (stmqspi_info->dev.pagesize > stmqspi_info->dev.size_in_bytes ||
+		(log2u(stmqspi_info->dev.pagesize) < 0)) {
+		command_print(CMD, "stmqspi: page size must be 2^n and <= device size");
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	COMMAND_PARSE_NUMBER(u8, CMD_ARGV[index++], stmqspi_info->dev.read_cmd);
+	if ((stmqspi_info->dev.read_cmd != 0x03) &&
+		(stmqspi_info->dev.read_cmd != 0x13)) {
+		command_print(CMD, "stmqspi: only 0x03/0x13 READ cmd allowed");
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	COMMAND_PARSE_NUMBER(u8, CMD_ARGV[index++], stmqspi_info->dev.qread_cmd);
+	if ((stmqspi_info->dev.qread_cmd != 0x00) &&
+		(stmqspi_info->dev.qread_cmd != 0x0B) &&
+		(stmqspi_info->dev.qread_cmd != 0x0C) &&
+		(stmqspi_info->dev.qread_cmd != 0x3B) &&
+		(stmqspi_info->dev.qread_cmd != 0x3C) &&
+		(stmqspi_info->dev.qread_cmd != 0x6B) &&
+		(stmqspi_info->dev.qread_cmd != 0x6C) &&
+		(stmqspi_info->dev.qread_cmd != 0xBB) &&
+		(stmqspi_info->dev.qread_cmd != 0xBC) &&
+		(stmqspi_info->dev.qread_cmd != 0xEB) &&
+		(stmqspi_info->dev.qread_cmd != 0xEC) &&
+		(stmqspi_info->dev.qread_cmd != 0xEE)) {
+		command_print(CMD, "stmqspi: only 0x0B/0x0C/0x3B/0x3C/"
+			"0x6B/0x6C/0xBB/0xBC/0xEB/0xEC/0xEE QREAD allowed");
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	COMMAND_PARSE_NUMBER(u8, CMD_ARGV[index++], stmqspi_info->dev.pprog_cmd);
+	if ((stmqspi_info->dev.pprog_cmd != 0x02) &&
+		(stmqspi_info->dev.pprog_cmd != 0x12) &&
+		(stmqspi_info->dev.pprog_cmd != 0x32)) {
+		command_print(CMD, "stmqspi: only 0x02/0x12/0x32 PPRG cmd allowed");
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	if (index < CMD_ARGC)
+		COMMAND_PARSE_NUMBER(u8, CMD_ARGV[index++], stmqspi_info->dev.chip_erase_cmd);
+	else
+		stmqspi_info->dev.chip_erase_cmd = 0x00;
+
+	if (index < CMD_ARGC) {
+		COMMAND_PARSE_NUMBER(u32, CMD_ARGV[index++], stmqspi_info->dev.sectorsize);
+		if ((stmqspi_info->dev.sectorsize > stmqspi_info->dev.size_in_bytes) ||
+			(stmqspi_info->dev.sectorsize < stmqspi_info->dev.pagesize) ||
+			(log2u(stmqspi_info->dev.sectorsize) < 0)) {
+			command_print(CMD, "stmqspi: sector size must be 2^n and <= device size");
+			return ERROR_COMMAND_SYNTAX_ERROR;
+		}
+
+		if (index < CMD_ARGC)
+			COMMAND_PARSE_NUMBER(u8, CMD_ARGV[index++], stmqspi_info->dev.erase_cmd);
+		else
+			return ERROR_COMMAND_SYNTAX_ERROR;
+	} else {
+		/* no sector size / sector erase cmd given, treat whole bank as a single sector */
+		stmqspi_info->dev.erase_cmd = 0x00;
+		stmqspi_info->dev.sectorsize = stmqspi_info->dev.size_in_bytes;
+	}
+
+	/* set correct size value */
+	bank->size = stmqspi_info->dev.size_in_bytes << dual;
+
+	io_base = stmqspi_info->io_base;
+	fsize = (READ_REG(SPI_DCR)>>SPI_FSIZE_POS) & ((1U<<SPI_FSIZE_LEN) - 1);
+	if (retval != ERROR_OK)
+		return retval;
+
+	LOG_DEBUG("FSIZE = 0x%04x", fsize);
+	if (bank->size == (1U<<(fsize + 1)))
+		LOG_DEBUG("FSIZE in DCR(1) matches actual capacity. Beware of silicon bug in H7, L4+, MP1.");
+	else if (bank->size == (1U<<(fsize + 0)))
+		LOG_DEBUG("FSIZE in DCR(1) is off by one regarding actual capacity. Fix for silicon bug?");
+	else
+		LOG_ERROR("FSIZE in DCR(1) doesn't match actual capacity.");
+
+	/* create and fill sectors array */
+	bank->num_sectors =
+		stmqspi_info->dev.size_in_bytes / stmqspi_info->dev.sectorsize;
+	sectors = malloc(sizeof(struct flash_sector) * bank->num_sectors);
+	if (sectors == NULL) {
+		LOG_ERROR("not enough memory");
+		return ERROR_FAIL;
+	}
+
+	for (unsigned int sector = 0; sector < bank->num_sectors; sector++) {
+		sectors[sector].offset = sector * (stmqspi_info->dev.sectorsize << dual);
+		sectors[sector].size = (stmqspi_info->dev.sectorsize << dual);
+		sectors[sector].is_erased = -1;
+		sectors[sector].is_protected = 0;
+	}
+
+	bank->sectors = sectors;
+	stmqspi_info->dev.name = stmqspi_info->devname;
+	if (stmqspi_info->dev.size_in_bytes / 4096)
+		LOG_INFO("flash \'%s\' id = unknown\nchip size = %" PRIu32 "kbytes,"
+			" bank size = %" PRIu32 "kbytes", stmqspi_info->dev.name,
+			stmqspi_info->dev.size_in_bytes / 1024,
+			(stmqspi_info->dev.size_in_bytes / 1024)<<dual);
+	else
+		LOG_INFO("flash \'%s\' id = unknown\nchip size = %" PRIu32 "bytes,"
+			" bank size = %" PRIu32 "bytes", stmqspi_info->dev.name,
+			stmqspi_info->dev.size_in_bytes,
+			stmqspi_info->dev.size_in_bytes<<dual);
+
+	stmqspi_info->probed = true;
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(stmqspi_handle_cmd)
+{
+	struct target *target = NULL;
+	struct flash_bank *bank;
+	struct stmqspi_flash_bank *stmqspi_info = NULL;
+	uint32_t io_base, addr;
+	uint8_t num_write, num_read, cmd_byte, data;
+	unsigned int count;
+	const int max = 21;
+	char temp[4], output[(2 + max + 256) * 3 + 8];
+	int retval;
+
+	LOG_DEBUG("%s", __func__);
+
+	if (CMD_ARGC < 3)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	num_write = CMD_ARGC - 2;
+	if (num_write > max) {
+		LOG_ERROR("at most %d bytes may be sent", max);
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
+	if (ERROR_OK != retval)
+		return retval;
+
+	target = bank->target;
+	stmqspi_info = bank->driver_priv;
+	io_base = stmqspi_info->io_base;
+
+	if (target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	COMMAND_PARSE_NUMBER(u8, CMD_ARGV[1], num_read);
+	COMMAND_PARSE_NUMBER(u8, CMD_ARGV[2], cmd_byte);
+
+	if (num_read == 0) {
+		/* nothing to read, then one command byte and for dual flash
+		 * an *even* number of data bytes to follow */
+		if (stmqspi_info->saved_cr & (1U<<SPI_DUAL_FLASH)) {
+			if ((num_write & 1) == 0) {
+				LOG_ERROR("number of data bytes to write must be even in dual mode");
+				return ERROR_COMMAND_SYNTAX_ERROR;
+			}
+		}
+	} else {
+		/* read mode, one command byte and up to four following address bytes */
+		if (stmqspi_info->saved_cr & (1U<<SPI_DUAL_FLASH)) {
+			if ((num_read & 1) != 0) {
+				LOG_ERROR("number of bytes to read must be even in dual mode");
+				return ERROR_COMMAND_SYNTAX_ERROR;
+			}
+		}
+		if ((num_write < 1) || (num_write > 5)) {
+			LOG_ERROR("one cmd and up to four addr bytes must be send when reading");
+			return ERROR_COMMAND_SYNTAX_ERROR;
+		}
+	}
+
+	/* Abort any previous operation */
+	retval = target_write_u32(target, io_base + SPI_CR,
+		READ_REG(SPI_CR) | (1U<<SPI_ABORT));
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Wait for busy to be cleared */
+	retval = poll_busy(bank, SPI_PROBE_TIMEOUT);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* send command byte */
+	snprintf(output, sizeof(output), "spi: %02x ", cmd_byte);
+	if (num_read == 0) {
+		/* write, send cmd byte */
+		retval = target_write_u32(target, io_base + SPI_DLR, ((uint32_t) num_write) - 2);
+		if (retval != ERROR_OK)
+			goto err;
+
+		if (IS_OCTOSPI)
+			retval = OCTOSPI_CMD(OCTOSPI_WRITE_MODE,
+				(OCTOSPI_MODE_CCR & OCTOSPI_NO_ALTB & OCTOSPI_NO_ADDR &
+				((num_write == 1) ? OCTOSPI_NO_DATA : ~0U)), cmd_byte);
+		else
+			retval = target_write_u32(target, io_base + QSPI_CCR,
+				(QSPI_MODE & ~QSPI_DCYC_MASK & QSPI_NO_ALTB & QSPI_NO_ADDR &
+				((num_write == 1) ? QSPI_NO_DATA : ~0U)) |
+				(QSPI_WRITE_MODE | cmd_byte));
+		if (retval != ERROR_OK)
+			goto err;
+
+		/* send additional data bytes */
+		for (count = 3; count < CMD_ARGC; count++) {
+			COMMAND_PARSE_NUMBER(u8, CMD_ARGV[count], data);
+			snprintf(temp, sizeof(temp), "%02" PRIx8 " ", data);
+			retval = target_write_u8(target, io_base + SPI_DR, data); \
+			if (retval != ERROR_OK)
+				goto err;
+			strncat(output, temp, sizeof(output) - strlen(output) - 1);
+		}
+		strncat(output, "-> ", sizeof(output) - strlen(output) - 1);
+	} else {
+		/* read, pack additional bytes into address */
+		addr = 0;
+		for (count = 3; count < CMD_ARGC; count++) {
+			COMMAND_PARSE_NUMBER(u8, CMD_ARGV[count], data);
+			snprintf(temp, sizeof(temp), "%02" PRIx8 " ", data);
+			addr = (addr << 8) | data;
+			strncat(output, temp, sizeof(output) - strlen(output) - 1);
+		}
+		strncat(output, "-> ", sizeof(output) - strlen(output) - 1);
+
+		/* send cmd byte, if ADMODE indicates no address, this already triggers command */
+		retval = target_write_u32(target, io_base + SPI_DLR, ((uint32_t) num_read) - 1);
+		if (retval != ERROR_OK)
+			goto err;
+		if (IS_OCTOSPI)
+			retval = OCTOSPI_CMD(OCTOSPI_READ_MODE,
+				(OCTOSPI_MODE_CCR & OCTOSPI_NO_DDTR & OCTOSPI_NO_ALTB & ~OCTOSPI_ADDR4 &
+					((num_write == 1) ? OCTOSPI_NO_ADDR : ~0U)) |
+				(((num_write - 2) & 0x3U)<<SPI_ADSIZE_POS), cmd_byte);
+		else
+			retval = target_write_u32(target, io_base + QSPI_CCR,
+				(QSPI_MODE & ~QSPI_DCYC_MASK & QSPI_NO_ALTB & ~QSPI_ADDR4 &
+					((num_write == 1) ? QSPI_NO_ADDR : ~0U)) |
+				((QSPI_READ_MODE | (((num_write - 2) & 0x3U)<<SPI_ADSIZE_POS) | cmd_byte)));
+		if (retval != ERROR_OK)
+			goto err;
+
+		if (num_write > 1) {
+			/* if ADMODE indicates address required, only the write to AR triggers command */
+			retval = target_write_u32(target, io_base + SPI_AR, addr);
+			if (retval != ERROR_OK)
+				goto err;
+		}
+
+		/* read response bytes */
+		for ( ; num_read > 0; num_read--) {
+			retval = target_read_u8(target, io_base + SPI_DR, &data);
+			if (retval != ERROR_OK)
+				goto err;
+			snprintf(temp, sizeof(temp), "%02" PRIx8 " ", data);
+			strncat(output, temp, sizeof(output) - strlen(output) - 1);
+		}
+	}
+	command_print(CMD, "%s", output);
+
+err:
+	/* Switch to memory mapped mode before return to prompt */
+	set_mm_mode(bank);
+
+	return retval;
+}
+
+static int qspi_erase_sector(struct flash_bank *bank, unsigned int sector)
+{
+	struct target *target = bank->target;
+	struct stmqspi_flash_bank *stmqspi_info = bank->driver_priv;
+	uint32_t io_base = stmqspi_info->io_base;
+	uint16_t status;
+	int retval;
+
+	retval = qspi_write_enable(bank);
+	if (retval != ERROR_OK)
+		goto err;
+
+	/* Send Sector Erase command */
+	if (IS_OCTOSPI)
+		retval = OCTOSPI_CMD(OCTOSPI_WRITE_MODE, OCTOSPI_CCR_SECTOR_ERASE,
+			stmqspi_info->dev.erase_cmd);
+	else
+		retval = target_write_u32(target, io_base + QSPI_CCR, QSPI_CCR_SECTOR_ERASE);
+	if (retval != ERROR_OK)
+		goto err;
+
+	/* Address is sector offset, this write initiates command transmission */
+	retval = target_write_u32(target, io_base + SPI_AR, bank->sectors[sector].offset);
+	if (retval != ERROR_OK)
+		goto err;
+
+	/* Wait for transmit of command completed */
+	poll_busy(bank, SPI_CMD_TIMEOUT);
+	if (retval != ERROR_OK)
+		goto err;
+
+	/* Read flash status register(s) */
+	retval = read_status_reg(bank, &status);
+	if (retval != ERROR_OK)
+		goto err;
+
+	LOG_DEBUG("erase status regs: 0x%04" PRIx16, status);
+
+	/* Check for command in progress for flash 1 */
+	/* If BSY and WE are already cleared the erase did probably complete already */
+	if (((stmqspi_info->saved_cr & ((1U<<SPI_DUAL_FLASH) | (1U<<SPI_FSEL_FLASH)))
+		!= (1U<<SPI_FSEL_FLASH)) && ((status & SPIFLASH_BSY_BIT) == 0) &&
+		((status & SPIFLASH_WE_BIT) != 0)) {
+		LOG_ERROR("Sector erase command not accepted by flash1. Status=0x%02" PRIx8,
+			status & 0xFF);
+		retval = ERROR_FLASH_OPERATION_FAILED;
+		goto err;
+	}
+
+	/* Check for command in progress for flash 2 */
+	/* If BSY and WE are already cleared the erase did probably complete already */
+	status >>= 8;
+	if (((stmqspi_info->saved_cr & ((1U<<SPI_DUAL_FLASH) | (1U<<SPI_FSEL_FLASH)))
+		!= (0U<<SPI_FSEL_FLASH)) && ((status & SPIFLASH_BSY_BIT) == 0) &&
+		((status & SPIFLASH_WE_BIT) != 0)) {
+		LOG_ERROR("Sector erase command not accepted by flash2. Status=0x%02" PRIx8,
+			status & 0xFF);
+		retval = ERROR_FLASH_OPERATION_FAILED;
+		goto err;
+	}
+
+	/* Erase takes a long time, so some sort of progress message is a good idea */
+	LOG_DEBUG("erasing sector %4u", sector);
+
+	/* Poll WIP for end of self timed Sector Erase cycle */
+	retval = wait_till_ready(bank, SPI_MAX_TIMEOUT);
+
+err:
+	return retval;
+}
+
+static int stmqspi_erase(struct flash_bank *bank, unsigned int first, unsigned int last)
+{
+	struct target *target = bank->target;
+	struct stmqspi_flash_bank *stmqspi_info = bank->driver_priv;
+	unsigned int sector;
+	int retval = ERROR_OK;
+
+	LOG_DEBUG("%s: from sector %u to sector %u", __func__, first, last);
+
+	if (target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if (!(stmqspi_info->probed)) {
+		LOG_ERROR("Flash bank not probed");
+		return ERROR_FLASH_BANK_NOT_PROBED;
+	}
+
+	if (stmqspi_info->dev.erase_cmd == 0x00) {
+		LOG_ERROR("Sector erase not available for this device");
+		return ERROR_FLASH_OPER_UNSUPPORTED;
+	}
+
+	if ((last < first) || (last >= bank->num_sectors)) {
+		LOG_ERROR("Flash sector invalid");
+		return ERROR_FLASH_SECTOR_INVALID;
+	}
+
+	for (sector = first; sector <= last; sector++) {
+		if (bank->sectors[sector].is_protected) {
+			LOG_ERROR("Flash sector %u protected", sector);
+			return ERROR_FLASH_PROTECTED;
+		}
+	}
+
+	for (sector = first; sector <= last; sector++) {
+		retval = qspi_erase_sector(bank, sector);
+		if (retval != ERROR_OK)
+			break;
+		alive_sleep(10);
+		keep_alive();
+	}
+
+	if (retval != ERROR_OK)
+		LOG_ERROR("Flash sector_erase failed on sector %u", sector);
+
+	/* Switch to memory mapped mode before return to prompt */
+	set_mm_mode(bank);
+
+	return retval;
+}
+
+static int stmqspi_protect(struct flash_bank *bank, int set,
+	unsigned int first, unsigned int last)
+{
+	unsigned int sector;
+
+	for (sector = first; sector <= last; sector++)
+		bank->sectors[sector].is_protected = set;
+
+	if (set)
+		LOG_WARNING("setting soft protection only, not related to flash's hardware write protection");
+
+	return ERROR_OK;
+}
+
+/* Check whether flash is blank */
+static int stmqspi_blank_check(struct flash_bank *bank)
+{
+	struct target *target = bank->target;
+	struct stmqspi_flash_bank *stmqspi_info = bank->driver_priv;
+	uint32_t io_base = stmqspi_info->io_base;
+	struct duration bench;
+	struct reg_param reg_params[2];
+	struct armv7m_algorithm armv7m_info;
+	struct working_area *algorithm;
+	const uint8_t *code;
+	struct sector_info erase_check_info;
+	uint32_t codesize, maxsize, result, exit_point;
+	unsigned int count, index, num_sectors, sector;
+	int retval;
+	const uint32_t erased = 0x00FF;
+
+	if (target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if (!(stmqspi_info->probed)) {
+		LOG_ERROR("Flash bank not probed");
+		return ERROR_FLASH_BANK_NOT_PROBED;
+	}
+
+	/* Abort any previous operation */
+	retval = target_write_u32(target, io_base + SPI_CR,
+		READ_REG(SPI_CR) | (1U<<SPI_ABORT));
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Wait for busy to be cleared */
+	retval = poll_busy(bank, SPI_PROBE_TIMEOUT);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* see contrib/loaders/flash/stmqspi/stmqspi_erase_check.S for src */
+	static const uint8_t stmqspi_erase_check_code[] = {
+		#include "../../../contrib/loaders/flash/stmqspi/stmqspi_erase_check.inc"
+	};
+
+	/* see contrib/loaders/flash/stmqspi/stmoctospi_erase_check.S for src */
+	static const uint8_t stmoctospi_erase_check_code[] = {
+		#include "../../../contrib/loaders/flash/stmqspi/stmoctospi_erase_check.inc"
+	};
+
+	if (IS_OCTOSPI) {
+		code = stmoctospi_erase_check_code;
+		codesize = sizeof(stmoctospi_erase_check_code);
+	} else {
+		code = stmqspi_erase_check_code;
+		codesize = sizeof(stmqspi_erase_check_code);
+	}
+
+	/* This will overlay the last 4 words of stmqspi/stmoctospi_erase_check_code in target */
+	/* for read use the saved settings (memory mapped mode) but indirect read mode */
+	uint32_t ccr_buffer[][4] = {
+		/* cr  (not used for QSPI)			*
+		 * ccr (for both QSPI and OCTOSPI)	*
+		 * tcr (not used for QSPI)			*
+		 * ir  (not used for QSPI)			*/
+		{
+			h_to_le_32(OCTOSPI_MODE | OCTOSPI_READ_MODE),
+			h_to_le_32(IS_OCTOSPI ? OCTOSPI_CCR_READ : QSPI_CCR_READ),
+			h_to_le_32(stmqspi_info->saved_tcr),
+			h_to_le_32(stmqspi_info->saved_ir),
+		},
+	};
+
+	maxsize = target_get_working_area_avail(target);
+	if (maxsize < codesize + sizeof(erase_check_info)) {
+		LOG_ERROR("Not enough working area, can't do QSPI blank check");
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	}
+
+	num_sectors = (maxsize - codesize) / sizeof(erase_check_info);
+	num_sectors = (bank->num_sectors < num_sectors) ? bank->num_sectors : num_sectors;
+
+	if (target_alloc_working_area_try(target,
+			codesize + num_sectors * sizeof(erase_check_info), &algorithm) != ERROR_OK) {
+		LOG_ERROR("allocating working area failed");
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	};
+
+	/* prepare blank check code, excluding ccr_buffer */
+	retval = target_write_buffer(target, algorithm->address,
+		codesize - sizeof(ccr_buffer), code);
+	if (retval != ERROR_OK)
+		goto err;
+
+	/* prepare QSPI/OCTOSPI_CCR register values */
+	retval = target_write_buffer(target, algorithm->address
+		+ codesize - sizeof(ccr_buffer),
+		sizeof(ccr_buffer), (uint8_t *) ccr_buffer);
+	if (retval != ERROR_OK)
+		goto err;
+
+	duration_start(&bench);
+
+	/* after breakpoint instruction (halfword), one nop (halfword) and
+	 * port_buffer till end of code */
+	exit_point = algorithm->address + codesize - sizeof(uint32_t) - sizeof(ccr_buffer);
+
+	init_reg_param(&reg_params[0], "r0", 32, PARAM_OUT);	/* sector count */
+	init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);	/* QSPI/OCTOSPI io_base */
+
+	sector = 0;
+	while (sector < bank->num_sectors) {
+		/* at most num_sectors sectors to handle in one run */
+		count = bank->num_sectors - sector;
+		if (count > num_sectors)
+			count = num_sectors;
+
+		for (index = 0; index < count; index++) {
+			erase_check_info.offset = h_to_le_32(bank->sectors[sector + index].offset);
+			erase_check_info.size = h_to_le_32(bank->sectors[sector + index].size);
+			erase_check_info.result = h_to_le_32(erased);
+
+			retval = target_write_buffer(target, algorithm->address
+				+ codesize + index * sizeof(erase_check_info),
+					sizeof(erase_check_info), (uint8_t *) &erase_check_info);
+			if (retval != ERROR_OK)
+				goto err;
+		}
+
+		buf_set_u32(reg_params[0].value, 0, 32, count);
+		buf_set_u32(reg_params[1].value, 0, 32, stmqspi_info->io_base);
+
+		armv7m_info.common_magic = ARMV7M_COMMON_MAGIC;
+		armv7m_info.core_mode = ARM_MODE_THREAD;
+
+		LOG_DEBUG("checking sectors %u to %u", sector, sector + count - 1);
+		/* check a block of sectors */
+		retval = target_run_algorithm(target,
+			0, NULL,
+			ARRAY_SIZE(reg_params), reg_params,
+			algorithm->address, exit_point,
+			count * ((bank->sectors[sector].size >> 6) + 1) + 1000,
+			&armv7m_info);
+		if (retval != ERROR_OK)
+			break;
+
+		for (index = 0; index < count; index++) {
+			retval = target_read_buffer(target, algorithm->address
+				+ codesize + index * sizeof(erase_check_info),
+					sizeof(erase_check_info), (uint8_t *) &erase_check_info);
+			if (retval != ERROR_OK)
+				goto err;
+
+			if ((erase_check_info.offset != h_to_le_32(bank->sectors[sector + index].offset)) ||
+				(erase_check_info.size != 0)) {
+				LOG_ERROR("corrupted blank check info");
+				goto err;
+			}
+
+			/* we need le_32_to_h, but that's the same as h_to_le_32 */
+			result = h_to_le_32(erase_check_info.result);
+			bank->sectors[sector + index].is_erased = ((result & 0xFF) == 0xFF);
+			LOG_DEBUG("Flash sector %u checked: 0x%04" PRIx16, sector + index, result & 0xFFFF);
+		}
+		keep_alive();
+		sector += count;
+	}
+
+	destroy_reg_param(&reg_params[0]);
+	destroy_reg_param(&reg_params[1]);
+
+	duration_measure(&bench);
+	LOG_INFO("stmqspi blank checked in %fs (%0.3f KiB/s)", duration_elapsed(&bench),
+		duration_kbps(&bench, bank->size));
+
+err:
+	target_free_working_area(target, algorithm);
+
+	/* Switch to memory mapped mode before return to prompt */
+	set_mm_mode(bank);
+
+	return retval;
+}
+
+/* Verify checksum */
+static int qspi_verify(struct flash_bank *bank, uint8_t *buffer,
+	uint32_t offset, uint32_t count)
+{
+	struct target *target = bank->target;
+	struct stmqspi_flash_bank *stmqspi_info = bank->driver_priv;
+	struct reg_param reg_params[4];
+	struct armv7m_algorithm armv7m_info;
+	struct working_area *algorithm;
+	const uint8_t *code;
+	uint32_t pagesize, codesize, crc32, result, exit_point;
+	int retval;
+
+	/* see contrib/loaders/flash/stmqspi/stmqspi_crc32.S for src */
+	static const uint8_t stmqspi_crc32_code[] = {
+		#include "../../../contrib/loaders/flash/stmqspi/stmqspi_crc32.inc"
+	};
+
+	/* see contrib/loaders/flash/stmqspi/stmoctospi_crc32.S for src */
+	static const uint8_t stmoctospi_crc32_code[] = {
+		#include "../../../contrib/loaders/flash/stmqspi/stmoctospi_crc32.inc"
+	};
+
+	if (IS_OCTOSPI) {
+		code = stmoctospi_crc32_code;
+		codesize = sizeof(stmoctospi_crc32_code);
+	} else {
+		code = stmqspi_crc32_code;
+		codesize = sizeof(stmqspi_crc32_code);
+	}
+
+	/* block size doesn't matter that much here */
+	pagesize = stmqspi_info->dev.sectorsize;
+	if (pagesize == 0)
+		pagesize = stmqspi_info->dev.pagesize;
+	if (pagesize == 0)
+		pagesize = SPIFLASH_DEF_PAGESIZE;
+
+	/* adjust size according to dual flash mode */
+	pagesize = (stmqspi_info->saved_cr & (1U<<SPI_DUAL_FLASH)) ? pagesize << 1 : pagesize;
+
+	/* This will overlay the last 4 words of stmqspi/stmoctospi_crc32_code in target */
+	/* for read use the saved settings (memory mapped mode) but indirect read mode */
+	uint32_t ccr_buffer[][4] = {
+		/* cr  (not used for QSPI)			*
+		 * ccr (for both QSPI and OCTOSPI)	*
+		 * tcr (not used for QSPI)			*
+		 * ir  (not used for QSPI)			*/
+		{
+			h_to_le_32(OCTOSPI_MODE | OCTOSPI_READ_MODE),
+			h_to_le_32(IS_OCTOSPI ? OCTOSPI_CCR_READ : QSPI_CCR_READ),
+			h_to_le_32(stmqspi_info->saved_tcr),
+			h_to_le_32(stmqspi_info->saved_ir),
+		},
+	};
+
+	if (target_alloc_working_area_try(target, codesize, &algorithm) != ERROR_OK) {
+		LOG_ERROR("Not enough working area, can't do QSPI verify");
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	};
+
+	/* prepare verify code, excluding ccr_buffer */
+	retval = target_write_buffer(target, algorithm->address,
+		codesize - sizeof(ccr_buffer), code);
+	if (retval != ERROR_OK)
+		goto err;
+
+	/* prepare QSPI/OCTOSPI_CCR register values */
+	retval = target_write_buffer(target, algorithm->address
+		+ codesize - sizeof(ccr_buffer),
+		sizeof(ccr_buffer), (uint8_t *) ccr_buffer);
+	if (retval != ERROR_OK)
+		goto err;
+
+	/* after breakpoint instruction (halfword), one nop (halfword) and
+	 * port_buffer till end of code */
+	exit_point = algorithm->address + codesize - sizeof(uint32_t) - sizeof(ccr_buffer);
+
+	init_reg_param(&reg_params[0], "r0", 32, PARAM_IN_OUT); /* count (in), crc32 (out) */
+	init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);	/* pagesize */
+	init_reg_param(&reg_params[2], "r2", 32, PARAM_OUT);	/* offset into flash address */
+	init_reg_param(&reg_params[3], "r3", 32, PARAM_OUT);	/* QSPI/OCTOSPI io_base */
+
+	buf_set_u32(reg_params[0].value, 0, 32, count);
+	buf_set_u32(reg_params[1].value, 0, 32, pagesize);
+	buf_set_u32(reg_params[2].value, 0, 32, offset);
+	buf_set_u32(reg_params[3].value, 0, 32, stmqspi_info->io_base);
+
+
+	armv7m_info.common_magic = ARMV7M_COMMON_MAGIC;
+	armv7m_info.core_mode = ARM_MODE_THREAD;
+
+	retval = target_run_algorithm(target,
+		0, NULL,
+		ARRAY_SIZE(reg_params), reg_params,
+		algorithm->address, exit_point,
+		(count >> 5) + 1000,
+		&armv7m_info);
+	keep_alive();
+
+	image_calculate_checksum(buffer, count, &crc32);
+
+	if (retval == ERROR_OK) {
+		result = buf_get_u32(reg_params[0].value, 0, 32);
+		LOG_DEBUG("addr " TARGET_ADDR_FMT ", len 0x%08" PRIx32 ", crc 0x%08" PRIx32 " 0x%08" PRIx32,
+			offset + bank->base, count, ~crc32, result);
+		if (~crc32 != result)
+			retval = ERROR_FAIL;
+	}
+
+	destroy_reg_param(&reg_params[0]);
+	destroy_reg_param(&reg_params[1]);
+	destroy_reg_param(&reg_params[2]);
+	destroy_reg_param(&reg_params[3]);
+
+err:
+	target_free_working_area(target, algorithm);
+
+	/* Switch to memory mapped mode before return to prompt */
+	set_mm_mode(bank);
+
+	return retval;
+}
+
+static int qspi_read_write_block(struct flash_bank *bank, uint8_t *buffer,
+	uint32_t offset, uint32_t count, bool write)
+{
+	struct target *target = bank->target;
+	struct stmqspi_flash_bank *stmqspi_info = bank->driver_priv;
+	uint32_t io_base = stmqspi_info->io_base;
+	struct reg_param reg_params[6];
+	struct armv7m_algorithm armv7m_info;
+	struct working_area *algorithm;
+	uint32_t pagesize, fifo_start, fifosize, remaining;
+	uint32_t maxsize, codesize, exit_point;
+	const uint8_t *code = NULL;
+	unsigned int dual;
+	int retval;
+
+	LOG_DEBUG("%s: offset=0x%08" PRIx32 " len=0x%08" PRIx32,
+		__func__, offset, count);
+
+	dual = (stmqspi_info->saved_cr & (1U<<SPI_DUAL_FLASH)) ? 1 : 0;
+
+	/* see contrib/loaders/flash/stmqspi/stmqspi_read.S for src */
+	static const uint8_t stmqspi_read_code[] = {
+#include "../../../contrib/loaders/flash/stmqspi/stmqspi_read.inc"
+	};
+
+	/* see contrib/loaders/flash/stmqspi/stmoctospi_read.S for src */
+	static const uint8_t stmoctospi_read_code[] = {
+#include "../../../contrib/loaders/flash/stmqspi/stmoctospi_read.inc"
+	};
+
+	/* see contrib/loaders/flash/stmqspi/stmqspi_write.S for src */
+	static const uint8_t stmqspi_write_code[] = {
+#include "../../../contrib/loaders/flash/stmqspi/stmqspi_write.inc"
+	};
+
+	/* see contrib/loaders/flash/stmqspi/stmoctospi_write.S for src */
+	static const uint8_t stmoctospi_write_code[] = {
+#include "../../../contrib/loaders/flash/stmqspi/stmoctospi_write.inc"
+	};
+
+	/* This will overlay the last 12 words of stmqspi/stmoctospi_read/write_code in target */
+	/* for read use the saved settings (memory mapped mode) but indirect read mode */
+	uint32_t ccr_buffer[][4] = {
+		/* cr  (not used for QSPI)			*
+		 * ccr (for both QSPI and OCTOSPI)	*
+		 * tcr (not used for QSPI)			*
+		 * ir  (not used for QSPI)			*/
+		{
+			h_to_le_32(OCTOSPI_MODE | OCTOSPI_READ_MODE),
+			h_to_le_32(IS_OCTOSPI ? OCTOSPI_CCR_READ_STATUS : QSPI_CCR_READ_STATUS),
+			h_to_le_32((stmqspi_info->saved_tcr & ~OCTOSPI_DCYC_MASK) |
+						(OPI_MODE ? (OPI_DUMMY<<OCTOSPI_DCYC_POS) : 0)),
+			h_to_le_32(OPI_CMD(SPIFLASH_READ_STATUS)),
+		},
+		{
+			h_to_le_32(OCTOSPI_MODE | OCTOSPI_WRITE_MODE),
+			h_to_le_32(IS_OCTOSPI ? OCTOSPI_CCR_WRITE_ENABLE : QSPI_CCR_WRITE_ENABLE),
+			h_to_le_32(stmqspi_info->saved_tcr & ~OCTOSPI_DCYC_MASK),
+			h_to_le_32(OPI_CMD(SPIFLASH_WRITE_ENABLE)),
+		},
+		{
+			h_to_le_32(OCTOSPI_MODE | (write ? OCTOSPI_WRITE_MODE : OCTOSPI_READ_MODE)),
+			h_to_le_32(write ? (IS_OCTOSPI ? OCTOSPI_CCR_PAGE_PROG : QSPI_CCR_PAGE_PROG) :
+				(IS_OCTOSPI ? OCTOSPI_CCR_READ : QSPI_CCR_READ)),
+			h_to_le_32(write ? (stmqspi_info->saved_tcr & ~OCTOSPI_DCYC_MASK) :
+				stmqspi_info->saved_tcr),
+			h_to_le_32(write ? OPI_CMD(stmqspi_info->dev.pprog_cmd) : stmqspi_info->saved_ir),
+		},
+	};
+
+	/* force reasonable defaults */
+	fifosize = stmqspi_info->dev.sectorsize ?
+		stmqspi_info->dev.sectorsize : stmqspi_info->dev.size_in_bytes;
+
+	if (write) {
+		if (IS_OCTOSPI) {
+			code = stmoctospi_write_code;
+			codesize = sizeof(stmoctospi_write_code);
+		} else {
+			code = stmqspi_write_code;
+			codesize = sizeof(stmqspi_write_code);
+		}
+	} else {
+		if (IS_OCTOSPI) {
+			code = stmoctospi_read_code;
+			codesize = sizeof(stmoctospi_read_code);
+		} else {
+			code = stmqspi_read_code;
+			codesize = sizeof(stmqspi_read_code);
+		}
+	}
+
+	/* for write, pagesize must be taken into account */
+	/* for read, the page size doesn't matter that much */
+	pagesize = stmqspi_info->dev.pagesize;
+	if (pagesize == 0)
+		pagesize = (fifosize <= SPIFLASH_DEF_PAGESIZE) ?
+			fifosize : SPIFLASH_DEF_PAGESIZE;
+
+	/* adjust sizes according to dual flash mode */
+	pagesize <<= dual;
+	fifosize <<= dual;
+
+	/* memory buffer, we assume sectorsize to be a power of 2 times pagesize */
+	maxsize = target_get_working_area_avail(target);
+	if (maxsize < codesize + 2 * sizeof(uint32_t) + pagesize) {
+		LOG_ERROR("not enough working area, can't do QSPI page reads/writes");
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	}
+
+	/* fifo size at most sector size, and multiple of page size */
+	maxsize -= (codesize + 2 * sizeof(uint32_t));
+	fifosize = ((maxsize < fifosize) ? maxsize : fifosize) & ~(pagesize - 1);
+
+	if (target_alloc_working_area_try(target,
+		codesize + 2 * sizeof(uint32_t) + fifosize, &algorithm) != ERROR_OK) {
+		LOG_ERROR("allocating working area failed");
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	};
+
+	/* prepare flash write code, excluding ccr_buffer */
+	retval = target_write_buffer(target, algorithm->address,
+		codesize - sizeof(ccr_buffer), code);
+	if (retval != ERROR_OK)
+		goto err;
+
+	/* prepare QSPI/OCTOSPI_CCR register values */
+	retval = target_write_buffer(target, algorithm->address
+		+ codesize - sizeof(ccr_buffer),
+		sizeof(ccr_buffer), (uint8_t *) ccr_buffer);
+	if (retval != ERROR_OK)
+		goto err;
+
+	/* target buffer starts right after flash_write_code, i.e.
+	 * wp and rp are implicitly included in buffer!!! */
+	fifo_start = algorithm->address + codesize + 2 * sizeof(uint32_t);
+
+	init_reg_param(&reg_params[0], "r0", 32, PARAM_IN_OUT); /* count (in), status (out) */
+	init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);	/* pagesize */
+	init_reg_param(&reg_params[2], "r2", 32, PARAM_IN_OUT);	/* offset into flash address */
+	init_reg_param(&reg_params[3], "r3", 32, PARAM_OUT);	/* QSPI/OCTOSPI io_base */
+	init_reg_param(&reg_params[4], "r8", 32, PARAM_OUT);	/* fifo start */
+	init_reg_param(&reg_params[5], "r9", 32, PARAM_OUT);	/* fifo end + 1 */
+
+	buf_set_u32(reg_params[0].value, 0, 32, count);
+	buf_set_u32(reg_params[1].value, 0, 32, pagesize);
+	buf_set_u32(reg_params[2].value, 0, 32, offset);
+	buf_set_u32(reg_params[3].value, 0, 32, io_base);
+	buf_set_u32(reg_params[4].value, 0, 32, fifo_start);
+	buf_set_u32(reg_params[5].value, 0, 32, fifo_start + fifosize);
+
+	armv7m_info.common_magic = ARMV7M_COMMON_MAGIC;
+	armv7m_info.core_mode = ARM_MODE_THREAD;
+
+	/* after breakpoint instruction (halfword), one nop (halfword) and
+	 * ccr_buffer follow till end of code */
+	exit_point = algorithm->address + codesize
+		- (sizeof(ccr_buffer) + sizeof(uint32_t));
+
+	if (write) {
+		retval = target_run_flash_async_algorithm(target, buffer, count, 1,
+				0, NULL,
+				ARRAY_SIZE(reg_params), reg_params,
+				algorithm->address + codesize,
+				fifosize + 2 * sizeof(uint32_t),
+				algorithm->address, exit_point,
+				&armv7m_info);
+	} else {
+		retval = target_run_read_async_algorithm(target, buffer, count, 1,
+				0, NULL,
+				ARRAY_SIZE(reg_params), reg_params,
+				algorithm->address + codesize,
+				fifosize + 2 * sizeof(uint32_t),
+				algorithm->address, exit_point,
+				&armv7m_info);
+	}
+
+	remaining = buf_get_u32(reg_params[0].value, 0, 32);
+	if ((retval == ERROR_OK) && remaining)
+		retval = ERROR_FLASH_OPERATION_FAILED;
+
+	if (retval != ERROR_OK) {
+		offset = buf_get_u32(reg_params[2].value, 0, 32);
+		LOG_ERROR("flash %s failed at address 0x%" PRIx32 ", remaining 0x%" PRIx32,
+			write ? "write" : "read", offset, remaining);
+	}
+
+	destroy_reg_param(&reg_params[0]);
+	destroy_reg_param(&reg_params[1]);
+	destroy_reg_param(&reg_params[2]);
+	destroy_reg_param(&reg_params[3]);
+	destroy_reg_param(&reg_params[4]);
+	destroy_reg_param(&reg_params[5]);
+
+err:
+	target_free_working_area(target, algorithm);
+
+	/* Switch to memory mapped mode before return to prompt */
+	set_mm_mode(bank);
+
+	return retval;
+}
+
+static int stmqspi_read(struct flash_bank *bank, uint8_t *buffer,
+	uint32_t offset, uint32_t count)
+{
+	struct target *target = bank->target;
+	struct stmqspi_flash_bank *stmqspi_info = bank->driver_priv;
+	uint32_t io_base = stmqspi_info->io_base;
+	int retval;
+
+	LOG_DEBUG("%s: offset=0x%08" PRIx32 " count=0x%08" PRIx32,
+		__func__, offset, count);
+
+	if (target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if (!(stmqspi_info->probed)) {
+		LOG_ERROR("Flash bank not probed");
+		return ERROR_FLASH_BANK_NOT_PROBED;
+	}
+
+	if (offset + count > bank->size) {
+		LOG_WARNING("Read beyond end of flash. Extra data to be ignored.");
+		count = bank->size - offset;
+	}
+
+	/* Abort any previous operation */
+	retval = target_write_u32(target, io_base + SPI_CR,
+		READ_REG(SPI_CR) | (1U<<SPI_ABORT));
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Wait for busy to be cleared */
+	retval = poll_busy(bank, SPI_PROBE_TIMEOUT);
+	if (retval != ERROR_OK)
+		return retval;
+
+	return qspi_read_write_block(bank, buffer, offset, count, false);
+}
+
+static int stmqspi_write(struct flash_bank *bank, const uint8_t *buffer,
+	uint32_t offset, uint32_t count)
+{
+	struct target *target = bank->target;
+	struct stmqspi_flash_bank *stmqspi_info = bank->driver_priv;
+	uint32_t io_base = stmqspi_info->io_base;
+	unsigned int dual, sector;
+	bool octal_dtr;
+	int retval;
+
+	LOG_DEBUG("%s: offset=0x%08" PRIx32 " count=0x%08" PRIx32,
+		__func__, offset, count);
+
+	dual = (stmqspi_info->saved_cr & (1U<<SPI_DUAL_FLASH)) ? 1 : 0;
+	octal_dtr = IS_OCTOSPI && (stmqspi_info->saved_ccr & (1U<<OCTOSPI_DDTR));
+
+	if (target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if (!(stmqspi_info->probed)) {
+		LOG_ERROR("Flash bank not probed");
+		return ERROR_FLASH_BANK_NOT_PROBED;
+	}
+
+	if (offset + count > bank->size) {
+		LOG_WARNING("Write beyond end of flash. Extra data discarded.");
+		count = bank->size - offset;
+	}
+
+	/* Check sector protection */
+	for (sector = 0; sector < bank->num_sectors; sector++) {
+		/* Start offset in or before this sector? */
+		/* End offset in or behind this sector? */
+		if ((offset < (bank->sectors[sector].offset + bank->sectors[sector].size))
+			&& ((offset + count - 1) >= bank->sectors[sector].offset)
+			&& bank->sectors[sector].is_protected) {
+			LOG_ERROR("Flash sector %u protected", sector);
+			return ERROR_FLASH_PROTECTED;
+		}
+	}
+
+	if ((dual || octal_dtr) && ((offset & 1) != 0 || (count & 1) != 0)) {
+		LOG_ERROR("In dual-QSPI and octal-DTR modes writes must be two byte aligned: "
+			"%s: address=0x%08" PRIx32 " len=0x%08" PRIx32, __func__, offset, count);
+		return ERROR_FLASH_DST_BREAKS_ALIGNMENT;
+	}
+
+	/* Abort any previous operation */
+	retval = target_write_u32(target, io_base + SPI_CR,
+		READ_REG(SPI_CR) | (1U<<SPI_ABORT));
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Wait for busy to be cleared */
+	retval = poll_busy(bank, SPI_PROBE_TIMEOUT);
+	if (retval != ERROR_OK)
+		return retval;
+
+	return qspi_read_write_block(bank, (uint8_t *) buffer, offset, count, true);
+}
+
+static int stmqspi_verify(struct flash_bank *bank, const uint8_t *buffer,
+	uint32_t offset, uint32_t count)
+{
+	struct target *target = bank->target;
+	struct stmqspi_flash_bank *stmqspi_info = bank->driver_priv;
+	uint32_t io_base = stmqspi_info->io_base;
+	unsigned int dual;
+	bool octal_dtr;
+	int retval;
+
+	LOG_DEBUG("%s: offset=0x%08" PRIx32 " count=0x%08" PRIx32,
+		__func__, offset, count);
+
+	dual = (stmqspi_info->saved_cr & (1U<<SPI_DUAL_FLASH)) ? 1 : 0;
+	octal_dtr = IS_OCTOSPI && (stmqspi_info->saved_ccr & (1U<<OCTOSPI_DDTR));
+
+	if (target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if (!(stmqspi_info->probed)) {
+		LOG_ERROR("Flash bank not probed");
+		return ERROR_FLASH_BANK_NOT_PROBED;
+	}
+
+	if (offset + count > bank->size) {
+		LOG_WARNING("Verify beyond end of flash. Extra data ignored.");
+		count = bank->size - offset;
+	}
+
+	if ((dual || octal_dtr) && ((offset & 1) != 0 || (count & 1) != 0)) {
+		LOG_ERROR("In dual-QSPI and octal-DTR modes reads must be two byte aligned: "
+			"%s: address=0x%08" PRIx32 " len=0x%08" PRIx32, __func__, offset, count);
+		return ERROR_FLASH_DST_BREAKS_ALIGNMENT;
+	}
+
+	/* Abort any previous operation */
+	retval = target_write_u32(target, io_base + SPI_CR,
+		READ_REG(SPI_CR) | (1U<<SPI_ABORT));
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Wait for busy to be cleared */
+	retval = poll_busy(bank, SPI_PROBE_TIMEOUT);
+	if (retval != ERROR_OK)
+		return retval;
+
+	return qspi_verify(bank, (uint8_t *) buffer, offset, count);
+}
+
+/* Find appropriate dummy setting, in particular octo mode */
+static int find_sfdp_dummy(struct flash_bank *bank, int len)
+{
+	struct target *target = bank->target;
+	struct stmqspi_flash_bank *stmqspi_info = bank->driver_priv;
+	uint32_t io_base = stmqspi_info->io_base;
+	uint8_t data;
+	unsigned int dual, count;
+	bool flash1 = !(stmqspi_info->saved_cr & (1U<<SPI_FSEL_FLASH));
+	int retval;
+	const unsigned int max_bytes = 64;
+
+	dual = (stmqspi_info->saved_cr & (1U<<SPI_DUAL_FLASH)) ? 1 : 0;
+
+	LOG_DEBUG("%s: len=%d, dual=%u, flash1=%d",
+		__func__, len, dual, flash1);
+
+	/* Abort any previous operation */
+	retval = target_write_u32(target, io_base + SPI_CR,
+		stmqspi_info->saved_cr | (1U<<SPI_ABORT));
+	if (retval != ERROR_OK)
+		goto err;
+
+	/* Wait for busy to be cleared */
+	retval = poll_busy(bank, SPI_PROBE_TIMEOUT);
+	if (retval != ERROR_OK)
+		goto err;
+
+	/* Switch to saved_cr (had to be set accordingly before this call) */
+	retval = target_write_u32(target, io_base + SPI_CR, stmqspi_info->saved_cr);
+	if (retval != ERROR_OK)
+		goto err;
+
+	/* Read at most that many bytes */
+	retval = target_write_u32(target, io_base + SPI_DLR, (max_bytes << dual) - 1);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Read SFDP block */
+	if (IS_OCTOSPI)
+		retval = OCTOSPI_CMD(OCTOSPI_READ_MODE, OCTOSPI_CCR_READ_SFDP(len),
+			SPIFLASH_READ_SFDP);
+	else
+		retval = target_write_u32(target, io_base + QSPI_CCR, QSPI_CCR_READ_SFDP);
+	if (retval != ERROR_OK)
+		goto err;
+
+	/* Read from start of sfdp block */
+	retval = target_write_u32(target, io_base + SPI_AR, 0);
+	if (retval != ERROR_OK)
+		goto err;
+
+	for (count = 0 ; count < max_bytes; count++) {
+		if ((dual != 0) && !flash1) {
+			/* discard even byte in dual flash-mode if flash2 */
+			retval = target_read_u8(target, io_base + SPI_DR, &data);
+			if (retval != ERROR_OK)
+				goto err;
+		}
+
+		retval = target_read_u8(target, io_base + SPI_DR, &data);
+		if (retval != ERROR_OK)
+			goto err;
+
+		if (data == 0x53) {
+			LOG_DEBUG("start of SFDP header for flash%c after %u dummy bytes",
+				flash1 ? '1' : '2', count);
+			if (flash1)
+				stmqspi_info->sfdp_dummy1 = count;
+			else
+				stmqspi_info->sfdp_dummy2 = count;
+			return ERROR_OK;
+		}
+
+		if ((dual != 0) && flash1) {
+			/* discard odd byte in dual flash-mode if flash1 */
+			retval = target_read_u8(target, io_base + SPI_DR, &data);
+			if (retval != ERROR_OK)
+				goto err;
+		}
+	}
+
+	retval = ERROR_FAIL;
+	LOG_DEBUG("no start of SFDP header even after %u dummy bytes", count);
+
+err:
+	/* Abort operation */
+	retval = target_write_u32(target, io_base + SPI_CR,
+		READ_REG(SPI_CR) | (1U<<SPI_ABORT));
+
+	return retval;
+}
+
+/* Read SFDP parameter block */
+static int read_sfdp_block(struct flash_bank *bank, uint32_t addr,
+	uint32_t words, uint32_t *buffer)
+{
+	struct target *target = bank->target;
+	struct stmqspi_flash_bank *stmqspi_info = bank->driver_priv;
+	uint32_t io_base = stmqspi_info->io_base;
+	bool flash1 = !(stmqspi_info->saved_cr & (1U<<SPI_FSEL_FLASH));
+	unsigned int dual, count, len, *dummy;
+	int retval;
+
+	dual = (stmqspi_info->saved_cr & (1U<<SPI_DUAL_FLASH)) ? 1 : 0;
+
+	if (IS_OCTOSPI && (((stmqspi_info->saved_ccr >> SPI_DMODE_POS) & 0x7) > 3)) {
+		/* in OCTO mode 4-byte address and (yet) unknown number of dummy clocks */
+		len = 4;
+
+		/* in octo mode, use sfdp_dummy1 only */
+		dummy = &stmqspi_info->sfdp_dummy1;
+		if (*dummy == 0) {
+			retval = find_sfdp_dummy(bank, len);
+			if (retval != ERROR_OK)
+				return retval;
+		}
+	} else {
+		/* in all other modes 3-byte-address and 8(?) dummy clocks */
+		len = 3;
+
+		/* use sfdp_dummy1/2 according to currently selected flash */
+		dummy = (stmqspi_info->saved_cr & (1U<<SPI_FSEL_FLASH)) ?
+			&stmqspi_info->sfdp_dummy2 : &stmqspi_info->sfdp_dummy1;
+
+		/* according to SFDP standard, there should always be 8 dummy *CLOCKS*
+		 * giving 1, 2 or 4 dummy *BYTES*, however, this is apparently not
+		 * always implemented correctly, so determine the number of dummy bytes
+		 * dynamically */
+		if (*dummy == 0) {
+			retval = find_sfdp_dummy(bank, len);
+			if (retval != ERROR_OK)
+				return retval;
+		}
+	}
+
+	LOG_DEBUG("%s: addr=0x%08" PRIx32 " words=0x%08" PRIx32 " dummy=%u",
+		__func__, addr, words, *dummy);
+
+	/* Abort any previous operation */
+	retval = target_write_u32(target, io_base + SPI_CR,
+		stmqspi_info->saved_cr | (1U<<SPI_ABORT));
+	if (retval != ERROR_OK)
+		goto err;
+
+	/* Wait for busy to be cleared */
+	retval = poll_busy(bank, SPI_PROBE_TIMEOUT);
+	if (retval != ERROR_OK)
+		goto err;
+
+	/* Switch to one flash only */
+	retval = target_write_u32(target, io_base + SPI_CR, stmqspi_info->saved_cr);
+	if (retval != ERROR_OK)
+		goto err;
+
+	/* Read that many words plus dummy bytes */
+	retval = target_write_u32(target, io_base + SPI_DLR,
+		((*dummy + words * sizeof(uint32_t)) << dual) - 1);
+	if (retval != ERROR_OK)
+		goto err;
+
+	/* Read SFDP block */
+	if (IS_OCTOSPI)
+		retval = OCTOSPI_CMD(OCTOSPI_READ_MODE, OCTOSPI_CCR_READ_SFDP(len),
+			SPIFLASH_READ_SFDP);
+	else
+		retval = target_write_u32(target, io_base + QSPI_CCR, QSPI_CCR_READ_SFDP);
+	if (retval != ERROR_OK)
+		goto err;
+
+	retval = target_write_u32(target, io_base + SPI_AR, addr << dual);
+	if (retval != ERROR_OK)
+		goto err;
+
+	/* dummy clocks */
+	for (count = *dummy << dual; count > 0; --count) {
+		retval = target_read_u8(target, io_base + SPI_DR, (uint8_t *) buffer);
+		if (retval != ERROR_OK)
+			goto err;
+	}
+
+	for ( ; words > 0; words--) {
+		if (dual != 0) {
+			uint32_t word1, word2;
+
+			retval = target_read_u32(target, io_base + SPI_DR, &word1);
+			if (retval != ERROR_OK)
+				goto err;
+			retval = target_read_u32(target, io_base + SPI_DR, &word2);
+			if (retval != ERROR_OK)
+				goto err;
+
+			if (!flash1) {
+				/* shift odd numbered bytes into even numbered ones */
+				word1 >>= 8;
+				word2 >>= 8;
+			}
+
+			/* pack even numbered bytes into one word */
+			*buffer = (word1 & 0xFFU) | ((word1 & 0xFF0000U) >> 8) |
+				((word2 & 0xFFU) << 16) | ((word2 & 0xFF0000U) << 8);
+
+
+		} else {
+			retval = target_read_u32(target, io_base + SPI_DR, buffer);
+			if (retval != ERROR_OK)
+				goto err;
+		}
+		LOG_DEBUG("raw SFDP data 0x%08" PRIx32, *buffer);
+
+		/* endian correction, sfdp data is always le uint32_t based */
+		*buffer = le_to_h_u32((uint8_t *) buffer);
+		buffer++;
+	}
+
+err:
+	return retval;
+}
+
+/* Return ID of flash device(s) */
+/* On exit, indirect mode is kept */
+static int read_flash_id(struct flash_bank *bank, uint32_t *id1, uint32_t *id2)
+{
+	struct target *target = bank->target;
+	struct stmqspi_flash_bank *stmqspi_info = bank->driver_priv;
+	uint32_t io_base = stmqspi_info->io_base;
+	uint8_t byte;
+	unsigned int type, count, len1, len2;
+	int retval;
+
+	/* invalidate both ids */
+	*id1 = 0;
+	*id2 = 0;
+
+	if (target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	/* SPIFLASH_READ_MID causes device in octal mode to go berserk, so don't use in this case */
+	for (type = (IS_OCTOSPI && OPI_MODE) ? 1 : 0; type < 2 ; type++) {
+		/* Abort any previous operation */
+		retval = target_write_u32(target, io_base + SPI_CR,
+			READ_REG(SPI_CR) | (1U<<SPI_ABORT));
+		if (retval != ERROR_OK)
+			goto err;
+
+		/* Poll WIP */
+		retval = wait_till_ready(bank, SPI_PROBE_TIMEOUT);
+		if (retval != ERROR_OK)
+			goto err;
+
+		/* Wait for busy to be cleared */
+		retval = poll_busy(bank, SPI_PROBE_TIMEOUT);
+		if (retval != ERROR_OK)
+			goto err;
+
+		/* Read at most 16 bytes per chip */
+		count = 16;
+		retval = target_write_u32(target, io_base + SPI_DLR,
+			(stmqspi_info->saved_cr & (1U<<SPI_DUAL_FLASH) ? count * 2 : count) - 1);
+		if (retval != ERROR_OK)
+			goto err;
+
+		/* Read id: one particular flash chip (N25Q128) switches back to SPI mode when receiving
+		 * SPI_FLASH_READ_ID in QPI mode, hence try SPIFLASH_READ_MID first */
+		switch (type) {
+			case 0:
+				if (IS_OCTOSPI)
+					retval = OCTOSPI_CMD(OCTOSPI_READ_MODE, OCTOSPI_CCR_READ_MID, SPIFLASH_READ_MID);
+				else
+					retval = target_write_u32(target, io_base + QSPI_CCR, QSPI_CCR_READ_MID);
+				break;
+
+			case 1:
+				if (IS_OCTOSPI)
+					retval = OCTOSPI_CMD(OCTOSPI_READ_MODE, OCTOSPI_CCR_READ_ID, SPIFLASH_READ_ID);
+				else
+					retval = target_write_u32(target, io_base + QSPI_CCR, QSPI_CCR_READ_ID);
+				break;
+
+			default:
+				return ERROR_FAIL;
+		}
+
+		if (retval != ERROR_OK)
+			goto err;
+
+		/* Dummy address 0, only required for 8-line mode */
+		if (IS_OCTOSPI && OPI_MODE) {
+			retval = target_write_u32(target, io_base + SPI_AR, 0);
+			if (retval != ERROR_OK)
+				goto err;
+		}
+
+		/* for debugging only */
+		(void) READ_REG(SPI_SR);
+
+		/* Read ID from Data Register */
+		for (len1 = 0, len2 = 0; count > 0; --count) {
+			if ((stmqspi_info->saved_cr & ((1U<<SPI_DUAL_FLASH) |
+				(1U<<SPI_FSEL_FLASH))) != (1U<<SPI_FSEL_FLASH)) {
+				retval = target_read_u8(target, io_base + SPI_DR, &byte);
+				if (retval != ERROR_OK)
+					goto err;
+				/* collect 3 bytes without continuation codes */
+				if ((byte != 0x7F) && (len1 < 3)) {
+					*id1 = (*id1 >> 8) | ((uint32_t) byte) << 16;
+					len1++;
+				}
+			}
+			if ((stmqspi_info->saved_cr & ((1U<<SPI_DUAL_FLASH) |
+				(1U<<SPI_FSEL_FLASH))) != 0) {
+				retval = target_read_u8(target, io_base + SPI_DR, &byte);
+				if (retval != ERROR_OK)
+					goto err;
+				/* collect 3 bytes without continuation codes */
+				if ((byte != 0x7F) && (len2 < 3)) {
+					*id2 = (*id2 >> 8) | ((uint32_t) byte) << 16;
+					len2++;
+				}
+			}
+		}
+
+		if (((*id1 != 0x000000) && (*id1 != 0xFFFFFF)) ||
+			((*id2 != 0x000000) && (*id2 != 0xFFFFFF)))
+			break;
+	}
+
+	if ((stmqspi_info->saved_cr & ((1U<<SPI_DUAL_FLASH) |
+		(1U<<SPI_FSEL_FLASH))) != (1U<<SPI_FSEL_FLASH)) {
+		if ((*id1 == 0x000000) || (*id1 == 0xFFFFFF)) {
+			/* no id retrieved, so id must be set manually */
+			LOG_INFO("No id from flash1");
+			retval = ERROR_FLASH_BANK_NOT_PROBED;
+		}
+	}
+
+	if ((stmqspi_info->saved_cr & ((1U<<SPI_DUAL_FLASH) |
+		(1U<<SPI_FSEL_FLASH))) != (0U<<SPI_FSEL_FLASH)) {
+		if ((*id2 == 0x000000) || (*id2 == 0xFFFFFF)) {
+			/* no id retrieved, so id must be set manually */
+			LOG_INFO("No id from flash2");
+			retval = ERROR_FLASH_BANK_NOT_PROBED;
+		}
+	}
+
+err:
+	return retval;
+}
+
+static int stmqspi_probe(struct flash_bank *bank)
+{
+	struct target *target = bank->target;
+	struct stmqspi_flash_bank *stmqspi_info = bank->driver_priv;
+	struct flash_sector *sectors = NULL;
+	uint32_t io_base = stmqspi_info->io_base;
+	uint32_t id1 = 0, id2 = 0, data = 0;
+	const struct flash_device *p;
+	const uint32_t magic = 0xAEF1510E;
+	unsigned int dual, fsize;
+	bool octal_dtr;
+	int retval;
+
+	if (stmqspi_info->probed) {
+		bank->size = 0;
+		bank->num_sectors = 0;
+		if (bank->sectors)
+			free(bank->sectors);
+		bank->sectors = NULL;
+		memset(&stmqspi_info->dev, 0, sizeof(stmqspi_info->dev));
+		stmqspi_info->sfdp_dummy1 = 0;
+		stmqspi_info->sfdp_dummy2 = 0;
+		stmqspi_info->probed = false;
+	}
+
+	/* Abort any previous operation */
+	retval = target_write_u32(target, io_base + SPI_CR,
+		READ_REG(SPI_CR) | (1U<<SPI_ABORT));
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Wait for busy to be cleared */
+	retval = poll_busy(bank, SPI_PROBE_TIMEOUT);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* check whether QSPI_ABR is writeable and readback returns the value written */
+	retval = target_write_u32(target, io_base + QSPI_ABR, magic);
+	if (retval == ERROR_OK) {
+		retval = target_read_u32(target, io_base + QSPI_ABR, &data);
+		retval = target_write_u32(target, io_base + QSPI_ABR, 0);
+	}
+
+	if (data == magic) {
+		LOG_DEBUG("QSPI_ABR register present");
+		stmqspi_info->octo = false;
+	} else if (READ_REG(OCTOSPI_MAGIC) == OCTO_MAGIC_ID) {
+		LOG_DEBUG("OCTOSPI_MAGIC present");
+		stmqspi_info->octo = true;
+	} else {
+		LOG_ERROR("No QSPI, no OCTOSPI at 0x%08" PRIx32, io_base);
+		stmqspi_info->probed = false;
+		stmqspi_info->dev.name = "none";
+		return ERROR_FAIL;
+	}
+
+	/* save current FSEL and DFM bits in QSPI/OCTOSPI_CR, current QSPI/OCTOSPI_CCR value */
+	stmqspi_info->saved_cr = READ_REG(SPI_CR);
+	if (retval == ERROR_OK)
+		stmqspi_info->saved_ccr = READ_REG(SPI_CCR);
+
+	if (IS_OCTOSPI) {
+		uint32_t mtyp;
+
+		mtyp = ((READ_REG(OCTOSPI_DCR1) & OCTOSPI_MTYP_MASK))>>OCTOSPI_MTYP_POS;
+		if (retval == ERROR_OK)
+			stmqspi_info->saved_tcr = READ_REG(OCTOSPI_TCR);
+		if (retval == ERROR_OK)
+			stmqspi_info->saved_ir = READ_REG(OCTOSPI_IR);
+		if ((mtyp != 0x0) && (mtyp != 0x1)) {
+			retval = ERROR_FAIL;
+			LOG_ERROR("Only regular SPI protocol supported in OCTOSPI");
+		}
+		if (retval == ERROR_OK) {
+			LOG_DEBUG("OCTOSPI at 0x%08" PRIx64 ", io_base at 0x%08" PRIx32 ", OCTOSPI_CR 0x%08"
+				PRIx32 ", OCTOSPI_CCR 0x%08" PRIx32 ", %d-byte addr", bank->base, io_base,
+				stmqspi_info->saved_cr, stmqspi_info->saved_ccr, SPI_ADSIZE);
+		} else {
+			LOG_ERROR("No OCTOSPI at io_base 0x%08" PRIx32, io_base);
+			stmqspi_info->probed = false;
+			stmqspi_info->dev.name = "none";
+			return ERROR_FAIL;
+		}
+	} else {
+		if (retval == ERROR_OK) {
+			LOG_DEBUG("QSPI at 0x%08" PRIx64 ", io_base at 0x%08" PRIx32 ", QSPI_CR 0x%08"
+				PRIx32 ", QSPI_CCR 0x%08" PRIx32 ", %d-byte addr", bank->base, io_base,
+				stmqspi_info->saved_cr, stmqspi_info->saved_ccr, SPI_ADSIZE);
+				if (stmqspi_info->saved_ccr & (1U << QSPI_DDRM))
+					LOG_WARNING("DDR mode is untested and suffers from some silicon bugs");
+		} else {
+			LOG_ERROR("No QSPI at io_base 0x%08" PRIx32, io_base);
+			stmqspi_info->probed = false;
+			stmqspi_info->dev.name = "none";
+			return ERROR_FAIL;
+		}
+	}
+
+	dual = (stmqspi_info->saved_cr & (1U<<SPI_DUAL_FLASH)) ? 1 : 0;
+	octal_dtr = IS_OCTOSPI && (stmqspi_info->saved_ccr & (1U<<OCTOSPI_DDTR));
+	if (dual || octal_dtr)
+		bank->write_start_alignment = bank->write_end_alignment = 2;
+	else
+		bank->write_start_alignment = bank->write_end_alignment = 1;
+
+	/* read and decode flash ID; returns in indirect mode */
+	retval = read_flash_id(bank, &id1, &id2);
+	LOG_DEBUG("id1 0x%06" PRIx32 ", id2 0x%06" PRIx32, id1, id2);
+	if (retval == ERROR_FLASH_BANK_NOT_PROBED) {
+		/* no id retrieved, so id must be set manually */
+		LOG_INFO("No id - set flash parameters manually");
+		retval = ERROR_OK;
+		goto err;
+	}
+
+	if (retval != ERROR_OK)
+		goto err;
+
+	/* identify flash1 */
+	for (p = flash_devices; id1 && p->name ; p++) {
+		if (p->device_id == id1) {
+			memcpy(&stmqspi_info->dev, p, sizeof(stmqspi_info->dev));
+			if (p->size_in_bytes / 4096)
+				LOG_INFO("flash1 \'%s\' id = 0x%06" PRIx32 " size = %" PRIu32
+					"kbytes", p->name, id1, p->size_in_bytes / 1024);
+			else
+				LOG_INFO("flash1 \'%s\' id = 0x%06" PRIx32 " size = %" PRIu32
+					"bytes", p->name, id1, p->size_in_bytes);
+			break;
+		}
+	}
+
+	if (id1 && !p->name) {
+		/* chip not been identified by id, then try SFDP */
+		struct flash_device temp;
+		uint32_t saved_cr = stmqspi_info->saved_cr;
+
+		/* select flash1 */
+		stmqspi_info->saved_cr = stmqspi_info->saved_cr & ~(1U<<SPI_FSEL_FLASH);
+		retval = spi_sfdp(bank, &temp, &read_sfdp_block);
+
+		/* restore saved_cr */
+		stmqspi_info->saved_cr = saved_cr;
+
+		if (retval == ERROR_OK) {
+			LOG_INFO("flash1 \'%s\' id = 0x%06" PRIx32 " size = %" PRIu32
+				"kbytes", temp.name, id1, temp.size_in_bytes / 1024);
+			/* save info and retrieved *good* id as spi_sfdp clears all info */
+			memcpy(&stmqspi_info->dev, &temp, sizeof(stmqspi_info->dev));
+			stmqspi_info->dev.device_id = id1;
+		} else {
+			/* even not identified by SFDP, then give up */
+			LOG_WARNING("Unknown flash1 device id = 0x%06" PRIx32
+				" - set flash parameters manually", id1);
+			retval = ERROR_OK;
+			goto err;
+		}
+	}
+
+	/* identify flash2 */
+	for (p = flash_devices; id2 && p->name ; p++) {
+		if (p->device_id == id2) {
+			if (p->size_in_bytes / 4096)
+				LOG_INFO("flash2 \'%s\' id = 0x%06" PRIx32 " size = %" PRIu32
+					"kbytes", p->name, id2, p->size_in_bytes / 1024);
+			else
+				LOG_INFO("flash2 \'%s\' id = 0x%06" PRIx32 " size = %" PRIu32
+					"bytes", p->name, id2, p->size_in_bytes);
+
+			if (!id1)
+				memcpy(&stmqspi_info->dev, p, sizeof(stmqspi_info->dev));
+			else {
+				if ((stmqspi_info->dev.read_cmd != p->read_cmd) ||
+					(stmqspi_info->dev.qread_cmd != p->qread_cmd) ||
+					(stmqspi_info->dev.pprog_cmd != p->pprog_cmd) ||
+					(stmqspi_info->dev.erase_cmd != p->erase_cmd) ||
+					(stmqspi_info->dev.chip_erase_cmd != p->chip_erase_cmd) ||
+					(stmqspi_info->dev.sectorsize != p->sectorsize) ||
+					(stmqspi_info->dev.size_in_bytes != p->size_in_bytes)) {
+					LOG_ERROR("Incompatible flash1/flash2 devices");
+					goto err;
+				}
+				/* page size is optional in SFDP, so accept smallest value */
+				if (p->pagesize < stmqspi_info->dev.pagesize)
+					stmqspi_info->dev.pagesize = p->pagesize;
+			}
+			break;
+		}
+	}
+
+	if (id2 && !p->name) {
+		/* chip not been identified by id, then try SFDP */
+		struct flash_device temp;
+		uint32_t saved_cr = stmqspi_info->saved_cr;
+
+		/* select flash2 */
+		stmqspi_info->saved_cr = stmqspi_info->saved_cr | (1U<<SPI_FSEL_FLASH);
+		retval = spi_sfdp(bank, &temp, &read_sfdp_block);
+
+		/* restore saved_cr */
+		stmqspi_info->saved_cr = saved_cr;
+
+		if (retval == ERROR_OK)
+			LOG_INFO("flash2 \'%s\' id = 0x%06" PRIx32 " size = %" PRIu32
+				"kbytes", temp.name, id2, temp.size_in_bytes / 1024);
+		else {
+			/* even not identified by SFDP, then give up */
+			LOG_WARNING("Unknown flash2 device id = 0x%06" PRIx32
+				" - set flash parameters manually", id2);
+			retval = ERROR_OK;
+			goto err;
+		}
+
+		if (!id1)
+			memcpy(&stmqspi_info->dev, &temp, sizeof(stmqspi_info->dev));
+		else {
+			if ((stmqspi_info->dev.read_cmd != temp.read_cmd) ||
+				(stmqspi_info->dev.qread_cmd != temp.qread_cmd) ||
+				(stmqspi_info->dev.pprog_cmd != temp.pprog_cmd) ||
+				(stmqspi_info->dev.erase_cmd != temp.erase_cmd) ||
+				(stmqspi_info->dev.chip_erase_cmd != temp.chip_erase_cmd) ||
+				(stmqspi_info->dev.sectorsize != temp.sectorsize) ||
+				(stmqspi_info->dev.size_in_bytes != temp.size_in_bytes)) {
+				LOG_ERROR("Incompatible flash1/flash2 devices");
+				goto err;
+			}
+			/* page size is optional in SFDP, so accept smallest value */
+			if (temp.pagesize < stmqspi_info->dev.pagesize)
+				stmqspi_info->dev.pagesize = temp.pagesize;
+		}
+	}
+
+	/* Set correct size value */
+	bank->size = stmqspi_info->dev.size_in_bytes << dual;
+
+	fsize = ((READ_REG(SPI_DCR)>>SPI_FSIZE_POS) & ((1U<<SPI_FSIZE_LEN) - 1));
+	if (retval != ERROR_OK)
+		goto err;
+
+	LOG_DEBUG("FSIZE = 0x%04x", fsize);
+	if (bank->size == (1U<<(fsize + 1)))
+		LOG_DEBUG("FSIZE in DCR(1) matches actual capacity. Beware of silicon bug in H7, L4+, MP1.");
+	else if (bank->size == (1U<<(fsize + 0)))
+		LOG_DEBUG("FSIZE in DCR(1) is off by one regarding actual capacity. Fix for silicon bug?");
+	else
+		LOG_ERROR("FSIZE in DCR(1) doesn't match actual capacity.");
+
+	/* if no sectors, then treat whole flash as single sector */
+	if (stmqspi_info->dev.sectorsize == 0)
+		stmqspi_info->dev.sectorsize = stmqspi_info->dev.size_in_bytes;
+	/* if no page_size, then use sectorsize as page_size */
+	if (stmqspi_info->dev.pagesize == 0)
+		stmqspi_info->dev.pagesize = stmqspi_info->dev.sectorsize;
+
+	/* create and fill sectors array */
+	bank->num_sectors = stmqspi_info->dev.size_in_bytes / stmqspi_info->dev.sectorsize;
+	sectors = malloc(sizeof(struct flash_sector) * bank->num_sectors);
+	if (sectors == NULL) {
+		LOG_ERROR("not enough memory");
+		retval = ERROR_FAIL;
+		goto err;
+	}
+
+	for (unsigned int sector = 0; sector < bank->num_sectors; sector++) {
+		sectors[sector].offset = sector * (stmqspi_info->dev.sectorsize << dual);
+		sectors[sector].size = (stmqspi_info->dev.sectorsize << dual);
+		sectors[sector].is_erased = -1;
+		sectors[sector].is_protected = 0;
+	}
+
+	bank->sectors = sectors;
+	stmqspi_info->probed = true;
+
+err:
+	/* Switch to memory mapped mode before return to prompt */
+	set_mm_mode(bank);
+
+	return retval;
+}
+
+static int stmqspi_auto_probe(struct flash_bank *bank)
+{
+	struct stmqspi_flash_bank *stmqspi_info = bank->driver_priv;
+
+	if (stmqspi_info->probed)
+		return ERROR_OK;
+	stmqspi_probe(bank);
+	return ERROR_OK;
+}
+
+static int stmqspi_protect_check(struct flash_bank *bank)
+{
+	/* Nothing to do. Protection is only handled in SW. */
+	return ERROR_OK;
+}
+
+static int get_stmqspi_info(struct flash_bank *bank, char *buf, int buf_size)
+{
+	struct stmqspi_flash_bank *stmqspi_info = bank->driver_priv;
+
+	if (!(stmqspi_info->probed)) {
+		snprintf(buf, buf_size,
+			"\nQSPI flash bank not probed yet\n");
+		return ERROR_FLASH_BANK_NOT_PROBED;
+	}
+
+	snprintf(buf, buf_size, "flash%s%s \'%s\', device id = 0x%06" PRIx32
+			", flash size = %" PRIu32 "%sbytes\n(page size = %" PRIu32
+			", read = 0x%02" PRIx8 ", qread = 0x%02" PRIx8
+			", pprog = 0x%02" PRIx8 ", mass_erase = 0x%02" PRIx8
+			", sector size = %" PRIu32 "%sbytes, sector_erase = 0x%02" PRIx8 ")",
+			((stmqspi_info->saved_cr & ((1U<<SPI_DUAL_FLASH) |
+			(1U<<SPI_FSEL_FLASH))) != (1U<<SPI_FSEL_FLASH)) ? "1" : "",
+			((stmqspi_info->saved_cr & ((1U<<SPI_DUAL_FLASH) |
+			(1U<<SPI_FSEL_FLASH))) != (0U<<SPI_FSEL_FLASH)) ? "2" : "",
+			stmqspi_info->dev.name, stmqspi_info->dev.device_id,
+			bank->size / 4096 ? bank->size / 1024 : bank->size,
+			bank->size / 4096 ? "k" : "", stmqspi_info->dev.pagesize,
+			stmqspi_info->dev.read_cmd, stmqspi_info->dev.qread_cmd,
+			stmqspi_info->dev.pprog_cmd, stmqspi_info->dev.chip_erase_cmd,
+			stmqspi_info->dev.sectorsize / 4096 ?
+				stmqspi_info->dev.sectorsize / 1024 : stmqspi_info->dev.sectorsize,
+			stmqspi_info->dev.sectorsize / 4096 ? "k" : "",
+			stmqspi_info->dev.erase_cmd);
+
+	return ERROR_OK;
+}
+
+static const struct command_registration stmqspi_exec_command_handlers[] = {
+	{
+		.name = "mass_erase",
+		.handler = stmqspi_handle_mass_erase_command,
+		.mode = COMMAND_EXEC,
+		.usage = "bank_id",
+		.help = "Mass erase entire flash device.",
+	},
+	{
+		.name = "set",
+		.handler = stmqspi_handle_set,
+		.mode = COMMAND_EXEC,
+		.usage = "bank_id name chip_size page_size read_cmd qread_cmd pprg_cmd "
+			"[ mass_erase_cmd ] [ sector_size sector_erase_cmd ]",
+		.help = "Set params of single flash chip",
+	},
+	{
+		.name = "cmd",
+		.handler = stmqspi_handle_cmd,
+		.mode = COMMAND_EXEC,
+		.usage = "bank_id num_resp cmd_byte ...",
+		.help = "Send low-level command cmd_byte and following bytes or read num_resp.",
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+static const struct command_registration stmqspi_command_handlers[] = {
+	{
+		.name = "stmqspi",
+		.mode = COMMAND_ANY,
+		.help = "stmqspi flash command group",
+		.usage = "",
+		.chain = stmqspi_exec_command_handlers,
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+struct flash_driver stmqspi_flash = {
+	.name = "stmqspi",
+	.commands = stmqspi_command_handlers,
+	.flash_bank_command = stmqspi_flash_bank_command,
+	.erase = stmqspi_erase,
+	.protect = stmqspi_protect,
+	.write = stmqspi_write,
+	.read = stmqspi_read,
+	.verify = stmqspi_verify,
+	.probe = stmqspi_probe,
+	.auto_probe = stmqspi_auto_probe,
+	.erase_check = stmqspi_blank_check,
+	.protect_check = stmqspi_protect_check,
+	.info = get_stmqspi_info,
+	.free_driver_priv = default_flash_free_driver_priv,
+};
diff --git a/src/flash/nor/stmqspi.h b/src/flash/nor/stmqspi.h
new file mode 100644
index 0000000..d8510ab
--- /dev/null
+++ b/src/flash/nor/stmqspi.h
@@ -0,0 +1,125 @@
+/***************************************************************************
+ *   Copyright (C) 2016 - 2018 by Andreas Bolsch                           *
+ *   andreas.bolsch@mni.thm.de                                             *
+ *                                                                         *
+ *   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     *
+ *   (at your option) 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.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
+ ***************************************************************************/
+
+#ifndef OPENOCD_FLASH_NOR_STMQSPI_H
+#define OPENOCD_FLASH_NOR_STMQSPI_H
+
+#include "spi.h"
+
+/* QSPI register offsets */
+#define QSPI_CR			(0x00)	/* Control register */
+#define QSPI_DCR		(0x04)	/* Device configuration register */
+#define QSPI_SR			(0x08)	/* Status register */
+#define QSPI_FCR		(0x0C)	/* Flag clear register */
+#define QSPI_DLR		(0x10)	/* Data length register */
+#define QSPI_CCR		(0x14)	/* Communication configuration register */
+#define QSPI_AR			(0x18)	/* Address register */
+#define QSPI_ABR		(0x1C)	/* Alternate bytes register */
+#define QSPI_DR			(0x20)	/* Data register */
+
+/* common bits in QSPI_CR and OCTOSPI_CR */
+#define SPI_FSEL_FLASH	7		/* Select flash 2 */
+#define SPI_DUAL_FLASH	6		/* Dual flash mode */
+#define SPI_ABORT		1		/* Abort bit */
+
+/* common bits in QSPI_DCR and OCTOSPI_DCR1 */
+#define SPI_FSIZE_POS	16		/* bit position of FSIZE */
+#define SPI_FSIZE_LEN	5		/* width of FSIZE field */
+
+/* common bits in QSPI_SR/FCR and OCTOSPI_SR/FCR */
+#define SPI_BUSY		5		/* Busy flag */
+#define SPI_FTF			2		/* FIFO threshold flag */
+#define SPI_TCF			1		/* Transfer complete flag */
+
+/* fields in QSPI_CCR */
+#define QSPI_DDRM			31					/* position of DDRM bit */
+#define SPI_DMODE_POS		24					/* bit position of DMODE */
+#define QSPI_DCYC_POS		18					/* bit position of DCYC */
+#define QSPI_DCYC_LEN		5					/* width of DCYC field */
+#define QSPI_DCYC_MASK		(((1U<<QSPI_DCYC_LEN) - 1)<<QSPI_DCYC_POS)
+#define SPI_ADSIZE_POS		12					/* bit position of ADSIZE */
+
+#define QSPI_WRITE_MODE		0x00000000U			/* indirect write mode */
+#define QSPI_READ_MODE		0x04000000U			/* indirect read mode */
+#define QSPI_MM_MODE		0x0C000000U			/* memory mapped mode */
+#define QSPI_ALTB_MODE		0x0003C000U			/* alternate byte mode */
+#define QSPI_4LINE_MODE		0x03000F00U			/* 4 lines for data, addr, instr */
+#define QSPI_NO_DATA		(~0x03000000U)		/* no data */
+#define QSPI_NO_ALTB		(~QSPI_ALTB_MODE)	/* no alternate */
+#define QSPI_NO_ADDR		(~0x00000C00U)		/* no address */
+#define QSPI_ADDR3			(0x2U<<SPI_ADSIZE_POS)	/* 3 byte address */
+#define QSPI_ADDR4			(0x3U<<SPI_ADSIZE_POS)	/* 4 byte address */
+
+/* OCTOSPI register offsets */
+#define OCTOSPI_CR		(0x000)	/* Control register */
+#define OCTOSPI_DCR1	(0x008)	/* Device configuration register 1 */
+#define OCTOSPI_DCR2	(0x00C)	/* Device configuration register 2 */
+#define OCTOSPI_DCR3	(0x010)	/* Device configuration register 3 */
+#define	OCTOSPI_SR		(0x020)	/* Status register */
+#define OCTOSPI_FCR		(0x024)	/* Flag clear register */
+#define OCTOSPI_DLR		(0x040)	/* Data length register */
+#define OCTOSPI_AR		(0x048)	/* Address register */
+#define OCTOSPI_DR		(0x050)	/* Data register */
+#define OCTOSPI_CCR		(0x100)	/* Communication configuration register */
+#define OCTOSPI_TCR		(0x108)	/* Timing configuration register */
+#define OCTOSPI_IR		(0x110)	/* Instruction register */
+#define OCTOSPI_WCCR	(0x180)	/* Write communication configuration register */
+#define OCTOSPI_WIR		(0x190)	/* Write instruction register */
+#define OCTOSPI_MAGIC	(0x3FC)	/* Magic ID register, deleted from RM, why? */
+
+#define OCTO_MAGIC_ID	0xA3C5DD01	/* Magic ID, deleted from RM, why? */
+
+/* additional bits in OCTOSPI_CR */
+#define OCTOSPI_WRITE_MODE	0x00000000U			/* indirect write mode */
+#define OCTOSPI_READ_MODE	0x10000000U			/* indirect read mode */
+#define OCTOSPI_MM_MODE		0x30000000U			/* memory mapped mode */
+
+/* additional fields in OCTOSPI_DCR1 */
+#define OCTOSPI_MTYP_POS	(24)				/* bit position of MTYP */
+#define OCTOSPI_MTYP_LEN	(3)					/* width of MTYP field */
+#define OCTOSPI_MTYP_MASK	(((1U<<OCTOSPI_MTYP_LEN) - 1)<<OCTOSPI_MTYP_POS)
+
+/* fields in OCTOSPI_CCR */
+#define OCTOSPI_ALTB_MODE	0x001F0000U				/* alternate byte mode */
+#define OCTOSPI_8LINE_MODE	0x0F003F3FU				/* 8 lines DTR for data, addr, instr */
+#define OCTOSPI_NO_DATA		(~0x0F000000U)			/* no data */
+#define OCTOSPI_NO_ALTB		(~OCTOSPI_ALTB_MODE)	/* no alternate */
+#define OCTOSPI_NO_ADDR		(~0x00000F00U)			/* no address */
+#define OCTOSPI_ADDR3		(0x2U<<SPI_ADSIZE_POS)	/* 3 byte address */
+#define OCTOSPI_ADDR4		(0x3U<<SPI_ADSIZE_POS)	/* 4 byte address */
+#define OCTOSPI_DQSEN		29						/* DQS enable */
+#define OCTOSPI_DDTR		27						/* DTR for data */
+#define OCTOSPI_NO_DDTR		(~(1U<<OCTOSPI_DDTR))	/* no DTR for data, but maybe still DQS */
+#define OCTOSPI_ISIZE_MASK	(0x30)					/* ISIZE field */
+
+/* fields in OCTOSPI_TCR */
+#define OCTOSPI_DCYC_POS	0					/* bit position of DCYC */
+#define OCTOSPI_DCYC_LEN	5					/* width of DCYC field */
+#define OCTOSPI_DCYC_MASK	(((1U<<OCTOSPI_DCYC_LEN) - 1)<<OCTOSPI_DCYC_POS)
+
+#define IS_OCTOSPI			(stmqspi_info->octo)
+#define SPI_CR				(IS_OCTOSPI ? OCTOSPI_CR : QSPI_CR)
+#define SPI_DCR				(IS_OCTOSPI ? OCTOSPI_DCR1 : QSPI_DCR)
+#define	SPI_SR				(IS_OCTOSPI ? OCTOSPI_SR : QSPI_SR)
+#define SPI_FCR				(IS_OCTOSPI ? OCTOSPI_FCR : QSPI_FCR)
+#define SPI_DLR				(IS_OCTOSPI ? OCTOSPI_DLR : QSPI_DLR)
+#define SPI_AR				(IS_OCTOSPI ? OCTOSPI_AR : QSPI_AR)
+#define SPI_DR				(IS_OCTOSPI ? OCTOSPI_DR : QSPI_DR)
+#define SPI_CCR				(IS_OCTOSPI ? OCTOSPI_CCR : QSPI_CCR)
+
+#endif /* OPENOCD_FLASH_NOR_STMQSPI_H */
diff --git a/src/flash/nor/stmsmi.c b/src/flash/nor/stmsmi.c
index 278c73e..e73dd22 100644
--- a/src/flash/nor/stmsmi.c
+++ b/src/flash/nor/stmsmi.c
@@ -44,31 +44,31 @@
 #define SMI_READ_REG(a) (_SMI_READ_REG(a))
 #define _SMI_READ_REG(a)			\
 {									\
-	int __a;						\
-	uint32_t __v;					\
+	int _ret;						\
+	uint32_t _value;				\
 									\
-	__a = target_read_u32(target, io_base + (a), &__v); \
-	if (__a != ERROR_OK)			\
-		return __a;					\
-	__v;							\
+	_ret = target_read_u32(target, io_base + (a), &_value); \
+	if (_ret != ERROR_OK)			\
+		return _ret;				\
+	_value;							\
 }
 
 #define SMI_WRITE_REG(a, v)			\
 {									\
-	int __r;						\
+	int _retval;					\
 									\
-	__r = target_write_u32(target, io_base + (a), (v)); \
-	if (__r != ERROR_OK)			\
-		return __r;					\
+	_retval = target_write_u32(target, io_base + (a), (v)); \
+	if (_retval != ERROR_OK)		\
+		return _retval;				\
 }
 
 #define SMI_POLL_TFF(timeout)		\
 {									\
-	int __r;						\
+	int _retval;					\
 									\
-	__r = poll_tff(target, io_base, timeout); \
-	if (__r != ERROR_OK)			\
-		return __r;					\
+	_retval = poll_tff(target, io_base, timeout); \
+	if (_retval != ERROR_OK)		\
+		return _retval;				\
 }
 
 #define SMI_SET_SW_MODE()	SMI_WRITE_REG(SMI_CR1, \
diff --git a/src/flash/nor/tcl.c b/src/flash/nor/tcl.c
index 87c8ced..66b9a4c 100644
--- a/src/flash/nor/tcl.c
+++ b/src/flash/nor/tcl.c
@@ -454,7 +454,8 @@ COMMAND_HANDLER(handle_flash_write_image_command)
 	if (retval != ERROR_OK)
 		return retval;
 
-	retval = flash_write_unlock(target, &image, &written, auto_erase, auto_unlock);
+	retval = flash_write_unlock_verify(target, &image, &written, auto_erase,
+		auto_unlock, true, false);
 	if (retval != ERROR_OK) {
 		image_close(&image);
 		return retval;
@@ -471,6 +472,58 @@ COMMAND_HANDLER(handle_flash_write_image_command)
 	return retval;
 }
 
+COMMAND_HANDLER(handle_flash_verify_image_command)
+{
+	struct target *target = get_current_target(CMD_CTX);
+
+	struct image image;
+	uint32_t verified;
+
+	int retval;
+
+	if (CMD_ARGC < 1)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	if (!target) {
+		LOG_ERROR("no target selected");
+		return ERROR_FAIL;
+	}
+
+	struct duration bench;
+	duration_start(&bench);
+
+	if (CMD_ARGC >= 2) {
+		image.base_address_set = 1;
+		COMMAND_PARSE_NUMBER(llong, CMD_ARGV[1], image.base_address);
+	} else {
+		image.base_address_set = 0;
+		image.base_address = 0x0;
+	}
+
+	image.start_address_set = 0;
+
+	retval = image_open(&image, CMD_ARGV[0], (CMD_ARGC == 3) ? CMD_ARGV[2] : NULL);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = flash_write_unlock_verify(target, &image, &verified, false,
+		false, false, true);
+	if (retval != ERROR_OK) {
+		image_close(&image);
+		return retval;
+	}
+
+	if ((ERROR_OK == retval) && (duration_measure(&bench) == ERROR_OK)) {
+		command_print(CMD, "verified %" PRIu32 " bytes from file %s "
+			"in %fs (%0.3f KiB/s)", verified, CMD_ARGV[0],
+			duration_elapsed(&bench), duration_kbps(&bench, verified));
+	}
+
+	image_close(&image);
+
+	return retval;
+}
+
 COMMAND_HANDLER(handle_flash_fill_command)
 {
 	target_addr_t address;
@@ -1142,7 +1195,15 @@ static const struct command_registration flash_exec_command_handlers[] = {
 		.mode = COMMAND_EXEC,
 		.usage = "[erase] [unlock] filename [offset [file_type]]",
 		.help = "Write an image to flash.  Optionally first unprotect "
-			"and/or erase the region to be used.  Allow optional "
+			"and/or erase the region to be used. Allow optional "
+			"offset from beginning of bank (defaults to zero)",
+	},
+	{
+		.name = "verify_image",
+		.handler = handle_flash_verify_image_command,
+		.mode = COMMAND_EXEC,
+		.usage = "filename [offset [file_type]]",
+		.help = "Verify an image against flash. Allow optional "
 			"offset from beginning of bank (defaults to zero)",
 	},
 	{
diff --git a/src/target/image.c b/src/target/image.c
index 0b7deba..20c6d77 100644
--- a/src/target/image.c
+++ b/src/target/image.c
@@ -1019,7 +1019,7 @@ void image_close(struct image *image)
 	image->sections = NULL;
 }
 
-int image_calculate_checksum(uint8_t *buffer, uint32_t nbytes, uint32_t *checksum)
+int image_calculate_checksum(const uint8_t *buffer, uint32_t nbytes, uint32_t *checksum)
 {
 	uint32_t crc = 0xffffffff;
 	LOG_DEBUG("Calculating checksum");
diff --git a/src/target/image.h b/src/target/image.h
index 765d290..53c27d8 100644
--- a/src/target/image.h
+++ b/src/target/image.h
@@ -99,7 +99,7 @@ void image_close(struct image *image);
 int image_add_section(struct image *image, uint32_t base, uint32_t size,
 		int flags, uint8_t const *data);
 
-int image_calculate_checksum(uint8_t *buffer, uint32_t nbytes,
+int image_calculate_checksum(const uint8_t *buffer, uint32_t nbytes,
 		uint32_t *checksum);
 
 #define ERROR_IMAGE_FORMAT_ERROR	(-1400)
diff --git a/src/target/target.c b/src/target/target.c
index 3b1c666..db759d9 100644
--- a/src/target/target.c
+++ b/src/target/target.c
@@ -1031,11 +1031,11 @@ int target_run_flash_async_algorithm(struct target *target,
 			 * programming. The exact delay shouldn't matter as long as it's
 			 * less than buffer size / flash speed. This is very unlikely to
 			 * run when using high latency connections such as USB. */
-			alive_sleep(10);
+			alive_sleep(2);
 
 			/* to stop an infinite loop on some targets check and increment a timeout
 			 * this issue was observed on a stellaris using the new ICDI interface */
-			if (timeout++ >= 500) {
+			if (timeout++ >= 2500) {
 				LOG_ERROR("timeout waiting for algorithm, a target reset is recommended");
 				return ERROR_FLASH_OPERATION_FAILED;
 			}
@@ -1049,6 +1049,10 @@ int target_run_flash_async_algorithm(struct target *target,
 		if (thisrun_bytes > count * block_size)
 			thisrun_bytes = count * block_size;
 
+		/* Force end of large blocks to be word aligned */
+		if (thisrun_bytes >= 16)
+			thisrun_bytes -= (rp + thisrun_bytes) & 0x03;
+
 		/* Write data to fifo */
 		retval = target_write_buffer(target, wp, thisrun_bytes, buffer);
 		if (retval != ERROR_OK)
@@ -1098,6 +1102,156 @@ int target_run_flash_async_algorithm(struct target *target,
 	return retval;
 }
 
+int target_run_read_async_algorithm(struct target *target,
+		uint8_t *buffer, uint32_t count, int block_size,
+		int num_mem_params, struct mem_param *mem_params,
+		int num_reg_params, struct reg_param *reg_params,
+		uint32_t buffer_start, uint32_t buffer_size,
+		uint32_t entry_point, uint32_t exit_point, void *arch_info)
+{
+	int retval;
+	int timeout = 0;
+
+	const uint8_t *buffer_orig = buffer;
+
+	/* Set up working area. First word is write pointer, second word is read pointer,
+	 * rest is fifo data area. */
+	uint32_t wp_addr = buffer_start;
+	uint32_t rp_addr = buffer_start + 4;
+	uint32_t fifo_start_addr = buffer_start + 8;
+	uint32_t fifo_end_addr = buffer_start + buffer_size;
+
+	uint32_t wp = fifo_start_addr;
+	uint32_t rp = fifo_start_addr;
+
+	/* validate block_size is 2^n */
+	assert(!block_size || !(block_size & (block_size - 1)));
+
+	retval = target_write_u32(target, wp_addr, wp);
+	if (retval != ERROR_OK)
+		return retval;
+	retval = target_write_u32(target, rp_addr, rp);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Start up algorithm on target */
+	retval = target_start_algorithm(target, num_mem_params, mem_params,
+			num_reg_params, reg_params,
+			entry_point,
+			exit_point,
+			arch_info);
+
+	if (retval != ERROR_OK) {
+		LOG_ERROR("error starting target flash read algorithm");
+		return retval;
+	}
+
+	while (count > 0) {
+		retval = target_read_u32(target, wp_addr, &wp);
+		if (retval != ERROR_OK) {
+			LOG_ERROR("failed to get write pointer");
+			break;
+		}
+
+		LOG_DEBUG("offs 0x%zx count 0x%" PRIx32 " wp 0x%" PRIx32 " rp 0x%" PRIx32,
+			(size_t) (buffer - buffer_orig), count, wp, rp);
+
+		if (wp == 0) {
+			LOG_ERROR("flash read algorithm aborted by target");
+			retval = ERROR_FLASH_OPERATION_FAILED;
+			break;
+		}
+
+		if (((wp - fifo_start_addr) & (block_size - 1)) || wp < fifo_start_addr || wp >= fifo_end_addr) {
+			LOG_ERROR("corrupted fifo write pointer 0x%" PRIx32, wp);
+			break;
+		}
+
+		/* Count the number of bytes available in the fifo without
+		 * crossing the wrap around. */
+		uint32_t thisrun_bytes;
+		if (wp >= rp)
+			thisrun_bytes = wp - rp;
+		else
+			thisrun_bytes = fifo_end_addr - rp;
+
+		if (thisrun_bytes == 0) {
+			/* Throttle polling a bit if transfer is (much) faster than flash
+			 * reading. The exact delay shouldn't matter as long as it's
+			 * less than buffer size / flash speed. This is very unlikely to
+			 * run when using high latency connections such as USB. */
+			alive_sleep(2);
+
+			/* to stop an infinite loop on some targets check and increment a timeout
+			 * this issue was observed on a stellaris using the new ICDI interface */
+			if (timeout++ >= 2500) {
+				LOG_ERROR("timeout waiting for algorithm, a target reset is recommended");
+				return ERROR_FLASH_OPERATION_FAILED;
+			}
+			continue;
+		}
+
+		/* Reset our timeout */
+		timeout = 0;
+
+		/* Limit to the amount of data we actually want to read */
+		if (thisrun_bytes > count * block_size)
+			thisrun_bytes = count * block_size;
+
+		/* Force end of large blocks to be word aligned */
+		if (thisrun_bytes >= 16)
+			thisrun_bytes -= (rp + thisrun_bytes) & 0x03;
+
+		/* Read data from fifo */
+		retval = target_read_buffer(target, rp, thisrun_bytes, buffer);
+		if (retval != ERROR_OK)
+			break;
+
+		/* Update counters and wrap write pointer */
+		buffer += thisrun_bytes;
+		count -= thisrun_bytes / block_size;
+		rp += thisrun_bytes;
+		if (rp >= fifo_end_addr)
+			rp = fifo_start_addr;
+
+		/* Store updated write pointer to target */
+		retval = target_write_u32(target, rp_addr, rp);
+		if (retval != ERROR_OK)
+			break;
+
+		/* Avoid GDB timeouts */
+		keep_alive();
+
+	}
+
+	if (retval != ERROR_OK) {
+		/* abort flash write algorithm on target */
+		target_write_u32(target, rp_addr, 0);
+	}
+
+	int retval2 = target_wait_algorithm(target, num_mem_params, mem_params,
+			num_reg_params, reg_params,
+			exit_point,
+			10000,
+			arch_info);
+
+	if (retval2 != ERROR_OK) {
+		LOG_ERROR("error waiting for target flash write algorithm");
+		retval = retval2;
+	}
+
+	if (retval == ERROR_OK) {
+		/* check if algorithm set wp = 0 after fifo writer loop finished */
+		retval = target_read_u32(target, wp_addr, &wp);
+		if (retval == ERROR_OK && wp == 0) {
+			LOG_ERROR("flash read algorithm aborted by target");
+			retval = ERROR_FLASH_OPERATION_FAILED;
+		}
+	}
+
+	return retval;
+}
+
 int target_read_memory(struct target *target,
 		target_addr_t address, uint32_t size, uint32_t count, uint8_t *buffer)
 {
diff --git a/src/target/target.h b/src/target/target.h
index ee0bdfb..44463b7 100644
--- a/src/target/target.h
+++ b/src/target/target.h
@@ -578,6 +578,18 @@ int target_run_flash_async_algorithm(struct target *target,
 		void *arch_info);
 
 /**
+ * This routine is a wrapper for asynchronous algorithms.
+ *
+ */
+int target_run_read_async_algorithm(struct target *target,
+		uint8_t *buffer, uint32_t count, int block_size,
+		int num_mem_params, struct mem_param *mem_params,
+		int num_reg_params, struct reg_param *reg_params,
+		uint32_t buffer_start, uint32_t buffer_size,
+		uint32_t entry_point, uint32_t exit_point,
+		void *arch_info);
+
+/**
  * Read @a count items of @a size bytes from the memory of @a target at
  * the @a address given.
  *