dev-embedded/openocd/files/openocd-0.10.0-add-stm32h7x-driver-support.patch

From 5aceb7b2b44be4e135221afe0f5e04d47ab560c8 Mon Sep 17 00:00:00 2001
From: Alexandre TORGUE <alexandre.torgue@st.com>
Date: Mon, 17 Jul 2017 13:56:18 +0200
Subject: [PATCH 1/2] flash: Add new stm32h7x driver support
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Add basic support for:
     -STM32H7x (Embedded flash 2M)

Erase and write tested on stm32h743.

Change-Id: Ie8d8786227cdeee39fcf5663167a053ad8dcef4c
Signed-off-by: Rémi Prud'homme <remi.prudhomme@st.com>
Signed-off-by: Alexandre TORGUE <alexandre.torgue@st.com>
---
 doc/openocd.texi          |   27 +
 Makefile.in               |    5 ++++-
 src/flash/nor/Makefile.am |    1 +
 src/flash/nor/drivers.c   |    2 +
 src/flash/nor/stm32h7x.c  | 1214 +++++++++++++++++++++++++++++++++++++++++++++
 5 files changed, 1248 insertions(+), 1 deletion(-)
 create mode 100644 src/flash/nor/stm32h7x.c

diff --git a/doc/openocd.texi b/doc/openocd.texi
index 45b341c4..4c85f389 100644
--- a/doc/openocd.texi
+++ b/doc/openocd.texi
@@ -5868,6 +5868,33 @@ two halfwords (of FLASH_OPTCR1).
 @end deffn
 @end deffn
 
+@deffn {Flash Driver} stm32h7x
+All members of the STM32H7 microcontroller families from ST Microelectronics
+include internal flash and use ARM Cortex-M7 core.
+The driver automatically recognizes a number of these chips using
+the chip identification register, and autoconfigures itself.
+
+Note that some devices have been found that have a flash size register that contains
+an invalid value, to workaround this issue you can override the probed value used by
+the flash driver.
+
+@example
+flash bank $_FLASHNAME stm32h7x 0 0x20000 0 0 $_TARGETNAME
+@end example
+
+Some stm32h7x-specific commands are defined:
+
+@deffn Command {stm32h7x lock} num
+Locks the entire stm32 device.
+The @var{num} parameter is a value shown by @command{flash banks}.
+@end deffn
+
+@deffn Command {stm32h7x unlock} num
+Unlocks the entire stm32 device.
+The @var{num} parameter is a value shown by @command{flash banks}.
+@end deffn
+@end deffn
+
 @deffn {Flash Driver} stm32lx
 All members of the STM32L microcontroller families from ST Microelectronics
 include internal flash and use ARM Cortex-M3 and Cortex-M0+ cores.
diff --git a/src/flash/nor/Makefile.am b/src/flash/nor/Makefile.am
index 727e4f2f..22999006 100644
--- a/src/flash/nor/Makefile.am
+++ b/src/flash/nor/Makefile.am
@@ -48,6 +48,7 @@ NOR_DRIVERS = \
 	%D%/stm32f2x.c \
 	%D%/stm32lx.c \
 	%D%/stm32l4x.c \
+	%D%/stm32h7x.c \
 	%D%/str7x.c \
 	%D%/str9x.c \
 	%D%/str9xpec.c \
diff --git a/Makefile.in b/Makefile.in
index 3c393aa..74b684f 100644
--- a/Makefile.in
+++ b/Makefile.in
@@ -251,7 +251,7 @@ am__objects_3 = src/flash/nor/aduc702x.lo src/flash/nor/aducm360.lo \
 	src/flash/nor/psoc4.lo src/flash/nor/sim3x.lo \
 	src/flash/nor/spi.lo src/flash/nor/stmsmi.lo \
 	src/flash/nor/stellaris.lo src/flash/nor/stm32f1x.lo \
-	src/flash/nor/stm32f2x.lo src/flash/nor/stm32lx.lo \
+	src/flash/nor/stm32f2x.lo src/flash/nor/stm32h7x.lo src/flash/nor/stm32lx.lo \
 	src/flash/nor/stm32l4x.lo src/flash/nor/str7x.lo \
 	src/flash/nor/str9x.lo src/flash/nor/str9xpec.lo \
 	src/flash/nor/tms470.lo src/flash/nor/virtual.lo \
@@ -1504,6 +1504,7 @@ NOR_DRIVERS = \
 	src/flash/nor/stellaris.c \
 	src/flash/nor/stm32f1x.c \
 	src/flash/nor/stm32f2x.c \
+	src/flash/nor/stm32h7x.c \
 	src/flash/nor/stm32lx.c \
 	src/flash/nor/stm32l4x.c \
 	src/flash/nor/str7x.c \
@@ -1793,6 +1794,8 @@ src/flash/nor/stm32f1x.lo: src/flash/nor/$(am__dirstamp) \
 	src/flash/nor/$(DEPDIR)/$(am__dirstamp)
 src/flash/nor/stm32f2x.lo: src/flash/nor/$(am__dirstamp) \
 	src/flash/nor/$(DEPDIR)/$(am__dirstamp)
+src/flash/nor/stm32h7x.lo: src/flash/nor/$(am__dirstamp) \
+	src/flash/nor/$(DEPDIR)/$(am__dirstamp)
 src/flash/nor/stm32lx.lo: src/flash/nor/$(am__dirstamp) \
 	src/flash/nor/$(DEPDIR)/$(am__dirstamp)
 src/flash/nor/stm32l4x.lo: src/flash/nor/$(am__dirstamp) \
diff --git a/src/flash/nor/drivers.c b/src/flash/nor/drivers.c
index 56a5cb24..b9bc7323 100644
--- a/src/flash/nor/drivers.c
+++ b/src/flash/nor/drivers.c
@@ -58,6 +58,7 @@ extern struct flash_driver stm32f1x_flash;
 extern struct flash_driver stm32f2x_flash;
 extern struct flash_driver stm32lx_flash;
 extern struct flash_driver stm32l4x_flash;
+extern struct flash_driver stm32h7x_flash;
 extern struct flash_driver stmsmi_flash;
 extern struct flash_driver str7x_flash;
 extern struct flash_driver str9x_flash;
@@ -110,6 +111,7 @@ static struct flash_driver *flash_drivers[] = {
 	&stm32f2x_flash,
 	&stm32lx_flash,
 	&stm32l4x_flash,
+	&stm32h7x_flash,
 	&stmsmi_flash,
 	&str7x_flash,
 	&str9x_flash,
diff --git a/src/flash/nor/stm32h7x.c b/src/flash/nor/stm32h7x.c
new file mode 100644
index 00000000..2298e4d0
--- /dev/null
+++ b/src/flash/nor/stm32h7x.c
@@ -0,0 +1,1214 @@
+/***************************************************************************
+ *                                                                         *
+ *   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.                          *
+ *                                                                         *
+ ***************************************************************************/
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "imp.h"
+#include <helper/binarybuffer.h>
+#include <target/algorithm.h>
+#include <target/armv7m.h>
+
+/* Erase time can be as high as 1000ms, 10x this and it's toast... */
+#define FLASH_ERASE_TIMEOUT 10000
+#define FLASH_WRITE_TIMEOUT 5
+
+/* RM 399 */
+#define FLASH_ACR       0x00
+#define FLASH_KEYR      0x04
+#define FLASH_OPTKEYR   0x08
+#define FLASH_CR        0x0C
+#define FLASH_SR        0x10
+#define FLASH_CCR       0x14
+#define FLASH_OPTCR     0x18
+#define FLASH_OPTCUR    0x1C
+#define FLASH_OPTPRG    0x20
+#define FLASH_OPTCCR    0x24
+#define FLASH_WPSNCUR   0x38
+#define FLASH_WPSNPRG   0x3C
+
+/* FLASH_CR register bits */
+#define FLASH_LOCK     (1 << 0)
+#define FLASH_PG       (1 << 1)
+#define FLASH_SER      (1 << 2)
+#define FLASH_BER_CMD  (1 << 3)
+#define FLASH_PSIZE_8  (0 << 4)
+#define FLASH_PSIZE_16 (1 << 4)
+#define FLASH_PSIZE_32 (2 << 4)
+#define FLASH_PSIZE_64 (3 << 4)
+#define FLASH_FW       (1 << 6)
+#define FLASH_START    (1 << 7)
+
+#define FLASH_SNB(a)   ((((a) >= 8) ? (((a) - 8) << 8) : (a)) << 8)
+
+/* FLASH_SR register bits */
+#define FLASH_BSY      (1 << 0)  /* Operation in progress */
+#define FLASH_EOP      (1 << 16) /* End of OPeration */
+#define FLASH_WRPERR   (1 << 17) /* Write protection error */
+#define FLASH_PGSERR   (1 << 18) /* Programming sequence error */
+#define FLASH_STRBERR  (1 << 19) /* Strobe error */
+#define FLASH_INCERR   (1 << 21) /* Increment error */
+#define FLASH_OPERR    (1 << 22) /* Operation error */
+#define FLASH_RDPERR   (1 << 23) /* Read Protection error */
+#define FLASH_RDSERR   (1 << 24) /* Secure Protection error */
+#define FLASH_SNECCERR (1 << 25) /* Single ECC error */
+#define FLASH_DBECCERR (1 << 26) /* Double ECC error */
+
+#define FLASH_ERROR (FLASH_WRPERR | FLASH_PGSERR | FLASH_STRBERR | FLASH_INCERR | FLASH_OPERR | \
+					 FLASH_RDPERR | FLASH_RDSERR | FLASH_SNECCERR | FLASH_DBECCERR)
+
+/* FLASH_OPTCR register bits */
+#define OPT_LOCK       (1 << 0)
+#define OPT_START      (1 << 1)
+
+/* FLASH_OPTCUR bit definitions (reading) */
+#define OPT_BUSY       (1 << 0)
+#define OPT_BOR0       (1 << 2)
+#define OPT_BOR1       (1 << 3)
+#define IDWG1_SW       (1 << 4)
+#define IDWG2_SW       (1 << 5)
+#define RSTSTOP_D1     (1 << 6)
+#define RSTSTBY_D1     (1 << 7)
+#define FZ_IWDGSTOP    (1 << 16)
+#define FZ_IWDGSTBY    (1 << 17)
+#define RSTSTOP_D2     (1 << 24)
+#define RSTSTBY_D2     (1 << 25)
+#define CHANGE_ERR     (1 << 30)
+#define SWAP_BANK      (1 << 31)
+
+/* register unlock keys */
+#define KEY1           0x45670123
+#define KEY2           0xCDEF89AB
+
+/* option register unlock key */
+#define OPTKEY1        0x08192A3B
+#define OPTKEY2        0x4C5D6E7F
+
+#define DBGMCU_IDCODE_REGISTER  0x5C001000
+#define FLASH_BANK0_ADDRESS     0x08000000
+#define FLASH_BANK1_ADDRESS     0x08100000
+#define FLASH_REG_BASE_B0       0x52002000
+#define FLASH_REG_BASE_B1       0x52002100
+
+#define FLASH_BLOCK_SIZE        32
+
+struct stm32h7x_rev {
+	uint16_t rev;
+	const char *str;
+};
+
+struct stm32x_options {
+	uint8_t RDP;
+	uint32_t protection;  /* bank1 WRP */
+	uint32_t protection2; /* bank2 WRP */
+	uint8_t user_options;
+	uint8_t user2_options;
+	uint8_t user3_options;
+	uint8_t independent_watchdog_selection;
+	uint8_t independent_watchdog_selection2;
+};
+
+struct stm32h7x_part_info {
+	uint16_t id;
+	const char *device_str;
+	const struct stm32h7x_rev *revs;
+	size_t num_revs;
+	unsigned int page_size;
+	unsigned int pages_per_sector;
+	uint16_t max_flash_size_kb;
+	uint8_t has_dual_bank;
+	uint16_t first_bank_size_kb; /* Used when has_dual_bank is true */
+	uint32_t flash_base;         /* Flash controller registers location */
+	uint32_t fsize_base;         /* Location of FSIZE register */
+};
+
+struct stm32h7x_flash_bank {
+	int probed;
+	uint32_t idcode;
+	uint32_t user_bank_size;
+	uint32_t flash_base;    /* Address of flash reg controller */
+	struct stm32x_options option_bytes;
+	const struct stm32h7x_part_info *part_info;
+};
+
+static const struct stm32h7x_rev stm32_450_revs[] = {
+	{ 0x1000, "A" }, { 0x1001, "Z" }, { 0x1003, "Y" },
+};
+
+static const struct stm32h7x_part_info stm32h7x_parts[] = {
+	{
+		.id					= 0x450,
+		.revs				= stm32_450_revs,
+		.num_revs			= ARRAY_SIZE(stm32_450_revs),
+		.device_str			= "STM32H7xx 2M",
+		.page_size			= 128,  /* 128 KB */
+		.max_flash_size_kb	= 2048,
+		.first_bank_size_kb	= 1024,
+		.has_dual_bank		= 1,
+		.flash_base			= 0x52002000,
+		.fsize_base			= 0x1FF1E880,
+	},
+};
+
+static int stm32x_unlock_reg(struct flash_bank *bank);
+static int stm32x_lock_reg(struct flash_bank *bank);
+static int stm32x_probe(struct flash_bank *bank);
+
+/* flash bank stm32x <base> <size> 0 0 <target#> */
+
+FLASH_BANK_COMMAND_HANDLER(stm32x_flash_bank_command)
+{
+	struct stm32h7x_flash_bank *stm32x_info;
+
+	if (CMD_ARGC < 6)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	stm32x_info = malloc(sizeof(struct stm32h7x_flash_bank));
+	bank->driver_priv = stm32x_info;
+
+	stm32x_info->probed = 0;
+	stm32x_info->user_bank_size = bank->size;
+
+	return ERROR_OK;
+}
+
+static inline int stm32x_get_flash_reg(struct flash_bank *bank, uint32_t reg)
+{
+	struct stm32h7x_flash_bank *stm32x_info = bank->driver_priv;
+	return reg + stm32x_info->flash_base;
+}
+
+static inline int stm32x_get_flash_status(struct flash_bank *bank, uint32_t *status)
+{
+	struct target *target = bank->target;
+	return target_read_u32(target, stm32x_get_flash_reg(bank, FLASH_SR), status);
+}
+
+static int stm32x_wait_status_busy(struct flash_bank *bank, int timeout)
+{
+	struct target *target = bank->target;
+	struct stm32h7x_flash_bank *stm32x_info = bank->driver_priv;
+	uint32_t status;
+	int retval;
+
+	/* wait for busy to clear */
+	for (;;) {
+		retval = stm32x_get_flash_status(bank, &status);
+		if (retval != ERROR_OK) {
+			LOG_INFO("wait_status_busy, target_read_u32 : error : remote address 0x%x", stm32x_info->flash_base);
+			return retval;
+		}
+
+		if ((status & FLASH_BSY) == 0)
+			break;
+
+		if (timeout-- <= 0) {
+			LOG_INFO("wait_status_busy, time out expired, status: 0x%" PRIx32 "", status);
+			return ERROR_FAIL;
+		}
+		alive_sleep(1);
+	}
+
+	if (status & FLASH_WRPERR) {
+		LOG_INFO("wait_status_busy, WRPERR : error : remote address 0x%x", stm32x_info->flash_base);
+		retval = ERROR_FAIL;
+	}
+
+	/* Clear error + EOP flags but report errors */
+	if (status & FLASH_ERROR) {
+		/* If this operation fails, we ignore it and report the original retval */
+		target_write_u32(target, stm32x_get_flash_reg(bank, FLASH_CCR), status);
+	}
+	return retval;
+}
+
+static int stm32x_unlock_reg(struct flash_bank *bank)
+{
+	uint32_t ctrl;
+	struct target *target = bank->target;
+
+	/* first check if not already unlocked
+	 * otherwise writing on FLASH_KEYR will fail
+	 */
+	int retval = target_read_u32(target, stm32x_get_flash_reg(bank, FLASH_CR), &ctrl);
+	if (retval != ERROR_OK)
+		return retval;
+
+	if ((ctrl & FLASH_LOCK) == 0)
+		return ERROR_OK;
+
+	/* unlock flash registers for bank */
+	retval = target_write_u32(target, stm32x_get_flash_reg(bank, FLASH_KEYR), KEY1);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = target_write_u32(target, stm32x_get_flash_reg(bank, FLASH_KEYR), KEY2);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = target_read_u32(target, stm32x_get_flash_reg(bank, FLASH_CR), &ctrl);
+	if (retval != ERROR_OK)
+		return retval;
+
+	if (ctrl & FLASH_LOCK) {
+		LOG_ERROR("flash not unlocked STM32_FLASH_CRx: %" PRIx32, ctrl);
+		return ERROR_TARGET_FAILURE;
+	}
+	return ERROR_OK;
+}
+
+static int stm32x_unlock_option_reg(struct flash_bank *bank)
+{
+	uint32_t ctrl;
+	struct target *target = bank->target;
+
+	int retval = target_read_u32(target, FLASH_REG_BASE_B0 + FLASH_OPTCR, &ctrl);
+	if (retval != ERROR_OK)
+		return retval;
+
+	if ((ctrl & OPT_LOCK) == 0)
+		return ERROR_OK;
+
+	/* unlock option registers */
+	retval = target_write_u32(target, FLASH_REG_BASE_B0 + FLASH_OPTKEYR, OPTKEY1);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = target_write_u32(target, FLASH_REG_BASE_B0 + FLASH_OPTKEYR, OPTKEY2);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = target_read_u32(target, FLASH_REG_BASE_B0 + FLASH_OPTCR, &ctrl);
+	if (retval != ERROR_OK)
+		return retval;
+
+	if (ctrl & OPT_LOCK) {
+		LOG_ERROR("options not unlocked STM32_FLASH_OPTCR: %" PRIx32, ctrl);
+		return ERROR_TARGET_FAILURE;
+	}
+
+	return ERROR_OK;
+}
+
+static int stm32x_lock_reg(struct flash_bank *bank)
+{
+	struct target *target = bank->target;
+
+	/* Lock bank reg */
+	int retval = target_write_u32(target, stm32x_get_flash_reg(bank, FLASH_CR), FLASH_LOCK);
+	if (retval != ERROR_OK)
+		return retval;
+
+	return ERROR_OK;
+}
+
+static int stm32x_read_options(struct flash_bank *bank)
+{
+	uint32_t optiondata;
+	struct stm32h7x_flash_bank *stm32x_info = NULL;
+	struct target *target = bank->target;
+
+	stm32x_info = bank->driver_priv;
+
+	/* read current option bytes */
+	int retval = target_read_u32(target, FLASH_REG_BASE_B0 + FLASH_OPTCUR, &optiondata);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* decode option data */
+	stm32x_info->option_bytes.user_options = optiondata & 0xfc;
+	stm32x_info->option_bytes.RDP = (optiondata >> 8) & 0xff;
+	stm32x_info->option_bytes.user2_options = (optiondata >> 16) & 0xff;
+	stm32x_info->option_bytes.user3_options = (optiondata >> 24) & 0x83;
+
+	if (optiondata & IDWG1_SW)
+		stm32x_info->option_bytes.independent_watchdog_selection = 1;
+	else
+		stm32x_info->option_bytes.independent_watchdog_selection = 0;
+
+	if (optiondata & IDWG2_SW)
+		stm32x_info->option_bytes.independent_watchdog_selection2 = 1;
+	else
+		stm32x_info->option_bytes.independent_watchdog_selection2 = 0;
+
+	if (stm32x_info->option_bytes.RDP != 0xAA)
+		LOG_INFO("Device Security Bit Set");
+
+	/* read current WPSN option bytes */
+	retval = target_read_u32(target, FLASH_REG_BASE_B0 + FLASH_WPSNCUR, &optiondata);
+	if (retval != ERROR_OK)
+		return retval;
+	stm32x_info->option_bytes.protection = optiondata & 0xff;
+
+	/* read current WPSN2 option bytes */
+	retval = target_read_u32(target, FLASH_REG_BASE_B1 + FLASH_WPSNCUR, &optiondata);
+	if (retval != ERROR_OK)
+		return retval;
+	stm32x_info->option_bytes.protection2 = optiondata & 0xff;
+
+	return ERROR_OK;
+}
+
+static int stm32x_write_options(struct flash_bank *bank)
+{
+	struct stm32h7x_flash_bank *stm32x_info = NULL;
+	struct target *target = bank->target;
+	uint32_t optiondata;
+
+	stm32x_info = bank->driver_priv;
+
+	int retval = stm32x_unlock_option_reg(bank);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* rebuild option data */
+	optiondata = stm32x_info->option_bytes.user_options;
+	optiondata |= (stm32x_info->option_bytes.RDP << 8);
+	optiondata |= (stm32x_info->option_bytes.user2_options & 0xff) << 16;
+	optiondata |= (stm32x_info->option_bytes.user3_options & 0x83) << 24;
+
+	if (stm32x_info->option_bytes.independent_watchdog_selection)
+		optiondata |= IDWG1_SW;
+	else
+		optiondata &= ~IDWG1_SW;
+
+	if (stm32x_info->option_bytes.independent_watchdog_selection2)
+		optiondata |= IDWG2_SW;
+	else
+		optiondata &= ~IDWG2_SW;
+
+	/* program options */
+	retval = target_write_u32(target, FLASH_REG_BASE_B0 + FLASH_OPTPRG, optiondata);
+		if (retval != ERROR_OK)
+			return retval;
+
+	optiondata = stm32x_info->option_bytes.protection & 0xff;
+	/* Program protection WPSNPRG */
+	retval = target_write_u32(target, FLASH_REG_BASE_B0 + FLASH_WPSNPRG, optiondata);
+		if (retval != ERROR_OK)
+			return retval;
+
+	optiondata = stm32x_info->option_bytes.protection2 & 0xff;
+	/* Program protection WPSNPRG2 */
+	retval = target_write_u32(target, FLASH_REG_BASE_B1 + FLASH_WPSNPRG, optiondata);
+		if (retval != ERROR_OK)
+			return retval;
+
+	optiondata = 0x40000000;
+	/* Remove OPT error flag before programming */
+	retval = target_write_u32(target, FLASH_REG_BASE_B0 + FLASH_OPTCCR, optiondata);
+		if (retval != ERROR_OK)
+			return retval;
+
+	/* start programming cycle */
+	retval = target_write_u32(target, FLASH_REG_BASE_B0 + FLASH_OPTCR, OPT_START);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* wait for completion */
+	int timeout = FLASH_ERASE_TIMEOUT;
+	for (;;) {
+		uint32_t status;
+		retval = target_read_u32(target, FLASH_REG_BASE_B0 + FLASH_SR, &status);
+		if (retval != ERROR_OK) {
+			LOG_INFO("stm32x_write_options: wait_status_busy : error");
+			return retval;
+		}
+		if ((status & FLASH_BSY) == 0)
+			break;
+
+		if (timeout-- <= 0) {
+			LOG_INFO("wait_status_busy, time out expired, status: 0x%" PRIx32 "", status);
+			return ERROR_FAIL;
+		}
+		alive_sleep(1);
+	}
+
+	/* relock option registers */
+	retval = target_write_u32(target, FLASH_REG_BASE_B0 + FLASH_OPTCR, OPT_LOCK);
+	if (retval != ERROR_OK)
+		return retval;
+
+	return ERROR_OK;
+}
+
+static int stm32x_protect_check(struct flash_bank *bank)
+{
+	struct stm32h7x_flash_bank *stm32x_info = bank->driver_priv;
+
+	/* read 'write protection' settings */
+	int retval = stm32x_read_options(bank);
+	if (retval != ERROR_OK) {
+		LOG_DEBUG("unable to read option bytes");
+		return retval;
+	}
+
+	for (int i = 0; i < bank->num_sectors; i++) {
+		if (stm32x_info->flash_base == FLASH_REG_BASE_B0) {
+			if (stm32x_info->option_bytes.protection & (1 << i))
+				bank->sectors[i].is_protected = 0;
+			else
+				bank->sectors[i].is_protected = 1;
+		} else {
+			if (stm32x_info->option_bytes.protection2 & (1 << i))
+				bank->sectors[i].is_protected = 0;
+			else
+				bank->sectors[i].is_protected = 1;
+		}
+	}
+	return ERROR_OK;
+}
+
+static int stm32x_erase(struct flash_bank *bank, int first, int last)
+{
+	struct target *target = bank->target;
+	int retval;
+
+	assert(first < bank->num_sectors);
+	assert(last < bank->num_sectors);
+
+	if (bank->target->state != TARGET_HALTED)
+		return ERROR_TARGET_NOT_HALTED;
+
+	retval = stm32x_unlock_reg(bank);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/*
+	Sector Erase
+	To erase a sector, follow the procedure below:
+	1. Check that no Flash memory operation is ongoing by checking the BSY bit in the
+	  FLASH_SR register
+	2. Set the SER bit and select the sector
+	  you wish to erase (SNB) in the FLASH_CR register
+	3. Set the STRT bit in the FLASH_CR register
+	4. Wait for the BSY bit to be cleared
+	 */
+	for (int i = first; i <= last; i++) {
+		LOG_DEBUG("erase sector %d", i);
+		retval = target_write_u32(target, stm32x_get_flash_reg(bank, FLASH_CR),
+				FLASH_SER | FLASH_SNB(i) | FLASH_PSIZE_64);
+		if (retval != ERROR_OK) {
+			LOG_ERROR("Error erase sector %d", i);
+			return retval;
+		}
+		retval = target_write_u32(target, stm32x_get_flash_reg(bank, FLASH_CR),
+				FLASH_SER | FLASH_SNB(i) | FLASH_PSIZE_64 | FLASH_START);
+		if (retval != ERROR_OK) {
+			LOG_ERROR("Error erase sector %d", i);
+			return retval;
+		}
+		retval = stm32x_wait_status_busy(bank, FLASH_ERASE_TIMEOUT);
+
+		if (retval != ERROR_OK) {
+			LOG_ERROR("erase time-out error sector %d", i);
+			return retval;
+		}
+		bank->sectors[i].is_erased = 1;
+	}
+
+	retval = stm32x_lock_reg(bank);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("error during the lock of flash");
+		return retval;
+	}
+
+	return ERROR_OK;
+}
+
+static int stm32x_protect(struct flash_bank *bank, int set, int first, int last)
+{
+	struct target *target = bank->target;
+	struct stm32h7x_flash_bank *stm32x_info = bank->driver_priv;
+
+	if (target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+	/* read protection settings */
+	int retval = stm32x_read_options(bank);
+	if (retval != ERROR_OK) {
+		LOG_DEBUG("unable to read option bytes");
+		return retval;
+	}
+
+	for (int i = first; i <= last; i++) {
+		if (stm32x_info->flash_base == FLASH_REG_BASE_B0) {
+			if (set)
+				stm32x_info->option_bytes.protection &= ~(1 << i);
+			else
+				stm32x_info->option_bytes.protection |= (1 << i);
+		} else {
+			if (set)
+				stm32x_info->option_bytes.protection2 &= ~(1 << i);
+			else
+				stm32x_info->option_bytes.protection2 |= (1 << i);
+		}
+	}
+
+	LOG_INFO("stm32x_protect, option_bytes written WRP1 0x%x , WRP2 0x%x",
+	  (stm32x_info->option_bytes.protection & 0xff), (stm32x_info->option_bytes.protection2 & 0xff));
+
+	retval = stm32x_write_options(bank);
+	if (retval != ERROR_OK)
+		return retval;
+
+	return ERROR_OK;
+}
+
+static int stm32x_write_block(struct flash_bank *bank, const uint8_t *buffer,
+		uint32_t offset, uint32_t count)
+{
+	struct target *target = bank->target;
+	uint32_t buffer_size = 16392;
+	struct working_area *write_algorithm;
+	struct working_area *source;
+	uint32_t address = bank->base + offset;
+	struct reg_param reg_params[5];
+	struct armv7m_algorithm armv7m_info;
+	struct stm32h7x_flash_bank *stm32x_info = bank->driver_priv;
+
+	int retval = ERROR_OK;
+
+	/* see contrib/loaders/flash/smt32h7x.S for src */
+	static const uint8_t stm32x_flash_write_code[] = {
+								/*  <wait_fifo>: */
+		0xd0, 0xf8, 0x00, 0x80,	/* ldr		r8, [r0, #0] */
+		0xb8, 0xf1, 0x00, 0x0f,	/* cmp		r8, #0 */
+		0x25, 0xd0,				/* beq		exit */
+		0x47, 0x68,				/* ldr		r7, [r0, #4] */
+		0xb8, 0xeb, 0x07, 0x06,	/* subs		r6, r8, r7 */
+		0x1f, 0x2e,				/* cmp		r6, #31 */
+		0xf5, 0xd3,				/* bcc		wait_fifo */
+
+		0x4f, 0xf0, 0x32, 0x06,	/* mov		r6, #STM32_PROG */
+		0xe6, 0x60,				/* str		r6, [r4, #STM32_FLASH_CR_OFFSET] */
+
+		0x4f, 0xf0, 0x08, 0x05,	/* mov		r5, #8 */
+								/* <write_flash>: */
+		0x57, 0xf8, 0x04, 0x6b,	/* ldr		r6, [r7], #0x04 */
+		0x42, 0xf8, 0x04, 0x6b,	/* str		r6, [r2], #0x04 */
+		0xbf, 0xf3, 0x4f, 0x8f,	/* dsb */
+		0x01, 0x3d,				/* subs		r5, r5, #1 */
+		0xf7, 0xd1,				/* bne		write_flash */
+
+								/* <busy>: */
+		0x26, 0x69,				/* ldr		r6, [r4, #STM32_FLASH_SR_OFFSET] */
+		0x16, 0xf0, 0x01, 0x0f,	/* tst		r6, #0x1 */
+		0xfb, 0xd1,				/* bne		busy */
+		0x16, 0xf0, 0xf7, 0x7f,	/* tst		r6, #0x01ee0000 */
+		0x0a, 0xd1,				/* bne		error */
+		0x16, 0xf0, 0x00, 0x7f,	/* tst		r6, #0x02000000 */
+		0x07, 0xd1,				/* bne		error */
+
+		0x8f, 0x42,				/* cmp		r7, r1 */
+		0x28, 0xbf,				/* it		cs */
+		0x00, 0xf1, 0x08, 0x07,	/* addcs	r7, r0, #8 */
+		0x47, 0x60,				/* str		r7, [r0, #4] */
+		0x01, 0x3b,				/* subs		r3, r3, #1 */
+		0x13, 0xb1,				/* cbz		r3, exit */
+		0xd6, 0xe7,				/* b		wait_fifo */
+								/* <error>: */
+		0x00, 0x21,				/* movs		r1, #0 */
+		0x41, 0x60,				/* str		r1, [r0, #4] */
+								/* <exit>: */
+		0x30, 0x46,				/* mov		r0, r6 */
+		0x00, 0xbe				/* bkpt		#0x00 */
+	};
+
+	if (target_alloc_working_area(target, sizeof(stm32x_flash_write_code),
+			&write_algorithm) != ERROR_OK) {
+		LOG_WARNING("no working area available, can't do block memory writes");
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	}
+
+	retval = target_write_buffer(target, write_algorithm->address,
+			sizeof(stm32x_flash_write_code),
+			stm32x_flash_write_code);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* memory buffer */
+	while (target_alloc_working_area_try(target, buffer_size, &source) != ERROR_OK) {
+		buffer_size /= 2;
+		if (buffer_size <= 256) {
+			/* we already allocated the writing code, but failed to get a
+			 * buffer, free the algorithm */
+			target_free_working_area(target, write_algorithm);
+
+			LOG_WARNING("no large enough working area available, can't do block memory writes");
+			return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+		}
+	}
+
+	LOG_DEBUG("target_alloc_working_area_try : buffer_size -> 0x%x", buffer_size);
+
+	armv7m_info.common_magic = ARMV7M_COMMON_MAGIC;
+	armv7m_info.core_mode = ARM_MODE_THREAD;
+
+	init_reg_param(&reg_params[0], "r0", 32, PARAM_IN_OUT);		/* buffer start, status (out) */
+	init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);		/* buffer end */
+	init_reg_param(&reg_params[2], "r2", 32, PARAM_OUT);		/* target address */
+	init_reg_param(&reg_params[3], "r3", 32, PARAM_OUT);		/* count (word-256 bits) */
+	init_reg_param(&reg_params[4], "r4", 32, PARAM_OUT);		/* flash reg base */
+
+	buf_set_u32(reg_params[0].value, 0, 32, source->address);
+	buf_set_u32(reg_params[1].value, 0, 32, source->address + source->size);
+	buf_set_u32(reg_params[2].value, 0, 32, address);
+	buf_set_u32(reg_params[3].value, 0, 32, count);
+	buf_set_u32(reg_params[4].value, 0, 32, stm32x_info->flash_base);
+
+	retval = target_run_flash_async_algorithm(target,
+						  buffer,
+						  count,
+						  FLASH_BLOCK_SIZE,
+						  0, NULL,
+						  5, reg_params,
+						  source->address, source->size,
+						  write_algorithm->address, 0,
+						  &armv7m_info);
+
+	if (retval == ERROR_FLASH_OPERATION_FAILED) {
+		LOG_INFO("error executing stm32h7x flash write algorithm");
+
+		uint32_t flash_sr = buf_get_u32(reg_params[0].value, 0, 32);
+
+		if (flash_sr & FLASH_WRPERR)
+			LOG_ERROR("flash memory write protected");
+
+		if ((flash_sr & FLASH_ERROR) != 0) {
+			LOG_ERROR("flash write failed, FLASH_SR = %08" PRIx32, flash_sr);
+			/* Clear error + EOP flags but report errors */
+			target_write_u32(target, stm32x_get_flash_reg(bank, FLASH_CCR), flash_sr);
+			retval = ERROR_FAIL;
+		}
+	}
+
+	target_free_working_area(target, source);
+	target_free_working_area(target, write_algorithm);
+
+	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]);
+	return retval;
+}
+
+static int stm32x_write(struct flash_bank *bank, const uint8_t *buffer,
+		uint32_t offset, uint32_t count)
+{
+	struct target *target = bank->target;
+	uint32_t address = bank->base + offset;
+	int retval;
+
+	if (bank->target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if (offset % FLASH_BLOCK_SIZE) {
+		LOG_WARNING("offset 0x%" PRIx32 " breaks required 32-byte alignment", offset);
+		return ERROR_FLASH_DST_BREAKS_ALIGNMENT;
+	}
+
+	retval = stm32x_unlock_reg(bank);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* small transfer */
+	if (count < FLASH_BLOCK_SIZE) {
+		LOG_DEBUG("write %d bytes", count);
+		retval = target_write_u32(target, stm32x_get_flash_reg(bank, FLASH_CR), FLASH_PG | FLASH_PSIZE_64);
+		if (retval != ERROR_OK)
+			return retval;
+
+		retval = target_write_buffer(target, address, count, buffer);
+		if (retval != ERROR_OK)
+			return retval;
+
+		/* Force Write buffer of 32 bytes (padding with 0xff value automatically) */
+		retval = target_write_u32(target, stm32x_get_flash_reg(bank, FLASH_CR), FLASH_PG | FLASH_PSIZE_64 | FLASH_FW);
+		if (retval != ERROR_OK)
+			return retval;
+
+		retval = stm32x_wait_status_busy(bank, FLASH_WRITE_TIMEOUT);
+		if (retval != ERROR_OK)
+			return retval;
+
+		return ERROR_OK;
+	}
+
+	uint32_t blocks_remaining = count / FLASH_BLOCK_SIZE;
+	uint32_t bytes_remaining = count % FLASH_BLOCK_SIZE;
+
+	/* multiple words (32-bytes) to be programmed */
+	if (blocks_remaining > 0) {
+		/* try using a block write */
+		retval = stm32x_write_block(bank, buffer, offset, blocks_remaining);
+		if (retval != ERROR_OK) {
+			if (retval == ERROR_TARGET_RESOURCE_NOT_AVAILABLE) {
+				/* if block write failed (no sufficient working area),
+				 * we use normal (slow) dword accesses */
+				LOG_WARNING("couldn't use block writes, falling back to single memory accesses");
+			}
+		} else {
+			buffer += blocks_remaining * FLASH_BLOCK_SIZE;
+			address += blocks_remaining * FLASH_BLOCK_SIZE;
+			blocks_remaining = 0;
+		}
+	}
+	if ((retval != ERROR_OK) && (retval != ERROR_TARGET_RESOURCE_NOT_AVAILABLE))
+		return retval;
+
+	/*
+	Standard programming
+	The Flash memory programming sequence is as follows:
+	1. Check that no main Flash memory operation is ongoing by checking the BSY bit in the
+	  FLASH_SR register.
+	2. Set the PG bit in the FLASH_CR register
+	3. 8 x Word access (or Force Write FW)
+	   Wait for the BSY bit to be cleared
+	*/
+	while (blocks_remaining > 0) {
+		retval = target_write_u32(target, stm32x_get_flash_reg(bank, FLASH_CR), FLASH_PG | FLASH_PSIZE_64);
+		if (retval != ERROR_OK)
+			return retval;
+
+		retval = target_write_buffer(target, address, FLASH_BLOCK_SIZE, buffer);
+		if (retval != ERROR_OK)
+			return retval;
+
+		retval = stm32x_wait_status_busy(bank, FLASH_WRITE_TIMEOUT);
+		if (retval != ERROR_OK)
+			return retval;
+
+		buffer += FLASH_BLOCK_SIZE;
+		address += FLASH_BLOCK_SIZE;
+		blocks_remaining--;
+	}
+
+	if (bytes_remaining) {
+		retval = target_write_u32(target, stm32x_get_flash_reg(bank, FLASH_CR), FLASH_PG | FLASH_PSIZE_64);
+		if (retval != ERROR_OK)
+			return retval;
+
+		LOG_DEBUG("bytes_remaining %d", bytes_remaining);
+		retval = target_write_buffer(target, address, bytes_remaining, buffer);
+		if (retval != ERROR_OK)
+			return retval;
+
+		/* Force Write buffer of FLASH_BLOCK_SIZE = 32 bytes */
+		retval = target_write_u32(target, stm32x_get_flash_reg(bank, FLASH_CR), FLASH_PG | FLASH_PSIZE_64 | FLASH_FW);
+		if (retval != ERROR_OK)
+			return retval;
+
+		retval = stm32x_wait_status_busy(bank, FLASH_WRITE_TIMEOUT);
+		if (retval != ERROR_OK)
+			return retval;
+	}
+
+	retval = stm32x_lock_reg(bank);
+	if (retval != ERROR_OK)
+		LOG_ERROR("error during the lock of flash");
+
+	return retval;
+}
+
+static void setup_sector(struct flash_bank *bank, int start, int num, int size)
+{
+	for (int i = start; i < (start + num) ; i++) {
+		assert(i < bank->num_sectors);
+		bank->sectors[i].offset = bank->size;
+		bank->sectors[i].size = size;
+		bank->size += bank->sectors[i].size;
+	}
+}
+
+static int stm32x_read_id_code(struct flash_bank *bank, uint32_t *id)
+{
+	/* read stm32 device id register */
+	int retval = target_read_u32(bank->target, DBGMCU_IDCODE_REGISTER, id);
+	if (retval != ERROR_OK)
+		return retval;
+	return ERROR_OK;
+}
+
+static int stm32x_probe(struct flash_bank *bank)
+{
+	struct target *target = bank->target;
+	struct stm32h7x_flash_bank *stm32x_info = bank->driver_priv;
+	int i;
+	uint16_t flash_size_in_kb;
+	uint32_t device_id;
+	uint32_t base_address = FLASH_BANK0_ADDRESS;
+	uint32_t second_bank_base;
+
+	stm32x_info->probed = 0;
+	stm32x_info->part_info = NULL;
+
+	int retval = stm32x_read_id_code(bank, &stm32x_info->idcode);
+	if (retval != ERROR_OK)
+		return retval;
+
+	LOG_DEBUG("device id = 0x%08" PRIx32 "", stm32x_info->idcode);
+
+	device_id = stm32x_info->idcode & 0xfff;
+
+	for (unsigned int n = 0; n < ARRAY_SIZE(stm32h7x_parts); n++) {
+		if (device_id == stm32h7x_parts[n].id)
+			stm32x_info->part_info = &stm32h7x_parts[n];
+	}
+	if (!stm32x_info->part_info) {
+		LOG_WARNING("Cannot identify target as a STM32H7xx family.");
+		return ERROR_FAIL;
+	} else {
+		LOG_INFO("Device: %s", stm32x_info->part_info->device_str);
+	}
+
+	/* update the address of controller from data base */
+	stm32x_info->flash_base = stm32x_info->part_info->flash_base;
+
+	/* get flash size from target */
+	retval = target_read_u16(target, stm32x_info->part_info->fsize_base, &flash_size_in_kb);
+	if (retval != ERROR_OK) {
+		/* read error when device has invalid value, set max flash size */
+		flash_size_in_kb = stm32x_info->part_info->max_flash_size_kb;
+	} else
+		LOG_INFO("flash size probed value %d", flash_size_in_kb);
+
+	/* Lower flash size devices are single bank */
+	if (stm32x_info->part_info->has_dual_bank && (flash_size_in_kb > stm32x_info->part_info->first_bank_size_kb)) {
+		/* Use the configured base address to determine if this is the first or second flash bank.
+		 * Verify that the base address is reasonably correct and determine the flash bank size
+		 */
+		second_bank_base = base_address + stm32x_info->part_info->first_bank_size_kb * 1024;
+		if (bank->base == second_bank_base) {
+			/* This is the second bank  */
+			base_address = second_bank_base;
+			flash_size_in_kb = flash_size_in_kb - stm32x_info->part_info->first_bank_size_kb;
+			/* bank1 also uses a register offset */
+			stm32x_info->flash_base = FLASH_REG_BASE_B1;
+		} else if (bank->base == base_address) {
+			/* This is the first bank */
+			flash_size_in_kb = stm32x_info->part_info->first_bank_size_kb;
+		} else {
+			LOG_WARNING("STM32H flash bank base address config is incorrect."
+				    " 0x%" PRIx32 " but should rather be 0x%" PRIx32 " or 0x%" PRIx32,
+					bank->base, base_address, second_bank_base);
+			return ERROR_FAIL;
+		}
+		LOG_INFO("STM32H flash has dual banks. Bank (%d) size is %dkb, base address is 0x%" PRIx32,
+				bank->bank_number, flash_size_in_kb, base_address);
+	} else
+		LOG_INFO("STM32H flash size is %dkb, base address is 0x%" PRIx32, flash_size_in_kb, base_address);
+
+	/* if the user sets the size manually then ignore the probed value
+	 * this allows us to work around devices that have an invalid flash size register value */
+	if (stm32x_info->user_bank_size) {
+		LOG_INFO("ignoring flash probed value, using configured bank size");
+		flash_size_in_kb = stm32x_info->user_bank_size / 1024;
+	} else if (flash_size_in_kb == 0xffff) {
+		/* die flash size */
+		flash_size_in_kb = stm32x_info->part_info->max_flash_size_kb;
+	}
+
+	/* did we assign flash size? */
+	assert(flash_size_in_kb != 0xffff);
+
+	/* calculate numbers of pages */
+	int num_pages = flash_size_in_kb / stm32x_info->part_info->page_size;
+
+	/* check that calculation result makes sense */
+	assert(num_pages > 0);
+
+	if (bank->sectors) {
+		free(bank->sectors);
+		bank->sectors = NULL;
+	}
+
+	bank->base = base_address;
+	bank->num_sectors = num_pages;
+	bank->sectors = malloc(sizeof(struct flash_sector) * num_pages);
+	if (bank->sectors == NULL) {
+		LOG_ERROR("failed to allocate bank sectors");
+		return ERROR_FAIL;
+	}
+	bank->size = 0;
+
+	/* fixed memory */
+	setup_sector(bank, 0, num_pages, stm32x_info->part_info->page_size * 1024);
+
+	for (i = 0; i < num_pages; i++) {
+		bank->sectors[i].is_erased = -1;
+		bank->sectors[i].is_protected = 0;
+	}
+
+	stm32x_info->probed = 1;
+	return ERROR_OK;
+}
+
+static int stm32x_auto_probe(struct flash_bank *bank)
+{
+	struct stm32h7x_flash_bank *stm32x_info = bank->driver_priv;
+
+	if (stm32x_info->probed)
+		return ERROR_OK;
+
+	return stm32x_probe(bank);
+}
+
+/* This method must return a string displaying information about the bank */
+static int stm32x_get_info(struct flash_bank *bank, char *buf, int buf_size)
+{
+	struct stm32h7x_flash_bank *stm32x_info = bank->driver_priv;
+	const struct stm32h7x_part_info *info = stm32x_info->part_info;
+
+	if (!stm32x_info->probed) {
+		int retval = stm32x_probe(bank);
+		if (retval != ERROR_OK) {
+			snprintf(buf, buf_size, "Unable to find bank information.");
+			return retval;
+		}
+	}
+
+	if (info) {
+		const char *rev_str = NULL;
+		uint16_t rev_id = stm32x_info->idcode >> 16;
+
+		for (unsigned int i = 0; i < info->num_revs; i++)
+			if (rev_id == info->revs[i].rev)
+				rev_str = info->revs[i].str;
+
+		if (rev_str != NULL) {
+			snprintf(buf, buf_size, "%s - Rev: %s",
+				stm32x_info->part_info->device_str, rev_str);
+		} else {
+			snprintf(buf, buf_size,
+				 "%s - Rev: unknown (0x%04x)",
+				stm32x_info->part_info->device_str, rev_id);
+		}
+	} else {
+	  snprintf(buf, buf_size, "Cannot identify target as a STM32H7x");
+	  return ERROR_FAIL;
+	}
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(stm32x_handle_lock_command)
+{
+	struct target *target = NULL;
+	struct stm32h7x_flash_bank *stm32x_info = NULL;
+
+	if (CMD_ARGC < 1)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	struct flash_bank *bank;
+	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
+	if (ERROR_OK != retval)
+		return retval;
+
+	stm32x_info = bank->driver_priv;
+	target = bank->target;
+
+	/* if we have a dual flash bank device then
+	 * we need to perform option byte lock on bank0 only */
+	if (stm32x_info->flash_base != FLASH_REG_BASE_B0) {
+		LOG_ERROR("Option Byte Lock Operation must use bank0");
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	if (target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if (stm32x_read_options(bank) != ERROR_OK) {
+		command_print(CMD_CTX, "%s failed to read options",
+			      bank->driver->name);
+		return ERROR_OK;
+	}
+	/* set readout protection */
+	stm32x_info->option_bytes.RDP = 0;
+
+	if (stm32x_write_options(bank) != ERROR_OK) {
+		command_print(CMD_CTX, "%s failed to lock device",
+			      bank->driver->name);
+		return ERROR_OK;
+	}
+	command_print(CMD_CTX, "%s locked", bank->driver->name);
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(stm32x_handle_unlock_command)
+{
+	struct target *target = NULL;
+	struct stm32h7x_flash_bank *stm32x_info = NULL;
+
+	if (CMD_ARGC < 1)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	struct flash_bank *bank;
+	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
+	if (ERROR_OK != retval)
+		return retval;
+
+	stm32x_info = bank->driver_priv;
+	target = bank->target;
+
+	/* if we have a dual flash bank device then
+	 * we need to perform option byte unlock on bank0 only */
+	if (stm32x_info->flash_base != FLASH_REG_BASE_B0) {
+		LOG_ERROR("Option Byte Unlock Operation must use bank0");
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	if (target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if (stm32x_read_options(bank) != ERROR_OK) {
+		command_print(CMD_CTX, "%s failed to read options", bank->driver->name);
+		return ERROR_OK;
+	}
+
+	/* clear readout protection option byte
+	 * this will also force a device unlock if set */
+	stm32x_info->option_bytes.RDP = 0xAA;
+
+	if (stm32x_write_options(bank) != ERROR_OK) {
+		command_print(CMD_CTX, "%s failed to unlock device", bank->driver->name);
+		return ERROR_OK;
+	}
+	command_print(CMD_CTX, "%s unlocked.\n", bank->driver->name);
+
+	return ERROR_OK;
+}
+
+static int stm32x_mass_erase(struct flash_bank *bank)
+{
+	int retval;
+	struct target *target = bank->target;
+
+	if (target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	retval = stm32x_unlock_reg(bank);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* mass erase flash memory bank */
+	retval = target_write_u32(target, stm32x_get_flash_reg(bank, FLASH_CR), FLASH_BER_CMD | FLASH_PSIZE_64);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = target_write_u32(target, stm32x_get_flash_reg(bank, FLASH_CR),
+							  FLASH_BER_CMD | FLASH_PSIZE_64 | FLASH_START);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = stm32x_wait_status_busy(bank, 30000);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = stm32x_lock_reg(bank);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("error during the lock of flash");
+		return retval;
+	}
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(stm32x_handle_mass_erase_command)
+{
+	int i;
+
+	if (CMD_ARGC < 1) {
+		command_print(CMD_CTX, "stm32h7x mass_erase <bank>");
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	struct flash_bank *bank;
+	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
+	if (ERROR_OK != retval)
+		return retval;
+
+	retval = stm32x_mass_erase(bank);
+	if (retval == ERROR_OK) {
+		/* set all sectors as erased */
+		for (i = 0; i < bank->num_sectors; i++)
+			bank->sectors[i].is_erased = 1;
+
+		command_print(CMD_CTX, "stm32h7x mass erase complete");
+	} else {
+		command_print(CMD_CTX, "stm32h7x mass erase failed");
+	}
+
+	return retval;
+}
+
+static const struct command_registration stm32x_exec_command_handlers[] = {
+	{
+		.name = "lock",
+		.handler = stm32x_handle_lock_command,
+		.mode = COMMAND_EXEC,
+		.usage = "bank_id",
+		.help = "Lock entire flash device.",
+	},
+	{
+		.name = "unlock",
+		.handler = stm32x_handle_unlock_command,
+		.mode = COMMAND_EXEC,
+		.usage = "bank_id",
+		.help = "Unlock entire protected flash device.",
+	},
+	{
+		.name = "mass_erase",
+		.handler = stm32x_handle_mass_erase_command,
+		.mode = COMMAND_EXEC,
+		.usage = "bank_id",
+		.help = "Erase entire flash device.",
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+static const struct command_registration stm32x_command_handlers[] = {
+	{
+		.name = "stm32h7x",
+		.mode = COMMAND_ANY,
+		.help = "stm32h7x flash command group",
+		.usage = "",
+		.chain = stm32x_exec_command_handlers,
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+struct flash_driver stm32h7x_flash = {
+	.name = "stm32h7x",
+	.commands = stm32x_command_handlers,
+	.flash_bank_command = stm32x_flash_bank_command,
+	.erase = stm32x_erase,
+	.protect = stm32x_protect,
+	.write = stm32x_write,
+	.read = default_flash_read,
+	.probe = stm32x_probe,
+	.auto_probe = stm32x_auto_probe,
+	.erase_check = default_flash_blank_check,
+	.protect_check = stm32x_protect_check,
+	.info = stm32x_get_info,
+};
-- 
2.14.1.821.g8fa685d3b7-goog