Merge branch 'master' into update

Change-Id: Icec244b174cc0c67ab58961649a369db7f344824

54 files changed, 4228 insertions, 791 deletions

diff --git a/src/flash/mflash.c b/src/flash/mflash.c
index b699955..4c95d21 100644
--- a/src/flash/mflash.c
+++ b/src/flash/mflash.c
@@ -259,11 +259,11 @@ static int mg_dsk_wait(mg_io_type_wait wait_local, uint32_t time_var)
 				case mg_io_wait_rdy:
 					if (status & mg_io_rbit_status_ready)
 						return ERROR_OK;
-
+					/* fallthrough */
 				case mg_io_wait_drq:
 					if (status & mg_io_rbit_status_data_req)
 						return ERROR_OK;
-
+					/* fallthrough */
 				default:
 					break;
 			}
diff --git a/src/flash/nand/mx3.c b/src/flash/nand/mx3.c
index b61e475..5fdc923 100644
--- a/src/flash/nand/mx3.c
+++ b/src/flash/nand/mx3.c
@@ -281,6 +281,7 @@ static int imx31_command(struct nand_device *nand, uint8_t command)
 			 * offset == one half of page size
 			 */
 			in_sram_address = MX3_NF_MAIN_BUFFER0 + (nand->page_size >> 1);
+			break;
 		default:
 			in_sram_address = MX3_NF_MAIN_BUFFER0;
 	}
diff --git a/src/flash/nor/Makefile.am b/src/flash/nor/Makefile.am
index c647cbb..8a57f4f 100644
--- a/src/flash/nor/Makefile.am
+++ b/src/flash/nor/Makefile.am
@@ -37,7 +37,7 @@ NOR_DRIVERS = \
 	%D%/mrvlqspi.c \
 	%D%/niietcm4.c \
 	%D%/non_cfi.c \
-	%D%/nrf51.c \
+	%D%/nrf5.c \
 	%D%/numicro.c \
 	%D%/ocl.c \
 	%D%/pic32mx.c \
@@ -50,6 +50,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/src/flash/nor/drivers.c b/src/flash/nor/drivers.c
index 56b451c..3f0c3c7 100644
--- a/src/flash/nor/drivers.c
+++ b/src/flash/nor/drivers.c
@@ -49,6 +49,7 @@ extern struct flash_driver lpcspifi_flash;
 extern struct flash_driver mdr_flash;
 extern struct flash_driver mrvlqspi_flash;
 extern struct flash_driver niietcm4_flash;
+extern struct flash_driver nrf5_flash;
 extern struct flash_driver nrf51_flash;
 extern struct flash_driver numicro_flash;
 extern struct flash_driver ocl_flash;
@@ -60,6 +61,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;
@@ -103,6 +105,7 @@ static struct flash_driver *flash_drivers[] = {
 	&mdr_flash,
 	&mrvlqspi_flash,
 	&niietcm4_flash,
+	&nrf5_flash,
 	&nrf51_flash,
 	&numicro_flash,
 	&ocl_flash,
@@ -114,6 +117,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/efm32.c b/src/flash/nor/efm32.c
index 117cd8a..b8453e1 100644
--- a/src/flash/nor/efm32.c
+++ b/src/flash/nor/efm32.c
@@ -49,6 +49,8 @@
 #define EZR_FAMILY_ID_WONDER_GECKO		120
 #define EZR_FAMILY_ID_LEOPARD_GECKO		121
 #define EZR_FAMILY_ID_HAPPY_GECKO               122
+#define EFR_FAMILY_ID_MIGHTY_GECKO	16
+#define EFR_FAMILY_ID_BLUE_GECKO	20
 
 #define EFM32_FLASH_ERASE_TMO           100
 #define EFM32_FLASH_WDATAREADY_TMO      100
@@ -72,27 +74,31 @@
 #define EFM32_MSC_DI_PROD_REV           (EFM32_MSC_DEV_INFO+0x1ff)
 
 #define EFM32_MSC_REGBASE               0x400c0000
-#define EFM32_MSC_WRITECTRL             (EFM32_MSC_REGBASE+0x008)
+#define EFR32_MSC_REGBASE               0x400e0000
+#define EFM32_MSC_REG_WRITECTRL         0x008
 #define EFM32_MSC_WRITECTRL_WREN_MASK   0x1
-#define EFM32_MSC_WRITECMD              (EFM32_MSC_REGBASE+0x00c)
+#define EFM32_MSC_REG_WRITECMD          0x00c
 #define EFM32_MSC_WRITECMD_LADDRIM_MASK 0x1
 #define EFM32_MSC_WRITECMD_ERASEPAGE_MASK 0x2
 #define EFM32_MSC_WRITECMD_WRITEONCE_MASK 0x8
-#define EFM32_MSC_ADDRB                 (EFM32_MSC_REGBASE+0x010)
-#define EFM32_MSC_WDATA                 (EFM32_MSC_REGBASE+0x018)
-#define EFM32_MSC_STATUS                (EFM32_MSC_REGBASE+0x01c)
+#define EFM32_MSC_REG_ADDRB             0x010
+#define EFM32_MSC_REG_WDATA             0x018
+#define EFM32_MSC_REG_STATUS            0x01c
 #define EFM32_MSC_STATUS_BUSY_MASK      0x1
 #define EFM32_MSC_STATUS_LOCKED_MASK    0x2
 #define EFM32_MSC_STATUS_INVADDR_MASK   0x4
 #define EFM32_MSC_STATUS_WDATAREADY_MASK 0x8
 #define EFM32_MSC_STATUS_WORDTIMEOUT_MASK 0x10
 #define EFM32_MSC_STATUS_ERASEABORTED_MASK 0x20
-#define EFM32_MSC_LOCK                  (EFM32_MSC_REGBASE+0x03c)
+#define EFM32_MSC_REG_LOCK              0x03c
+#define EFR32_MSC_REG_LOCK              0x040
 #define EFM32_MSC_LOCK_LOCKKEY          0x1b71
 
 struct efm32x_flash_bank {
 	int probed;
 	uint32_t lb_page[LOCKBITS_PAGE_SZ/4];
+	uint32_t reg_base;
+	uint32_t reg_lock;
 };
 
 struct efm32_info {
@@ -132,11 +138,30 @@ static int efm32x_get_prod_rev(struct flash_bank *bank, uint8_t *prev)
 	return target_read_u8(bank->target, EFM32_MSC_DI_PROD_REV, prev);
 }
 
+static int efm32x_read_reg_u32(struct flash_bank *bank, target_addr_t offset,
+			       uint32_t *value)
+{
+	struct efm32x_flash_bank *efm32x_info = bank->driver_priv;
+	uint32_t base = efm32x_info->reg_base;
+
+	return target_read_u32(bank->target, base + offset, value);
+}
+
+static int efm32x_write_reg_u32(struct flash_bank *bank, target_addr_t offset,
+			       uint32_t value)
+{
+	struct efm32x_flash_bank *efm32x_info = bank->driver_priv;
+	uint32_t base = efm32x_info->reg_base;
+
+	return target_write_u32(bank->target, base + offset, value);
+}
+
 static int efm32x_read_info(struct flash_bank *bank,
 	struct efm32_info *efm32_info)
 {
 	int ret;
 	uint32_t cpuid = 0;
+	struct efm32x_flash_bank *efm32x_info = bank->driver_priv;
 
 	memset(efm32_info, 0, sizeof(struct efm32_info));
 
@@ -175,6 +200,15 @@ static int efm32x_read_info(struct flash_bank *bank,
 	if (ERROR_OK != ret)
 		return ret;
 
+	if (EFR_FAMILY_ID_BLUE_GECKO == efm32_info->part_family ||
+	    EFR_FAMILY_ID_MIGHTY_GECKO == efm32_info->part_family) {
+		efm32x_info->reg_base = EFR32_MSC_REGBASE;
+		efm32x_info->reg_lock = EFR32_MSC_REG_LOCK;
+	} else {
+		efm32x_info->reg_base = EFM32_MSC_REGBASE;
+		efm32x_info->reg_lock = EFM32_MSC_REG_LOCK;
+	}
+
 	if (EFM_FAMILY_ID_GECKO == efm32_info->part_family ||
 			EFM_FAMILY_ID_TINY_GECKO == efm32_info->part_family)
 		efm32_info->page_size = 512;
@@ -208,7 +242,9 @@ static int efm32x_read_info(struct flash_bank *bank,
 		}
 	} else if (EFM_FAMILY_ID_WONDER_GECKO == efm32_info->part_family ||
 			EZR_FAMILY_ID_WONDER_GECKO == efm32_info->part_family ||
-			EZR_FAMILY_ID_LEOPARD_GECKO == efm32_info->part_family) {
+			EZR_FAMILY_ID_LEOPARD_GECKO == efm32_info->part_family ||
+			EFR_FAMILY_ID_BLUE_GECKO == efm32_info->part_family ||
+			EFR_FAMILY_ID_MIGHTY_GECKO == efm32_info->part_family) {
 		uint8_t pg_size = 0;
 		ret = target_read_u8(bank->target, EFM32_MSC_DI_PAGE_SIZE,
 			&pg_size);
@@ -241,6 +277,10 @@ static int efm32x_decode_info(struct efm32_info *info, char *buf, int buf_size)
 		case EZR_FAMILY_ID_HAPPY_GECKO:
 			printed = snprintf(buf, buf_size, "EZR32 ");
 			break;
+		case EFR_FAMILY_ID_MIGHTY_GECKO:
+		case EFR_FAMILY_ID_BLUE_GECKO:
+			printed = snprintf(buf, buf_size, "EFR32 ");
+			break;
 		default:
 			printed = snprintf(buf, buf_size, "EFM32 ");
 	}
@@ -276,6 +316,12 @@ static int efm32x_decode_info(struct efm32_info *info, char *buf, int buf_size)
 		case EZR_FAMILY_ID_HAPPY_GECKO:
 			printed = snprintf(buf, buf_size, "Happy Gecko");
 			break;
+		case EFR_FAMILY_ID_BLUE_GECKO:
+			printed = snprintf(buf, buf_size, "Blue Gecko");
+			break;
+		case EFR_FAMILY_ID_MIGHTY_GECKO:
+			printed = snprintf(buf, buf_size, "Mighty Gecko");
+			break;
 	}
 
 	buf += printed;
@@ -319,7 +365,7 @@ static int efm32x_set_reg_bits(struct flash_bank *bank, uint32_t reg,
 	int ret = 0;
 	uint32_t reg_val = 0;
 
-	ret = target_read_u32(bank->target, reg, &reg_val);
+	ret = efm32x_read_reg_u32(bank, reg, &reg_val);
 	if (ERROR_OK != ret)
 		return ret;
 
@@ -328,18 +374,19 @@ static int efm32x_set_reg_bits(struct flash_bank *bank, uint32_t reg,
 	else
 		reg_val &= ~bitmask;
 
-	return target_write_u32(bank->target, reg, reg_val);
+	return efm32x_write_reg_u32(bank, reg, reg_val);
 }
 
 static int efm32x_set_wren(struct flash_bank *bank, int write_enable)
 {
-	return efm32x_set_reg_bits(bank, EFM32_MSC_WRITECTRL,
+	return efm32x_set_reg_bits(bank, EFM32_MSC_REG_WRITECTRL,
 		EFM32_MSC_WRITECTRL_WREN_MASK, write_enable);
 }
 
 static int efm32x_msc_lock(struct flash_bank *bank, int lock)
 {
-	return target_write_u32(bank->target, EFM32_MSC_LOCK,
+	struct efm32x_flash_bank *efm32x_info = bank->driver_priv;
+	return efm32x_write_reg_u32(bank, efm32x_info->reg_lock,
 		(lock ? 0 : EFM32_MSC_LOCK_LOCKKEY));
 }
 
@@ -350,7 +397,7 @@ static int efm32x_wait_status(struct flash_bank *bank, int timeout,
 	uint32_t status = 0;
 
 	while (1) {
-		ret = target_read_u32(bank->target, EFM32_MSC_STATUS, &status);
+		ret = efm32x_read_reg_u32(bank, EFM32_MSC_REG_STATUS, &status);
 		if (ERROR_OK != ret)
 			break;
 
@@ -389,16 +436,16 @@ static int efm32x_erase_page(struct flash_bank *bank, uint32_t addr)
 
 	LOG_DEBUG("erasing flash page at 0x%08" PRIx32, addr);
 
-	ret = target_write_u32(bank->target, EFM32_MSC_ADDRB, addr);
+	ret = efm32x_write_reg_u32(bank, EFM32_MSC_REG_ADDRB, addr);
 	if (ERROR_OK != ret)
 		return ret;
 
-	ret = efm32x_set_reg_bits(bank, EFM32_MSC_WRITECMD,
+	ret = efm32x_set_reg_bits(bank, EFM32_MSC_REG_WRITECMD,
 		EFM32_MSC_WRITECMD_LADDRIM_MASK, 1);
 	if (ERROR_OK != ret)
 		return ret;
 
-	ret = target_read_u32(bank->target, EFM32_MSC_STATUS, &status);
+	ret = efm32x_read_reg_u32(bank, EFM32_MSC_REG_STATUS, &status);
 	if (ERROR_OK != ret)
 		return ret;
 
@@ -412,7 +459,7 @@ static int efm32x_erase_page(struct flash_bank *bank, uint32_t addr)
 		return ERROR_FAIL;
 	}
 
-	ret = efm32x_set_reg_bits(bank, EFM32_MSC_WRITECMD,
+	ret = efm32x_set_reg_bits(bank, EFM32_MSC_REG_WRITECMD,
 		EFM32_MSC_WRITECMD_ERASEPAGE_MASK, 1);
 	if (ERROR_OK != ret)
 		return ret;
@@ -589,6 +636,7 @@ static int efm32x_write_block(struct flash_bank *bank, const uint8_t *buf,
 	uint32_t address = bank->base + offset;
 	struct reg_param reg_params[5];
 	struct armv7m_algorithm armv7m_info;
+	struct efm32x_flash_bank *efm32x_info = bank->driver_priv;
 	int ret = ERROR_OK;
 
 	/* see contrib/loaders/flash/efm32.S for src */
@@ -598,10 +646,7 @@ static int efm32x_write_block(struct flash_bank *bank, const uint8_t *buf,
 		/* #define EFM32_MSC_ADDRB_OFFSET          0x010 */
 		/* #define EFM32_MSC_WDATA_OFFSET          0x018 */
 		/* #define EFM32_MSC_STATUS_OFFSET         0x01c */
-		/* #define EFM32_MSC_LOCK_OFFSET           0x03c */
 
-			0x15, 0x4e,    /* ldr     r6, =#0x1b71 */
-			0xc6, 0x63,    /* str     r6, [r0, #EFM32_MSC_LOCK_OFFSET] */
 			0x01, 0x26,    /* movs    r6, #1 */
 			0x86, 0x60,    /* str     r6, [r0, #EFM32_MSC_WRITECTRL_OFFSET] */
 
@@ -660,11 +705,9 @@ static int efm32x_write_block(struct flash_bank *bank, const uint8_t *buf,
 		/* exit: */
 			0x30, 0x46,    /* mov     r0, r6 */
 			0x00, 0xbe,    /* bkpt    #0 */
-
-		/* LOCKKEY */
-			0x71, 0x1b, 0x00, 0x00
 	};
 
+
 	/* flash write code */
 	if (target_alloc_working_area(target, sizeof(efm32x_flash_write_code),
 			&write_algorithm) != ERROR_OK) {
@@ -697,7 +740,7 @@ static int efm32x_write_block(struct flash_bank *bank, const uint8_t *buf,
 	init_reg_param(&reg_params[3], "r3", 32, PARAM_OUT);	/* buffer end */
 	init_reg_param(&reg_params[4], "r4", 32, PARAM_IN_OUT);	/* target address */
 
-	buf_set_u32(reg_params[0].value, 0, 32, EFM32_MSC_REGBASE);
+	buf_set_u32(reg_params[0].value, 0, 32, efm32x_info->reg_base);
 	buf_set_u32(reg_params[1].value, 0, 32, count);
 	buf_set_u32(reg_params[2].value, 0, 32, source->address);
 	buf_set_u32(reg_params[3].value, 0, 32, source->address + source->size);
@@ -762,16 +805,16 @@ static int efm32x_write_word(struct flash_bank *bank, uint32_t addr,
 	/* if not called, GDB errors will be reported during large writes */
 	keep_alive();
 
-	ret = target_write_u32(bank->target, EFM32_MSC_ADDRB, addr);
+	ret = efm32x_write_reg_u32(bank, EFM32_MSC_REG_ADDRB, addr);
 	if (ERROR_OK != ret)
 		return ret;
 
-	ret = efm32x_set_reg_bits(bank, EFM32_MSC_WRITECMD,
+	ret = efm32x_set_reg_bits(bank, EFM32_MSC_REG_WRITECMD,
 		EFM32_MSC_WRITECMD_LADDRIM_MASK, 1);
 	if (ERROR_OK != ret)
 		return ret;
 
-	ret = target_read_u32(bank->target, EFM32_MSC_STATUS, &status);
+	ret = efm32x_read_reg_u32(bank, EFM32_MSC_REG_STATUS, &status);
 	if (ERROR_OK != ret)
 		return ret;
 
@@ -792,13 +835,13 @@ static int efm32x_write_word(struct flash_bank *bank, uint32_t addr,
 		return ret;
 	}
 
-	ret = target_write_u32(bank->target, EFM32_MSC_WDATA, val);
+	ret = efm32x_write_reg_u32(bank, EFM32_MSC_REG_WDATA, val);
 	if (ERROR_OK != ret) {
 		LOG_ERROR("WDATA write failed");
 		return ret;
 	}
 
-	ret = target_write_u32(bank->target, EFM32_MSC_WRITECMD,
+	ret = efm32x_write_reg_u32(bank, EFM32_MSC_REG_WRITECMD,
 		EFM32_MSC_WRITECMD_WRITEONCE_MASK);
 	if (ERROR_OK != ret) {
 		LOG_ERROR("WRITECMD write failed");
diff --git a/src/flash/nor/kinetis.c b/src/flash/nor/kinetis.c
index 4ef4385..5c0ffbd 100644
--- a/src/flash/nor/kinetis.c
+++ b/src/flash/nor/kinetis.c
@@ -1959,7 +1959,7 @@ static int kinetis_probe_chip(struct kinetis_chip *k_chip)
 	unsigned cpu_mhz = 120;
 	unsigned idx;
 	bool use_nvm_marking = false;
-	char flash_marking[8], nvm_marking[2];
+	char flash_marking[11], nvm_marking[2];
 	char name[40];
 
 	k_chip->probed = false;
@@ -2126,6 +2126,7 @@ static int kinetis_probe_chip(struct kinetis_chip *k_chip)
 			case KINETIS_SDID_FAMILYID_K6X | KINETIS_SDID_SUBFAMID_KX1:	/* errata 7534 - should be K63 */
 			case KINETIS_SDID_FAMILYID_K6X | KINETIS_SDID_SUBFAMID_KX2:	/* errata 7534 - should be K64 */
 				subfamid += 2; /* errata 7534 fix */
+				/* fallthrough */
 			case KINETIS_SDID_FAMILYID_K6X | KINETIS_SDID_SUBFAMID_KX3:
 				/* K63FN1M0 */
 			case KINETIS_SDID_FAMILYID_K6X | KINETIS_SDID_SUBFAMID_KX4:
diff --git a/src/flash/nor/nrf51.c b/src/flash/nor/nrf5.c
index 7b7acf4..11e5729 100644
--- a/src/flash/nor/nrf51.c
+++ b/src/flash/nor/nrf5.c
@@ -28,97 +28,97 @@
 #include <helper/types.h>
 
 enum {
-	NRF51_FLASH_BASE = 0x00000000,
+	NRF5_FLASH_BASE = 0x00000000,
 };
 
-enum nrf51_ficr_registers {
-	NRF51_FICR_BASE = 0x10000000, /* Factory Information Configuration Registers */
-
-#define NRF51_FICR_REG(offset) (NRF51_FICR_BASE + offset)
-
-	NRF51_FICR_CODEPAGESIZE		= NRF51_FICR_REG(0x010),
-	NRF51_FICR_CODESIZE		= NRF51_FICR_REG(0x014),
-	NRF51_FICR_CLENR0		= NRF51_FICR_REG(0x028),
-	NRF51_FICR_PPFC			= NRF51_FICR_REG(0x02C),
-	NRF51_FICR_NUMRAMBLOCK		= NRF51_FICR_REG(0x034),
-	NRF51_FICR_SIZERAMBLOCK0	= NRF51_FICR_REG(0x038),
-	NRF51_FICR_SIZERAMBLOCK1	= NRF51_FICR_REG(0x03C),
-	NRF51_FICR_SIZERAMBLOCK2	= NRF51_FICR_REG(0x040),
-	NRF51_FICR_SIZERAMBLOCK3	= NRF51_FICR_REG(0x044),
-	NRF51_FICR_CONFIGID		= NRF51_FICR_REG(0x05C),
-	NRF51_FICR_DEVICEID0		= NRF51_FICR_REG(0x060),
-	NRF51_FICR_DEVICEID1		= NRF51_FICR_REG(0x064),
-	NRF51_FICR_ER0			= NRF51_FICR_REG(0x080),
-	NRF51_FICR_ER1			= NRF51_FICR_REG(0x084),
-	NRF51_FICR_ER2			= NRF51_FICR_REG(0x088),
-	NRF51_FICR_ER3			= NRF51_FICR_REG(0x08C),
-	NRF51_FICR_IR0			= NRF51_FICR_REG(0x090),
-	NRF51_FICR_IR1			= NRF51_FICR_REG(0x094),
-	NRF51_FICR_IR2			= NRF51_FICR_REG(0x098),
-	NRF51_FICR_IR3			= NRF51_FICR_REG(0x09C),
-	NRF51_FICR_DEVICEADDRTYPE	= NRF51_FICR_REG(0x0A0),
-	NRF51_FICR_DEVICEADDR0		= NRF51_FICR_REG(0x0A4),
-	NRF51_FICR_DEVICEADDR1		= NRF51_FICR_REG(0x0A8),
-	NRF51_FICR_OVERRIDEN		= NRF51_FICR_REG(0x0AC),
-	NRF51_FICR_NRF_1MBIT0		= NRF51_FICR_REG(0x0B0),
-	NRF51_FICR_NRF_1MBIT1		= NRF51_FICR_REG(0x0B4),
-	NRF51_FICR_NRF_1MBIT2		= NRF51_FICR_REG(0x0B8),
-	NRF51_FICR_NRF_1MBIT3		= NRF51_FICR_REG(0x0BC),
-	NRF51_FICR_NRF_1MBIT4		= NRF51_FICR_REG(0x0C0),
-	NRF51_FICR_BLE_1MBIT0		= NRF51_FICR_REG(0x0EC),
-	NRF51_FICR_BLE_1MBIT1		= NRF51_FICR_REG(0x0F0),
-	NRF51_FICR_BLE_1MBIT2		= NRF51_FICR_REG(0x0F4),
-	NRF51_FICR_BLE_1MBIT3		= NRF51_FICR_REG(0x0F8),
-	NRF51_FICR_BLE_1MBIT4		= NRF51_FICR_REG(0x0FC),
+enum nrf5_ficr_registers {
+	NRF5_FICR_BASE = 0x10000000, /* Factory Information Configuration Registers */
+
+#define NRF5_FICR_REG(offset) (NRF5_FICR_BASE + offset)
+
+	NRF5_FICR_CODEPAGESIZE		= NRF5_FICR_REG(0x010),
+	NRF5_FICR_CODESIZE		= NRF5_FICR_REG(0x014),
+	NRF5_FICR_CLENR0		= NRF5_FICR_REG(0x028),
+	NRF5_FICR_PPFC			= NRF5_FICR_REG(0x02C),
+	NRF5_FICR_NUMRAMBLOCK		= NRF5_FICR_REG(0x034),
+	NRF5_FICR_SIZERAMBLOCK0	= NRF5_FICR_REG(0x038),
+	NRF5_FICR_SIZERAMBLOCK1	= NRF5_FICR_REG(0x03C),
+	NRF5_FICR_SIZERAMBLOCK2	= NRF5_FICR_REG(0x040),
+	NRF5_FICR_SIZERAMBLOCK3	= NRF5_FICR_REG(0x044),
+	NRF5_FICR_CONFIGID		= NRF5_FICR_REG(0x05C),
+	NRF5_FICR_DEVICEID0		= NRF5_FICR_REG(0x060),
+	NRF5_FICR_DEVICEID1		= NRF5_FICR_REG(0x064),
+	NRF5_FICR_ER0			= NRF5_FICR_REG(0x080),
+	NRF5_FICR_ER1			= NRF5_FICR_REG(0x084),
+	NRF5_FICR_ER2			= NRF5_FICR_REG(0x088),
+	NRF5_FICR_ER3			= NRF5_FICR_REG(0x08C),
+	NRF5_FICR_IR0			= NRF5_FICR_REG(0x090),
+	NRF5_FICR_IR1			= NRF5_FICR_REG(0x094),
+	NRF5_FICR_IR2			= NRF5_FICR_REG(0x098),
+	NRF5_FICR_IR3			= NRF5_FICR_REG(0x09C),
+	NRF5_FICR_DEVICEADDRTYPE	= NRF5_FICR_REG(0x0A0),
+	NRF5_FICR_DEVICEADDR0		= NRF5_FICR_REG(0x0A4),
+	NRF5_FICR_DEVICEADDR1		= NRF5_FICR_REG(0x0A8),
+	NRF5_FICR_OVERRIDEN		= NRF5_FICR_REG(0x0AC),
+	NRF5_FICR_NRF_1MBIT0		= NRF5_FICR_REG(0x0B0),
+	NRF5_FICR_NRF_1MBIT1		= NRF5_FICR_REG(0x0B4),
+	NRF5_FICR_NRF_1MBIT2		= NRF5_FICR_REG(0x0B8),
+	NRF5_FICR_NRF_1MBIT3		= NRF5_FICR_REG(0x0BC),
+	NRF5_FICR_NRF_1MBIT4		= NRF5_FICR_REG(0x0C0),
+	NRF5_FICR_BLE_1MBIT0		= NRF5_FICR_REG(0x0EC),
+	NRF5_FICR_BLE_1MBIT1		= NRF5_FICR_REG(0x0F0),
+	NRF5_FICR_BLE_1MBIT2		= NRF5_FICR_REG(0x0F4),
+	NRF5_FICR_BLE_1MBIT3		= NRF5_FICR_REG(0x0F8),
+	NRF5_FICR_BLE_1MBIT4		= NRF5_FICR_REG(0x0FC),
 };
 
-enum nrf51_uicr_registers {
-	NRF51_UICR_BASE = 0x10001000, /* User Information
+enum nrf5_uicr_registers {
+	NRF5_UICR_BASE = 0x10001000, /* User Information
 				       * Configuration Regsters */
 
-	NRF51_UICR_SIZE = 0x100,
+	NRF5_UICR_SIZE = 0x100,
 
-#define NRF51_UICR_REG(offset) (NRF51_UICR_BASE + offset)
+#define NRF5_UICR_REG(offset) (NRF5_UICR_BASE + offset)
 
-	NRF51_UICR_CLENR0	= NRF51_UICR_REG(0x000),
-	NRF51_UICR_RBPCONF	= NRF51_UICR_REG(0x004),
-	NRF51_UICR_XTALFREQ	= NRF51_UICR_REG(0x008),
-	NRF51_UICR_FWID		= NRF51_UICR_REG(0x010),
+	NRF5_UICR_CLENR0	= NRF5_UICR_REG(0x000),
+	NRF5_UICR_RBPCONF	= NRF5_UICR_REG(0x004),
+	NRF5_UICR_XTALFREQ	= NRF5_UICR_REG(0x008),
+	NRF5_UICR_FWID		= NRF5_UICR_REG(0x010),
 };
 
-enum nrf51_nvmc_registers {
-	NRF51_NVMC_BASE = 0x4001E000, /* Non-Volatile Memory
+enum nrf5_nvmc_registers {
+	NRF5_NVMC_BASE = 0x4001E000, /* Non-Volatile Memory
 				       * Controller Regsters */
 
-#define NRF51_NVMC_REG(offset) (NRF51_NVMC_BASE + offset)
+#define NRF5_NVMC_REG(offset) (NRF5_NVMC_BASE + offset)
 
-	NRF51_NVMC_READY	= NRF51_NVMC_REG(0x400),
-	NRF51_NVMC_CONFIG	= NRF51_NVMC_REG(0x504),
-	NRF51_NVMC_ERASEPAGE	= NRF51_NVMC_REG(0x508),
-	NRF51_NVMC_ERASEALL	= NRF51_NVMC_REG(0x50C),
-	NRF51_NVMC_ERASEUICR	= NRF51_NVMC_REG(0x514),
+	NRF5_NVMC_READY	= NRF5_NVMC_REG(0x400),
+	NRF5_NVMC_CONFIG	= NRF5_NVMC_REG(0x504),
+	NRF5_NVMC_ERASEPAGE	= NRF5_NVMC_REG(0x508),
+	NRF5_NVMC_ERASEALL	= NRF5_NVMC_REG(0x50C),
+	NRF5_NVMC_ERASEUICR	= NRF5_NVMC_REG(0x514),
 };
 
-enum nrf51_nvmc_config_bits {
-	NRF51_NVMC_CONFIG_REN = 0x00,
-	NRF51_NVMC_CONFIG_WEN = 0x01,
-	NRF51_NVMC_CONFIG_EEN = 0x02,
+enum nrf5_nvmc_config_bits {
+	NRF5_NVMC_CONFIG_REN = 0x00,
+	NRF5_NVMC_CONFIG_WEN = 0x01,
+	NRF5_NVMC_CONFIG_EEN = 0x02,
 
 };
 
-struct nrf51_info {
+struct nrf5_info {
 	uint32_t code_page_size;
 
 	struct {
 		bool probed;
 		int (*write) (struct flash_bank *bank,
-			      struct nrf51_info *chip,
+			      struct nrf5_info *chip,
 			      const uint8_t *buffer, uint32_t offset, uint32_t count);
 	} bank[2];
 	struct target *target;
 };
 
-struct nrf51_device_spec {
+struct nrf5_device_spec {
 	uint16_t hwid;
 	const char *part;
 	const char *variant;
@@ -126,6 +126,15 @@ struct nrf51_device_spec {
 	unsigned int flash_size_kb;
 };
 
+#define NRF5_DEVICE_DEF(id, pt, var, bcode, fsize) \
+{                                                   \
+.hwid          = (id),                              \
+.part          = pt,                                \
+.variant       = var,                               \
+.build_code    = bcode,                             \
+.flash_size_kb = (fsize),                           \
+}
+
 /* The known devices table below is derived from the "nRF51 Series
  * Compatibility Matrix" document, which can be found by searching for
  * ATTN-51 on the Nordic Semi website:
@@ -138,294 +147,74 @@ struct nrf51_device_spec {
  * shown as Gx0, Bx0, etc. In these cases the HWID in the matrix is
  * for x==0, x!=0 means different (unspecified) HWIDs.
  */
-static const struct nrf51_device_spec nrf51_known_devices_table[] = {
+static const struct nrf5_device_spec nrf5_known_devices_table[] = {
 	/* nRF51822 Devices (IC rev 1). */
-	{
-		.hwid		= 0x001D,
-		.part		= "51822",
-		.variant	= "QFAA",
-		.build_code	= "CA/C0",
-		.flash_size_kb	= 256,
-	},
-	{
-		.hwid		= 0x0026,
-		.part		= "51822",
-		.variant	= "QFAB",
-		.build_code	= "AA",
-		.flash_size_kb	= 128,
-	},
-	{
-		.hwid		= 0x0027,
-		.part		= "51822",
-		.variant	= "QFAB",
-		.build_code	= "A0",
-		.flash_size_kb	= 128,
-	},
-	{
-		.hwid		= 0x0020,
-		.part		= "51822",
-		.variant	= "CEAA",
-		.build_code	= "BA",
-		.flash_size_kb	= 256,
-	},
-	{
-		.hwid		= 0x002F,
-		.part		= "51822",
-		.variant	= "CEAA",
-		.build_code	= "B0",
-		.flash_size_kb	= 256,
-	},
+	NRF5_DEVICE_DEF(0x001D, "51822", "QFAA", "CA/C0", 256),
+	NRF5_DEVICE_DEF(0x0026, "51822", "QFAB", "AA",    128),
+	NRF5_DEVICE_DEF(0x0027, "51822", "QFAB", "A0",    128),
+	NRF5_DEVICE_DEF(0x0020, "51822", "CEAA", "BA",    256),
+	NRF5_DEVICE_DEF(0x002F, "51822", "CEAA", "B0",    256),
 
 	/* nRF51822 Devices (IC rev 2). */
-	{
-		.hwid		= 0x002A,
-		.part		= "51822",
-		.variant	= "QFAA",
-		.build_code	= "FA0",
-		.flash_size_kb	= 256,
-	},
-	{
-		.hwid		= 0x0044,
-		.part		= "51822",
-		.variant	= "QFAA",
-		.build_code	= "GC0",
-		.flash_size_kb	= 256,
-	},
-	{
-		.hwid		= 0x003C,
-		.part		= "51822",
-		.variant	= "QFAA",
-		.build_code	= "G0",
-		.flash_size_kb	= 256,
-	},
-	{
-		.hwid		= 0x0057,
-		.part		= "51822",
-		.variant	= "QFAA",
-		.build_code	= "G2",
-		.flash_size_kb	= 256,
-	},
-	{
-		.hwid		= 0x0058,
-		.part		= "51822",
-		.variant	= "QFAA",
-		.build_code	= "G3",
-		.flash_size_kb	= 256,
-	},
-	{
-		.hwid		= 0x004C,
-		.part		= "51822",
-		.variant	= "QFAB",
-		.build_code	= "B0",
-		.flash_size_kb	= 128,
-	},
-	{
-		.hwid		= 0x0040,
-		.part		= "51822",
-		.variant	= "CEAA",
-		.build_code	= "CA0",
-		.flash_size_kb	= 256,
-	},
-	{
-		.hwid		= 0x0047,
-		.part		= "51822",
-		.variant	= "CEAA",
-		.build_code	= "DA0",
-		.flash_size_kb	= 256,
-	},
-	{
-		.hwid		= 0x004D,
-		.part		= "51822",
-		.variant	= "CEAA",
-		.build_code	= "D00",
-		.flash_size_kb	= 256,
-	},
+	NRF5_DEVICE_DEF(0x002A, "51822", "QFAA", "FA0",   256),
+	NRF5_DEVICE_DEF(0x0044, "51822", "QFAA", "GC0",   256),
+	NRF5_DEVICE_DEF(0x003C, "51822", "QFAA", "G0",    256),
+	NRF5_DEVICE_DEF(0x0057, "51822", "QFAA", "G2",    256),
+	NRF5_DEVICE_DEF(0x0058, "51822", "QFAA", "G3",    256),
+	NRF5_DEVICE_DEF(0x004C, "51822", "QFAB", "B0",    128),
+	NRF5_DEVICE_DEF(0x0040, "51822", "CEAA", "CA0",   256),
+	NRF5_DEVICE_DEF(0x0047, "51822", "CEAA", "DA0",   256),
+	NRF5_DEVICE_DEF(0x004D, "51822", "CEAA", "D00",   256),
 
 	/* nRF51822 Devices (IC rev 3). */
-	{
-		.hwid		= 0x0072,
-		.part		= "51822",
-		.variant	= "QFAA",
-		.build_code	= "H0",
-		.flash_size_kb	= 256,
-	},
-	{
-		.hwid		= 0x007B,
-		.part		= "51822",
-		.variant	= "QFAB",
-		.build_code	= "C0",
-		.flash_size_kb	= 128,
-	},
-	{
-		.hwid		= 0x0083,
-		.part		= "51822",
-		.variant	= "QFAC",
-		.build_code	= "A0",
-		.flash_size_kb	= 256,
-	},
-	{
-		.hwid		= 0x0084,
-		.part		= "51822",
-		.variant	= "QFAC",
-		.build_code	= "A1",
-		.flash_size_kb	= 256,
-	},
-	{
-		.hwid		= 0x007D,
-		.part		= "51822",
-		.variant	= "CDAB",
-		.build_code	= "A0",
-		.flash_size_kb	= 128,
-	},
-	{
-		.hwid		= 0x0079,
-		.part		= "51822",
-		.variant	= "CEAA",
-		.build_code	= "E0",
-		.flash_size_kb	= 256,
-	},
-	{
-		.hwid		= 0x0087,
-		.part		= "51822",
-		.variant	= "CFAC",
-		.build_code	= "A0",
-		.flash_size_kb	= 256,
-	},
-	{
-		.hwid		= 0x008F,
-		.part		= "51822",
-		.variant	= "QFAA",
-		.build_code	= "H1",
-		.flash_size_kb	= 256,
-	},
+	NRF5_DEVICE_DEF(0x0072, "51822", "QFAA", "H0",    256),
+	NRF5_DEVICE_DEF(0x007B, "51822", "QFAB", "C0",    128),
+	NRF5_DEVICE_DEF(0x0083, "51822", "QFAC", "A0",    256),
+	NRF5_DEVICE_DEF(0x0084, "51822", "QFAC", "A1",    256),
+	NRF5_DEVICE_DEF(0x007D, "51822", "CDAB", "A0",    128),
+	NRF5_DEVICE_DEF(0x0079, "51822", "CEAA", "E0",    256),
+	NRF5_DEVICE_DEF(0x0087, "51822", "CFAC", "A0",    256),
 
 	/* nRF51422 Devices (IC rev 1). */
-	{
-		.hwid		= 0x001E,
-		.part		= "51422",
-		.variant	= "QFAA",
-		.build_code	= "CA",
-		.flash_size_kb	= 256,
-	},
-	{
-		.hwid		= 0x0024,
-		.part		= "51422",
-		.variant	= "QFAA",
-		.build_code	= "C0",
-		.flash_size_kb	= 256,
-	},
-	{
-		.hwid		= 0x0031,
-		.part		= "51422",
-		.variant	= "CEAA",
-		.build_code	= "A0A",
-		.flash_size_kb	= 256,
-	},
+	NRF5_DEVICE_DEF(0x001E, "51422", "QFAA", "CA",    256),
+	NRF5_DEVICE_DEF(0x0024, "51422", "QFAA", "C0",    256),
+	NRF5_DEVICE_DEF(0x0031, "51422", "CEAA", "A0A",   256),
 
 	/* nRF51422 Devices (IC rev 2). */
-	{
-		.hwid		= 0x002D,
-		.part		= "51422",
-		.variant	= "QFAA",
-		.build_code	= "DAA",
-		.flash_size_kb	= 256,
-	},
-	{
-		.hwid		= 0x002E,
-		.part		= "51422",
-		.variant	= "QFAA",
-		.build_code	= "E0",
-		.flash_size_kb	= 256,
-	},
-	{
-		.hwid		= 0x0061,
-		.part		= "51422",
-		.variant	= "QFAB",
-		.build_code	= "A00",
-		.flash_size_kb	= 128,
-	},
-	{
-		.hwid		= 0x0050,
-		.part		= "51422",
-		.variant	= "CEAA",
-		.build_code	= "B0",
-		.flash_size_kb	= 256,
-	},
+	NRF5_DEVICE_DEF(0x002D, "51422", "QFAA", "DAA",   256),
+	NRF5_DEVICE_DEF(0x002E, "51422", "QFAA", "E0",    256),
+	NRF5_DEVICE_DEF(0x0061, "51422", "QFAB", "A00",   128),
+	NRF5_DEVICE_DEF(0x0050, "51422", "CEAA", "B0",    256),
 
 	/* nRF51422 Devices (IC rev 3). */
-	{
-		.hwid		= 0x0073,
-		.part		= "51422",
-		.variant	= "QFAA",
-		.build_code	= "F0",
-		.flash_size_kb	= 256,
-	},
-	{
-		.hwid		= 0x007C,
-		.part		= "51422",
-		.variant	= "QFAB",
-		.build_code	= "B0",
-		.flash_size_kb	= 128,
-	},
-	{
-		.hwid		= 0x0085,
-		.part		= "51422",
-		.variant	= "QFAC",
-		.build_code	= "A0",
-		.flash_size_kb	= 256,
-	},
-	{
-		.hwid		= 0x0086,
-		.part		= "51422",
-		.variant	= "QFAC",
-		.build_code	= "A1",
-		.flash_size_kb	= 256,
-	},
-	{
-		.hwid		= 0x007E,
-		.part		= "51422",
-		.variant	= "CDAB",
-		.build_code	= "A0",
-		.flash_size_kb	= 128,
-	},
-	{
-		.hwid		= 0x007A,
-		.part		= "51422",
-		.variant	= "CEAA",
-		.build_code	= "C0",
-		.flash_size_kb	= 256,
-	},
-	{
-		.hwid		= 0x0088,
-		.part		= "51422",
-		.variant	= "CFAC",
-		.build_code	= "A0",
-		.flash_size_kb	= 256,
-	},
+	NRF5_DEVICE_DEF(0x0073, "51422", "QFAA", "F0",    256),
+	NRF5_DEVICE_DEF(0x007C, "51422", "QFAB", "B0",    128),
+	NRF5_DEVICE_DEF(0x0085, "51422", "QFAC", "A0",    256),
+	NRF5_DEVICE_DEF(0x0086, "51422", "QFAC", "A1",    256),
+	NRF5_DEVICE_DEF(0x007E, "51422", "CDAB", "A0",    128),
+	NRF5_DEVICE_DEF(0x007A, "51422", "CEAA", "C0",    256),
+	NRF5_DEVICE_DEF(0x0088, "51422", "CFAC", "A0",    256),
+
+	/* nRF52832 Devices */
+	NRF5_DEVICE_DEF(0x00C7, "52832", "QFAA", "B0",    512),
 
 	/* Some early nRF51-DK (PCA10028) & nRF51-Dongle (PCA10031) boards
 	   with built-in jlink seem to use engineering samples not listed
 	   in the nRF51 Series Compatibility Matrix V1.0. */
-	{
-		.hwid		= 0x0071,
-		.part		= "51822",
-		.variant	= "QFAC",
-		.build_code	= "AB",
-		.flash_size_kb	= 256,
-	},
+	NRF5_DEVICE_DEF(0x0071, "51822", "QFAC", "AB",    256),
 };
 
-static int nrf51_bank_is_probed(struct flash_bank *bank)
+static int nrf5_bank_is_probed(struct flash_bank *bank)
 {
-	struct nrf51_info *chip = bank->driver_priv;
+	struct nrf5_info *chip = bank->driver_priv;
 
 	assert(chip != NULL);
 
 	return chip->bank[bank->bank_number].probed;
 }
-static int nrf51_probe(struct flash_bank *bank);
+static int nrf5_probe(struct flash_bank *bank);
 
-static int nrf51_get_probed_chip_if_halted(struct flash_bank *bank, struct nrf51_info **chip)
+static int nrf5_get_probed_chip_if_halted(struct flash_bank *bank, struct nrf5_info **chip)
 {
 	if (bank->target->state != TARGET_HALTED) {
 		LOG_ERROR("Target not halted");
@@ -434,23 +223,23 @@ static int nrf51_get_probed_chip_if_halted(struct flash_bank *bank, struct nrf51
 
 	*chip = bank->driver_priv;
 
-	int probed = nrf51_bank_is_probed(bank);
+	int probed = nrf5_bank_is_probed(bank);
 	if (probed < 0)
 		return probed;
 	else if (!probed)
-		return nrf51_probe(bank);
+		return nrf5_probe(bank);
 	else
 		return ERROR_OK;
 }
 
-static int nrf51_wait_for_nvmc(struct nrf51_info *chip)
+static int nrf5_wait_for_nvmc(struct nrf5_info *chip)
 {
 	uint32_t ready;
 	int res;
 	int timeout = 100;
 
 	do {
-		res = target_read_u32(chip->target, NRF51_NVMC_READY, &ready);
+		res = target_read_u32(chip->target, NRF5_NVMC_READY, &ready);
 		if (res != ERROR_OK) {
 			LOG_ERROR("Couldn't read NVMC_READY register");
 			return res;
@@ -466,12 +255,12 @@ static int nrf51_wait_for_nvmc(struct nrf51_info *chip)
 	return ERROR_FLASH_BUSY;
 }
 
-static int nrf51_nvmc_erase_enable(struct nrf51_info *chip)
+static int nrf5_nvmc_erase_enable(struct nrf5_info *chip)
 {
 	int res;
 	res = target_write_u32(chip->target,
-			       NRF51_NVMC_CONFIG,
-			       NRF51_NVMC_CONFIG_EEN);
+			       NRF5_NVMC_CONFIG,
+			       NRF5_NVMC_CONFIG_EEN);
 
 	if (res != ERROR_OK) {
 		LOG_ERROR("Failed to enable erase operation");
@@ -482,19 +271,19 @@ static int nrf51_nvmc_erase_enable(struct nrf51_info *chip)
 	  According to NVMC examples in Nordic SDK busy status must be
 	  checked after writing to NVMC_CONFIG
 	 */
-	res = nrf51_wait_for_nvmc(chip);
+	res = nrf5_wait_for_nvmc(chip);
 	if (res != ERROR_OK)
 		LOG_ERROR("Erase enable did not complete");
 
 	return res;
 }
 
-static int nrf51_nvmc_write_enable(struct nrf51_info *chip)
+static int nrf5_nvmc_write_enable(struct nrf5_info *chip)
 {
 	int res;
 	res = target_write_u32(chip->target,
-			       NRF51_NVMC_CONFIG,
-			       NRF51_NVMC_CONFIG_WEN);
+			       NRF5_NVMC_CONFIG,
+			       NRF5_NVMC_CONFIG_WEN);
 
 	if (res != ERROR_OK) {
 		LOG_ERROR("Failed to enable write operation");
@@ -505,19 +294,19 @@ static int nrf51_nvmc_write_enable(struct nrf51_info *chip)
 	  According to NVMC examples in Nordic SDK busy status must be
 	  checked after writing to NVMC_CONFIG
 	 */
-	res = nrf51_wait_for_nvmc(chip);
+	res = nrf5_wait_for_nvmc(chip);
 	if (res != ERROR_OK)
 		LOG_ERROR("Write enable did not complete");
 
 	return res;
 }
 
-static int nrf51_nvmc_read_only(struct nrf51_info *chip)
+static int nrf5_nvmc_read_only(struct nrf5_info *chip)
 {
 	int res;
 	res = target_write_u32(chip->target,
-			       NRF51_NVMC_CONFIG,
-			       NRF51_NVMC_CONFIG_REN);
+			       NRF5_NVMC_CONFIG,
+			       NRF5_NVMC_CONFIG_REN);
 
 	if (res != ERROR_OK) {
 		LOG_ERROR("Failed to enable read-only operation");
@@ -527,19 +316,19 @@ static int nrf51_nvmc_read_only(struct nrf51_info *chip)
 	  According to NVMC examples in Nordic SDK busy status must be
 	  checked after writing to NVMC_CONFIG
 	 */
-	res = nrf51_wait_for_nvmc(chip);
+	res = nrf5_wait_for_nvmc(chip);
 	if (res != ERROR_OK)
 		LOG_ERROR("Read only enable did not complete");
 
 	return res;
 }
 
-static int nrf51_nvmc_generic_erase(struct nrf51_info *chip,
+static int nrf5_nvmc_generic_erase(struct nrf5_info *chip,
 			       uint32_t erase_register, uint32_t erase_value)
 {
 	int res;
 
-	res = nrf51_nvmc_erase_enable(chip);
+	res = nrf5_nvmc_erase_enable(chip);
 	if (res != ERROR_OK)
 		goto error;
 
@@ -549,34 +338,34 @@ static int nrf51_nvmc_generic_erase(struct nrf51_info *chip,
 	if (res != ERROR_OK)
 		goto set_read_only;
 
-	res = nrf51_wait_for_nvmc(chip);
+	res = nrf5_wait_for_nvmc(chip);
 	if (res != ERROR_OK)
 		goto set_read_only;
 
-	return nrf51_nvmc_read_only(chip);
+	return nrf5_nvmc_read_only(chip);
 
 set_read_only:
-	nrf51_nvmc_read_only(chip);
+	nrf5_nvmc_read_only(chip);
 error:
 	LOG_ERROR("Failed to erase reg: 0x%08"PRIx32" val: 0x%08"PRIx32,
 		  erase_register, erase_value);
 	return ERROR_FAIL;
 }
 
-static int nrf51_protect_check(struct flash_bank *bank)
+static int nrf5_protect_check(struct flash_bank *bank)
 {
 	int res;
 	uint32_t clenr0;
 
 	/* UICR cannot be write protected so just return early */
-	if (bank->base == NRF51_UICR_BASE)
+	if (bank->base == NRF5_UICR_BASE)
 		return ERROR_OK;
 
-	struct nrf51_info *chip = bank->driver_priv;
+	struct nrf5_info *chip = bank->driver_priv;
 
 	assert(chip != NULL);
 
-	res = target_read_u32(chip->target, NRF51_FICR_CLENR0,
+	res = target_read_u32(chip->target, NRF5_FICR_CLENR0,
 			      &clenr0);
 	if (res != ERROR_OK) {
 		LOG_ERROR("Couldn't read code region 0 size[FICR]");
@@ -584,7 +373,7 @@ static int nrf51_protect_check(struct flash_bank *bank)
 	}
 
 	if (clenr0 == 0xFFFFFFFF) {
-		res = target_read_u32(chip->target, NRF51_UICR_CLENR0,
+		res = target_read_u32(chip->target, NRF5_UICR_CLENR0,
 				      &clenr0);
 		if (res != ERROR_OK) {
 			LOG_ERROR("Couldn't read code region 0 size[UICR]");
@@ -599,17 +388,17 @@ static int nrf51_protect_check(struct flash_bank *bank)
 	return ERROR_OK;
 }
 
-static int nrf51_protect(struct flash_bank *bank, int set, int first, int last)
+static int nrf5_protect(struct flash_bank *bank, int set, int first, int last)
 {
 	int res;
 	uint32_t clenr0, ppfc;
-	struct nrf51_info *chip;
+	struct nrf5_info *chip;
 
 	/* UICR cannot be write protected so just bail out early */
-	if (bank->base == NRF51_UICR_BASE)
+	if (bank->base == NRF5_UICR_BASE)
 		return ERROR_FAIL;
 
-	res = nrf51_get_probed_chip_if_halted(bank, &chip);
+	res = nrf5_get_probed_chip_if_halted(bank, &chip);
 	if (res != ERROR_OK)
 		return res;
 
@@ -618,7 +407,7 @@ static int nrf51_protect(struct flash_bank *bank, int set, int first, int last)
 		return ERROR_FAIL;
 	}
 
-	res = target_read_u32(chip->target, NRF51_FICR_PPFC,
+	res = target_read_u32(chip->target, NRF5_FICR_PPFC,
 			      &ppfc);
 	if (res != ERROR_OK) {
 		LOG_ERROR("Couldn't read PPFC register");
@@ -630,7 +419,7 @@ static int nrf51_protect(struct flash_bank *bank, int set, int first, int last)
 		return ERROR_FAIL;
 	}
 
-	res = target_read_u32(chip->target, NRF51_UICR_CLENR0,
+	res = target_read_u32(chip->target, NRF5_UICR_CLENR0,
 			      &clenr0);
 	if (res != ERROR_OK) {
 		LOG_ERROR("Couldn't read code region 0 size[UICR]");
@@ -638,7 +427,7 @@ static int nrf51_protect(struct flash_bank *bank, int set, int first, int last)
 	}
 
 	if (clenr0 == 0xFFFFFFFF) {
-		res = target_write_u32(chip->target, NRF51_UICR_CLENR0,
+		res = target_write_u32(chip->target, NRF5_UICR_CLENR0,
 				       clenr0);
 		if (res != ERROR_OK) {
 			LOG_ERROR("Couldn't write code region 0 size[UICR]");
@@ -649,18 +438,18 @@ static int nrf51_protect(struct flash_bank *bank, int set, int first, int last)
 		LOG_ERROR("You need to perform chip erase before changing the protection settings");
 	}
 
-	nrf51_protect_check(bank);
+	nrf5_protect_check(bank);
 
 	return ERROR_OK;
 }
 
-static int nrf51_probe(struct flash_bank *bank)
+static int nrf5_probe(struct flash_bank *bank)
 {
 	uint32_t hwid;
 	int res;
-	struct nrf51_info *chip = bank->driver_priv;
+	struct nrf5_info *chip = bank->driver_priv;
 
-	res = target_read_u32(chip->target, NRF51_FICR_CONFIGID, &hwid);
+	res = target_read_u32(chip->target, NRF5_FICR_CONFIGID, &hwid);
 	if (res != ERROR_OK) {
 		LOG_ERROR("Couldn't read CONFIGID register");
 		return res;
@@ -669,10 +458,10 @@ static int nrf51_probe(struct flash_bank *bank)
 	hwid &= 0xFFFF;	/* HWID is stored in the lower two
 			 * bytes of the CONFIGID register */
 
-	const struct nrf51_device_spec *spec = NULL;
-	for (size_t i = 0; i < ARRAY_SIZE(nrf51_known_devices_table); i++) {
-		if (hwid == nrf51_known_devices_table[i].hwid) {
-			spec = &nrf51_known_devices_table[i];
+	const struct nrf5_device_spec *spec = NULL;
+	for (size_t i = 0; i < ARRAY_SIZE(nrf5_known_devices_table); i++) {
+		if (hwid == nrf5_known_devices_table[i].hwid) {
+			spec = &nrf5_known_devices_table[i];
 			break;
 		}
 	}
@@ -686,9 +475,9 @@ static int nrf51_probe(struct flash_bank *bank)
 			LOG_WARNING("Unknown device (HWID 0x%08" PRIx32 ")", hwid);
 	}
 
-	if (bank->base == NRF51_FLASH_BASE) {
-		/* The value stored in NRF51_FICR_CODEPAGESIZE is the number of bytes in one page of FLASH. */
-		res = target_read_u32(chip->target, NRF51_FICR_CODEPAGESIZE,
+	if (bank->base == NRF5_FLASH_BASE) {
+		/* The value stored in NRF5_FICR_CODEPAGESIZE is the number of bytes in one page of FLASH. */
+		res = target_read_u32(chip->target, NRF5_FICR_CODEPAGESIZE,
 				&chip->code_page_size);
 		if (res != ERROR_OK) {
 			LOG_ERROR("Couldn't read code page size");
@@ -696,9 +485,9 @@ static int nrf51_probe(struct flash_bank *bank)
 		}
 
 		/* Note the register name is misleading,
-		 * NRF51_FICR_CODESIZE is the number of pages in flash memory, not the number of bytes! */
+		 * NRF5_FICR_CODESIZE is the number of pages in flash memory, not the number of bytes! */
 		uint32_t num_sectors;
-		res = target_read_u32(chip->target, NRF51_FICR_CODESIZE, &num_sectors);
+		res = target_read_u32(chip->target, NRF5_FICR_CODESIZE, &num_sectors);
 		if (res != ERROR_OK) {
 			LOG_ERROR("Couldn't read code memory size");
 			return res;
@@ -715,7 +504,7 @@ static int nrf51_probe(struct flash_bank *bank)
 		if (!bank->sectors)
 			return ERROR_FLASH_BANK_NOT_PROBED;
 
-		/* Fill out the sector information: all NRF51 sectors are the same size and
+		/* Fill out the sector information: all NRF5 sectors are the same size and
 		 * there is always a fixed number of them. */
 		for (int i = 0; i < bank->num_sectors; i++) {
 			bank->sectors[i].size = chip->code_page_size;
@@ -726,11 +515,11 @@ static int nrf51_probe(struct flash_bank *bank)
 			bank->sectors[i].is_protected = -1;
 		}
 
-		nrf51_protect_check(bank);
+		nrf5_protect_check(bank);
 
 		chip->bank[0].probed = true;
 	} else {
-		bank->size = NRF51_UICR_SIZE;
+		bank->size = NRF5_UICR_SIZE;
 		bank->num_sectors = 1;
 		bank->sectors = calloc(bank->num_sectors,
 				       sizeof((bank->sectors)[0]));
@@ -750,21 +539,21 @@ static int nrf51_probe(struct flash_bank *bank)
 	return ERROR_OK;
 }
 
-static int nrf51_auto_probe(struct flash_bank *bank)
+static int nrf5_auto_probe(struct flash_bank *bank)
 {
-	int probed = nrf51_bank_is_probed(bank);
+	int probed = nrf5_bank_is_probed(bank);
 
 	if (probed < 0)
 		return probed;
 	else if (probed)
 		return ERROR_OK;
 	else
-		return nrf51_probe(bank);
+		return nrf5_probe(bank);
 }
 
-static struct flash_sector *nrf51_find_sector_by_address(struct flash_bank *bank, uint32_t address)
+static struct flash_sector *nrf5_find_sector_by_address(struct flash_bank *bank, uint32_t address)
 {
-	struct nrf51_info *chip = bank->driver_priv;
+	struct nrf5_info *chip = bank->driver_priv;
 
 	for (int i = 0; i < bank->num_sectors; i++)
 		if (bank->sectors[i].offset <= address &&
@@ -773,16 +562,16 @@ static struct flash_sector *nrf51_find_sector_by_address(struct flash_bank *bank
 	return NULL;
 }
 
-static int nrf51_erase_all(struct nrf51_info *chip)
+static int nrf5_erase_all(struct nrf5_info *chip)
 {
 	LOG_DEBUG("Erasing all non-volatile memory");
-	return nrf51_nvmc_generic_erase(chip,
-					NRF51_NVMC_ERASEALL,
+	return nrf5_nvmc_generic_erase(chip,
+					NRF5_NVMC_ERASEALL,
 					0x00000001);
 }
 
-static int nrf51_erase_page(struct flash_bank *bank,
-							struct nrf51_info *chip,
+static int nrf5_erase_page(struct flash_bank *bank,
+							struct nrf5_info *chip,
 							struct flash_sector *sector)
 {
 	int res;
@@ -793,9 +582,9 @@ static int nrf51_erase_page(struct flash_bank *bank,
 		return ERROR_FAIL;
 	}
 
-	if (bank->base == NRF51_UICR_BASE) {
+	if (bank->base == NRF5_UICR_BASE) {
 		uint32_t ppfc;
-		res = target_read_u32(chip->target, NRF51_FICR_PPFC,
+		res = target_read_u32(chip->target, NRF5_FICR_PPFC,
 				      &ppfc);
 		if (res != ERROR_OK) {
 			LOG_ERROR("Couldn't read PPFC register");
@@ -813,14 +602,14 @@ static int nrf51_erase_page(struct flash_bank *bank,
 			return ERROR_FAIL;
 		}
 
-		res = nrf51_nvmc_generic_erase(chip,
-					       NRF51_NVMC_ERASEUICR,
+		res = nrf5_nvmc_generic_erase(chip,
+					       NRF5_NVMC_ERASEUICR,
 					       0x00000001);
 
 
 	} else {
-		res = nrf51_nvmc_generic_erase(chip,
-					       NRF51_NVMC_ERASEPAGE,
+		res = nrf5_nvmc_generic_erase(chip,
+					       NRF5_NVMC_ERASEPAGE,
 					       sector->offset);
 	}
 
@@ -830,7 +619,7 @@ static int nrf51_erase_page(struct flash_bank *bank,
 	return res;
 }
 
-static const uint8_t nrf51_flash_write_code[] = {
+static const uint8_t nrf5_flash_write_code[] = {
 	/* See contrib/loaders/flash/cortex-m0.S */
 /* <wait_fifo>: */
 	0x0d, 0x68,		/* ldr	r5,	[r1,	#0] */
@@ -855,13 +644,13 @@ static const uint8_t nrf51_flash_write_code[] = {
 
 
 /* Start a low level flash write for the specified region */
-static int nrf51_ll_flash_write(struct nrf51_info *chip, uint32_t offset, const uint8_t *buffer, uint32_t bytes)
+static int nrf5_ll_flash_write(struct nrf5_info *chip, uint32_t offset, const uint8_t *buffer, uint32_t bytes)
 {
 	struct target *target = chip->target;
 	uint32_t buffer_size = 8192;
 	struct working_area *write_algorithm;
 	struct working_area *source;
-	uint32_t address = NRF51_FLASH_BASE + offset;
+	uint32_t address = NRF5_FLASH_BASE + offset;
 	struct reg_param reg_params[4];
 	struct armv7m_algorithm armv7m_info;
 	int retval = ERROR_OK;
@@ -871,7 +660,7 @@ static int nrf51_ll_flash_write(struct nrf51_info *chip, uint32_t offset, const
 	assert(bytes % 4 == 0);
 
 	/* allocate working area with flash programming code */
-	if (target_alloc_working_area(target, sizeof(nrf51_flash_write_code),
+	if (target_alloc_working_area(target, sizeof(nrf5_flash_write_code),
 			&write_algorithm) != ERROR_OK) {
 		LOG_WARNING("no working area available, falling back to slow memory writes");
 
@@ -880,7 +669,7 @@ static int nrf51_ll_flash_write(struct nrf51_info *chip, uint32_t offset, const
 			if (retval != ERROR_OK)
 				return retval;
 
-			retval = nrf51_wait_for_nvmc(chip);
+			retval = nrf5_wait_for_nvmc(chip);
 			if (retval != ERROR_OK)
 				return retval;
 
@@ -893,11 +682,11 @@ static int nrf51_ll_flash_write(struct nrf51_info *chip, uint32_t offset, const
 
 	LOG_WARNING("using fast async flash loader. This is currently supported");
 	LOG_WARNING("only with ST-Link and CMSIS-DAP. If you have issues, add");
-	LOG_WARNING("\"set WORKAREASIZE 0\" before sourcing nrf51.cfg to disable it");
+	LOG_WARNING("\"set WORKAREASIZE 0\" before sourcing nrf51.cfg/nrf52.cfg to disable it");
 
 	retval = target_write_buffer(target, write_algorithm->address,
-				sizeof(nrf51_flash_write_code),
-				nrf51_flash_write_code);
+				sizeof(nrf5_flash_write_code),
+				nrf5_flash_write_code);
 	if (retval != ERROR_OK)
 		return retval;
 
@@ -948,10 +737,10 @@ static int nrf51_ll_flash_write(struct nrf51_info *chip, uint32_t offset, const
 /* Check and erase flash sectors in specified range then start a low level page write.
    start/end must be sector aligned.
 */
-static int nrf51_write_pages(struct flash_bank *bank, uint32_t start, uint32_t end, const uint8_t *buffer)
+static int nrf5_write_pages(struct flash_bank *bank, uint32_t start, uint32_t end, const uint8_t *buffer)
 {
 	int res = ERROR_FAIL;
-	struct nrf51_info *chip = bank->driver_priv;
+	struct nrf5_info *chip = bank->driver_priv;
 	struct flash_sector *sector;
 	uint32_t offset;
 
@@ -960,7 +749,7 @@ static int nrf51_write_pages(struct flash_bank *bank, uint32_t start, uint32_t e
 
 	/* Erase all sectors */
 	for (offset = start; offset < end; offset += chip->code_page_size) {
-		sector = nrf51_find_sector_by_address(bank, offset);
+		sector = nrf5_find_sector_by_address(bank, offset);
 		if (!sector) {
 			LOG_ERROR("Invalid sector @ 0x%08"PRIx32, offset);
 			return ERROR_FLASH_SECTOR_INVALID;
@@ -972,7 +761,7 @@ static int nrf51_write_pages(struct flash_bank *bank, uint32_t start, uint32_t e
 		}
 
 		if (sector->is_erased != 1) {	/* 1 = erased, 0= not erased, -1 = unknown */
-			res = nrf51_erase_page(bank, chip, sector);
+			res = nrf5_erase_page(bank, chip, sector);
 			if (res != ERROR_OK) {
 				LOG_ERROR("Failed to erase sector @ 0x%08"PRIx32, sector->offset);
 				goto error;
@@ -981,41 +770,41 @@ static int nrf51_write_pages(struct flash_bank *bank, uint32_t start, uint32_t e
 		sector->is_erased = 0;
 	}
 
-	res = nrf51_nvmc_write_enable(chip);
+	res = nrf5_nvmc_write_enable(chip);
 	if (res != ERROR_OK)
 		goto error;
 
-	res = nrf51_ll_flash_write(chip, start, buffer, (end - start));
+	res = nrf5_ll_flash_write(chip, start, buffer, (end - start));
 	if (res != ERROR_OK)
 		goto set_read_only;
 
-	return nrf51_nvmc_read_only(chip);
+	return nrf5_nvmc_read_only(chip);
 
 set_read_only:
-	nrf51_nvmc_read_only(chip);
+	nrf5_nvmc_read_only(chip);
 error:
-	LOG_ERROR("Failed to write to nrf51 flash");
+	LOG_ERROR("Failed to write to nrf5 flash");
 	return res;
 }
 
-static int nrf51_erase(struct flash_bank *bank, int first, int last)
+static int nrf5_erase(struct flash_bank *bank, int first, int last)
 {
 	int res;
-	struct nrf51_info *chip;
+	struct nrf5_info *chip;
 
-	res = nrf51_get_probed_chip_if_halted(bank, &chip);
+	res = nrf5_get_probed_chip_if_halted(bank, &chip);
 	if (res != ERROR_OK)
 		return res;
 
 	/* For each sector to be erased */
 	for (int s = first; s <= last && res == ERROR_OK; s++)
-		res = nrf51_erase_page(bank, chip, &bank->sectors[s]);
+		res = nrf5_erase_page(bank, chip, &bank->sectors[s]);
 
 	return res;
 }
 
-static int nrf51_code_flash_write(struct flash_bank *bank,
-				  struct nrf51_info *chip,
+static int nrf5_code_flash_write(struct flash_bank *bank,
+				  struct nrf5_info *chip,
 				  const uint8_t *buffer, uint32_t offset, uint32_t count)
 {
 
@@ -1062,58 +851,58 @@ static int nrf51_code_flash_write(struct flash_bank *bank,
 			return res;
 	}
 
-	return nrf51_write_pages(bank, first_page_offset, last_page_offset, buffer_to_flash);
+	return nrf5_write_pages(bank, first_page_offset, last_page_offset, buffer_to_flash);
 }
 
-static int nrf51_uicr_flash_write(struct flash_bank *bank,
-				  struct nrf51_info *chip,
+static int nrf5_uicr_flash_write(struct flash_bank *bank,
+				  struct nrf5_info *chip,
 				  const uint8_t *buffer, uint32_t offset, uint32_t count)
 {
 	int res;
-	uint8_t uicr[NRF51_UICR_SIZE];
+	uint8_t uicr[NRF5_UICR_SIZE];
 	struct flash_sector *sector = &bank->sectors[0];
 
-	if ((offset + count) > NRF51_UICR_SIZE)
+	if ((offset + count) > NRF5_UICR_SIZE)
 		return ERROR_FAIL;
 
 	res = target_read_memory(bank->target,
-				 NRF51_UICR_BASE,
+				 NRF5_UICR_BASE,
 				 1,
-				 NRF51_UICR_SIZE,
+				 NRF5_UICR_SIZE,
 				 uicr);
 
 	if (res != ERROR_OK)
 		return res;
 
 	if (sector->is_erased != 1) {
-		res = nrf51_erase_page(bank, chip, sector);
+		res = nrf5_erase_page(bank, chip, sector);
 		if (res != ERROR_OK)
 			return res;
 	}
 
-	res = nrf51_nvmc_write_enable(chip);
+	res = nrf5_nvmc_write_enable(chip);
 	if (res != ERROR_OK)
 		return res;
 
 	memcpy(&uicr[offset], buffer, count);
 
-	res = nrf51_ll_flash_write(chip, NRF51_UICR_BASE, uicr, NRF51_UICR_SIZE);
+	res = nrf5_ll_flash_write(chip, NRF5_UICR_BASE, uicr, NRF5_UICR_SIZE);
 	if (res != ERROR_OK) {
-		nrf51_nvmc_read_only(chip);
+		nrf5_nvmc_read_only(chip);
 		return res;
 	}
 
-	return nrf51_nvmc_read_only(chip);
+	return nrf5_nvmc_read_only(chip);
 }
 
 
-static int nrf51_write(struct flash_bank *bank, const uint8_t *buffer,
+static int nrf5_write(struct flash_bank *bank, const uint8_t *buffer,
 		       uint32_t offset, uint32_t count)
 {
 	int res;
-	struct nrf51_info *chip;
+	struct nrf5_info *chip;
 
-	res = nrf51_get_probed_chip_if_halted(bank, &chip);
+	res = nrf5_get_probed_chip_if_halted(bank, &chip);
 	if (res != ERROR_OK)
 		return res;
 
@@ -1121,15 +910,15 @@ static int nrf51_write(struct flash_bank *bank, const uint8_t *buffer,
 }
 
 
-FLASH_BANK_COMMAND_HANDLER(nrf51_flash_bank_command)
+FLASH_BANK_COMMAND_HANDLER(nrf5_flash_bank_command)
 {
-	static struct nrf51_info *chip;
+	static struct nrf5_info *chip;
 
 	switch (bank->base) {
-	case NRF51_FLASH_BASE:
+	case NRF5_FLASH_BASE:
 		bank->bank_number = 0;
 		break;
-	case NRF51_UICR_BASE:
+	case NRF5_UICR_BASE:
 		bank->bank_number = 1;
 		break;
 	default:
@@ -1147,11 +936,11 @@ FLASH_BANK_COMMAND_HANDLER(nrf51_flash_bank_command)
 	}
 
 	switch (bank->base) {
-	case NRF51_FLASH_BASE:
-		chip->bank[bank->bank_number].write = nrf51_code_flash_write;
+	case NRF5_FLASH_BASE:
+		chip->bank[bank->bank_number].write = nrf5_code_flash_write;
 		break;
-	case NRF51_UICR_BASE:
-		chip->bank[bank->bank_number].write = nrf51_uicr_flash_write;
+	case NRF5_UICR_BASE:
+		chip->bank[bank->bank_number].write = nrf5_uicr_flash_write;
 		break;
 	}
 
@@ -1161,27 +950,27 @@ FLASH_BANK_COMMAND_HANDLER(nrf51_flash_bank_command)
 	return ERROR_OK;
 }
 
-COMMAND_HANDLER(nrf51_handle_mass_erase_command)
+COMMAND_HANDLER(nrf5_handle_mass_erase_command)
 {
 	int res;
 	struct flash_bank *bank = NULL;
 	struct target *target = get_current_target(CMD_CTX);
 
-	res = get_flash_bank_by_addr(target, NRF51_FLASH_BASE, true, &bank);
+	res = get_flash_bank_by_addr(target, NRF5_FLASH_BASE, true, &bank);
 	if (res != ERROR_OK)
 		return res;
 
 	assert(bank != NULL);
 
-	struct nrf51_info *chip;
+	struct nrf5_info *chip;
 
-	res = nrf51_get_probed_chip_if_halted(bank, &chip);
+	res = nrf5_get_probed_chip_if_halted(bank, &chip);
 	if (res != ERROR_OK)
 		return res;
 
 	uint32_t ppfc;
 
-	res = target_read_u32(target, NRF51_FICR_PPFC,
+	res = target_read_u32(target, NRF5_FICR_PPFC,
 			      &ppfc);
 	if (res != ERROR_OK) {
 		LOG_ERROR("Couldn't read PPFC register");
@@ -1194,23 +983,23 @@ COMMAND_HANDLER(nrf51_handle_mass_erase_command)
 		return ERROR_FAIL;
 	}
 
-	res = nrf51_erase_all(chip);
+	res = nrf5_erase_all(chip);
 	if (res != ERROR_OK) {
 		LOG_ERROR("Failed to erase the chip");
-		nrf51_protect_check(bank);
+		nrf5_protect_check(bank);
 		return res;
 	}
 
 	for (int i = 0; i < bank->num_sectors; i++)
 		bank->sectors[i].is_erased = 1;
 
-	res = nrf51_protect_check(bank);
+	res = nrf5_protect_check(bank);
 	if (res != ERROR_OK) {
 		LOG_ERROR("Failed to check chip's write protection");
 		return res;
 	}
 
-	res = get_flash_bank_by_addr(target, NRF51_UICR_BASE, true, &bank);
+	res = get_flash_bank_by_addr(target, NRF5_UICR_BASE, true, &bank);
 	if (res != ERROR_OK)
 		return res;
 
@@ -1219,13 +1008,13 @@ COMMAND_HANDLER(nrf51_handle_mass_erase_command)
 	return ERROR_OK;
 }
 
-static int nrf51_info(struct flash_bank *bank, char *buf, int buf_size)
+static int nrf5_info(struct flash_bank *bank, char *buf, int buf_size)
 {
 	int res;
 
-	struct nrf51_info *chip;
+	struct nrf5_info *chip;
 
-	res = nrf51_get_probed_chip_if_halted(bank, &chip);
+	res = nrf5_get_probed_chip_if_halted(bank, &chip);
 	if (res != ERROR_OK)
 		return res;
 
@@ -1233,45 +1022,45 @@ static int nrf51_info(struct flash_bank *bank, char *buf, int buf_size)
 		const uint32_t address;
 		uint32_t value;
 	} ficr[] = {
-		{ .address = NRF51_FICR_CODEPAGESIZE	},
-		{ .address = NRF51_FICR_CODESIZE	},
-		{ .address = NRF51_FICR_CLENR0		},
-		{ .address = NRF51_FICR_PPFC		},
-		{ .address = NRF51_FICR_NUMRAMBLOCK	},
-		{ .address = NRF51_FICR_SIZERAMBLOCK0	},
-		{ .address = NRF51_FICR_SIZERAMBLOCK1	},
-		{ .address = NRF51_FICR_SIZERAMBLOCK2	},
-		{ .address = NRF51_FICR_SIZERAMBLOCK3	},
-		{ .address = NRF51_FICR_CONFIGID	},
-		{ .address = NRF51_FICR_DEVICEID0	},
-		{ .address = NRF51_FICR_DEVICEID1	},
-		{ .address = NRF51_FICR_ER0		},
-		{ .address = NRF51_FICR_ER1		},
-		{ .address = NRF51_FICR_ER2		},
-		{ .address = NRF51_FICR_ER3		},
-		{ .address = NRF51_FICR_IR0		},
-		{ .address = NRF51_FICR_IR1		},
-		{ .address = NRF51_FICR_IR2		},
-		{ .address = NRF51_FICR_IR3		},
-		{ .address = NRF51_FICR_DEVICEADDRTYPE	},
-		{ .address = NRF51_FICR_DEVICEADDR0	},
-		{ .address = NRF51_FICR_DEVICEADDR1	},
-		{ .address = NRF51_FICR_OVERRIDEN	},
-		{ .address = NRF51_FICR_NRF_1MBIT0	},
-		{ .address = NRF51_FICR_NRF_1MBIT1	},
-		{ .address = NRF51_FICR_NRF_1MBIT2	},
-		{ .address = NRF51_FICR_NRF_1MBIT3	},
-		{ .address = NRF51_FICR_NRF_1MBIT4	},
-		{ .address = NRF51_FICR_BLE_1MBIT0	},
-		{ .address = NRF51_FICR_BLE_1MBIT1	},
-		{ .address = NRF51_FICR_BLE_1MBIT2	},
-		{ .address = NRF51_FICR_BLE_1MBIT3	},
-		{ .address = NRF51_FICR_BLE_1MBIT4	},
+		{ .address = NRF5_FICR_CODEPAGESIZE	},
+		{ .address = NRF5_FICR_CODESIZE	},
+		{ .address = NRF5_FICR_CLENR0		},
+		{ .address = NRF5_FICR_PPFC		},
+		{ .address = NRF5_FICR_NUMRAMBLOCK	},
+		{ .address = NRF5_FICR_SIZERAMBLOCK0	},
+		{ .address = NRF5_FICR_SIZERAMBLOCK1	},
+		{ .address = NRF5_FICR_SIZERAMBLOCK2	},
+		{ .address = NRF5_FICR_SIZERAMBLOCK3	},
+		{ .address = NRF5_FICR_CONFIGID	},
+		{ .address = NRF5_FICR_DEVICEID0	},
+		{ .address = NRF5_FICR_DEVICEID1	},
+		{ .address = NRF5_FICR_ER0		},
+		{ .address = NRF5_FICR_ER1		},
+		{ .address = NRF5_FICR_ER2		},
+		{ .address = NRF5_FICR_ER3		},
+		{ .address = NRF5_FICR_IR0		},
+		{ .address = NRF5_FICR_IR1		},
+		{ .address = NRF5_FICR_IR2		},
+		{ .address = NRF5_FICR_IR3		},
+		{ .address = NRF5_FICR_DEVICEADDRTYPE	},
+		{ .address = NRF5_FICR_DEVICEADDR0	},
+		{ .address = NRF5_FICR_DEVICEADDR1	},
+		{ .address = NRF5_FICR_OVERRIDEN	},
+		{ .address = NRF5_FICR_NRF_1MBIT0	},
+		{ .address = NRF5_FICR_NRF_1MBIT1	},
+		{ .address = NRF5_FICR_NRF_1MBIT2	},
+		{ .address = NRF5_FICR_NRF_1MBIT3	},
+		{ .address = NRF5_FICR_NRF_1MBIT4	},
+		{ .address = NRF5_FICR_BLE_1MBIT0	},
+		{ .address = NRF5_FICR_BLE_1MBIT1	},
+		{ .address = NRF5_FICR_BLE_1MBIT2	},
+		{ .address = NRF5_FICR_BLE_1MBIT3	},
+		{ .address = NRF5_FICR_BLE_1MBIT4	},
 	}, uicr[] = {
-		{ .address = NRF51_UICR_CLENR0,		},
-		{ .address = NRF51_UICR_RBPCONF		},
-		{ .address = NRF51_UICR_XTALFREQ	},
-		{ .address = NRF51_UICR_FWID		},
+		{ .address = NRF5_UICR_CLENR0,		},
+		{ .address = NRF5_UICR_RBPCONF		},
+		{ .address = NRF5_UICR_XTALFREQ	},
+		{ .address = NRF5_UICR_FWID		},
 	};
 
 	for (size_t i = 0; i < ARRAY_SIZE(ficr); i++) {
@@ -1343,38 +1132,62 @@ static int nrf51_info(struct flash_bank *bank, char *buf, int buf_size)
 	return ERROR_OK;
 }
 
-static const struct command_registration nrf51_exec_command_handlers[] = {
+static const struct command_registration nrf5_exec_command_handlers[] = {
 	{
 		.name		= "mass_erase",
-		.handler	= nrf51_handle_mass_erase_command,
+		.handler	= nrf5_handle_mass_erase_command,
 		.mode		= COMMAND_EXEC,
 		.help		= "Erase all flash contents of the chip.",
 	},
 	COMMAND_REGISTRATION_DONE
 };
 
-static const struct command_registration nrf51_command_handlers[] = {
+static const struct command_registration nrf5_command_handlers[] = {
+	{
+		.name	= "nrf5",
+		.mode	= COMMAND_ANY,
+		.help	= "nrf5 flash command group",
+		.usage	= "",
+		.chain	= nrf5_exec_command_handlers,
+	},
 	{
 		.name	= "nrf51",
 		.mode	= COMMAND_ANY,
 		.help	= "nrf51 flash command group",
 		.usage	= "",
-		.chain	= nrf51_exec_command_handlers,
+		.chain	= nrf5_exec_command_handlers,
 	},
 	COMMAND_REGISTRATION_DONE
 };
 
+struct flash_driver nrf5_flash = {
+	.name			= "nrf5",
+	.commands		= nrf5_command_handlers,
+	.flash_bank_command	= nrf5_flash_bank_command,
+	.info			= nrf5_info,
+	.erase			= nrf5_erase,
+	.protect		= nrf5_protect,
+	.write			= nrf5_write,
+	.read			= default_flash_read,
+	.probe			= nrf5_probe,
+	.auto_probe		= nrf5_auto_probe,
+	.erase_check		= default_flash_blank_check,
+	.protect_check		= nrf5_protect_check,
+};
+
+/* We need to retain the flash-driver name as well as the commands
+ * for backwards compatability */
 struct flash_driver nrf51_flash = {
 	.name			= "nrf51",
-	.commands		= nrf51_command_handlers,
-	.flash_bank_command	= nrf51_flash_bank_command,
-	.info			= nrf51_info,
-	.erase			= nrf51_erase,
-	.protect		= nrf51_protect,
-	.write			= nrf51_write,
+	.commands		= nrf5_command_handlers,
+	.flash_bank_command	= nrf5_flash_bank_command,
+	.info			= nrf5_info,
+	.erase			= nrf5_erase,
+	.protect		= nrf5_protect,
+	.write			= nrf5_write,
 	.read			= default_flash_read,
-	.probe			= nrf51_probe,
-	.auto_probe		= nrf51_auto_probe,
+	.probe			= nrf5_probe,
+	.auto_probe		= nrf5_auto_probe,
 	.erase_check		= default_flash_blank_check,
-	.protect_check		= nrf51_protect_check,
+	.protect_check		= nrf5_protect_check,
 };
diff --git a/src/flash/nor/spi.c b/src/flash/nor/spi.c
index c8239d2..ac5ef6b 100644
--- a/src/flash/nor/spi.c
+++ b/src/flash/nor/spi.c
@@ -31,53 +31,55 @@
   * from device datasheets and Linux SPI flash drivers. */
 const struct flash_device flash_devices[] = {
 	/* name, erase_cmd, chip_erase_cmd, device_id, pagesize, sectorsize, size_in_bytes */
-	FLASH_ID("st m25p05",      0xd8, 0xc7, 0x00102020, 0x80,  0x8000,  0x10000),
-	FLASH_ID("st m25p10",      0xd8, 0xc7, 0x00112020, 0x80,  0x8000,  0x20000),
-	FLASH_ID("st m25p20",      0xd8, 0xc7, 0x00122020, 0x100, 0x10000, 0x40000),
-	FLASH_ID("st m25p40",      0xd8, 0xc7, 0x00132020, 0x100, 0x10000, 0x80000),
-	FLASH_ID("st m25p80",      0xd8, 0xc7, 0x00142020, 0x100, 0x10000, 0x100000),
-	FLASH_ID("st m25p16",      0xd8, 0xc7, 0x00152020, 0x100, 0x10000, 0x200000),
-	FLASH_ID("st m25p32",      0xd8, 0xc7, 0x00162020, 0x100, 0x10000, 0x400000),
-	FLASH_ID("st m25p64",      0xd8, 0xc7, 0x00172020, 0x100, 0x10000, 0x800000),
-	FLASH_ID("st m25p128",     0xd8, 0xc7, 0x00182020, 0x100, 0x40000, 0x1000000),
-	FLASH_ID("st m45pe10",     0xd8, 0xd8, 0x00114020, 0x100, 0x10000, 0x20000),
-	FLASH_ID("st m45pe20",     0xd8, 0xd8, 0x00124020, 0x100, 0x10000, 0x40000),
-	FLASH_ID("st m45pe40",     0xd8, 0xd8, 0x00134020, 0x100, 0x10000, 0x80000),
-	FLASH_ID("st m45pe80",     0xd8, 0xd8, 0x00144020, 0x100, 0x10000, 0x100000),
-	FLASH_ID("sp s25fl004",    0xd8, 0xc7, 0x00120201, 0x100, 0x10000, 0x80000),
-	FLASH_ID("sp s25fl008",    0xd8, 0xc7, 0x00130201, 0x100, 0x10000, 0x100000),
-	FLASH_ID("sp s25fl016",    0xd8, 0xc7, 0x00140201, 0x100, 0x10000, 0x200000),
-	FLASH_ID("sp s25fl116k",   0xd8, 0xc7, 0x00154001, 0x100, 0x10000, 0x200000),
-	FLASH_ID("sp s25fl032",    0xd8, 0xc7, 0x00150201, 0x100, 0x10000, 0x400000),
-	FLASH_ID("sp s25fl132k",   0xd8, 0xc7, 0x00164001, 0x100, 0x10000, 0x400000),
-	FLASH_ID("sp s25fl064",    0xd8, 0xc7, 0x00160201, 0x100, 0x10000, 0x800000),
-	FLASH_ID("sp s25fl164k",   0xd8, 0xc7, 0x00174001, 0x100, 0x10000, 0x800000),
-	FLASH_ID("sp s25fl128",    0xd8, 0xc7, 0x00182001, 0x100, 0x10000, 0x1000000),
-	FLASH_ID("sp s25fl256",    0xd8, 0xc7, 0x00190201, 0x100, 0x10000, 0x2000000),
-	FLASH_ID("atmel 25f512",   0x52, 0xc7, 0x0065001f, 0x80,  0x8000,  0x10000),
-	FLASH_ID("atmel 25f1024",  0x52, 0x62, 0x0060001f, 0x100, 0x8000,  0x20000),
-	FLASH_ID("atmel 25f2048",  0x52, 0x62, 0x0063001f, 0x100, 0x10000, 0x40000),
-	FLASH_ID("atmel 25f4096",  0x52, 0x62, 0x0064001f, 0x100, 0x10000, 0x80000),
-	FLASH_ID("atmel 25fs040",  0xd7, 0xc7, 0x0004661f, 0x100, 0x10000, 0x80000),
-	FLASH_ID("mac 25l512",     0xd8, 0xc7, 0x001020c2, 0x010, 0x10000, 0x10000),
-	FLASH_ID("mac 25l1005",    0xd8, 0xc7, 0x001120c2, 0x010, 0x10000, 0x20000),
-	FLASH_ID("mac 25l2005",    0xd8, 0xc7, 0x001220c2, 0x010, 0x10000, 0x40000),
-	FLASH_ID("mac 25l4005",    0xd8, 0xc7, 0x001320c2, 0x010, 0x10000, 0x80000),
-	FLASH_ID("mac 25l8005",    0xd8, 0xc7, 0x001420c2, 0x010, 0x10000, 0x100000),
-	FLASH_ID("mac 25l1605",    0xd8, 0xc7, 0x001520c2, 0x100, 0x10000, 0x200000),
-	FLASH_ID("mac 25l3205",    0xd8, 0xc7, 0x001620c2, 0x100, 0x10000, 0x400000),
-	FLASH_ID("mac 25l6405",    0xd8, 0xc7, 0x001720c2, 0x100, 0x10000, 0x800000),
-	FLASH_ID("micron n25q064", 0xd8, 0xc7, 0x0017ba20, 0x100, 0x10000, 0x800000),
-	FLASH_ID("micron n25q128", 0xd8, 0xc7, 0x0018ba20, 0x100, 0x10000, 0x1000000),
-	FLASH_ID("issi is25lp128", 0xd8, 0xc7, 0x0018609d, 0x100, 0x10000, 0x1000000),
-	FLASH_ID("win w25q80bv",   0xd8, 0xc7, 0x001440ef, 0x100, 0x10000, 0x100000),
-	FLASH_ID("win w25q32fv",   0xd8, 0xc7, 0x001640ef, 0x100, 0x10000, 0x400000),
-	FLASH_ID("win w25q32dw",   0xd8, 0xc7, 0x001660ef, 0x100, 0x10000, 0x400000),
-	FLASH_ID("win w25q64cv",   0xd8, 0xc7, 0x001740ef, 0x100, 0x10000, 0x800000),
-	FLASH_ID("win w25q128fv",  0xd8, 0xc7, 0x001840ef, 0x100, 0x10000, 0x1000000),
-	FLASH_ID("gd gd25q20",     0x20, 0xc7, 0x00c84012, 0x100, 0x1000,  0x80000),
-	FLASH_ID("gd gd25q16c",    0xd8, 0xc7, 0x001540c8, 0x100, 0x10000, 0x200000),
-	FLASH_ID("gd gd25q32c",    0xd8, 0xc7, 0x001640c8, 0x100, 0x10000, 0x400000),
-	FLASH_ID("gd gd25q128c",   0xd8, 0xc7, 0x001840c8, 0x100, 0x10000, 0x1000000),
-	FLASH_ID(NULL,             0,    0,	   0,          0,     0,       0)
+	FLASH_ID("st m25p05",           0xd8, 0xc7, 0x00102020, 0x80,  0x8000,  0x10000),
+	FLASH_ID("st m25p10",           0xd8, 0xc7, 0x00112020, 0x80,  0x8000,  0x20000),
+	FLASH_ID("st m25p20",           0xd8, 0xc7, 0x00122020, 0x100, 0x10000, 0x40000),
+	FLASH_ID("st m25p40",           0xd8, 0xc7, 0x00132020, 0x100, 0x10000, 0x80000),
+	FLASH_ID("st m25p80",           0xd8, 0xc7, 0x00142020, 0x100, 0x10000, 0x100000),
+	FLASH_ID("st m25p16",           0xd8, 0xc7, 0x00152020, 0x100, 0x10000, 0x200000),
+	FLASH_ID("st m25p32",           0xd8, 0xc7, 0x00162020, 0x100, 0x10000, 0x400000),
+	FLASH_ID("st m25p64",           0xd8, 0xc7, 0x00172020, 0x100, 0x10000, 0x800000),
+	FLASH_ID("st m25p128",          0xd8, 0xc7, 0x00182020, 0x100, 0x40000, 0x1000000),
+	FLASH_ID("st m45pe10",          0xd8, 0xd8, 0x00114020, 0x100, 0x10000, 0x20000),
+	FLASH_ID("st m45pe20",          0xd8, 0xd8, 0x00124020, 0x100, 0x10000, 0x40000),
+	FLASH_ID("st m45pe40",          0xd8, 0xd8, 0x00134020, 0x100, 0x10000, 0x80000),
+	FLASH_ID("st m45pe80",          0xd8, 0xd8, 0x00144020, 0x100, 0x10000, 0x100000),
+	FLASH_ID("sp s25fl004",         0xd8, 0xc7, 0x00120201, 0x100, 0x10000, 0x80000),
+	FLASH_ID("sp s25fl008",         0xd8, 0xc7, 0x00130201, 0x100, 0x10000, 0x100000),
+	FLASH_ID("sp s25fl016",         0xd8, 0xc7, 0x00140201, 0x100, 0x10000, 0x200000),
+	FLASH_ID("sp s25fl116k",        0xd8, 0xc7, 0x00154001, 0x100, 0x10000, 0x200000),
+	FLASH_ID("sp s25fl032",         0xd8, 0xc7, 0x00150201, 0x100, 0x10000, 0x400000),
+	FLASH_ID("sp s25fl132k",        0xd8, 0xc7, 0x00164001, 0x100, 0x10000, 0x400000),
+	FLASH_ID("sp s25fl064",         0xd8, 0xc7, 0x00160201, 0x100, 0x10000, 0x800000),
+	FLASH_ID("sp s25fl164k",        0xd8, 0xc7, 0x00174001, 0x100, 0x10000, 0x800000),
+	FLASH_ID("sp s25fl128",         0xd8, 0xc7, 0x00182001, 0x100, 0x10000, 0x1000000),
+	FLASH_ID("sp s25fl256",         0xd8, 0xc7, 0x00190201, 0x100, 0x10000, 0x2000000),
+	FLASH_ID("atmel 25f512",        0x52, 0xc7, 0x0065001f, 0x80,  0x8000,  0x10000),
+	FLASH_ID("atmel 25f1024",       0x52, 0x62, 0x0060001f, 0x100, 0x8000,  0x20000),
+	FLASH_ID("atmel 25f2048",       0x52, 0x62, 0x0063001f, 0x100, 0x10000, 0x40000),
+	FLASH_ID("atmel 25f4096",       0x52, 0x62, 0x0064001f, 0x100, 0x10000, 0x80000),
+	FLASH_ID("atmel 25fs040",       0xd7, 0xc7, 0x0004661f, 0x100, 0x10000, 0x80000),
+	FLASH_ID("mac 25l512",          0xd8, 0xc7, 0x001020c2, 0x010, 0x10000, 0x10000),
+	FLASH_ID("mac 25l1005",         0xd8, 0xc7, 0x001120c2, 0x010, 0x10000, 0x20000),
+	FLASH_ID("mac 25l2005",         0xd8, 0xc7, 0x001220c2, 0x010, 0x10000, 0x40000),
+	FLASH_ID("mac 25l4005",         0xd8, 0xc7, 0x001320c2, 0x010, 0x10000, 0x80000),
+	FLASH_ID("mac 25l8005",         0xd8, 0xc7, 0x001420c2, 0x010, 0x10000, 0x100000),
+	FLASH_ID("mac 25l1605",         0xd8, 0xc7, 0x001520c2, 0x100, 0x10000, 0x200000),
+	FLASH_ID("mac 25l3205",         0xd8, 0xc7, 0x001620c2, 0x100, 0x10000, 0x400000),
+	FLASH_ID("mac 25l6405",         0xd8, 0xc7, 0x001720c2, 0x100, 0x10000, 0x800000),
+	FLASH_ID("micron n25q064",      0xd8, 0xc7, 0x0017ba20, 0x100, 0x10000, 0x800000),
+	FLASH_ID("micron n25q128",      0xd8, 0xc7, 0x0018ba20, 0x100, 0x10000, 0x1000000),
+	FLASH_ID("micron n25q256 3v",   0xd8, 0xc7, 0x0019ba20, 0x100, 0x10000, 0x2000000),
+	FLASH_ID("micron n25q256 1.8v", 0xd8, 0xc7, 0x0019bb20, 0x100, 0x10000, 0x2000000),
+	FLASH_ID("issi is25lp128",      0xd8, 0xc7, 0x0018609d, 0x100, 0x10000, 0x1000000),
+	FLASH_ID("win w25q80bv",        0xd8, 0xc7, 0x001440ef, 0x100, 0x10000, 0x100000),
+	FLASH_ID("win w25q32fv",        0xd8, 0xc7, 0x001640ef, 0x100, 0x10000, 0x400000),
+	FLASH_ID("win w25q32dw",        0xd8, 0xc7, 0x001660ef, 0x100, 0x10000, 0x400000),
+	FLASH_ID("win w25q64cv",        0xd8, 0xc7, 0x001740ef, 0x100, 0x10000, 0x800000),
+	FLASH_ID("win w25q128fv",       0xd8, 0xc7, 0x001840ef, 0x100, 0x10000, 0x1000000),
+	FLASH_ID("gd gd25q20",          0x20, 0xc7, 0x00c84012, 0x100, 0x1000,  0x80000),
+	FLASH_ID("gd gd25q16c",         0xd8, 0xc7, 0x001540c8, 0x100, 0x10000, 0x200000),
+	FLASH_ID("gd gd25q32c",         0xd8, 0xc7, 0x001640c8, 0x100, 0x10000, 0x400000),
+	FLASH_ID("gd gd25q128c",        0xd8, 0xc7, 0x001840c8, 0x100, 0x10000, 0x1000000),
+	FLASH_ID(NULL,                  0,    0,    0,          0,     0,       0)
 };
diff --git a/src/flash/nor/stm32f2x.c b/src/flash/nor/stm32f2x.c
index 65cb212..b0992b4 100644
--- a/src/flash/nor/stm32f2x.c
+++ b/src/flash/nor/stm32f2x.c
@@ -143,9 +143,8 @@
 #define FLASH_PSIZE_16 (1 << 8)
 #define FLASH_PSIZE_32 (2 << 8)
 #define FLASH_PSIZE_64 (3 << 8)
-/* The sector number encoding is not straight binary for dual bank flash.
- * Warning: evaluates the argument multiple times */
-#define FLASH_SNB(a)   ((((a) >= 12) ? 0x10 | ((a) - 12) : (a)) << 3)
+/* The sector number encoding is not straight binary for dual bank flash. */
+#define FLASH_SNB(a)   ((a) << 3)
 #define FLASH_LOCK     (1 << 31)
 
 /* FLASH_SR register bits */
@@ -489,6 +488,7 @@ static int stm32x_protect_check(struct flash_bank *bank)
 
 static int stm32x_erase(struct flash_bank *bank, int first, int last)
 {
+	struct stm32x_flash_bank *stm32x_info = bank->driver_priv;
 	struct target *target = bank->target;
 	int i;
 
@@ -516,8 +516,14 @@ static int stm32x_erase(struct flash_bank *bank, int first, int last)
 	 */
 
 	for (i = first; i <= last; i++) {
+		int snb;
+		if (stm32x_info->has_large_mem && i >= 12)
+			snb = (i - 12) | 0x10;
+		else
+			snb = i;
+
 		retval = target_write_u32(target,
-				stm32x_get_flash_reg(bank, STM32_FLASH_CR), FLASH_SER | FLASH_SNB(i) | FLASH_STRT);
+				stm32x_get_flash_reg(bank, STM32_FLASH_CR), FLASH_SER | FLASH_SNB(snb) | FLASH_STRT);
 		if (retval != ERROR_OK)
 			return retval;
 
@@ -1047,7 +1053,8 @@ static int stm32x_probe(struct flash_bank *bank)
 		if (device_id == 0x451) {
 			for (i = 0; i < num_prot_blocks; i++) {
 				bank->prot_blocks[i].offset = bank->sectors[i << 1].offset;
-				bank->prot_blocks[i].size = bank->sectors[i << 1].size << 1;
+				bank->prot_blocks[i].size = bank->sectors[i << 1].size
+						+ bank->sectors[(i << 1) + 1].size;
 			}
 		}
 	} else {
diff --git a/src/flash/nor/stm32h7x.c b/src/flash/nor/stm32h7x.c
new file mode 100644
index 0000000..01e6f06
--- /dev/null
+++ b/src/flash/nor/stm32h7x.c
@@ -0,0 +1,1183 @@
+/***************************************************************************
+ *   Copyright (C) 2017 by STMicroelectronics                              *
+ *                                                                         *
+ *   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 <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 433 */
+/* Same Flash registers for both banks, */
+/* access depends on Flash Base address */
+#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)
+
+/* FLASH_SR register bits */
+#define FLASH_BSY      (1 << 0)  /* Operation in progress */
+#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 IWDG1_HW       (1 << 4)
+
+/* 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_SIZE_ADDRESS      0x1FF1E880
+#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;
+};
+
+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			= FLASH_REG_BASE_B0,
+	.fsize_base			= FLASH_SIZE_ADDRESS,
+	},
+};
+
+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 uint32_t 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 & IWDG1_HW)
+		stm32x_info->option_bytes.independent_watchdog_selection = 1;
+	else
+		stm32x_info->option_bytes.independent_watchdog_selection = 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 |= IWDG1_HW;
+	else
+		optiondata &= ~IWDG1_HW;
+
+	/* 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;
+	/*
+	 * If the size of the data part of the buffer is not a multiple of FLASH_BLOCK_SIZE, we get
+	 * "corrupted fifo read" pointer in target_run_flash_async_algorithm()
+	 */
+	uint32_t data_size = 512 * FLASH_BLOCK_SIZE;	/* 16384 */
+	uint32_t buffer_size = 8 + data_size;
+	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[] = {
+								/* <code>: */
+		0x45, 0x68,				/*		ldr		r5, [r0, #4] */
+								/* <wait_fifo>: */
+		0x06, 0x68,				/*		ldr		r6, [r0, #0] */
+		0x26, 0xb3,				/*		cbz		r6, <exit> */
+		0x76, 0x1b,				/*		subs	r6, r6, r5 */
+		0x42, 0xbf,				/*		ittt	mi */
+		0x76, 0x18,				/*		addmi	r6, r6, r1 */
+		0x36, 0x1a,				/*		submi	r6, r6, r0 */
+		0x08, 0x3e,				/*		submi	r6, #8 */
+		0x20, 0x2e,				/*		cmp		r6, #32 */
+		0xf6, 0xd3,				/*		bcc.n	<wait_fifo> */
+		0x4f, 0xf0, 0x32, 0x06,	/*		mov.w	r6, #STM32_PROG */
+		0xe6, 0x60,				/*		str		r6, [r4, #STM32_FLASH_CR_OFFSET] */
+		0x4f, 0xf0, 0x08, 0x07,	/*		mov.w	r7, #8 */
+								/* <write_flash>: */
+		0x55, 0xf8, 0x04, 0x6b,	/*		ldr.w	r6, [r5], #4 */
+		0x42, 0xf8, 0x04, 0x6b,	/*		str.w	r6, [r2], #4 */
+		0xbf, 0xf3, 0x4f, 0x8f,	/*		dsb		sy */
+		0x8d, 0x42,				/*		cmp		r5, r1 */
+		0x28, 0xbf,				/*		it		cs */
+		0x00, 0xf1, 0x08, 0x05,	/*		addcs.w	r5, r0, #8 */
+		0x01, 0x3f,				/*		subs	r7, #1 */
+		0xf3, 0xd1,				/*		bne.n	<write_flash> */
+								/* <busy>: */
+		0x26, 0x69,				/*		ldr		r6, [r4, #STM32_FLASH_SR_OFFSET] */
+		0x16, 0xf0, 0x01, 0x0f,	/*		tst.w	r6, #STM32_SR_BUSY_MASK */
+		0xfb, 0xd1,				/*		bne.n	<busy> */
+		0x05, 0x4f,				/*		ldr		r7, [pc, #20] ; (<stm32_sr_error_mask>) */
+		0x3e, 0x42,				/*		tst		r6, r7 */
+		0x03, 0xd1,				/*		bne.n	<error> */
+		0x45, 0x60,				/*		str		r5, [r0, #4] */
+		0x01, 0x3b,				/*		subs	r3, #1 */
+		0xdb, 0xd1,				/*		bne.n	<wait_fifo> */
+		0x01, 0xe0,				/*		b.n		<exit> */
+								/* <error>: */
+		0x00, 0x27,				/*		movs	r7, #0 */
+		0x47, 0x60,				/*		str		r7, [r0, #4] */
+								/* <exit>: */
+		0x30, 0x46,				/*		mov		r0, r6 */
+		0x00, 0xbe,				/*		bkpt	0x0000 */
+								/* <stm32_sr_error_mask>: */
+		0x00, 0x00, 0xee, 0x03	/*		.word	0x03ee0000 ; (STM32_SR_ERROR_MASK) */
+	};
+
+	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) {
+		data_size /= 2;
+		buffer_size = 8 + data_size;
+		if (data_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, retval2;
+
+	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;
+
+	uint32_t blocks_remaining = count / FLASH_BLOCK_SIZE;
+	uint32_t bytes_remaining = count % FLASH_BLOCK_SIZE;
+
+	/* multiple words (32-bytes) to be programmed in block */
+	if (blocks_remaining) {
+		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))
+			goto flash_lock;
+	}
+
+	/*
+	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)
+	4. 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)
+			goto flash_lock;
+
+		retval = target_write_buffer(target, address, FLASH_BLOCK_SIZE, buffer);
+		if (retval != ERROR_OK)
+			goto flash_lock;
+
+		retval = stm32x_wait_status_busy(bank, FLASH_WRITE_TIMEOUT);
+		if (retval != ERROR_OK)
+			goto flash_lock;
+
+		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)
+			goto flash_lock;
+
+		retval = target_write_buffer(target, address, bytes_remaining, buffer);
+		if (retval != ERROR_OK)
+			goto flash_lock;
+
+		/* 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)
+			goto flash_lock;
+
+		retval = stm32x_wait_status_busy(bank, FLASH_WRITE_TIMEOUT);
+		if (retval != ERROR_OK)
+			goto flash_lock;
+	}
+
+flash_lock:
+	retval2 = stm32x_lock_reg(bank);
+	if (retval2 != ERROR_OK)
+		LOG_ERROR("error during the lock of flash");
+
+	if (retval == ERROR_OK)
+		retval = retval2;
+
+	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,
+};
diff --git a/src/flash/nor/stm32lx.c b/src/flash/nor/stm32lx.c
index 0c2fddc..fdfaad4 100644
--- a/src/flash/nor/stm32lx.c
+++ b/src/flash/nor/stm32lx.c
@@ -726,16 +726,13 @@ reset_pg_and_lock:
 
 static int stm32lx_read_id_code(struct target *target, uint32_t *id)
 {
-	/* read stm32 device id register */
-	int retval = target_read_u32(target, DBGMCU_IDCODE, id);
-	if (retval != ERROR_OK)
-		return retval;
-
-	/* STM32L0 parts will have 0 there, try reading the L0's location for
-	 * DBG_IDCODE in case this is an L0 part. */
-	if (*id == 0)
+	struct armv7m_common *armv7m = target_to_armv7m(target);
+	int retval;
+	if (armv7m->arm.is_armv6m == true)
 		retval = target_read_u32(target, DBGMCU_IDCODE_L0, id);
-
+	else
+	/* read stm32 device id register */
+		retval = target_read_u32(target, DBGMCU_IDCODE, id);
 	return retval;
 }
 
diff --git a/src/flash/nor/xmc4xxx.c b/src/flash/nor/xmc4xxx.c
index 02df46a..5677ef0 100644
--- a/src/flash/nor/xmc4xxx.c
+++ b/src/flash/nor/xmc4xxx.c
@@ -931,13 +931,13 @@ static int xmc4xxx_get_info_command(struct flash_bank *bank, char *buf, int buf_
 
 	/* If OTP Write protection is enabled (User 2), list each
 	 * sector that has it enabled */
-	char otp_str[8];
+	char otp_str[14];
 	if (otp_enabled) {
 		strcat(prot_str, "\nOTP Protection is enabled for sectors:\n");
 		for (int i = 0; i < bank->num_sectors; i++) {
 			if (fb->write_prot_otp[i]) {
 				snprintf(otp_str, sizeof(otp_str), "- %d\n", i);
-				strncat(prot_str, otp_str, ARRAY_SIZE(otp_str));
+				strncat(prot_str, otp_str, sizeof(prot_str) - strlen(prot_str) - 1);
 			}
 		}
 	}
diff --git a/src/helper/command.c b/src/helper/command.c
index 5deaee8..40e8b05 100644
--- a/src/helper/command.c
+++ b/src/helper/command.c
@@ -1456,8 +1456,8 @@ COMMAND_HELPER(handle_command_parse_bool, bool *out, const char *label)
 				LOG_ERROR("%s: argument '%s' is not valid", CMD_NAME, in);
 				return ERROR_COMMAND_SYNTAX_ERROR;
 			}
-			/* fall through */
 		}
+			/* fallthrough */
 		case 0:
 			LOG_INFO("%s is %s", label, *out ? "enabled" : "disabled");
 			break;
diff --git a/src/helper/options.c b/src/helper/options.c
index 1cfa553..12755e0 100644
--- a/src/helper/options.c
+++ b/src/helper/options.c
@@ -37,6 +37,9 @@
 #ifdef HAVE_SYS_SYSCTL_H
 #include <sys/sysctl.h>
 #endif
+#if IS_WIN32 && !IS_CYGWIN
+#include <windows.h>
+#endif
 
 static int help_flag, version_flag;
 
diff --git a/src/jtag/drivers/cmsis_dap_usb.c b/src/jtag/drivers/cmsis_dap_usb.c
index 19c3b19..345c1fd 100644
--- a/src/jtag/drivers/cmsis_dap_usb.c
+++ b/src/jtag/drivers/cmsis_dap_usb.c
@@ -958,11 +958,14 @@ static int cmsis_dap_init(void)
 	retval = cmsis_dap_cmd_DAP_TFER_Configure(0, 64, 0);
 	if (retval != ERROR_OK)
 		return ERROR_FAIL;
-	/* Data Phase (bit 2) must be set to 1 if sticky overrun
-	 * detection is enabled */
-	retval = cmsis_dap_cmd_DAP_SWD_Configure(0);	/* 1 TRN, no Data Phase */
-	if (retval != ERROR_OK)
-		return ERROR_FAIL;
+
+	if (swd_mode) {
+		/* Data Phase (bit 2) must be set to 1 if sticky overrun
+		 * detection is enabled */
+		retval = cmsis_dap_cmd_DAP_SWD_Configure(0);	/* 1 TRN, no Data Phase */
+		if (retval != ERROR_OK)
+			return ERROR_FAIL;
+	}
 
 	retval = cmsis_dap_cmd_DAP_LED(0x03);		/* Both LEDs on */
 	if (retval != ERROR_OK)
diff --git a/src/jtag/drivers/ftdi.c b/src/jtag/drivers/ftdi.c
index 342e321..32876ba 100644
--- a/src/jtag/drivers/ftdi.c
+++ b/src/jtag/drivers/ftdi.c
@@ -855,6 +855,7 @@ COMMAND_HANDLER(ftdi_handle_set_signal_command)
 			ftdi_set_signal(sig, *CMD_ARGV[1]);
 			break;
 		}
+		/* fallthrough */
 	default:
 		LOG_ERROR("unknown signal level '%s', use 0, 1 or z", CMD_ARGV[1]);
 		return ERROR_COMMAND_SYNTAX_ERROR;
@@ -1217,14 +1218,17 @@ static int ftdi_swd_switch_seq(enum swd_special_seq seq)
 	switch (seq) {
 	case LINE_RESET:
 		LOG_DEBUG("SWD line reset");
+		ftdi_swd_swdio_en(true);
 		mpsse_clock_data_out(mpsse_ctx, swd_seq_line_reset, 0, swd_seq_line_reset_len, SWD_MODE);
 		break;
 	case JTAG_TO_SWD:
 		LOG_DEBUG("JTAG-to-SWD");
+		ftdi_swd_swdio_en(true);
 		mpsse_clock_data_out(mpsse_ctx, swd_seq_jtag_to_swd, 0, swd_seq_jtag_to_swd_len, SWD_MODE);
 		break;
 	case SWD_TO_JTAG:
 		LOG_DEBUG("SWD-to-JTAG");
+		ftdi_swd_swdio_en(true);
 		mpsse_clock_data_out(mpsse_ctx, swd_seq_swd_to_jtag, 0, swd_seq_swd_to_jtag_len, SWD_MODE);
 		break;
 	default:
diff --git a/src/jtag/drivers/jlink.c b/src/jtag/drivers/jlink.c
index bd3c5e0..132ef06 100644
--- a/src/jtag/drivers/jlink.c
+++ b/src/jtag/drivers/jlink.c
@@ -322,7 +322,7 @@ static int jlink_speed(int speed)
 
 		if (ret != JAYLINK_OK) {
 			LOG_ERROR("jaylink_get_speeds() failed: %s.",
-				jaylink_strerror_name(ret));
+				jaylink_strerror(ret));
 			return ERROR_JTAG_DEVICE_ERROR;
 		}
 
@@ -349,7 +349,7 @@ static int jlink_speed(int speed)
 
 	if (ret != JAYLINK_OK) {
 		LOG_ERROR("jaylink_set_speed() failed: %s.",
-			jaylink_strerror_name(ret));
+			jaylink_strerror(ret));
 		return ERROR_JTAG_DEVICE_ERROR;
 	}
 
@@ -378,7 +378,7 @@ static bool read_device_config(struct device_config *cfg)
 
 	if (ret != JAYLINK_OK) {
 		LOG_ERROR("jaylink_read_raw_config() failed: %s.",
-			jaylink_strerror_name(ret));
+			jaylink_strerror(ret));
 		return false;
 	}
 
@@ -409,7 +409,7 @@ static int select_interface(void)
 
 	if (ret != JAYLINK_OK) {
 		LOG_ERROR("jaylink_get_available_interfaces() failed: %s.",
-			jaylink_strerror_name(ret));
+			jaylink_strerror(ret));
 		return ERROR_JTAG_INIT_FAILED;
 	}
 
@@ -422,7 +422,7 @@ static int select_interface(void)
 
 	if (ret < 0) {
 		LOG_ERROR("jaylink_select_interface() failed: %s.",
-			jaylink_strerror_name(ret));
+			jaylink_strerror(ret));
 		return ERROR_JTAG_INIT_FAILED;
 	}
 
@@ -442,8 +442,7 @@ static int jlink_register(void)
 	ret = jaylink_register(devh, &conn, connlist, &count);
 
 	if (ret != JAYLINK_OK) {
-		LOG_ERROR("jaylink_register() failed: %s.",
-			jaylink_strerror_name(ret));
+		LOG_ERROR("jaylink_register() failed: %s.", jaylink_strerror(ret));
 		return ERROR_FAIL;
 	}
 
@@ -482,7 +481,7 @@ static bool adjust_swd_buffer_size(void)
 
 	if (ret != JAYLINK_OK) {
 		LOG_ERROR("jaylink_get_free_memory() failed: %s.",
-			jaylink_strerror_name(ret));
+			jaylink_strerror(ret));
 		return false;
 	}
 
@@ -523,6 +522,9 @@ static int jaylink_log_handler(const struct jaylink_context *ctx,
 	case JAYLINK_LOG_LEVEL_DEBUG:
 		tmp = LOG_LVL_DEBUG;
 		break;
+	case JAYLINK_LOG_LEVEL_DEBUG_IO:
+		tmp = LOG_LVL_DEBUG_IO;
+		break;
 	default:
 		tmp = LOG_LVL_WARNING;
 	}
@@ -544,15 +546,21 @@ static int jlink_init(void)
 	struct jaylink_hardware_status hwstatus;
 	enum jaylink_usb_address address;
 	size_t length;
+	size_t num_devices;
+	uint32_t host_interfaces;
 
 	LOG_DEBUG("Using libjaylink %s (compiled with %s).",
 		jaylink_version_package_get_string(), JAYLINK_VERSION_PACKAGE_STRING);
 
+	if (!jaylink_library_has_cap(JAYLINK_CAP_HIF_USB) && use_usb_address) {
+		LOG_ERROR("J-Link driver does not support USB devices.");
+		return ERROR_JTAG_INIT_FAILED;
+	}
+
 	ret = jaylink_init(&jayctx);
 
 	if (ret != JAYLINK_OK) {
-		LOG_ERROR("jaylink_init() failed: %s.",
-			jaylink_strerror_name(ret));
+		LOG_ERROR("jaylink_init() failed: %s.", jaylink_strerror(ret));
 		return ERROR_JTAG_INIT_FAILED;
 	}
 
@@ -560,33 +568,41 @@ static int jlink_init(void)
 
 	if (ret != JAYLINK_OK) {
 		LOG_ERROR("jaylink_log_set_callback() failed: %s.",
-			jaylink_strerror_name(ret));
+			jaylink_strerror(ret));
 		jaylink_exit(jayctx);
 		return ERROR_JTAG_INIT_FAILED;
 	}
 
-	ret = jaylink_discovery_scan(jayctx, 0);
+	host_interfaces = JAYLINK_HIF_USB;
+
+	if (use_serial_number)
+		host_interfaces |= JAYLINK_HIF_TCP;
+
+	ret = jaylink_discovery_scan(jayctx, host_interfaces);
 
 	if (ret != JAYLINK_OK) {
 		LOG_ERROR("jaylink_discovery_scan() failed: %s.",
-			jaylink_strerror_name(ret));
+			jaylink_strerror(ret));
 		jaylink_exit(jayctx);
 		return ERROR_JTAG_INIT_FAILED;
 	}
 
-	ret = jaylink_get_devices(jayctx, &devs, NULL);
+	ret = jaylink_get_devices(jayctx, &devs, &num_devices);
 
 	if (ret != JAYLINK_OK) {
-		LOG_ERROR("jaylink_get_devices() failed: %s.",
-			jaylink_strerror_name(ret));
+		LOG_ERROR("jaylink_get_devices() failed: %s.", jaylink_strerror(ret));
 		jaylink_exit(jayctx);
 		return ERROR_JTAG_INIT_FAILED;
 	}
 
-	found_device = false;
+	if (!use_serial_number && !use_usb_address && num_devices > 1) {
+		LOG_ERROR("Multiple devices found, specify the desired device.");
+		jaylink_free_devices(devs, true);
+		jaylink_exit(jayctx);
+		return ERROR_JTAG_INIT_FAILED;
+	}
 
-	if (!use_serial_number && !use_usb_address)
-		LOG_INFO("No device selected, using first device.");
+	found_device = false;
 
 	for (i = 0; devs[i]; i++) {
 		if (use_serial_number) {
@@ -596,7 +612,7 @@ static int jlink_init(void)
 				continue;
 			} else if (ret != JAYLINK_OK) {
 				LOG_WARNING("jaylink_device_get_serial_number() failed: %s.",
-					jaylink_strerror_name(ret));
+					jaylink_strerror(ret));
 				continue;
 			}
 
@@ -607,9 +623,11 @@ static int jlink_init(void)
 		if (use_usb_address) {
 			ret = jaylink_device_get_usb_address(devs[i], &address);
 
-			if (ret != JAYLINK_OK) {
+			if (ret == JAYLINK_ERR_NOT_SUPPORTED) {
+				continue;
+			} else if (ret != JAYLINK_OK) {
 				LOG_WARNING("jaylink_device_get_usb_address() failed: %s.",
-					jaylink_strerror_name(ret));
+					jaylink_strerror(ret));
 				continue;
 			}
 
@@ -624,7 +642,7 @@ static int jlink_init(void)
 			break;
 		}
 
-		LOG_ERROR("Failed to open device: %s.", jaylink_strerror_name(ret));
+		LOG_ERROR("Failed to open device: %s.", jaylink_strerror(ret));
 	}
 
 	jaylink_free_devices(devs, true);
@@ -644,7 +662,7 @@ static int jlink_init(void)
 
 	if (ret != JAYLINK_OK) {
 		LOG_ERROR("jaylink_get_firmware_version() failed: %s.",
-			jaylink_strerror_name(ret));
+			jaylink_strerror(ret));
 		jaylink_close(devh);
 		jaylink_exit(jayctx);
 		return ERROR_JTAG_INIT_FAILED;
@@ -659,7 +677,7 @@ static int jlink_init(void)
 	ret = jaylink_get_caps(devh, caps);
 
 	if (ret != JAYLINK_OK) {
-		LOG_ERROR("jaylink_get_caps() failed: %s.", jaylink_strerror_name(ret));
+		LOG_ERROR("jaylink_get_caps() failed: %s.", jaylink_strerror(ret));
 		jaylink_close(devh);
 		jaylink_exit(jayctx);
 		return ERROR_JTAG_INIT_FAILED;
@@ -670,7 +688,7 @@ static int jlink_init(void)
 
 		if (ret != JAYLINK_OK) {
 			LOG_ERROR("jaylink_get_extended_caps() failed:  %s.",
-				jaylink_strerror_name(ret));
+				jaylink_strerror(ret));
 			jaylink_close(devh);
 			jaylink_exit(jayctx);
 			return ERROR_JTAG_INIT_FAILED;
@@ -684,7 +702,7 @@ static int jlink_init(void)
 
 		if (ret != JAYLINK_OK) {
 			LOG_ERROR("Failed to retrieve hardware version: %s.",
-				jaylink_strerror_name(ret));
+				jaylink_strerror(ret));
 			jaylink_close(devh);
 			jaylink_exit(jayctx);
 			return ERROR_JTAG_INIT_FAILED;
@@ -725,7 +743,7 @@ static int jlink_init(void)
 
 	if (ret != JAYLINK_OK) {
 		LOG_ERROR("jaylink_get_hardware_status() failed: %s.",
-			jaylink_strerror_name(ret));
+			jaylink_strerror(ret));
 		jaylink_close(devh);
 		jaylink_exit(jayctx);
 		return ERROR_JTAG_INIT_FAILED;
@@ -786,8 +804,7 @@ static int jlink_quit(void)
 		ret = jaylink_swo_stop(devh);
 
 		if (ret != JAYLINK_OK)
-			LOG_ERROR("jaylink_swo_stop() failed: %s.",
-				jaylink_strerror_name(ret));
+			LOG_ERROR("jaylink_swo_stop() failed: %s.", jaylink_strerror(ret));
 	}
 
 	if (jaylink_has_cap(caps, JAYLINK_DEV_CAP_REGISTER)) {
@@ -795,7 +812,7 @@ static int jlink_quit(void)
 
 		if (ret != JAYLINK_OK)
 			LOG_ERROR("jaylink_unregister() failed: %s.",
-				jaylink_strerror_name(ret));
+				jaylink_strerror(ret));
 	}
 
 	jaylink_close(devh);
@@ -944,7 +961,7 @@ COMMAND_HANDLER(jlink_serial_command)
 		return ERROR_FAIL;
 	} else if (ret != JAYLINK_OK) {
 		command_print(CMD_CTX, "jaylink_parse_serial_number() failed: %s.",
-			jaylink_strerror_name(ret));
+			jaylink_strerror(ret));
 		return ERROR_FAIL;
 	}
 
@@ -963,7 +980,7 @@ COMMAND_HANDLER(jlink_handle_hwstatus_command)
 
 	if (ret != JAYLINK_OK) {
 		command_print(CMD_CTX, "jaylink_get_hardware_status() failed: %s.",
-			jaylink_strerror_name(ret));
+			jaylink_strerror(ret));
 		return ERROR_FAIL;
 	}
 
@@ -995,7 +1012,7 @@ COMMAND_HANDLER(jlink_handle_free_memory_command)
 
 	if (ret != JAYLINK_OK) {
 		command_print(CMD_CTX, "jaylink_get_free_memory() failed: %s.",
-			jaylink_strerror_name(ret));
+			jaylink_strerror(ret));
 		return ERROR_FAIL;
 	}
 
@@ -1077,7 +1094,7 @@ COMMAND_HANDLER(jlink_handle_target_power_command)
 
 	if (ret != JAYLINK_OK) {
 		command_print(CMD_CTX, "jaylink_set_target_power() failed: %s.",
-			jaylink_strerror_name(ret));
+			jaylink_strerror(ret));
 		return ERROR_FAIL;
 	}
 
@@ -1183,7 +1200,7 @@ static int poll_trace(uint8_t *buf, size_t *size)
 	ret = jaylink_swo_read(devh, buf, &length);
 
 	if (ret != JAYLINK_OK) {
-		LOG_ERROR("jaylink_swo_read() failed: %s.", jaylink_strerror_name(ret));
+		LOG_ERROR("jaylink_swo_read() failed: %s.", jaylink_strerror(ret));
 		return ERROR_FAIL;
 	}
 
@@ -1204,7 +1221,7 @@ static uint32_t calculate_trace_buffer_size(void)
 
 	if (ret != JAYLINK_OK) {
 		LOG_ERROR("jaylink_get_free_memory() failed: %s.",
-			jaylink_strerror_name(ret));
+			jaylink_strerror(ret));
 		return ERROR_FAIL;
 	}
 
@@ -1268,7 +1285,7 @@ static int config_trace(bool enabled, enum tpio_pin_protocol pin_protocol,
 	ret = jaylink_swo_stop(devh);
 
 	if (ret != JAYLINK_OK) {
-		LOG_ERROR("jaylink_swo_stop() failed: %s.", jaylink_strerror_name(ret));
+		LOG_ERROR("jaylink_swo_stop() failed: %s.", jaylink_strerror(ret));
 		return ERROR_FAIL;
 	}
 
@@ -1294,7 +1311,7 @@ static int config_trace(bool enabled, enum tpio_pin_protocol pin_protocol,
 
 	if (ret != JAYLINK_OK) {
 		LOG_ERROR("jaylink_swo_get_speeds() failed: %s.",
-			jaylink_strerror_name(ret));
+			jaylink_strerror(ret));
 		return ERROR_FAIL;
 	}
 
@@ -1310,8 +1327,7 @@ static int config_trace(bool enabled, enum tpio_pin_protocol pin_protocol,
 		buffer_size);
 
 	if (ret != JAYLINK_OK) {
-		LOG_ERROR("jaylink_start_swo() failed: %s.",
-			jaylink_strerror_name(ret));
+		LOG_ERROR("jaylink_start_swo() failed: %s.", jaylink_strerror(ret));
 		return ERROR_FAIL;
 	}
 
@@ -1570,7 +1586,7 @@ COMMAND_HANDLER(jlink_handle_config_write_command)
 
 	if (ret != JAYLINK_OK) {
 		LOG_ERROR("jaylink_write_raw_config() failed: %s.",
-			jaylink_strerror_name(ret));
+			jaylink_strerror(ret));
 		return ERROR_FAIL;
 	}
 
@@ -1655,8 +1671,7 @@ COMMAND_HANDLER(jlink_handle_emucom_write_command)
 		LOG_ERROR("Channel not supported by the device.");
 		return ERROR_FAIL;
 	} else if (ret != JAYLINK_OK) {
-		LOG_ERROR("Failed to write to channel: %s.",
-			jaylink_strerror_name(ret));
+		LOG_ERROR("Failed to write to channel: %s.", jaylink_strerror(ret));
 		return ERROR_FAIL;
 	}
 
@@ -1704,8 +1719,7 @@ COMMAND_HANDLER(jlink_handle_emucom_read_command)
 		free(buf);
 		return ERROR_FAIL;
 	} else if (ret != JAYLINK_OK) {
-		LOG_ERROR("Failed to read from channel: %s.",
-			jaylink_strerror_name(ret));
+		LOG_ERROR("Failed to read from channel: %s.", jaylink_strerror(ret));
 		free(buf);
 		return ERROR_FAIL;
 	}
@@ -1972,7 +1986,7 @@ static int jlink_flush(void)
 		tap_length, jtag_command_version);
 
 	if (ret != JAYLINK_OK) {
-		LOG_ERROR("jaylink_jtag_io() failed: %s.", jaylink_strerror_name(ret));
+		LOG_ERROR("jaylink_jtag_io() failed: %s.", jaylink_strerror(ret));
 		jlink_tap_init();
 		return ERROR_JTAG_QUEUE_FAILED;
 	}
@@ -2078,7 +2092,7 @@ static int jlink_swd_run_queue(void)
 	ret = jaylink_swd_io(devh, tms_buffer, tdi_buffer, tdo_buffer, tap_length);
 
 	if (ret != JAYLINK_OK) {
-		LOG_ERROR("jaylink_swd_io() failed: %s.", jaylink_strerror_name(ret));
+		LOG_ERROR("jaylink_swd_io() failed: %s.", jaylink_strerror(ret));
 		goto skip;
 	}
 
diff --git a/src/jtag/drivers/kitprog.c b/src/jtag/drivers/kitprog.c
index c689848..584da8c 100644
--- a/src/jtag/drivers/kitprog.c
+++ b/src/jtag/drivers/kitprog.c
@@ -657,6 +657,7 @@ static int kitprog_swd_switch_seq(enum swd_special_seq seq)
 				LOG_DEBUG("JTAG to SWD not supported");
 				/* Fall through to fix target reset issue */
 			}
+			/* fallthrough */
 		case LINE_RESET:
 			LOG_DEBUG("SWD line reset");
 			if (kitprog_swd_seq(SEQUENCE_LINE_RESET) != ERROR_OK)
diff --git a/src/jtag/drivers/libjaylink b/src/jtag/drivers/libjaylink
-Subproject 699b7001d34a79c8e7064503dde1bede786fd7f
+Subproject 8645845c1abebd004e991ba9a7f808f4fd0c608
diff --git a/src/jtag/drivers/stlink_usb.c b/src/jtag/drivers/stlink_usb.c
index 0bdcd31..64868ea 100644
--- a/src/jtag/drivers/stlink_usb.c
+++ b/src/jtag/drivers/stlink_usb.c
@@ -1650,13 +1650,11 @@ static int stlink_usb_open(struct hl_interface_param_s *param, void **fd)
 
 	h->transport = param->transport;
 
-	const uint16_t vids[] = { param->vid, 0 };
-	const uint16_t pids[] = { param->pid, 0 };
-	const char *serial = param->serial;
-
-	LOG_DEBUG("transport: %d vid: 0x%04x pid: 0x%04x serial: %s",
-			param->transport, param->vid, param->pid,
-			param->serial ? param->serial : "");
+	for (unsigned i = 0; param->vid[i]; i++) {
+		LOG_DEBUG("transport: %d vid: 0x%04x pid: 0x%04x serial: %s",
+			  param->transport, param->vid[i], param->pid[i],
+			  param->serial ? param->serial : "");
+	}
 
 	/*
 	  On certain host USB configurations(e.g. MacBook Air)
@@ -1668,7 +1666,7 @@ static int stlink_usb_open(struct hl_interface_param_s *param, void **fd)
 	  in order to become operational.
 	 */
 	do {
-		if (jtag_libusb_open(vids, pids, serial, &h->fd) != ERROR_OK) {
+		if (jtag_libusb_open(param->vid, param->pid, param->serial, &h->fd) != ERROR_OK) {
 			LOG_ERROR("open failed");
 			goto error_open;
 		}
@@ -1683,8 +1681,14 @@ static int stlink_usb_open(struct hl_interface_param_s *param, void **fd)
 		/* RX EP is common for all versions */
 		h->rx_ep = STLINK_RX_EP;
 
+		uint16_t pid;
+		if (jtag_libusb_get_pid(jtag_libusb_get_device(h->fd), &pid) != ERROR_OK) {
+			LOG_DEBUG("libusb_get_pid failed");
+			goto error_open;
+		}
+
 		/* wrap version for first read */
-		switch (param->pid) {
+		switch (pid) {
 			case STLINK_V1_PID:
 				h->version.stlink = 1;
 				h->tx_ep = STLINK_TX_EP;
@@ -1736,12 +1740,6 @@ static int stlink_usb_open(struct hl_interface_param_s *param, void **fd)
 		}
 	} while (1);
 
-	/* compare usb vid/pid */
-	if ((param->vid != h->vid) || (param->pid != h->pid))
-		LOG_INFO("vid/pid are not identical: 0x%04X/0x%04X 0x%04X/0x%04X",
-			param->vid, param->pid,
-			h->vid, h->pid);
-
 	/* check if mode is supported */
 	err = ERROR_OK;
 
diff --git a/src/jtag/drivers/ti_icdi_usb.c b/src/jtag/drivers/ti_icdi_usb.c
index 171ac66..f316c82 100644
--- a/src/jtag/drivers/ti_icdi_usb.c
+++ b/src/jtag/drivers/ti_icdi_usb.c
@@ -688,14 +688,18 @@ static int icdi_usb_open(struct hl_interface_param_s *param, void **fd)
 	}
 
 	LOG_DEBUG("transport: %d vid: 0x%04x pid: 0x%04x", param->transport,
-		param->vid, param->pid);
+		  param->vid[0], param->pid[0]);
+
+	/* TODO: convert libusb_ calls to jtag_libusb_ */
+	if (param->vid[1])
+		LOG_WARNING("Bad configuration: 'hla_vid_pid' command does not accept more than one VID PID pair on ti-icdi!");
 
 	if (libusb_init(&h->usb_ctx) != 0) {
 		LOG_ERROR("libusb init failed");
 		goto error_open;
 	}
 
-	h->usb_dev = libusb_open_device_with_vid_pid(h->usb_ctx, param->vid, param->pid);
+	h->usb_dev = libusb_open_device_with_vid_pid(h->usb_ctx, param->vid[0], param->pid[0]);
 	if (!h->usb_dev) {
 		LOG_ERROR("open failed");
 		goto error_open;
diff --git a/src/jtag/hla/hla_interface.c b/src/jtag/hla/hla_interface.c
index 9217631..62a8f59 100644
--- a/src/jtag/hla/hla_interface.c
+++ b/src/jtag/hla/hla_interface.c
@@ -35,7 +35,7 @@
 
 #include <target/target.h>
 
-static struct hl_interface_s hl_if = { {0, 0, 0, 0, 0, HL_TRANSPORT_UNKNOWN, false, -1}, 0, 0 };
+static struct hl_interface_s hl_if = { {0, 0, { 0 }, { 0 }, 0, HL_TRANSPORT_UNKNOWN, false, -1}, 0, 0 };
 
 int hl_interface_open(enum hl_transports tr)
 {
@@ -264,15 +264,27 @@ COMMAND_HANDLER(hl_interface_handle_layout_command)
 
 COMMAND_HANDLER(hl_interface_handle_vid_pid_command)
 {
-	LOG_DEBUG("hl_interface_handle_vid_pid_command");
-
-	if (CMD_ARGC != 2) {
-		LOG_WARNING("ignoring extra IDs in hl_vid_pid (maximum is 1 pair)");
+	if (CMD_ARGC > HLA_MAX_USB_IDS * 2) {
+		LOG_WARNING("ignoring extra IDs in hla_vid_pid "
+			"(maximum is %d pairs)", HLA_MAX_USB_IDS);
+		CMD_ARGC = HLA_MAX_USB_IDS * 2;
+	}
+	if (CMD_ARGC < 2 || (CMD_ARGC & 1)) {
+		LOG_WARNING("incomplete hla_vid_pid configuration directive");
 		return ERROR_COMMAND_SYNTAX_ERROR;
 	}
 
-	COMMAND_PARSE_NUMBER(u16, CMD_ARGV[0], hl_if.param.vid);
-	COMMAND_PARSE_NUMBER(u16, CMD_ARGV[1], hl_if.param.pid);
+	unsigned i;
+	for (i = 0; i < CMD_ARGC; i += 2) {
+		COMMAND_PARSE_NUMBER(u16, CMD_ARGV[i], hl_if.param.vid[i / 2]);
+		COMMAND_PARSE_NUMBER(u16, CMD_ARGV[i + 1], hl_if.param.pid[i / 2]);
+	}
+
+	/*
+	 * Explicitly terminate, in case there are multiple instances of
+	 * hla_vid_pid.
+	 */
+	hl_if.param.vid[i / 2] = hl_if.param.pid[i / 2] = 0;
 
 	return ERROR_OK;
 }
diff --git a/src/jtag/hla/hla_interface.h b/src/jtag/hla/hla_interface.h
index 0992d1c..262025e 100644
--- a/src/jtag/hla/hla_interface.h
+++ b/src/jtag/hla/hla_interface.h
@@ -29,15 +29,17 @@ enum e_hl_transports;
 /** */
 extern const char *hl_transports[];
 
+#define HLA_MAX_USB_IDS 8
+
 struct hl_interface_param_s {
 	/** */
 	const char *device_desc;
 	/** */
 	const char *serial;
-	/** */
-	uint16_t vid;
-	/** */
-	uint16_t pid;
+	/** List of recognised VIDs */
+	uint16_t vid[HLA_MAX_USB_IDS + 1];
+	/** List of recognised PIDs */
+	uint16_t pid[HLA_MAX_USB_IDS + 1];
 	/** */
 	unsigned api;
 	/** */
diff --git a/src/rtos/ChibiOS.c b/src/rtos/ChibiOS.c
index 1bc1af8..ef0bb16 100644
--- a/src/rtos/ChibiOS.c
+++ b/src/rtos/ChibiOS.c
@@ -103,7 +103,7 @@ static struct ChibiOS_params ChibiOS_params_list[] = {
 };
 #define CHIBIOS_NUM_PARAMS ((int)(sizeof(ChibiOS_params_list)/sizeof(struct ChibiOS_params)))
 
-static int ChibiOS_detect_rtos(struct target *target);
+static bool ChibiOS_detect_rtos(struct target *target);
 static int ChibiOS_create(struct target *target);
 static int ChibiOS_update_threads(struct rtos *rtos);
 static int ChibiOS_get_thread_reg_list(struct rtos *rtos, int64_t thread_id, char **hex_reg_list);
@@ -510,7 +510,7 @@ static int ChibiOS_get_symbol_list_to_lookup(symbol_table_elem_t *symbol_list[])
 	return 0;
 }
 
-static int ChibiOS_detect_rtos(struct target *target)
+static bool ChibiOS_detect_rtos(struct target *target)
 {
 	if ((target->rtos->symbols != NULL) &&
 			((target->rtos->symbols[ChibiOS_VAL_rlist].address != 0) ||
@@ -519,14 +519,14 @@ static int ChibiOS_detect_rtos(struct target *target)
 		if (target->rtos->symbols[ChibiOS_VAL_ch_debug].address == 0) {
 			LOG_INFO("It looks like the target may be running ChibiOS "
 					"without ch_debug.");
-			return 0;
+			return false;
 		}
 
 		/* looks like ChibiOS with memory map enabled.*/
-		return 1;
+		return true;
 	}
 
-	return 0;
+	return false;
 }
 
 static int ChibiOS_create(struct target *target)
diff --git a/src/rtos/FreeRTOS.c b/src/rtos/FreeRTOS.c
index 83961eb..6027d67 100644
--- a/src/rtos/FreeRTOS.c
+++ b/src/rtos/FreeRTOS.c
@@ -99,7 +99,7 @@ static const struct FreeRTOS_params FreeRTOS_params_list[] = {
 
 #define FREERTOS_NUM_PARAMS ((int)(sizeof(FreeRTOS_params_list)/sizeof(struct FreeRTOS_params)))
 
-static int FreeRTOS_detect_rtos(struct target *target);
+static bool FreeRTOS_detect_rtos(struct target *target);
 static int FreeRTOS_create(struct target *target);
 static int FreeRTOS_update_threads(struct rtos *rtos);
 static int FreeRTOS_get_thread_reg_list(struct rtos *rtos, int64_t thread_id, char **hex_reg_list);
@@ -528,14 +528,14 @@ static int FreeRTOS_get_thread_ascii_info(struct rtos *rtos, threadid_t thread_i
 
 #endif
 
-static int FreeRTOS_detect_rtos(struct target *target)
+static bool FreeRTOS_detect_rtos(struct target *target)
 {
 	if ((target->rtos->symbols != NULL) &&
 			(target->rtos->symbols[FreeRTOS_VAL_pxReadyTasksLists].address != 0)) {
 		/* looks like FreeRTOS */
-		return 1;
+		return true;
 	}
-	return 0;
+	return false;
 }
 
 static int FreeRTOS_create(struct target *target)
diff --git a/src/rtos/ThreadX.c b/src/rtos/ThreadX.c
index ab8a66e..b7dbe6d 100644
--- a/src/rtos/ThreadX.c
+++ b/src/rtos/ThreadX.c
@@ -35,7 +35,7 @@ static const struct rtos_register_stacking *get_stacking_info_arm926ejs(const st
 static int is_thread_id_valid(const struct rtos *rtos, int64_t thread_id);
 static int is_thread_id_valid_arm926ejs(const struct rtos *rtos, int64_t thread_id);
 
-static int ThreadX_detect_rtos(struct target *target);
+static bool ThreadX_detect_rtos(struct target *target);
 static int ThreadX_create(struct target *target);
 static int ThreadX_update_threads(struct rtos *rtos);
 static int ThreadX_get_thread_reg_list(struct rtos *rtos, int64_t thread_id, char **hex_reg_list);
@@ -492,14 +492,14 @@ static int ThreadX_get_symbol_list_to_lookup(symbol_table_elem_t *symbol_list[])
 	return 0;
 }
 
-static int ThreadX_detect_rtos(struct target *target)
+static bool ThreadX_detect_rtos(struct target *target)
 {
 	if ((target->rtos->symbols != NULL) &&
 			(target->rtos->symbols[ThreadX_VAL_tx_thread_created_ptr].address != 0)) {
 		/* looks like ThreadX */
-		return 1;
+		return true;
 	}
-	return 0;
+	return false;
 }
 
 #if 0
diff --git a/src/rtos/eCos.c b/src/rtos/eCos.c
index edc3d8b..9e41030 100644
--- a/src/rtos/eCos.c
+++ b/src/rtos/eCos.c
@@ -27,7 +27,7 @@
 #include "helper/types.h"
 #include "rtos_ecos_stackings.h"
 
-static int eCos_detect_rtos(struct target *target);
+static bool eCos_detect_rtos(struct target *target);
 static int eCos_create(struct target *target);
 static int eCos_update_threads(struct rtos *rtos);
 static int eCos_get_thread_reg_list(struct rtos *rtos, int64_t thread_id, char **hex_reg_list);
@@ -363,14 +363,14 @@ static int eCos_get_symbol_list_to_lookup(symbol_table_elem_t *symbol_list[])
 	return 0;
 }
 
-static int eCos_detect_rtos(struct target *target)
+static bool eCos_detect_rtos(struct target *target)
 {
 	if ((target->rtos->symbols != NULL) &&
 			(target->rtos->symbols[eCos_VAL_thread_list].address != 0)) {
 		/* looks like eCos */
-		return 1;
+		return true;
 	}
-	return 0;
+	return false;
 }
 
 static int eCos_create(struct target *target)
diff --git a/src/rtos/embKernel.c b/src/rtos/embKernel.c
index e515383..a40c86c 100644
--- a/src/rtos/embKernel.c
+++ b/src/rtos/embKernel.c
@@ -31,7 +31,7 @@
 
 #define EMBKERNEL_MAX_THREAD_NAME_STR_SIZE (64)
 
-static int embKernel_detect_rtos(struct target *target);
+static bool embKernel_detect_rtos(struct target *target);
 static int embKernel_create(struct target *target);
 static int embKernel_update_threads(struct rtos *rtos);
 static int embKernel_get_thread_reg_list(struct rtos *rtos, int64_t thread_id, char **hex_reg_list);
@@ -107,13 +107,13 @@ static const struct embKernel_params embKernel_params_list[] = {
 		}
 };
 
-static int embKernel_detect_rtos(struct target *target)
+static bool embKernel_detect_rtos(struct target *target)
 {
 	if (target->rtos->symbols != NULL) {
 		if (target->rtos->symbols[SYMBOL_ID_sCurrentTask].address != 0)
-			return 1;
+			return true;
 	}
-	return 0;
+	return false;
 }
 
 static int embKernel_create(struct target *target)
diff --git a/src/rtos/linux.c b/src/rtos/linux.c
index 3efaab1..3b2a2b0 100644
--- a/src/rtos/linux.c
+++ b/src/rtos/linux.c
@@ -309,10 +309,10 @@ static int linux_os_thread_reg_list(struct rtos *rtos,
 	return ERROR_OK;
 }
 
-static int linux_os_detect(struct target *target)
+static bool linux_os_detect(struct target *target)
 {
 	LOG_INFO("should no be called");
-	return 0;
+	return false;
 }
 
 static int linux_os_smp_init(struct target *target);
diff --git a/src/rtos/mqx.c b/src/rtos/mqx.c
index 63a48c5..531b03b 100644
--- a/src/rtos/mqx.c
+++ b/src/rtos/mqx.c
@@ -244,9 +244,9 @@ static int mqx_is_scheduler_running(
 }
 
 /*
- * API function, return 1 if MQX is present
+ * API function, return true if MQX is present
  */
-static int mqx_detect_rtos(
+static bool mqx_detect_rtos(
 	struct target *target
 )
 {
@@ -254,9 +254,9 @@ static int mqx_detect_rtos(
 		(target->rtos->symbols != NULL) &&
 		(target->rtos->symbols[mqx_VAL_mqx_kernel_data].address != 0)
 	) {
-		return 1;
+		return true;
 	}
-	return 0;
+	return false;
 }
 
 /*
diff --git a/src/rtos/riscv_debug.c b/src/rtos/riscv_debug.c
index 3abdf74..7996fb2 100644
--- a/src/rtos/riscv_debug.c
+++ b/src/rtos/riscv_debug.c
@@ -10,7 +10,7 @@
 static int riscv_gdb_thread_packet(struct connection *connection, const char *packet, int packet_size);
 static int riscv_gdb_v_packet(struct connection *connection, const char *packet, int packet_size);
 
-static int riscv_detect_rtos(struct target *target)
+static bool riscv_detect_rtos(struct target *target)
 {
 	LOG_ERROR("riscv_detect_rtos() unimplemented");
 	return -1;
diff --git a/src/rtos/rtos.h b/src/rtos/rtos.h
index 70cec87..9da035a 100644
--- a/src/rtos/rtos.h
+++ b/src/rtos/rtos.h
@@ -60,7 +60,7 @@ struct rtos {
 
 struct rtos_type {
 	const char *name;
-	int (*detect_rtos)(struct target *target);
+	bool (*detect_rtos)(struct target *target);
 	int (*create)(struct target *target);
 	int (*smp_init)(struct target *target);
 	int (*update_threads)(struct rtos *rtos);
diff --git a/src/rtos/uCOS-III.c b/src/rtos/uCOS-III.c
index 0a0fb3e..8e63ea4 100644
--- a/src/rtos/uCOS-III.c
+++ b/src/rtos/uCOS-III.c
@@ -241,7 +241,7 @@ static int uCOS_III_update_thread_offsets(struct rtos *rtos)
 	return ERROR_OK;
 }
 
-static int uCOS_III_detect_rtos(struct target *target)
+static bool uCOS_III_detect_rtos(struct target *target)
 {
 	return target->rtos->symbols != NULL &&
 			target->rtos->symbols[uCOS_III_VAL_OSRunning].address != 0;
diff --git a/src/server/gdb_server.c b/src/server/gdb_server.c
index ecf46ad..09e7bb9 100644
--- a/src/server/gdb_server.c
+++ b/src/server/gdb_server.c
@@ -71,8 +71,8 @@ struct gdb_connection {
 	int ctrl_c;
 	enum target_state frontend_state;
 	struct image *vflash_image;
-	int closed;
-	int busy;
+	bool closed;
+	bool busy;
 	int noack_mode;
 	/* set flag to true if you want the next stepi to return immediately.
 	 * allowing GDB to pick up a fresh set of register values from the target
@@ -215,7 +215,7 @@ static int gdb_get_char_inner(struct connection *connection, int *next_char)
 		if (gdb_con->buf_cnt > 0)
 			break;
 		if (gdb_con->buf_cnt == 0) {
-			gdb_con->closed = 1;
+			gdb_con->closed = true;
 			return ERROR_SERVER_REMOTE_CLOSED;
 		}
 
@@ -227,10 +227,10 @@ static int gdb_get_char_inner(struct connection *connection, int *next_char)
 				usleep(1000);
 				break;
 			case WSAECONNABORTED:
-				gdb_con->closed = 1;
+				gdb_con->closed = true;
 				return ERROR_SERVER_REMOTE_CLOSED;
 			case WSAECONNRESET:
-				gdb_con->closed = 1;
+				gdb_con->closed = true;
 				return ERROR_SERVER_REMOTE_CLOSED;
 			default:
 				LOG_ERROR("read: %d", errno);
@@ -242,14 +242,14 @@ static int gdb_get_char_inner(struct connection *connection, int *next_char)
 				usleep(1000);
 				break;
 			case ECONNABORTED:
-				gdb_con->closed = 1;
+				gdb_con->closed = true;
 				return ERROR_SERVER_REMOTE_CLOSED;
 			case ECONNRESET:
-				gdb_con->closed = 1;
+				gdb_con->closed = true;
 				return ERROR_SERVER_REMOTE_CLOSED;
 			default:
 				LOG_ERROR("read: %s", strerror(errno));
-				gdb_con->closed = 1;
+				gdb_con->closed = true;
 				return ERROR_SERVER_REMOTE_CLOSED;
 		}
 #endif
@@ -341,7 +341,7 @@ static int gdb_write(struct connection *connection, void *data, int len)
 
 	if (connection_write(connection, data, len) == len)
 		return ERROR_OK;
-	gdb_con->closed = 1;
+	gdb_con->closed = true;
 	return ERROR_SERVER_REMOTE_CLOSED;
 }
 
@@ -448,7 +448,7 @@ static int gdb_put_packet_inner(struct connection *connection,
 				return ERROR_OK;
 			} else {
 				LOG_ERROR("unknown character(1) 0x%2.2x in reply, dropping connection", reply);
-				gdb_con->closed = 1;
+				gdb_con->closed = true;
 				return ERROR_SERVER_REMOTE_CLOSED;
 			}
 		} else if (reply == '$') {
@@ -458,7 +458,7 @@ static int gdb_put_packet_inner(struct connection *connection,
 		} else {
 			LOG_ERROR("unknown character(2) 0x%2.2x in reply, dropping connection",
 				reply);
-			gdb_con->closed = 1;
+			gdb_con->closed = true;
 			return ERROR_SERVER_REMOTE_CLOSED;
 		}
 	}
@@ -471,9 +471,9 @@ static int gdb_put_packet_inner(struct connection *connection,
 int gdb_put_packet(struct connection *connection, char *buffer, int len)
 {
 	struct gdb_connection *gdb_con = connection->priv;
-	gdb_con->busy = 1;
+	gdb_con->busy = true;
 	int retval = gdb_put_packet_inner(connection, buffer, len);
-	gdb_con->busy = 0;
+	gdb_con->busy = false;
 
 	/* we sent some data, reset timer for keep alive messages */
 	kept_alive();
@@ -679,9 +679,9 @@ static int gdb_get_packet_inner(struct connection *connection,
 static int gdb_get_packet(struct connection *connection, char *buffer, int *len)
 {
 	struct gdb_connection *gdb_con = connection->priv;
-	gdb_con->busy = 1;
+	gdb_con->busy = true;
 	int retval = gdb_get_packet_inner(connection, buffer, len);
-	gdb_con->busy = 0;
+	gdb_con->busy = false;
 	return retval;
 }
 
@@ -917,10 +917,11 @@ static int gdb_target_callback_event_handler(struct target *target,
 static int gdb_new_connection(struct connection *connection)
 {
 	struct gdb_connection *gdb_connection = malloc(sizeof(struct gdb_connection));
-	struct gdb_service *gdb_service = connection->service->priv;
+	struct target *target;
 	int retval;
 	int initial_ack;
 
+	target = get_target_from_connection(connection);
 	connection->priv = gdb_connection;
 
 	/* initialize gdb connection information */
@@ -929,8 +930,8 @@ static int gdb_new_connection(struct connection *connection)
 	gdb_connection->ctrl_c = 0;
 	gdb_connection->frontend_state = TARGET_HALTED;
 	gdb_connection->vflash_image = NULL;
-	gdb_connection->closed = 0;
-	gdb_connection->busy = 0;
+	gdb_connection->closed = false;
+	gdb_connection->busy = false;
 	gdb_connection->noack_mode = 0;
 	gdb_connection->sync = false;
 	gdb_connection->mem_write_error = false;
@@ -949,12 +950,12 @@ static int gdb_new_connection(struct connection *connection)
 	 * GDB session could leave dangling breakpoints if e.g. communication
 	 * timed out.
 	 */
-	breakpoint_clear_target(gdb_service->target);
-	watchpoint_clear_target(gdb_service->target);
+	breakpoint_clear_target(target);
+	watchpoint_clear_target(target);
 
 	/* clean previous rtos session if supported*/
-	if ((gdb_service->target->rtos) && (gdb_service->target->rtos->type->clean))
-		gdb_service->target->rtos->type->clean(gdb_service->target);
+	if ((target->rtos) && (target->rtos->type->clean))
+		target->rtos->type->clean(target);
 
 	/* remove the initial ACK from the incoming buffer */
 	retval = gdb_get_char(connection, &initial_ack);
@@ -966,7 +967,7 @@ static int gdb_new_connection(struct connection *connection)
 	 */
 	if (initial_ack != '+')
 		gdb_putback_char(connection, initial_ack);
-	target_call_event_callbacks(gdb_service->target, TARGET_EVENT_GDB_ATTACH);
+	target_call_event_callbacks(target, TARGET_EVENT_GDB_ATTACH);
 
 	if (gdb_use_memory_map) {
 		/* Connect must fail if the memory map can't be set up correctly.
@@ -978,7 +979,7 @@ static int gdb_new_connection(struct connection *connection)
 		for (i = 0; i < flash_get_bank_count(); i++) {
 			struct flash_bank *p;
 			p = get_flash_bank_by_num_noprobe(i);
-			if (p->target != gdb_service->target)
+			if (p->target != target)
 				continue;
 			retval = get_flash_bank_by_num(i, &p);
 			if (retval != ERROR_OK) {
@@ -992,8 +993,8 @@ static int gdb_new_connection(struct connection *connection)
 	gdb_actual_connections++;
 	LOG_DEBUG("New GDB Connection: %d, Target %s, state: %s",
 			gdb_actual_connections,
-			target_name(gdb_service->target),
-			target_state_name(gdb_service->target));
+			target_name(target),
+			target_state_name(target));
 
 	/* DANGER! If we fail subsequently, we must remove this handler,
 	 * otherwise we occasionally see crashes as the timer can invoke the
@@ -1007,9 +1008,11 @@ static int gdb_new_connection(struct connection *connection)
 
 static int gdb_connection_closed(struct connection *connection)
 {
-	struct gdb_service *gdb_service = connection->service->priv;
+	struct target *target;
 	struct gdb_connection *gdb_connection = connection->priv;
 
+	target = get_target_from_connection(connection);
+
 	/* we're done forwarding messages. Tear down callback before
 	 * cleaning up connection.
 	 */
@@ -1017,8 +1020,8 @@ static int gdb_connection_closed(struct connection *connection)
 
 	gdb_actual_connections--;
 	LOG_DEBUG("GDB Close, Target: %s, state: %s, gdb_actual_connections=%d",
-		target_name(gdb_service->target),
-		target_state_name(gdb_service->target),
+		target_name(target),
+		target_state_name(target),
 		gdb_actual_connections);
 
 	/* see if an image built with vFlash commands is left */
@@ -1029,7 +1032,7 @@ static int gdb_connection_closed(struct connection *connection)
 	}
 
 	/* if this connection registered a debug-message receiver delete it */
-	delete_debug_msg_receiver(connection->cmd_ctx, gdb_service->target);
+	delete_debug_msg_receiver(connection->cmd_ctx, target);
 
 	if (connection->priv) {
 		free(connection->priv);
@@ -1039,9 +1042,9 @@ static int gdb_connection_closed(struct connection *connection)
 
 	target_unregister_event_callback(gdb_target_callback_event_handler, connection);
 
-	target_call_event_callbacks(gdb_service->target, TARGET_EVENT_GDB_END);
+	target_call_event_callbacks(target, TARGET_EVENT_GDB_END);
 
-	target_call_event_callbacks(gdb_service->target, TARGET_EVENT_GDB_DETACH);
+	target_call_event_callbacks(target, TARGET_EVENT_GDB_DETACH);
 
 	return ERROR_OK;
 }
@@ -2583,7 +2586,6 @@ static int gdb_v_packet(struct connection *connection,
 		char const *packet, int packet_size)
 {
 	struct gdb_connection *gdb_connection = connection->priv;
-	struct gdb_service *gdb_service = connection->service->priv;
 	int result;
 
 	struct target *target = get_target_from_connection(connection);
@@ -2629,18 +2631,18 @@ static int gdb_v_packet(struct connection *connection,
 		flash_set_dirty();
 
 		/* perform any target specific operations before the erase */
-		target_call_event_callbacks(gdb_service->target,
+		target_call_event_callbacks(target,
 			TARGET_EVENT_GDB_FLASH_ERASE_START);
 
 		/* vFlashErase:addr,length messages require region start and
 		 * end to be "block" aligned ... if padding is ever needed,
 		 * GDB will have become dangerously confused.
 		 */
-		result = flash_erase_address_range(gdb_service->target,
-				false, addr, length);
+		result = flash_erase_address_range(target, false, addr,
+			length);
 
 		/* perform any target specific operations after the erase */
-		target_call_event_callbacks(gdb_service->target,
+		target_call_event_callbacks(target,
 			TARGET_EVENT_GDB_FLASH_ERASE_END);
 
 		/* perform erase */
@@ -2695,10 +2697,12 @@ static int gdb_v_packet(struct connection *connection,
 
 		/* process the flashing buffer. No need to erase as GDB
 		 * always issues a vFlashErase first. */
-		target_call_event_callbacks(gdb_service->target,
+		target_call_event_callbacks(target,
 				TARGET_EVENT_GDB_FLASH_WRITE_START);
-		result = flash_write(gdb_service->target, gdb_connection->vflash_image, &written, 0);
-		target_call_event_callbacks(gdb_service->target, TARGET_EVENT_GDB_FLASH_WRITE_END);
+		result = flash_write(target, gdb_connection->vflash_image,
+			&written, 0);
+		target_call_event_callbacks(target,
+			TARGET_EVENT_GDB_FLASH_WRITE_END);
 		if (result != ERROR_OK) {
 			if (result == ERROR_FLASH_DST_OUT_OF_BANK)
 				gdb_put_packet(connection, "E.memtype", 9);
@@ -2722,9 +2726,8 @@ static int gdb_v_packet(struct connection *connection,
 
 static int gdb_detach(struct connection *connection)
 {
-	struct gdb_service *gdb_service = connection->service->priv;
-
-	target_call_event_callbacks(gdb_service->target, TARGET_EVENT_GDB_DETACH);
+	target_call_event_callbacks(get_target_from_connection(connection),
+		TARGET_EVENT_GDB_DETACH);
 
 	return gdb_put_packet(connection, "OK", 2);
 }
@@ -2803,14 +2806,15 @@ static int gdb_input_inner(struct connection *connection)
 	/* Do not allocate this on the stack */
 	static char gdb_packet_buffer[GDB_BUFFER_SIZE];
 
-	struct gdb_service *gdb_service = connection->service->priv;
-	struct target *target = gdb_service->target;
+	struct target *target;
 	char const *packet = gdb_packet_buffer;
 	int packet_size;
 	int retval;
 	struct gdb_connection *gdb_con = connection->priv;
 	static int extended_protocol;
 
+	target = get_target_from_connection(connection);
+
 	/* drain input buffer. If one of the packets fail, then an error
 	 * packet is replied, if applicable.
 	 *
@@ -2980,8 +2984,8 @@ static int gdb_input_inner(struct connection *connection)
 					break;
 				case 'R':
 					/* handle extended restart packet */
-					breakpoint_clear_target(gdb_service->target);
-					watchpoint_clear_target(gdb_service->target);
+					breakpoint_clear_target(target);
+					watchpoint_clear_target(target);
 					command_run_linef(connection->cmd_ctx, "ocd_gdb_restart %s",
 							target_name(target));
 					/* set connection as attached after reset */
diff --git a/src/server/telnet_server.c b/src/server/telnet_server.c
index e33188b..7507afe 100644
--- a/src/server/telnet_server.c
+++ b/src/server/telnet_server.c
@@ -222,7 +222,6 @@ static int telnet_new_connection(struct connection *connection)
 	telnet_connection->closed = 0;
 	telnet_connection->line_size = 0;
 	telnet_connection->line_cursor = 0;
-	telnet_connection->option_size = 0;
 	telnet_connection->prompt = strdup("> ");
 	telnet_connection->state = TELNET_STATE_DATA;
 
diff --git a/src/server/telnet_server.h b/src/server/telnet_server.h
index 04ba965..f8fb826 100644
--- a/src/server/telnet_server.h
+++ b/src/server/telnet_server.h
@@ -27,11 +27,10 @@
 
 #include <server/server.h>
 
-#define TELNET_BUFFER_SIZE (1024)
+#define TELNET_BUFFER_SIZE (10*1024)
 
-#define TELNET_OPTION_MAX_SIZE (128)
 #define TELNET_LINE_HISTORY_SIZE (128)
-#define TELNET_LINE_MAX_SIZE (256)
+#define TELNET_LINE_MAX_SIZE (10*256)
 
 enum telnet_states {
 	TELNET_STATE_DATA,
@@ -51,8 +50,6 @@ struct telnet_connection {
 	char line[TELNET_LINE_MAX_SIZE];
 	int line_size;
 	int line_cursor;
-	char option[TELNET_OPTION_MAX_SIZE];
-	int option_size;
 	char last_escape;
 	char *history[TELNET_LINE_HISTORY_SIZE];
 	int next_history;
diff --git a/src/svf/svf.c b/src/svf/svf.c
index e7e815c..1d686ba 100644
--- a/src/svf/svf.c
+++ b/src/svf/svf.c
@@ -661,11 +661,13 @@ static int svf_read_command_from_file(FILE *fd)
 				if (svf_getline(&svf_read_line, &svf_read_line_size, svf_fd) <= 0)
 					return ERROR_FAIL;
 				i = -1;
+				/* fallthrough */
 			case '\r':
 				slash = 0;
 				/* Don't save '\r' and '\n' if no data is parsed */
 				if (!cmd_pos)
 					break;
+				/* fallthrough */
 			default:
 				/* The parsing code currently expects a space
 				 * before parentheses -- "TDI (123)".  Also a
diff --git a/src/target/Makefile.am b/src/target/Makefile.am
index 1e81f4a..9576b23 100644
--- a/src/target/Makefile.am
+++ b/src/target/Makefile.am
@@ -19,6 +19,7 @@ noinst_LTLIBRARIES += %D%/libtarget.la
 	$(AVR32_SRC) \
 	$(MIPS32_SRC) \
 	$(NDS32_SRC) \
+	$(STM8_SRC) \
 	$(INTEL_IA32_SRC) \
 	$(RISCV_SRC) \
 	%D%/avrt.c \
@@ -125,6 +126,9 @@ NDS32_SRC = \
 	%D%/nds32_v3m.c \
 	%D%/nds32_aice.c
 
+STM8_SRC = \
+	%D%/stm8.c
+
 INTEL_IA32_SRC = \
 	%D%/quark_x10xx.c \
 	%D%/quark_d20xx.c \
@@ -213,6 +217,7 @@ RISCV_SRC = \
 	%D%/nds32_v3.h \
 	%D%/nds32_v3m.h \
 	%D%/nds32_aice.h \
+	%D%/stm8.h \
 	%D%/lakemont.h \
 	%D%/x86_32_common.h \
 	%D%/arm_cti.h
diff --git a/src/target/adi_v5_swd.c b/src/target/adi_v5_swd.c
index 41ddbd7..a6aada3 100644
--- a/src/target/adi_v5_swd.c
+++ b/src/target/adi_v5_swd.c
@@ -427,7 +427,10 @@ static int swd_init(struct command_context *ctx)
 	/* First connect after init is not reconnecting. */
 	dap->do_reconnect = false;
 
-	return swd_connect(dap);
+	int retval = swd_connect(dap);
+	if (retval != ERROR_OK)
+		LOG_ERROR("SWD connect failed");
+	return retval;
 }
 
 static struct transport swd_transport = {
diff --git a/src/target/arm.h b/src/target/arm.h
index d63ead2..f89aa68 100644
--- a/src/target/arm.h
+++ b/src/target/arm.h
@@ -157,6 +157,9 @@ struct arm {
 
 	int (*setup_semihosting)(struct target *target, int enable);
 
+	/** Semihosting command line. */
+	char *semihosting_cmdline;
+
 	/** Backpointer to the target. */
 	struct target *target;
 
diff --git a/src/target/arm_adi_v5.c b/src/target/arm_adi_v5.c
index eafc2dd..2006290 100644
--- a/src/target/arm_adi_v5.c
+++ b/src/target/arm_adi_v5.c
@@ -346,8 +346,10 @@ static int mem_ap_write(struct adiv5_ap *ap, const uint8_t *buffer, uint32_t siz
 			case 4:
 				outvalue |= (uint32_t)*buffer++ << 8 * (address++ & 3);
 				outvalue |= (uint32_t)*buffer++ << 8 * (address++ & 3);
+				/* fallthrough */
 			case 2:
 				outvalue |= (uint32_t)*buffer++ << 8 * (address++ & 3);
+				/* fallthrough */
 			case 1:
 				outvalue |= (uint32_t)*buffer++ << 8 * (address++ & 3);
 			}
@@ -509,8 +511,10 @@ static int mem_ap_read(struct adiv5_ap *ap, uint8_t *buffer, uint32_t size, uint
 			case 4:
 				*buffer++ = *read_ptr >> 8 * (3 - (address++ & 3));
 				*buffer++ = *read_ptr >> 8 * (3 - (address++ & 3));
+				/* fallthrough */
 			case 2:
 				*buffer++ = *read_ptr >> 8 * (3 - (address++ & 3));
+				/* fallthrough */
 			case 1:
 				*buffer++ = *read_ptr >> 8 * (3 - (address++ & 3));
 			}
@@ -519,8 +523,10 @@ static int mem_ap_read(struct adiv5_ap *ap, uint8_t *buffer, uint32_t size, uint
 			case 4:
 				*buffer++ = *read_ptr >> 8 * (address++ & 3);
 				*buffer++ = *read_ptr >> 8 * (address++ & 3);
+				/* fallthrough */
 			case 2:
 				*buffer++ = *read_ptr >> 8 * (address++ & 3);
+				/* fallthrough */
 			case 1:
 				*buffer++ = *read_ptr >> 8 * (address++ & 3);
 			}
@@ -1053,7 +1059,7 @@ static int dap_rom_display(struct command_context *cmd_ctx,
 	int retval;
 	uint64_t pid;
 	uint32_t cid;
-	char tabs[7] = "";
+	char tabs[16] = "";
 
 	if (depth > 16) {
 		command_print(cmd_ctx, "\tTables too deep");
diff --git a/src/target/arm_disassembler.c b/src/target/arm_disassembler.c
index 5277b94..3f1daca 100644
--- a/src/target/arm_disassembler.c
+++ b/src/target/arm_disassembler.c
@@ -3299,6 +3299,7 @@ static int t2ev_data_immed(uint32_t opcode, uint32_t address,
 		case 0x10:
 		case 0x12:
 			is_signed = true;
+			/* fallthrough */
 		case 0x18:
 		case 0x1a:
 			/* signed/unsigned saturated add */
diff --git a/src/target/arm_semihosting.c b/src/target/arm_semihosting.c
index 2525119..f31f901 100644
--- a/src/target/arm_semihosting.c
+++ b/src/target/arm_semihosting.c
@@ -465,7 +465,7 @@ static int do_semihosting(struct target *target)
 		else {
 			uint32_t a = target_buffer_get_u32(target, params+0);
 			uint32_t l = target_buffer_get_u32(target, params+4);
-			char *arg = "foobar";
+			char *arg = arm->semihosting_cmdline != NULL ? arm->semihosting_cmdline : "";
 			uint32_t s = strlen(arg) + 1;
 			if (l < s)
 				arm->semihosting_result = -1;
diff --git a/src/target/armv4_5.c b/src/target/armv4_5.c
index 2029ca9..48050b0 100644
--- a/src/target/armv4_5.c
+++ b/src/target/armv4_5.c
@@ -1091,6 +1091,42 @@ COMMAND_HANDLER(handle_arm_semihosting_fileio_command)
 	return ERROR_OK;
 }
 
+COMMAND_HANDLER(handle_arm_semihosting_cmdline)
+{
+	struct target *target = get_current_target(CMD_CTX);
+	unsigned int i;
+
+	if (target == NULL) {
+		LOG_ERROR("No target selected");
+		return ERROR_FAIL;
+	}
+
+	struct arm *arm = target_to_arm(target);
+
+	if (!is_arm(arm)) {
+		command_print(CMD_CTX, "current target isn't an ARM");
+		return ERROR_FAIL;
+	}
+
+	if (!arm->setup_semihosting) {
+		command_print(CMD_CTX, "semihosting not supported for current target");
+		return ERROR_FAIL;
+	}
+
+	free(arm->semihosting_cmdline);
+	arm->semihosting_cmdline = CMD_ARGC > 0 ? strdup(CMD_ARGV[0]) : NULL;
+
+	for (i = 1; i < CMD_ARGC; i++) {
+		char *cmdline = alloc_printf("%s %s", arm->semihosting_cmdline, CMD_ARGV[i]);
+		if (cmdline == NULL)
+			break;
+		free(arm->semihosting_cmdline);
+		arm->semihosting_cmdline = cmdline;
+	}
+
+	return ERROR_OK;
+}
+
 static const struct command_registration arm_exec_command_handlers[] = {
 	{
 		.name = "reg",
@@ -1134,6 +1170,13 @@ static const struct command_registration arm_exec_command_handlers[] = {
 		.help = "activate support for semihosting operations",
 	},
 	{
+		"semihosting_cmdline",
+		.handler = handle_arm_semihosting_cmdline,
+		.mode = COMMAND_EXEC,
+		.usage = "arguments",
+		.help = "command line arguments to be passed to program",
+	},
+	{
 		"semihosting_fileio",
 		.handler = handle_arm_semihosting_fileio_command,
 		.mode = COMMAND_EXEC,
diff --git a/src/target/armv7a.c b/src/target/armv7a.c
index 6021def..db72afd 100644
--- a/src/target/armv7a.c
+++ b/src/target/armv7a.c
@@ -355,7 +355,7 @@ int armv7a_mmu_translate_va_pa(struct target *target, uint32_t va,
 				break;
 			case 7:
 				LOG_INFO("inner: Write-Back, no Write-Allocate");
-
+				break;
 			default:
 				LOG_INFO("inner: %" PRIx32 " ???", INNER);
 		}
diff --git a/src/target/armv7m.h b/src/target/armv7m.h
index 284bb9c..6f5d6f9 100644
--- a/src/target/armv7m.h
+++ b/src/target/armv7m.h
@@ -174,7 +174,7 @@ target_to_armv7m(struct target *target)
 	return container_of(target->arch_info, struct armv7m_common, arm);
 }
 
-static inline bool is_armv7m(struct armv7m_common *armv7m)
+static inline bool is_armv7m(const struct armv7m_common *armv7m)
 {
 	return armv7m->common_magic == ARMV7M_COMMON_MAGIC;
 }
diff --git a/src/target/cortex_m.c b/src/target/cortex_m.c
index e80cd23..2f8c2a2 100644
--- a/src/target/cortex_m.c
+++ b/src/target/cortex_m.c
@@ -168,12 +168,8 @@ static int cortex_m_single_step_core(struct target *target)
 {
 	struct cortex_m_common *cortex_m = target_to_cm(target);
 	struct armv7m_common *armv7m = &cortex_m->armv7m;
-	uint32_t dhcsr_save;
 	int retval;
 
-	/* backup dhcsr reg */
-	dhcsr_save = cortex_m->dcb_dhcsr;
-
 	/* Mask interrupts before clearing halt, if done already.  This avoids
 	 * Erratum 377497 (fixed in r1p0) where setting MASKINTS while clearing
 	 * HALT can put the core into an unknown state.
@@ -191,7 +187,6 @@ static int cortex_m_single_step_core(struct target *target)
 	LOG_DEBUG(" ");
 
 	/* restore dhcsr reg */
-	cortex_m->dcb_dhcsr = dhcsr_save;
 	cortex_m_clear_halt(target);
 
 	return ERROR_OK;
@@ -242,7 +237,7 @@ static int cortex_m_endreset_event(struct target *target)
 	if (retval != ERROR_OK)
 		return retval;
 	if (!(cortex_m->dcb_dhcsr & C_DEBUGEN)) {
-		retval = mem_ap_write_u32(armv7m->debug_ap, DCB_DHCSR, DBGKEY | C_DEBUGEN);
+		retval = cortex_m_write_debug_halt_mask(target, 0, C_HALT | C_STEP | C_MASKINTS);
 		if (retval != ERROR_OK)
 			return retval;
 	}
@@ -1005,12 +1000,12 @@ static int cortex_m_assert_reset(struct target *target)
 	/* Store important errors instead of failing and proceed to reset assert */
 
 	if (retval != ERROR_OK || !(cortex_m->dcb_dhcsr & C_DEBUGEN))
-		retval = mem_ap_write_u32(armv7m->debug_ap, DCB_DHCSR, DBGKEY | C_DEBUGEN);
+		retval = cortex_m_write_debug_halt_mask(target, 0, C_HALT | C_STEP | C_MASKINTS);
 
 	/* If the processor is sleeping in a WFI or WFE instruction, the
 	 * C_HALT bit must be asserted to regain control */
 	if (retval == ERROR_OK && (cortex_m->dcb_dhcsr & S_SLEEP))
-		retval = mem_ap_write_u32(armv7m->debug_ap, DCB_DHCSR, DBGKEY | C_HALT | C_DEBUGEN);
+		retval = cortex_m_write_debug_halt_mask(target, C_HALT, 0);
 
 	mem_ap_write_u32(armv7m->debug_ap, DCB_DCRDR, 0);
 	/* Ignore less important errors */
@@ -1018,8 +1013,7 @@ static int cortex_m_assert_reset(struct target *target)
 	if (!target->reset_halt) {
 		/* Set/Clear C_MASKINTS in a separate operation */
 		if (cortex_m->dcb_dhcsr & C_MASKINTS)
-			mem_ap_write_atomic_u32(armv7m->debug_ap, DCB_DHCSR,
-					DBGKEY | C_DEBUGEN | C_HALT);
+			cortex_m_write_debug_halt_mask(target, 0, C_MASKINTS);
 
 		/* clear any debug flags before resuming */
 		cortex_m_clear_halt(target);
diff --git a/src/target/nds32_cmd.c b/src/target/nds32_cmd.c
index edb4872..500651d 100644
--- a/src/target/nds32_cmd.c
+++ b/src/target/nds32_cmd.c
@@ -816,7 +816,7 @@ static int jim_nds32_bulk_read(Jim_Interp *interp, int argc, Jim_Obj * const *ar
 	uint32_t *data = malloc(count * sizeof(uint32_t));
 	int result;
 	result = target_read_buffer(target, address, count * 4, (uint8_t *)data);
-	char data_str[11];
+	char data_str[12];
 
 	jim_wide i;
 	Jim_SetResult(interp, Jim_NewEmptyStringObj(interp));
diff --git a/src/target/openrisc/jsp_server.c b/src/target/openrisc/jsp_server.c
index e581fb8..2d90114 100644
--- a/src/target/openrisc/jsp_server.c
+++ b/src/target/openrisc/jsp_server.c
@@ -88,7 +88,6 @@ static int jsp_new_connection(struct connection *connection)
 	telnet_connection->closed = 0;
 	telnet_connection->line_size = 0;
 	telnet_connection->line_cursor = 0;
-	telnet_connection->option_size = 0;
 	telnet_connection->state = TELNET_STATE_DATA;
 
 	/* negotiate telnet options */
diff --git a/src/target/register.h b/src/target/register.h
index d4c3281..dc18e9a 100644
--- a/src/target/register.h
+++ b/src/target/register.h
@@ -114,9 +114,9 @@ struct reg_data_type {
 };
 
 struct reg {
-	/** Canonical name of the register. */
+	/* Canonical name of the register. */
 	const char *name;
-	/** Number that gdb uses to access this register. */
+	/* Number that gdb uses to access this register. */
 	uint32_t number;
 	/* TODO. This should probably be const. */
 	struct reg_feature *feature;
diff --git a/src/target/stm8.c b/src/target/stm8.c
new file mode 100644
index 0000000..262497b
--- /dev/null
+++ b/src/target/stm8.c
@@ -0,0 +1,2219 @@
+/*
+    OpenOCD STM8 target driver
+    Copyright (C) 2017  Ake Rehnman
+    ake.rehnman(at)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 3 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 <helper/log.h>
+#include "target.h"
+#include "target_type.h"
+#include "hello.h"
+#include "jtag/jtag.h"
+#include "jtag/hla/hla_transport.h"
+#include "jtag/hla/hla_interface.h"
+#include "jtag/hla/hla_layout.h"
+#include "register.h"
+#include "breakpoints.h"
+#include "algorithm.h"
+#include "stm8.h"
+
+static struct reg_cache *stm8_build_reg_cache(struct target *target);
+static int stm8_read_core_reg(struct target *target, unsigned int num);
+static int stm8_write_core_reg(struct target *target, unsigned int num);
+static int stm8_save_context(struct target *target);
+static void stm8_enable_breakpoints(struct target *target);
+static int stm8_unset_breakpoint(struct target *target,
+		struct breakpoint *breakpoint);
+static int stm8_set_breakpoint(struct target *target,
+		struct breakpoint *breakpoint);
+static void stm8_enable_watchpoints(struct target *target);
+static int stm8_unset_watchpoint(struct target *target,
+		struct watchpoint *watchpoint);
+
+static const struct {
+	unsigned id;
+	const char *name;
+	const uint8_t bits;
+	enum reg_type type;
+	const char *group;
+	const char *feature;
+	int flag;
+} stm8_regs[] = {
+	{  0,  "pc", 32, REG_TYPE_UINT32, "general", "org.gnu.gdb.stm8.core", 0 },
+	{  1,  "a", 8, REG_TYPE_UINT8, "general", "org.gnu.gdb.stm8.core", 0 },
+	{  2,  "x", 16, REG_TYPE_UINT16, "general", "org.gnu.gdb.stm8.core", 0 },
+	{  3,  "y", 16, REG_TYPE_UINT16, "general", "org.gnu.gdb.stm8.core", 0 },
+	{  4,  "sp", 16, REG_TYPE_UINT16, "general", "org.gnu.gdb.stm8.core", 0 },
+	{  5,  "cc", 8, REG_TYPE_UINT8, "general", "org.gnu.gdb.stm8.core", 0 },
+};
+
+#define STM8_NUM_REGS ARRAY_SIZE(stm8_regs)
+#define STM8_PC 0
+#define STM8_A 1
+#define STM8_X 2
+#define STM8_Y 3
+#define STM8_SP 4
+#define STM8_CC 5
+
+#define CC_I0 0x8
+#define CC_I1 0x20
+
+#define DM_REGS 0x7f00
+#define DM_REG_A 0x7f00
+#define DM_REG_PC 0x7f01
+#define DM_REG_X 0x7f04
+#define DM_REG_Y 0x7f06
+#define DM_REG_SP 0x7f08
+#define DM_REG_CC 0x7f0a
+
+#define DM_BKR1E 0x7f90
+#define DM_BKR2E 0x7f93
+#define DM_CR1 0x7f96
+#define DM_CR2 0x7f97
+#define DM_CSR1 0x7f98
+#define DM_CSR2 0x7f99
+
+#define STE 0x40
+#define STF 0x20
+#define RST 0x10
+#define BRW 0x08
+#define BK2F 0x04
+#define BK1F 0x02
+
+#define SWBRK 0x20
+#define SWBKF 0x10
+#define STALL 0x08
+#define FLUSH 0x01
+
+#define FLASH_CR1_STM8S 0x505A
+#define FLASH_CR2_STM8S 0x505B
+#define FLASH_NCR2_STM8S 0x505C
+#define FLASH_IAPSR_STM8S 0x505F
+#define FLASH_PUKR_STM8S 0x5062
+#define FLASH_DUKR_STM8S 0x5064
+
+#define FLASH_CR1_STM8L 0x5050
+#define FLASH_CR2_STM8L 0x5051
+#define FLASH_NCR2_STM8L 0
+#define FLASH_PUKR_STM8L 0x5052
+#define FLASH_DUKR_STM8L 0x5053
+#define FLASH_IAPSR_STM8L 0x5054
+
+/* FLASH_IAPSR */
+#define HVOFF 0x40
+#define DUL 0x08
+#define EOP 0x04
+#define PUL 0x02
+#define WR_PG_DIS 0x01
+
+/* FLASH_CR2 */
+#define OPT 0x80
+#define WPRG 0x40
+#define ERASE 0x20
+#define FPRG 0x10
+#define PRG 0x01
+
+/* SWIM_CSR */
+#define SAFE_MASK 0x80
+#define NO_ACCESS 0x40
+#define SWIM_DM 0x20
+#define HS 0x10
+#define OSCOFF 0x08
+#define SWIM_RST 0x04
+#define HSIT 0x02
+#define PRI 0x01
+
+#define SWIM_CSR 0x7f80
+
+#define STM8_BREAK 0x8B
+
+enum mem_type {
+	RAM,
+	FLASH,
+	EEPROM,
+	OPTION
+};
+
+struct stm8_algorithm {
+	int common_magic;
+};
+
+struct stm8_core_reg {
+	uint32_t num;
+	struct target *target;
+	struct stm8_common *stm8_common;
+};
+
+enum hw_break_type {
+	/* break on execute */
+	HWBRK_EXEC,
+	/* break on read */
+	HWBRK_RD,
+	/* break on write */
+	HWBRK_WR,
+	/* break on read, write and execute */
+	HWBRK_ACC
+};
+
+struct stm8_comparator {
+	bool used;
+	uint32_t bp_value;
+	uint32_t reg_address;
+	enum hw_break_type type;
+};
+
+static inline struct hl_interface_s *target_to_adapter(struct target *target)
+{
+	return target->tap->priv;
+}
+
+static int stm8_adapter_read_memory(struct target *target,
+		uint32_t addr, int size, int count, void *buf)
+{
+	int ret;
+	struct hl_interface_s *adapter = target_to_adapter(target);
+
+	ret = adapter->layout->api->read_mem(adapter->handle,
+		addr, size, count, buf);
+	if (ret != ERROR_OK)
+		return ret;
+	return ERROR_OK;
+}
+
+static int stm8_adapter_write_memory(struct target *target,
+		uint32_t addr, int size, int count, const void *buf)
+{
+	int ret;
+	struct hl_interface_s *adapter = target_to_adapter(target);
+
+	ret = adapter->layout->api->write_mem(adapter->handle,
+		addr, size, count, buf);
+	if (ret != ERROR_OK)
+		return ret;
+	return ERROR_OK;
+}
+
+static int stm8_write_u8(struct target *target,
+		uint32_t addr, uint8_t val)
+{
+	int ret;
+	uint8_t buf[1];
+	struct hl_interface_s *adapter = target_to_adapter(target);
+
+	buf[0] = val;
+	ret =  adapter->layout->api->write_mem(adapter->handle, addr, 1, 1, buf);
+	if (ret != ERROR_OK)
+		return ret;
+	return ERROR_OK;
+}
+
+static int stm8_read_u8(struct target *target,
+		uint32_t addr, uint8_t *val)
+{
+	int ret;
+	struct hl_interface_s *adapter = target_to_adapter(target);
+
+	ret =  adapter->layout->api->read_mem(adapter->handle, addr, 1, 1, val);
+	if (ret != ERROR_OK)
+		return ret;
+	return ERROR_OK;
+}
+
+static int stm8_set_speed(struct target *target, int speed)
+{
+	struct hl_interface_s *adapter = target_to_adapter(target);
+	adapter->layout->api->speed(adapter->handle, speed, 0);
+	return ERROR_OK;
+}
+
+/*
+	<enable == 0> Disables interrupts.
+	If interrupts are enabled they are masked and the cc register
+	is saved.
+
+	<enable == 1> Enables interrupts.
+	Enable interrupts is actually restoring I1 I0 state from previous
+	call with enable == 0. Note that if stepping and breaking on a sim
+	instruction will NOT work since the interrupt flags are restored on
+	debug_entry. We don't have any way for the debugger to exclusively
+	disable the interrupts
+*/
+static int stm8_enable_interrupts(struct target *target, int enable)
+{
+	struct stm8_common *stm8 = target_to_stm8(target);
+	uint8_t cc;
+
+	if (enable) {
+		if (!stm8->cc_valid)
+			return ERROR_OK; /* cc was not stashed */
+		/* fetch current cc */
+		stm8_read_u8(target, DM_REG_CC, &cc);
+		/* clear I1 I0 */
+		cc &= ~(CC_I0 + CC_I1);
+		/* restore I1 & I0 from stash*/
+		cc |= (stm8->cc & (CC_I0+CC_I1));
+		/* update current cc */
+		stm8_write_u8(target, DM_REG_CC, cc);
+		stm8->cc_valid = false;
+	} else {
+		stm8_read_u8(target, DM_REG_CC, &cc);
+		if ((cc & CC_I0) && (cc & CC_I1))
+			return ERROR_OK; /* interrupts already masked */
+		/* stash cc */
+		stm8->cc = cc;
+		stm8->cc_valid = true;
+		/* mask interrupts (disable) */
+		cc |= (CC_I0 + CC_I1);
+		stm8_write_u8(target, DM_REG_CC, cc);
+	}
+
+	return ERROR_OK;
+}
+
+static int stm8_set_hwbreak(struct target *target,
+		struct stm8_comparator comparator_list[])
+{
+	uint8_t buf[3];
+	int i, ret;
+
+	/* Refer to Table 4 in UM0470 */
+	uint8_t bc = 0x5;
+	uint8_t bir = 0;
+	uint8_t biw = 0;
+
+	uint32_t data;
+	uint32_t addr;
+
+	if (!comparator_list[0].used) {
+		comparator_list[0].type = HWBRK_EXEC;
+		comparator_list[0].bp_value = -1;
+	}
+
+	if (!comparator_list[1].used) {
+		comparator_list[1].type = HWBRK_EXEC;
+		comparator_list[1].bp_value = -1;
+	}
+
+	if ((comparator_list[0].type == HWBRK_EXEC)
+			&& (comparator_list[1].type == HWBRK_EXEC)) {
+		comparator_list[0].reg_address = 0;
+		comparator_list[1].reg_address = 1;
+	}
+
+	if ((comparator_list[0].type == HWBRK_EXEC)
+			&& (comparator_list[1].type != HWBRK_EXEC)) {
+		comparator_list[0].reg_address = 0;
+		comparator_list[1].reg_address = 1;
+		switch (comparator_list[1].type) {
+		case HWBRK_RD:
+			bir = 1;
+			break;
+		case HWBRK_WR:
+			biw = 1;
+			break;
+		default:
+			bir = 1;
+			biw = 1;
+			break;
+		}
+	}
+
+	if ((comparator_list[1].type == HWBRK_EXEC)
+			&& (comparator_list[0].type != HWBRK_EXEC)) {
+		comparator_list[0].reg_address = 1;
+		comparator_list[1].reg_address = 0;
+		switch (comparator_list[0].type) {
+		case HWBRK_RD:
+			bir = 1;
+			break;
+		case HWBRK_WR:
+			biw = 1;
+			break;
+		default:
+			bir = 1;
+			biw = 1;
+			break;
+		}
+	}
+
+	if ((comparator_list[0].type != HWBRK_EXEC)
+			&& (comparator_list[1].type != HWBRK_EXEC)) {
+		if ((comparator_list[0].type != comparator_list[1].type)) {
+			LOG_ERROR("data hw breakpoints must be of same type");
+			return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+		}
+	}
+
+	for (i = 0; i < 2; i++) {
+		data = comparator_list[i].bp_value;
+		addr = comparator_list[i].reg_address;
+
+		buf[0] = data >> 16;
+		buf[1] = data >> 8;
+		buf[2] = data;
+
+		if (addr == 0) {
+			ret = stm8_adapter_write_memory(target, DM_BKR1E, 1, 3, buf);
+			LOG_DEBUG("DM_BKR1E=%" PRIx32, data);
+		} else if (addr == 1) {
+			ret = stm8_adapter_write_memory(target, DM_BKR2E, 1, 3, buf);
+			LOG_DEBUG("DM_BKR2E=%" PRIx32, data);
+		} else {
+			LOG_DEBUG("addr=%" PRIu32, addr);
+			return ERROR_FAIL;
+		}
+
+		if (ret != ERROR_OK)
+			return ret;
+
+		ret = stm8_write_u8(target, DM_CR1,
+			(bc << 3) + (bir << 2) + (biw << 1));
+		LOG_DEBUG("DM_CR1=%" PRIx8, buf[0]);
+		if (ret != ERROR_OK)
+			return ret;
+
+	}
+	return ERROR_OK;
+}
+
+/* read DM control and status regs */
+static int stm8_read_dm_csrx(struct target *target, uint8_t *csr1,
+		uint8_t *csr2)
+{
+	int ret;
+	uint8_t buf[2];
+
+	ret =  stm8_adapter_read_memory(target, DM_CSR1, 1, sizeof(buf), buf);
+	if (ret != ERROR_OK)
+		return ret;
+	if (csr1)
+		*csr1 = buf[0];
+	if (csr2)
+		*csr2 = buf[1];
+	return ERROR_OK;
+}
+
+/* set or clear the single step flag in DM */
+static int stm8_config_step(struct target *target, int enable)
+{
+	int ret;
+	uint8_t csr1, csr2;
+
+	ret = stm8_read_dm_csrx(target, &csr1, &csr2);
+	if (ret != ERROR_OK)
+		return ret;
+	if (enable)
+		csr1 |= STE;
+	else
+		csr1 &= ~STE;
+
+	ret =  stm8_write_u8(target, DM_CSR1, csr1);
+	if (ret != ERROR_OK)
+		return ret;
+	return ERROR_OK;
+}
+
+/* set the stall flag in DM */
+static int stm8_debug_stall(struct target *target)
+{
+	int ret;
+	uint8_t csr1, csr2;
+
+	ret = stm8_read_dm_csrx(target, &csr1, &csr2);
+	if (ret != ERROR_OK)
+		return ret;
+	csr2 |= STALL;
+	ret =  stm8_write_u8(target, DM_CSR2, csr2);
+	if (ret != ERROR_OK)
+		return ret;
+	return ERROR_OK;
+}
+
+static int stm8_configure_break_unit(struct target *target)
+{
+	/* get pointers to arch-specific information */
+	struct stm8_common *stm8 = target_to_stm8(target);
+
+	if (stm8->bp_scanned)
+		return ERROR_OK;
+
+	stm8->num_hw_bpoints = 2;
+	stm8->num_hw_bpoints_avail = stm8->num_hw_bpoints;
+
+	stm8->hw_break_list = calloc(stm8->num_hw_bpoints,
+		sizeof(struct stm8_comparator));
+
+	stm8->hw_break_list[0].reg_address = 0;
+	stm8->hw_break_list[1].reg_address = 1;
+
+	LOG_DEBUG("hw breakpoints: numinst %i numdata %i", stm8->num_hw_bpoints,
+		stm8->num_hw_bpoints);
+
+	stm8->bp_scanned = true;
+
+	return ERROR_OK;
+}
+
+static int stm8_examine_debug_reason(struct target *target)
+{
+	int retval;
+	uint8_t csr1, csr2;
+
+	retval = stm8_read_dm_csrx(target, &csr1, &csr2);
+	LOG_DEBUG("csr1 = 0x%02X csr2 = 0x%02X", csr1, csr2);
+
+	if ((target->debug_reason != DBG_REASON_DBGRQ)
+		&& (target->debug_reason != DBG_REASON_SINGLESTEP)) {
+
+		if (retval != ERROR_OK)
+			return retval;
+
+		if (csr1 & RST)
+			/* halted on reset */
+			target->debug_reason = DBG_REASON_UNDEFINED;
+
+		if (csr1 & (BK1F+BK2F))
+			/* we have halted on a  breakpoint (or wp)*/
+			target->debug_reason = DBG_REASON_BREAKPOINT;
+
+		if (csr2 & SWBKF)
+			/* we have halted on a  breakpoint */
+			target->debug_reason = DBG_REASON_BREAKPOINT;
+
+	}
+
+	return ERROR_OK;
+}
+
+static int stm8_debug_entry(struct target *target)
+{
+	struct stm8_common *stm8 = target_to_stm8(target);
+
+	/* restore interrupts */
+	stm8_enable_interrupts(target, 1);
+
+	stm8_save_context(target);
+
+	/* make sure stepping disabled STE bit in CSR1 cleared */
+	stm8_config_step(target, 0);
+
+	/* attempt to find halt reason */
+	stm8_examine_debug_reason(target);
+
+	LOG_DEBUG("entered debug state at PC 0x%" PRIx32 ", target->state: %s",
+		buf_get_u32(stm8->core_cache->reg_list[STM8_PC].value, 0, 32),
+		target_state_name(target));
+
+	return ERROR_OK;
+}
+
+/* clear stall flag in DM and flush instruction pipe */
+static int stm8_exit_debug(struct target *target)
+{
+	int ret;
+	uint8_t csr1, csr2;
+
+	ret = stm8_read_dm_csrx(target, &csr1, &csr2);
+	if (ret != ERROR_OK)
+		return ret;
+	csr2 |= FLUSH;
+	ret =  stm8_write_u8(target, DM_CSR2, csr2);
+	if (ret != ERROR_OK)
+		return ret;
+
+	csr2 &= ~STALL;
+	csr2 |= SWBRK;
+	ret =  stm8_write_u8(target, DM_CSR2, csr2);
+	if (ret != ERROR_OK)
+		return ret;
+	return ERROR_OK;
+}
+
+static int stm8_read_regs(struct target *target, uint32_t regs[])
+{
+	int ret;
+	uint8_t buf[11];
+
+	ret =  stm8_adapter_read_memory(target, DM_REGS, 1, sizeof(buf), buf);
+	if (ret != ERROR_OK)
+		return ret;
+
+	regs[0] = be_to_h_u24(buf+DM_REG_PC-DM_REGS);
+	regs[1] = buf[DM_REG_A-DM_REGS];
+	regs[2] = be_to_h_u16(buf+DM_REG_X-DM_REGS);
+	regs[3] = be_to_h_u16(buf+DM_REG_Y-DM_REGS);
+	regs[4] = be_to_h_u16(buf+DM_REG_SP-DM_REGS);
+	regs[5] = buf[DM_REG_CC-DM_REGS];
+
+	return ERROR_OK;
+}
+
+static int stm8_write_regs(struct target *target, uint32_t regs[])
+{
+	int ret;
+	uint8_t buf[11];
+
+	h_u24_to_be(buf+DM_REG_PC-DM_REGS, regs[0]);
+	buf[DM_REG_A-DM_REGS] = regs[1];
+	h_u16_to_be(buf+DM_REG_X-DM_REGS, regs[2]);
+	h_u16_to_be(buf+DM_REG_Y-DM_REGS, regs[3]);
+	h_u16_to_be(buf+DM_REG_SP-DM_REGS, regs[4]);
+	buf[DM_REG_CC-DM_REGS] = regs[5];
+
+	ret =  stm8_adapter_write_memory(target, DM_REGS, 1, sizeof(buf), buf);
+	if (ret != ERROR_OK)
+		return ret;
+
+	return ERROR_OK;
+}
+
+static int stm8_get_core_reg(struct reg *reg)
+{
+	int retval;
+	struct stm8_core_reg *stm8_reg = reg->arch_info;
+	struct target *target = stm8_reg->target;
+	struct stm8_common *stm8_target = target_to_stm8(target);
+
+	if (target->state != TARGET_HALTED)
+		return ERROR_TARGET_NOT_HALTED;
+
+	retval = stm8_target->read_core_reg(target, stm8_reg->num);
+
+	return retval;
+}
+
+static int stm8_set_core_reg(struct reg *reg, uint8_t *buf)
+{
+	struct stm8_core_reg *stm8_reg = reg->arch_info;
+	struct target *target = stm8_reg->target;
+	uint32_t value = buf_get_u32(buf, 0, reg->size);
+
+	if (target->state != TARGET_HALTED)
+		return ERROR_TARGET_NOT_HALTED;
+
+	buf_set_u32(reg->value, 0, 32, value);
+	reg->dirty = true;
+	reg->valid = true;
+
+	return ERROR_OK;
+}
+
+static int stm8_save_context(struct target *target)
+{
+	unsigned int i;
+
+	/* get pointers to arch-specific information */
+	struct stm8_common *stm8 = target_to_stm8(target);
+
+	/* read core registers */
+	stm8_read_regs(target, stm8->core_regs);
+
+	for (i = 0; i < STM8_NUM_REGS; i++) {
+		if (!stm8->core_cache->reg_list[i].valid)
+			stm8->read_core_reg(target, i);
+	}
+
+	return ERROR_OK;
+}
+
+static int stm8_restore_context(struct target *target)
+{
+	unsigned int i;
+
+	/* get pointers to arch-specific information */
+	struct stm8_common *stm8 = target_to_stm8(target);
+
+	for (i = 0; i < STM8_NUM_REGS; i++) {
+		if (stm8->core_cache->reg_list[i].dirty)
+			stm8->write_core_reg(target, i);
+	}
+
+	/* write core regs */
+	stm8_write_regs(target, stm8->core_regs);
+
+	return ERROR_OK;
+}
+
+static int stm8_unlock_flash(struct target *target)
+{
+	uint8_t data[1];
+
+	struct stm8_common *stm8 = target_to_stm8(target);
+
+	/* check if flash is unlocked */
+	stm8_read_u8(target, stm8->flash_iapsr, data);
+	if (~data[0] & PUL) {
+		/* unlock flash */
+		stm8_write_u8(target, stm8->flash_pukr, 0x56);
+		stm8_write_u8(target, stm8->flash_pukr, 0xae);
+	}
+
+	stm8_read_u8(target, stm8->flash_iapsr, data);
+	if (~data[0] & PUL)
+		return ERROR_FAIL;
+	return ERROR_OK;
+}
+
+static int stm8_unlock_eeprom(struct target *target)
+{
+	uint8_t data[1];
+
+	struct stm8_common *stm8 = target_to_stm8(target);
+
+	/* check if eeprom is unlocked */
+	stm8_read_u8(target, stm8->flash_iapsr, data);
+	if (~data[0] & DUL) {
+		/* unlock eeprom */
+		stm8_write_u8(target, stm8->flash_dukr, 0xae);
+		stm8_write_u8(target, stm8->flash_dukr, 0x56);
+	}
+
+	stm8_read_u8(target, stm8->flash_iapsr, data);
+	if (~data[0] & DUL)
+		return ERROR_FAIL;
+	return ERROR_OK;
+}
+
+static int stm8_write_flash(struct target *target, enum mem_type type,
+		uint32_t address,
+		uint32_t size, uint32_t count, uint32_t blocksize_param,
+		const uint8_t *buffer)
+{
+	struct stm8_common *stm8 = target_to_stm8(target);
+
+	uint8_t iapsr;
+	uint8_t opt = 0;
+	unsigned int i;
+	uint32_t blocksize = 0;
+	uint32_t bytecnt;
+	int res;
+
+	switch (type) {
+		case (FLASH):
+			stm8_unlock_flash(target);
+			break;
+		case (EEPROM):
+			stm8_unlock_eeprom(target);
+			break;
+		case (OPTION):
+			stm8_unlock_eeprom(target);
+			opt = OPT;
+			break;
+		default:
+			LOG_ERROR("BUG: wrong mem_type %d", type);
+			assert(0);
+	}
+
+	if (size == 2) {
+		/* we don't support short writes */
+		count = count * 2;
+		size = 1;
+	}
+
+	bytecnt = count * size;
+
+	while (bytecnt) {
+		if ((bytecnt >= blocksize_param) && ((address & (blocksize_param-1)) == 0)) {
+			if (stm8->flash_cr2)
+				stm8_write_u8(target, stm8->flash_cr2, PRG + opt);
+			if (stm8->flash_ncr2)
+				stm8_write_u8(target, stm8->flash_ncr2, ~(PRG + opt));
+			blocksize = blocksize_param;
+		} else
+		if ((bytecnt >= 4) && ((address & 0x3) == 0)) {
+			if (stm8->flash_cr2)
+				stm8_write_u8(target, stm8->flash_cr2, WPRG + opt);
+			if (stm8->flash_ncr2)
+				stm8_write_u8(target, stm8->flash_ncr2, ~(WPRG + opt));
+			blocksize = 4;
+		} else
+		if (blocksize != 1) {
+			if (stm8->flash_cr2)
+				stm8_write_u8(target, stm8->flash_cr2, opt);
+			if (stm8->flash_ncr2)
+				stm8_write_u8(target, stm8->flash_ncr2, ~opt);
+			blocksize = 1;
+		}
+
+		res = stm8_adapter_write_memory(target, address, 1, blocksize, buffer);
+		if (res != ERROR_OK)
+			return res;
+		address += blocksize;
+		buffer += blocksize;
+		bytecnt -= blocksize;
+
+		/* lets hang here until end of program (EOP) */
+		for (i = 0; i < 16; i++) {
+			stm8_read_u8(target, stm8->flash_iapsr, &iapsr);
+			if (iapsr & EOP)
+				break;
+			else
+				usleep(1000);
+		}
+		if (i == 16)
+			return ERROR_FAIL;
+	}
+
+	/* disable write access */
+	res = stm8_write_u8(target, stm8->flash_iapsr, 0x0);
+
+	if (res != ERROR_OK)
+		return ERROR_FAIL;
+
+	return ERROR_OK;
+}
+
+static int stm8_write_memory(struct target *target, target_addr_t address,
+		uint32_t size, uint32_t count,
+		const uint8_t *buffer)
+{
+	struct stm8_common *stm8 = target_to_stm8(target);
+
+	LOG_DEBUG("address: 0x%8.8" TARGET_PRIxADDR
+		", size: 0x%8.8" PRIx32
+		", count: 0x%8.8" PRIx32,
+		address, size, count);
+
+	if (target->state != TARGET_HALTED)
+		LOG_WARNING("target not halted");
+
+	int retval;
+
+	if ((address >= stm8->flashstart) && (address <= stm8->flashend))
+		retval = stm8_write_flash(target, FLASH, address, size, count,
+				stm8->blocksize, buffer);
+	else if ((address >= stm8->eepromstart) && (address <= stm8->eepromend))
+		retval = stm8_write_flash(target, EEPROM, address, size, count,
+				stm8->blocksize, buffer);
+	else if ((address >= stm8->optionstart) && (address <= stm8->optionend))
+		retval = stm8_write_flash(target, OPTION, address, size, count, 0, buffer);
+	else
+		retval = stm8_adapter_write_memory(target, address, size, count,
+				buffer);
+
+	if (retval != ERROR_OK)
+		return ERROR_TARGET_FAILURE;
+
+	return retval;
+}
+
+static int stm8_read_memory(struct target *target, target_addr_t address,
+		uint32_t size, uint32_t count, uint8_t *buffer)
+{
+	LOG_DEBUG("address: 0x%8.8" TARGET_PRIxADDR
+		", size: 0x%8.8" PRIx32
+		", count: 0x%8.8" PRIx32,
+		address, size, count);
+
+	if (target->state != TARGET_HALTED)
+		LOG_WARNING("target not halted");
+
+	int retval;
+	retval = stm8_adapter_read_memory(target, address, size, count, buffer);
+
+	if (retval != ERROR_OK)
+		return ERROR_TARGET_FAILURE;
+
+	return retval;
+}
+
+static int stm8_init(struct command_context *cmd_ctx, struct target *target)
+{
+	stm8_build_reg_cache(target);
+
+	return ERROR_OK;
+}
+
+static int stm8_poll(struct target *target)
+{
+	int retval = ERROR_OK;
+	uint8_t csr1, csr2;
+
+#ifdef LOG_STM8
+	LOG_DEBUG("target->state=%d", target->state);
+#endif
+
+	/* read dm_csrx control regs */
+	retval = stm8_read_dm_csrx(target, &csr1, &csr2);
+	if (retval != ERROR_OK) {
+		LOG_DEBUG("stm8_read_dm_csrx failed retval=%d", retval);
+		/*
+		   We return ERROR_OK here even if we didn't get an answer.
+		   openocd will call target_wait_state until we get target state TARGET_HALTED
+		*/
+		return ERROR_OK;
+	}
+
+	/* check for processor halted */
+	if (csr2 & STALL) {
+		if (target->state != TARGET_HALTED) {
+			if (target->state == TARGET_UNKNOWN)
+				LOG_DEBUG("DM_CSR2_STALL already set during server startup.");
+
+			retval = stm8_debug_entry(target);
+			if (retval != ERROR_OK) {
+				LOG_DEBUG("stm8_debug_entry failed retval=%d", retval);
+				return ERROR_TARGET_FAILURE;
+			}
+
+			if (target->state == TARGET_DEBUG_RUNNING) {
+				target->state = TARGET_HALTED;
+				target_call_event_callbacks(target, TARGET_EVENT_DEBUG_HALTED);
+			} else {
+				target->state = TARGET_HALTED;
+				target_call_event_callbacks(target, TARGET_EVENT_HALTED);
+			}
+		}
+	} else
+		target->state = TARGET_RUNNING;
+#ifdef LOG_STM8
+	LOG_DEBUG("csr1 = 0x%02X csr2 = 0x%02X", csr1, csr2);
+#endif
+	return ERROR_OK;
+}
+
+static int stm8_halt(struct target *target)
+{
+	LOG_DEBUG("target->state: %s", target_state_name(target));
+
+	if (target->state == TARGET_HALTED) {
+		LOG_DEBUG("target was already halted");
+		return ERROR_OK;
+	}
+
+	if (target->state == TARGET_UNKNOWN)
+		LOG_WARNING("target was in unknown state when halt was requested");
+
+	if (target->state == TARGET_RESET) {
+		/* we came here in a reset_halt or reset_init sequence
+		 * debug entry was already prepared in stm8_assert_reset()
+		 */
+		target->debug_reason = DBG_REASON_DBGRQ;
+
+		return ERROR_OK;
+	}
+
+
+	/* break processor */
+	stm8_debug_stall(target);
+
+	target->debug_reason = DBG_REASON_DBGRQ;
+
+	return ERROR_OK;
+}
+
+static int stm8_reset_assert(struct target *target)
+{
+	int res = ERROR_OK;
+	struct hl_interface_s *adapter = target_to_adapter(target);
+	struct stm8_common *stm8 = target_to_stm8(target);
+	bool use_srst_fallback = true;
+
+	enum reset_types jtag_reset_config = jtag_get_reset_config();
+
+	if (jtag_reset_config & RESET_HAS_SRST) {
+		jtag_add_reset(0, 1);
+		res = adapter->layout->api->assert_srst(adapter->handle, 0);
+
+		if (res == ERROR_OK)
+			/* hardware srst supported */
+			use_srst_fallback = false;
+		else if (res != ERROR_COMMAND_NOTFOUND)
+			/* some other failure */
+			return res;
+	}
+
+	if (use_srst_fallback) {
+		LOG_DEBUG("Hardware srst not supported, falling back to swim reset");
+		res = adapter->layout->api->reset(adapter->handle);
+		if (res != ERROR_OK)
+			return res;
+	}
+
+	/* registers are now invalid */
+	register_cache_invalidate(stm8->core_cache);
+
+	target->state = TARGET_RESET;
+	target->debug_reason = DBG_REASON_NOTHALTED;
+
+	if (target->reset_halt) {
+		res = target_halt(target);
+		if (res != ERROR_OK)
+			return res;
+	}
+
+	return ERROR_OK;
+}
+
+static int stm8_reset_deassert(struct target *target)
+{
+	int res;
+	struct hl_interface_s *adapter = target_to_adapter(target);
+
+	enum reset_types jtag_reset_config = jtag_get_reset_config();
+
+	if (jtag_reset_config & RESET_HAS_SRST) {
+		res = adapter->layout->api->assert_srst(adapter->handle, 1);
+		if ((res != ERROR_OK) && (res != ERROR_COMMAND_NOTFOUND))
+			return res;
+	}
+
+	/* virtual deassert reset, we need it for the internal
+	 * jtag state machine
+	 */
+	jtag_add_reset(0, 0);
+
+	/* The cpu should now be stalled. If halt was requested
+	   let poll detect the stall */
+	if (target->reset_halt)
+		return ERROR_OK;
+
+	/* Instead of going thrugh saving context, polling and
+	   then resuming target again just clear stall and proceed. */
+	target->state = TARGET_RUNNING;
+	return stm8_exit_debug(target);
+}
+
+/* stm8_single_step_core() is only used for stepping over breakpoints
+   from stm8_resume() */
+static int stm8_single_step_core(struct target *target)
+{
+	struct stm8_common *stm8 = target_to_stm8(target);
+
+	/* configure single step mode */
+	stm8_config_step(target, 1);
+
+	/* disable interrupts while stepping */
+	if (!stm8->enable_step_irq)
+		stm8_enable_interrupts(target, 0);
+
+	/* exit debug mode */
+	stm8_exit_debug(target);
+
+	stm8_debug_entry(target);
+
+	return ERROR_OK;
+}
+
+static int stm8_resume(struct target *target, int current,
+		target_addr_t address, int handle_breakpoints,
+		int debug_execution)
+{
+	struct stm8_common *stm8 = target_to_stm8(target);
+	struct breakpoint *breakpoint = NULL;
+	uint32_t resume_pc;
+
+	LOG_DEBUG("%d " TARGET_ADDR_FMT " %d %d", current, address,
+			handle_breakpoints, debug_execution);
+
+	if (target->state != TARGET_HALTED) {
+		LOG_WARNING("target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if (!debug_execution) {
+		target_free_all_working_areas(target);
+		stm8_enable_breakpoints(target);
+		stm8_enable_watchpoints(target);
+		struct stm8_comparator *comparator_list = stm8->hw_break_list;
+		stm8_set_hwbreak(target, comparator_list);
+	}
+
+	/* current = 1: continue on current pc,
+	   otherwise continue at <address> */
+	if (!current) {
+		buf_set_u32(stm8->core_cache->reg_list[STM8_PC].value,
+			0, 32, address);
+		stm8->core_cache->reg_list[STM8_PC].dirty = true;
+		stm8->core_cache->reg_list[STM8_PC].valid = true;
+	}
+
+	if (!current)
+		resume_pc = address;
+	else
+		resume_pc = buf_get_u32(
+			stm8->core_cache->reg_list[STM8_PC].value,
+			0, 32);
+
+	stm8_restore_context(target);
+
+	/* the front-end may request us not to handle breakpoints */
+	if (handle_breakpoints) {
+		/* Single step past breakpoint at current address */
+		breakpoint = breakpoint_find(target, resume_pc);
+		if (breakpoint) {
+			LOG_DEBUG("unset breakpoint at " TARGET_ADDR_FMT,
+					breakpoint->address);
+			stm8_unset_breakpoint(target, breakpoint);
+			stm8_single_step_core(target);
+			stm8_set_breakpoint(target, breakpoint);
+		}
+	}
+
+	/* disable interrupts if we are debugging */
+	if (debug_execution)
+		stm8_enable_interrupts(target, 0);
+
+	/* exit debug mode */
+	stm8_exit_debug(target);
+	target->debug_reason = DBG_REASON_NOTHALTED;
+
+	/* registers are now invalid */
+	register_cache_invalidate(stm8->core_cache);
+
+	if (!debug_execution) {
+		target->state = TARGET_RUNNING;
+		target_call_event_callbacks(target, TARGET_EVENT_RESUMED);
+		LOG_DEBUG("target resumed at 0x%" PRIx32 "", resume_pc);
+	} else {
+		target->state = TARGET_DEBUG_RUNNING;
+		target_call_event_callbacks(target, TARGET_EVENT_DEBUG_RESUMED);
+		LOG_DEBUG("target debug resumed at 0x%" PRIx32 "", resume_pc);
+	}
+
+	return ERROR_OK;
+}
+
+static int stm8_init_flash_regs(bool enable_stm8l, struct stm8_common *stm8)
+{
+	stm8->enable_stm8l = enable_stm8l;
+
+	if (stm8->enable_stm8l) {
+		stm8->flash_cr2 = FLASH_CR2_STM8L;
+		stm8->flash_ncr2 = FLASH_NCR2_STM8L;
+		stm8->flash_iapsr = FLASH_IAPSR_STM8L;
+		stm8->flash_dukr = FLASH_DUKR_STM8L;
+		stm8->flash_pukr = FLASH_PUKR_STM8L;
+	} else {
+		stm8->flash_cr2 = FLASH_CR2_STM8S;
+		stm8->flash_ncr2 = FLASH_NCR2_STM8S;
+		stm8->flash_iapsr = FLASH_IAPSR_STM8S;
+		stm8->flash_dukr = FLASH_DUKR_STM8S;
+		stm8->flash_pukr = FLASH_PUKR_STM8S;
+	}
+	return ERROR_OK;
+}
+
+static int stm8_init_arch_info(struct target *target,
+		struct stm8_common *stm8, struct jtag_tap *tap)
+{
+	target->endianness = TARGET_BIG_ENDIAN;
+	target->arch_info = stm8;
+	stm8->common_magic = STM8_COMMON_MAGIC;
+	stm8->fast_data_area = NULL;
+	stm8->blocksize = 0x80;
+	stm8->flashstart = 0x8000;
+	stm8->flashend = 0xffff;
+	stm8->eepromstart = 0x4000;
+	stm8->eepromend = 0x43ff;
+	stm8->optionstart = 0x4800;
+	stm8->optionend = 0x487F;
+
+	/* has breakpoint/watchpoint unit been scanned */
+	stm8->bp_scanned = false;
+	stm8->hw_break_list = NULL;
+
+	stm8->read_core_reg = stm8_read_core_reg;
+	stm8->write_core_reg = stm8_write_core_reg;
+
+	stm8_init_flash_regs(0, stm8);
+
+	return ERROR_OK;
+}
+
+static int stm8_target_create(struct target *target,
+		Jim_Interp *interp)
+{
+
+	struct stm8_common *stm8 = calloc(1, sizeof(struct stm8_common));
+
+	stm8_init_arch_info(target, stm8, target->tap);
+	stm8_configure_break_unit(target);
+
+	return ERROR_OK;
+}
+
+static int stm8_read_core_reg(struct target *target, unsigned int num)
+{
+	uint32_t reg_value;
+
+	/* get pointers to arch-specific information */
+	struct stm8_common *stm8 = target_to_stm8(target);
+
+	if (num >= STM8_NUM_REGS)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	reg_value = stm8->core_regs[num];
+	LOG_DEBUG("read core reg %i value 0x%" PRIx32 "", num , reg_value);
+	buf_set_u32(stm8->core_cache->reg_list[num].value, 0, 32, reg_value);
+	stm8->core_cache->reg_list[num].valid = true;
+	stm8->core_cache->reg_list[num].dirty = false;
+
+	return ERROR_OK;
+}
+
+static int stm8_write_core_reg(struct target *target, unsigned int num)
+{
+	uint32_t reg_value;
+
+	/* get pointers to arch-specific information */
+	struct stm8_common *stm8 = target_to_stm8(target);
+
+	if (num >= STM8_NUM_REGS)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	reg_value = buf_get_u32(stm8->core_cache->reg_list[num].value, 0, 32);
+	stm8->core_regs[num] = reg_value;
+	LOG_DEBUG("write core reg %i value 0x%" PRIx32 "", num , reg_value);
+	stm8->core_cache->reg_list[num].valid = true;
+	stm8->core_cache->reg_list[num].dirty = false;
+
+	return ERROR_OK;
+}
+
+static int stm8_get_gdb_reg_list(struct target *target, struct reg **reg_list[],
+		int *reg_list_size, enum target_register_class reg_class)
+{
+	/* get pointers to arch-specific information */
+	struct stm8_common *stm8 = target_to_stm8(target);
+	unsigned int i;
+
+	*reg_list_size = STM8_NUM_REGS;
+	*reg_list = malloc(sizeof(struct reg *) * (*reg_list_size));
+
+	for (i = 0; i < STM8_NUM_REGS; i++)
+		(*reg_list)[i] = &stm8->core_cache->reg_list[i];
+
+	return ERROR_OK;
+}
+
+static const struct reg_arch_type stm8_reg_type = {
+	.get = stm8_get_core_reg,
+	.set = stm8_set_core_reg,
+};
+
+static struct reg_cache *stm8_build_reg_cache(struct target *target)
+{
+	/* get pointers to arch-specific information */
+	struct stm8_common *stm8 = target_to_stm8(target);
+
+	int num_regs = STM8_NUM_REGS;
+	struct reg_cache **cache_p = register_get_last_cache_p(&target->reg_cache);
+	struct reg_cache *cache = malloc(sizeof(struct reg_cache));
+	struct reg *reg_list = calloc(num_regs, sizeof(struct reg));
+	struct stm8_core_reg *arch_info = malloc(
+			sizeof(struct stm8_core_reg) * num_regs);
+	struct reg_feature *feature;
+	int i;
+
+	/* Build the process context cache */
+	cache->name = "stm8 registers";
+	cache->next = NULL;
+	cache->reg_list = reg_list;
+	cache->num_regs = num_regs;
+	(*cache_p) = cache;
+	stm8->core_cache = cache;
+
+	for (i = 0; i < num_regs; i++) {
+		arch_info[i].num = stm8_regs[i].id;
+		arch_info[i].target = target;
+		arch_info[i].stm8_common = stm8;
+
+		reg_list[i].name = stm8_regs[i].name;
+		reg_list[i].size = stm8_regs[i].bits;
+
+		reg_list[i].value = calloc(1, 4);
+		reg_list[i].valid = false;
+		reg_list[i].type = &stm8_reg_type;
+		reg_list[i].arch_info = &arch_info[i];
+
+		reg_list[i].reg_data_type = calloc(1, sizeof(struct reg_data_type));
+		if (reg_list[i].reg_data_type)
+			reg_list[i].reg_data_type->type = stm8_regs[i].type;
+		else {
+			LOG_ERROR("unable to allocate reg type list");
+			return NULL;
+		}
+
+		reg_list[i].dirty = false;
+		reg_list[i].group = stm8_regs[i].group;
+		reg_list[i].number = stm8_regs[i].id;
+		reg_list[i].exist = true;
+		reg_list[i].caller_save = true;	/* gdb defaults to true */
+
+		feature = calloc(1, sizeof(struct reg_feature));
+		if (feature) {
+			feature->name = stm8_regs[i].feature;
+			reg_list[i].feature = feature;
+		} else
+			LOG_ERROR("unable to allocate feature list");
+	}
+
+	return cache;
+}
+
+static void stm8_free_reg_cache(struct target *target)
+{
+	struct stm8_common *stm8 = target_to_stm8(target);
+	struct reg_cache *cache;
+	struct reg *reg;
+	unsigned int i;
+
+	cache = stm8->core_cache;
+
+	if (!cache)
+		return;
+
+	for (i = 0; i < cache->num_regs; i++) {
+		reg = &cache->reg_list[i];
+
+		free(reg->feature);
+		free(reg->reg_data_type);
+		free(reg->value);
+	}
+
+	free(cache->reg_list[0].arch_info);
+	free(cache->reg_list);
+	free(cache);
+
+	stm8->core_cache = NULL;
+}
+
+static void stm8_deinit(struct target *target)
+{
+	struct stm8_common *stm8 = target_to_stm8(target);
+
+	free(stm8->hw_break_list);
+
+	stm8_free_reg_cache(target);
+
+	free(stm8);
+}
+
+static int stm8_arch_state(struct target *target)
+{
+	struct stm8_common *stm8 = target_to_stm8(target);
+
+	LOG_USER("target halted due to %s, pc: 0x%8.8" PRIx32 "",
+		debug_reason_name(target),
+		buf_get_u32(stm8->core_cache->reg_list[STM8_PC].value, 0, 32));
+
+	return ERROR_OK;
+}
+
+static int stm8_step(struct target *target, int current,
+		target_addr_t address, int handle_breakpoints)
+{
+	LOG_DEBUG("%" PRIx32 " " TARGET_ADDR_FMT " %" PRIx32,
+		current, address, handle_breakpoints);
+
+	/* get pointers to arch-specific information */
+	struct stm8_common *stm8 = target_to_stm8(target);
+	struct breakpoint *breakpoint = NULL;
+
+	if (target->state != TARGET_HALTED) {
+		LOG_WARNING("target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	/* current = 1: continue on current pc, otherwise continue at <address> */
+	if (!current) {
+		buf_set_u32(stm8->core_cache->reg_list[STM8_PC].value, 0, 32, address);
+		stm8->core_cache->reg_list[STM8_PC].dirty = true;
+		stm8->core_cache->reg_list[STM8_PC].valid = true;
+	}
+
+	/* the front-end may request us not to handle breakpoints */
+	if (handle_breakpoints) {
+		breakpoint = breakpoint_find(target,
+				buf_get_u32(stm8->core_cache->reg_list[STM8_PC].value, 0, 32));
+		if (breakpoint)
+			stm8_unset_breakpoint(target, breakpoint);
+	}
+
+	/* restore context */
+	stm8_restore_context(target);
+
+	/* configure single step mode */
+	stm8_config_step(target, 1);
+
+	target->debug_reason = DBG_REASON_SINGLESTEP;
+
+	target_call_event_callbacks(target, TARGET_EVENT_RESUMED);
+
+	/* disable interrupts while stepping */
+	if (!stm8->enable_step_irq)
+		stm8_enable_interrupts(target, 0);
+
+	/* exit debug mode */
+	stm8_exit_debug(target);
+
+	/* registers are now invalid */
+	register_cache_invalidate(stm8->core_cache);
+
+	LOG_DEBUG("target stepped ");
+	stm8_debug_entry(target);
+
+	if (breakpoint)
+		stm8_set_breakpoint(target, breakpoint);
+
+	target_call_event_callbacks(target, TARGET_EVENT_HALTED);
+
+	return ERROR_OK;
+}
+
+static void stm8_enable_breakpoints(struct target *target)
+{
+	struct breakpoint *breakpoint = target->breakpoints;
+
+	/* set any pending breakpoints */
+	while (breakpoint) {
+		if (breakpoint->set == 0)
+			stm8_set_breakpoint(target, breakpoint);
+		breakpoint = breakpoint->next;
+	}
+}
+
+static int stm8_set_breakpoint(struct target *target,
+		struct breakpoint *breakpoint)
+{
+	struct stm8_common *stm8 = target_to_stm8(target);
+	struct stm8_comparator *comparator_list = stm8->hw_break_list;
+	int retval;
+
+	if (breakpoint->set) {
+		LOG_WARNING("breakpoint already set");
+		return ERROR_OK;
+	}
+
+	if (breakpoint->type == BKPT_HARD) {
+		int bp_num = 0;
+
+		while (comparator_list[bp_num].used && (bp_num < stm8->num_hw_bpoints))
+			bp_num++;
+		if (bp_num >= stm8->num_hw_bpoints) {
+			LOG_ERROR("Can not find free breakpoint register (bpid: %" PRIu32 ")",
+					breakpoint->unique_id);
+			return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+		}
+		breakpoint->set = bp_num + 1;
+		comparator_list[bp_num].used = true;
+		comparator_list[bp_num].bp_value = breakpoint->address;
+		comparator_list[bp_num].type = HWBRK_EXEC;
+
+		retval = stm8_set_hwbreak(target, comparator_list);
+		if (retval != ERROR_OK)
+			return retval;
+
+		LOG_DEBUG("bpid: %" PRIu32 ", bp_num %i bp_value 0x%" PRIx32 "",
+				  breakpoint->unique_id,
+				  bp_num, comparator_list[bp_num].bp_value);
+	} else if (breakpoint->type == BKPT_SOFT) {
+		LOG_DEBUG("bpid: %" PRIu32, breakpoint->unique_id);
+		if (breakpoint->length == 1) {
+			uint8_t verify = 0x55;
+
+			retval = target_read_u8(target, breakpoint->address,
+					breakpoint->orig_instr);
+			if (retval != ERROR_OK)
+				return retval;
+			retval = target_write_u8(target, breakpoint->address, STM8_BREAK);
+			if (retval != ERROR_OK)
+				return retval;
+
+			retval = target_read_u8(target, breakpoint->address, &verify);
+			if (retval != ERROR_OK)
+				return retval;
+			if (verify != STM8_BREAK) {
+				LOG_ERROR("Unable to set breakpoint at address " TARGET_ADDR_FMT
+						" - check that memory is read/writable",
+						breakpoint->address);
+				return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+			}
+		} else {
+			return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+		}
+		breakpoint->set = 1; /* Any nice value but 0 */
+	}
+
+	return ERROR_OK;
+}
+
+static int stm8_add_breakpoint(struct target *target,
+		struct breakpoint *breakpoint)
+{
+	struct stm8_common *stm8 = target_to_stm8(target);
+	int ret;
+
+	if (breakpoint->type == BKPT_HARD) {
+		if (stm8->num_hw_bpoints_avail < 1) {
+			LOG_INFO("no hardware breakpoint available");
+			return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+		}
+
+		ret = stm8_set_breakpoint(target, breakpoint);
+		if (ret != ERROR_OK)
+			return ret;
+
+		stm8->num_hw_bpoints_avail--;
+		return ERROR_OK;
+	}
+
+	ret = stm8_set_breakpoint(target, breakpoint);
+	if (ret != ERROR_OK)
+		return ret;
+
+	return ERROR_OK;
+}
+
+static int stm8_unset_breakpoint(struct target *target,
+		struct breakpoint *breakpoint)
+{
+	/* get pointers to arch-specific information */
+	struct stm8_common *stm8 = target_to_stm8(target);
+	struct stm8_comparator *comparator_list = stm8->hw_break_list;
+	int retval;
+
+	if (!breakpoint->set) {
+		LOG_WARNING("breakpoint not set");
+		return ERROR_OK;
+	}
+
+	if (breakpoint->type == BKPT_HARD) {
+		int bp_num = breakpoint->set - 1;
+		if ((bp_num < 0) || (bp_num >= stm8->num_hw_bpoints)) {
+			LOG_DEBUG("Invalid comparator number in breakpoint (bpid: %" PRIu32 ")",
+					  breakpoint->unique_id);
+			return ERROR_OK;
+		}
+		LOG_DEBUG("bpid: %" PRIu32 " - releasing hw: %d",
+				breakpoint->unique_id,
+				bp_num);
+		comparator_list[bp_num].used = false;
+		retval = stm8_set_hwbreak(target, comparator_list);
+		if (retval != ERROR_OK)
+			return retval;
+	} else {
+		/* restore original instruction (kept in target endianness) */
+		LOG_DEBUG("bpid: %" PRIu32, breakpoint->unique_id);
+		if (breakpoint->length == 1) {
+			uint8_t current_instr;
+
+			/* check that user program has not
+			  modified breakpoint instruction */
+			retval = target_read_memory(target, breakpoint->address, 1, 1,
+					(uint8_t *)&current_instr);
+			if (retval != ERROR_OK)
+				return retval;
+
+			if (current_instr == STM8_BREAK) {
+				retval = target_write_memory(target, breakpoint->address, 1, 1,
+						breakpoint->orig_instr);
+				if (retval != ERROR_OK)
+					return retval;
+			}
+		} else
+			return ERROR_FAIL;
+	}
+	breakpoint->set = 0;
+
+	return ERROR_OK;
+}
+
+static int stm8_remove_breakpoint(struct target *target,
+		struct breakpoint *breakpoint)
+{
+	/* get pointers to arch-specific information */
+	struct stm8_common *stm8 = target_to_stm8(target);
+
+	if (target->state != TARGET_HALTED) {
+		LOG_WARNING("target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if (breakpoint->set)
+		stm8_unset_breakpoint(target, breakpoint);
+
+	if (breakpoint->type == BKPT_HARD)
+		stm8->num_hw_bpoints_avail++;
+
+	return ERROR_OK;
+}
+
+static int stm8_set_watchpoint(struct target *target,
+		struct watchpoint *watchpoint)
+{
+	struct stm8_common *stm8 = target_to_stm8(target);
+	struct stm8_comparator *comparator_list = stm8->hw_break_list;
+	int wp_num = 0;
+	int ret;
+
+	if (watchpoint->set) {
+		LOG_WARNING("watchpoint already set");
+		return ERROR_OK;
+	}
+
+	while (comparator_list[wp_num].used && (wp_num < stm8->num_hw_bpoints))
+		wp_num++;
+	if (wp_num >= stm8->num_hw_bpoints) {
+		LOG_ERROR("Can not find free hw breakpoint");
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	}
+
+	if (watchpoint->length != 1) {
+			LOG_ERROR("Only watchpoints of length 1 are supported");
+			return ERROR_TARGET_UNALIGNED_ACCESS;
+	}
+
+	enum hw_break_type enable = 0;
+
+	switch (watchpoint->rw) {
+		case WPT_READ:
+			enable = HWBRK_RD;
+			break;
+		case WPT_WRITE:
+			enable = HWBRK_WR;
+			break;
+		case WPT_ACCESS:
+			enable = HWBRK_ACC;
+			break;
+		default:
+			LOG_ERROR("BUG: watchpoint->rw neither read, write nor access");
+	}
+
+	comparator_list[wp_num].used = true;
+	comparator_list[wp_num].bp_value = watchpoint->address;
+	comparator_list[wp_num].type = enable;
+
+	ret = stm8_set_hwbreak(target, comparator_list);
+	if (ret != ERROR_OK) {
+		comparator_list[wp_num].used = false;
+		return ret;
+	}
+
+	watchpoint->set = wp_num + 1;
+
+	LOG_DEBUG("wp_num %i bp_value 0x%" PRIx32 "",
+			wp_num,
+			comparator_list[wp_num].bp_value);
+
+	return ERROR_OK;
+}
+
+static int stm8_add_watchpoint(struct target *target,
+		struct watchpoint *watchpoint)
+{
+	int ret;
+	struct stm8_common *stm8 = target_to_stm8(target);
+
+	if (stm8->num_hw_bpoints_avail < 1) {
+		LOG_INFO("no hardware watchpoints available");
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	}
+
+	ret = stm8_set_watchpoint(target, watchpoint);
+	if (ret != ERROR_OK)
+		return ret;
+
+	stm8->num_hw_bpoints_avail--;
+	return ERROR_OK;
+}
+
+static void stm8_enable_watchpoints(struct target *target)
+{
+	struct watchpoint *watchpoint = target->watchpoints;
+
+	/* set any pending watchpoints */
+	while (watchpoint) {
+		if (watchpoint->set == 0)
+			stm8_set_watchpoint(target, watchpoint);
+		watchpoint = watchpoint->next;
+	}
+}
+
+static int stm8_unset_watchpoint(struct target *target,
+		struct watchpoint *watchpoint)
+{
+	/* get pointers to arch-specific information */
+	struct stm8_common *stm8 = target_to_stm8(target);
+	struct stm8_comparator *comparator_list = stm8->hw_break_list;
+
+	if (!watchpoint->set) {
+		LOG_WARNING("watchpoint not set");
+		return ERROR_OK;
+	}
+
+	int wp_num = watchpoint->set - 1;
+	if ((wp_num < 0) || (wp_num >= stm8->num_hw_bpoints)) {
+		LOG_DEBUG("Invalid hw comparator number in watchpoint");
+		return ERROR_OK;
+	}
+	comparator_list[wp_num].used = false;
+	watchpoint->set = 0;
+
+	stm8_set_hwbreak(target, comparator_list);
+
+	return ERROR_OK;
+}
+
+static int stm8_remove_watchpoint(struct target *target,
+		struct watchpoint *watchpoint)
+{
+	/* get pointers to arch-specific information */
+	struct stm8_common *stm8 = target_to_stm8(target);
+
+	if (target->state != TARGET_HALTED) {
+		LOG_WARNING("target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if (watchpoint->set)
+		stm8_unset_watchpoint(target, watchpoint);
+
+	stm8->num_hw_bpoints_avail++;
+
+	return ERROR_OK;
+}
+
+static int stm8_examine(struct target *target)
+{
+	int retval;
+	uint8_t csr1, csr2;
+	/* get pointers to arch-specific information */
+	struct stm8_common *stm8 = target_to_stm8(target);
+	struct hl_interface_s *adapter = target_to_adapter(target);
+
+	if (!target_was_examined(target)) {
+		if (!stm8->swim_configured) {
+			/* set SWIM_CSR = 0xa0 (enable mem access & mask reset) */
+			LOG_DEBUG("writing A0 to SWIM_CSR (SAFE_MASK + SWIM_DM)");
+			retval = stm8_write_u8(target, SWIM_CSR, SAFE_MASK + SWIM_DM);
+			if (retval != ERROR_OK)
+				return retval;
+			/* set high speed */
+			LOG_DEBUG("writing B0 to SWIM_CSR (SAFE_MASK + SWIM_DM + HS)");
+			retval = stm8_write_u8(target, SWIM_CSR, SAFE_MASK + SWIM_DM + HS);
+			if (retval != ERROR_OK)
+				return retval;
+			retval = stm8_set_speed(target, 1);
+			if (retval == ERROR_OK)
+				stm8->swim_configured = true;
+			/*
+				Now is the time to deassert reset if connect_under_reset.
+				Releasing reset line will cause the option bytes to load.
+				The core will still be stalled.
+			*/
+			if (adapter->param.connect_under_reset)
+				stm8_reset_deassert(target);
+		} else {
+			LOG_INFO("trying to reconnect");
+
+			retval = adapter->layout->api->state(adapter->handle);
+			if (retval != ERROR_OK) {
+				LOG_ERROR("reconnect failed");
+				return ERROR_FAIL;
+			}
+
+			/* read dm_csrx control regs */
+			retval = stm8_read_dm_csrx(target, &csr1, &csr2);
+			if (retval != ERROR_OK) {
+				LOG_ERROR("state query failed");
+				return ERROR_FAIL;
+			}
+		}
+
+		target_set_examined(target);
+
+		return ERROR_OK;
+	}
+
+	return ERROR_OK;
+}
+
+/** Checks whether a memory region is erased. */
+static int stm8_blank_check_memory(struct target *target,
+		target_addr_t address, uint32_t count, uint32_t *blank, uint8_t erased_value)
+{
+	struct working_area *erase_check_algorithm;
+	struct reg_param reg_params[2];
+	struct mem_param mem_params[2];
+	struct stm8_algorithm stm8_info;
+
+	static const uint8_t stm8_erase_check_code[] = {
+#include "../../contrib/loaders/erase_check/stm8_erase_check.inc"
+	};
+
+	if (erased_value != 0xff) {
+		LOG_ERROR("Erase value 0x%02" PRIx8 " not yet supported for STM8",
+			erased_value);
+		return ERROR_FAIL;
+	}
+
+	/* make sure we have a working area */
+	if (target_alloc_working_area(target, sizeof(stm8_erase_check_code),
+			&erase_check_algorithm) != ERROR_OK)
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+
+	target_write_buffer(target, erase_check_algorithm->address,
+			sizeof(stm8_erase_check_code), stm8_erase_check_code);
+
+	stm8_info.common_magic = STM8_COMMON_MAGIC;
+
+	init_mem_param(&mem_params[0], 0x0, 3, PARAM_OUT);
+	buf_set_u32(mem_params[0].value, 0, 24, address);
+
+	init_mem_param(&mem_params[1], 0x3, 3, PARAM_OUT);
+	buf_set_u32(mem_params[1].value, 0, 24, count);
+
+	init_reg_param(&reg_params[0], "a", 32, PARAM_IN_OUT);
+	buf_set_u32(reg_params[0].value, 0, 32, erased_value);
+
+	init_reg_param(&reg_params[1], "sp", 32, PARAM_OUT);
+	buf_set_u32(reg_params[1].value, 0, 32, erase_check_algorithm->address);
+
+	int retval = target_run_algorithm(target, 2, mem_params, 2, reg_params,
+			erase_check_algorithm->address + 6,
+			erase_check_algorithm->address + (sizeof(stm8_erase_check_code) - 1),
+			10000, &stm8_info);
+
+	if (retval == ERROR_OK)
+		*blank = (*(reg_params[0].value) == 0xff);
+
+	destroy_mem_param(&mem_params[0]);
+	destroy_mem_param(&mem_params[1]);
+	destroy_reg_param(&reg_params[0]);
+
+	target_free_working_area(target, erase_check_algorithm);
+
+	return retval;
+}
+
+static int stm8_checksum_memory(struct target *target, target_addr_t address,
+		uint32_t count, uint32_t *checksum)
+{
+	/* let image_calculate_checksum() take care of business */
+	return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+}
+
+/* run to exit point. return error if exit point was not reached. */
+static int stm8_run_and_wait(struct target *target, uint32_t entry_point,
+		int timeout_ms, uint32_t exit_point, struct stm8_common *stm8)
+{
+	uint32_t pc;
+	int retval;
+	/* This code relies on the target specific resume() and
+	   poll()->debug_entry() sequence to write register values to the
+	   processor and the read them back */
+	retval = target_resume(target, 0, entry_point, 0, 1);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = target_wait_state(target, TARGET_HALTED, timeout_ms);
+	/* If the target fails to halt due to the breakpoint, force a halt */
+	if (retval != ERROR_OK || target->state != TARGET_HALTED) {
+		retval = target_halt(target);
+		if (retval != ERROR_OK)
+			return retval;
+		retval = target_wait_state(target, TARGET_HALTED, 500);
+		if (retval != ERROR_OK)
+			return retval;
+		return ERROR_TARGET_TIMEOUT;
+	}
+
+	pc = buf_get_u32(stm8->core_cache->reg_list[STM8_PC].value, 0, 32);
+	if (exit_point && (pc != exit_point)) {
+		LOG_DEBUG("failed algorithm halted at 0x%" PRIx32 " ", pc);
+		return ERROR_TARGET_TIMEOUT;
+	}
+
+	return ERROR_OK;
+}
+
+static int stm8_run_algorithm(struct target *target, int num_mem_params,
+		struct mem_param *mem_params, int num_reg_params,
+		struct reg_param *reg_params, target_addr_t entry_point,
+		target_addr_t exit_point, int timeout_ms, void *arch_info)
+{
+	struct stm8_common *stm8 = target_to_stm8(target);
+
+	uint32_t context[STM8_NUM_REGS];
+	int retval = ERROR_OK;
+
+	LOG_DEBUG("Running algorithm");
+
+	/* NOTE: stm8_run_algorithm requires that each
+	   algorithm uses a software breakpoint
+	   at the exit point */
+
+	if (stm8->common_magic != STM8_COMMON_MAGIC) {
+		LOG_ERROR("current target isn't a STM8 target");
+		return ERROR_TARGET_INVALID;
+	}
+
+	if (target->state != TARGET_HALTED) {
+		LOG_WARNING("target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	/* refresh core register cache */
+	for (unsigned int i = 0; i < STM8_NUM_REGS; i++) {
+		if (!stm8->core_cache->reg_list[i].valid)
+			stm8->read_core_reg(target, i);
+		context[i] = buf_get_u32(stm8->core_cache->reg_list[i].value, 0, 32);
+	}
+
+	for (int i = 0; i < num_mem_params; i++) {
+		retval = target_write_buffer(target, mem_params[i].address,
+				mem_params[i].size, mem_params[i].value);
+		if (retval != ERROR_OK)
+			return retval;
+	}
+
+	for (int i = 0; i < num_reg_params; i++) {
+		struct reg *reg = register_get_by_name(stm8->core_cache,
+				reg_params[i].reg_name, 0);
+
+		if (!reg) {
+			LOG_ERROR("BUG: register '%s' not found", reg_params[i].reg_name);
+			return ERROR_COMMAND_SYNTAX_ERROR;
+		}
+
+		if (reg_params[i].size != 32) {
+			LOG_ERROR("BUG: register '%s' size doesn't match reg_params[i].size",
+					reg_params[i].reg_name);
+			return ERROR_COMMAND_SYNTAX_ERROR;
+		}
+
+		stm8_set_core_reg(reg, reg_params[i].value);
+	}
+
+	retval = stm8_run_and_wait(target, entry_point,
+			timeout_ms, exit_point, stm8);
+
+	if (retval != ERROR_OK)
+		return retval;
+
+	for (int i = 0; i < num_mem_params; i++) {
+		if (mem_params[i].direction != PARAM_OUT) {
+			retval = target_read_buffer(target, mem_params[i].address,
+					mem_params[i].size, mem_params[i].value);
+			if (retval != ERROR_OK)
+				return retval;
+		}
+	}
+
+	for (int i = 0; i < num_reg_params; i++) {
+		if (reg_params[i].direction != PARAM_OUT) {
+			struct reg *reg = register_get_by_name(stm8->core_cache,
+					reg_params[i].reg_name, 0);
+			if (!reg) {
+				LOG_ERROR("BUG: register '%s' not found",
+						reg_params[i].reg_name);
+				return ERROR_COMMAND_SYNTAX_ERROR;
+			}
+
+			if (reg_params[i].size != 32) {
+				LOG_ERROR("BUG: register '%s' size doesn't match reg_params[i].size",
+						reg_params[i].reg_name);
+				return ERROR_COMMAND_SYNTAX_ERROR;
+			}
+
+			buf_set_u32(reg_params[i].value,
+					0, 32, buf_get_u32(reg->value, 0, 32));
+		}
+	}
+
+	/* restore everything we saved before */
+	for (unsigned int i = 0; i < STM8_NUM_REGS; i++) {
+		uint32_t regvalue;
+		regvalue = buf_get_u32(stm8->core_cache->reg_list[i].value, 0, 32);
+		if (regvalue != context[i]) {
+			LOG_DEBUG("restoring register %s with value 0x%8.8" PRIx32,
+				stm8->core_cache->reg_list[i].name, context[i]);
+			buf_set_u32(stm8->core_cache->reg_list[i].value,
+					0, 32, context[i]);
+			stm8->core_cache->reg_list[i].valid = true;
+			stm8->core_cache->reg_list[i].dirty = true;
+		}
+	}
+
+	return ERROR_OK;
+}
+
+int stm8_jim_configure(struct target *target, Jim_GetOptInfo *goi)
+{
+	struct stm8_common *stm8 = target_to_stm8(target);
+	jim_wide w;
+	int e;
+	const char *arg;
+
+	arg = Jim_GetString(goi->argv[0], NULL);
+	if (!strcmp(arg, "-blocksize")) {
+		e = Jim_GetOpt_String(goi, &arg, NULL);
+		if (e != JIM_OK)
+			return e;
+
+		if (goi->argc == 0) {
+			Jim_WrongNumArgs(goi->interp, goi->argc, goi->argv,
+					"-blocksize ?bytes? ...");
+			return JIM_ERR;
+		}
+
+		e = Jim_GetOpt_Wide(goi, &w);
+		if (e != JIM_OK)
+			return e;
+
+		stm8->blocksize = w;
+		LOG_DEBUG("blocksize=%8.8x", stm8->blocksize);
+		return JIM_OK;
+	}
+	if (!strcmp(arg, "-flashstart")) {
+		e = Jim_GetOpt_String(goi, &arg, NULL);
+		if (e != JIM_OK)
+			return e;
+
+		if (goi->argc == 0) {
+			Jim_WrongNumArgs(goi->interp, goi->argc, goi->argv,
+					"-flashstart ?address? ...");
+			return JIM_ERR;
+		}
+
+		e = Jim_GetOpt_Wide(goi, &w);
+		if (e != JIM_OK)
+			return e;
+
+		stm8->flashstart = w;
+		LOG_DEBUG("flashstart=%8.8x", stm8->flashstart);
+		return JIM_OK;
+	}
+	if (!strcmp(arg, "-flashend")) {
+		e = Jim_GetOpt_String(goi, &arg, NULL);
+		if (e != JIM_OK)
+			return e;
+
+		if (goi->argc == 0) {
+			Jim_WrongNumArgs(goi->interp, goi->argc, goi->argv,
+					"-flashend ?address? ...");
+			return JIM_ERR;
+		}
+
+		e = Jim_GetOpt_Wide(goi, &w);
+		if (e != JIM_OK)
+			return e;
+
+		stm8->flashend = w;
+		LOG_DEBUG("flashend=%8.8x", stm8->flashend);
+		return JIM_OK;
+	}
+	if (!strcmp(arg, "-eepromstart")) {
+		e = Jim_GetOpt_String(goi, &arg, NULL);
+		if (e != JIM_OK)
+			return e;
+
+		if (goi->argc == 0) {
+			Jim_WrongNumArgs(goi->interp, goi->argc, goi->argv,
+					"-eepromstart ?address? ...");
+			return JIM_ERR;
+		}
+
+		e = Jim_GetOpt_Wide(goi, &w);
+		if (e != JIM_OK)
+			return e;
+
+		stm8->eepromstart = w;
+		LOG_DEBUG("eepromstart=%8.8x", stm8->eepromstart);
+		return JIM_OK;
+	}
+	if (!strcmp(arg, "-eepromend")) {
+		e = Jim_GetOpt_String(goi, &arg, NULL);
+		if (e != JIM_OK)
+			return e;
+
+		if (goi->argc == 0) {
+			Jim_WrongNumArgs(goi->interp, goi->argc, goi->argv,
+					"-eepromend ?address? ...");
+			return JIM_ERR;
+		}
+
+		e = Jim_GetOpt_Wide(goi, &w);
+		if (e != JIM_OK)
+			return e;
+
+		stm8->eepromend = w;
+		LOG_DEBUG("eepromend=%8.8x", stm8->eepromend);
+		return JIM_OK;
+	}
+	if (!strcmp(arg, "-optionstart")) {
+		e = Jim_GetOpt_String(goi, &arg, NULL);
+		if (e != JIM_OK)
+			return e;
+
+		if (goi->argc == 0) {
+			Jim_WrongNumArgs(goi->interp, goi->argc, goi->argv,
+					"-optionstart ?address? ...");
+			return JIM_ERR;
+		}
+
+		e = Jim_GetOpt_Wide(goi, &w);
+		if (e != JIM_OK)
+			return e;
+
+		stm8->optionstart = w;
+		LOG_DEBUG("optionstart=%8.8x", stm8->optionstart);
+		return JIM_OK;
+	}
+	if (!strcmp(arg, "-optionend")) {
+		e = Jim_GetOpt_String(goi, &arg, NULL);
+		if (e != JIM_OK)
+			return e;
+
+		if (goi->argc == 0) {
+			Jim_WrongNumArgs(goi->interp, goi->argc, goi->argv,
+					"-optionend ?address? ...");
+			return JIM_ERR;
+		}
+
+		e = Jim_GetOpt_Wide(goi, &w);
+		if (e != JIM_OK)
+			return e;
+
+		stm8->optionend = w;
+		LOG_DEBUG("optionend=%8.8x", stm8->optionend);
+		return JIM_OK;
+	}
+	if (!strcmp(arg, "-enable_step_irq")) {
+		e = Jim_GetOpt_String(goi, &arg, NULL);
+		if (e != JIM_OK)
+			return e;
+
+		stm8->enable_step_irq = true;
+		LOG_DEBUG("enable_step_irq=%8.8x", stm8->enable_step_irq);
+		return JIM_OK;
+	}
+	if (!strcmp(arg, "-enable_stm8l")) {
+		e = Jim_GetOpt_String(goi, &arg, NULL);
+		if (e != JIM_OK)
+			return e;
+
+		stm8->enable_stm8l = true;
+		LOG_DEBUG("enable_stm8l=%8.8x", stm8->enable_stm8l);
+		stm8_init_flash_regs(stm8->enable_stm8l, stm8);
+		return JIM_OK;
+	}
+	return JIM_CONTINUE;
+}
+
+COMMAND_HANDLER(stm8_handle_enable_step_irq_command)
+{
+	const char *msg;
+	struct target *target = get_current_target(CMD_CTX);
+	struct stm8_common *stm8 = target_to_stm8(target);
+	bool enable = stm8->enable_step_irq;
+
+	if (CMD_ARGC > 0) {
+		COMMAND_PARSE_ENABLE(CMD_ARGV[0], enable);
+		stm8->enable_step_irq = enable;
+	}
+	msg = stm8->enable_step_irq ? "enabled" : "disabled";
+	command_print(CMD_CTX, "enable_step_irq = %s", msg);
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(stm8_handle_enable_stm8l_command)
+{
+	const char *msg;
+	struct target *target = get_current_target(CMD_CTX);
+	struct stm8_common *stm8 = target_to_stm8(target);
+	bool enable = stm8->enable_stm8l;
+
+	if (CMD_ARGC > 0) {
+		COMMAND_PARSE_ENABLE(CMD_ARGV[0], enable);
+		stm8->enable_stm8l = enable;
+	}
+	msg = stm8->enable_stm8l ? "enabled" : "disabled";
+	command_print(CMD_CTX, "enable_stm8l = %s", msg);
+	stm8_init_flash_regs(stm8->enable_stm8l, stm8);
+	return ERROR_OK;
+}
+
+static const struct command_registration stm8_exec_command_handlers[] = {
+	{
+		.name = "enable_step_irq",
+		.handler = stm8_handle_enable_step_irq_command,
+		.mode = COMMAND_ANY,
+		.help = "Enable/disable irq handling during step",
+		.usage = "[1/0]",
+	},
+	{
+		.name = "enable_stm8l",
+		.handler = stm8_handle_enable_stm8l_command,
+		.mode = COMMAND_ANY,
+		.help = "Enable/disable STM8L flash programming",
+		.usage = "[1/0]",
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+const struct command_registration stm8_command_handlers[] = {
+	{
+		.name = "stm8",
+		.mode = COMMAND_ANY,
+		.help = "stm8 command group",
+		.usage = "",
+		.chain = stm8_exec_command_handlers,
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+struct target_type stm8_target = {
+	.name = "stm8",
+
+	.poll = stm8_poll,
+	.arch_state = stm8_arch_state,
+
+	.halt = stm8_halt,
+	.resume = stm8_resume,
+	.step = stm8_step,
+
+	.assert_reset = stm8_reset_assert,
+	.deassert_reset = stm8_reset_deassert,
+
+	.get_gdb_reg_list = stm8_get_gdb_reg_list,
+
+	.read_memory = stm8_read_memory,
+	.write_memory = stm8_write_memory,
+	.checksum_memory = stm8_checksum_memory,
+	.blank_check_memory = stm8_blank_check_memory,
+
+	.run_algorithm = stm8_run_algorithm,
+
+	.add_breakpoint = stm8_add_breakpoint,
+	.remove_breakpoint = stm8_remove_breakpoint,
+	.add_watchpoint = stm8_add_watchpoint,
+	.remove_watchpoint = stm8_remove_watchpoint,
+
+	.commands = stm8_command_handlers,
+	.target_create = stm8_target_create,
+	.init_target = stm8_init,
+	.examine = stm8_examine,
+
+	.deinit_target = stm8_deinit,
+	.target_jim_configure = stm8_jim_configure,
+};
diff --git a/src/target/stm8.h b/src/target/stm8.h
new file mode 100644
index 0000000..39fac3e
--- /dev/null
+++ b/src/target/stm8.h
@@ -0,0 +1,75 @@
+/*
+    OpenOCD STM8 target driver
+    Copyright (C) 2017  Ake Rehnman
+    ake.rehnman(at)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 3 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_TARGET_STM8_H
+#define OPENOCD_TARGET_STM8_H
+
+struct target;
+
+#define STM8_COMMON_MAGIC	0x53544D38
+#define STM8_NUM_CORE_REGS 6
+
+struct stm8_common {
+	uint32_t common_magic;
+	void *arch_info;
+	struct reg_cache *core_cache;
+	uint32_t core_regs[STM8_NUM_CORE_REGS];
+
+	/* working area for fastdata access */
+	struct working_area *fast_data_area;
+
+	bool swim_configured;
+	bool bp_scanned;
+	uint8_t num_hw_bpoints;
+	uint8_t num_hw_bpoints_avail;
+	struct stm8_comparator *hw_break_list;
+	uint32_t blocksize;
+	uint32_t flashstart;
+	uint32_t flashend;
+	uint32_t eepromstart;
+	uint32_t eepromend;
+	uint32_t optionstart;
+	uint32_t optionend;
+	bool enable_step_irq;
+
+	bool enable_stm8l;
+	uint32_t flash_cr2;
+	uint32_t flash_ncr2;
+	uint32_t flash_iapsr;
+	uint32_t flash_dukr;
+	uint32_t flash_pukr;
+
+	/* cc value used for interrupt flags restore */
+	uint32_t cc;
+	bool cc_valid;
+
+	/* register cache to processor synchronization */
+	int (*read_core_reg)(struct target *target, unsigned int num);
+	int (*write_core_reg)(struct target *target, unsigned int num);
+};
+
+static inline struct stm8_common *
+target_to_stm8(struct target *target)
+{
+	return target->arch_info;
+}
+
+const struct command_registration stm8_command_handlers[];
+
+#endif /* OPENOCD_TARGET_STM8_H */
diff --git a/src/target/target.c b/src/target/target.c
index 3278444..ded20f2 100644
--- a/src/target/target.c
+++ b/src/target/target.c
@@ -105,6 +105,7 @@ extern struct target_type nds32_v3m_target;
 extern struct target_type or1k_target;
 extern struct target_type quark_x10xx_target;
 extern struct target_type quark_d20xx_target;
+extern struct target_type stm8_target;
 extern struct target_type riscv_target;
 
 static struct target_type *target_types[] = {
@@ -137,6 +138,7 @@ static struct target_type *target_types[] = {
 	&or1k_target,
 	&quark_x10xx_target,
 	&quark_d20xx_target,
+	&stm8_target,
 	&riscv_target,
 #if BUILD_TARGET64
 	&aarch64_target,
@@ -204,10 +206,6 @@ static const Jim_Nvp nvp_target_event[] = {
 	{ .value = TARGET_EVENT_RESET_ASSERT_POST,   .name = "reset-assert-post" },
 	{ .value = TARGET_EVENT_RESET_DEASSERT_PRE,  .name = "reset-deassert-pre" },
 	{ .value = TARGET_EVENT_RESET_DEASSERT_POST, .name = "reset-deassert-post" },
-	{ .value = TARGET_EVENT_RESET_HALT_PRE,      .name = "reset-halt-pre" },
-	{ .value = TARGET_EVENT_RESET_HALT_POST,     .name = "reset-halt-post" },
-	{ .value = TARGET_EVENT_RESET_WAIT_PRE,      .name = "reset-wait-pre" },
-	{ .value = TARGET_EVENT_RESET_WAIT_POST,     .name = "reset-wait-post" },
 	{ .value = TARGET_EVENT_RESET_INIT,          .name = "reset-init" },
 	{ .value = TARGET_EVENT_RESET_END,           .name = "reset-end" },
 
@@ -3699,7 +3697,7 @@ COMMAND_HANDLER(handle_bp_command)
 				addr = 0;
 				return handle_bp_command_set(CMD_CTX, addr, asid, length, hw);
 			}
-
+			/* fallthrough */
 		case 4:
 			hw = BKPT_HARD;
 			COMMAND_PARSE_ADDRESS(CMD_ARGV[0], addr);
diff --git a/src/target/target.h b/src/target/target.h
index 53f9e26..0096cae 100644
--- a/src/target/target.h
+++ b/src/target/target.h
@@ -253,10 +253,6 @@ enum target_event {
 	TARGET_EVENT_RESET_ASSERT_POST,
 	TARGET_EVENT_RESET_DEASSERT_PRE,
 	TARGET_EVENT_RESET_DEASSERT_POST,
-	TARGET_EVENT_RESET_HALT_PRE,
-	TARGET_EVENT_RESET_HALT_POST,
-	TARGET_EVENT_RESET_WAIT_PRE,
-	TARGET_EVENT_RESET_WAIT_POST,
 	TARGET_EVENT_RESET_INIT,
 	TARGET_EVENT_RESET_END,