flash: New Spansion FM4 flash driver

The Spansion FM4 family of microcontrollers does not offer a way to
identify the chip model nor the flash size, except for Dual Flash vs.
regular layout. Therefore the family is passed as argument and
wildcard-matched - MB9BFx6x and S6E2CC families are supported.

Iterations showed that ...
1) Just doing the flash command sequence from SRAM loader code for each
half-word took 20 minutes for an 8 KB block.
2) Doing the busy-wait in the loader merely reduced the time to 19 minutes.
3) Significant performance gains were achieved by looping in loader code
rather than in OpenOCD and by maximizing the batch size across sectors,
getting us down to ~2 seconds for 8 KB and ~2.5 minutes for 1.1 MB.
(Tested with SK-FM4-176L-S6E2CC-ETH v11, CMSIS-DAP v23.)

gcc, objcopy -Obinary and bin2char.sh are used for automating the
integration of hand-written assembler snippets.

Change-Id: I092c81074662534f50b71b91d54eb8e0098fec76
Signed-off-by: Andreas Färber <afaerber@suse.de>
Reviewed-on: http://openocd.zylin.com/2190
Tested-by: jenkins
Reviewed-by: Spencer Oliver <spen@spen-soft.co.uk>

15 files changed, 918 insertions, 1 deletions

diff --git a/README b/README
index 8f98e7f..a6b4c5b 100644
--- a/README
+++ b/README
@@ -126,7 +126,7 @@ XScale, Intel Quark.
 Flash drivers
 -------------
 
-ADUC702x, AT91SAM, AVR, CFI, DSP5680xx, EFM32, EM357, FM3, Kinetis,
+ADUC702x, AT91SAM, AVR, CFI, DSP5680xx, EFM32, EM357, FM3, FM4, Kinetis,
 LPC8xx/LPC1xxx/LPC2xxx/LPC541xx, LPC2900, LPCSPIFI, Marvell QSPI,
 Milandr, NIIET, NuMicro, PIC32mx, PSoC4, SiM3x, Stellaris, STM32, STMSMI,
 STR7x, STR9x, nRF51; NAND controllers of AT91SAM9, LPC3180, LPC32xx,
diff --git a/contrib/loaders/flash/fm4/Makefile b/contrib/loaders/flash/fm4/Makefile
new file mode 100644
index 0000000..eb4cbd0
--- /dev/null
+++ b/contrib/loaders/flash/fm4/Makefile
@@ -0,0 +1,30 @@
+BIN2C = ../../../../src/helper/bin2char.sh
+
+CROSS_COMPILE ?= arm-none-eabi-
+
+CC=$(CROSS_COMPILE)gcc
+OBJCOPY=$(CROSS_COMPILE)objcopy
+OBJDUMP=$(CROSS_COMPILE)objdump
+
+all: erase.inc write.inc
+
+.PHONY: clean
+
+.INTERMEDIATE: erase.elf write.elf
+
+erase.elf write.elf: fm4.h
+
+%.elf: %.S
+	$(CC) -static -nostartfiles $< -o $@
+
+%.lst: %.elf
+	$(OBJDUMP) -S $< > $@
+
+%.bin: %.elf
+	$(OBJCOPY) -Obinary $< $@
+
+%.inc: %.bin
+	$(BIN2C) < $< > $@
+
+clean:
+	-rm -f *.elf *.lst *.bin *.inc
diff --git a/contrib/loaders/flash/fm4/erase.S b/contrib/loaders/flash/fm4/erase.S
new file mode 100644
index 0000000..6fdf81d
--- /dev/null
+++ b/contrib/loaders/flash/fm4/erase.S
@@ -0,0 +1,77 @@
+/*
+ * Spansion FM4 flash sector erase algorithm
+ *
+ * Copyright (c) 2015 Andreas Färber
+ *
+ * Based on S6E2CC_MN709-00007 for S6E2CC/C5/C4/C3/C2/C1 series
+ */
+
+#include "fm4.h"
+
+#define RESULT_OKAY	0
+#define RESULT_NONE	1
+#define RESULT_TIMEOUT	2
+
+	.macro busy_wait, res, addr, tmp1, tmp2, tmp3
+
+	ldrb	\tmp1, [\addr] /* ignore */
+1001:
+	ldrb	\tmp1, [\addr]
+	ldrb	\tmp2, [\addr]
+
+	and	\tmp3, \tmp1, #FLASH_TOGG
+	and	\tmp2, \tmp2, #FLASH_TOGG
+	cmp	\tmp3, \tmp2
+	beq	1010f
+
+	and	\tmp2, \tmp1, #FLASH_TLOV
+	cmp	\tmp2, #0
+	beq	1001b
+
+	ldrb	\tmp1, [\addr]
+	ldrb	\tmp2, [\addr]
+
+	and	\tmp3, \tmp1, #FLASH_TOGG
+	and	\tmp2, \tmp2, #FLASH_TOGG
+	cmp	\tmp3, \tmp2
+	beq	1010f
+
+	mov	\res, #RESULT_TIMEOUT
+	bkpt	#0
+1010:
+	mov	\res, #RESULT_OKAY
+
+	.endm
+
+
+	.macro erase, cmdseqaddr1, cmdseqaddr2, sa, res, tmp1, tmp2, tmp3
+
+	mov	\res, #RESULT_NONE
+
+	mov	\tmp1, #0xAA
+	strh	\tmp1, [\cmdseqaddr1]
+	mov	\tmp2, #0x55
+	strh	\tmp2, [\cmdseqaddr2]
+	mov	\tmp3, #0x80
+	strh	\tmp3, [\cmdseqaddr1]
+	strh	\tmp1, [\cmdseqaddr1]
+	strh	\tmp2, [\cmdseqaddr2]
+	mov	\tmp3, #0x30
+	strh	\tmp3, [\sa]
+
+	busy_wait \res, \sa, \tmp1, \tmp2, \tmp3
+
+	.endm
+
+
+	/* r0 = 0xAA8
+	 * r1 = 0x554
+	 * r2 = SA
+	 * r3 = result
+	 */
+erase:
+	erase r0, r1, r2, r3, r4, r5, r6
+
+	bkpt	#0
+
+data:
diff --git a/contrib/loaders/flash/fm4/erase.inc b/contrib/loaders/flash/fm4/erase.inc
new file mode 100644
index 0000000..9f38067
--- /dev/null
+++ b/contrib/loaders/flash/fm4/erase.inc
@@ -0,0 +1,7 @@
+/* Autogenerated with ../../../../src/helper/bin2char.sh */
+0x4f,0xf0,0x01,0x03,0x4f,0xf0,0xaa,0x04,0x04,0x80,0x4f,0xf0,0x55,0x05,0x0d,0x80,
+0x4f,0xf0,0x80,0x06,0x06,0x80,0x04,0x80,0x0d,0x80,0x4f,0xf0,0x30,0x06,0x16,0x80,
+0x14,0x78,0x14,0x78,0x15,0x78,0x04,0xf0,0x40,0x06,0x05,0xf0,0x40,0x05,0xae,0x42,
+0x0e,0xd0,0x04,0xf0,0x20,0x05,0x00,0x2d,0xf3,0xd0,0x14,0x78,0x15,0x78,0x04,0xf0,
+0x40,0x06,0x05,0xf0,0x40,0x05,0xae,0x42,0x02,0xd0,0x4f,0xf0,0x02,0x03,0x00,0xbe,
+0x4f,0xf0,0x00,0x03,0x00,0xbe,
diff --git a/contrib/loaders/flash/fm4/fm4.h b/contrib/loaders/flash/fm4/fm4.h
new file mode 100644
index 0000000..603aac8
--- /dev/null
+++ b/contrib/loaders/flash/fm4/fm4.h
@@ -0,0 +1,19 @@
+/*
+ * Spansion FM4 flash macros
+ *
+ * Copyright (c) 2015 Andreas Färber
+ *
+ * Based on S6E2CC_MN709-00007 for S6E2CC/C5/C4/C3/C2/C1 series
+ */
+
+	.text
+	.syntax unified
+	.cpu cortex-m4
+	.thumb
+	.thumb_func
+
+
+#define FLASH_DPOL	(1 << 7)
+#define FLASH_TOGG	(1 << 6)
+#define FLASH_TLOV	(1 << 5)
+#define FLASH_TOGG2	(1 << 2)
diff --git a/contrib/loaders/flash/fm4/write.S b/contrib/loaders/flash/fm4/write.S
new file mode 100644
index 0000000..a8d01cd
--- /dev/null
+++ b/contrib/loaders/flash/fm4/write.S
@@ -0,0 +1,85 @@
+/*
+ * Spansion FM4 flash write algorithm
+ *
+ * Copyright (c) 2015 Andreas Färber
+ *
+ * Based on S6E2CC_MN709-00007 for S6E2CC/C5/C4/C3/C2/C1 series
+ */
+
+#include "fm4.h"
+
+#define RESULT_OKAY	0
+#define RESULT_NONE	1
+#define RESULT_TIMEOUT	2
+
+	.macro busy_wait, res, addr, data, tmp1, tmp2, tmp3
+
+	ldrb	\tmp1, [\addr] /* ignore */
+	and	\tmp2, \data, #FLASH_DPOL
+1001:
+	ldrb	\tmp1, [\addr]
+	and	\tmp3, \tmp1, #FLASH_DPOL
+	cmp	\tmp3, \tmp2
+	beq	1010f
+
+	and	\tmp3, \tmp1, #FLASH_TLOV
+	cmp	\tmp3, #0
+	beq	1001b
+
+	ldrb	\tmp1, [\addr]
+	and	\tmp3, \tmp1, #FLASH_DPOL
+	cmp	\tmp3, \tmp2
+	beq	1010f
+
+	mov	\res, #RESULT_TIMEOUT
+	bkpt	#0
+1010:
+	.endm
+
+
+	.macro write_one, res, cmdseqaddr1, cmdseqaddr2, pa, pd, tmp1, tmp2, tmp3
+
+	mov	\tmp1, #0xAA
+	strh	\tmp1, [\cmdseqaddr1]
+	mov	\tmp1, #0x55
+	strh	\tmp1, [\cmdseqaddr2]
+	mov	\tmp1, #0xA0
+	strh	\tmp1, [\cmdseqaddr1]
+	strh	\pd, [\pa]
+
+	busy_wait \res, \pa, \pd, \tmp1, \tmp2, \tmp3
+
+	.endm
+
+
+	.macro write, cmdseqaddr1, cmdseqaddr2, dest, src, cnt, res, tmp1, tmp2, tmp3, tmp4
+
+	mov	\res, #RESULT_NONE
+2001:
+	cbz	\cnt, 2010f
+
+	ldrh	\tmp1, [\src]
+	write_one \res, \cmdseqaddr1, \cmdseqaddr2, \dest, \tmp1, \tmp2, \tmp3, \tmp4
+
+	sub	\cnt, \cnt, #1
+	add	\dest, \dest, #2
+	add	\src, \src, #2
+	b	2001b
+2010:
+	mov	\res, #RESULT_OKAY
+	.endm
+
+
+	/* r0 = 0xAA8
+	 * r1 = 0x554
+	 * r2 = dest
+	 * r3 = src
+	 * r4 = cnt
+	 * r5 = result
+	 */
+write:
+	write r0, r1, r2, r3, r4, r5, r6, r7, r8, r9
+
+	bkpt	#0
+
+data:
diff --git a/contrib/loaders/flash/fm4/write.inc b/contrib/loaders/flash/fm4/write.inc
new file mode 100644
index 0000000..3d8472b
--- /dev/null
+++ b/contrib/loaders/flash/fm4/write.inc
@@ -0,0 +1,7 @@
+/* Autogenerated with ../../../../src/helper/bin2char.sh */
+0x4f,0xf0,0x01,0x05,0x34,0xb3,0x1e,0x88,0x4f,0xf0,0xaa,0x07,0x07,0x80,0x4f,0xf0,
+0x55,0x07,0x0f,0x80,0x4f,0xf0,0xa0,0x07,0x07,0x80,0x16,0x80,0x17,0x78,0x06,0xf0,
+0x80,0x08,0x17,0x78,0x07,0xf0,0x80,0x09,0xc1,0x45,0x0c,0xd0,0x07,0xf0,0x20,0x09,
+0xb9,0xf1,0x00,0x0f,0xf5,0xd0,0x17,0x78,0x07,0xf0,0x80,0x09,0xc1,0x45,0x02,0xd0,
+0x4f,0xf0,0x02,0x05,0x00,0xbe,0xa4,0xf1,0x01,0x04,0x02,0xf1,0x02,0x02,0x03,0xf1,
+0x02,0x03,0xd7,0xe7,0x4f,0xf0,0x00,0x05,0x00,0xbe,
diff --git a/doc/openocd.texi b/doc/openocd.texi
index d1f7855..e131f1e 100644
--- a/doc/openocd.texi
+++ b/doc/openocd.texi
@@ -5240,6 +5240,24 @@ Command disables watchdog timer.
 @end deffn
 @end deffn
 
+@deffn {Flash Driver} fm4
+All members of the FM4 microcontroller family from Spansion (formerly Fujitsu)
+include internal flash and use ARM Cortex-M4 cores.
+The @var{fm4} driver uses a @var{family} parameter to select the
+correct bank config, it can currently be one of the following:
+@code{MB9BFx66}, @code{MB9BFx67}, @code{MB9BFx68},
+@code{S6E2Cx8}, @code{S6E2Cx9} or @code{S6E2CxA},
+with @code{x} treated as wildcard and otherwise case (and any trailing
+characters) ignored.
+
+@example
+flash bank $@{_FLASHNAME@}0 fm4 0x00000000 0 0 0 $_TARGETNAME S6E2CCAJ0A
+flash bank $@{_FLASHNAME@}1 fm4 0x00100000 0 0 0 $_TARGETNAME S6E2CCAJ0A
+@end example
+@emph{The current implementation is incomplete. Protection is not supported,
+nor is Chip Erase (only Sector Erase is implemented).}
+@end deffn
+
 @deffn {Flash Driver} lpc2000
 This is the driver to support internal flash of all members of the
 LPC11(x)00 and LPC1300 microcontroller families and most members of
diff --git a/src/flash/nor/Makefile.am b/src/flash/nor/Makefile.am
index 834e4d4..c2a9d0c 100644
--- a/src/flash/nor/Makefile.am
+++ b/src/flash/nor/Makefile.am
@@ -43,6 +43,7 @@ NOR_DRIVERS = \
 	tms470.c \
 	virtual.c \
 	fm3.c \
+	fm4.c \
 	dsp5680xx_flash.c \
 	kinetis.c \
 	numicro.c \
diff --git a/src/flash/nor/drivers.c b/src/flash/nor/drivers.c
index 48d584e..b2f05b8 100644
--- a/src/flash/nor/drivers.c
+++ b/src/flash/nor/drivers.c
@@ -53,6 +53,7 @@ extern struct flash_driver stmsmi_flash;
 extern struct flash_driver em357_flash;
 extern struct flash_driver dsp5680xx_flash;
 extern struct flash_driver fm3_flash;
+extern struct flash_driver fm4_flash;
 extern struct flash_driver kinetis_flash;
 extern struct flash_driver efm32_flash;
 extern struct flash_driver mdr_flash;
@@ -100,6 +101,7 @@ static struct flash_driver *flash_drivers[] = {
 	&stmsmi_flash,
 	&em357_flash,
 	&fm3_flash,
+	&fm4_flash,
 	&dsp5680xx_flash,
 	&kinetis_flash,
 	&efm32_flash,
diff --git a/src/flash/nor/fm4.c b/src/flash/nor/fm4.c
new file mode 100644
index 0000000..917ff01
--- /dev/null
+++ b/src/flash/nor/fm4.c
@@ -0,0 +1,662 @@
+/*
+ * Spansion FM4 flash
+ *
+ * Copyright (c) 2015 Andreas Färber
+ *
+ * Based on S6E2CC_MN709-00007 for S6E2CC/C5/C4/C3/C2/C1 series
+ * Based on MB9B560R_MN709-00005 for MB9BFx66/x67/x68 series
+ */
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "imp.h"
+#include <helper/binarybuffer.h>
+#include <target/algorithm.h>
+#include <target/armv7m.h>
+
+#define FLASH_BASE 0x40000000
+#define FASZR   (FLASH_BASE + 0x000)
+#define DFCTRLR (FLASH_BASE + 0x030)
+#define DFCTRLR_DFE (1UL << 0)
+
+#define WDG_BASE 0x40011000
+#define WDG_CTL (WDG_BASE + 0x008)
+#define WDG_LCK (WDG_BASE + 0xC00)
+
+enum fm4_variant {
+	mb9bfx66,
+	mb9bfx67,
+	mb9bfx68,
+
+	s6e2cx8,
+	s6e2cx9,
+	s6e2cxa,
+};
+
+struct fm4_flash_bank {
+	enum fm4_variant variant;
+	int macro_nr;
+	bool probed;
+};
+
+static int fm4_disable_hw_watchdog(struct target *target)
+{
+	int retval;
+
+	retval = target_write_u32(target, WDG_LCK, 0x1ACCE551);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = target_write_u32(target, WDG_LCK, 0xE5331AAE);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = target_write_u32(target, WDG_CTL, 0);
+	if (retval != ERROR_OK)
+		return retval;
+
+	return ERROR_OK;
+}
+
+static int fm4_enter_flash_cpu_programming_mode(struct target *target)
+{
+	uint32_t u32_value;
+	int retval;
+
+	/* FASZR ASZ = CPU programming mode */
+	retval = target_write_u32(target, FASZR, 0x00000001);
+	if (retval != ERROR_OK)
+		return retval;
+	retval = target_read_u32(target, FASZR, &u32_value);
+	if (retval != ERROR_OK)
+		return retval;
+
+	return ERROR_OK;
+}
+
+static int fm4_enter_flash_cpu_rom_mode(struct target *target)
+{
+	uint32_t u32_value;
+	int retval;
+
+	/* FASZR ASZ = CPU ROM mode */
+	retval = target_write_u32(target, FASZR, 0x00000002);
+	if (retval != ERROR_OK)
+		return retval;
+	retval = target_read_u32(target, FASZR, &u32_value);
+	if (retval != ERROR_OK)
+		return retval;
+
+	return ERROR_OK;
+}
+
+static int fm4_flash_erase(struct flash_bank *bank, int first, int last)
+{
+	struct target *target = bank->target;
+	struct working_area *workarea;
+	struct reg_param reg_params[4];
+	struct armv7m_algorithm armv7m_algo;
+	unsigned i;
+	int retval, sector;
+	const uint8_t erase_sector_code[] = {
+#include "../../../contrib/loaders/flash/fm4/erase.inc"
+	};
+
+	if (target->state != TARGET_HALTED) {
+		LOG_WARNING("Cannot communicate... target not halted.");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	LOG_DEBUG("Spansion FM4 erase sectors %d to %d", first, last);
+
+	retval = fm4_disable_hw_watchdog(target);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = fm4_enter_flash_cpu_programming_mode(target);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = target_alloc_working_area(target, sizeof(erase_sector_code),
+			&workarea);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("No working area available.");
+		retval = ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+		goto err_alloc_code;
+	}
+	retval = target_write_buffer(target, workarea->address,
+			sizeof(erase_sector_code), erase_sector_code);
+	if (retval != ERROR_OK)
+		goto err_write_code;
+
+	armv7m_algo.common_magic = ARMV7M_COMMON_MAGIC;
+	armv7m_algo.core_mode = ARM_MODE_THREAD;
+
+	init_reg_param(&reg_params[0], "r0", 32, PARAM_OUT);
+	init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);
+	init_reg_param(&reg_params[2], "r2", 32, PARAM_OUT);
+	init_reg_param(&reg_params[3], "r3", 32, PARAM_IN);
+
+	for (sector = first; sector <= last; sector++) {
+		uint32_t addr = bank->base + bank->sectors[sector].offset;
+		uint32_t result;
+
+		buf_set_u32(reg_params[0].value, 0, 32, (addr & ~0xffff) | 0xAA8);
+		buf_set_u32(reg_params[1].value, 0, 32, (addr & ~0xffff) | 0x554);
+		buf_set_u32(reg_params[2].value, 0, 32, addr);
+
+		retval = target_run_algorithm(target,
+				0, NULL,
+				ARRAY_SIZE(reg_params), reg_params,
+				workarea->address, 0,
+				1000, &armv7m_algo);
+		if (retval != ERROR_OK) {
+			LOG_ERROR("Error executing flash sector erase "
+				"programming algorithm");
+			retval = ERROR_FLASH_OPERATION_FAILED;
+			goto err_run;
+		}
+
+		result = buf_get_u32(reg_params[3].value, 0, 32);
+		if (result == 2) {
+			LOG_ERROR("Timeout error from flash sector erase programming algorithm");
+			retval = ERROR_FLASH_OPERATION_FAILED;
+			goto err_run_ret;
+		} else if (result != 0) {
+			LOG_ERROR("Unexpected error %d from flash sector erase programming algorithm", result);
+			retval = ERROR_FLASH_OPERATION_FAILED;
+			goto err_run_ret;
+		} else
+			retval = ERROR_OK;
+
+		bank->sectors[sector].is_erased = 1;
+	}
+
+err_run_ret:
+err_run:
+	for (i = 0; i < ARRAY_SIZE(reg_params); i++)
+		destroy_reg_param(&reg_params[i]);
+
+err_write_code:
+	target_free_working_area(target, workarea);
+
+err_alloc_code:
+	if (retval != ERROR_OK)
+		fm4_enter_flash_cpu_rom_mode(target);
+	else
+		retval = fm4_enter_flash_cpu_rom_mode(target);
+
+	return retval;
+}
+
+static int fm4_flash_write(struct flash_bank *bank, const uint8_t *buffer,
+		uint32_t offset, uint32_t byte_count)
+{
+	struct target *target = bank->target;
+	struct working_area *code_workarea, *data_workarea;
+	struct reg_param reg_params[6];
+	struct armv7m_algorithm armv7m_algo;
+	uint32_t halfword_count = DIV_ROUND_UP(byte_count, 2);
+	uint32_t result;
+	unsigned i;
+	int retval;
+	const uint8_t write_block_code[] = {
+#include "../../../contrib/loaders/flash/fm4/write.inc"
+	};
+
+	LOG_DEBUG("Spansion FM4 write at 0x%08" PRIx32 " (%" PRId32 " bytes)",
+		offset, byte_count);
+
+	if (offset & 0x1) {
+		LOG_ERROR("offset 0x%" PRIx32 " breaks required 2-byte alignment",
+			offset);
+		return ERROR_FLASH_DST_BREAKS_ALIGNMENT;
+	}
+	if (byte_count & 0x1) {
+		LOG_WARNING("length %" PRId32 " is not 2-byte aligned, rounding up",
+			byte_count);
+	}
+
+	if (target->state != TARGET_HALTED) {
+		LOG_WARNING("Cannot communicate... target not halted.");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	retval = fm4_disable_hw_watchdog(target);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = target_alloc_working_area(target, sizeof(write_block_code),
+			&code_workarea);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("No working area available for write code.");
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	}
+	retval = target_write_buffer(target, code_workarea->address,
+			sizeof(write_block_code), write_block_code);
+	if (retval != ERROR_OK)
+		goto err_write_code;
+
+	retval = target_alloc_working_area(target,
+		MIN(halfword_count * 2, target_get_working_area_avail(target)),
+		&data_workarea);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("No working area available for write data.");
+		retval = ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+		goto err_alloc_data;
+	}
+
+	armv7m_algo.common_magic = ARMV7M_COMMON_MAGIC;
+	armv7m_algo.core_mode = ARM_MODE_THREAD;
+
+	init_reg_param(&reg_params[0], "r0", 32, PARAM_OUT);
+	init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);
+	init_reg_param(&reg_params[2], "r2", 32, PARAM_OUT);
+	init_reg_param(&reg_params[3], "r3", 32, PARAM_OUT);
+	init_reg_param(&reg_params[4], "r4", 32, PARAM_OUT);
+	init_reg_param(&reg_params[5], "r5", 32, PARAM_IN);
+
+	retval = fm4_enter_flash_cpu_programming_mode(target);
+	if (retval != ERROR_OK)
+		goto err_flash_mode;
+
+	while (byte_count > 0) {
+		uint32_t halfwords = MIN(halfword_count, data_workarea->size / 2);
+		uint32_t addr = bank->base + offset;
+
+		LOG_DEBUG("copying %" PRId32 " bytes to SRAM 0x%08" PRIx32,
+			MIN(halfwords * 2, byte_count), data_workarea->address);
+
+		retval = target_write_buffer(target, data_workarea->address,
+			MIN(halfwords * 2, byte_count), buffer);
+		if (retval != ERROR_OK) {
+			LOG_ERROR("Error writing data buffer");
+			retval = ERROR_FLASH_OPERATION_FAILED;
+			goto err_write_data;
+		}
+
+		LOG_DEBUG("writing 0x%08" PRIx32 "-0x%08" PRIx32 " (%" PRId32 "x)",
+			addr, addr + halfwords * 2 - 1, halfwords);
+
+		buf_set_u32(reg_params[0].value, 0, 32, (addr & ~0xffff) | 0xAA8);
+		buf_set_u32(reg_params[1].value, 0, 32, (addr & ~0xffff) | 0x554);
+		buf_set_u32(reg_params[2].value, 0, 32, addr);
+		buf_set_u32(reg_params[3].value, 0, 32, data_workarea->address);
+		buf_set_u32(reg_params[4].value, 0, 32, halfwords);
+
+		retval = target_run_algorithm(target,
+				0, NULL,
+				ARRAY_SIZE(reg_params), reg_params,
+				code_workarea->address, 0,
+				5 * 60 * 1000, &armv7m_algo);
+		if (retval != ERROR_OK) {
+			LOG_ERROR("Error executing flash sector erase "
+				"programming algorithm");
+			retval = ERROR_FLASH_OPERATION_FAILED;
+			goto err_run;
+		}
+
+		result = buf_get_u32(reg_params[5].value, 0, 32);
+		if (result == 2) {
+			LOG_ERROR("Timeout error from flash write "
+				"programming algorithm");
+			retval = ERROR_FLASH_OPERATION_FAILED;
+			goto err_run_ret;
+		} else if (result != 0) {
+			LOG_ERROR("Unexpected error %d from flash write "
+				"programming algorithm", result);
+			retval = ERROR_FLASH_OPERATION_FAILED;
+			goto err_run_ret;
+		} else
+			retval = ERROR_OK;
+
+		halfword_count -= halfwords;
+		offset += halfwords * 2;
+		buffer += halfwords * 2;
+		byte_count -= MIN(halfwords * 2, byte_count);
+	}
+
+err_run_ret:
+err_run:
+err_write_data:
+	retval = fm4_enter_flash_cpu_rom_mode(target);
+
+err_flash_mode:
+	for (i = 0; i < ARRAY_SIZE(reg_params); i++)
+		destroy_reg_param(&reg_params[i]);
+
+	target_free_working_area(target, data_workarea);
+err_alloc_data:
+err_write_code:
+	target_free_working_area(target, code_workarea);
+
+	return retval;
+}
+
+static int mb9bf_probe(struct flash_bank *bank)
+{
+	struct fm4_flash_bank *fm4_bank = bank->driver_priv;
+	uint32_t flash_addr = bank->base;
+	int i;
+
+	switch (fm4_bank->variant) {
+	case mb9bfx66:
+		bank->num_sectors = 12;
+		break;
+	case mb9bfx67:
+		bank->num_sectors = 16;
+		break;
+	case mb9bfx68:
+		bank->num_sectors = 20;
+		break;
+	default:
+		return ERROR_FLASH_OPER_UNSUPPORTED;
+	}
+
+	LOG_DEBUG("%d sectors", bank->num_sectors);
+	bank->sectors = calloc(bank->num_sectors,
+				sizeof(struct flash_sector));
+	for (i = 0; i < bank->num_sectors; i++) {
+		if (i < 4)
+			bank->sectors[i].size = 8 * 1024;
+		else if (i == 4)
+			bank->sectors[i].size = 32 * 1024;
+		else
+			bank->sectors[i].size = 64 * 1024;
+		bank->sectors[i].offset = flash_addr - bank->base;
+		bank->sectors[i].is_erased = -1;
+		bank->sectors[i].is_protected = -1;
+
+		bank->size += bank->sectors[i].size;
+		flash_addr += bank->sectors[i].size;
+	}
+
+	return ERROR_OK;
+}
+
+static void s6e2cc_init_sector(struct flash_sector *sector, int sa)
+{
+	if (sa < 8)
+		sector->size = 8 * 1024;
+	else if (sa == 8)
+		sector->size = 32 * 1024;
+	else
+		sector->size = 64 * 1024;
+
+	sector->is_erased = -1;
+	sector->is_protected = -1;
+}
+
+static int s6e2cc_probe(struct flash_bank *bank)
+{
+	struct target *target = bank->target;
+	struct fm4_flash_bank *fm4_bank = bank->driver_priv;
+	uint32_t u32_value;
+	uint32_t flash_addr = bank->base;
+	int i, retval, num_sectors, num_extra_sectors;
+
+	retval = target_read_u32(target, DFCTRLR, &u32_value);
+	if (retval != ERROR_OK)
+		return retval;
+	if (u32_value & DFCTRLR_DFE) {
+		LOG_WARNING("Dual Flash mode is not implemented.");
+		return ERROR_FLASH_OPER_UNSUPPORTED;
+	}
+
+	switch (fm4_bank->variant) {
+	case s6e2cx8:
+		num_sectors = (fm4_bank->macro_nr == 0) ? 20 : 0;
+		break;
+	case s6e2cx9:
+		num_sectors = (fm4_bank->macro_nr == 0) ? 20 : 12;
+		break;
+	case s6e2cxa:
+		num_sectors = 20;
+		break;
+	default:
+		return ERROR_FLASH_OPER_UNSUPPORTED;
+	}
+	num_extra_sectors = (fm4_bank->macro_nr == 0) ? 1 : 4;
+	bank->num_sectors = num_sectors + num_extra_sectors;
+
+	LOG_DEBUG("%d sectors", bank->num_sectors);
+	bank->sectors = calloc(bank->num_sectors,
+				sizeof(struct flash_sector));
+	for (i = 0; i < num_sectors; i++) {
+		int sa = 4 + i;
+		bank->sectors[i].offset = flash_addr - bank->base;
+		s6e2cc_init_sector(&bank->sectors[i], sa);
+
+		bank->size += bank->sectors[i].size;
+		flash_addr += bank->sectors[i].size;
+	}
+
+	flash_addr = (fm4_bank->macro_nr == 0) ? 0x00406000 : 0x00408000;
+	for (; i < bank->num_sectors; i++) {
+		int sa = 4 - num_extra_sectors + (i - num_sectors);
+		bank->sectors[i].offset = flash_addr - bank->base;
+		s6e2cc_init_sector(&bank->sectors[i], sa);
+
+		/*
+		 * Don't increase bank->size for these sectors
+		 * to avoid an overlap between Flash Macros #0 and #1.
+		 */
+		flash_addr += bank->sectors[i].size;
+	}
+
+	return ERROR_OK;
+}
+
+static int fm4_probe(struct flash_bank *bank)
+{
+	struct fm4_flash_bank *fm4_bank = bank->driver_priv;
+	int retval;
+
+	if (fm4_bank->probed)
+		return ERROR_OK;
+
+	if (bank->target->state != TARGET_HALTED) {
+		LOG_WARNING("Cannot communicate... target not halted.");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	switch (fm4_bank->variant) {
+	case mb9bfx66:
+	case mb9bfx67:
+	case mb9bfx68:
+		retval = mb9bf_probe(bank);
+		break;
+	case s6e2cx8:
+	case s6e2cx9:
+	case s6e2cxa:
+		retval = s6e2cc_probe(bank);
+		break;
+	default:
+		return ERROR_FLASH_OPER_UNSUPPORTED;
+	}
+	if (retval != ERROR_OK)
+		return retval;
+
+	fm4_bank->probed = true;
+
+	return ERROR_OK;
+}
+
+static int fm4_auto_probe(struct flash_bank *bank)
+{
+	struct fm4_flash_bank *fm4_bank = bank->driver_priv;
+
+	if (fm4_bank->probed)
+		return ERROR_OK;
+
+	return fm4_probe(bank);
+}
+
+static int fm4_protect_check(struct flash_bank *bank)
+{
+	return ERROR_OK;
+}
+
+static int fm4_get_info_command(struct flash_bank *bank, char *buf, int buf_size)
+{
+	struct fm4_flash_bank *fm4_bank = bank->driver_priv;
+	const char *name;
+
+	if (bank->target->state != TARGET_HALTED) {
+		LOG_WARNING("Cannot communicate... target not halted.");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	switch (fm4_bank->variant) {
+	case mb9bfx66:
+		name = "MB9BFx66";
+		break;
+	case mb9bfx67:
+		name = "MB9BFx67";
+		break;
+	case mb9bfx68:
+		name = "MB9BFx68";
+		break;
+	case s6e2cx8:
+		name = "S6E2Cx8";
+		break;
+	case s6e2cx9:
+		name = "S6E2Cx9";
+		break;
+	case s6e2cxa:
+		name = "S6E2CxA";
+		break;
+	default:
+		name = "unknown";
+		break;
+	}
+
+	switch (fm4_bank->variant) {
+	case s6e2cx8:
+	case s6e2cx9:
+	case s6e2cxa:
+		snprintf(buf, buf_size, "%s MainFlash Macro #%i",
+			name, fm4_bank->macro_nr);
+		break;
+	default:
+		snprintf(buf, buf_size, "%s MainFlash", name);
+		break;
+	}
+
+	return ERROR_OK;
+}
+
+static bool fm4_name_match(const char *s, const char *pattern)
+{
+	int i = 0;
+
+	while (s[i]) {
+		/* If the match string is shorter, ignore excess */
+		if (!pattern[i])
+			return true;
+		/* Use x as wildcard */
+		if (pattern[i] != 'x' && tolower(s[i]) != tolower(pattern[i]))
+			return false;
+		i++;
+	}
+	return true;
+}
+
+static int mb9bf_bank_setup(struct flash_bank *bank, const char *variant)
+{
+	struct fm4_flash_bank *fm4_bank = bank->driver_priv;
+
+	if (fm4_name_match(variant, "MB9BFx66")) {
+		fm4_bank->variant = mb9bfx66;
+	} else if (fm4_name_match(variant, "MB9BFx67")) {
+		fm4_bank->variant = mb9bfx67;
+	} else if (fm4_name_match(variant, "MB9BFx68")) {
+		fm4_bank->variant = mb9bfx68;
+	} else {
+		LOG_WARNING("MB9BF variant %s not recognized.", variant);
+		return ERROR_FLASH_OPER_UNSUPPORTED;
+	}
+
+	return ERROR_OK;
+}
+
+static int s6e2cc_bank_setup(struct flash_bank *bank, const char *variant)
+{
+	struct fm4_flash_bank *fm4_bank = bank->driver_priv;
+
+	if (fm4_name_match(variant, "S6E2Cx8")) {
+		fm4_bank->variant = s6e2cx8;
+	} else if (fm4_name_match(variant, "S6E2Cx9")) {
+		fm4_bank->variant = s6e2cx9;
+	} else if (fm4_name_match(variant, "S6E2CxA")) {
+		fm4_bank->variant = s6e2cxa;
+	} else {
+		LOG_WARNING("S6E2CC variant %s not recognized.", variant);
+		return ERROR_FLASH_OPER_UNSUPPORTED;
+	}
+
+	return ERROR_OK;
+}
+
+FLASH_BANK_COMMAND_HANDLER(fm4_flash_bank_command)
+{
+	struct fm4_flash_bank *fm4_bank;
+	const char *variant;
+	int ret;
+
+	if (CMD_ARGC < 7)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	variant = CMD_ARGV[6];
+
+	fm4_bank = malloc(sizeof(struct fm4_flash_bank));
+	if (!fm4_bank)
+		return ERROR_FLASH_OPERATION_FAILED;
+
+	fm4_bank->probed = false;
+	fm4_bank->macro_nr = (bank->base == 0x00000000) ? 0 : 1;
+
+	bank->driver_priv = fm4_bank;
+
+	if (fm4_name_match(variant, "MB9BF"))
+		ret = mb9bf_bank_setup(bank, variant);
+	else if (fm4_name_match(variant, "S6E2Cx"))
+		ret = s6e2cc_bank_setup(bank, variant);
+	else {
+		LOG_WARNING("Family %s not recognized.", variant);
+		ret = ERROR_FLASH_OPER_UNSUPPORTED;
+	}
+	if (ret != ERROR_OK)
+		free(fm4_bank);
+	return ret;
+}
+
+static const struct command_registration fm4_exec_command_handlers[] = {
+	COMMAND_REGISTRATION_DONE
+};
+
+static const struct command_registration fm4_command_handlers[] = {
+	{
+		.name = "fm4",
+		.mode = COMMAND_ANY,
+		.help = "fm4 flash command group",
+		.usage = "",
+		.chain = fm4_exec_command_handlers,
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+struct flash_driver fm4_flash = {
+	.name = "fm4",
+	.commands = fm4_command_handlers,
+	.flash_bank_command = fm4_flash_bank_command,
+	.info = fm4_get_info_command,
+	.probe = fm4_probe,
+	.auto_probe = fm4_auto_probe,
+	.protect_check = fm4_protect_check,
+	.erase = fm4_flash_erase,
+	.erase_check = default_flash_blank_check,
+	.write = fm4_flash_write,
+};
diff --git a/tcl/board/spansion_sk-fm4-176l-s6e2cc.cfg b/tcl/board/spansion_sk-fm4-176l-s6e2cc.cfg
index 4b23146..2855c5d 100644
--- a/tcl/board/spansion_sk-fm4-176l-s6e2cc.cfg
+++ b/tcl/board/spansion_sk-fm4-176l-s6e2cc.cfg
@@ -13,6 +13,7 @@ source [find interface/cmsis-dap.cfg]
 # FM4 S6E2CCAJ0A w/ 192 KB SRAM0
 #
 set CHIPNAME s6e2cc
+set CHIPSERIES S6E2CCAJ0A
 set WORKAREASIZE 0x30000
 source [find target/fm4_s6e2cc.cfg]
 
diff --git a/tcl/board/spansion_sk-fm4-u120-9b560.cfg b/tcl/board/spansion_sk-fm4-u120-9b560.cfg
index 64cbade..38ad4a8 100644
--- a/tcl/board/spansion_sk-fm4-u120-9b560.cfg
+++ b/tcl/board/spansion_sk-fm4-u120-9b560.cfg
@@ -11,6 +11,7 @@
 # FM4 MB9BF568R w/ 64 KB SRAM0
 #
 set CHIPNAME mb9bf568
+set CHIPSERIES MB9BF568R
 set WORKAREASIZE 0x10000
 source [find target/fm4_mb9bf.cfg]
 
diff --git a/tcl/target/fm4_mb9bf.cfg b/tcl/target/fm4_mb9bf.cfg
index 12a6aee..e53fdc8 100644
--- a/tcl/target/fm4_mb9bf.cfg
+++ b/tcl/target/fm4_mb9bf.cfg
@@ -13,3 +13,6 @@ if { [info exists WORKAREASIZE] } {
 
 $_TARGETNAME configure -work-area-phys [expr 0x20000000 - $_WORKAREASIZE] \
                        -work-area-size $_WORKAREASIZE -work-area-backup 0
+
+set _FLASHNAME $_CHIPNAME.flash
+flash bank $_FLASHNAME fm4 0x00000000 0 0 0 $_TARGETNAME $CHIPSERIES
diff --git a/tcl/target/fm4_s6e2cc.cfg b/tcl/target/fm4_s6e2cc.cfg
index 18cc630..60b73b9 100644
--- a/tcl/target/fm4_s6e2cc.cfg
+++ b/tcl/target/fm4_s6e2cc.cfg
@@ -13,3 +13,7 @@ if { [info exists WORKAREASIZE] } {
 
 $_TARGETNAME configure -work-area-phys [expr 0x20000000 - $_WORKAREASIZE] \
                        -work-area-size $_WORKAREASIZE -work-area-backup 0
+
+set _FLASHNAME $_CHIPNAME.flash
+flash bank ${_FLASHNAME}0 fm4 0x00000000 0 0 0 $_TARGETNAME $CHIPSERIES
+flash bank ${_FLASHNAME}1 fm4 0x00100000 0 0 0 $_TARGETNAME $CHIPSERIES