From ae6f0eadc9748deced4e48074eb7d73064cc4e28 Mon Sep 17 00:00:00 2001
From: newbrain <federico.zuccardimerli@gmail.com>
Date: Wed, 6 Jun 2018 22:34:21 +0200
Subject: [PATCH 1/7] Support for MSP432 TI MCUs (#353)

Introduces flashing and debugging support for Texas Instruments MSP432
series of MCUs
---
 src/Makefile                 |   1 +
 src/target/cortexm.c         |   1 +
 src/target/msp432.c          | 374 +++++++++++++++++++++++++++++++++++
 src/target/target_internal.h |   1 +
 4 files changed, 377 insertions(+)
 create mode 100644 src/target/msp432.c

diff --git a/src/Makefile b/src/Makefile
index 6ef91994..797fd15b 100644
--- a/src/Makefile
+++ b/src/Makefile
@@ -46,6 +46,7 @@ SRC =			\
 	kinetis.c	\
 	main.c		\
 	morse.c		\
+	msp432.c	\
 	nrf51.c		\
 	platform.c	\
 	sam3x.c		\
diff --git a/src/target/cortexm.c b/src/target/cortexm.c
index 1687fa92..33344d65 100644
--- a/src/target/cortexm.c
+++ b/src/target/cortexm.c
@@ -314,6 +314,7 @@ bool cortexm_probe(ADIv5_AP_t *ap)
 	PROBE(lmi_probe);
 	PROBE(kinetis_probe);
 	PROBE(efm32_probe);
+	PROBE(msp432_probe);
 #undef PROBE
 
 	return true;
diff --git a/src/target/msp432.c b/src/target/msp432.c
new file mode 100644
index 00000000..38289fb2
--- /dev/null
+++ b/src/target/msp432.c
@@ -0,0 +1,374 @@
+/*
+ * This file is part of the Black Magic Debug project.
+ *
+ * Copyright (C) 2017  newbrain.
+ *
+ * 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/>.
+ */
+
+/* This file implements MSP432 target specific functions for detecting
+ * the device, providing the XML memory map and Flash memory programming.
+ *
+ * Refereces:
+ * TI doc - SLAU356G
+ *   MSP423P4xx Technical Reference Manual
+ * TI doc - SLAS826G
+ *   MSP432P401R, MSP432P401M SimpleLink Mixed-Signal Microcontrollers
+ * TI doc - SLAA704
+ *   Flash Operations on MSP432 MCUs
+ * TI doc -
+ *   MSP432® Peripheral Driver Library User's Guide
+ *
+ */
+
+#include "general.h"
+#include "target.h"
+#include "target_internal.h"
+#include "cortexm.h"
+
+/* TLV: Device info tag, address and expected value */
+#define DEVINFO_TAG_ADDR 0x00201004u
+#define DEVINFO_TAG_VALUE 0x0000000Bu
+
+/* TLV: Device info length, address and expected value */
+#define DEVINFO_LEN_ADDR 0x00201008u
+#define DEVINFO_LEN_VALUE 0x00000004u
+
+/* TLV: Device ID, address and expected values */
+#define DEVID_ADDR 0x0020100Cu
+#define DEVID_MSP432P401RIPZ 0x0000A000u
+#define DEVID_MSP432P401MIPZ 0x0000A001u
+#define DEVID_MSP432P401RIZXH 0x0000A002u
+#define DEVID_MSP432P401MIZXH 0x0000A003u
+#define DEVID_MSP432P401RIRGC 0x0000A004u
+#define DEVID_MSP432P401MIRGC 0x0000A005u
+
+/* TLV: Hardware revision, address and minimum expected value */
+#define HWREV_ADDR 0x00201010u
+#define HWREV_MIN_VALUE 0x00000043u
+
+/* ROM Device Driver Table pointer addresses */
+#define ROM_APITABLE 0x02000800u
+
+#define OFS_FLASHCTLTABLE 28             /* ROM_APITABLE[7] */
+#define OFS_FlashCtl_performMassErase 32 /* ROM_FLASHCTLTABLE[8] */
+#define OFS_FlashCtl_eraseSector 36      /* ROM_FLASHCTLTABLE[9] */
+#define OFS_FlashCtl_programMemory 40    /* ROM_FLASHCTLTABLE[10] */
+
+/* Memory sizes and base addresses */
+#define MAIN_FLASH_BASE 0x00000000u /* Beginning of Main Flash */
+#define INFO_FLASH_BASE 0x00200000u /* Beginning of Info Flash */
+#define INFO_BANK_SIZE 0x00002000u  /* Size of 1 bank of Info Flash */
+#define SECTOR_SIZE 0x1000u         /* Size of erase page: 4KB */
+
+/* Flash protection registers */
+#define INFO_BANK0_WEPROT 0x400110B0u /* Write/Erase protection Bank 0 Info */
+#define MAIN_BANK0_WEPROT 0x400110B4u /* Write/Erase protection Bank 0 Main */
+#define INFO_BANK1_WEPROT 0x400110C0u /* Write/Erase protection Bank 1 Info */
+#define MAIN_BANK1_WEPROT 0x400110C4u /* Write/Erase protection Bank 1 Main */
+
+/* Main Flash and SRAM size registers */
+#define SYS_SRAM_SIZE 0xE0043010u  /* Size of SRAM in SYSCTL */
+#define SYS_FLASH_SIZE 0xE0043020u /* Size of main flash in SYSCTL */
+
+/* RAM info */
+#define SRAM_BASE 0x20000000u       /* Beginning of SRAM */
+#define SRAM_CODE_BASE 0x01000000u  /* Beginning of SRAM, Code zone alias */
+#define P401M_SRAM_SIZE 0x00008000u /* Size of SRAM, M: 32KB */
+#define P401R_SRAM_SIZE 0x00010000u /* Size of SRAM, R: 64KB */
+
+/* Flash write buffer and stack */
+#define SRAM_STACK_OFFSET 0x00000200u /* A bit less than 512 stack room */
+#define SRAM_STACK_PTR (SRAM_BASE + SRAM_STACK_OFFSET)
+#define SRAM_WRITE_BUFFER SRAM_STACK_PTR /* Buffer right above stack */
+#define SRAM_WRITE_BUF_SIZE 0x00000400u  /* Write 1024 bytes at a tima */
+
+/* Watchdog */
+#define WDT_A_WTDCTL 0x4000480Cu /* Control register for watchdog */
+#define WDT_A_HOLD 0x5A88u       /* Clears and halts the watchdog */
+
+/* Support variables to call code in ROM */
+struct msp432_flash
+{
+	struct target_flash f;
+	target_addr flash_protect_register; /* Address of the WEPROT register*/
+	target_addr FlashCtl_eraseSector;   /* Erase flash sector routine in ROM*/
+	target_addr FlashCtl_programMemory; /* Flash programming routine in ROM */
+};
+
+/* Flash operations */
+static bool msp432_sector_erase(struct target_flash *f, target_addr addr);
+static int msp432_flash_erase(struct target_flash *f, target_addr addr, size_t len);
+static int msp432_flash_write(struct target_flash *f, target_addr dest,
+			      const void *src, size_t len);
+
+/* Utility functions */
+/* Find the the target flash that conatins a specific address */
+static struct target_flash *get_target_flash(target *t, target_addr addr);
+
+/* Call a subroutine in the MSP432 ROM (or anywhere else...)*/
+static void msp432_call_ROM(target *t, uint32_t address, uint32_t regs[]);
+
+/* Protect or unprotect the sector containing address */
+static inline uint32_t msp432_sector_unprotect(struct msp432_flash *mf, target_addr addr)
+{
+	/* Read the old protection register */
+	uint32_t old_mask = target_mem_read32(mf->f.t, mf->flash_protect_register);
+	/* Find the bit representing the sector and set it to 0  */
+	uint32_t sec_mask = ~(1u << ((addr - mf->f.start) / SECTOR_SIZE));
+	/* Clear the potection bit */
+	sec_mask &= old_mask;
+	target_mem_write32(mf->f.t, mf->flash_protect_register, sec_mask);
+	return old_mask;
+}
+
+/* Optional commands handlers */
+/* Erase all of main flash */
+static bool msp432_cmd_erase_main(target *t);
+/* Erase a single (4KB) sector */
+static bool msp432_cmd_sector_erase(target *t, int argc, char *argv[]);
+
+/* Optional commands structure*/
+const struct command_s msp432_cmd_list[] = {
+	{"erase", (cmd_handler)msp432_cmd_erase_main, "Erase main flash"},
+	{"sector_erase", (cmd_handler)msp432_cmd_sector_erase, "Erase sector containing given address"},
+	{NULL, NULL, NULL}};
+
+static void msp432_add_flash(target *t, uint32_t addr, size_t length, target_addr prot_reg)
+{
+	struct msp432_flash *mf = calloc(1, sizeof(*mf));
+	struct target_flash *f = &mf->f;
+	f->start = addr;
+	f->length = length;
+	f->blocksize = SECTOR_SIZE;
+	f->erase = msp432_flash_erase;
+	f->write = msp432_flash_write;
+	f->buf_size = SRAM_WRITE_BUF_SIZE;
+	f->erased = 0xff;
+	target_add_flash(t, f);
+	/* Initialize ROM call pointers. Silicon rev B is not supported */
+	uint32_t flashctltable =
+		target_mem_read32(t, ROM_APITABLE + OFS_FLASHCTLTABLE);
+	mf->FlashCtl_eraseSector =
+		target_mem_read32(t, flashctltable + OFS_FlashCtl_eraseSector);
+	mf->FlashCtl_programMemory =
+		target_mem_read32(t, flashctltable + OFS_FlashCtl_programMemory);
+	mf->flash_protect_register = prot_reg;
+}
+
+bool msp432_probe(target *t)
+{
+	/* Check for the right device info tag in the TLV ROM structure */
+	if (target_mem_read32(t, DEVINFO_TAG_ADDR) != DEVINFO_TAG_VALUE)
+		return false;
+
+	/* Check for the right device info length tag in the TLV ROM structure */
+	if (target_mem_read32(t, DEVINFO_LEN_ADDR) != DEVINFO_LEN_VALUE)
+		return false;
+
+	/* Check for the right HW revision: at least C, as no flash support for B */
+	if (target_mem_read32(t, HWREV_ADDR) < HWREV_MIN_VALUE)
+		return false;
+
+	/* If we got till this point, we are most probably looking at a real TLV  */
+	/* Device Information structure. Now check for the correct device         */
+	switch (target_mem_read32(t, DEVID_ADDR)) { 
+	case DEVID_MSP432P401RIPZ:
+	case DEVID_MSP432P401RIZXH:
+	case DEVID_MSP432P401RIRGC:
+		/* R series: 256kB Flash, 64kB RAM */
+		t->driver = "MSP432P401R 256KB Flash 64KB RAM";
+		break;
+
+	case DEVID_MSP432P401MIPZ:
+	case DEVID_MSP432P401MIZXH:
+	case DEVID_MSP432P401MIRGC:
+		/* M series: 128kB Flash, 32kB RAM */
+		t->driver = "MSP432P401M 128KB Flash 32KB RAM";
+		break;
+
+	default:
+		/* Unknown device, not an MSP432 or not a real TLV */
+		return false;
+	}
+	/* SRAM region, SRAM zone */
+	target_add_ram(t, SRAM_BASE, target_mem_read32(t, SYS_SRAM_SIZE));
+	/* Flash bank size */
+	uint32_t banksize = target_mem_read32(t, SYS_FLASH_SIZE) / 2;
+	/* Main Flash Bank 0 */
+	msp432_add_flash(t, MAIN_FLASH_BASE, banksize, MAIN_BANK0_WEPROT);
+	/* Main Flash Bank 1 */
+	msp432_add_flash(t, MAIN_FLASH_BASE + banksize, banksize, MAIN_BANK1_WEPROT);
+	/* Info Flash Bank 0 */
+	msp432_add_flash(t, INFO_FLASH_BASE, INFO_BANK_SIZE, INFO_BANK0_WEPROT);
+	/* Info Flash Bank 1 */
+	msp432_add_flash(t, INFO_FLASH_BASE + INFO_BANK_SIZE, INFO_BANK_SIZE, INFO_BANK1_WEPROT);
+
+	/* Connect the optional commands */
+	target_add_commands(t, msp432_cmd_list, "MSP432P401x");
+
+	/* All done */
+	return true;
+}
+
+/* Flash operations */
+/* Erase a single sector at addr calling the ROM routine*/
+static bool msp432_sector_erase(struct target_flash *f, target_addr addr)
+{
+	target *t = f->t;
+	struct msp432_flash *mf = (struct msp432_flash *)f;
+
+	/* Unprotect sector */
+	uint32_t old_prot = msp432_sector_unprotect(mf, addr);
+	DEBUG("Flash protect: 0x%08"PRIX32"\n", target_mem_read32(t, mf->flash_protect_register));
+
+	/* Prepare input data */
+	uint32_t regs[t->regs_size / sizeof(uint32_t)]; // Use of VLA
+	target_regs_read(t, regs);
+	regs[0] = addr; // Address of sector to erase in R0
+
+	DEBUG("Erasing sector at 0x%08"PRIX32"\n", addr);
+
+	/* Call ROM */
+	msp432_call_ROM(t, mf->FlashCtl_eraseSector, regs);
+
+	// Result value in R0 is true for success
+	DEBUG("ROM return value: %"PRIu32"\n", regs[0]);
+
+	/* Restore original protection */
+	target_mem_write32(t, mf->flash_protect_register, old_prot);
+
+	return !regs[0];
+}
+
+/* Erase from addr for len bytes */
+static int msp432_flash_erase(struct target_flash *f, target_addr addr, size_t len)
+{
+	int ret = 0;
+	while (len) {
+		ret |= msp432_sector_erase(f, addr);
+
+		/* update len and addr */
+		len -= f->blocksize;
+		addr += f->blocksize;
+	}
+
+	return ret;
+}
+
+/* Program flash */
+static int msp432_flash_write(struct target_flash *f, target_addr dest,
+			      const void *src, size_t len)
+{
+	struct msp432_flash *mf = (struct msp432_flash *)f;
+	target *t = f->t;
+
+	/* Prepare RAM buffer in target */
+	target_mem_write(t, SRAM_WRITE_BUFFER, src, len);
+
+	/* Unprotect sector, len is always < SECTOR_SIZE */
+	uint32_t old_prot = msp432_sector_unprotect(mf, dest);
+
+	DEBUG("Flash protect: 0x%08"PRIX32"\n", target_mem_read32(t, mf->flash_protect_register));
+
+	/* Prepare input data */
+	uint32_t regs[t->regs_size / sizeof(uint32_t)]; // Use of VLA
+	target_regs_read(t, regs);
+	regs[0] = SRAM_WRITE_BUFFER; // Address of buffer to be flashed in R0
+	regs[1] = dest;              // Flash address to be write to in R1
+	regs[2] = len;               // Size of buffer to be flashed in R2
+
+	DEBUG("Writing 0x%04"PRIX32" bytes at 0x%08zX\n", dest, len);
+	/* Call ROM */
+	msp432_call_ROM(t, mf->FlashCtl_programMemory, regs);
+
+	/* Restore original protection */
+	target_mem_write32(t, mf->flash_protect_register, old_prot);
+
+	DEBUG("ROM return value: %"PRIu32"\n", regs[0]);
+	// Result value in R0 is true for success
+	return !regs[0];
+}
+
+/* Optional commands handlers */
+static bool msp432_cmd_erase_main(target *t)
+{
+	/* The mass erase routine in ROM will also erase the Info Flash. */
+	/* Usually, this is not wanted, so go sector by sector...        */
+
+	uint32_t banksize = target_mem_read32(t, SYS_FLASH_SIZE) / 2;
+	DEBUG("Bank Size: 0x%08"PRIX32"\n", banksize);
+
+	/* Erase first bank */
+	struct target_flash *f = get_target_flash(t, MAIN_FLASH_BASE);
+	bool ret = msp432_flash_erase(f, MAIN_FLASH_BASE, banksize);
+
+	/* Erase second bank */
+	f = get_target_flash(t, MAIN_FLASH_BASE + banksize);
+	ret |= msp432_flash_erase(f, MAIN_FLASH_BASE + banksize, banksize);
+
+	return ret;
+}
+
+static bool msp432_cmd_sector_erase(target *t, int argc, char *argv[])
+{
+	if (argc < 2)
+		tc_printf(t, "usage: monitor sector_erase <addr>\n");
+
+	uint32_t addr = strtoul(argv[1], NULL, 0);
+
+	/* Find the flash structure (for the rigth protect register) */
+	struct target_flash *f = get_target_flash(t, addr);
+
+	if (f)
+		return msp432_sector_erase(f, addr);
+	tc_printf(t, "Invalid sector address\n");
+	return false;
+}
+
+/* Returns flash bank containing addr, or NULL if not found */
+static struct target_flash *get_target_flash(target *t, target_addr addr)
+{
+	struct target_flash *f = t->flash;
+	while (f) {
+		if ((f->start <= addr) && (addr < f->start + f->length))
+			break;
+		f = f->next;
+	}
+	return f;
+}
+
+/* MSP432 ROM routine invocation */
+static void msp432_call_ROM(target *t, uint32_t address, uint32_t regs[])
+{
+	/* Kill watchdog */
+	target_mem_write16(t, WDT_A_WTDCTL, WDT_A_HOLD);
+
+	/* Breakpoint at the beginning of CODE SRAM alias area */
+	target_mem_write16(t, SRAM_CODE_BASE, ARM_THUMB_BREAKPOINT);
+
+	/* Prepare registers */
+	regs[REG_MSP] = SRAM_STACK_PTR;    /* Stack space */
+	regs[REG_LR] = SRAM_CODE_BASE | 1; /* Return to beginning of SRAM CODE alias */
+	regs[REG_PC] = address;            /* Start at given address */
+	target_regs_write(t, regs);
+
+	/* Call ROM */
+	/* start the target and wait for it to halt again */
+	target_halt_resume(t, false);
+	while (!target_halt_poll(t, NULL));
+
+	// Read registers to get result
+	target_regs_read(t, regs);
+}
\ No newline at end of file
diff --git a/src/target/target_internal.h b/src/target/target_internal.h
index caa542af..d78ed537 100644
--- a/src/target/target_internal.h
+++ b/src/target/target_internal.h
@@ -175,5 +175,6 @@ bool nrf51_probe(target *t);
 bool samd_probe(target *t);
 bool kinetis_probe(target *t);
 bool efm32_probe(target *t);
+bool msp432_probe(target *t);
 
 #endif

From 54f73858f97443c8a604a8f209c29d25656d2d53 Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Thu, 31 May 2018 21:39:19 +0200
Subject: [PATCH 2/7] Provide a target function to write with given size.

---
 src/target/adiv5.c   | 19 ++++++++++++-------
 src/target/adiv5.h   |  9 +++++++++
 src/target/cortexm.c |  8 ++++++++
 src/target/cortexm.h |  2 ++
 4 files changed, 31 insertions(+), 7 deletions(-)

diff --git a/src/target/adiv5.c b/src/target/adiv5.c
index 8d869363..b6236d84 100644
--- a/src/target/adiv5.c
+++ b/src/target/adiv5.c
@@ -457,11 +457,6 @@ void adiv5_dp_init(ADIv5_DP_t *dp)
 	adiv5_dp_unref(dp);
 }
 
-enum align {
-	ALIGN_BYTE =  0,
-	ALIGN_HALFWORD = 1,
-	ALIGN_WORD = 2
-};
 #define ALIGNOF(x) (((x) & 3) == 0 ? ALIGN_WORD : \
                     (((x) & 1) == 0 ? ALIGN_HALFWORD : ALIGN_BYTE))
 
@@ -477,6 +472,7 @@ static void ap_mem_access_setup(ADIv5_AP_t *ap, uint32_t addr, enum align align)
 	case ALIGN_HALFWORD:
 		csw |= ADIV5_AP_CSW_SIZE_HALFWORD;
 		break;
+	case ALIGN_DWORD:
 	case ALIGN_WORD:
 		csw |= ADIV5_AP_CSW_SIZE_WORD;
 		break;
@@ -495,6 +491,7 @@ static void * extract(void *dest, uint32_t src, uint32_t val, enum align align)
 	case ALIGN_HALFWORD:
 		*(uint16_t *)dest = (val >> ((src & 0x2) << 3) & 0xFFFF);
 		break;
+	case ALIGN_DWORD:
 	case ALIGN_WORD:
 		*(uint32_t *)dest = val;
 		break;
@@ -534,10 +531,10 @@ adiv5_mem_read(ADIv5_AP_t *ap, void *dest, uint32_t src, size_t len)
 }
 
 void
-adiv5_mem_write(ADIv5_AP_t *ap, uint32_t dest, const void *src, size_t len)
+adiv5_mem_write_sized(ADIv5_AP_t *ap, uint32_t dest, const void *src,
+					  size_t len, enum align align)
 {
 	uint32_t odest = dest;
-	enum align align = MIN(ALIGNOF(dest), ALIGNOF(len));
 
 	len >>= align;
 	ap_mem_access_setup(ap, dest, align);
@@ -551,6 +548,7 @@ adiv5_mem_write(ADIv5_AP_t *ap, uint32_t dest, const void *src, size_t len)
 		case ALIGN_HALFWORD:
 			tmp = ((uint32_t)*(uint16_t *)src) << ((dest & 2) << 3);
 			break;
+		case ALIGN_DWORD:
 		case ALIGN_WORD:
 			tmp = *(uint32_t *)src;
 			break;
@@ -568,6 +566,13 @@ adiv5_mem_write(ADIv5_AP_t *ap, uint32_t dest, const void *src, size_t len)
 	}
 }
 
+void
+adiv5_mem_write(ADIv5_AP_t *ap, uint32_t dest, const void *src, size_t len)
+{
+	enum align align = MIN(ALIGNOF(dest), ALIGNOF(len));
+	adiv5_mem_write_sized(ap, dest, src, len, align);
+}
+
 void adiv5_ap_write(ADIv5_AP_t *ap, uint16_t addr, uint32_t value)
 {
 	adiv5_dp_write(ap->dp, ADIV5_DP_SELECT,
diff --git a/src/target/adiv5.h b/src/target/adiv5.h
index ca814719..2929a315 100644
--- a/src/target/adiv5.h
+++ b/src/target/adiv5.h
@@ -100,6 +100,13 @@
 #define ADIV5_LOW_WRITE		0
 #define ADIV5_LOW_READ		1
 
+enum align {
+	ALIGN_BYTE     = 0,
+	ALIGN_HALFWORD = 1,
+	ALIGN_WORD     = 2,
+	ALIGN_DWORD    = 3
+};
+
 /* Try to keep this somewhat absract for later adding SW-DP */
 typedef struct ADIv5_DP_s {
 	int refcnt;
@@ -167,6 +174,8 @@ void adiv5_jtag_dp_handler(jtag_dev_t *dev);
 
 void adiv5_mem_read(ADIv5_AP_t *ap, void *dest, uint32_t src, size_t len);
 void adiv5_mem_write(ADIv5_AP_t *ap, uint32_t dest, const void *src, size_t len);
+void adiv5_mem_write_sized(ADIv5_AP_t *ap, uint32_t dest, const void *src,
+						   size_t len, enum align align);
 
 #endif
 
diff --git a/src/target/cortexm.c b/src/target/cortexm.c
index 33344d65..dba23498 100644
--- a/src/target/cortexm.c
+++ b/src/target/cortexm.c
@@ -459,6 +459,14 @@ static void cortexm_regs_write(target *t, const void *data)
 		}
 }
 
+int cortexm_mem_write_sized(
+	target *t, target_addr dest, const void *src, size_t len, enum align align)
+{
+	cortexm_cache_clean(t, dest, len, true);
+	adiv5_mem_write_sized(cortexm_ap(t), dest, src, len, align);
+	return target_check_error(t);
+}
+
 static uint32_t cortexm_pc_read(target *t)
 {
 	target_mem_write32(t, CORTEXM_DCRSR, 0x0F);
diff --git a/src/target/cortexm.h b/src/target/cortexm.h
index e9bf5476..9aa47339 100644
--- a/src/target/cortexm.h
+++ b/src/target/cortexm.h
@@ -175,6 +175,8 @@ void cortexm_detach(target *t);
 void cortexm_halt_resume(target *t, bool step);
 int cortexm_run_stub(target *t, uint32_t loadaddr,
                      uint32_t r0, uint32_t r1, uint32_t r2, uint32_t r3);
+int cortexm_mem_write_sized(
+	target *t, target_addr dest, const void *src, size_t len, enum align align);
 
 #endif
 

From bfeb6f0db9b33696937de003478118c121e8a634 Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Thu, 15 Mar 2018 18:23:53 +0100
Subject: [PATCH 3/7] stm32f4: Use buffered direct flash write with choosen
 size.

---
 src/target/flashstub/Makefile         |  4 +-
 src/target/flashstub/stm32f4_x32.c    | 41 ----------------
 src/target/flashstub/stm32f4_x32.stub |  1 -
 src/target/flashstub/stm32f4_x8.c     | 44 -----------------
 src/target/flashstub/stm32f4_x8.stub  |  1 -
 src/target/stm32f4.c                  | 69 ++++++++++++++-------------
 6 files changed, 36 insertions(+), 124 deletions(-)
 delete mode 100644 src/target/flashstub/stm32f4_x32.c
 delete mode 100644 src/target/flashstub/stm32f4_x32.stub
 delete mode 100644 src/target/flashstub/stm32f4_x8.c
 delete mode 100644 src/target/flashstub/stm32f4_x8.stub

diff --git a/src/target/flashstub/Makefile b/src/target/flashstub/Makefile
index c16b3d1a..d6abdd1e 100644
--- a/src/target/flashstub/Makefile
+++ b/src/target/flashstub/Makefile
@@ -11,12 +11,10 @@ endif
 CFLAGS=-Os -std=gnu99 -mcpu=cortex-m0 -mthumb -I../../../libopencm3/include
 ASFLAGS=-mcpu=cortex-m3 -mthumb
 
-all:	lmi.stub stm32f4_x8.stub stm32f4_x32.stub stm32l4.stub nrf51.stub \
+all:	lmi.stub stm32l4.stub nrf51.stub \
 	stm32f1.stub efm32.stub
 
 stm32f1.o: CFLAGS += -DSTM32F1
-stm32f4_x8.o: CFLAGS += -DSTM32F4
-stm32f4_x32.o: CFLAGS += -DSTM32F4
 
 %.o:    %.c
 	$(Q)echo "  CC      $<"
diff --git a/src/target/flashstub/stm32f4_x32.c b/src/target/flashstub/stm32f4_x32.c
deleted file mode 100644
index 117c1323..00000000
--- a/src/target/flashstub/stm32f4_x32.c
+++ /dev/null
@@ -1,41 +0,0 @@
-/*
- * This file is part of the Black Magic Debug project.
- *
- * Copyright (C) 2015  Black Sphere Technologies Ltd.
- * Written by Gareth McMullin <gareth@blacksphere.co.nz>
- *
- * 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/>.
- */
-#include "libopencm3/stm32/flash.h"
-#include "stub.h"
-
-#define SR_ERROR_MASK 0xF2
-
-void __attribute__((naked))
-stm32f4_flash_write_x32_stub(uint32_t *dest, uint32_t *src, uint32_t size)
-{
-	for (int i = 0; i < size; i += 4) {
-		FLASH_CR = FLASH_CR_PROGRAM_X32 | FLASH_CR_PG;
-		*dest++ = *src++;
-		__asm("dsb");
-		while (FLASH_SR & FLASH_SR_BSY)
-			;
-	}
-
-	if (FLASH_SR & SR_ERROR_MASK)
-		stub_exit(1);
-
-	stub_exit(0);
-}
-
diff --git a/src/target/flashstub/stm32f4_x32.stub b/src/target/flashstub/stm32f4_x32.stub
deleted file mode 100644
index b504ce2c..00000000
--- a/src/target/flashstub/stm32f4_x32.stub
+++ /dev/null
@@ -1 +0,0 @@
-0x2300, 0x4C0A, 0x4293, 0xD20B, 0x4E09, 0x4D0A, 0x602E, 0x58CD, 0x50C5, 0xF3BF, 0x8F4F, 0x6825, 0x03ED, 0xD4FC, 0x3304, 0xE7F0, 0x23F2, 0x6822, 0x421A, 0xD000, 0xBE01, 0xBE00, 0x3C0C, 0x4002, 0x0201, 0x0000, 0x3C10, 0x4002,
diff --git a/src/target/flashstub/stm32f4_x8.c b/src/target/flashstub/stm32f4_x8.c
deleted file mode 100644
index 84f91175..00000000
--- a/src/target/flashstub/stm32f4_x8.c
+++ /dev/null
@@ -1,44 +0,0 @@
-/*
- * This file is part of the Black Magic Debug project.
- *
- * Copyright (C) 2017  Black Sphere Technologies Ltd.
- * Written by Gordon Smith <gordonhj.smith@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/>.
- */
-#include "libopencm3/stm32/flash.h"
-#include "stub.h"
-
-#define SR_ERROR_MASK 0xF2
-
-void __attribute__((naked))
-stm32f4_flash_write_x8_stub(uint32_t *dest, uint32_t *src, uint32_t size)
-{
-	uint8_t *b_dest, *b_src;
-	b_dest = (void *)dest;
-	b_src = (void *)src;
-	for (int i = 0; i < size; i += 1) {
-		FLASH_CR = FLASH_CR_PROGRAM_X8 | FLASH_CR_PG;
-		*b_dest++ = *b_src++;
-		__asm("dsb");
-		while (FLASH_SR & FLASH_SR_BSY)
-			;
-	}
-
-	if (FLASH_SR & SR_ERROR_MASK)
-		stub_exit(1);
-
-	stub_exit(0);
-}
-
diff --git a/src/target/flashstub/stm32f4_x8.stub b/src/target/flashstub/stm32f4_x8.stub
deleted file mode 100644
index 08d17dc2..00000000
--- a/src/target/flashstub/stm32f4_x8.stub
+++ /dev/null
@@ -1 +0,0 @@
-0x2300, 0x4C0A, 0x4293, 0xD00B, 0x2601, 0x4D09, 0x602E, 0x5CCD, 0x54C5, 0xF3BF, 0x8F4F, 0x6825, 0x03ED, 0xD4FC, 0x3301, 0xE7F0, 0x23F2, 0x6822, 0x421A, 0xD000, 0xBE01, 0xBE00, 0x3C0C, 0x4002, 0x3C10, 0x4002,
diff --git a/src/target/stm32f4.c b/src/target/stm32f4.c
index 7f2c597d..2701cef9 100644
--- a/src/target/stm32f4.c
+++ b/src/target/stm32f4.c
@@ -3,6 +3,8 @@
  *
  * Copyright (C) 2011  Black Sphere Technologies Ltd.
  * Written by Gareth McMullin <gareth@blacksphere.co.nz>
+ * Copyright (C) 2017, 2018  Uwe Bonnes
+ *                           <bon@elektron.ikp.physik.tu-darmstadt.de>
  *
  * This program is free software: you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -44,7 +46,7 @@ const struct command_s stm32f4_cmd_list[] = {
 	 "Erase entire flash memory"},
 	{"option", (cmd_handler)stm32f4_cmd_option, "Manipulate option bytes"},
 	{"psize", (cmd_handler)stm32f4_cmd_psize,
-	 "Configure flash write parallelism: (x8|x32(default))"},
+	 "Configure flash write parallelism: (x8|x16|x32(default))"},
 	{NULL, NULL, NULL}
 };
 
@@ -108,28 +110,13 @@ static int stm32f4_flash_write(struct target_flash *f,
 #define DBG_WWDG_STOP	(1 << 11)
 #define DBG_IWDG_STOP	(1 << 12)
 
-/* This routine uses word access.  Only usable on target voltage >2.7V */
-static const uint16_t stm32f4_flash_write_x32_stub[] = {
-#include "flashstub/stm32f4_x32.stub"
-};
-
-/* This routine uses byte access. Usable on target voltage <2.2V */
-static const uint16_t stm32f4_flash_write_x8_stub[] = {
-#include "flashstub/stm32f4_x8.stub"
-};
-
-#define SRAM_BASE 0x20000000
-#define STUB_BUFFER_BASE \
-	ALIGN(SRAM_BASE + MAX(sizeof(stm32f4_flash_write_x8_stub), \
-			      sizeof(stm32f4_flash_write_x32_stub)), 4)
-
 #define AXIM_BASE 0x8000000
 #define ITCM_BASE 0x0200000
 
 struct stm32f4_flash {
 	struct target_flash f;
+	enum align psize;
 	uint8_t base_sector;
-	uint8_t psize;
 	uint8_t bank_split;
 };
 
@@ -161,10 +148,11 @@ static void stm32f4_add_flash(target *t,
 	f->blocksize = blocksize;
 	f->erase = stm32f4_flash_erase;
 	f->write = stm32f4_flash_write;
+	f->buf_size = 1024;
 	f->erased = 0xff;
 	sf->base_sector = base_sector;
-	sf->psize = 32;
 	sf->bank_split = split;
+	sf->psize = ALIGN_WORD;
 	target_add_flash(t, f);
 }
 
@@ -417,17 +405,27 @@ static int stm32f4_flash_write(struct target_flash *f,
 	if ((dest >= ITCM_BASE) && (dest < AXIM_BASE)) {
 		dest = AXIM_BASE + (dest - ITCM_BASE);
 	}
+	target *t = f->t;
+	uint32_t sr;
+	enum align psize = ((struct stm32f4_flash *)f)->psize;
+	target_mem_write32(t, FLASH_CR,
+					   (psize * FLASH_CR_PSIZE16) | FLASH_CR_PG);
+	cortexm_mem_write_sized(t, dest, src, len, psize);
+	/* Read FLASH_SR to poll for BSY bit */
+	/* Wait for completion or an error */
+	do {
+		sr = target_mem_read32(t, FLASH_SR);
+		if(target_check_error(t)) {
+			DEBUG("stm32f4 flash write: comm error\n");
+			return -1;
+		}
+	} while (sr & FLASH_SR_BSY);
 
-	/* Write buffer to target ram call stub */
-	if (((struct stm32f4_flash *)f)->psize == 32)
-		target_mem_write(f->t, SRAM_BASE, stm32f4_flash_write_x32_stub,
-		                 sizeof(stm32f4_flash_write_x32_stub));
-	else
-		target_mem_write(f->t, SRAM_BASE, stm32f4_flash_write_x8_stub,
-		                 sizeof(stm32f4_flash_write_x8_stub));
-	target_mem_write(f->t, STUB_BUFFER_BASE, src, len);
-	return cortexm_run_stub(f->t, SRAM_BASE, dest,
-	                        STUB_BUFFER_BASE, len, 0);
+	if (sr & SR_ERROR_MASK) {
+		DEBUG("stm32f4 flash write error 0x%" PRIx32 "\n", sr);
+			return -1;
+	}
+	return 0;
 }
 
 static bool stm32f4_cmd_erase_mass(target *t)
@@ -665,22 +663,25 @@ static bool stm32f4_cmd_option(target *t, int argc, char *argv[])
 static bool stm32f4_cmd_psize(target *t, int argc, char *argv[])
 {
 	if (argc == 1) {
-		uint8_t psize = 8;
+		enum align psize = ALIGN_WORD;
 		for (struct target_flash *f = t->flash; f; f = f->next) {
 			if (f->write == stm32f4_flash_write) {
 				psize = ((struct stm32f4_flash *)f)->psize;
 			}
 		}
 		tc_printf(t, "Flash write parallelism: %s\n",
-		          psize == 32 ? "x32" : "x8");
+		          psize == ALIGN_WORD ? "x32" :
+				  psize == ALIGN_HALFWORD ? "x16" : "x8");
 	} else {
-		uint8_t psize;
+		enum align psize;
 		if (!strcmp(argv[1], "x8")) {
-			psize = 8;
+			psize = ALIGN_BYTE;
+		} else if (!strcmp(argv[1], "x16")) {
+			psize = ALIGN_HALFWORD;
 		} else if (!strcmp(argv[1], "x32")) {
-			psize = 32;
+			psize = ALIGN_WORD;
 		} else {
-			tc_printf(t, "usage: monitor psize (x8|x32)\n");
+			tc_printf(t, "usage: monitor psize (x8|x16|x32)\n");
 			return false;
 		}
 		for (struct target_flash *f = t->flash; f; f = f->next) {

From 15312eb86c422b89b83df8f113d7df3cd8c154b5 Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Thu, 15 Mar 2018 22:21:55 +0100
Subject: [PATCH 4/7] stm32f4: Honor parallelism also for erase.

---
 src/target/stm32f4.c | 8 +++++++-
 1 file changed, 7 insertions(+), 1 deletion(-)

diff --git a/src/target/stm32f4.c b/src/target/stm32f4.c
index 2701cef9..265fb7c0 100644
--- a/src/target/stm32f4.c
+++ b/src/target/stm32f4.c
@@ -369,9 +369,15 @@ static int stm32f4_flash_erase(struct target_flash *f, target_addr addr,
 	uint8_t sector = sf->base_sector + (addr - f->start)/f->blocksize;
 	stm32f4_flash_unlock(t);
 
+	enum align psize = ALIGN_WORD;
+	for (struct target_flash *f = t->flash; f; f = f->next) {
+		if (f->write == stm32f4_flash_write) {
+			psize = ((struct stm32f4_flash *)f)->psize;
+		}
+	}
 	while(len) {
 		uint32_t cr = FLASH_CR_EOPIE | FLASH_CR_ERRIE | FLASH_CR_SER |
-		              (sector << 3);
+			(psize * FLASH_CR_PSIZE16) | (sector << 3);
 		/* Flash page erase instruction */
 		target_mem_write32(t, FLASH_CR, cr);
 		/* write address to FMA */

From f1752c7a1a4386477a102fd37143d5a68269e52a Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Thu, 15 Mar 2018 22:26:59 +0100
Subject: [PATCH 5/7] stm32f4: Allow DWORD parallelism.

Needs external VPP!
---
 src/target/stm32f4.c | 7 +++++--
 1 file changed, 5 insertions(+), 2 deletions(-)

diff --git a/src/target/stm32f4.c b/src/target/stm32f4.c
index 265fb7c0..3e90baca 100644
--- a/src/target/stm32f4.c
+++ b/src/target/stm32f4.c
@@ -46,7 +46,7 @@ const struct command_s stm32f4_cmd_list[] = {
 	 "Erase entire flash memory"},
 	{"option", (cmd_handler)stm32f4_cmd_option, "Manipulate option bytes"},
 	{"psize", (cmd_handler)stm32f4_cmd_psize,
-	 "Configure flash write parallelism: (x8|x16|x32(default))"},
+	 "Configure flash write parallelism: (x8|x16|x32(default)|x64)"},
 	{NULL, NULL, NULL}
 };
 
@@ -676,6 +676,7 @@ static bool stm32f4_cmd_psize(target *t, int argc, char *argv[])
 			}
 		}
 		tc_printf(t, "Flash write parallelism: %s\n",
+		          psize == ALIGN_DWORD ? "x64" :
 		          psize == ALIGN_WORD ? "x32" :
 				  psize == ALIGN_HALFWORD ? "x16" : "x8");
 	} else {
@@ -686,8 +687,10 @@ static bool stm32f4_cmd_psize(target *t, int argc, char *argv[])
 			psize = ALIGN_HALFWORD;
 		} else if (!strcmp(argv[1], "x32")) {
 			psize = ALIGN_WORD;
+		} else if (!strcmp(argv[1], "x64")) {
+			psize = ALIGN_DWORD;
 		} else {
-			tc_printf(t, "usage: monitor psize (x8|x16|x32)\n");
+			tc_printf(t, "usage: monitor psize (x8|x16|x32|x32)\n");
 			return false;
 		}
 		for (struct target_flash *f = t->flash; f; f = f->next) {

From 891d6de8eb164ae6c03095d7129886f7e843c2a8 Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Thu, 15 Mar 2018 18:27:02 +0100
Subject: [PATCH 6/7] stm32f1.c: Use buffered direct write to flash with half
 word access.

---
 src/target/flashstub/Makefile     |  5 +---
 src/target/flashstub/stm32f1.c    | 40 -------------------------------
 src/target/flashstub/stm32f1.stub |  1 -
 src/target/stm32f1.c              | 32 +++++++++++++++----------
 4 files changed, 21 insertions(+), 57 deletions(-)
 delete mode 100644 src/target/flashstub/stm32f1.c
 delete mode 100644 src/target/flashstub/stm32f1.stub

diff --git a/src/target/flashstub/Makefile b/src/target/flashstub/Makefile
index d6abdd1e..abc03a4e 100644
--- a/src/target/flashstub/Makefile
+++ b/src/target/flashstub/Makefile
@@ -11,10 +11,7 @@ endif
 CFLAGS=-Os -std=gnu99 -mcpu=cortex-m0 -mthumb -I../../../libopencm3/include
 ASFLAGS=-mcpu=cortex-m3 -mthumb
 
-all:	lmi.stub stm32l4.stub nrf51.stub \
-	stm32f1.stub efm32.stub
-
-stm32f1.o: CFLAGS += -DSTM32F1
+all:	lmi.stub stm32l4.stub nrf51.stub efm32.stub
 
 %.o:    %.c
 	$(Q)echo "  CC      $<"
diff --git a/src/target/flashstub/stm32f1.c b/src/target/flashstub/stm32f1.c
deleted file mode 100644
index f9ba0a15..00000000
--- a/src/target/flashstub/stm32f1.c
+++ /dev/null
@@ -1,40 +0,0 @@
-/*
- * This file is part of the Black Magic Debug project.
- *
- * Copyright (C) 2015  Black Sphere Technologies Ltd.
- * Written by Gareth McMullin <gareth@blacksphere.co.nz>
- *
- * 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/>.
- */
-#include "libopencm3/stm32/flash.h"
-#include "stub.h"
-
-#define SR_ERROR_MASK 0x14
-
-void __attribute__((naked))
-stm32f1_flash_write_stub(uint16_t *dest, uint16_t *src, uint32_t size)
-{
-	for (int i; i < size; i += 2) {
-		FLASH_CR = FLASH_CR_PG;
-		*dest++ = *src++;
-		while (FLASH_SR & FLASH_SR_BSY)
-			;
-	}
-
-	if (FLASH_SR & SR_ERROR_MASK)
-		stub_exit(1);
-
-	stub_exit(0);
-}
-
diff --git a/src/target/flashstub/stm32f1.stub b/src/target/flashstub/stm32f1.stub
deleted file mode 100644
index 3fe7cd03..00000000
--- a/src/target/flashstub/stm32f1.stub
+++ /dev/null
@@ -1 +0,0 @@
-0x2300, 0x4C09, 0x4293, 0xD209, 0x2601, 0x4D08, 0x602E, 0x5ACD, 0x52C5, 0x6825, 0x07ED, 0xD4FC, 0x3302, 0xE7F2, 0x2314, 0x6822, 0x421A, 0xD000, 0xBE01, 0xBE00, 0x200C, 0x4002, 0x2010, 0x4002,
\ No newline at end of file
diff --git a/src/target/stm32f1.c b/src/target/stm32f1.c
index fcdc0c8e..3394a0d4 100644
--- a/src/target/stm32f1.c
+++ b/src/target/stm32f1.c
@@ -72,6 +72,7 @@ static int stm32f1_flash_write(struct target_flash *f,
 #define FLASH_CR_OPTPG	(1 << 4)
 #define FLASH_CR_MER	(1 << 2)
 #define FLASH_CR_PER	(1 << 1)
+#define FLASH_CR_PG		(1 << 0)
 
 #define FLASH_OBR_RDPRT (1 << 1)
 
@@ -93,13 +94,6 @@ static int stm32f1_flash_write(struct target_flash *f,
 #define FLASHSIZE     0x1FFFF7E0
 #define FLASHSIZE_F0  0x1FFFF7CC
 
-static const uint16_t stm32f1_flash_write_stub[] = {
-#include "flashstub/stm32f1.stub"
-};
-
-#define SRAM_BASE 0x20000000
-#define STUB_BUFFER_BASE ALIGN(SRAM_BASE + sizeof(stm32f1_flash_write_stub), 4)
-
 static void stm32f1_add_flash(target *t,
                               uint32_t addr, size_t length, size_t erasesize)
 {
@@ -109,6 +103,7 @@ static void stm32f1_add_flash(target *t,
 	f->blocksize = erasesize;
 	f->erase = stm32f1_flash_erase;
 	f->write = stm32f1_flash_write;
+	f->buf_size = erasesize;
 	f->erased = 0xff;
 	target_add_flash(t, f);
 }
@@ -226,11 +221,24 @@ static int stm32f1_flash_write(struct target_flash *f,
                                target_addr dest, const void *src, size_t len)
 {
 	target *t = f->t;
-	/* Write stub and data to target ram and set PC */
-	target_mem_write(t, SRAM_BASE, stm32f1_flash_write_stub,
-	                 sizeof(stm32f1_flash_write_stub));
-	target_mem_write(t, STUB_BUFFER_BASE, src, len);
-	return cortexm_run_stub(t, SRAM_BASE, dest, STUB_BUFFER_BASE, len, 0);
+	uint32_t sr;
+	target_mem_write32(t, FLASH_CR, FLASH_CR_PG);
+	cortexm_mem_write_sized(t, dest, src, len, ALIGN_HALFWORD);
+	/* Read FLASH_SR to poll for BSY bit */
+	/* Wait for completion or an error */
+	do {
+		sr = target_mem_read32(t, FLASH_SR);
+		if(target_check_error(t)) {
+			DEBUG("stm32f1 flash write: comm error\n");
+			return -1;
+		}
+	} while (sr & FLASH_SR_BSY);
+
+	if (sr & SR_ERROR_MASK) {
+		DEBUG("stm32f1 flash write error 0x%" PRIx32 "\n", sr);
+			return -1;
+	}
+	return 0;
 }
 
 static bool stm32f1_cmd_erase_mass(target *t)

From b59bbac0b24d70513d6d9bf92b06d7202ea054ad Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Thu, 15 Mar 2018 21:04:47 +0100
Subject: [PATCH 7/7] stm32l4: Use buffered direct write to flash.

---
 src/target/flashstub/stm32l4.c    | 53 -------------------------------
 src/target/flashstub/stm32l4.stub |  1 -
 src/target/stm32l4.c              | 34 +++++++++++---------
 3 files changed, 19 insertions(+), 69 deletions(-)
 delete mode 100644 src/target/flashstub/stm32l4.c
 delete mode 100644 src/target/flashstub/stm32l4.stub

diff --git a/src/target/flashstub/stm32l4.c b/src/target/flashstub/stm32l4.c
deleted file mode 100644
index 820bca4e..00000000
--- a/src/target/flashstub/stm32l4.c
+++ /dev/null
@@ -1,53 +0,0 @@
-/*
- * This file is part of the Black Magic Debug project.
- *
- * Copyright (C) 2015  Black Sphere Technologies Ltd.
- * Written by Gareth McMullin <gareth@blacksphere.co.nz>
- *
- * 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/>.
- */
-#include "stub.h"
-#include <stdint.h>
-
-/* No STM32L4 definitions in libopencm3 yet */
-#define FLASH_SR ((volatile uint32_t *) 0x40022010)
-#define FLASH_SR_EOP		(1 << 0)
-#define SR_ERROR_MASK		0xC3FA
-#define FLASH_SR_BSY		(1 << 16)
-
-#define FLASH_CR ((volatile uint32_t *) 0x40022014)
-#define FLASH_CR_PG			(1 << 0)
-#define FLASH_CR_EOPIE		(1 << 24)
-#define FLASH_CR_ERRIE		(1 << 25)
-#define FLASH_SR_EOP		(1 << 0)
-
-void __attribute__((naked))
-stm32l4_flash_write_stub(uint32_t *dest, uint32_t *src, uint32_t size)
-{
-	if ((size & 7) || ((uint32_t)dest & 7))
-		stub_exit(1);
-	for (int i = 0; i < size; i += 8) {
-		*FLASH_CR =  FLASH_CR_EOPIE | FLASH_CR_ERRIE | FLASH_CR_PG;
-		*dest++ = *src++;
-		*dest++ = *src++;
-		__asm("dsb");
-		while (*FLASH_SR & FLASH_SR_BSY)
-			;
-		if ((*FLASH_SR & SR_ERROR_MASK) || !(*FLASH_SR & FLASH_SR_EOP))
-			stub_exit(1);
-		*FLASH_SR |= FLASH_SR_EOP;
-	}
-	*FLASH_CR = 0;
-	stub_exit(0);
-}
diff --git a/src/target/flashstub/stm32l4.stub b/src/target/flashstub/stm32l4.stub
deleted file mode 100644
index 73cd969d..00000000
--- a/src/target/flashstub/stm32l4.stub
+++ /dev/null
@@ -1 +0,0 @@
-0x1C04, 0x4314, 0x2300, 0x0764, 0xD011, 0xBE01, 0xE00F, 0x4C11, 0x6825, 0x03ED, 0xD4FB, 0x6826, 0x4D0F, 0x422E, 0xD115, 0x6825, 0x07ED, 0xD512, 0x2601, 0x6825, 0x3308, 0x4335, 0x6025, 0x4C0B, 0x4293, 0xD20C, 0x4D0A, 0x6025, 0x58CC, 0x50C4, 0x18CC, 0x6865, 0x18C4, 0x6065, 0xF3BF, 0x8F4F, 0xE7E1, 0xBE01, 0xE7EA, 0x2300, 0x6023, 0xBE00, 0x2010, 0x4002, 0xC3FA, 0x0000, 0x2014, 0x4002, 0x0001, 0x0300,
\ No newline at end of file
diff --git a/src/target/stm32l4.c b/src/target/stm32l4.c
index f765599c..f67b8588 100644
--- a/src/target/stm32l4.c
+++ b/src/target/stm32l4.c
@@ -1,7 +1,7 @@
 /*
  * This file is part of the Black Magic Debug project.
  *
- * Copyright (C) 2015, 2017  Uwe Bonnes
+ * Copyright (C) 2015, 2017, 2018  Uwe Bonnes
  * Written by Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
  *
  * This program is free software: you can redistribute it and/or modify
@@ -112,14 +112,6 @@ static const char stm32l4_driver_str[] = "STM32L4xx";
 #define DBGMCU_IDCODE	0xE0042000
 #define FLASH_SIZE_REG  0x1FFF75E0
 
-/* This routine is uses double word access.*/
-static const uint16_t stm32l4_flash_write_stub[] = {
-#include "flashstub/stm32l4.stub"
-};
-
-#define SRAM_BASE 0x20000000
-#define STUB_BUFFER_BASE ALIGN(SRAM_BASE + sizeof(stm32l4_flash_write_stub), 8)
-
 struct stm32l4_flash {
 	struct target_flash f;
 	uint32_t bank1_start;
@@ -258,12 +250,24 @@ static int stm32l4_flash_erase(struct target_flash *f, target_addr addr, size_t
 static int stm32l4_flash_write(struct target_flash *f,
                                target_addr dest, const void *src, size_t len)
 {
-	/* Write buffer to target ram call stub */
-	target_mem_write(f->t, SRAM_BASE, stm32l4_flash_write_stub,
-	                 sizeof(stm32l4_flash_write_stub));
-	target_mem_write(f->t, STUB_BUFFER_BASE, src, len);
-	return cortexm_run_stub(f->t, SRAM_BASE, dest,
-	                        STUB_BUFFER_BASE, len, 0);
+	target *t = f->t;
+	target_mem_write32(t, FLASH_CR, FLASH_CR_PG);
+	target_mem_write(t, dest, src, len);
+	/* Wait for completion or an error */
+	uint32_t sr;
+	do {
+		sr = target_mem_read32(t, FLASH_SR);
+		if (target_check_error(t)) {
+			DEBUG("stm32l4 flash write: comm error\n");
+			return -1;
+		}
+	} while (sr & FLASH_SR_BSY);
+
+	if(sr & FLASH_SR_ERROR_MASK) {
+		DEBUG("stm32l4 flash write error: sr 0x%" PRIu32 "\n", sr);
+		return -1;
+	}
+	return 0;
 }
 
 static bool stm32l4_cmd_erase(target *t, uint32_t action)