Merge branch 'remotes/openocd/master' into riscv64

Merged 1025be363e2bf42f1613083223a2322cc3a9bd4c Conflicts: src/flash/nor/Makefile.am src/rtos/Makefile.am src/rtos/rtos.c src/target/Makefile.am src/target/target.c src/target/target_type.h Doesn't build yet, but I fixed the conflicts that git pointed out.
author: Tim Newsome <tim@sifive.com> 2017-06-13 11:52:50 -0700
committer: Tim Newsome <tim@sifive.com> 2017-06-13 11:52:50 -0700
commit: 845c2f6b692736d3f6ee113908b98cb60ec60119 (patch)
tree: 32b9315ff67d97c397184dfe98b7523fed95dcb1 /src
parent: 7af58e6283c8e7d350b6b55c93a1d326326ed831 (diff)
parent: 1025be363e2bf42f1613083223a2322cc3a9bd4c (diff)
download: riscv-openocd-845c2f6b692736d3f6ee113908b98cb60ec60119.zip
riscv-openocd-845c2f6b692736d3f6ee113908b98cb60ec60119.tar.gz
riscv-openocd-845c2f6b692736d3f6ee113908b98cb60ec60119.tar.bz2
188 files changed, 15088 insertions, 8991 deletions
diff --git a/src/Makefile.am b/src/Makefile.am
index 26e02d0..07981aa 100644
--- a/src/Makefile.am
+++ b/src/Makefile.am
@@ -1,60 +1,41 @@
-include $(top_srcdir)/common.mk
+noinst_LTLIBRARIES += %D%/libopenocd.la
+bin_PROGRAMS += %D%/openocd
 
-SUBDIRS = \
-	jtag \
-	helper \
-	target \
-	transport \
-	flash \
-	svf \
-	xsvf \
-	pld \
-	server \
-	rtos
+%C%_openocd_SOURCES = \
+	%D%/main.c
 
-noinst_LTLIBRARIES = libopenocd.la
-bin_PROGRAMS = openocd
+%C%_libopenocd_la_SOURCES = \
+	%D%/hello.c %D%/hello.h \
+	%D%/openocd.c %D%/openocd.h
 
-MAINFILE = main.c
+%C%_openocd_LDADD = %D%/libopenocd.la
 
-openocd_SOURCES = $(MAINFILE)
-openocd_LDADD = libopenocd.la
+%C%_openocd_LDADD += $(MINGWLDADD)
 
 if INTERNAL_JIMTCL
-openocd_LDADD += $(top_builddir)/jimtcl/libjim.a
+%C%_openocd_LDADD += $(top_builddir)/jimtcl/libjim.a
 else
-openocd_LDADD += -ljim
+%C%_openocd_LDADD += -ljim
 endif
 
-if ULINK
-openocd_LDADD += -lm
-endif
-
-libopenocd_la_SOURCES = \
-	hello.c \
-	openocd.c
-
-noinst_HEADERS = \
-	hello.h \
-	openocd.h
-
-libopenocd_la_CPPFLAGS = -DPKGBLDDATE=\"`date +%F-%R`\"
+%C%_libopenocd_la_CPPFLAGS =
 
 # banner output includes RELSTR appended to $VERSION from the configure script
 # guess-rev.sh returns either a repository version ID or "-snapshot"
 if RELEASE
-libopenocd_la_CPPFLAGS += -DRELSTR=\"\"
-libopenocd_la_CPPFLAGS += -DGITVERSION=\"\"
+%C%_libopenocd_la_CPPFLAGS += -DRELSTR=\"\"
+%C%_libopenocd_la_CPPFLAGS += -DGITVERSION=\"\"
 else
-libopenocd_la_CPPFLAGS += -DRELSTR=\"`$(top_srcdir)/guess-rev.sh $(top_srcdir)`\"
-libopenocd_la_CPPFLAGS += -DGITVERSION=\"`cd $(top_srcdir) && git describe`\"
+%C%_libopenocd_la_CPPFLAGS += -DRELSTR=\"`$(top_srcdir)/guess-rev.sh $(top_srcdir)`\"
+%C%_libopenocd_la_CPPFLAGS += -DGITVERSION=\"`cd $(top_srcdir) && git describe`\"
+%C%_libopenocd_la_CPPFLAGS += -DPKGBLDDATE=\"`date +%F-%R`\"
 endif
 
 # add default CPPFLAGS
-libopenocd_la_CPPFLAGS += $(AM_CPPFLAGS) $(CPPFLAGS)
+%C%_libopenocd_la_CPPFLAGS += $(AM_CPPFLAGS) $(CPPFLAGS)
 
 # the library search path.
-libopenocd_la_LDFLAGS = $(all_libraries)
+%C%_libopenocd_la_LDFLAGS = $(all_libraries)
 
 if IS_MINGW
 MINGWLDADD = -lws2_32
@@ -62,55 +43,43 @@ else
 MINGWLDADD =
 endif
 
-libopenocd_la_LIBADD = \
-	$(top_builddir)/src/xsvf/libxsvf.la \
-	$(top_builddir)/src/svf/libsvf.la \
-	$(top_builddir)/src/pld/libpld.la \
-	$(top_builddir)/src/jtag/libjtag.la \
-	$(top_builddir)/src/transport/libtransport.la \
-	$(top_builddir)/src/flash/libflash.la \
-	$(top_builddir)/src/target/libtarget.la \
-	$(top_builddir)/src/server/libserver.la \
-	$(top_builddir)/src/rtos/librtos.la \
-	$(top_builddir)/src/helper/libhelper.la \
-	$(LIBFTDI_LIBS) $(MINGWLDADD) \
-	$(HIDAPI_LIBS) $(LIBUSB0_LIBS) $(LIBUSB1_LIBS)
-
-STARTUP_TCL_SRCS = \
-	$(srcdir)/helper/startup.tcl \
-	$(srcdir)/jtag/startup.tcl \
-	$(srcdir)/target/startup.tcl \
-	$(srcdir)/flash/startup.tcl \
-	$(srcdir)/server/startup.tcl
+%C%_libopenocd_la_LIBADD = \
+	%D%/xsvf/libxsvf.la \
+	%D%/svf/libsvf.la \
+	%D%/pld/libpld.la \
+	%D%/jtag/libjtag.la \
+	%D%/transport/libtransport.la \
+	%D%/flash/libflash.la \
+	%D%/target/libtarget.la \
+	%D%/server/libserver.la \
+	%D%/rtos/librtos.la \
+	%D%/helper/libhelper.la
 
-EXTRA_DIST = $(STARTUP_TCL_SRCS)
+BIN2C = $(srcdir)/%D%/helper/bin2char.sh
 
-BUILT_SOURCES = startup_tcl.inc
+STARTUP_TCL_SRCS =
+EXTRA_DIST += $(STARTUP_TCL_SRCS)
 
-startup.tcl: $(STARTUP_TCL_SRCS)
-	cat $^ > $@
-
-BIN2C = $(top_srcdir)/src/helper/bin2char.sh
+BUILT_SOURCES += %D%/startup_tcl.inc
 
 # Convert .tcl to c-array
-startup_tcl.inc: startup.tcl $(BIN2C)
-	$(BIN2C) < $< > $@ || { rm -f $@; false; }
+%D%/startup_tcl.inc: $(STARTUP_TCL_SRCS)
+	cat $^ | $(BIN2C) > $@ || { rm -f $@; false; }
 
 # add generated files to make clean list
-CLEANFILES = startup.tcl startup_tcl.inc
+CLEANFILES += %D%/startup_tcl.inc
 
 # we do not want generated file in the dist
-dist-hook:
-	rm -f $(distdir)/startup_tcl.inc
-
-MAINTAINERCLEANFILES = $(srcdir)/Makefile.in
-
-# The "quick" target builds executables & reinstalls the executables
-# Primary use: developer types to quicken the edit/compile/debug
-# cycle.  by not requiring a "full build and full install". Note the
-# assumption is: You are only rebuilding the EXE.... and everything
-# else is/was previously installed.
-#
-# use at your own risk
-quick: all install-binPROGRAMS
-
+#dist-hook:
+#	rm -f $(distdir)/%D%/startup_tcl.inc
+
+include %D%/helper/Makefile.am
+include %D%/jtag/Makefile.am
+include %D%/transport/Makefile.am
+include %D%/xsvf/Makefile.am
+include %D%/svf/Makefile.am
+include %D%/target/Makefile.am
+include %D%/rtos/Makefile.am
+include %D%/server/Makefile.am
+include %D%/flash/Makefile.am
+include %D%/pld/Makefile.am
diff --git a/src/flash/Makefile.am b/src/flash/Makefile.am
index ece4018..a1b46f8 100644
--- a/src/flash/Makefile.am
+++ b/src/flash/Makefile.am
@@ -1,23 +1,13 @@
-include $(top_srcdir)/common.mk
+noinst_LTLIBRARIES += %D%/libflash.la
+%C%_libflash_la_SOURCES = \
+	%D%/common.c %D%/common.h \
+	%D%/mflash.c %D%/mflash.h
 
-SUBDIRS = \
-	nor \
-	nand
+%C%_libflash_la_LIBADD = \
+	%D%/nor/libocdflashnor.la \
+	%D%/nand/libocdflashnand.la
 
-METASOURCES = AUTO
-noinst_LTLIBRARIES = libflash.la
-libflash_la_SOURCES = \
-	common.c \
-	mflash.c
+STARTUP_TCL_SRCS += %D%/startup.tcl
 
-libflash_la_LIBADD = \
-	$(top_builddir)/src/flash/nor/libocdflashnor.la \
-	$(top_builddir)/src/flash/nand/libocdflashnand.la
-
-noinst_HEADERS = \
-	common.h \
-	mflash.h
-
-EXTRA_DIST = startup.tcl
-
-MAINTAINERCLEANFILES = $(srcdir)/Makefile.in
+include %D%/nor/Makefile.am
+include %D%/nand/Makefile.am
diff --git a/src/flash/common.h b/src/flash/common.h
index ce26fcc..4244f13 100644
--- a/src/flash/common.h
+++ b/src/flash/common.h
@@ -44,5 +44,6 @@ bool flash_driver_name_matches(const char *name, const char *expected);
 #define ERROR_FLASH_SECTOR_NOT_ERASED		(-906)
 #define ERROR_FLASH_BANK_NOT_PROBED			(-907)
 #define ERROR_FLASH_OPER_UNSUPPORTED		(-908)
+#define ERROR_FLASH_PROTECTED			(-909)
 
 #endif /* OPENOCD_FLASH_COMMON_H */
diff --git a/src/flash/nand/Makefile.am b/src/flash/nand/Makefile.am
index 2ddd096..abe90f8 100644
--- a/src/flash/nand/Makefile.am
+++ b/src/flash/nand/Makefile.am
@@ -1,46 +1,43 @@
-include $(top_srcdir)/common.mk
+noinst_LTLIBRARIES += %D%/libocdflashnand.la
 
-noinst_LTLIBRARIES = libocdflashnand.la
-
-libocdflashnand_la_SOURCES = \
-	ecc.c \
-	ecc_kw.c \
-	core.c \
-	fileio.c \
-	tcl.c \
-	arm_io.c \
+%C%_libocdflashnand_la_SOURCES = \
+	%D%/ecc.c \
+	%D%/ecc_kw.c \
+	%D%/core.c \
+	%D%/fileio.c \
+	%D%/tcl.c \
+	%D%/arm_io.c \
 	$(NAND_DRIVERS) \
-	driver.c
+	%D%/driver.c \
+	$(NANDHEADERS)
 
 NAND_DRIVERS = \
-	nonce.c \
-	davinci.c \
-	lpc3180.c \
-	lpc32xx.c \
-	mxc.c \
-	mx3.c \
-	orion.c \
-	s3c24xx.c \
-	s3c2410.c \
-	s3c2412.c \
-	s3c2440.c \
-	s3c2443.c \
-	s3c6400.c \
-	at91sam9.c \
-	nuc910.c
-
-noinst_HEADERS = \
-	arm_io.h \
-	core.h \
-	driver.h \
-	fileio.h \
-	imp.h \
-	lpc3180.h \
-	lpc32xx.h \
-	mxc.h \
-	mx3.h \
-	s3c24xx.h \
-	s3c24xx_regs.h \
-	nuc910.h
+	%D%/nonce.c \
+	%D%/davinci.c \
+	%D%/lpc3180.c \
+	%D%/lpc32xx.c \
+	%D%/mxc.c \
+	%D%/mx3.c \
+	%D%/orion.c \
+	%D%/s3c24xx.c \
+	%D%/s3c2410.c \
+	%D%/s3c2412.c \
+	%D%/s3c2440.c \
+	%D%/s3c2443.c \
+	%D%/s3c6400.c \
+	%D%/at91sam9.c \
+	%D%/nuc910.c
 
-MAINTAINERCLEANFILES = $(srcdir)/Makefile.in
+NANDHEADERS = \
+	%D%/arm_io.h \
+	%D%/core.h \
+	%D%/driver.h \
+	%D%/fileio.h \
+	%D%/imp.h \
+	%D%/lpc3180.h \
+	%D%/lpc32xx.h \
+	%D%/mxc.h \
+	%D%/mx3.h \
+	%D%/s3c24xx.h \
+	%D%/s3c24xx_regs.h \
+	%D%/nuc910.h
diff --git a/src/flash/nand/tcl.c b/src/flash/nand/tcl.c
index cbdeda5..d9738c5 100644
--- a/src/flash/nand/tcl.c
+++ b/src/flash/nand/tcl.c
@@ -254,7 +254,8 @@ COMMAND_HANDLER(handle_nand_write_command)
 		int bytes_read = nand_fileio_read(nand, &s);
 		if (bytes_read <= 0) {
 			command_print(CMD_CTX, "error while reading file");
-			return nand_fileio_cleanup(&s);
+			nand_fileio_cleanup(&s);
+			return ERROR_FAIL;
 		}
 		s.size -= bytes_read;
 
@@ -264,7 +265,8 @@ COMMAND_HANDLER(handle_nand_write_command)
 			command_print(CMD_CTX, "failed writing file %s "
 				"to NAND flash %s at offset 0x%8.8" PRIx32,
 				CMD_ARGV[1], CMD_ARGV[0], s.address);
-			return nand_fileio_cleanup(&s);
+			nand_fileio_cleanup(&s);
+			return retval;
 		}
 		s.address += s.page_size;
 	}
diff --git a/src/flash/nor/Makefile.am b/src/flash/nor/Makefile.am
index 084e6b6..759e98c 100644
--- a/src/flash/nor/Makefile.am
+++ b/src/flash/nor/Makefile.am
@@ -1,70 +1,68 @@
-include $(top_srcdir)/common.mk
-
-noinst_LTLIBRARIES = libocdflashnor.la
-libocdflashnor_la_SOURCES = \
-	core.c \
-	tcl.c \
+noinst_LTLIBRARIES += %D%/libocdflashnor.la
+%C%_libocdflashnor_la_SOURCES = \
+	%D%/core.c \
+	%D%/tcl.c \
 	$(NOR_DRIVERS) \
-	drivers.c
+	%D%/drivers.c \
+	$(NORHEADERS)
 
 NOR_DRIVERS = \
-	aduc702x.c \
-	aducm360.c \
-	ambiqmicro.c \
-	at91sam4.c \
-	at91sam4l.c \
-	at91samd.c \
-	at91sam3.c \
-	at91sam7.c \
-	atsamv.c \
-	avrf.c \
-	cfi.c \
-	dsp5680xx_flash.c \
-	efm32.c \
-	em357.c \
-	fespi.c \
-	faux.c \
-	fm3.c \
-	fm4.c \
-	jtagspi.c \
-	kinetis.c \
-	kinetis_ke.c \
-	lpc2000.c \
-	lpc288x.c \
-	lpc2900.c \
-	lpcspifi.c \
-	mdr.c \
-	mrvlqspi.c \
-	niietcm4.c \
-	non_cfi.c \
-	nrf51.c \
-	numicro.c \
-	ocl.c \
-	pic32mx.c \
-	psoc4.c \
-	sim3x.c \
-	spi.c \
-	stmsmi.c \
-	stellaris.c \
-	stm32f1x.c \
-	stm32f2x.c \
-	stm32lx.c \
-	stm32l4x.c \
-	str7x.c \
-	str9x.c \
-	str9xpec.c \
-	tms470.c \
-	virtual.c \
-	xmc1xxx.c \
-	xmc4xxx.c
-
-noinst_HEADERS = \
-	core.h \
-	cfi.h \
-	driver.h \
-	imp.h \
-	non_cfi.h \
-	ocl.h \
-	spi.h
+	%D%/aduc702x.c \
+	%D%/aducm360.c \
+	%D%/ambiqmicro.c \
+	%D%/at91sam4.c \
+	%D%/at91sam4l.c \
+	%D%/at91samd.c \
+	%D%/at91sam3.c \
+	%D%/at91sam7.c \
+	%D%/ath79.c \
+	%D%/atsamv.c \
+	%D%/avrf.c \
+	%D%/cfi.c \
+	%D%/dsp5680xx_flash.c \
+	%D%/efm32.c \
+	%D%/em357.c \
+	%D%/fespi.c \
+	%D%/faux.c \
+	%D%/fm3.c \
+	%D%/fm4.c \
+	%D%/jtagspi.c \
+	%D%/kinetis.c \
+	%D%/kinetis_ke.c \
+	%D%/lpc2000.c \
+	%D%/lpc288x.c \
+	%D%/lpc2900.c \
+	%D%/lpcspifi.c \
+	%D%/mdr.c \
+	%D%/mrvlqspi.c \
+	%D%/niietcm4.c \
+	%D%/non_cfi.c \
+	%D%/nrf51.c \
+	%D%/numicro.c \
+	%D%/ocl.c \
+	%D%/pic32mx.c \
+	%D%/psoc4.c \
+	%D%/sim3x.c \
+	%D%/spi.c \
+	%D%/stmsmi.c \
+	%D%/stellaris.c \
+	%D%/stm32f1x.c \
+	%D%/stm32f2x.c \
+	%D%/stm32lx.c \
+	%D%/stm32l4x.c \
+	%D%/str7x.c \
+	%D%/str9x.c \
+	%D%/str9xpec.c \
+	%D%/tms470.c \
+	%D%/virtual.c \
+	%D%/xmc1xxx.c \
+	%D%/xmc4xxx.c
 
-MAINTAINERCLEANFILES = $(srcdir)/Makefile.in
+NORHEADERS = \
+	%D%/core.h \
+	%D%/cfi.h \
+	%D%/driver.h \
+	%D%/imp.h \
+	%D%/non_cfi.h \
+	%D%/ocl.h \
+	%D%/spi.h
diff --git a/src/flash/nor/at91sam4.c b/src/flash/nor/at91sam4.c
index 50aa98b..ff75b41 100644
--- a/src/flash/nor/at91sam4.c
+++ b/src/flash/nor/at91sam4.c
@@ -65,8 +65,9 @@
 
 #define REG_NAME_WIDTH  (12)
 
-/* at91sam4s/at91sam4e series (has always one flash bank)*/
+/* at91sam4s/at91sam4e/at91sam4c series (has always one flash bank)*/
 #define FLASH_BANK_BASE_S   0x00400000
+#define FLASH_BANK_BASE_C   0x01000000
 
 /* at91sam4sd series (two one flash banks), first bank address */
 #define FLASH_BANK0_BASE_SD FLASH_BANK_BASE_S
@@ -75,6 +76,10 @@
 /* at91sam4sd32x, second bank address */
 #define FLASH_BANK1_BASE_2048K_SD (FLASH_BANK0_BASE_SD+(2048*1024/2))
 
+/* at91sam4c32x, first and second bank address */
+#define FLASH_BANK0_BASE_C32 FLASH_BANK_BASE_C
+#define FLASH_BANK1_BASE_C32 (FLASH_BANK_BASE_C+(2048*1024/2))
+
 #define         AT91C_EFC_FCMD_GETD                 (0x0)	/* (EFC) Get Flash Descriptor */
 #define         AT91C_EFC_FCMD_WP                   (0x1)	/* (EFC) Write Page */
 #define         AT91C_EFC_FCMD_WPL                  (0x2)	/* (EFC) Write Page and Lock */
@@ -258,6 +263,188 @@ static struct sam4_chip *get_current_sam4(struct command_context *cmd_ctx)
 
 /* these are used to *initialize* the "pChip->details" structure. */
 static const struct sam4_chip_details all_sam4_details[] = {
+	/* Start at91sam4c* series */
+	/* at91sam4c32e - LQFP144 */
+	{
+		.chipid_cidr    = 0xA66D0EE0,
+		.name           = "at91sam4c32e",
+		.total_flash_size     = 2024 * 1024,
+		.total_sram_size      = 256 * 1024,
+		.n_gpnvms       = 3,
+		.n_banks        = 2,
+/*		.bank[0] = { */
+		{
+			{
+				.probed = 0,
+				.pChip  = NULL,
+				.pBank  = NULL,
+				.bank_number = 0,
+				.base_address = FLASH_BANK0_BASE_C32,
+				.controller_address = 0x400e0a00,
+				.flash_wait_states = 5,
+				.present = 1,
+				.size_bytes =  1024 * 1024,
+				.nsectors   =  128,
+				.sector_size = 8192,
+				.page_size   = 512,
+			},
+/*		.bank[1] = { */
+			{
+				.probed = 0,
+				.pChip  = NULL,
+				.pBank  = NULL,
+				.bank_number = 1,
+				.base_address = FLASH_BANK1_BASE_C32,
+				.controller_address = 0x400e0c00,
+				.flash_wait_states = 5,
+				.present = 1,
+				.size_bytes =  1024 * 1024,
+				.nsectors   =  128,
+				.sector_size = 8192,
+				.page_size   = 512,
+			},
+		},
+	},
+	/* at91sam4c32c - LQFP100 */
+	{
+		.chipid_cidr    = 0xA64D0EE0,
+		.name           = "at91sam4c32c",
+		.total_flash_size     = 2024 * 1024,
+		.total_sram_size      = 256 * 1024,
+		.n_gpnvms       = 3,
+		.n_banks        = 2,
+/*		.bank[0] = { */
+		{
+			{
+				.probed = 0,
+				.pChip  = NULL,
+				.pBank  = NULL,
+				.bank_number = 0,
+				.base_address = FLASH_BANK0_BASE_C32,
+				.controller_address = 0x400e0a00,
+				.flash_wait_states = 5,
+				.present = 1,
+				.size_bytes =  1024 * 1024,
+				.nsectors   =  128,
+				.sector_size = 8192,
+				.page_size   = 512,
+			},
+/*		.bank[1] = { */
+			{
+				.probed = 0,
+				.pChip  = NULL,
+				.pBank  = NULL,
+				.bank_number = 1,
+				.base_address = FLASH_BANK1_BASE_C32,
+				.controller_address = 0x400e0c00,
+				.flash_wait_states = 5,
+				.present = 1,
+				.size_bytes =  1024 * 1024,
+				.nsectors   =  128,
+				.sector_size = 8192,
+				.page_size   = 512,
+			},
+		},
+	},
+	/* at91sam4c16c - LQFP100 */
+	{
+		.chipid_cidr    = 0xA64C0CE0,
+		.name           = "at91sam4c16c",
+		.total_flash_size     = 1024 * 1024,
+		.total_sram_size      = 128 * 1024,
+		.n_gpnvms       = 2,
+		.n_banks        = 1,
+		{
+/*		.bank[0] = {*/
+		  {
+			.probed = 0,
+			.pChip  = NULL,
+			.pBank  = NULL,
+			.bank_number = 0,
+			.base_address = FLASH_BANK_BASE_C,
+			.controller_address = 0x400e0a00,
+			.flash_wait_states = 5,
+			.present = 1,
+			.size_bytes =  1024 * 1024,
+			.nsectors   =  128,
+			.sector_size = 8192,
+			.page_size   = 512,
+		  },
+/*		.bank[1] = {*/
+		  {
+			.present = 0,
+			.probed = 0,
+			.bank_number = 1,
+
+		  },
+		},
+	},
+	/* at91sam4c8c - LQFP100 */
+	{
+		.chipid_cidr    = 0xA64C0AE0,
+		.name           = "at91sam4c8c",
+		.total_flash_size     = 512 * 1024,
+		.total_sram_size      = 128 * 1024,
+		.n_gpnvms       = 2,
+		.n_banks        = 1,
+		{
+/*		.bank[0] = {*/
+		  {
+			.probed = 0,
+			.pChip  = NULL,
+			.pBank  = NULL,
+			.bank_number = 0,
+			.base_address = FLASH_BANK_BASE_C,
+			.controller_address = 0x400e0a00,
+			.flash_wait_states = 5,
+			.present = 1,
+			.size_bytes =  512 * 1024,
+			.nsectors   =  64,
+			.sector_size = 8192,
+			.page_size   = 512,
+		  },
+/*		.bank[1] = {*/
+		  {
+			.present = 0,
+			.probed = 0,
+			.bank_number = 1,
+
+		  },
+		},
+	},
+	/* at91sam4c4c (rev B) - LQFP100 */
+	{
+		.chipid_cidr    = 0xA64C0CE5,
+		.name           = "at91sam4c4c",
+		.total_flash_size     = 256 * 1024,
+		.total_sram_size      = 128 * 1024,
+		.n_gpnvms       = 2,
+		.n_banks        = 1,
+		{
+/*		.bank[0] = {*/
+		  {
+			.probed = 0,
+			.pChip  = NULL,
+			.pBank  = NULL,
+			.bank_number = 0,
+			.base_address = FLASH_BANK_BASE_C,
+			.controller_address = 0x400e0a00,
+			.flash_wait_states = 5,
+			.present = 1,
+			.size_bytes =  256 * 1024,
+			.nsectors   =  32,
+			.sector_size = 8192,
+			.page_size   = 512,
+		  },
+/*		.bank[1] = {*/
+		  {
+			.present = 0,
+			.probed = 0,
+			.bank_number = 1,
+
+		  },
+		},
+	},
 
 	/* Start at91sam4e* series */
 	/*atsam4e16e - LQFP144/LFBGA144*/
@@ -479,7 +666,7 @@ static const struct sam4_chip_details all_sam4_details[] = {
 			.bank_number = 0,
 			.base_address = FLASH_BANK_BASE_S,
 			.controller_address = 0x400e0a00,
-			.flash_wait_states = 6,	/* workaround silicon bug */
+			.flash_wait_states = 5,
 			.present = 1,
 			.size_bytes =  1024 * 1024,
 			.nsectors   =  128,
@@ -495,7 +682,7 @@ static const struct sam4_chip_details all_sam4_details[] = {
 		  },
 		},
 	},
-	/*atsam4s16b - LQFP64/QFN64*/
+	/*atsam4s16b - LQFP64/QFN64/WLCSP64*/
 	{
 		.chipid_cidr    = 0x289C0CE0,
 		.name           = "at91sam4s16b",
@@ -512,7 +699,7 @@ static const struct sam4_chip_details all_sam4_details[] = {
 			.bank_number = 0,
 			.base_address = FLASH_BANK_BASE_S,
 			.controller_address = 0x400e0a00,
-			.flash_wait_states = 6,	/* workaround silicon bug */
+			.flash_wait_states = 5,
 			.present = 1,
 			.size_bytes =  1024 * 1024,
 			.nsectors   =  128,
@@ -545,7 +732,7 @@ static const struct sam4_chip_details all_sam4_details[] = {
 			.bank_number = 0,
 			.base_address = FLASH_BANK_BASE_S,
 			.controller_address = 0x400e0a00,
-			.flash_wait_states = 6,	/* workaround silicon bug */
+			.flash_wait_states = 5,
 			.present = 1,
 			.size_bytes =  1024 * 1024,
 			.nsectors   =  128,
@@ -578,7 +765,7 @@ static const struct sam4_chip_details all_sam4_details[] = {
 			.bank_number = 0,
 			.base_address = FLASH_BANK_BASE_S,
 			.controller_address = 0x400e0a00,
-			.flash_wait_states = 6,	/* workaround silicon bug */
+			.flash_wait_states = 5,
 			.present = 1,
 			.size_bytes =  1024 * 1024,
 			.nsectors   =  128,
@@ -611,7 +798,7 @@ static const struct sam4_chip_details all_sam4_details[] = {
 			.bank_number = 0,
 			.base_address = FLASH_BANK_BASE_S,
 			.controller_address = 0x400e0a00,
-			.flash_wait_states = 6,	/* workaround silicon bug */
+			.flash_wait_states = 5,
 			.present = 1,
 			.size_bytes =  512 * 1024,
 			.nsectors   =  64,
@@ -627,7 +814,7 @@ static const struct sam4_chip_details all_sam4_details[] = {
 		  },
 		},
 	},
-	/*atsam4s8b - LQFP64/BGA64*/
+	/*atsam4s8b - LQFP64/QFN64/WLCSP64*/
 	{
 		.chipid_cidr    = 0x289C0AE0,
 		.name           = "at91sam4s8b",
@@ -644,7 +831,7 @@ static const struct sam4_chip_details all_sam4_details[] = {
 			.bank_number = 0,
 			.base_address = FLASH_BANK_BASE_S,
 			.controller_address = 0x400e0a00,
-			.flash_wait_states = 6,	/* workaround silicon bug */
+			.flash_wait_states = 5,
 			.present = 1,
 			.size_bytes =  512 * 1024,
 			.nsectors   =  64,
@@ -677,7 +864,7 @@ static const struct sam4_chip_details all_sam4_details[] = {
 			.bank_number = 0,
 			.base_address = FLASH_BANK_BASE_S,
 			.controller_address = 0x400e0a00,
-			.flash_wait_states = 6,	/* workaround silicon bug */
+			.flash_wait_states = 5,
 			.present = 1,
 			.size_bytes =  512 * 1024,
 			.nsectors   =  64,
@@ -694,7 +881,75 @@ static const struct sam4_chip_details all_sam4_details[] = {
 		},
 	},
 
-	/*atsam4s4a - LQFP48/BGA48*/
+	/*atsam4s4c - LQFP100/BGA100*/
+	{
+		.chipid_cidr    = 0x28ab09e0,
+		.name           = "at91sam4s4c",
+		.total_flash_size     = 256 * 1024,
+		.total_sram_size      = 64 * 1024,
+		.n_gpnvms       = 2,
+		.n_banks        = 1,
+		{
+/*		.bank[0] = {*/
+		  {
+			.probed = 0,
+			.pChip  = NULL,
+			.pBank  = NULL,
+			.bank_number = 0,
+			.base_address = FLASH_BANK_BASE_S,
+			.controller_address = 0x400e0a00,
+			.flash_wait_states = 5,
+			.present = 1,
+			.size_bytes =  256 * 1024,
+			.nsectors   =  32,
+			.sector_size = 8192,
+			.page_size   = 512,
+		  },
+/*		.bank[1] = {*/
+		  {
+			.present = 0,
+			.probed = 0,
+			.bank_number = 1,
+
+		  },
+		},
+	},
+
+	/*atsam4s4b - LQFP64/QFN64/WLCSP64*/
+	{
+		.chipid_cidr    = 0x289b09e0,
+		.name           = "at91sam4s4b",
+		.total_flash_size     = 256 * 1024,
+		.total_sram_size      = 64 * 1024,
+		.n_gpnvms       = 2,
+		.n_banks        = 1,
+		{
+/*		.bank[0] = {*/
+		  {
+			.probed = 0,
+			.pChip  = NULL,
+			.pBank  = NULL,
+			.bank_number = 0,
+			.base_address = FLASH_BANK_BASE_S,
+			.controller_address = 0x400e0a00,
+			.flash_wait_states = 5,
+			.present = 1,
+			.size_bytes =  256 * 1024,
+			.nsectors   =  32,
+			.sector_size = 8192,
+			.page_size   = 512,
+		  },
+/*		.bank[1] = {*/
+		  {
+			.present = 0,
+			.probed = 0,
+			.bank_number = 1,
+
+		  },
+		},
+	},
+
+	/*atsam4s4a - LQFP48/QFN48*/
 	{
 		.chipid_cidr    = 0x288b09e0,
 		.name           = "at91sam4s4a",
@@ -711,7 +966,7 @@ static const struct sam4_chip_details all_sam4_details[] = {
 			.bank_number = 0,
 			.base_address = FLASH_BANK_BASE_S,
 			.controller_address = 0x400e0a00,
-			.flash_wait_states = 6,	/* workaround silicon bug */
+			.flash_wait_states = 5,
 			.present = 1,
 			.size_bytes =  256 * 1024,
 			.nsectors   =  32,
@@ -728,7 +983,109 @@ static const struct sam4_chip_details all_sam4_details[] = {
 		},
 	},
 
-	/*at91sam4sd32c*/
+	/*atsam4s2c - LQFP100/BGA100*/
+	{
+		.chipid_cidr    = 0x28ab07e0,
+		.name           = "at91sam4s2c",
+		.total_flash_size     = 128 * 1024,
+		.total_sram_size      = 64 * 1024,
+		.n_gpnvms       = 2,
+		.n_banks        = 1,
+		{
+/*		.bank[0] = {*/
+		  {
+			.probed = 0,
+			.pChip  = NULL,
+			.pBank  = NULL,
+			.bank_number = 0,
+			.base_address = FLASH_BANK_BASE_S,
+			.controller_address = 0x400e0a00,
+			.flash_wait_states = 5,
+			.present = 1,
+			.size_bytes =  128 * 1024,
+			.nsectors   =  16,
+			.sector_size = 8192,
+			.page_size   = 512,
+		  },
+/*		.bank[1] = {*/
+		  {
+			.present = 0,
+			.probed = 0,
+			.bank_number = 1,
+
+		  },
+		},
+	},
+
+	/*atsam4s2b - LQPF64/QFN64/WLCSP64*/
+	{
+		.chipid_cidr    = 0x289b07e0,
+		.name           = "at91sam4s2b",
+		.total_flash_size     = 128 * 1024,
+		.total_sram_size      = 64 * 1024,
+		.n_gpnvms       = 2,
+		.n_banks        = 1,
+		{
+/*		.bank[0] = {*/
+		  {
+			.probed = 0,
+			.pChip  = NULL,
+			.pBank  = NULL,
+			.bank_number = 0,
+			.base_address = FLASH_BANK_BASE_S,
+			.controller_address = 0x400e0a00,
+			.flash_wait_states = 5,
+			.present = 1,
+			.size_bytes =  128 * 1024,
+			.nsectors   =  16,
+			.sector_size = 8192,
+			.page_size   = 512,
+		  },
+/*		.bank[1] = {*/
+		  {
+			.present = 0,
+			.probed = 0,
+			.bank_number = 1,
+
+		  },
+		},
+	},
+
+	/*atsam4s2a - LQFP48/QFN48*/
+	{
+		.chipid_cidr    = 0x288b07e0,
+		.name           = "at91sam4s2a",
+		.total_flash_size     = 128 * 1024,
+		.total_sram_size      = 64 * 1024,
+		.n_gpnvms       = 2,
+		.n_banks        = 1,
+		{
+/*		.bank[0] = {*/
+		  {
+			.probed = 0,
+			.pChip  = NULL,
+			.pBank  = NULL,
+			.bank_number = 0,
+			.base_address = FLASH_BANK_BASE_S,
+			.controller_address = 0x400e0a00,
+			.flash_wait_states = 5,
+			.present = 1,
+			.size_bytes =  128 * 1024,
+			.nsectors   =  16,
+			.sector_size = 8192,
+			.page_size   = 512,
+		  },
+/*		.bank[1] = {*/
+		  {
+			.present = 0,
+			.probed = 0,
+			.bank_number = 1,
+
+		  },
+		},
+	},
+
+	/*at91sam4sd32c  - LQFP100/BGA100*/
 	{
 		.chipid_cidr    = 0x29a70ee0,
 		.name           = "at91sam4sd32c",
@@ -746,7 +1103,7 @@ static const struct sam4_chip_details all_sam4_details[] = {
 				.bank_number = 0,
 				.base_address = FLASH_BANK0_BASE_SD,
 				.controller_address = 0x400e0a00,
-				.flash_wait_states = 6,	/* workaround silicon bug */
+				.flash_wait_states = 5,
 				.present = 1,
 				.size_bytes =  1024 * 1024,
 				.nsectors   =  128,
@@ -762,7 +1119,7 @@ static const struct sam4_chip_details all_sam4_details[] = {
 				.bank_number = 1,
 				.base_address = FLASH_BANK1_BASE_2048K_SD,
 				.controller_address = 0x400e0c00,
-				.flash_wait_states = 6,	/* workaround silicon bug */
+				.flash_wait_states = 5,
 				.present = 1,
 				.size_bytes =  1024 * 1024,
 				.nsectors   =  128,
@@ -772,7 +1129,51 @@ static const struct sam4_chip_details all_sam4_details[] = {
 		},
 	},
 
-	/*at91sam4sd16c*/
+	/*at91sam4sd32b  - LQFP64/BGA64*/
+	{
+		.chipid_cidr    = 0x29970ee0,
+		.name           = "at91sam4sd32b",
+		.total_flash_size     = 2048 * 1024,
+		.total_sram_size      = 160 * 1024,
+		.n_gpnvms       = 3,
+		.n_banks        = 2,
+
+/*		.bank[0] = { */
+		{
+			{
+				.probed = 0,
+				.pChip  = NULL,
+				.pBank  = NULL,
+				.bank_number = 0,
+				.base_address = FLASH_BANK0_BASE_SD,
+				.controller_address = 0x400e0a00,
+				.flash_wait_states = 5,
+				.present = 1,
+				.size_bytes =  1024 * 1024,
+				.nsectors   =  128,
+				.sector_size = 8192,
+				.page_size   = 512,
+			},
+
+/*		.bank[1] = { */
+			{
+				.probed = 0,
+				.pChip  = NULL,
+				.pBank  = NULL,
+				.bank_number = 1,
+				.base_address = FLASH_BANK1_BASE_2048K_SD,
+				.controller_address = 0x400e0c00,
+				.flash_wait_states = 5,
+				.present = 1,
+				.size_bytes =  1024 * 1024,
+				.nsectors   =  128,
+				.sector_size = 8192,
+				.page_size   = 512,
+			},
+		},
+	},
+
+	/*at91sam4sd16c - LQFP100/BGA100*/
 	{
 		.chipid_cidr    = 0x29a70ce0,
 		.name           = "at91sam4sd16c",
@@ -790,7 +1191,7 @@ static const struct sam4_chip_details all_sam4_details[] = {
 				.bank_number = 0,
 				.base_address = FLASH_BANK0_BASE_SD,
 				.controller_address = 0x400e0a00,
-				.flash_wait_states = 6,	/* workaround silicon bug */
+				.flash_wait_states = 5,
 				.present = 1,
 				.size_bytes =  512 * 1024,
 				.nsectors   =  64,
@@ -806,7 +1207,51 @@ static const struct sam4_chip_details all_sam4_details[] = {
 				.bank_number = 1,
 				.base_address = FLASH_BANK1_BASE_1024K_SD,
 				.controller_address = 0x400e0c00,
-				.flash_wait_states = 6,	/* workaround silicon bug */
+				.flash_wait_states = 5,
+				.present = 1,
+				.size_bytes =  512 * 1024,
+				.nsectors   =  64,
+				.sector_size = 8192,
+				.page_size   = 512,
+			},
+		},
+	},
+
+	/*at91sam4sd16b - LQFP64/BGA64*/
+	{
+		.chipid_cidr    = 0x29970ce0,
+		.name           = "at91sam4sd16b",
+		.total_flash_size     = 1024 * 1024,
+		.total_sram_size      = 160 * 1024,
+		.n_gpnvms       = 3,
+		.n_banks        = 2,
+
+/*		.bank[0] = { */
+		{
+			{
+				.probed = 0,
+				.pChip  = NULL,
+				.pBank  = NULL,
+				.bank_number = 0,
+				.base_address = FLASH_BANK0_BASE_SD,
+				.controller_address = 0x400e0a00,
+				.flash_wait_states = 5,
+				.present = 1,
+				.size_bytes =  512 * 1024,
+				.nsectors   =  64,
+				.sector_size = 8192,
+				.page_size   = 512,
+			},
+
+/*		.bank[1] = { */
+			{
+				.probed = 0,
+				.pChip  = NULL,
+				.pBank  = NULL,
+				.bank_number = 1,
+				.base_address = FLASH_BANK1_BASE_1024K_SD,
+				.controller_address = 0x400e0c00,
+				.flash_wait_states = 5,
 				.present = 1,
 				.size_bytes =  512 * 1024,
 				.nsectors   =  64,
@@ -895,6 +1340,74 @@ static const struct sam4_chip_details all_sam4_details[] = {
 		}
 	},
 
+	/* atsamg55g19 */
+	{
+		.chipid_cidr    = 0x24470ae0,
+		.name           = "atsamg55g19",
+		.total_flash_size     = 512 * 1024,
+		.total_sram_size      = 160 * 1024,
+		.n_gpnvms       = 2,
+		.n_banks        = 1,
+
+		{
+/*			.bank[0] = */
+			{
+				.probed = 0,
+				.pChip  = NULL,
+				.pBank  = NULL,
+				.bank_number = 0,
+				.base_address = FLASH_BANK_BASE_S,
+				.controller_address = 0x400e0a00,
+				.flash_wait_states = 5,
+				.present = 1,
+				.size_bytes =  512 * 1024,
+				.nsectors   =  64,
+				.sector_size = 8192,
+				.page_size   = 512,
+			},
+/*			.bank[1] = */
+			{
+				.present = 0,
+				.probed = 0,
+				.bank_number = 1,
+			},
+		}
+	},
+
+	/* atsamg55j19 */
+	{
+		.chipid_cidr    = 0x24570ae0,
+		.name           = "atsamg55j19",
+		.total_flash_size     = 512 * 1024,
+		.total_sram_size      = 160 * 1024,
+		.n_gpnvms       = 2,
+		.n_banks        = 1,
+
+		{
+/*			.bank[0] = */
+			{
+				.probed = 0,
+				.pChip  = NULL,
+				.pBank  = NULL,
+				.bank_number = 0,
+				.base_address = FLASH_BANK_BASE_S,
+				.controller_address = 0x400e0a00,
+				.flash_wait_states = 5,
+				.present = 1,
+				.size_bytes =  512 * 1024,
+				.nsectors   =  64,
+				.sector_size = 8192,
+				.page_size   = 512,
+			},
+/*			.bank[1] = */
+			{
+				.present = 0,
+				.probed = 0,
+				.bank_number = 1,
+			},
+		}
+	},
+
 	/* terminate */
 	{
 		.chipid_cidr    = 0,
@@ -1402,7 +1915,7 @@ static uint32_t sam4_reg_fieldname(struct sam4_chip *pChip,
 
 static const char _unknown[] = "unknown";
 static const char *const eproc_names[] = {
-	_unknown,					/* 0 */
+	"Cortex-M7",				/* 0 */
 	"arm946es",					/* 1 */
 	"arm7tdmi",					/* 2 */
 	"Cortex-M3",				/* 3 */
@@ -1430,7 +1943,7 @@ static const char *const nvpsize[] = {
 	"64K bytes",				/*  5 */
 	_unknown,					/*  6 */
 	"128K bytes",				/*  7 */
-	_unknown,					/*  8 */
+	"160K bytes",				/*  8 */
 	"256K bytes",				/*  9 */
 	"512K bytes",				/* 10 */
 	_unknown,					/* 11 */
@@ -1472,12 +1985,16 @@ static const struct archnames { unsigned value; const char *name; } archnames[]
 	{ 0x42,  "AT91x42 Series"                                                       },
 	{ 0x43,  "SAMG51 Series"
 	},
+	{ 0x44,  "SAMG55 Series (49-pin WLCSP)"                                         },
+	{ 0x45,  "SAMG55 Series (64-pin)"                                                        },
 	{ 0x47,  "SAMG53 Series"
 	},
 	{ 0x55,  "AT91x55 Series"                                                       },
 	{ 0x60,  "AT91SAM7Axx Series"                                           },
 	{ 0x61,  "AT91SAM7AQxx Series"                                          },
 	{ 0x63,  "AT91x63 Series"                                                       },
+	{ 0x64,  "SAM4CxxC (100-pin version)"                                           },
+	{ 0x66,  "SAM4CxxE (144-pin version)"                                           },
 	{ 0x70,  "AT91SAM7Sxx Series"                                           },
 	{ 0x71,  "AT91SAM7XCxx Series"                                          },
 	{ 0x72,  "AT91SAM7SExx Series"                                          },
@@ -1975,15 +2492,17 @@ FLASH_BANK_COMMAND_HANDLER(sam4_flash_bank_command)
 		/* at91sam4s series only has bank 0*/
 		/* at91sam4sd series has the same address for bank 0 (FLASH_BANK0_BASE_SD)*/
 		case FLASH_BANK_BASE_S:
+		case FLASH_BANK_BASE_C:
 			bank->driver_priv = &(pChip->details.bank[0]);
 			bank->bank_number = 0;
 			pChip->details.bank[0].pChip = pChip;
 			pChip->details.bank[0].pBank = bank;
 			break;
 
-		/* Bank 1 of at91sam4sd series */
+		/* Bank 1 of at91sam4sd/at91sam4c32 series */
 		case FLASH_BANK1_BASE_1024K_SD:
 		case FLASH_BANK1_BASE_2048K_SD:
+		case FLASH_BANK1_BASE_C32:
 			bank->driver_priv = &(pChip->details.bank[1]);
 			bank->bank_number = 1;
 			pChip->details.bank[1].pChip = pChip;
diff --git a/src/flash/nor/at91sam4l.c b/src/flash/nor/at91sam4l.c
index 4710512..0a605d5 100644
--- a/src/flash/nor/at91sam4l.c
+++ b/src/flash/nor/at91sam4l.c
@@ -645,10 +645,15 @@ static int sam4l_write(struct flash_bank *bank, const uint8_t *buffer,
 COMMAND_HANDLER(sam4l_handle_reset_deassert)
 {
 	struct target *target = get_current_target(CMD_CTX);
-	struct armv7m_common *armv7m = target_to_armv7m(target);
 	int retval = ERROR_OK;
 	enum reset_types jtag_reset_config = jtag_get_reset_config();
 
+	/* If the target has been unresponsive before, try to re-establish
+	 * communication now - CPU is held in reset by DSU, DAP is working */
+	if (!target_was_examined(target))
+		target_examine_one(target);
+	target_poll(target);
+
 	/* In case of sysresetreq, debug retains state set in cortex_m_assert_reset()
 	 * so we just release reset held by SMAP
 	 *
@@ -657,14 +662,14 @@ COMMAND_HANDLER(sam4l_handle_reset_deassert)
 	 * After vectreset SMAP release is not needed however makes no harm
 	 */
 	if (target->reset_halt && (jtag_reset_config & RESET_HAS_SRST)) {
-		retval = mem_ap_write_u32(armv7m->debug_ap, DCB_DHCSR, DBGKEY | C_HALT | C_DEBUGEN);
+		retval = target_write_u32(target, DCB_DHCSR, DBGKEY | C_HALT | C_DEBUGEN);
 		if (retval == ERROR_OK)
-			retval = mem_ap_write_atomic_u32(armv7m->debug_ap, DCB_DEMCR,
+			retval = target_write_u32(target, DCB_DEMCR,
 				TRCENA | VC_HARDERR | VC_BUSERR | VC_CORERESET);
 		/* do not return on error here, releasing SMAP reset is more important */
 	}
 
-	int retval2 = mem_ap_write_atomic_u32(armv7m->debug_ap, SMAP_SCR, SMAP_SCR_HCR);
+	int retval2 = target_write_u32(target, SMAP_SCR, SMAP_SCR_HCR);
 	if (retval2 != ERROR_OK)
 		return retval2;
 
diff --git a/src/flash/nor/at91sam7.c b/src/flash/nor/at91sam7.c
index ccb1a74..03f771c 100644
--- a/src/flash/nor/at91sam7.c
+++ b/src/flash/nor/at91sam7.c
@@ -661,7 +661,7 @@ static int at91sam7_erase_check(struct flash_bank *bank)
 		retval = target_blank_check_memory(target,
 				bank->base + bank->sectors[nSector].offset,
 				bank->sectors[nSector].size,
-				&blank);
+				&blank, bank->erased_value);
 		if (retval != ERROR_OK) {
 			fast_check = 0;
 			break;
diff --git a/src/flash/nor/at91samd.c b/src/flash/nor/at91samd.c
index 58b367a..f018e89 100644
--- a/src/flash/nor/at91samd.c
+++ b/src/flash/nor/at91samd.c
@@ -36,6 +36,7 @@
 
 #define SAMD_DSU_STATUSA        1               /* DSU status register */
 #define SAMD_DSU_DID		0x18		/* Device ID register */
+#define SAMD_DSU_CTRL_EXT	0x100		/* CTRL register, external access */
 
 #define SAMD_NVMCTRL_CTRLA		0x00	/* NVM control A register */
 #define SAMD_NVMCTRL_CTRLB		0x04	/* NVM control B register */
@@ -423,39 +424,43 @@ static int samd_probe(struct flash_bank *bank)
 	return ERROR_OK;
 }
 
-static bool samd_check_error(struct target *target)
+static int samd_check_error(struct target *target)
 {
-	int ret;
-	bool error;
+	int ret, ret2;
 	uint16_t status;
 
 	ret = target_read_u16(target,
 			SAMD_NVMCTRL + SAMD_NVMCTRL_STATUS, &status);
 	if (ret != ERROR_OK) {
 		LOG_ERROR("Can't read NVM status");
-		return true;
+		return ret;
 	}
 
-	if (status & 0x001C) {
-		if (status & (1 << 4)) /* NVME */
-			LOG_ERROR("SAMD: NVM Error");
-		if (status & (1 << 3)) /* LOCKE */
-			LOG_ERROR("SAMD: NVM lock error");
-		if (status & (1 << 2)) /* PROGE */
-			LOG_ERROR("SAMD: NVM programming error");
+	if ((status & 0x001C) == 0)
+		return ERROR_OK;
 
-		error = true;
-	} else {
-		error = false;
+	if (status & (1 << 4)) { /* NVME */
+		LOG_ERROR("SAMD: NVM Error");
+		ret = ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	if (status & (1 << 3)) { /* LOCKE */
+		LOG_ERROR("SAMD: NVM lock error");
+		ret = ERROR_FLASH_PROTECTED;
+	}
+
+	if (status & (1 << 2)) { /* PROGE */
+		LOG_ERROR("SAMD: NVM programming error");
+		ret = ERROR_FLASH_OPER_UNSUPPORTED;
 	}
 
 	/* Clear the error conditions by writing a one to them */
-	ret = target_write_u16(target,
+	ret2 = target_write_u16(target,
 			SAMD_NVMCTRL + SAMD_NVMCTRL_STATUS, status);
-	if (ret != ERROR_OK)
+	if (ret2 != ERROR_OK)
 		LOG_ERROR("Can't clear NVM error conditions");
 
-	return error;
+	return ret;
 }
 
 static int samd_issue_nvmctrl_command(struct target *target, uint16_t cmd)
@@ -474,10 +479,7 @@ static int samd_issue_nvmctrl_command(struct target *target, uint16_t cmd)
 		return res;
 
 	/* Check to see if the NVM command resulted in an error condition. */
-	if (samd_check_error(target))
-		return ERROR_FAIL;
-
-	return ERROR_OK;
+	return samd_check_error(target);
 }
 
 static int samd_erase_row(struct target *target, uint32_t address)
@@ -531,12 +533,19 @@ static int samd_modify_user_row(struct target *target, uint32_t value,
 		uint8_t startb, uint8_t endb)
 {
 	int res;
+	uint32_t nvm_ctrlb;
+	bool manual_wp = true;
 
 	if (is_user_row_reserved_bit(startb) || is_user_row_reserved_bit(endb)) {
 		LOG_ERROR("Can't modify bits in the requested range");
 		return ERROR_FAIL;
 	}
 
+	/* Check if we need to do manual page write commands */
+	res = target_read_u32(target, SAMD_NVMCTRL + SAMD_NVMCTRL_CTRLB, &nvm_ctrlb);
+	if (res == ERROR_OK)
+		manual_wp = (nvm_ctrlb & SAMD_NVM_CTRLB_MANW) != 0;
+
 	/* Retrieve the MCU's page size, in bytes. This is also the size of the
 	 * entire User Row. */
 	uint32_t page_size;
@@ -559,8 +568,8 @@ static int samd_modify_user_row(struct target *target, uint32_t value,
 	if (!buf)
 		return ERROR_FAIL;
 
-	/* Read the user row (comprising one page) by half-words. */
-	res = target_read_memory(target, SAMD_USER_ROW, 2, page_size / 2, buf);
+	/* Read the user row (comprising one page) by words. */
+	res = target_read_memory(target, SAMD_USER_ROW, 4, page_size / 4, buf);
 	if (res != ERROR_OK)
 		goto out_user_row;
 
@@ -579,20 +588,18 @@ static int samd_modify_user_row(struct target *target, uint32_t value,
 	/* Modify */
 	buf_set_u32(buf, startb, endb - startb + 1, value);
 
-	/* Write the page buffer back out to the target.  A Flash write will be
-	 * triggered automatically. */
+	/* Write the page buffer back out to the target. */
 	res = target_write_memory(target, SAMD_USER_ROW, 4, page_size / 4, buf);
 	if (res != ERROR_OK)
 		goto out_user_row;
 
-	if (samd_check_error(target)) {
-		res = ERROR_FAIL;
-		goto out_user_row;
+	if (manual_wp) {
+		/* Trigger flash write */
+		res = samd_issue_nvmctrl_command(target, SAMD_NVM_CMD_WAP);
+	} else {
+		res = samd_check_error(target);
 	}
 
-	/* Success */
-	res = ERROR_OK;
-
 out_user_row:
 	free(buf);
 
@@ -784,18 +791,15 @@ static int samd_write(struct flash_bank *bank, const uint8_t *buffer,
 		 * then issue CMD_WP always */
 		if (manual_wp || pg_offset + 4 * nw < chip->page_size) {
 			res = samd_issue_nvmctrl_command(bank->target, SAMD_NVM_CMD_WP);
-			if (res != ERROR_OK) {
-				LOG_ERROR("%s: %d", __func__, __LINE__);
-				goto free_pb;
-			}
-		}
+		} else {
+			/* Access through AHB is stalled while flash is being programmed */
+			usleep(200);
 
-		/* Access through AHB is stalled while flash is being programmed */
-		usleep(200);
+			res = samd_check_error(bank->target);
+		}
 
-		if (samd_check_error(bank->target)) {
+		if (res != ERROR_OK) {
 			LOG_ERROR("%s: write failed at address 0x%08" PRIx32, __func__, address);
-			res = ERROR_FAIL;
 			goto free_pb;
 		}
 
@@ -856,18 +860,23 @@ COMMAND_HANDLER(samd_handle_info_command)
 COMMAND_HANDLER(samd_handle_chip_erase_command)
 {
 	struct target *target = get_current_target(CMD_CTX);
+	int res = ERROR_FAIL;
 
 	if (target) {
 		/* Enable access to the DSU by disabling the write protect bit */
 		target_write_u32(target, SAMD_PAC1, (1<<1));
+		/* intentionally without error checking - not accessible on secured chip */
+
 		/* Tell the DSU to perform a full chip erase.  It takes about 240ms to
 		 * perform the erase. */
-		target_write_u8(target, SAMD_DSU, (1<<4));
-
-		command_print(CMD_CTX, "chip erased");
+		res = target_write_u8(target, SAMD_DSU + SAMD_DSU_CTRL_EXT, (1<<4));
+		if (res == ERROR_OK)
+			command_print(CMD_CTX, "chip erase started");
+		else
+			command_print(CMD_CTX, "write to DSU CTRL failed");
 	}
 
-	return ERROR_OK;
+	return res;
 }
 
 COMMAND_HANDLER(samd_handle_set_security_command)
@@ -1018,10 +1027,15 @@ COMMAND_HANDLER(samd_handle_bootloader_command)
 COMMAND_HANDLER(samd_handle_reset_deassert)
 {
 	struct target *target = get_current_target(CMD_CTX);
-	struct armv7m_common *armv7m = target_to_armv7m(target);
 	int retval = ERROR_OK;
 	enum reset_types jtag_reset_config = jtag_get_reset_config();
 
+	/* If the target has been unresponsive before, try to re-establish
+	 * communication now - CPU is held in reset by DSU, DAP is working */
+	if (!target_was_examined(target))
+		target_examine_one(target);
+	target_poll(target);
+
 	/* In case of sysresetreq, debug retains state set in cortex_m_assert_reset()
 	 * so we just release reset held by DSU
 	 *
@@ -1030,9 +1044,9 @@ COMMAND_HANDLER(samd_handle_reset_deassert)
 	 * After vectreset DSU release is not needed however makes no harm
 	 */
 	if (target->reset_halt && (jtag_reset_config & RESET_HAS_SRST)) {
-		retval = mem_ap_write_u32(armv7m->debug_ap, DCB_DHCSR, DBGKEY | C_HALT | C_DEBUGEN);
+		retval = target_write_u32(target, DCB_DHCSR, DBGKEY | C_HALT | C_DEBUGEN);
 		if (retval == ERROR_OK)
-			retval = mem_ap_write_u32(armv7m->debug_ap, DCB_DEMCR,
+			retval = target_write_u32(target, DCB_DEMCR,
 				TRCENA | VC_HARDERR | VC_BUSERR | VC_CORERESET);
 		/* do not return on error here, releasing DSU reset is more important */
 	}
diff --git a/src/flash/nor/ath79.c b/src/flash/nor/ath79.c
new file mode 100644
index 0000000..451e843
--- /dev/null
+++ b/src/flash/nor/ath79.c
@@ -0,0 +1,901 @@
+/***************************************************************************
+ *   Copyright (C) 2015 by Tobias Diedrich                                 *
+ *   <ranma+openwrt@tdiedrich.de>                                          *
+ *                                                                         *
+ *   based on the stmsmi code written by Antonio Borneo                    *
+ *   <borneo.antonio@gmail.com>                                            *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.                                        *
+ *                                                                         *
+ ***************************************************************************/
+/*
+ * Driver for the Atheros AR7xxx/AR9xxx SPI flash interface.
+ *
+ * Since no SPI mode register is present, presumably only
+ * SPI "mode 3" (CPOL=1 and CPHA=1) is supported.
+ *
+ * The SPI interface supports up to 3 chip selects, however the SPI flash
+ * used for booting the system must be connected to CS0.
+ *
+ * On boot, the first 4MiB of flash space are memory-mapped into the
+ * area bf000000 - bfffffff (4 copies), so the MIPS bootstrap
+ * vector bfc00000 is mapped to the beginning of the flash.
+ *
+ * By writing a 1 to the REMAP_DISABLE bit in the SPI_CONTROL register,
+ * the full area of 16MiB is mapped.
+ *
+ * By writing a 0 to the SPI_FUNCTION_SELECT register (write-only dword
+ * register @bf000000), memory mapping is disabled and the SPI registers
+ * are exposed to the CPU instead:
+ * bf000000 SPI_FUNCTION_SELECT
+ * bf000004 SPI_CONTROL
+ * bf000008 SPI_IO_CONTROL
+ * bf00000c SPI_READ_DATA
+ *
+ * When not memory-mapped, the SPI interface is essentially bitbanged
+ * using SPI_CONTROL and SPI_IO_CONTROL with the only hardware-assistance
+ * being the 32bit read-only shift-register SPI_READ_DATA.
+ */
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "imp.h"
+#include "spi.h"
+#include <jtag/jtag.h>
+#include <helper/time_support.h>
+#include <helper/types.h>
+#include <target/mips32.h>
+#include <target/mips32_pracc.h>
+#include <target/target.h>
+
+#define BITS_PER_BYTE 8
+
+#define ATH79_REG_FS     0
+#define ATH79_REG_CLOCK  4
+#define ATH79_REG_WRITE  8
+#define ATH79_REG_DATA  12
+
+#define ATH79_SPI_CS_ALLHI 0x70000
+#define ATH79_SPI_CS0_HI   0x10000
+#define ATH79_SPI_CS1_HI   0x20000
+#define ATH79_SPI_CS2_HI   0x40000
+#define ATH79_SPI_CE_HI    0x00100
+#define ATH79_SPI_DO_HI    0x00001
+
+#define ATH79_XFER_FINAL   0x00000001
+#define ATH79_XFER_PARTIAL 0x00000000
+
+/* Timeout in ms */
+#define ATH79_MAX_TIMEOUT  (3000)
+
+struct ath79_spi_ctx {
+	uint8_t *page_buf;
+	int pre_deselect;
+	int post_deselect;
+};
+
+struct ath79_flash_bank {
+	int probed;
+	int chipselect;
+	uint32_t io_base;
+	const struct flash_device *dev;
+	struct ath79_spi_ctx spi;
+};
+
+struct ath79_target {
+	char *name;
+	uint32_t tap_idcode;
+	uint32_t io_base;
+};
+
+static const struct ath79_target target_devices[] = {
+	/* name,   tap_idcode, io_base */
+	{ "ATH79", 0x00000001, 0xbf000000 },
+	{ NULL,    0,          0 }
+};
+
+static const uint32_t ath79_chipselects[] = {
+	(~ATH79_SPI_CS0_HI & ATH79_SPI_CS_ALLHI),
+	(~ATH79_SPI_CS1_HI & ATH79_SPI_CS_ALLHI),
+	(~ATH79_SPI_CS2_HI & ATH79_SPI_CS_ALLHI),
+};
+
+static void ath79_pracc_addn(struct pracc_queue_info *ctx,
+			     const uint32_t *instr,
+			     int n)
+{
+	for (int i = 0; i < n; i++)
+		pracc_add(ctx, 0, instr[i]);
+}
+
+static int ath79_spi_bitbang_codegen(struct ath79_flash_bank *ath79_info,
+				     struct pracc_queue_info *ctx,
+				     uint8_t *data, int len,
+				     int partial_xfer)
+{
+	uint32_t cs_high = ATH79_SPI_CS_ALLHI;
+	uint32_t cs_low = ath79_chipselects[ath79_info->chipselect];
+	uint32_t clock_high = cs_low | ATH79_SPI_CE_HI;
+	uint32_t clock_low = cs_low;
+	uint32_t pracc_out = 0;
+	uint32_t io_base = ath79_info->io_base;
+
+	const uint32_t preamble1[] = {
+		/* $15 = MIPS32_PRACC_BASE_ADDR */
+		MIPS32_LUI(0, 15, PRACC_UPPER_BASE_ADDR),
+		/* $1 = io_base */
+		MIPS32_LUI(0, 1, UPPER16(io_base)),
+	};
+	ath79_pracc_addn(ctx, preamble1, ARRAY_SIZE(preamble1));
+	if (ath79_info->spi.pre_deselect) {
+		/* Clear deselect flag so we don't deselect again if
+		 * this is a partial xfer.
+		 */
+		ath79_info->spi.pre_deselect = 0;
+		const uint32_t pre_deselect[] = {
+			/* [$1 + FS] = 1  (enable flash io register access) */
+			MIPS32_LUI(0, 2, UPPER16(1)),
+			MIPS32_ORI(0, 2, 2, LOWER16(1)),
+			MIPS32_SW(0, 2, ATH79_REG_FS, 1),
+			/* deselect flash just in case */
+			/* $2 = SPI_CS_DIS */
+			MIPS32_LUI(0, 2, UPPER16(cs_high)),
+			MIPS32_ORI(0, 2, 2, LOWER16(cs_high)),
+			/* [$1 + WRITE] = $2 */
+			MIPS32_SW(0, 2, ATH79_REG_WRITE, 1),
+		};
+		ath79_pracc_addn(ctx, pre_deselect, ARRAY_SIZE(pre_deselect));
+	}
+	const uint32_t preamble2[] = {
+		/* t0 = CLOCK_LOW + 0-bit */
+		MIPS32_LUI(0, 8, UPPER16((clock_low + 0))),
+		MIPS32_ORI(0, 8, 8, LOWER16((clock_low + 0))),
+		/* t1 = CLOCK_LOW + 1-bit */
+		MIPS32_LUI(0, 9, UPPER16((clock_low + 1))),
+		MIPS32_ORI(0, 9, 9, LOWER16((clock_low + 1))),
+		/* t2 = CLOCK_HIGH + 0-bit */
+		MIPS32_LUI(0, 10, UPPER16((clock_high + 0))),
+		MIPS32_ORI(0, 10, 10, LOWER16((clock_high + 0))),
+		/* t3 = CLOCK_HIGH + 1-bit */
+		MIPS32_LUI(0, 11, UPPER16((clock_high + 1))),
+		MIPS32_ORI(0, 11, 11, LOWER16((clock_high + 1))),
+	};
+	ath79_pracc_addn(ctx, preamble2, ARRAY_SIZE(preamble2));
+
+	for (int i = 0; i < len; i++) {
+		uint8_t x = data[i];
+
+		/* Generate bitbang code for one byte, highest bit first .*/
+		for (int j = BITS_PER_BYTE - 1; j >= 0; j--) {
+			int bit = ((x >> j) & 1);
+
+			if (bit) {
+				/* [$1 + WRITE] = t1 */
+				pracc_add(ctx, 0,
+					  MIPS32_SW(0, 9, ATH79_REG_WRITE, 1));
+				/* [$1 + WRITE] = t3 */
+				pracc_add(ctx, 0,
+					  MIPS32_SW(0, 11, ATH79_REG_WRITE, 1));
+			} else {
+				/* [$1 + WRITE] = t0 */
+				pracc_add(ctx, 0,
+					  MIPS32_SW(0, 8, ATH79_REG_WRITE, 1));
+				/* [$1 + WRITE] = t2 */
+				pracc_add(ctx, 0,
+					  MIPS32_SW(0, 10, ATH79_REG_WRITE, 1));
+			}
+		}
+		if (i % 4 == 3) {
+			/* $3 = [$1 + DATA] */
+			pracc_add(ctx, 0, MIPS32_LW(0, 3, ATH79_REG_DATA, 1));
+			/* [OUTi] = $3 */
+			pracc_add(ctx, MIPS32_PRACC_PARAM_OUT + pracc_out,
+				  MIPS32_SW(0, 3, PRACC_OUT_OFFSET +
+				 pracc_out, 15));
+			pracc_out += 4;
+		}
+	}
+	if (len & 3) { /* not a multiple of 4 bytes */
+		/* $3 = [$1 + DATA] */
+		pracc_add(ctx, 0, MIPS32_LW(0, 3, ATH79_REG_DATA, 1));
+		/* [OUTi] = $3 */
+		pracc_add(ctx, MIPS32_PRACC_PARAM_OUT + pracc_out,
+			  MIPS32_SW(0, 3, PRACC_OUT_OFFSET + pracc_out, 15));
+		pracc_out += 4;
+	}
+
+	if (ath79_info->spi.post_deselect && !partial_xfer) {
+		const uint32_t post_deselect[] = {
+			/* $2 = SPI_CS_DIS */
+			MIPS32_LUI(0, 2, UPPER16(cs_high)),
+			MIPS32_ORI(0, 2, 2, LOWER16(cs_high)),
+			/* [$1 + WRITE] = $2 */
+			MIPS32_SW(0, 2, ATH79_REG_WRITE, 1),
+
+			/* [$1 + FS] = 0  (disable flash io register access) */
+			MIPS32_XORI(0, 2, 2, 0),
+			MIPS32_SW(0, 2, ATH79_REG_FS, 1),
+		};
+		ath79_pracc_addn(ctx, post_deselect, ARRAY_SIZE(post_deselect));
+	}
+
+	/* common pracc epilogue */
+	/* jump to start */
+	pracc_add(ctx, 0, MIPS32_B(0, NEG16(ctx->code_count + 1)));
+	/* restore $15 from DeSave */
+	pracc_add(ctx, 0, MIPS32_MFC0(0, 15, 31, 0));
+
+	return pracc_out / 4;
+}
+
+static int ath79_spi_bitbang_chunk(struct flash_bank *bank,
+				   uint8_t *data, int len, int *transferred)
+{
+	struct target *target = bank->target;
+	struct ath79_flash_bank *ath79_info = bank->driver_priv;
+	struct mips32_common *mips32 = target_to_mips32(target);
+	struct mips_ejtag *ejtag_info = &mips32->ejtag_info;
+	int pracc_words;
+
+	/*
+	 * These constants must match the worst case in the above code
+	 * generator function ath79_spi_bitbang_codegen.
+	 */
+	const int pracc_pre_post = 26;
+	const int pracc_loop_byte = 8 * 2 + 2;
+
+	struct pracc_queue_info ctx = {
+		.ejtag_info = ejtag_info
+	};
+	int max_len = (PRACC_MAX_INSTRUCTIONS - pracc_pre_post) / pracc_loop_byte;
+	int to_xfer = len > max_len ? max_len : len;
+	int partial_xfer = len != to_xfer;
+	int padded_len = (to_xfer + 3) & ~3;
+	uint32_t *out = malloc(padded_len);
+
+	if (!out) {
+		LOG_ERROR("not enough memory");
+		return ERROR_FAIL;
+	}
+
+	*transferred = 0;
+	pracc_queue_init(&ctx);
+
+	LOG_DEBUG("ath79_spi_bitbang_bytes(%p, %08x, %p, %d)",
+		  target, ath79_info->io_base, data, len);
+
+	LOG_DEBUG("max code %d => max len %d. to_xfer %d",
+		  PRACC_MAX_INSTRUCTIONS, max_len, to_xfer);
+
+	pracc_words = ath79_spi_bitbang_codegen(
+		ath79_info, &ctx, data, to_xfer, partial_xfer);
+
+	LOG_DEBUG("Assembled %d instructions, %d stores",
+		  ctx.code_count, ctx.store_count);
+
+	ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, out, 1);
+	if (ctx.retval != ERROR_OK)
+		goto exit;
+
+	if (to_xfer & 3) { /* Not a multiple of 4 bytes. */
+		/*
+		 * Need to realign last word since we didn't shift the
+		 * full 32 bits.
+		 */
+		int missed_bytes = 4 - (to_xfer & 3);
+
+		out[pracc_words - 1] <<= BITS_PER_BYTE * missed_bytes;
+	}
+
+	/*
+	 * pracc reads return uint32_t in host endianness, convert to
+	 * target endianness.
+	 * Since we know the ATH79 target is big endian and the SPI
+	 * shift register has the bytes in highest to lowest bit order,
+	 * this will ensure correct memory byte order regardless of host
+	 * endianness.
+	 */
+	target_buffer_set_u32_array(target, (uint8_t *)out, pracc_words, out);
+
+	if (LOG_LEVEL_IS(LOG_LVL_DEBUG)) {
+		for (int i = 0; i < to_xfer; i++) {
+			LOG_DEBUG("bitbang %02x => %02x",
+				  data[i], ((uint8_t *)out)[i]);
+		}
+	}
+	memcpy(data, out, to_xfer);
+	*transferred = to_xfer;
+
+exit:
+	pracc_queue_free(&ctx);
+	free(out);
+	return ctx.retval;
+}
+
+static void ath79_spi_bitbang_prepare(struct flash_bank *bank)
+{
+	struct ath79_flash_bank *ath79_info = bank->driver_priv;
+
+	ath79_info->spi.pre_deselect = 1;
+}
+
+static int ath79_spi_bitbang_bytes(struct flash_bank *bank,
+				   uint8_t *data, int len, uint32_t flags)
+{
+	struct ath79_flash_bank *ath79_info = bank->driver_priv;
+	int retval;
+	int transferred;
+
+	ath79_info->spi.post_deselect = !!(flags & ATH79_XFER_FINAL);
+
+	do {
+		transferred = 0;
+		retval = ath79_spi_bitbang_chunk(
+			bank, data, len, &transferred);
+		if (retval != ERROR_OK)
+			return retval;
+
+		data += transferred;
+		len -= transferred;
+	} while (len > 0);
+
+	return ERROR_OK;
+}
+
+FLASH_BANK_COMMAND_HANDLER(ath79_flash_bank_command)
+{
+	struct ath79_flash_bank *ath79_info;
+	int chipselect = 0;
+
+	LOG_DEBUG("%s", __func__);
+
+	if (CMD_ARGC < 6 || CMD_ARGC > 7)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	if (CMD_ARGC == 7) {
+		if (strcmp(CMD_ARGV[6], "cs0") == 0)
+			chipselect = 0;  /* default */
+		else if (strcmp(CMD_ARGV[6], "cs1") == 0)
+			chipselect = 1;
+		else if (strcmp(CMD_ARGV[6], "cs2") == 0)
+			chipselect = 2;
+		else {
+			LOG_ERROR("Unknown arg: %s", CMD_ARGV[6]);
+			return ERROR_COMMAND_SYNTAX_ERROR;
+		}
+	}
+
+	ath79_info = calloc(1, sizeof(struct ath79_flash_bank));
+	if (!ath79_info) {
+		LOG_ERROR("not enough memory");
+		return ERROR_FAIL;
+	}
+
+	ath79_info->chipselect = chipselect;
+	bank->driver_priv = ath79_info;
+
+	return ERROR_OK;
+}
+
+/* Read the status register of the external SPI flash chip. */
+static int read_status_reg(struct flash_bank *bank, uint32_t *status)
+{
+	uint8_t spi_bytes[] = {SPIFLASH_READ_STATUS, 0};
+	int retval;
+
+	/* Send SPI command "read STATUS" */
+	ath79_spi_bitbang_prepare(bank);
+	retval = ath79_spi_bitbang_bytes(
+		bank, spi_bytes, sizeof(spi_bytes),
+		ATH79_XFER_FINAL);
+
+	*status = spi_bytes[1];
+
+	return retval;
+}
+
+/* check for WIP (write in progress) bit in status register */
+/* timeout in ms */
+static int wait_till_ready(struct flash_bank *bank, int timeout)
+{
+	uint32_t status;
+	int retval;
+	long long endtime;
+
+	endtime = timeval_ms() + timeout;
+	do {
+		/* read flash status register */
+		retval = read_status_reg(bank, &status);
+		if (retval != ERROR_OK)
+			return retval;
+
+		if ((status & SPIFLASH_BSY_BIT) == 0)
+			return ERROR_OK;
+		alive_sleep(1);
+	} while (timeval_ms() < endtime);
+
+	LOG_ERROR("timeout");
+	return ERROR_FAIL;
+}
+
+/* Send "write enable" command to SPI flash chip. */
+static int ath79_write_enable(struct flash_bank *bank)
+{
+	uint32_t status;
+	int retval;
+
+	uint8_t spi_bytes[] = {SPIFLASH_WRITE_ENABLE};
+
+	/* Send SPI command "write enable" */
+	ath79_spi_bitbang_prepare(bank);
+	retval = ath79_spi_bitbang_bytes(
+		bank, spi_bytes, sizeof(spi_bytes),
+		ATH79_XFER_FINAL);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* read flash status register */
+	retval = read_status_reg(bank, &status);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Check write enabled */
+	if ((status & SPIFLASH_WE_BIT) == 0) {
+		LOG_ERROR("Cannot enable write to flash. Status=0x%08" PRIx32,
+			  status);
+		return ERROR_FAIL;
+	}
+
+	return ERROR_OK;
+}
+
+static int erase_command(struct flash_bank *bank, int sector)
+{
+	struct ath79_flash_bank *ath79_info = bank->driver_priv;
+	uint32_t offset = bank->sectors[sector].offset;
+
+	uint8_t spi_bytes[] = {
+		ath79_info->dev->erase_cmd,
+		offset >> 16,
+		offset >> 8,
+		offset
+	};
+
+	/* bitbang command */
+	ath79_spi_bitbang_prepare(bank);
+	return ath79_spi_bitbang_bytes(
+		bank, spi_bytes, sizeof(spi_bytes),
+		ATH79_XFER_FINAL);
+}
+
+static int ath79_erase_sector(struct flash_bank *bank, int sector)
+{
+	int retval = ath79_write_enable(bank);
+
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* send SPI command "block erase" */
+	retval = erase_command(bank, sector);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* poll WIP for end of self timed Sector Erase cycle */
+	return wait_till_ready(bank, ATH79_MAX_TIMEOUT);
+}
+
+static int ath79_erase(struct flash_bank *bank, int first, int last)
+{
+	struct target *target = bank->target;
+	struct ath79_flash_bank *ath79_info = bank->driver_priv;
+	int retval = ERROR_OK;
+	int sector;
+
+	LOG_DEBUG("%s: from sector %d to sector %d", __func__, first, last);
+
+	if (target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if ((first < 0) || (last < first) || (last >= bank->num_sectors)) {
+		LOG_ERROR("Flash sector invalid");
+		return ERROR_FLASH_SECTOR_INVALID;
+	}
+
+	if (!ath79_info->probed) {
+		LOG_ERROR("Flash bank not probed");
+		return ERROR_FLASH_BANK_NOT_PROBED;
+	}
+
+	for (sector = first; sector <= last; sector++) {
+		if (bank->sectors[sector].is_protected) {
+			LOG_ERROR("Flash sector %d protected", sector);
+			return ERROR_FAIL;
+		}
+	}
+
+	for (sector = first; sector <= last; sector++) {
+		retval = ath79_erase_sector(bank, sector);
+		if (retval != ERROR_OK)
+			break;
+		keep_alive();
+	}
+
+	return retval;
+}
+
+static int ath79_protect(struct flash_bank *bank, int set,
+			 int first, int last)
+{
+	int sector;
+
+	for (sector = first; sector <= last; sector++)
+		bank->sectors[sector].is_protected = set;
+	return ERROR_OK;
+}
+
+static int ath79_write_page(struct flash_bank *bank, const uint8_t *buffer,
+			    uint32_t address, uint32_t len)
+{
+	struct ath79_flash_bank *ath79_info = bank->driver_priv;
+	uint8_t spi_bytes[] = {
+		SPIFLASH_PAGE_PROGRAM,
+		address >> 16,
+		address >> 8,
+		address,
+	};
+	int retval;
+	uint32_t i;
+
+	if (address & 0xff) {
+		LOG_ERROR("ath79_write_page: unaligned write address: %08x",
+			  address);
+		return ERROR_FAIL;
+	}
+	if (!ath79_info->spi.page_buf) {
+		LOG_ERROR("ath79_write_page: page buffer not initialized");
+		return ERROR_FAIL;
+	}
+	if (len > ath79_info->dev->pagesize) {
+		LOG_ERROR("ath79_write_page: len bigger than page size %d: %d",
+			  ath79_info->dev->pagesize, len);
+		return ERROR_FAIL;
+	}
+
+	for (i = 0; i < len; i++) {
+		if (buffer[i] != 0xff)
+			break;
+	}
+	if (i == len)  /* all 0xff, no need to program. */
+		return ERROR_OK;
+
+	LOG_INFO("writing %d bytes to flash page @0x%08x", len, address);
+
+	memcpy(ath79_info->spi.page_buf, buffer, len);
+
+	/* unlock writes */
+	retval = ath79_write_enable(bank);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* bitbang command */
+	ath79_spi_bitbang_prepare(bank);
+	retval = ath79_spi_bitbang_bytes(
+		bank, spi_bytes, sizeof(spi_bytes),
+		ATH79_XFER_PARTIAL);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* write data */
+	return ath79_spi_bitbang_bytes(
+		bank, ath79_info->spi.page_buf, len,
+		ATH79_XFER_FINAL);
+}
+
+static int ath79_write_buffer(struct flash_bank *bank, const uint8_t *buffer,
+			      uint32_t address, uint32_t len)
+{
+	struct ath79_flash_bank *ath79_info = bank->driver_priv;
+	const uint32_t page_size = ath79_info->dev->pagesize;
+	int retval;
+
+	LOG_DEBUG("%s: address=0x%08" PRIx32 " len=0x%08" PRIx32,
+		  __func__, address, len);
+
+	while (len > 0) {
+		int page_len = len > page_size ? page_size : len;
+
+		retval = ath79_write_page(
+			bank, buffer, address, page_len);
+		if (retval != ERROR_OK)
+			return retval;
+
+		buffer += page_size;
+		address += page_size;
+		len -= page_len;
+	}
+
+	return ERROR_OK;
+}
+
+static int ath79_write(struct flash_bank *bank, const uint8_t *buffer,
+		       uint32_t offset, uint32_t count)
+{
+	struct target *target = bank->target;
+	int sector;
+
+	LOG_DEBUG("%s: offset=0x%08" PRIx32 " count=0x%08" PRIx32,
+		  __func__, offset, count);
+
+	if (offset < bank->base || offset >= bank->base + bank->size) {
+		LOG_ERROR("Start address out of range");
+		return ERROR_FAIL;
+	}
+
+	offset -= bank->base;
+
+	if (target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if (offset + count > bank->size) {
+		LOG_WARNING("Write pasts end of flash. Extra data discarded.");
+		count = bank->size - offset;
+	}
+
+	/* Check sector protection */
+	for (sector = 0; sector < bank->num_sectors; sector++) {
+		/* Start offset in or before this sector? */
+		/* End offset in or behind this sector? */
+		struct flash_sector *bs = &bank->sectors[sector];
+
+		if ((offset < (bs->offset + bs->size)) &&
+		    ((offset + count - 1) >= bs->offset) &&
+		    bs->is_protected) {
+			LOG_ERROR("Flash sector %d protected", sector);
+			return ERROR_FAIL;
+		}
+	}
+
+	return ath79_write_buffer(bank, buffer, offset, count);
+}
+
+static int ath79_read_buffer(struct flash_bank *bank, uint8_t *buffer,
+			     uint32_t address, uint32_t len)
+{
+	uint8_t spi_bytes[] = {
+		SPIFLASH_READ,
+		address >> 16,
+		address >> 8,
+		address,
+	};
+	int retval;
+
+	LOG_DEBUG("%s: address=0x%08" PRIx32 " len=0x%08" PRIx32,
+		  __func__, address, len);
+
+	if (address & 0xff) {
+		LOG_ERROR("ath79_read_buffer: unaligned read address: %08x",
+			  address);
+		return ERROR_FAIL;
+	}
+
+	LOG_INFO("reading %d bytes from flash @0x%08x", len, address);
+
+	/* bitbang command */
+	ath79_spi_bitbang_prepare(bank);
+	retval = ath79_spi_bitbang_bytes(
+		bank, spi_bytes, sizeof(spi_bytes), ATH79_XFER_PARTIAL);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* read data */
+	return ath79_spi_bitbang_bytes(
+		bank, buffer, len, ATH79_XFER_FINAL);
+}
+
+static int ath79_read(struct flash_bank *bank, uint8_t *buffer,
+		      uint32_t offset, uint32_t count)
+{
+	struct target *target = bank->target;
+
+	LOG_DEBUG("%s: offset=0x%08" PRIx32 " count=0x%08" PRIx32,
+		  __func__, offset, count);
+
+	if (offset < bank->base || offset >= bank->base + bank->size) {
+		LOG_ERROR("Start address out of range");
+		return ERROR_FAIL;
+	}
+
+	offset -= bank->base;
+
+	if (target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if (offset + count > bank->size) {
+		LOG_WARNING("Reads past end of flash. Extra data discarded.");
+		count = bank->size - offset;
+	}
+
+	return ath79_read_buffer(bank, buffer, offset, count);
+}
+
+/* Return ID of flash device */
+static int read_flash_id(struct flash_bank *bank, uint32_t *id)
+{
+	struct target *target = bank->target;
+	int retval;
+	uint8_t spi_bytes[] = {SPIFLASH_READ_ID, 0, 0, 0};
+
+	if (target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	/* Send SPI command "read ID" */
+	ath79_spi_bitbang_prepare(bank);
+	retval = ath79_spi_bitbang_bytes(
+		bank, spi_bytes, sizeof(spi_bytes), ATH79_XFER_FINAL);
+	if (retval != ERROR_OK)
+		return retval;
+
+	*id = (spi_bytes[1] << 0)
+		| (spi_bytes[2] << 8)
+		| (spi_bytes[3] << 16);
+
+	if (*id == 0xffffff) {
+		LOG_ERROR("No SPI flash found");
+		return ERROR_FAIL;
+	}
+
+	return ERROR_OK;
+}
+
+static int ath79_probe(struct flash_bank *bank)
+{
+	struct target *target = bank->target;
+	struct ath79_flash_bank *ath79_info = bank->driver_priv;
+	struct flash_sector *sectors;
+	uint32_t id = 0; /* silence uninitialized warning */
+	const struct ath79_target *target_device;
+	int retval;
+
+	if (ath79_info->probed) {
+		free(bank->sectors);
+		free(ath79_info->spi.page_buf);
+	}
+	ath79_info->probed = 0;
+
+	for (target_device = target_devices; target_device->name;
+		++target_device)
+		if (target_device->tap_idcode == target->tap->idcode)
+			break;
+	if (!target_device->name) {
+		LOG_ERROR("Device ID 0x%" PRIx32 " is not known",
+			  target->tap->idcode);
+		return ERROR_FAIL;
+	}
+
+	ath79_info->io_base = target_device->io_base;
+
+	LOG_DEBUG("Found device %s at address 0x%" PRIx32,
+		  target_device->name, bank->base);
+
+	retval = read_flash_id(bank, &id);
+	if (retval != ERROR_OK)
+		return retval;
+
+	ath79_info->dev = NULL;
+	for (const struct flash_device *p = flash_devices; p->name; p++)
+		if (p->device_id == id) {
+			ath79_info->dev = p;
+			break;
+		}
+
+	if (!ath79_info->dev) {
+		LOG_ERROR("Unknown flash device (ID 0x%08" PRIx32 ")", id);
+		return ERROR_FAIL;
+	}
+
+	LOG_INFO("Found flash device \'%s\' (ID 0x%08" PRIx32 ")",
+		 ath79_info->dev->name, ath79_info->dev->device_id);
+
+	/* Set correct size value */
+	bank->size = ath79_info->dev->size_in_bytes;
+
+	/* create and fill sectors array */
+	bank->num_sectors =
+		ath79_info->dev->size_in_bytes / ath79_info->dev->sectorsize;
+	sectors = calloc(1, sizeof(struct flash_sector) * bank->num_sectors);
+	if (!sectors) {
+		LOG_ERROR("not enough memory");
+		return ERROR_FAIL;
+	}
+	ath79_info->spi.page_buf = malloc(ath79_info->dev->pagesize);
+	if (!ath79_info->spi.page_buf) {
+		LOG_ERROR("not enough memory");
+		free(sectors);
+		return ERROR_FAIL;
+	}
+
+	for (int sector = 0; sector < bank->num_sectors; sector++) {
+		sectors[sector].offset = sector * ath79_info->dev->sectorsize;
+		sectors[sector].size = ath79_info->dev->sectorsize;
+		sectors[sector].is_erased = 0;
+		sectors[sector].is_protected = 1;
+	}
+
+	bank->sectors = sectors;
+	ath79_info->probed = 1;
+	return ERROR_OK;
+}
+
+static int ath79_auto_probe(struct flash_bank *bank)
+{
+	struct ath79_flash_bank *ath79_info = bank->driver_priv;
+
+	if (ath79_info->probed)
+		return ERROR_OK;
+	return ath79_probe(bank);
+}
+
+static int ath79_flash_blank_check(struct flash_bank *bank)
+{
+	/* Not implemented */
+	return ERROR_OK;
+}
+
+static int ath79_protect_check(struct flash_bank *bank)
+{
+	/* Not implemented */
+	return ERROR_OK;
+}
+
+static int get_ath79_info(struct flash_bank *bank, char *buf, int buf_size)
+{
+	struct ath79_flash_bank *ath79_info = bank->driver_priv;
+
+	if (!ath79_info->probed) {
+		snprintf(buf, buf_size,
+			 "\nATH79 flash bank not probed yet\n");
+		return ERROR_OK;
+	}
+
+	snprintf(buf, buf_size, "\nATH79 flash information:\n"
+		"  Device \'%s\' (ID 0x%08" PRIx32 ")\n",
+		ath79_info->dev->name, ath79_info->dev->device_id);
+
+	return ERROR_OK;
+}
+
+struct flash_driver ath79_flash = {
+	.name = "ath79",
+	.flash_bank_command = ath79_flash_bank_command,
+	.erase = ath79_erase,
+	.protect = ath79_protect,
+	.write = ath79_write,
+	.read = ath79_read,
+	.probe = ath79_probe,
+	.auto_probe = ath79_auto_probe,
+	.erase_check = ath79_flash_blank_check,
+	.protect_check = ath79_protect_check,
+	.info = get_ath79_info,
+};
diff --git a/src/flash/nor/atsamv.c b/src/flash/nor/atsamv.c
index d21419d..73f0238 100644
--- a/src/flash/nor/atsamv.c
+++ b/src/flash/nor/atsamv.c
@@ -363,6 +363,9 @@ static int samv_probe(struct flash_bank *bank)
 
 	uint8_t nvm_size_code = (device_id >> 8) & 0xf;
 	switch (nvm_size_code) {
+		case 10:
+			bank->size = 512 * 1024;
+			break;
 		case 12:
 			bank->size = 1024 * 1024;
 			break;
diff --git a/src/flash/nor/avrf.c b/src/flash/nor/avrf.c
index 627418c..11cc3b2 100644
--- a/src/flash/nor/avrf.c
+++ b/src/flash/nor/avrf.c
@@ -66,6 +66,7 @@ static const struct avrf_type avft_chips_info[] = {
  *			eeprom_page_size, eeprom_page_num
  */
 	{"atmega128", 0x9702, 256, 512, 8, 512},
+	{"atmega128rfa1", 0xa701, 128, 512, 8, 512},
 	{"at90can128", 0x9781, 256, 512, 8, 512},
 	{"at90usb128", 0x9782, 256, 512, 8, 512},
 	{"atmega164p", 0x940a, 128, 128, 4, 128},
diff --git a/src/flash/nor/cfi.c b/src/flash/nor/cfi.c
index f7d8a90..ac0db82 100644
--- a/src/flash/nor/cfi.c
+++ b/src/flash/nor/cfi.c
@@ -1312,7 +1312,7 @@ static int cfi_intel_write_block(struct flash_bank *bank, const uint8_t *buffer,
 	busy_pattern_val  = cfi_command_val(bank, 0x80);
 	error_pattern_val = cfi_command_val(bank, 0x7e);
 
-	LOG_DEBUG("Using target buffer at 0x%08" PRIx32 " and of size 0x%04" PRIx32,
+	LOG_DEBUG("Using target buffer at " TARGET_ADDR_FMT " and of size 0x%04" PRIx32,
 		source->address, buffer_size);
 
 	/* Programming main loop */
@@ -1424,50 +1424,50 @@ static int cfi_spansion_write_block_mips(struct flash_bank *bank, const uint8_t
 
 	static const uint32_t mips_word_16_code[] = {
 		/* start:	*/
-		MIPS32_LHU(9, 0, 4),			/* lhu $t1, ($a0)		; out = &saddr */
-		MIPS32_ADDI(4, 4, 2),			/* addi $a0, $a0, 2		; saddr += 2 */
-		MIPS32_SH(13, 0, 12),			/* sh $t5, ($t4)		; *fl_unl_addr1 = fl_unl_cmd1 */
-		MIPS32_SH(15, 0, 14),			/* sh $t7, ($t6)		; *fl_unl_addr2 = fl_unl_cmd2 */
-		MIPS32_SH(7, 0, 12),			/* sh $a3, ($t4)		; *fl_unl_addr1 = fl_write_cmd */
-		MIPS32_SH(9, 0, 5),				/* sh $t1, ($a1)		; *daddr = out */
+		MIPS32_LHU(0, 9, 0, 4),		/* lhu $t1, ($a0)		; out = &saddr */
+		MIPS32_ADDI(0, 4, 4, 2),	/* addi $a0, $a0, 2		; saddr += 2 */
+		MIPS32_SH(0, 13, 0, 12),	/* sh $t5, ($t4)		; *fl_unl_addr1 = fl_unl_cmd1 */
+		MIPS32_SH(0, 15, 0, 14),	/* sh $t7, ($t6)		; *fl_unl_addr2 = fl_unl_cmd2 */
+		MIPS32_SH(0, 7, 0, 12),		/* sh $a3, ($t4)		; *fl_unl_addr1 = fl_write_cmd */
+		MIPS32_SH(0, 9, 0, 5),		/* sh $t1, ($a1)		; *daddr = out */
 		MIPS32_NOP,						/* nop */
 		/* busy:	*/
-		MIPS32_LHU(10, 0, 5),			/* lhu $t2, ($a1)		; temp1 = *daddr */
-		MIPS32_XOR(11, 9, 10),			/* xor $t3, $a0, $t2	; temp2 = out ^ temp1; */
-		MIPS32_AND(11, 8, 11),			/* and $t3, $t0, $t3	; temp2 = temp2 & DQ7mask */
-		MIPS32_BNE(11, 8, 13),			/* bne $t3, $t0, cont	; if (temp2 != DQ7mask) goto cont */
-		MIPS32_NOP,						/* nop									*/
-
-		MIPS32_SRL(10, 8, 2),			/* srl $t2,$t0,2		; temp1 = DQ7mask >> 2 */
-		MIPS32_AND(11, 10, 11),			/* and $t3, $t2, $t3	; temp2 = temp2 & temp1	*/
-		MIPS32_BNE(11, 10, NEG16(8)),	/* bne $t3, $t2, busy	; if (temp2 != temp1) goto busy	*/
-		MIPS32_NOP,						/* nop									*/
-
-		MIPS32_LHU(10, 0, 5),			/* lhu $t2, ($a1)		; temp1 = *daddr */
-		MIPS32_XOR(11, 9, 10),			/* xor $t3, $a0, $t2	; temp2 = out ^ temp1; */
-		MIPS32_AND(11, 8, 11),			/* and $t3, $t0, $t3	; temp2 = temp2 & DQ7mask */
-		MIPS32_BNE(11, 8, 4),			/* bne $t3, $t0, cont	; if (temp2 != DQ7mask) goto cont */
+		MIPS32_LHU(0, 10, 0, 5),		/* lhu $t2, ($a1)		; temp1 = *daddr */
+		MIPS32_XOR(0, 11, 9, 10),		/* xor $t3, $a0, $t2	; temp2 = out ^ temp1; */
+		MIPS32_AND(0, 11, 8, 11),		/* and $t3, $t0, $t3	; temp2 = temp2 & DQ7mask */
+		MIPS32_BNE(0, 11, 8, 13),		/* bne $t3, $t0, cont	; if (temp2 != DQ7mask) goto cont */
+		MIPS32_NOP,						/* nop					*/
+
+		MIPS32_SRL(0, 10, 8, 2),		/* srl $t2,$t0,2		; temp1 = DQ7mask >> 2 */
+		MIPS32_AND(0, 11, 10, 11),			/* and $t3, $t2, $t3	; temp2 = temp2 & temp1	*/
+		MIPS32_BNE(0, 11, 10, NEG16(8)),	/* bne $t3, $t2, busy	; if (temp2 != temp1) goto busy	*/
+		MIPS32_NOP,						/* nop					*/
+
+		MIPS32_LHU(0, 10, 0, 5),		/* lhu $t2, ($a1)		; temp1 = *daddr */
+		MIPS32_XOR(0, 11, 9, 10),		/* xor $t3, $a0, $t2	; temp2 = out ^ temp1; */
+		MIPS32_AND(0, 11, 8, 11),		/* and $t3, $t0, $t3	; temp2 = temp2 & DQ7mask */
+		MIPS32_BNE(0, 11, 8, 4),		/* bne $t3, $t0, cont	; if (temp2 != DQ7mask) goto cont */
 		MIPS32_NOP,						/* nop */
 
-		MIPS32_XOR(9, 9, 9),			/* xor $t1, $t1, $t1	; out = 0 */
-		MIPS32_BEQ(9, 0, 11),			/* beq $t1, $zero, done	; if (out == 0) goto done */
+		MIPS32_XOR(0, 9, 9, 9),			/* xor $t1, $t1, $t1	; out = 0 */
+		MIPS32_BEQ(0, 9, 0, 11),			/* beq $t1, $zero, done	; if (out == 0) goto done */
 		MIPS32_NOP,						/* nop */
 		/* cont:	*/
-		MIPS32_ADDI(6, 6, NEG16(1)),	/* addi, $a2, $a2, -1	; numwrites-- */
-		MIPS32_BNE(6, 0, 5),			/* bne $a2, $zero, cont2	; if (numwrite != 0) goto cont2 */
+		MIPS32_ADDI(0, 6, 6, NEG16(1)),	/* addi, $a2, $a2, -1	; numwrites-- */
+		MIPS32_BNE(0, 6, 0, 5),		/* bne $a2, $zero, cont2	; if (numwrite != 0) goto cont2 */
 		MIPS32_NOP,						/* nop */
 
-		MIPS32_LUI(9, 0),				/* lui $t1, 0 */
-		MIPS32_ORI(9, 9, 0x80),			/* ori $t1, $t1, 0x80	; out = 0x80 */
+		MIPS32_LUI(0, 9, 0),				/* lui $t1, 0 */
+		MIPS32_ORI(0, 9, 9, 0x80),			/* ori $t1, $t1, 0x80	; out = 0x80 */
 
-		MIPS32_B(4),					/* b done			; goto done */
+		MIPS32_B(0, 4),					/* b done			; goto done */
 		MIPS32_NOP,						/* nop */
 		/* cont2:	*/
-		MIPS32_ADDI(5, 5, 2),			/* addi $a0, $a0, 2	; daddr += 2 */
-		MIPS32_B(NEG16(33)),			/* b start			; goto start */
+		MIPS32_ADDI(0, 5, 5, 2),			/* addi $a0, $a0, 2	; daddr += 2 */
+		MIPS32_B(0, NEG16(33)),			/* b start			; goto start */
 		MIPS32_NOP,						/* nop */
 		/* done: */
-		MIPS32_SDBBP,					/* sdbbp			; break(); */
+		MIPS32_SDBBP(0),					/* sdbbp			; break(); */
 	};
 
 	mips32_info.common_magic = MIPS32_COMMON_MAGIC;
diff --git a/src/flash/nor/core.c b/src/flash/nor/core.c
index 7a62ba1..ab69a32 100644
--- a/src/flash/nor/core.c
+++ b/src/flash/nor/core.c
@@ -50,10 +50,17 @@ int flash_driver_erase(struct flash_bank *bank, int first, int last)
 int flash_driver_protect(struct flash_bank *bank, int set, int first, int last)
 {
 	int retval;
+	int num_blocks;
+
+	if (bank->num_prot_blocks)
+		num_blocks = bank->num_prot_blocks;
+	else
+		num_blocks = bank->num_sectors;
+
 
 	/* callers may not supply illegal parameters ... */
-	if (first < 0 || first > last || last >= bank->num_sectors) {
-		LOG_ERROR("illegal sector range");
+	if (first < 0 || first > last || last >= num_blocks) {
+		LOG_ERROR("illegal protection block range");
 		return ERROR_FAIL;
 	}
 
@@ -69,11 +76,11 @@ int flash_driver_protect(struct flash_bank *bank, int set, int first, int last)
 	 * the target could have reset, power cycled, been hot plugged,
 	 * the application could have run, etc.
 	 *
-	 * Drivers only receive valid sector range.
+	 * Drivers only receive valid protection block range.
 	 */
 	retval = bank->driver->protect(bank, set, first, last);
 	if (retval != ERROR_OK)
-		LOG_ERROR("failed setting protection for areas %d to %d", first, last);
+		LOG_ERROR("failed setting protection for blocks %d to %d", first, last);
 
 	return retval;
 }
@@ -288,7 +295,7 @@ static int default_flash_mem_blank_check(struct flash_bank *bank)
 				goto done;
 
 			for (nBytes = 0; nBytes < chunk; nBytes++) {
-				if (buffer[nBytes] != 0xFF) {
+				if (buffer[nBytes] != bank->erased_value) {
 					bank->sectors[i].is_erased = 0;
 					break;
 				}
@@ -319,12 +326,12 @@ int default_flash_blank_check(struct flash_bank *bank)
 		uint32_t address = bank->base + bank->sectors[i].offset;
 		uint32_t size = bank->sectors[i].size;
 
-		retval = target_blank_check_memory(target, address, size, &blank);
+		retval = target_blank_check_memory(target, address, size, &blank, bank->erased_value);
 		if (retval != ERROR_OK) {
 			fast_check = 0;
 			break;
 		}
-		if (blank == 0xFF)
+		if (blank == bank->erased_value)
 			bank->sectors[i].is_erased = 1;
 		else
 			bank->sectors[i].is_erased = 0;
diff --git a/src/flash/nor/core.h b/src/flash/nor/core.h
index 60d4483..338363e 100644
--- a/src/flash/nor/core.h
+++ b/src/flash/nor/core.h
@@ -90,6 +90,9 @@ struct flash_bank {
 	int chip_width; /**< Width of the chip in bytes (1,2,4 bytes) */
 	int bus_width; /**< Maximum bus width, in bytes (1,2,4 bytes) */
 
+	/** Erased value. Defaults to 0xFF. */
+	uint8_t erased_value;
+
 	/** Default padded value used, normally this matches the  flash
 	 * erased value. Defaults to 0xFF. */
 	uint8_t default_padded_value;
diff --git a/src/flash/nor/drivers.c b/src/flash/nor/drivers.c
index e7b6a88..56b451c 100644
--- a/src/flash/nor/drivers.c
+++ b/src/flash/nor/drivers.c
@@ -28,6 +28,7 @@ extern struct flash_driver at91sam4_flash;
 extern struct flash_driver at91sam4l_flash;
 extern struct flash_driver at91sam7_flash;
 extern struct flash_driver at91samd_flash;
+extern struct flash_driver ath79_flash;
 extern struct flash_driver atsamv_flash;
 extern struct flash_driver avr_flash;
 extern struct flash_driver cfi_flash;
@@ -81,6 +82,7 @@ static struct flash_driver *flash_drivers[] = {
 	&at91sam4l_flash,
 	&at91sam7_flash,
 	&at91samd_flash,
+	&ath79_flash,
 	&atsamv_flash,
 	&avr_flash,
 	&cfi_flash,
diff --git a/src/flash/nor/efm32.c b/src/flash/nor/efm32.c
index 0b33829..81c1a37 100644
--- a/src/flash/nor/efm32.c
+++ b/src/flash/nor/efm32.c
@@ -456,10 +456,10 @@ static int efm32x_read_lock_data(struct flash_bank *bank)
 	uint32_t *ptr = NULL;
 	int ret = 0;
 
-	assert(!(bank->num_sectors & 0x1f));
+	assert(bank->num_sectors > 0);
 
-	data_size = bank->num_sectors / 8; /* number of data bytes */
-	data_size /= 4; /* ...and data dwords */
+	/* calculate the number of 32-bit words to read (one lock bit per sector) */
+	data_size = (bank->num_sectors + 31) / 32;
 
 	ptr = efm32x_info->lb_page;
 
diff --git a/src/flash/nor/em357.c b/src/flash/nor/em357.c
index 1501562..a11743b 100644
--- a/src/flash/nor/em357.c
+++ b/src/flash/nor/em357.c
@@ -702,6 +702,11 @@ static int em357_probe(struct flash_bank *bank)
 			num_pages = 128;
 			page_size = 2048;
 			break;
+		case 0x80000:
+			/* 512k -- 256 2k pages */
+			num_pages = 256;
+			page_size = 2048;
+			break;
 		default:
 			LOG_WARNING("No size specified for em357 flash driver, assuming 192k!");
 			num_pages = 96;
diff --git a/src/flash/nor/fm4.c b/src/flash/nor/fm4.c
index c348c1d..c8fe8b6 100644
--- a/src/flash/nor/fm4.c
+++ b/src/flash/nor/fm4.c
@@ -272,7 +272,7 @@ static int fm4_flash_write(struct flash_bank *bank, const uint8_t *buffer,
 		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,
+		LOG_DEBUG("copying %" PRId32 " bytes to SRAM 0x%08" TARGET_PRIxADDR,
 			MIN(halfwords * 2, byte_count), data_workarea->address);
 
 		retval = target_write_buffer(target, data_workarea->address,
diff --git a/src/flash/nor/kinetis.c b/src/flash/nor/kinetis.c
index e9cf8fa..7e9bbde 100644
--- a/src/flash/nor/kinetis.c
+++ b/src/flash/nor/kinetis.c
@@ -102,6 +102,7 @@
 #define WDOG_STCTRH	0x40052000
 #define SMC_PMCTRL	0x4007E001
 #define SMC_PMSTAT	0x4007E003
+#define MCM_PLACR	0xF000300C
 
 /* Values */
 #define PM_STAT_RUN		0x01
@@ -206,6 +207,7 @@
 #define KINETIS_SDID_FAMILYID_K4X   0x40000000
 #define KINETIS_SDID_FAMILYID_K6X   0x60000000
 #define KINETIS_SDID_FAMILYID_K7X   0x70000000
+#define KINETIS_SDID_FAMILYID_K8X   0x80000000
 
 struct kinetis_flash_bank {
 	bool probed;
@@ -231,7 +233,8 @@ struct kinetis_flash_bank {
 		FS_PROGRAM_SECTOR = 1,
 		FS_PROGRAM_LONGWORD = 2,
 		FS_PROGRAM_PHRASE = 4, /* Unsupported */
-		FS_INVALIDATE_CACHE = 8,
+		FS_INVALIDATE_CACHE_K = 8,
+		FS_INVALIDATE_CACHE_L = 0x10,
 	} flash_support;
 };
 
@@ -609,6 +612,9 @@ COMMAND_HANDLER(kinetis_check_flash_security_status)
 		return ERROR_OK;
 	}
 
+	if (!dap->ops)
+		return ERROR_OK;	/* too early to check, in JTAG mode ops may not be initialised */
+
 	uint32_t val;
 	int retval;
 
@@ -623,7 +629,7 @@ COMMAND_HANDLER(kinetis_check_flash_security_status)
 	}
 
 	if (val == 0)
-		return ERROR_OK;
+		return ERROR_OK;	/* dap not yet initialised */
 
 	bool found = false;
 	for (size_t i = 0; i < ARRAY_SIZE(kinetis_known_mdm_ids); i++) {
@@ -862,65 +868,7 @@ static int kinetis_ftfx_prepare(struct target *target)
 
 /* Kinetis Program-LongWord Microcodes */
 static const uint8_t kinetis_flash_write_code[] = {
-	/* Params:
-	 * r0 - workarea buffer
-	* r1 - target address
-	* r2 - wordcount
-	* Clobbered:
-	* r4 - tmp
-	* r5 - tmp
-	* r6 - tmp
-	* r7 - tmp
-	*/
-
-							/* .L1: */
-						/* for(register uint32_t i=0;i<wcount;i++){ */
-	0x04, 0x1C,					/* mov    r4, r0          */
-	0x00, 0x23,					/* mov    r3, #0          */
-							/* .L2: */
-	0x0E, 0x1A,					/* sub    r6, r1, r0      */
-	0xA6, 0x19,					/* add    r6, r4, r6      */
-	0x93, 0x42,					/* cmp    r3, r2          */
-	0x16, 0xD0,					/* beq    .L9             */
-							/* .L5: */
-						/* while((FTFx_FSTAT&FTFA_FSTAT_CCIF_MASK) != FTFA_FSTAT_CCIF_MASK){}; */
-	0x0B, 0x4D,					/* ldr    r5, .L10        */
-	0x2F, 0x78,					/* ldrb   r7, [r5]        */
-	0x7F, 0xB2,					/* sxtb   r7, r7          */
-	0x00, 0x2F,					/* cmp    r7, #0          */
-	0xFA, 0xDA,					/* bge    .L5             */
-						/* FTFx_FSTAT = FTFA_FSTAT_ACCERR_MASK|FTFA_FSTAT_FPVIOL_MASK|FTFA_FSTAT_RDCO */
-	0x70, 0x27,					/* mov    r7, #112        */
-	0x2F, 0x70,					/* strb   r7, [r5]        */
-						/* FTFx_FCCOB3 = faddr; */
-	0x09, 0x4F,					/* ldr    r7, .L10+4      */
-	0x3E, 0x60,					/* str    r6, [r7]        */
-	0x06, 0x27,					/* mov    r7, #6          */
-						/* FTFx_FCCOB0 = 0x06;  */
-	0x08, 0x4E,					/* ldr    r6, .L10+8      */
-	0x37, 0x70,					/* strb   r7, [r6]        */
-						/* FTFx_FCCOB7 = *pLW;  */
-	0x80, 0xCC,					/* ldmia  r4!, {r7}       */
-	0x08, 0x4E,					/* ldr    r6, .L10+12     */
-	0x37, 0x60,					/* str    r7, [r6]        */
-						/* FTFx_FSTAT = FTFA_FSTAT_CCIF_MASK; */
-	0x80, 0x27,					/* mov    r7, #128        */
-	0x2F, 0x70,					/* strb   r7, [r5]        */
-							/* .L4: */
-						/* while((FTFx_FSTAT&FTFA_FSTAT_CCIF_MASK) != FTFA_FSTAT_CCIF_MASK){}; */
-	0x2E, 0x78,					/* ldrb    r6, [r5]       */
-	0x77, 0xB2,					/* sxtb    r7, r6         */
-	0x00, 0x2F,					/* cmp     r7, #0         */
-	0xFB, 0xDA,					/* bge     .L4            */
-	0x01, 0x33,					/* add     r3, r3, #1     */
-	0xE4, 0xE7,					/* b       .L2            */
-							/* .L9: */
-	0x00, 0xBE,					/* bkpt #0                */
-							/* .L10: */
-	0x00, 0x00, 0x02, 0x40,		/* .word    1073872896    */
-	0x04, 0x00, 0x02, 0x40,		/* .word    1073872900    */
-	0x07, 0x00, 0x02, 0x40,		/* .word    1073872903    */
-	0x08, 0x00, 0x02, 0x40,		/* .word    1073872904    */
+#include "../../../contrib/loaders/flash/kinetis/kinetis_flash.inc"
 };
 
 /* Program LongWord Block Write */
@@ -933,20 +881,11 @@ static int kinetis_write_block(struct flash_bank *bank, const uint8_t *buffer,
 	struct working_area *source;
 	struct kinetis_flash_bank *kinfo = bank->driver_priv;
 	uint32_t address = kinfo->prog_base + offset;
-	struct reg_param reg_params[3];
+	uint32_t end_address;
+	struct reg_param reg_params[5];
 	struct armv7m_algorithm armv7m_info;
-	int retval = ERROR_OK;
-
-	/* Params:
-	 * r0 - workarea buffer
-	 * r1 - target address
-	 * r2 - wordcount
-	 * Clobbered:
-	 * r4 - tmp
-	 * r5 - tmp
-	 * r6 - tmp
-	 * r7 - tmp
-	 */
+	int retval;
+	uint8_t fstat;
 
 	/* Increase buffer_size if needed */
 	if (buffer_size < (target->working_area_size/2))
@@ -979,35 +918,39 @@ static int kinetis_write_block(struct flash_bank *bank, const uint8_t *buffer,
 	armv7m_info.common_magic = ARMV7M_COMMON_MAGIC;
 	armv7m_info.core_mode = ARM_MODE_THREAD;
 
-	init_reg_param(&reg_params[0], "r0", 32, PARAM_OUT); /* *pLW (*buffer) */
-	init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT); /* faddr */
-	init_reg_param(&reg_params[2], "r2", 32, PARAM_OUT); /* number of words to program */
+	init_reg_param(&reg_params[0], "r0", 32, PARAM_IN_OUT); /* address */
+	init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT); /* word count */
+	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);
 
-	/* write code buffer and use Flash programming code within kinetis       */
-	/* Set breakpoint to 0 with time-out of 1000 ms                          */
-	while (wcount > 0) {
-		uint32_t thisrun_count = (wcount > (buffer_size / 4)) ? (buffer_size / 4) : wcount;
+	buf_set_u32(reg_params[0].value, 0, 32, address);
+	buf_set_u32(reg_params[1].value, 0, 32, wcount);
+	buf_set_u32(reg_params[2].value, 0, 32, source->address);
+	buf_set_u32(reg_params[3].value, 0, 32, source->address + source->size);
+	buf_set_u32(reg_params[4].value, 0, 32, FTFx_FSTAT);
 
-		retval = target_write_buffer(target, source->address, thisrun_count * 4, buffer);
-		if (retval != ERROR_OK)
-			break;
+	retval = target_run_flash_async_algorithm(target, buffer, wcount, 4,
+						0, NULL,
+						5, reg_params,
+						source->address, source->size,
+						write_algorithm->address, 0,
+						&armv7m_info);
 
-		buf_set_u32(reg_params[0].value, 0, 32, source->address);
-		buf_set_u32(reg_params[1].value, 0, 32, address);
-		buf_set_u32(reg_params[2].value, 0, 32, thisrun_count);
+	if (retval == ERROR_FLASH_OPERATION_FAILED) {
+		end_address = buf_get_u32(reg_params[0].value, 0, 32);
 
-		retval = target_run_algorithm(target, 0, NULL, 3, reg_params,
-				write_algorithm->address, 0, 100000, &armv7m_info);
-		if (retval != ERROR_OK) {
-			LOG_ERROR("Error executing kinetis Flash programming algorithm");
-			retval = ERROR_FLASH_OPERATION_FAILED;
-			break;
-		}
+		LOG_ERROR("Error writing flash at %08" PRIx32, end_address);
 
-		buffer += thisrun_count * 4;
-		address += thisrun_count * 4;
-		wcount -= thisrun_count;
-	}
+		retval = target_read_u8(target, FTFx_FSTAT, &fstat);
+		if (retval == ERROR_OK) {
+			retval = kinetis_ftfx_decode_error(fstat);
+
+			/* reset error flags */
+			target_write_u8(target, FTFx_FSTAT, 0x70);
+		}
+	} else if (retval != ERROR_OK)
+		LOG_ERROR("Error executing kinetis Flash programming algorithm");
 
 	target_free_working_area(target, source);
 	target_free_working_area(target, write_algorithm);
@@ -1015,6 +958,8 @@ static int kinetis_write_block(struct flash_bank *bank, const uint8_t *buffer,
 	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;
 }
@@ -1236,12 +1181,13 @@ static int kinetis_check_run_mode(struct target *target)
 static void kinetis_invalidate_flash_cache(struct flash_bank *bank)
 {
 	struct kinetis_flash_bank *kinfo = bank->driver_priv;
-	uint8_t pfb01cr_byte2 = 0xf0;
 
-	if (!(kinfo->flash_support & FS_INVALIDATE_CACHE))
-		return;
+	if (kinfo->flash_support & FS_INVALIDATE_CACHE_K)
+		target_write_u8(bank->target, FMC_PFB01CR + 2, 0xf0);
+
+	else if (kinfo->flash_support & FS_INVALIDATE_CACHE_L)
+		target_write_u8(bank->target, MCM_PLACR + 1, 0x04);
 
-	target_write_memory(bank->target, FMC_PFB01CR + 2, 1, 1, &pfb01cr_byte2);
 	return;
 }
 
@@ -1425,7 +1371,7 @@ static int kinetis_write_inner(struct flash_bank *bank, const uint8_t *buffer,
 	if (!(kinfo->flash_support & FS_PROGRAM_SECTOR)) {
 		/* fallback to longword write */
 		fallback = 1;
-		LOG_WARNING("This device supports Program Longword execution only.");
+		LOG_INFO("This device supports Program Longword execution only.");
 	} else {
 		result = kinetis_make_ram_ready(bank->target);
 		if (result != ERROR_OK) {
@@ -1604,7 +1550,7 @@ static int kinetis_probe(struct flash_bank *bank)
 			pflash_sector_size_bytes = 1<<10;
 			nvm_sector_size_bytes = 1<<10;
 			num_blocks = 2;
-			kinfo->flash_support = FS_PROGRAM_LONGWORD | FS_PROGRAM_SECTOR | FS_INVALIDATE_CACHE;
+			kinfo->flash_support = FS_PROGRAM_LONGWORD | FS_PROGRAM_SECTOR | FS_INVALIDATE_CACHE_K;
 			break;
 		case KINETIS_K_SDID_K10_M72:
 		case KINETIS_K_SDID_K20_M72:
@@ -1617,7 +1563,7 @@ static int kinetis_probe(struct flash_bank *bank)
 			pflash_sector_size_bytes = 2<<10;
 			nvm_sector_size_bytes = 1<<10;
 			num_blocks = 2;
-			kinfo->flash_support = FS_PROGRAM_LONGWORD | FS_PROGRAM_SECTOR | FS_INVALIDATE_CACHE;
+			kinfo->flash_support = FS_PROGRAM_LONGWORD | FS_PROGRAM_SECTOR | FS_INVALIDATE_CACHE_K;
 			kinfo->max_flash_prog_size = 1<<10;
 			break;
 		case KINETIS_K_SDID_K10_M100:
@@ -1633,7 +1579,7 @@ static int kinetis_probe(struct flash_bank *bank)
 			pflash_sector_size_bytes = 2<<10;
 			nvm_sector_size_bytes = 2<<10;
 			num_blocks = 2;
-			kinfo->flash_support = FS_PROGRAM_LONGWORD | FS_PROGRAM_SECTOR | FS_INVALIDATE_CACHE;
+			kinfo->flash_support = FS_PROGRAM_LONGWORD | FS_PROGRAM_SECTOR | FS_INVALIDATE_CACHE_K;
 			break;
 		case KINETIS_K_SDID_K21_M120:
 		case KINETIS_K_SDID_K22_M120:
@@ -1642,7 +1588,7 @@ static int kinetis_probe(struct flash_bank *bank)
 			kinfo->max_flash_prog_size = 1<<10;
 			nvm_sector_size_bytes = 4<<10;
 			num_blocks = 2;
-			kinfo->flash_support = FS_PROGRAM_PHRASE | FS_PROGRAM_SECTOR | FS_INVALIDATE_CACHE;
+			kinfo->flash_support = FS_PROGRAM_PHRASE | FS_PROGRAM_SECTOR | FS_INVALIDATE_CACHE_K;
 			break;
 		case KINETIS_K_SDID_K10_M120:
 		case KINETIS_K_SDID_K20_M120:
@@ -1652,7 +1598,7 @@ static int kinetis_probe(struct flash_bank *bank)
 			pflash_sector_size_bytes = 4<<10;
 			nvm_sector_size_bytes = 4<<10;
 			num_blocks = 4;
-			kinfo->flash_support = FS_PROGRAM_PHRASE | FS_PROGRAM_SECTOR | FS_INVALIDATE_CACHE;
+			kinfo->flash_support = FS_PROGRAM_PHRASE | FS_PROGRAM_SECTOR | FS_INVALIDATE_CACHE_K;
 			break;
 		default:
 			LOG_ERROR("Unsupported K-family FAMID");
@@ -1666,7 +1612,7 @@ static int kinetis_probe(struct flash_bank *bank)
 				/* K02FN64, K02FN128: FTFA, 2kB sectors */
 				pflash_sector_size_bytes = 2<<10;
 				num_blocks = 1;
-				kinfo->flash_support = FS_PROGRAM_LONGWORD | FS_INVALIDATE_CACHE;
+				kinfo->flash_support = FS_PROGRAM_LONGWORD | FS_INVALIDATE_CACHE_K;
 				break;
 
 			case KINETIS_SDID_FAMILYID_K2X | KINETIS_SDID_SUBFAMID_KX2: {
@@ -1681,7 +1627,7 @@ static int kinetis_probe(struct flash_bank *bank)
 					/* MK24FN1M */
 					pflash_sector_size_bytes = 4<<10;
 					num_blocks = 2;
-					kinfo->flash_support = FS_PROGRAM_PHRASE | FS_PROGRAM_SECTOR | FS_INVALIDATE_CACHE;
+					kinfo->flash_support = FS_PROGRAM_PHRASE | FS_PROGRAM_SECTOR | FS_INVALIDATE_CACHE_K;
 					kinfo->max_flash_prog_size = 1<<10;
 					break;
 				}
@@ -1691,7 +1637,7 @@ static int kinetis_probe(struct flash_bank *bank)
 					/* K22 with new-style SDID - smaller pflash with FTFA, 2kB sectors */
 					pflash_sector_size_bytes = 2<<10;
 					/* autodetect 1 or 2 blocks */
-					kinfo->flash_support = FS_PROGRAM_LONGWORD | FS_INVALIDATE_CACHE;
+					kinfo->flash_support = FS_PROGRAM_LONGWORD | FS_INVALIDATE_CACHE_K;
 					break;
 				}
 				LOG_ERROR("Unsupported Kinetis K22 DIEID");
@@ -1702,12 +1648,12 @@ static int kinetis_probe(struct flash_bank *bank)
 				if ((kinfo->sim_sdid & (KINETIS_SDID_DIEID_MASK)) == KINETIS_SDID_DIEID_K24FN256) {
 					/* K24FN256 - smaller pflash with FTFA */
 					num_blocks = 1;
-					kinfo->flash_support = FS_PROGRAM_LONGWORD | FS_INVALIDATE_CACHE;
+					kinfo->flash_support = FS_PROGRAM_LONGWORD | FS_INVALIDATE_CACHE_K;
 					break;
 				}
 				/* K24FN1M without errata 7534 */
 				num_blocks = 2;
-				kinfo->flash_support = FS_PROGRAM_PHRASE | FS_PROGRAM_SECTOR | FS_INVALIDATE_CACHE;
+				kinfo->flash_support = FS_PROGRAM_PHRASE | FS_PROGRAM_SECTOR | FS_INVALIDATE_CACHE_K;
 				kinfo->max_flash_prog_size = 1<<10;
 				break;
 
@@ -1721,7 +1667,7 @@ static int kinetis_probe(struct flash_bank *bank)
 				nvm_sector_size_bytes = 4<<10;
 				kinfo->max_flash_prog_size = 1<<10;
 				num_blocks = 2;
-				kinfo->flash_support = FS_PROGRAM_PHRASE | FS_PROGRAM_SECTOR | FS_INVALIDATE_CACHE;
+				kinfo->flash_support = FS_PROGRAM_PHRASE | FS_PROGRAM_SECTOR | FS_INVALIDATE_CACHE_K;
 				break;
 
 			case KINETIS_SDID_FAMILYID_K2X | KINETIS_SDID_SUBFAMID_KX6:
@@ -1732,8 +1678,18 @@ static int kinetis_probe(struct flash_bank *bank)
 				nvm_sector_size_bytes = 4<<10;
 				kinfo->max_flash_prog_size = 1<<10;
 				num_blocks = 4;
-				kinfo->flash_support = FS_PROGRAM_PHRASE | FS_PROGRAM_SECTOR | FS_INVALIDATE_CACHE;
+				kinfo->flash_support = FS_PROGRAM_PHRASE | FS_PROGRAM_SECTOR | FS_INVALIDATE_CACHE_K;
 				break;
+
+			case KINETIS_SDID_FAMILYID_K8X | KINETIS_SDID_SUBFAMID_KX0:
+			case KINETIS_SDID_FAMILYID_K8X | KINETIS_SDID_SUBFAMID_KX1:
+			case KINETIS_SDID_FAMILYID_K8X | KINETIS_SDID_SUBFAMID_KX2:
+				/* K80FN256, K81FN256, K82FN256 */
+				pflash_sector_size_bytes = 4<<10;
+				num_blocks = 1;
+				kinfo->flash_support = FS_PROGRAM_LONGWORD | FS_INVALIDATE_CACHE_K;
+				break;
+
 			default:
 				LOG_ERROR("Unsupported Kinetis FAMILYID SUBFAMID");
 			}
@@ -1744,7 +1700,7 @@ static int kinetis_probe(struct flash_bank *bank)
 			pflash_sector_size_bytes = 1<<10;
 			nvm_sector_size_bytes = 1<<10;
 			/* autodetect 1 or 2 blocks */
-			kinfo->flash_support = FS_PROGRAM_LONGWORD;
+			kinfo->flash_support = FS_PROGRAM_LONGWORD | FS_INVALIDATE_CACHE_L;
 			break;
 
 		case KINETIS_SDID_SERIESID_KV:
@@ -1754,14 +1710,14 @@ static int kinetis_probe(struct flash_bank *bank)
 				/* KV10: FTFA, 1kB sectors */
 				pflash_sector_size_bytes = 1<<10;
 				num_blocks = 1;
-				kinfo->flash_support = FS_PROGRAM_LONGWORD;
+				kinfo->flash_support = FS_PROGRAM_LONGWORD | FS_INVALIDATE_CACHE_L;
 				break;
 
 			case KINETIS_SDID_FAMILYID_K1X | KINETIS_SDID_SUBFAMID_KX1:
 				/* KV11: FTFA, 2kB sectors */
 				pflash_sector_size_bytes = 2<<10;
 				num_blocks = 1;
-				kinfo->flash_support = FS_PROGRAM_LONGWORD;
+				kinfo->flash_support = FS_PROGRAM_LONGWORD | FS_INVALIDATE_CACHE_L;
 				break;
 
 			case KINETIS_SDID_FAMILYID_K3X | KINETIS_SDID_SUBFAMID_KX0:
@@ -1770,7 +1726,7 @@ static int kinetis_probe(struct flash_bank *bank)
 				/* KV31: FTFA, 2kB sectors, 2 blocks */
 				pflash_sector_size_bytes = 2<<10;
 				/* autodetect 1 or 2 blocks */
-				kinfo->flash_support = FS_PROGRAM_LONGWORD | FS_INVALIDATE_CACHE;
+				kinfo->flash_support = FS_PROGRAM_LONGWORD | FS_INVALIDATE_CACHE_K;
 				break;
 
 			case KINETIS_SDID_FAMILYID_K4X | KINETIS_SDID_SUBFAMID_KX2:
@@ -1779,7 +1735,7 @@ static int kinetis_probe(struct flash_bank *bank)
 				/* KV4x: FTFA, 4kB sectors */
 				pflash_sector_size_bytes = 4<<10;
 				num_blocks = 1;
-				kinfo->flash_support = FS_PROGRAM_LONGWORD | FS_INVALIDATE_CACHE;
+				kinfo->flash_support = FS_PROGRAM_LONGWORD | FS_INVALIDATE_CACHE_K;
 				break;
 
 			default:
diff --git a/src/flash/nor/lpc2000.c b/src/flash/nor/lpc2000.c
index 76cd86b..9da5da2 100644
--- a/src/flash/nor/lpc2000.c
+++ b/src/flash/nor/lpc2000.c
@@ -259,6 +259,8 @@
 
 #define IAP_CODE_LEN 0x34
 
+#define LPC11xx_REG_SECTORS	24
+
 typedef enum {
 	lpc2000_v1,
 	lpc2000_v2,
@@ -554,14 +556,21 @@ static int lpc2000_build_sector_list(struct flash_bank *bank)
 			exit(-1);
 		}
 		lpc2000_info->cmd51_max_buffer = 512; /* smallest MCU in the series, LPC1110, has 1 kB of SRAM */
-		bank->num_sectors = bank->size / 4096;
+		unsigned int large_sectors = 0;
+		unsigned int normal_sectors = bank->size / 4096;
+
+		if (normal_sectors > LPC11xx_REG_SECTORS) {
+			large_sectors = (normal_sectors - LPC11xx_REG_SECTORS) / 8;
+			normal_sectors = LPC11xx_REG_SECTORS;
+		}
+
+		bank->num_sectors = normal_sectors + large_sectors;
 
 		bank->sectors = malloc(sizeof(struct flash_sector) * bank->num_sectors);
 
 		for (int i = 0; i < bank->num_sectors; i++) {
 			bank->sectors[i].offset = offset;
-			/* all sectors are 4kB-sized */
-			bank->sectors[i].size = 4 * 1024;
+			bank->sectors[i].size = (i < LPC11xx_REG_SECTORS ? 4 : 32) * 1024;
 			offset += bank->sectors[i].size;
 			bank->sectors[i].is_erased = -1;
 			bank->sectors[i].is_protected = 1;
@@ -679,7 +688,7 @@ static int lpc2000_iap_working_area_init(struct flash_bank *bank, struct working
 
 	int retval = target_write_memory(target, (*iap_working_area)->address, 4, 2, jump_gate);
 	if (retval != ERROR_OK) {
-		LOG_ERROR("Write memory at address 0x%8.8" PRIx32 " failed (check work_area definition)",
+		LOG_ERROR("Write memory at address 0x%8.8" TARGET_PRIxADDR " failed (check work_area definition)",
 				(*iap_working_area)->address);
 		target_free_working_area(target, *iap_working_area);
 	}
diff --git a/src/flash/nor/lpcspifi.c b/src/flash/nor/lpcspifi.c
index 4eb6cc3..943c151 100644
--- a/src/flash/nor/lpcspifi.c
+++ b/src/flash/nor/lpcspifi.c
@@ -186,7 +186,7 @@ static int lpcspifi_set_hw_mode(struct flash_bank *bank)
 		return retval;
 	}
 
-	LOG_DEBUG("Writing algorithm to working area at 0x%08" PRIx32,
+	LOG_DEBUG("Writing algorithm to working area at 0x%08" TARGET_PRIxADDR,
 		spifi_init_algorithm->address);
 	/* Write algorithm to working area */
 	retval = target_write_buffer(target,
diff --git a/src/flash/nor/mdr.c b/src/flash/nor/mdr.c
index 374cb6f..8ceb1bf 100644
--- a/src/flash/nor/mdr.c
+++ b/src/flash/nor/mdr.c
@@ -171,7 +171,8 @@ static int mdr_erase(struct flash_bank *bank, int first, int last)
 	if (retval != ERROR_OK)
 		goto reset_pg_and_lock;
 
-	if ((first == 0) && (last == (bank->num_sectors - 1))) {
+	if ((first == 0) && (last == (bank->num_sectors - 1)) &&
+		!mdr_info->mem_type) {
 		retval = mdr_mass_erase(bank);
 		goto reset_pg_and_lock;
 	}
diff --git a/src/flash/nor/nrf51.c b/src/flash/nor/nrf51.c
index 69bf666..7b7acf4 100644
--- a/src/flash/nor/nrf51.c
+++ b/src/flash/nor/nrf51.c
@@ -108,7 +108,6 @@ enum nrf51_nvmc_config_bits {
 
 struct nrf51_info {
 	uint32_t code_page_size;
-	uint32_t code_memory_size;
 
 	struct {
 		bool probed;
@@ -121,6 +120,7 @@ struct nrf51_info {
 
 struct nrf51_device_spec {
 	uint16_t hwid;
+	const char *part;
 	const char *variant;
 	const char *build_code;
 	unsigned int flash_size_kb;
@@ -142,30 +142,35 @@ static const struct nrf51_device_spec nrf51_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,
@@ -174,54 +179,63 @@ static const struct nrf51_device_spec nrf51_known_devices_table[] = {
 	/* 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,
@@ -230,62 +244,79 @@ static const struct nrf51_device_spec nrf51_known_devices_table[] = {
 	/* 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,
+	},
 
 	/* 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,
@@ -294,24 +325,28 @@ static const struct nrf51_device_spec nrf51_known_devices_table[] = {
 	/* 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,
@@ -320,42 +355,49 @@ static const struct nrf51_device_spec nrf51_known_devices_table[] = {
 	/* 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,
@@ -366,6 +408,7 @@ static const struct nrf51_device_spec nrf51_known_devices_table[] = {
 	   in the nRF51 Series Compatibility Matrix V1.0. */
 	{
 		.hwid		= 0x0071,
+		.part		= "51822",
 		.variant	= "QFAC",
 		.build_code	= "AB",
 		.flash_size_kb	= 256,
@@ -627,43 +670,46 @@ static int nrf51_probe(struct flash_bank *bank)
 			 * 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++)
+	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];
 			break;
 		}
+	}
 
 	if (!chip->bank[0].probed && !chip->bank[1].probed) {
 		if (spec)
-			LOG_INFO("nRF51822-%s(build code: %s) %ukB Flash",
-				 spec->variant, spec->build_code, spec->flash_size_kb);
+			LOG_INFO("nRF%s-%s(build code: %s) %ukB Flash",
+				 spec->part, spec->variant, spec->build_code,
+				 spec->flash_size_kb);
 		else
 			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,
-				      &chip->code_page_size);
+				&chip->code_page_size);
 		if (res != ERROR_OK) {
 			LOG_ERROR("Couldn't read code page size");
 			return res;
 		}
 
-		res = target_read_u32(chip->target, NRF51_FICR_CODESIZE,
-				      &chip->code_memory_size);
+		/* Note the register name is misleading,
+		 * NRF51_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);
 		if (res != ERROR_OK) {
 			LOG_ERROR("Couldn't read code memory size");
 			return res;
 		}
 
-		if (spec && chip->code_memory_size != spec->flash_size_kb) {
-			LOG_ERROR("Chip's reported Flash capacity does not match expected one");
-			return ERROR_FAIL;
-		}
+		bank->num_sectors = num_sectors;
+		bank->size = num_sectors * chip->code_page_size;
+
+		if (spec && bank->size / 1024 != spec->flash_size_kb)
+			LOG_WARNING("Chip's reported Flash capacity does not match expected one");
 
-		bank->size = chip->code_memory_size * 1024;
-		bank->num_sectors = bank->size / chip->code_page_size;
 		bank->sectors = calloc(bank->num_sectors,
 				       sizeof((bank->sectors)[0]));
 		if (!bank->sectors)
@@ -1272,7 +1318,7 @@ static int nrf51_info(struct flash_bank *bank, char *buf, int buf_size)
 		 "reset value for XTALFREQ: %"PRIx32"\n"
 		 "firmware id: 0x%04"PRIx32,
 		 ficr[0].value,
-		 ficr[1].value,
+		 (ficr[1].value * ficr[0].value) / 1024,
 		 (ficr[2].value == 0xFFFFFFFF) ? 0 : ficr[2].value / 1024,
 		 ((ficr[3].value & 0xFF) == 0x00) ? "present" : "not present",
 		 ficr[4].value,
diff --git a/src/flash/nor/pic32mx.c b/src/flash/nor/pic32mx.c
index de212ed..1f148fd 100644
--- a/src/flash/nor/pic32mx.c
+++ b/src/flash/nor/pic32mx.c
@@ -879,7 +879,6 @@ COMMAND_HANDLER(pic32mx_handle_pgm_word_command)
 
 COMMAND_HANDLER(pic32mx_handle_unlock_command)
 {
-	uint32_t mchip_cmd;
 	struct target *target = NULL;
 	struct mips_m4k_common *mips_m4k;
 	struct mips_ejtag *ejtag_info;
@@ -904,7 +903,7 @@ COMMAND_HANDLER(pic32mx_handle_unlock_command)
 	mips_ejtag_set_instr(ejtag_info, MTAP_COMMAND);
 
 	/* first check status of device */
-	mchip_cmd = MCHP_STATUS;
+	uint8_t mchip_cmd = MCHP_STATUS;
 	mips_ejtag_drscan_8(ejtag_info, &mchip_cmd);
 	if (mchip_cmd & (1 << 7)) {
 		/* device is not locked */
diff --git a/src/flash/nor/stm32f2x.c b/src/flash/nor/stm32f2x.c
index 1acab82..4d75095 100644
--- a/src/flash/nor/stm32f2x.c
+++ b/src/flash/nor/stm32f2x.c
@@ -110,6 +110,9 @@
 #define FLASH_ERASE_TIMEOUT 10000
 #define FLASH_WRITE_TIMEOUT 5
 
+/* Mass erase time can be as high as 32 s in x8 mode. */
+#define FLASH_MASS_ERASE_TIMEOUT 33000
+
 #define STM32_FLASH_BASE    0x40023c00
 #define STM32_FLASH_ACR     0x40023c00
 #define STM32_FLASH_KEYR    0x40023c04
@@ -399,8 +402,8 @@ static int stm32x_write_options(struct flash_bank *bank)
 	if (retval != ERROR_OK)
 		return retval;
 
-	/* wait for completion */
-	retval = stm32x_wait_status_busy(bank, FLASH_ERASE_TIMEOUT);
+	/* wait for completion, this might trigger a security erase and take a while */
+	retval = stm32x_wait_status_busy(bank, FLASH_MASS_ERASE_TIMEOUT);
 	if (retval != ERROR_OK)
 		return retval;
 
@@ -1257,7 +1260,7 @@ static int stm32x_mass_erase(struct flash_bank *bank)
 	if (retval != ERROR_OK)
 		return retval;
 
-	retval = stm32x_wait_status_busy(bank, 30000);
+	retval = stm32x_wait_status_busy(bank, FLASH_MASS_ERASE_TIMEOUT);
 	if (retval != ERROR_OK)
 		return retval;
 
diff --git a/src/flash/nor/stm32l4x.c b/src/flash/nor/stm32l4x.c
index 129b281..fa0c48b 100644
--- a/src/flash/nor/stm32l4x.c
+++ b/src/flash/nor/stm32l4x.c
@@ -27,18 +27,22 @@
 
 /* STM32L4xxx series for reference.
  *
- * RM0351
- * http://www.st.com/st-web-ui/static/active/en/resource/technical/document/reference_manual/DM00083560.pdf
+ * RM0351 (STM32L4x5/STM32L4x6)
+ * http://www.st.com/resource/en/reference_manual/dm00083560.pdf
  *
- * STM32L476RG Datasheet (for erase timing)
- * http://www.st.com/st-web-ui/static/active/en/resource/technical/document/datasheet/DM00108832.pdf
+ * RM0394 (STM32L43x/44x/45x/46x)
+ * http://www.st.com/resource/en/reference_manual/dm00151940.pdf
  *
+ * STM32L476RG Datasheet (for erase timing)
+ * http://www.st.com/resource/en/datasheet/stm32l476rg.pdf
  *
- * The device has normally two banks, but on 512 and 256 kiB devices an
- * option byte is available to map all sectors to the first bank.
+ * The RM0351 devices have normally two banks, but on 512 and 256 kiB devices
+ * an option byte is available to map all sectors to the first bank.
  * Both STM32 banks are treated as one OpenOCD bank, as other STM32 devices
  * handlers do!
  *
+ * RM0394 devices have a single bank only.
+ *
  */
 
 /* Erase time can be as high as 25ms, 10x this and assume it's toast... */
@@ -614,9 +618,16 @@ static int stm32l4_probe(struct flash_bank *bank)
 
 	/* set max flash size depending on family */
 	switch (device_id & 0xfff) {
+	case 0x461:
 	case 0x415:
 		max_flash_size_in_kb = 1024;
 		break;
+	case 0x462:
+		max_flash_size_in_kb = 512;
+		break;
+	case 0x435:
+		max_flash_size_in_kb = 256;
+		break;
 	default:
 		LOG_WARNING("Cannot identify target as a STM32L4 family.");
 		return ERROR_FAIL;
@@ -698,7 +709,7 @@ static int get_stm32l4_info(struct flash_bank *bank, char *buf, int buf_size)
 	if (retval != ERROR_OK)
 		return retval;
 
-	uint16_t device_id = dbgmcu_idcode & 0xffff;
+	uint16_t device_id = dbgmcu_idcode & 0xfff;
 	uint8_t rev_id = dbgmcu_idcode >> 28;
 	uint8_t rev_minor = 0;
 	int i;
@@ -713,8 +724,20 @@ static int get_stm32l4_info(struct flash_bank *bank, char *buf, int buf_size)
 	const char *device_str;
 
 	switch (device_id) {
-	case 0x6415:
-		device_str = "STM32L4xx";
+	case 0x461:
+		device_str = "STM32L496/4A6";
+		break;
+
+	case 0x415:
+		device_str = "STM32L475/476/486";
+		break;
+
+	case 0x462:
+		device_str = "STM32L45x/46x";
+		break;
+
+	case 0x435:
+		device_str = "STM32L43x/44x";
 		break;
 
 	default:
diff --git a/src/flash/nor/stm32lx.c b/src/flash/nor/stm32lx.c
index 376c0f8..e4f499d 100644
--- a/src/flash/nor/stm32lx.c
+++ b/src/flash/nor/stm32lx.c
@@ -105,6 +105,7 @@ static int stm32lx_lock(struct flash_bank *bank);
 static int stm32lx_unlock(struct flash_bank *bank);
 static int stm32lx_mass_erase(struct flash_bank *bank);
 static int stm32lx_wait_until_bsy_clear_timeout(struct flash_bank *bank, int timeout);
+static int stm32lx_update_part_info(struct flash_bank *bank, uint16_t flash_size_in_kb);
 
 struct stm32lx_rev {
 	uint16_t rev;
@@ -132,7 +133,7 @@ struct stm32lx_flash_bank {
 	uint32_t user_bank_size;
 	uint32_t flash_base;
 
-	const struct stm32lx_part_info *part_info;
+	struct stm32lx_part_info part_info;
 };
 
 static const struct stm32lx_rev stm32_416_revs[] = {
@@ -245,7 +246,7 @@ static const struct stm32lx_part_info stm32lx_parts[] = {
 		.page_size			= 256,
 		.pages_per_sector	= 16,
 		.max_flash_size_kb	= 512,
-		.first_bank_size_kb	= 256,
+		.first_bank_size_kb	= 0,		/* determined in runtime */
 		.has_dual_banks		= true,
 		.flash_base			= 0x40023C00,
 		.fsize_base			= 0x1FF800CC,
@@ -258,8 +259,8 @@ static const struct stm32lx_part_info stm32lx_parts[] = {
 		.page_size			= 128,
 		.pages_per_sector	= 32,
 		.max_flash_size_kb	= 192,
-		.first_bank_size_kb	= 128,
-		.has_dual_banks		= true,
+		.first_bank_size_kb	= 0,		/* determined in runtime */
+		.has_dual_banks		= false,	/* determined in runtime */
 		.flash_base			= 0x40022000,
 		.fsize_base			= 0x1FF8007C,
 	},
@@ -300,7 +301,7 @@ FLASH_BANK_COMMAND_HANDLER(stm32lx_flash_bank_command)
 	stm32lx_info->user_bank_size = bank->size;
 
 	/* the stm32l erased value is 0x00 */
-	bank->default_padded_value = 0x00;
+	bank->default_padded_value = bank->erased_value = 0x00;
 
 	return ERROR_OK;
 }
@@ -436,7 +437,7 @@ static int stm32lx_write_half_pages(struct flash_bank *bank, const uint8_t *buff
 	struct target *target = bank->target;
 	struct stm32lx_flash_bank *stm32lx_info = bank->driver_priv;
 
-	uint32_t hp_nb = stm32lx_info->part_info->page_size / 2;
+	uint32_t hp_nb = stm32lx_info->part_info.page_size / 2;
 	uint32_t buffer_size = 16384;
 	struct working_area *write_algorithm;
 	struct working_area *source;
@@ -450,19 +451,7 @@ static int stm32lx_write_half_pages(struct flash_bank *bank, const uint8_t *buff
 	/* see contib/loaders/flash/stm32lx.S for src */
 
 	static const uint8_t stm32lx_flash_write_code[] = {
-		/* write_word: */
-		0x00, 0x23,             /* movs r3, #0 */
-		0x04, 0xe0,             /* b test_done */
-
-		/* write_word: */
-		0x51, 0xf8, 0x04, 0xcb, /* ldr ip, [r1], #4 */
-		0x40, 0xf8, 0x04, 0xcb, /* str ip, [r0], #4 */
-		0x01, 0x33,             /* adds r3, #1 */
-
-		/* test_done: */
-		0x93, 0x42,             /* cmp r3, r2 */
-		0xf8, 0xd3,             /* bcc write_word */
-		0x00, 0xbe,             /* bkpt 0 */
+			0x92, 0x00, 0x8A, 0x18, 0x01, 0xE0, 0x08, 0xC9, 0x08, 0xC0, 0x91, 0x42, 0xFB, 0xD1, 0x00, 0xBE
 	};
 
 	/* Make sure we're performing a half-page aligned write. */
@@ -495,7 +484,7 @@ static int stm32lx_write_half_pages(struct flash_bank *bank, const uint8_t *buff
 		else
 			buffer_size /= 2;
 
-		if (buffer_size <= stm32lx_info->part_info->page_size) {
+		if (buffer_size <= stm32lx_info->part_info.page_size) {
 			/* we already allocated the writing code, but failed to get a
 			 * buffer, free the algorithm */
 			target_free_working_area(target, write_algorithm);
@@ -588,7 +577,7 @@ static int stm32lx_write_half_pages(struct flash_bank *bank, const uint8_t *buff
 		 * is reduced by 50% using this slower method.
 		 */
 
-		LOG_WARNING("couldn't use loader, falling back to page memory writes");
+		LOG_WARNING("Couldn't use loader, falling back to page memory writes");
 
 		while (count > 0) {
 			uint32_t this_count;
@@ -629,7 +618,7 @@ static int stm32lx_write(struct flash_bank *bank, const uint8_t *buffer,
 	struct target *target = bank->target;
 	struct stm32lx_flash_bank *stm32lx_info = bank->driver_priv;
 
-	uint32_t hp_nb = stm32lx_info->part_info->page_size / 2;
+	uint32_t hp_nb = stm32lx_info->part_info.page_size / 2;
 	uint32_t halfpages_number;
 	uint32_t bytes_remaining = 0;
 	uint32_t address = bank->base + offset;
@@ -759,9 +748,9 @@ static int stm32lx_probe(struct flash_bank *bank)
 	uint32_t device_id;
 	uint32_t base_address = FLASH_BANK0_ADDRESS;
 	uint32_t second_bank_base;
+	unsigned int n;
 
 	stm32lx_info->probed = 0;
-	stm32lx_info->part_info = NULL;
 
 	int retval = stm32lx_read_id_code(bank->target, &device_id);
 	if (retval != ERROR_OK)
@@ -771,22 +760,24 @@ static int stm32lx_probe(struct flash_bank *bank)
 
 	LOG_DEBUG("device id = 0x%08" PRIx32 "", device_id);
 
-	for (unsigned int n = 0; n < ARRAY_SIZE(stm32lx_parts); n++) {
-		if ((device_id & 0xfff) == stm32lx_parts[n].id)
-			stm32lx_info->part_info = &stm32lx_parts[n];
+	for (n = 0; n < ARRAY_SIZE(stm32lx_parts); n++) {
+		if ((device_id & 0xfff) == stm32lx_parts[n].id) {
+			stm32lx_info->part_info = stm32lx_parts[n];
+			break;
+		}
 	}
 
-	if (!stm32lx_info->part_info) {
+	if (n == ARRAY_SIZE(stm32lx_parts)) {
 		LOG_WARNING("Cannot identify target as a STM32L family.");
 		return ERROR_FAIL;
 	} else {
-		LOG_INFO("Device: %s", stm32lx_info->part_info->device_str);
+		LOG_INFO("Device: %s", stm32lx_info->part_info.device_str);
 	}
 
-	stm32lx_info->flash_base = stm32lx_info->part_info->flash_base;
+	stm32lx_info->flash_base = stm32lx_info->part_info.flash_base;
 
 	/* Get the flash size from target. */
-	retval = target_read_u16(target, stm32lx_info->part_info->fsize_base,
+	retval = target_read_u16(target, stm32lx_info->part_info.fsize_base,
 			&flash_size_in_kb);
 
 	/* 0x436 devices report their flash size as a 0 or 1 code indicating 384K
@@ -803,29 +794,34 @@ static int stm32lx_probe(struct flash_bank *bank)
 	 * default to max target family */
 	if (retval != ERROR_OK || flash_size_in_kb == 0xffff || flash_size_in_kb == 0) {
 		LOG_WARNING("STM32L flash size failed, probe inaccurate - assuming %dk flash",
-			stm32lx_info->part_info->max_flash_size_kb);
-		flash_size_in_kb = stm32lx_info->part_info->max_flash_size_kb;
-	} else if (flash_size_in_kb > stm32lx_info->part_info->max_flash_size_kb) {
+			stm32lx_info->part_info.max_flash_size_kb);
+		flash_size_in_kb = stm32lx_info->part_info.max_flash_size_kb;
+	} else if (flash_size_in_kb > stm32lx_info->part_info.max_flash_size_kb) {
 		LOG_WARNING("STM32L probed flash size assumed incorrect since FLASH_SIZE=%dk > %dk, - assuming %dk flash",
-			flash_size_in_kb, stm32lx_info->part_info->max_flash_size_kb,
-			stm32lx_info->part_info->max_flash_size_kb);
-		flash_size_in_kb = stm32lx_info->part_info->max_flash_size_kb;
+			flash_size_in_kb, stm32lx_info->part_info.max_flash_size_kb,
+			stm32lx_info->part_info.max_flash_size_kb);
+		flash_size_in_kb = stm32lx_info->part_info.max_flash_size_kb;
 	}
 
-	if (stm32lx_info->part_info->has_dual_banks) {
+	/* Overwrite default dual-bank configuration */
+	retval = stm32lx_update_part_info(bank, flash_size_in_kb);
+	if (retval != ERROR_OK)
+		return ERROR_FAIL;
+
+	if (stm32lx_info->part_info.has_dual_banks) {
 		/* 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 +
-			stm32lx_info->part_info->first_bank_size_kb * 1024;
+			stm32lx_info->part_info.first_bank_size_kb * 1024;
 		if (bank->base == second_bank_base || !bank->base) {
 			/* This is the second bank  */
 			base_address = second_bank_base;
 			flash_size_in_kb = flash_size_in_kb -
-				stm32lx_info->part_info->first_bank_size_kb;
+				stm32lx_info->part_info.first_bank_size_kb;
 		} else if (bank->base == base_address) {
 			/* This is the first bank */
-			flash_size_in_kb = stm32lx_info->part_info->first_bank_size_kb;
+			flash_size_in_kb = stm32lx_info->part_info.first_bank_size_kb;
 		} else {
 			LOG_WARNING("STM32L flash bank base address config is incorrect."
 				    " 0x%" PRIx32 " but should rather be 0x%" PRIx32 " or 0x%" PRIx32,
@@ -884,60 +880,13 @@ static int stm32lx_auto_probe(struct flash_bank *bank)
 	return stm32lx_probe(bank);
 }
 
-static int stm32lx_erase_check(struct flash_bank *bank)
-{
-	struct target *target = bank->target;
-	const int buffer_size = 4096;
-	int i;
-	uint32_t nBytes;
-	int retval = ERROR_OK;
-
-	if (bank->target->state != TARGET_HALTED) {
-		LOG_ERROR("Target not halted");
-		return ERROR_TARGET_NOT_HALTED;
-	}
-
-	uint8_t *buffer = malloc(buffer_size);
-	if (buffer == NULL) {
-		LOG_ERROR("failed to allocate read buffer");
-		return ERROR_FAIL;
-	}
-
-	for (i = 0; i < bank->num_sectors; i++) {
-		uint32_t j;
-		bank->sectors[i].is_erased = 1;
-
-		/* Loop chunk by chunk over the sector */
-		for (j = 0; j < bank->sectors[i].size; j += buffer_size) {
-			uint32_t chunk;
-			chunk = buffer_size;
-			if (chunk > (j - bank->sectors[i].size))
-				chunk = (j - bank->sectors[i].size);
-
-			retval = target_read_memory(target, bank->base
-					+ bank->sectors[i].offset + j, 4, chunk / 4, buffer);
-			if (retval != ERROR_OK)
-				break;
-
-			for (nBytes = 0; nBytes < chunk; nBytes++) {
-				if (buffer[nBytes] != 0x00) {
-					bank->sectors[i].is_erased = 0;
-					break;
-				}
-			}
-		}
-		if (retval != ERROR_OK)
-			break;
-	}
-	free(buffer);
-
-	return retval;
-}
-
 /* This method must return a string displaying information about the bank */
 static int stm32lx_get_info(struct flash_bank *bank, char *buf, int buf_size)
 {
 	struct stm32lx_flash_bank *stm32lx_info = bank->driver_priv;
+	const struct stm32lx_part_info *info = &stm32lx_info->part_info;
+	uint16_t rev_id = stm32lx_info->idcode >> 16;
+	const char *rev_str = NULL;
 
 	if (!stm32lx_info->probed) {
 		int retval = stm32lx_probe(bank);
@@ -948,32 +897,21 @@ static int stm32lx_get_info(struct flash_bank *bank, char *buf, int buf_size)
 		}
 	}
 
-	const struct stm32lx_part_info *info = stm32lx_info->part_info;
-
-	if (info) {
-		const char *rev_str = NULL;
-		uint16_t rev_id = stm32lx_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;
 
-		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",
-				stm32lx_info->part_info->device_str, rev_str);
-		} else {
-			snprintf(buf, buf_size,
-				"%s - Rev: unknown (0x%04x)",
-				stm32lx_info->part_info->device_str, rev_id);
-		}
-
-		return ERROR_OK;
+	if (rev_str != NULL) {
+		snprintf(buf, buf_size,
+			"%s - Rev: %s",
+			info->device_str, rev_str);
 	} else {
-		snprintf(buf, buf_size, "Cannot identify target as a STM32Lx");
-
-		return ERROR_FAIL;
+		snprintf(buf, buf_size,
+			"%s - Rev: unknown (0x%04x)",
+			info->device_str, rev_id);
 	}
+
+	return ERROR_OK;
 }
 
 static const struct command_registration stm32lx_exec_command_handlers[] = {
@@ -1022,7 +960,7 @@ struct flash_driver stm32lx_flash = {
 		.read = default_flash_read,
 		.probe = stm32lx_probe,
 		.auto_probe = stm32lx_auto_probe,
-		.erase_check = stm32lx_erase_check,
+		.erase_check = default_flash_blank_check,
 		.protect_check = stm32lx_protect_check,
 		.info = stm32lx_get_info,
 };
@@ -1182,7 +1120,7 @@ static int stm32lx_erase_sector(struct flash_bank *bank, int sector)
 	if (retval != ERROR_OK)
 		return retval;
 
-	for (int page = 0; page < (int)stm32lx_info->part_info->pages_per_sector;
+	for (int page = 0; page < (int)stm32lx_info->part_info.pages_per_sector;
 			page++) {
 		reg32 = FLASH_PECR__PROG | FLASH_PECR__ERASE;
 		retval = target_write_u32(target,
@@ -1195,7 +1133,7 @@ static int stm32lx_erase_sector(struct flash_bank *bank, int sector)
 			return retval;
 
 		uint32_t addr = bank->base + bank->sectors[sector].offset + (page
-				* stm32lx_info->part_info->page_size);
+				* stm32lx_info->part_info.page_size);
 		retval = target_write_u32(target, addr, 0x0);
 		if (retval != ERROR_OK)
 			return retval;
@@ -1419,3 +1357,22 @@ static int stm32lx_mass_erase(struct flash_bank *bank)
 
 	return ERROR_OK;
 }
+
+static int stm32lx_update_part_info(struct flash_bank *bank, uint16_t flash_size_in_kb)
+{
+	struct stm32lx_flash_bank *stm32lx_info = bank->driver_priv;
+
+	switch (stm32lx_info->part_info.id) {
+	case 0x447: /* STM32L0xx (Cat.5) devices */
+		if (flash_size_in_kb == 192 || flash_size_in_kb == 128) {
+			stm32lx_info->part_info.first_bank_size_kb = flash_size_in_kb / 2;
+			stm32lx_info->part_info.has_dual_banks = true;
+		}
+		break;
+	case 0x437: /* STM32L1xx (Cat.5/Cat.6) */
+		stm32lx_info->part_info.first_bank_size_kb = flash_size_in_kb / 2;
+		break;
+	}
+
+	return ERROR_OK;
+}
diff --git a/src/flash/nor/tcl.c b/src/flash/nor/tcl.c
index 6dd2140..b93d126 100644
--- a/src/flash/nor/tcl.c
+++ b/src/flash/nor/tcl.c
@@ -297,8 +297,8 @@ static int flash_check_sector_parameters(struct command_context *cmd_ctx,
 	}
 
 	if (!(last <= (num_sectors - 1))) {
-		command_print(cmd_ctx, "ERROR: last sector must be <= %d",
-			(int) num_sectors - 1);
+		command_print(cmd_ctx, "ERROR: last sector must be <= %" PRIu32,
+			num_sectors - 1);
 		return ERROR_FAIL;
 	}
 
@@ -341,7 +341,7 @@ COMMAND_HANDLER(handle_flash_erase_command)
 			"in %fs", first, last, p->bank_number, duration_elapsed(&bench));
 	}
 
-	return ERROR_OK;
+	return retval;
 }
 
 COMMAND_HANDLER(handle_flash_protect_command)
@@ -380,10 +380,9 @@ COMMAND_HANDLER(handle_flash_protect_command)
 
 	retval = flash_driver_protect(p, set, first, last);
 	if (retval == ERROR_OK) {
-		command_print(CMD_CTX, "%s protection for sectors %i "
-			"through %i on flash bank %d",
-			(set) ? "set" : "cleared", (int) first,
-			(int) last, p->bank_number);
+		command_print(CMD_CTX, "%s protection for sectors %" PRIu32
+			" through %" PRIu32 " on flash bank %d",
+			(set) ? "set" : "cleared", first, last, p->bank_number);
 	}
 
 	return retval;
@@ -600,7 +599,7 @@ COMMAND_HANDLER(handle_flash_write_bank_command)
 	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[2], offset);
 
 	if (fileio_open(&fileio, CMD_ARGV[1], FILEIO_READ, FILEIO_BINARY) != ERROR_OK)
-		return ERROR_OK;
+		return ERROR_FAIL;
 
 	size_t filesize;
 	retval = fileio_size(fileio, &filesize);
@@ -619,7 +618,7 @@ COMMAND_HANDLER(handle_flash_write_bank_command)
 	if (fileio_read(fileio, filesize, buffer, &buf_cnt) != ERROR_OK) {
 		free(buffer);
 		fileio_close(fileio);
-		return ERROR_OK;
+		return ERROR_FAIL;
 	}
 
 	retval = flash_driver_write(p, buffer, offset, buf_cnt);
@@ -690,9 +689,9 @@ COMMAND_HANDLER(handle_flash_read_bank_command)
 	}
 
 	if (duration_measure(&bench) == ERROR_OK)
-		command_print(CMD_CTX, "wrote %ld bytes to file %s from flash bank %u"
+		command_print(CMD_CTX, "wrote %zd bytes to file %s from flash bank %u"
 			" at offset 0x%8.8" PRIx32 " in %fs (%0.3f KiB/s)",
-			(long)written, CMD_ARGV[1], p->bank_number, offset,
+			written, CMD_ARGV[1], p->bank_number, offset,
 			duration_elapsed(&bench), duration_kbps(&bench, written));
 
 	return retval;
@@ -708,7 +707,7 @@ COMMAND_HANDLER(handle_flash_verify_bank_command)
 	size_t filesize;
 	int differ;
 
-	if (CMD_ARGC != 3)
+	if (CMD_ARGC < 2 || CMD_ARGC > 3)
 		return ERROR_COMMAND_SYNTAX_ERROR;
 
 	struct duration bench;
@@ -719,7 +718,16 @@ COMMAND_HANDLER(handle_flash_verify_bank_command)
 	if (ERROR_OK != retval)
 		return retval;
 
-	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[2], offset);
+	offset = 0;
+
+	if (CMD_ARGC > 2)
+		COMMAND_PARSE_NUMBER(u32, CMD_ARGV[2], offset);
+
+	if (offset > p->size) {
+		LOG_ERROR("Offset 0x%8.8" PRIx32 " is out of range of the flash bank",
+			offset);
+		return ERROR_COMMAND_ARGUMENT_INVALID;
+	}
 
 	retval = fileio_open(&fileio, CMD_ARGV[1], FILEIO_READ, FILEIO_BINARY);
 	if (retval != ERROR_OK) {
@@ -770,9 +778,9 @@ COMMAND_HANDLER(handle_flash_verify_bank_command)
 	}
 
 	if (duration_measure(&bench) == ERROR_OK)
-		command_print(CMD_CTX, "read %ld bytes from file %s and flash bank %u"
+		command_print(CMD_CTX, "read %zd bytes from file %s and flash bank %u"
 			" at offset 0x%8.8" PRIx32 " in %fs (%0.3f KiB/s)",
-			(long)read_cnt, CMD_ARGV[1], p->bank_number, offset,
+			read_cnt, CMD_ARGV[1], p->bank_number, offset,
 			duration_elapsed(&bench), duration_kbps(&bench, read_cnt));
 
 	differ = memcmp(buffer_file, buffer_flash, read_cnt);
@@ -927,19 +935,20 @@ static const struct command_registration flash_exec_command_handlers[] = {
 		.name = "verify_bank",
 		.handler = handle_flash_verify_bank_command,
 		.mode = COMMAND_EXEC,
-		.usage = "bank_id filename offset",
-		.help = "Read binary data from flash bank and file, "
-			"starting at specified byte offset from the "
-			"beginning of the bank. Compare the contents.",
+		.usage = "bank_id filename [offset]",
+		.help = "Compare the contents of a file with the contents of the "
+			"flash bank. Allow optional offset from beginning of the bank "
+			"(defaults to zero).",
 	},
 	{
 		.name = "protect",
 		.handler = handle_flash_protect_command,
 		.mode = COMMAND_EXEC,
-		.usage = "bank_id first_sector [last_sector|'last'] "
+		.usage = "bank_id first_block [last_block|'last'] "
 			"('on'|'off')",
-		.help = "Turn protection on or off for a range of sectors "
-			"in a given flash bank.",
+		.help = "Turn protection on or off for a range of protection "
+			"blocks or sectors in a given flash bank. "
+			"See 'flash info' output for a list of blocks.",
 	},
 	{
 		.name = "padded_value",
@@ -1012,7 +1021,7 @@ COMMAND_HANDLER(handle_flash_bank_command)
 	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[2], c->size);
 	COMMAND_PARSE_NUMBER(int, CMD_ARGV[3], c->chip_width);
 	COMMAND_PARSE_NUMBER(int, CMD_ARGV[4], c->bus_width);
-	c->default_padded_value = 0xff;
+	c->default_padded_value = c->erased_value = 0xff;
 	c->num_sectors = 0;
 	c->sectors = NULL;
 	c->num_prot_blocks = 0;
diff --git a/src/flash/nor/virtual.c b/src/flash/nor/virtual.c
index 3cb793e..06981f4 100644
--- a/src/flash/nor/virtual.c
+++ b/src/flash/nor/virtual.c
@@ -44,6 +44,7 @@ static void virtual_update_bank_info(struct flash_bank *bank)
 	bank->size = master_bank->size;
 	bank->chip_width = master_bank->chip_width;
 	bank->bus_width = master_bank->bus_width;
+	bank->erased_value = master_bank->erased_value;
 	bank->default_padded_value = master_bank->default_padded_value;
 	bank->num_sectors = master_bank->num_sectors;
 	bank->sectors = master_bank->sectors;
diff --git a/src/flash/nor/xmc1xxx.c b/src/flash/nor/xmc1xxx.c
index bb2ec12..0a76b21 100644
--- a/src/flash/nor/xmc1xxx.c
+++ b/src/flash/nor/xmc1xxx.c
@@ -305,7 +305,7 @@ static int xmc1xxx_write(struct flash_bank *bank, const uint8_t *buffer,
 		uint32_t blocks = MIN(block_count, data_workarea->size / NVM_BLOCK_SIZE);
 		uint32_t addr = bank->base + offset;
 
-		LOG_DEBUG("copying %" PRId32 " bytes to SRAM 0x%08" PRIx32,
+		LOG_DEBUG("copying %" PRId32 " bytes to SRAM 0x%08" TARGET_PRIxADDR,
 			MIN(blocks * NVM_BLOCK_SIZE, byte_count),
 			data_workarea->address);
 
diff --git a/src/flash/nor/xmc4xxx.c b/src/flash/nor/xmc4xxx.c
index 39aec79..02df46a 100644
--- a/src/flash/nor/xmc4xxx.c
+++ b/src/flash/nor/xmc4xxx.c
@@ -183,7 +183,7 @@
 
 /* Flash controller configuration values */
 #define FLASH_ID_XMC4500        0xA2
-#define FLASH_ID_XMC4700_4800   0x92
+#define FLASH_ID_XMC4300_XMC4700_4800   0x92
 #define FLASH_ID_XMC4100_4200   0x9C
 #define FLASH_ID_XMC4400        0x9F
 
@@ -319,8 +319,8 @@ static int xmc4xxx_load_bank_layout(struct flash_bank *bank)
 	}
 
 	/* This part doesn't follow the typical standard of 0xff
-	 * being the default padding value.*/
-	bank->default_padded_value = 0x00;
+	 * being the erased value.*/
+	bank->default_padded_value = bank->erased_value = 0x00;
 
 	return ERROR_OK;
 }
@@ -383,7 +383,7 @@ static int xmc4xxx_probe(struct flash_bank *bank)
 		bank->num_sectors = 12;
 		LOG_DEBUG("XMC4xxx: XMC4500 detected.");
 		break;
-	case FLASH_ID_XMC4700_4800:
+	case FLASH_ID_XMC4300_XMC4700_4800:
 		bank->num_sectors = 16;
 		LOG_DEBUG("XMC4xxx: XMC4700/4800 detected.");
 		break;
@@ -617,99 +617,6 @@ static int xmc4xxx_enter_page_mode(struct flash_bank *bank)
 	return res;
 }
 
-/* The logical erase value of an xmc4xxx memory cell is 0x00,
- * therefore, we cannot use the built in flash blank check and must
- * implement our own */
-
-/** Checks whether a memory region is zeroed. */
-static int xmc4xxx_blank_check_memory(struct target *target,
-	uint32_t address, uint32_t count, uint32_t *blank)
-{
-	struct working_area *erase_check_algorithm;
-	struct reg_param reg_params[3];
-	struct armv7m_algorithm armv7m_info;
-	int retval;
-
-	static const uint8_t erase_check_code[] = {
-#include "../../../contrib/loaders/erase_check/armv7m_0_erase_check.inc"
-	};
-
-	/* make sure we have a working area */
-	if (target_alloc_working_area(target, sizeof(erase_check_code),
-		&erase_check_algorithm) != ERROR_OK)
-		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
-
-	retval = target_write_buffer(target, erase_check_algorithm->address,
-			sizeof(erase_check_code), (uint8_t *)erase_check_code);
-	if (retval != ERROR_OK)
-		goto cleanup;
-
-	armv7m_info.common_magic = ARMV7M_COMMON_MAGIC;
-	armv7m_info.core_mode = ARM_MODE_THREAD;
-
-	init_reg_param(&reg_params[0], "r0", 32, PARAM_OUT);
-	buf_set_u32(reg_params[0].value, 0, 32, address);
-
-	init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);
-	buf_set_u32(reg_params[1].value, 0, 32, count);
-
-	init_reg_param(&reg_params[2], "r2", 32, PARAM_IN_OUT);
-	buf_set_u32(reg_params[2].value, 0, 32, 0x00);
-
-	retval = target_run_algorithm(target,
-				      0,
-				      NULL,
-				      3,
-				      reg_params,
-				      erase_check_algorithm->address,
-				      erase_check_algorithm->address + (sizeof(erase_check_code) - 2),
-				      10000,
-				      &armv7m_info);
-
-	if (retval == ERROR_OK)
-		*blank = buf_get_u32(reg_params[2].value, 0, 32);
-
-	destroy_reg_param(&reg_params[0]);
-	destroy_reg_param(&reg_params[1]);
-	destroy_reg_param(&reg_params[2]);
-
-cleanup:
-	target_free_working_area(target, erase_check_algorithm);
-
-	return retval;
-}
-
-static int xmc4xxx_flash_blank_check(struct flash_bank *bank)
-{
-	struct target *target = bank->target;
-	int i;
-	int retval = ERROR_OK;
-	uint32_t blank;
-
-	if (bank->target->state != TARGET_HALTED) {
-		LOG_ERROR("Target not halted");
-		return ERROR_TARGET_NOT_HALTED;
-	}
-
-	for (i = 0; i < bank->num_sectors; i++) {
-		uint32_t address = bank->base + bank->sectors[i].offset;
-		uint32_t size = bank->sectors[i].size;
-
-		LOG_DEBUG("Erase checking 0x%08"PRIx32, address);
-		retval = xmc4xxx_blank_check_memory(target, address, size, &blank);
-
-		if (retval != ERROR_OK)
-			break;
-
-		if (blank == 0x00)
-			bank->sectors[i].is_erased = 1;
-		else
-			bank->sectors[i].is_erased = 0;
-	}
-
-	return retval;
-}
-
 static int xmc4xxx_write_page(struct flash_bank *bank, const uint8_t *pg_buf,
 			      uint32_t offset, bool user_config)
 {
@@ -944,6 +851,14 @@ static int xmc4xxx_get_info_command(struct flash_bank *bank, char *buf, int buf_
 			break;
 		}
 		break;
+	case 0x300:
+		dev_str = "XMC4300";
+
+		switch (rev_id) {
+		case 0x1:
+			rev_str = "AA";
+		}
+		break;
 	case 0x400:
 		dev_str = "XMC4400";
 
@@ -1437,7 +1352,7 @@ struct flash_driver xmc4xxx_flash = {
 	.read = default_flash_read,
 	.probe = xmc4xxx_probe,
 	.auto_probe = xmc4xxx_probe,
-	.erase_check = xmc4xxx_flash_blank_check,
+	.erase_check = default_flash_blank_check,
 	.info = xmc4xxx_get_info_command,
 	.protect_check = xmc4xxx_protect_check,
 	.protect = xmc4xxx_protect,
diff --git a/src/helper/Makefile.am b/src/helper/Makefile.am
index 64caf98..e0f7f49 100644
--- a/src/helper/Makefile.am
+++ b/src/helper/Makefile.am
@@ -1,56 +1,49 @@
-include $(top_srcdir)/common.mk
-
-METASOURCES = AUTO
-noinst_LTLIBRARIES = libhelper.la
-
-CONFIGFILES = options.c time_support_common.c
-
-libhelper_la_CPPFLAGS = $(AM_CPPFLAGS) $(LIBUSB1_CFLAGS)
-
-libhelper_la_SOURCES = \
-	binarybuffer.c \
-	$(CONFIGFILES) \
-	configuration.c \
-	log.c \
-	command.c \
-	time_support.c \
-	replacements.c \
-	fileio.c \
-	util.c \
-	jep106.c \
-	jim-nvp.c
+noinst_LTLIBRARIES += %D%/libhelper.la
+
+%C%_libhelper_la_CPPFLAGS = $(AM_CPPFLAGS) $(LIBUSB1_CFLAGS)
+
+%C%_libhelper_la_SOURCES = \
+	%D%/binarybuffer.c \
+	%D%/options.c \
+	%D%/time_support_common.c \
+	%D%/configuration.c \
+	%D%/log.c \
+	%D%/command.c \
+	%D%/time_support.c \
+	%D%/replacements.c \
+	%D%/fileio.c \
+	%D%/util.c \
+	%D%/jep106.c \
+	%D%/jim-nvp.c \
+	%D%/binarybuffer.h \
+	%D%/configuration.h \
+	%D%/ioutil.h \
+	%D%/list.h \
+	%D%/util.h \
+	%D%/types.h \
+	%D%/log.h \
+	%D%/command.h \
+	%D%/time_support.h \
+	%D%/replacements.h \
+	%D%/fileio.h \
+	%D%/system.h \
+	%D%/jep106.h \
+	%D%/jep106.inc \
+	%D%/jim-nvp.h
 
 if IOUTIL
-libhelper_la_SOURCES += ioutil.c
+%C%_libhelper_la_SOURCES += %D%/ioutil.c
 else
-libhelper_la_SOURCES += ioutil_stubs.c
+%C%_libhelper_la_SOURCES += %D%/ioutil_stubs.c
 endif
 
-libhelper_la_CFLAGS =
+%C%_libhelper_la_CFLAGS = $(AM_CFLAGS)
 if IS_MINGW
 # FD_* macros are sloppy with their signs on MinGW32 platform
-libhelper_la_CFLAGS += -Wno-sign-compare
+%C%_libhelper_la_CFLAGS += -Wno-sign-compare
 endif
 
-noinst_HEADERS = \
-	binarybuffer.h \
-	configuration.h \
-	ioutil.h \
-	list.h \
-	util.h \
-	types.h \
-	log.h \
-	command.h \
-	time_support.h \
-	replacements.h \
-	fileio.h \
-	system.h \
-	bin2char.sh \
-	jep106.h \
-	jep106.inc \
-	update_jep106.pl \
-	jim-nvp.h
-
-EXTRA_DIST = startup.tcl
-
-MAINTAINERCLEANFILES = $(srcdir)/Makefile.in
+STARTUP_TCL_SRCS += %D%/startup.tcl
+EXTRA_DIST += \
+	%D%/bin2char.sh \
+	%D%/update_jep106.pl
diff --git a/src/helper/binarybuffer.c b/src/helper/binarybuffer.c
index c1e6322..76f657f 100644
--- a/src/helper/binarybuffer.c
+++ b/src/helper/binarybuffer.c
@@ -45,6 +45,11 @@ static const unsigned char bit_reverse_table256[] = {
 	0x0F, 0x8F, 0x4F, 0xCF, 0x2F, 0xAF, 0x6F, 0xEF, 0x1F, 0x9F, 0x5F, 0xDF, 0x3F, 0xBF, 0x7F, 0xFF
 };
 
+static const char hex_digits[] = {
+	'0', '1', '2', '3', '4', '5', '6', '7', '8', '9',
+	'a', 'b', 'c', 'd', 'e', 'f'
+};
+
 void *buf_cpy(const void *from, void *_to, unsigned size)
 {
 	if (NULL == from || NULL == _to)
@@ -369,31 +374,72 @@ void bit_copy_discard(struct bit_copy_queue *q)
 	}
 }
 
-int unhexify(char *bin, const char *hex, int count)
+/**
+ * Convert a string of hexadecimal pairs into its binary
+ * representation.
+ *
+ * @param[out] bin Buffer to store binary representation. The buffer size must
+ *                 be at least @p count.
+ * @param[in] hex String with hexadecimal pairs to convert into its binary
+ *                representation.
+ * @param[in] count Number of hexadecimal pairs to convert.
+ *
+ * @return The number of converted hexadecimal pairs.
+ */
+size_t unhexify(uint8_t *bin, const char *hex, size_t count)
 {
-	int i, tmp;
+	size_t i;
+	char tmp;
+
+	if (!bin || !hex)
+		return 0;
+
+	memset(bin, 0, count);
+
+	for (i = 0; i < 2 * count; i++) {
+		if (hex[i] >= 'a' && hex[i] <= 'f')
+			tmp = hex[i] - 'a' + 10;
+		else if (hex[i] >= 'A' && hex[i] <= 'F')
+			tmp = hex[i] - 'A' + 10;
+		else if (hex[i] >= '0' && hex[i] <= '9')
+			tmp = hex[i] - '0';
+		else
+			return i / 2;
 
-	for (i = 0; i < count; i++) {
-		if (sscanf(hex + (2 * i), "%02x", &tmp) != 1)
-			return i;
-		bin[i] = tmp;
+		bin[i / 2] |= tmp << (4 * ((i + 1) % 2));
 	}
 
-	return i;
+	return i / 2;
 }
 
-int hexify(char *hex, const char *bin, int count, int out_maxlen)
+/**
+ * Convert binary data into a string of hexadecimal pairs.
+ *
+ * @param[out] hex Buffer to store string of hexadecimal pairs. The buffer size
+ *                 must be at least @p length.
+ * @param[in] bin Buffer with binary data to convert into hexadecimal pairs.
+ * @param[in] count Number of bytes to convert.
+ * @param[in] length Maximum number of characters, including null-terminator,
+ *                   to store into @p hex.
+ *
+ * @returns The length of the converted string excluding null-terminator.
+ */
+size_t hexify(char *hex, const uint8_t *bin, size_t count, size_t length)
 {
-	int i, cmd_len = 0;
+	size_t i;
+	uint8_t tmp;
+
+	if (!length)
+		return 0;
 
-	/* May use a length, or a null-terminated string as input. */
-	if (count == 0)
-		count = strlen(bin);
+	for (i = 0; i < length - 1 && i < 2 * count; i++) {
+		tmp = (bin[i / 2] >> (4 * ((i + 1) % 2))) & 0x0f;
+		hex[i] = hex_digits[tmp];
+	}
 
-	for (i = 0; i < count; i++)
-		cmd_len += snprintf(hex + cmd_len, out_maxlen - cmd_len, "%02x", bin[i] & 0xff);
+	hex[i] = 0;
 
-	return cmd_len;
+	return i;
 }
 
 void buffer_shr(void *_buf, unsigned buf_len, unsigned count)
diff --git a/src/helper/binarybuffer.h b/src/helper/binarybuffer.h
index dd0d275..f1da8c4 100644
--- a/src/helper/binarybuffer.h
+++ b/src/helper/binarybuffer.h
@@ -234,8 +234,8 @@ void bit_copy_discard(struct bit_copy_queue *q);
 
 /* functions to convert to/from hex encoded buffer
  * used in ti-icdi driver and gdb server */
-int unhexify(char *bin, const char *hex, int count);
-int hexify(char *hex, const char *bin, int count, int out_maxlen);
+size_t unhexify(uint8_t *bin, const char *hex, size_t count);
+size_t hexify(char *hex, const uint8_t *bin, size_t count, size_t out_maxlen);
 void buffer_shr(void *_buf, unsigned buf_len, unsigned count);
 
 #endif /* OPENOCD_HELPER_BINARYBUFFER_H */
diff --git a/src/helper/command.c b/src/helper/command.c
index fc4aac7..5deaee8 100644
--- a/src/helper/command.c
+++ b/src/helper/command.c
@@ -1410,6 +1410,8 @@ DEFINE_PARSE_ULONGLONG(_u32,  uint32_t, 0, UINT32_MAX)
 DEFINE_PARSE_ULONGLONG(_u16,  uint16_t, 0, UINT16_MAX)
 DEFINE_PARSE_ULONGLONG(_u8,   uint8_t,  0, UINT8_MAX)
 
+DEFINE_PARSE_ULONGLONG(_target_addr, target_addr_t, 0, TARGET_ADDR_MAX)
+
 #define DEFINE_PARSE_LONGLONG(name, type, min, max) \
 	DEFINE_PARSE_WRAPPER(name, type, min, max, long long, _llong)
 DEFINE_PARSE_LONGLONG(_int, int,     n < INT_MIN,   INT_MAX)
diff --git a/src/helper/command.h b/src/helper/command.h
index 5c39660..bd24156 100644
--- a/src/helper/command.h
+++ b/src/helper/command.h
@@ -357,10 +357,13 @@ DECLARE_PARSE_WRAPPER(_u16, uint16_t);
 DECLARE_PARSE_WRAPPER(_u8, uint8_t);
 
 DECLARE_PARSE_WRAPPER(_int, int);
+DECLARE_PARSE_WRAPPER(_s64, int64_t);
 DECLARE_PARSE_WRAPPER(_s32, int32_t);
 DECLARE_PARSE_WRAPPER(_s16, int16_t);
 DECLARE_PARSE_WRAPPER(_s8, int8_t);
 
+DECLARE_PARSE_WRAPPER(_target_addr, target_addr_t);
+
 /**
  * @brief parses the string @a in into @a out as a @a type, or prints
  * a command error and passes the error code to the caller.  If an error
@@ -382,6 +385,9 @@ DECLARE_PARSE_WRAPPER(_s8, int8_t);
 		} \
 	} while (0)
 
+#define COMMAND_PARSE_ADDRESS(in, out) \
+	COMMAND_PARSE_NUMBER(target_addr, in, out)
+
 /**
  * Parse the string @c as a binary parameter, storing the boolean value
  * in @c out.  The strings @c on and @c off are used to match different
diff --git a/src/helper/jep106.inc b/src/helper/jep106.inc
index 1895005..c0295a6 100644
--- a/src/helper/jep106.inc
+++ b/src/helper/jep106.inc
@@ -884,7 +884,7 @@
 [7][0x01 - 1] = "Siklu Communication Ltd.",
 [7][0x02 - 1] = "A Force Manufacturing Ltd.",
 [7][0x03 - 1] = "Strontium",
-[7][0x04 - 1] = "Abilis Systems",
+[7][0x04 - 1] = "ALi Corp (Abilis Systems)",
 [7][0x05 - 1] = "Siglead, Inc.",
 [7][0x06 - 1] = "Ubicom, Inc.",
 [7][0x07 - 1] = "Unifosa Corporation",
@@ -893,7 +893,7 @@
 [7][0x0a - 1] = "Visipro.",
 [7][0x0b - 1] = "EKMemory",
 [7][0x0c - 1] = "Microelectronics Institute ZTE",
-[7][0x0d - 1] = "Cognovo Ltd.",
+[7][0x0d - 1] = "u-blox AG",
 [7][0x0e - 1] = "Carry Technology Co. Ltd.",
 [7][0x0f - 1] = "Nokia",
 [7][0x10 - 1] = "King Tiger Technology",
@@ -1101,12 +1101,12 @@
 [8][0x5c - 1] = "Vitesse Enterprise Co.",
 [8][0x5d - 1] = "Foxtronn International Corporation",
 [8][0x5e - 1] = "Bretelon Inc.",
-[8][0x5f - 1] = "Zbit Semiconductor, Inc.",
+[8][0x5f - 1] = "Graphcore",
 [8][0x60 - 1] = "Eoplex Inc",
 [8][0x61 - 1] = "MaxLinear, Inc.",
 [8][0x62 - 1] = "ETA Devices",
 [8][0x63 - 1] = "LOKI",
-[8][0x64 - 1] = "IMS Semiconductor Co., Ltd",
+[8][0x64 - 1] = "IMS Electronics Co., Ltd.",
 [8][0x65 - 1] = "Dosilicon Co., Ltd.",
 [8][0x66 - 1] = "Dolphin Integration",
 [8][0x67 - 1] = "Shenzhen Mic Electronics Technology",
@@ -1116,4 +1116,41 @@
 [8][0x6b - 1] = "Kingred Electronic Technology Ltd.",
 [8][0x6c - 1] = "Chao Yue Zhuo Computer Business Dept.",
 [8][0x6d - 1] = "Guangzhou Si Nuo Electronic Technology.",
+[8][0x6e - 1] = "Crocus Technology Inc.",
+[8][0x6f - 1] = "Creative Chips GmbH",
+[8][0x70 - 1] = "GE Aviation Systems LLC.",
+[8][0x71 - 1] = "Asgard",
+[8][0x72 - 1] = "Good Wealth Technology Ltd.",
+[8][0x73 - 1] = "TriCor Technologies",
+[8][0x74 - 1] = "Nova-Systems GmbH",
+[8][0x75 - 1] = "JUHOR",
+[8][0x76 - 1] = "Zhuhai Douke Commerce Co. Ltd.",
+[8][0x77 - 1] = "DSL Memory",
+[8][0x78 - 1] = "Anvo-Systems Dresden GmbH",
+[8][0x79 - 1] = "Realtek",
+[8][0x7a - 1] = "AltoBeam",
+[8][0x7b - 1] = "Wave Computing",
+[8][0x7c - 1] = "Beijing TrustNet Technology Co. Ltd.",
+[8][0x7d - 1] = "Innovium, Inc.",
+[8][0x7e - 1] = "Starsway Technology Limited",
+[9][0x01 - 1] = "Weltronics Co. LTD",
+[9][0x02 - 1] = "VMware, Inc.",
+[9][0x03 - 1] = "Hewlett Packard Enterprise",
+[9][0x04 - 1] = "INTENSO",
+[9][0x05 - 1] = "Puya Semiconductor",
+[9][0x06 - 1] = "MEMORFI",
+[9][0x07 - 1] = "MSC Technologies GmbH",
+[9][0x08 - 1] = "Txrui",
+[9][0x09 - 1] = "SiFive, Inc.",
+[9][0x0a - 1] = "Spreadtrum Communications",
+[9][0x0b - 1] = "Paragon Technology (Shenzhen) Ltd.",
+[9][0x0c - 1] = "UMAX Technology",
+[9][0x0d - 1] = "Shenzhen Yong Sheng Technology",
+[9][0x0e - 1] = "SNOAMOO (Shenzhen Kai Zhuo Yue)",
+[9][0x0f - 1] = "Daten Tecnologia LTDA",
+[9][0x10 - 1] = "Shenzhen XinRuiYan Electronics",
+[9][0x11 - 1] = "Eta Compute",
+[9][0x12 - 1] = "Energous",
+[9][0x13 - 1] = "Raspberry Pi Trading Ltd.",
+[9][0x14 - 1] = "Shenzhen Chixingzhe Tech Co. Ltd.",
 /* EOF */
diff --git a/src/helper/log.c b/src/helper/log.c
index e33f869..d4e87f6 100644
--- a/src/helper/log.c
+++ b/src/helper/log.c
@@ -191,6 +191,30 @@ void log_printf(enum log_levels level,
 	va_end(ap);
 }
 
+void log_vprintf_lf(enum log_levels level, const char *file, unsigned line,
+		const char *function, const char *format, va_list args)
+{
+	char *tmp;
+
+	count++;
+
+	if (level > debug_level)
+		return;
+
+	tmp = alloc_vprintf(format, args);
+
+	if (!tmp)
+		return;
+
+	/*
+	 * Note: alloc_vprintf() guarantees that the buffer is at least one
+	 * character longer.
+	 */
+	strcat(tmp, "\n");
+	log_puts(level, file, line, function, tmp);
+	free(tmp);
+}
+
 void log_printf_lf(enum log_levels level,
 	const char *file,
 	unsigned line,
@@ -198,23 +222,10 @@ void log_printf_lf(enum log_levels level,
 	const char *format,
 	...)
 {
-	char *string;
 	va_list ap;
 
-	count++;
-	if (level > debug_level)
-		return;
-
 	va_start(ap, format);
-
-	string = alloc_vprintf(format, ap);
-	if (string != NULL) {
-		strcat(string, "\n");	/* alloc_vprintf guaranteed the buffer to be at least one
-					 *char longer */
-		log_puts(level, file, line, function, string);
-		free(string);
-	}
-
+	log_vprintf_lf(level, file, line, function, format, ap);
 	va_end(ap);
 }
 
@@ -240,9 +251,15 @@ COMMAND_HANDLER(handle_log_output_command)
 {
 	if (CMD_ARGC == 1) {
 		FILE *file = fopen(CMD_ARGV[0], "w");
-
-		if (file)
-			log_output = file;
+		if (file == NULL) {
+			LOG_ERROR("failed to open output log '%s'", CMD_ARGV[0]);
+			return ERROR_FAIL;
+		}
+		if (log_output != stderr && log_output != NULL) {
+			/* Close previous log file, if it was open and wasn't stderr. */
+			fclose(log_output);
+		}
+		log_output = file;
 	}
 
 	return ERROR_OK;
diff --git a/src/helper/log.h b/src/helper/log.h
index eb222cb..6b93816 100644
--- a/src/helper/log.h
+++ b/src/helper/log.h
@@ -60,6 +60,8 @@ enum log_levels {
 void log_printf(enum log_levels level, const char *file, unsigned line,
 		const char *function, const char *format, ...)
 __attribute__ ((format (PRINTF_ATTRIBUTE_FORMAT, 5, 6)));
+void log_vprintf_lf(enum log_levels level, const char *file, unsigned line,
+		const char *function, const char *format, va_list args);
 void log_printf_lf(enum log_levels level, const char *file, unsigned line,
 		const char *function, const char *format, ...)
 __attribute__ ((format (PRINTF_ATTRIBUTE_FORMAT, 5, 6)));
diff --git a/src/helper/options.c b/src/helper/options.c
index b7db10f..1cfa553 100644
--- a/src/helper/options.c
+++ b/src/helper/options.c
@@ -29,6 +29,15 @@
 
 #include <getopt.h>
 
+#include <limits.h>
+#include <stdlib.h>
+#if IS_DARWIN
+#include <libproc.h>
+#endif
+#ifdef HAVE_SYS_SYSCTL_H
+#include <sys/sysctl.h>
+#endif
+
 static int help_flag, version_flag;
 
 static const struct option long_options[] = {
@@ -50,52 +59,129 @@ int configuration_output_handler(struct command_context *context, const char *li
 	return ERROR_OK;
 }
 
-#ifdef _WIN32
-static char *find_suffix(const char *text, const char *suffix)
+/* Return the canonical path to the directory the openocd executable is in.
+ * The path should be absolute, use / as path separator and have all symlinks
+ * resolved. The returned string is malloc'd. */
+static char *find_exe_path(void)
 {
-	size_t text_len = strlen(text);
-	size_t suffix_len = strlen(suffix);
+	char *exepath = NULL;
 
-	if (suffix_len == 0)
-		return (char *)text + text_len;
+	do {
+#if IS_WIN32 && !IS_CYGWIN
+		exepath = malloc(MAX_PATH);
+		if (exepath == NULL)
+			break;
+		GetModuleFileName(NULL, exepath, MAX_PATH);
 
-	if (suffix_len > text_len || strncmp(text + text_len - suffix_len, suffix, suffix_len) != 0)
-		return NULL; /* Not a suffix of text */
+		/* Convert path separators to UNIX style, should work on Windows also. */
+		for (char *p = exepath; *p; p++) {
+			if (*p == '\\')
+				*p = '/';
+		}
 
-	return (char *)text + text_len - suffix_len;
-}
+#elif IS_DARWIN
+		exepath = malloc(PROC_PIDPATHINFO_MAXSIZE);
+		if (exepath == NULL)
+			break;
+		if (proc_pidpath(getpid(), exepath, PROC_PIDPATHINFO_MAXSIZE) <= 0) {
+			free(exepath);
+			exepath = NULL;
+		}
+
+#elif defined(CTL_KERN) && defined(KERN_PROC) && defined(KERN_PROC_PATHNAME) /* *BSD */
+#ifndef PATH_MAX
+#define PATH_MAX 1024
 #endif
+		char *path = malloc(PATH_MAX);
+		if (path == NULL)
+			break;
+		int mib[] = { CTL_KERN, KERN_PROC, KERN_PROC_PATHNAME, -1 };
+		size_t size = PATH_MAX;
 
-static void add_default_dirs(void)
-{
-	const char *run_prefix;
-	char *path;
+		if (sysctl(mib, (u_int)ARRAY_SIZE(mib), path, &size, NULL, 0) != 0)
+			break;
 
-#ifdef _WIN32
-	char strExePath[MAX_PATH];
-	GetModuleFileName(NULL, strExePath, MAX_PATH);
+#ifdef HAVE_REALPATH
+		exepath = realpath(path, NULL);
+		free(path);
+#else
+		exepath = path;
+#endif
 
-	/* Strip executable file name, leaving path */
-	*strrchr(strExePath, '\\') = '\0';
+#elif defined(HAVE_REALPATH) /* Assume POSIX.1-2008 */
+		/* Try Unices in order of likelihood. */
+		exepath = realpath("/proc/self/exe", NULL); /* Linux/Cygwin */
+		if (exepath == NULL)
+			exepath = realpath("/proc/self/path/a.out", NULL); /* Solaris */
+		if (exepath == NULL)
+			exepath = realpath("/proc/curproc/file", NULL); /* FreeBSD (Should be covered above) */
+#endif
+	} while (0);
+
+	if (exepath != NULL) {
+		/* Strip executable file name, leaving path */
+		*strrchr(exepath, '/') = '\0';
+	} else {
+		LOG_WARNING("Could not determine executable path, using configured BINDIR.");
+		LOG_DEBUG("BINDIR = %s", BINDIR);
+#ifdef HAVE_REALPATH
+		exepath = realpath(BINDIR, NULL);
+#else
+		exepath = strdup(BINDIR);
+#endif
+	}
+
+	return exepath;
+}
+
+static char *find_relative_path(const char *from, const char *to)
+{
+	size_t i;
 
-	/* Convert path separators to UNIX style, should work on Windows also. */
-	for (char *p = strExePath; *p; p++) {
-		if (*p == '\\')
-			*p = '/';
+	/* Skip common /-separated parts of from and to */
+	i = 0;
+	for (size_t n = 0; from[n] == to[n]; n++) {
+		if (from[n] == '\0') {
+			i = n;
+			break;
+		}
+		if (from[n] == '/')
+			i = n + 1;
+	}
+	from += i;
+	to += i;
+
+	/* Count number of /-separated non-empty parts of from */
+	i = 0;
+	while (from[0] != '\0') {
+		if (from[0] != '/')
+			i++;
+		char *next = strchr(from, '/');
+		if (next == NULL)
+			break;
+		from = next + 1;
 	}
 
-	char *end_of_prefix = find_suffix(strExePath, BINDIR);
-	if (end_of_prefix != NULL)
-		*end_of_prefix = '\0';
+	/* Prepend that number of ../ in front of to */
+	char *relpath = malloc(i * 3 + strlen(to) + 1);
+	relpath[0] = '\0';
+	for (size_t n = 0; n < i; n++)
+		strcat(relpath, "../");
+	strcat(relpath, to);
 
-	run_prefix = strExePath;
-#else
-	run_prefix = "";
-#endif
+	return relpath;
+}
+
+static void add_default_dirs(void)
+{
+	char *path;
+	char *exepath = find_exe_path();
+	char *bin2data = find_relative_path(BINDIR, PKGDATADIR);
 
 	LOG_DEBUG("bindir=%s", BINDIR);
 	LOG_DEBUG("pkgdatadir=%s", PKGDATADIR);
-	LOG_DEBUG("run_prefix=%s", run_prefix);
+	LOG_DEBUG("exepath=%s", exepath);
+	LOG_DEBUG("bin2data=%s", bin2data);
 
 	/*
 	 * The directory containing OpenOCD-supplied scripts should be
@@ -129,17 +215,20 @@ static void add_default_dirs(void)
 	}
 #endif
 
-	path = alloc_printf("%s%s%s", run_prefix, PKGDATADIR, "/site");
+	path = alloc_printf("%s/%s/%s", exepath, bin2data, "site");
 	if (path) {
 		add_script_search_dir(path);
 		free(path);
 	}
 
-	path = alloc_printf("%s%s%s", run_prefix, PKGDATADIR, "/scripts");
+	path = alloc_printf("%s/%s/%s", exepath, bin2data, "scripts");
 	if (path) {
 		add_script_search_dir(path);
 		free(path);
 	}
+
+	free(exepath);
+	free(bin2data);
 }
 
 int parse_cmdline_args(struct command_context *cmd_ctx, int argc, char *argv[])
@@ -178,8 +267,10 @@ int parse_cmdline_args(struct command_context *cmd_ctx, int argc, char *argv[])
 			case 'd':		/* --debug | -d */
 			{
 				char *command = alloc_printf("debug_level %s", optarg ? optarg : "3");
-				command_run_line(cmd_ctx, command);
+				int retval = command_run_line(cmd_ctx, command);
 				free(command);
+				if (retval != ERROR_OK)
+					return retval;
 				break;
 			}
 			case 'l':		/* --log_output | -l */
@@ -200,16 +291,26 @@ int parse_cmdline_args(struct command_context *cmd_ctx, int argc, char *argv[])
 				LOG_WARNING("deprecated option: -p/--pipe. Use '-c \"gdb_port pipe; "
 						"log_output openocd.log\"' instead.");
 				break;
+			default:  /* '?' */
+				/* getopt will emit an error message, all we have to do is bail. */
+				return ERROR_FAIL;
 		}
 	}
 
+	if (optind < argc) {
+		/* Catch extra arguments on the command line. */
+		LOG_OUTPUT("Unexpected command line argument: %s\n", argv[optind]);
+		return ERROR_FAIL;
+	}
+
 	if (help_flag) {
 		LOG_OUTPUT("Open On-Chip Debugger\nLicensed under GNU GPL v2\n");
 		LOG_OUTPUT("--help       | -h\tdisplay this help\n");
 		LOG_OUTPUT("--version    | -v\tdisplay OpenOCD version\n");
 		LOG_OUTPUT("--file       | -f\tuse configuration file <name>\n");
 		LOG_OUTPUT("--search     | -s\tdir to search for config files and scripts\n");
-		LOG_OUTPUT("--debug      | -d\tset debug level <0-3>\n");
+		LOG_OUTPUT("--debug      | -d\tset debug level to 3\n");
+		LOG_OUTPUT("             | -d<n>\tset debug level to <level>\n");
 		LOG_OUTPUT("--log_output | -l\tredirect log output to file <name>\n");
 		LOG_OUTPUT("--command    | -c\trun <command>\n");
 		exit(-1);
diff --git a/src/helper/types.h b/src/helper/types.h
index 1854ba8..58c9e72 100644
--- a/src/helper/types.h
+++ b/src/helper/types.h
@@ -296,14 +296,21 @@ static inline int parity_u32(uint32_t x)
  */
 
 #if !defined(_STDINT_H)
-#define PRIx32 "x"
 #define PRId32 "d"
-#define SCNx32 "x"
 #define PRIi32 "i"
+#define PRIo32 "o"
 #define PRIu32 "u"
+#define PRIx32 "x"
+#define PRIX32 "X"
+#define SCNx32 "x"
 #define PRId8 PRId32
 #define SCNx64 "llx"
+#define PRId64 "lld"
+#define PRIi64 "lli"
+#define PRIo64 "llo"
+#define PRIu64 "llu"
 #define PRIx64 "llx"
+#define PRIX64 "llX"
 
 typedef CYG_ADDRWORD intptr_t;
 typedef int64_t intmax_t;
@@ -337,4 +344,23 @@ typedef uint64_t uintmax_t;
 
 #endif
 
+#if BUILD_TARGET64
+typedef uint64_t target_addr_t;
+#define TARGET_ADDR_MAX UINT64_MAX
+#define TARGET_PRIdADDR PRId64
+#define TARGET_PRIuADDR PRIu64
+#define TARGET_PRIoADDR PRIo64
+#define TARGET_PRIxADDR PRIx64
+#define TARGET_PRIXADDR PRIX64
+#else
+typedef uint32_t target_addr_t;
+#define TARGET_ADDR_MAX UINT32_MAX
+#define TARGET_PRIdADDR PRId32
+#define TARGET_PRIuADDR PRIu32
+#define TARGET_PRIoADDR PRIo32
+#define TARGET_PRIxADDR PRIx32
+#define TARGET_PRIXADDR PRIX32
+#endif
+#define TARGET_ADDR_FMT "0x%8.8" TARGET_PRIxADDR
+
 #endif /* OPENOCD_HELPER_TYPES_H */
diff --git a/src/jtag/Makefile.am b/src/jtag/Makefile.am
index db3e6ff..50ee263 100644
--- a/src/jtag/Makefile.am
+++ b/src/jtag/Makefile.am
@@ -1,86 +1,72 @@
-include $(top_srcdir)/common.mk
+noinst_LTLIBRARIES += %D%/libjtag.la
 
-METASOURCES = AUTO
-noinst_LTLIBRARIES = libjtag.la
+JTAG_SRCS =
+%C%_libjtag_la_LIBADD =
 
-SUBDIRS =
-DRIVERFILES =
-libjtag_la_LIBADD =
-
-CLEANFILES =
-
-BUILT_SOURCES =
-
-BUILT_SOURCES += minidriver_imp.h
-CLEANFILES += minidriver_imp.h
+BUILT_SOURCES += %D%/minidriver_imp.h
+CLEANFILES += %D%/minidriver_imp.h
 
 if MINIDRIVER
 
 if ZY1000
-DRIVERFILES += zy1000/zy1000.c
-JTAG_MINIDRIVER_DIR = $(srcdir)/zy1000
+JTAG_SRCS += %D%/zy1000/zy1000.c
+JTAG_MINIDRIVER_DIR = %D%/zy1000
 endif
 if MINIDRIVER_DUMMY
-DRIVERFILES += minidummy/minidummy.c commands.c
-JTAG_MINIDRIVER_DIR = $(srcdir)/minidummy
+JTAG_SRCS += %D%/minidummy/minidummy.c %D%/commands.c
+JTAG_MINIDRIVER_DIR = %D%/minidummy
 endif
 
-MINIDRIVER_IMP_DIR = $(srcdir)/minidriver
+MINIDRIVER_IMP_DIR = %D%/minidriver
 
-jtag_minidriver.h: $(JTAG_MINIDRIVER_DIR)/jtag_minidriver.h
+%D%/jtag_minidriver.h: $(JTAG_MINIDRIVER_DIR)/jtag_minidriver.h
 	cp $< $@
 
-BUILT_SOURCES += jtag_minidriver.h
+BUILT_SOURCES += %D%/jtag_minidriver.h
 
-CLEANFILES += jtag_minidriver.h
+CLEANFILES += %D%/jtag_minidriver.h
 
 else
 
-MINIDRIVER_IMP_DIR = $(srcdir)/drivers
-DRIVERFILES += commands.c
+MINIDRIVER_IMP_DIR = %D%/drivers
+JTAG_SRCS += %D%/commands.c
 
 if HLADAPTER
-SUBDIRS += hla
-libjtag_la_LIBADD += $(top_builddir)/src/jtag/hla/libocdhla.la
+include %D%/hla/Makefile.am
+%C%_libjtag_la_LIBADD += $(top_builddir)/%D%/hla/libocdhla.la
 endif
 
 if AICE
-SUBDIRS += aice
-libjtag_la_LIBADD += $(top_builddir)/src/jtag/aice/libocdaice.la
+include %D%/aice/Makefile.am
+%C%_libjtag_la_LIBADD += $(top_builddir)/%D%/aice/libocdaice.la
 endif
 
-SUBDIRS += drivers
-libjtag_la_LIBADD += $(top_builddir)/src/jtag/drivers/libocdjtagdrivers.la
-
+include %D%/drivers/Makefile.am
+%C%_libjtag_la_LIBADD += $(top_builddir)/%D%/drivers/libocdjtagdrivers.la
 
 endif
-
 # endif // MINIDRIVER
 
-minidriver_imp.h: $(MINIDRIVER_IMP_DIR)/minidriver_imp.h
+%D%/minidriver_imp.h: $(MINIDRIVER_IMP_DIR)/minidriver_imp.h
 	cp $< $@
 
 
-libjtag_la_SOURCES = \
-	adapter.c \
-	core.c \
-	interface.c \
-	interfaces.c \
-	tcl.c \
-	$(DRIVERFILES)
-
-noinst_HEADERS = \
-	commands.h \
-	driver.h \
-	interface.h \
-	interfaces.h \
-	minidriver.h \
-	jtag.h \
-	minidriver/minidriver_imp.h \
-	minidummy/jtag_minidriver.h \
-	swd.h \
-	tcl.h
-
-EXTRA_DIST = startup.tcl
-
-MAINTAINERCLEANFILES = $(srcdir)/Makefile.in
+%C%_libjtag_la_SOURCES = \
+	%D%/adapter.c \
+	%D%/core.c \
+	%D%/interface.c \
+	%D%/interfaces.c \
+	%D%/tcl.c \
+	%D%/commands.h \
+	%D%/driver.h \
+	%D%/interface.h \
+	%D%/interfaces.h \
+	%D%/minidriver.h \
+	%D%/jtag.h \
+	%D%/minidriver/minidriver_imp.h \
+	%D%/minidummy/jtag_minidriver.h \
+	%D%/swd.h \
+	%D%/tcl.h \
+	$(JTAG_SRCS)
+
+STARTUP_TCL_SRCS += %D%/startup.tcl
diff --git a/src/jtag/aice/Makefile.am b/src/jtag/aice/Makefile.am
index 7b9469d..97e3825 100644
--- a/src/jtag/aice/Makefile.am
+++ b/src/jtag/aice/Makefile.am
@@ -1,27 +1,14 @@
-include $(top_srcdir)/common.mk
-
-AM_CPPFLAGS += -I$(top_srcdir)/src/jtag/drivers $(LIBUSB1_CFLAGS) $(LIBUSB0_CFLAGS)
-
-noinst_LTLIBRARIES = libocdaice.la
-
-libocdaice_la_SOURCES = \
-	$(AICEFILES)
-
-AICEFILES =
-
-if AICE
-AICEFILES += aice_transport.c
-AICEFILES += aice_interface.c
-AICEFILES += aice_port.c
-AICEFILES += aice_usb.c
-AICEFILES += aice_pipe.c
-endif
-
-noinst_HEADERS = \
-	aice_transport.h \
-	aice_interface.h \
-	aice_port.h \
-	aice_usb.h \
-	aice_pipe.h
-
-MAINTAINERCLEANFILES = $(srcdir)/Makefile.in
+noinst_LTLIBRARIES += %D%/libocdaice.la
+
+%C%_libocdaice_la_CPPFLAGS = -I$(top_srcdir)/src/jtag/drivers $(AM_CPPFLAGS) $(LIBUSB1_CFLAGS) $(LIBUSB0_CFLAGS)
+%C%_libocdaice_la_SOURCES = \
+	%D%/aice_transport.c \
+	%D%/aice_interface.c \
+	%D%/aice_port.c \
+	%D%/aice_usb.c \
+	%D%/aice_pipe.c \
+	%D%/aice_transport.h \
+	%D%/aice_interface.h \
+	%D%/aice_port.h \
+	%D%/aice_usb.h \
+	%D%/aice_pipe.h
diff --git a/src/jtag/aice/aice_pipe.c b/src/jtag/aice/aice_pipe.c
index 18ad40e..bdc8c09 100644
--- a/src/jtag/aice/aice_pipe.c
+++ b/src/jtag/aice/aice_pipe.c
@@ -786,8 +786,8 @@ static int aice_pipe_memory_mode(uint32_t coreid, enum nds_memory_select mem_sel
 		return ERROR_FAIL;
 }
 
-static int aice_pipe_read_tlb(uint32_t coreid, uint32_t virtual_address,
-		uint32_t *physical_address)
+static int aice_pipe_read_tlb(uint32_t coreid, target_addr_t virtual_address,
+		target_addr_t *physical_address)
 {
 	char line[AICE_PIPE_MAXLINE];
 	char command[AICE_PIPE_MAXLINE];
diff --git a/src/jtag/aice/aice_port.h b/src/jtag/aice/aice_port.h
index 4568ce1..d3d6a1a 100644
--- a/src/jtag/aice/aice_port.h
+++ b/src/jtag/aice/aice_port.h
@@ -180,7 +180,7 @@ struct aice_port_api_s {
 	int (*memory_mode)(uint32_t coreid, enum nds_memory_select mem_select);
 
 	/** */
-	int (*read_tlb)(uint32_t coreid, uint32_t virtual_address, uint32_t *physical_address);
+	int (*read_tlb)(uint32_t coreid, target_addr_t virtual_address, target_addr_t *physical_address);
 
 	/** */
 	int (*cache_ctl)(uint32_t coreid, uint32_t subtype, uint32_t address);
diff --git a/src/jtag/aice/aice_usb.c b/src/jtag/aice/aice_usb.c
index b27f720..50468f3 100644
--- a/src/jtag/aice/aice_usb.c
+++ b/src/jtag/aice/aice_usb.c
@@ -2135,10 +2135,16 @@ static int aice_usb_open(struct aice_port_param_s *param)
 
 	/* usb_set_configuration required under win32 */
 	jtag_libusb_set_configuration(devh, 0);
+	jtag_libusb_claim_interface(devh, 0);
 
 	unsigned int aice_read_ep;
 	unsigned int aice_write_ep;
-	jtag_libusb_choose_interface(devh, &aice_read_ep, &aice_write_ep, -1, -1, -1);
+#ifdef HAVE_LIBUSB1
+	jtag_libusb_choose_interface(devh, &aice_read_ep, &aice_write_ep, -1, -1, -1, LIBUSB_TRANSFER_TYPE_BULK);
+#else
+	jtag_libusb_choose_interface(devh, &aice_read_ep, &aice_write_ep, -1, -1, -1, USB_ENDPOINT_TYPE_BULK);
+#endif
+	LOG_DEBUG("aice_read_ep=0x%x, aice_write_ep=0x%x", aice_read_ep, aice_write_ep);
 
 	aice_handler.usb_read_ep = aice_read_ep;
 	aice_handler.usb_write_ep = aice_write_ep;
@@ -3424,10 +3430,10 @@ static int aice_usb_memory_mode(uint32_t coreid, enum nds_memory_select mem_sele
 	return ERROR_OK;
 }
 
-static int aice_usb_read_tlb(uint32_t coreid, uint32_t virtual_address,
-		uint32_t *physical_address)
+static int aice_usb_read_tlb(uint32_t coreid, target_addr_t virtual_address,
+		target_addr_t *physical_address)
 {
-	LOG_DEBUG("aice_usb_read_tlb, virtual address: 0x%08" PRIx32, virtual_address);
+	LOG_DEBUG("aice_usb_read_tlb, virtual address: 0x%08" TARGET_PRIxADDR, virtual_address);
 
 	uint32_t instructions[4];
 	uint32_t probe_result;
diff --git a/src/jtag/core.c b/src/jtag/core.c
index 3da69ff..e21d476 100644
--- a/src/jtag/core.c
+++ b/src/jtag/core.c
@@ -1831,11 +1831,11 @@ void jtag_set_reset_config(enum reset_types type)
 
 int jtag_get_trst(void)
 {
-	return jtag_trst;
+	return jtag_trst == 1;
 }
 int jtag_get_srst(void)
 {
-	return jtag_srst;
+	return jtag_srst == 1;
 }
 
 void jtag_set_nsrst_delay(unsigned delay)
diff --git a/src/jtag/drivers/Makefile.am b/src/jtag/drivers/Makefile.am
index 2aaf8fd..3e5974d 100644
--- a/src/jtag/drivers/Makefile.am
+++ b/src/jtag/drivers/Makefile.am
@@ -1,173 +1,179 @@
-include $(top_srcdir)/common.mk
+noinst_LTLIBRARIES += %D%/libocdjtagdrivers.la
+%C%_libocdjtagdrivers_la_LIBADD =
 
-noinst_LTLIBRARIES = libocdjtagdrivers.la
-libocdjtagdrivers_la_LIBADD =
+%C%_libocdjtagdrivers_la_SOURCES = \
+	$(DRIVERFILES) \
+	$(DRIVERHEADERS)
 
-libocdjtagdrivers_la_SOURCES = \
-	$(DRIVERFILES)
+%C%_libocdjtagdrivers_la_CPPFLAGS = $(AM_CPPFLAGS)
 
-libocdjtagdrivers_la_CPPFLAGS = $(AM_CPPFLAGS) $(LIBUSB1_CFLAGS) \
-	$(LIBUSB0_CFLAGS) $(HIDAPI_CFLAGS) $(LIBFTDI_CFLAGS)
+ULINK_FIRMWARE = %D%/OpenULINK
 
-ULINK_FIRMWARE = $(srcdir)/OpenULINK
-
-EXTRA_DIST = $(ULINK_FIRMWARE) \
-	usb_blaster/README.CheapClone \
-	Makefile.rlink \
-	rlink_call.m4 \
-	rlink_init.m4
+EXTRA_DIST += $(ULINK_FIRMWARE) \
+	%D%/usb_blaster/README.CheapClone \
+	%D%/Makefile.rlink \
+	%D%/rlink_call.m4 \
+	%D%/rlink_init.m4
 
 DRIVERFILES =
-SUBDIRS=
 
-if JLINK
-if INTERNAL_LIBJAYLINK
-SUBDIRS += libjaylink
+# Standard Driver: common files
+DRIVERFILES += %D%/driver.c
 
-libjaylink_internal_la_SOURCES = jlink.c
-libjaylink_internal_la_LIBADD = libjaylink/libjaylink/libjaylink.la
-libjaylink_internal_la_CPPFLAGS = -I$(builddir)/libjaylink/libjaylink \
-	-I$(srcdir)/libjaylink $(AM_CPPFLAGS)
+if USE_LIBUSB1
+DRIVERFILES += %D%/libusb1_common.c
+%C%_libocdjtagdrivers_la_CPPFLAGS += $(LIBUSB1_CFLAGS)
+%C%_libocdjtagdrivers_la_LIBADD += $(LIBUSB1_LIBS)
+endif
 
-noinst_LTLIBRARIES += libjaylink_internal.la
-libocdjtagdrivers_la_LIBADD += libjaylink_internal.la
-else
-DRIVERFILES += jlink.c
+if USE_LIBUSB0
+DRIVERFILES += %D%/usb_common.c
+%C%_libocdjtagdrivers_la_CPPFLAGS += $(LIBUSB0_CFLAGS)
+%C%_libocdjtagdrivers_la_LIBADD += $(LIBUSB0_LIBS)
+if !USE_LIBUSB1
+DRIVERFILES += %D%/libusb0_common.c
 endif
 endif
 
-# Standard Driver: common files
-DRIVERFILES += driver.c
+if USE_LIBFTDI
+%C%_libocdjtagdrivers_la_CPPFLAGS += $(LIBFTDI_CFLAGS)
+%C%_libocdjtagdrivers_la_LIBADD += $(LIBFTDI_LIBS)
+endif
 
-if USE_LIBUSB1
-DRIVERFILES += libusb1_common.c
+if USE_HIDAPI
+%C%_libocdjtagdrivers_la_CPPFLAGS += $(HIDAPI_CFLAGS)
+%C%_libocdjtagdrivers_la_LIBADD += $(HIDAPI_LIBS)
 endif
 
-if USE_LIBUSB0
-DRIVERFILES += usb_common.c
-if !USE_LIBUSB1
-DRIVERFILES += libusb0_common.c
+if USE_LIBJAYLINK
+%C%_libocdjtagdrivers_la_CPPFLAGS += $(LIBJAYLINK_CFLAGS)
+%C%_libocdjtagdrivers_la_LIBADD += $(LIBJAYLINK_LIBS)
+endif
+
+if JLINK
+DRIVERFILES += %D%/jlink.c
+if INTERNAL_LIBJAYLINK
+SUBDIRS += %D%/libjaylink
+DIST_SUBDIRS += %D%/libjaylink
+
+%C%_libocdjtagdrivers_la_LIBADD += %D%/libjaylink/libjaylink/libjaylink.la
+%C%_libocdjtagdrivers_la_CPPFLAGS += -I$(builddir)/%D%/libjaylink/libjaylink -I$(srcdir)/%D%/libjaylink
 endif
 endif
 
 if BITBANG
-DRIVERFILES += bitbang.c
+DRIVERFILES += %D%/bitbang.c
 endif
 if PARPORT
-DRIVERFILES += parport.c
+DRIVERFILES += %D%/parport.c
 endif
 if DUMMY
-DRIVERFILES += dummy.c
-endif
-if FT2232_DRIVER
-DRIVERFILES += ft2232.c
+DRIVERFILES += %D%/dummy.c
 endif
 if FTDI
-DRIVERFILES += ftdi.c mpsse.c
+DRIVERFILES += %D%/ftdi.c %D%/mpsse.c
 endif
 if JTAG_VPI
-DRIVERFILES += jtag_vpi.c
+DRIVERFILES += %D%/jtag_vpi.c
 endif
 if USB_BLASTER_DRIVER
-SUBDIRS += usb_blaster
-libocdjtagdrivers_la_LIBADD += $(top_builddir)/src/jtag/drivers/usb_blaster/libocdusbblaster.la
+%C%_libocdjtagdrivers_la_LIBADD += %D%/usb_blaster/libocdusbblaster.la
+include %D%/usb_blaster/Makefile.am
 endif
 if AMTJTAGACCEL
-DRIVERFILES += amt_jtagaccel.c
+DRIVERFILES += %D%/amt_jtagaccel.c
 endif
 if EP93XX
-DRIVERFILES += ep93xx.c
+DRIVERFILES += %D%/ep93xx.c
 endif
 if AT91RM9200
-DRIVERFILES += at91rm9200.c
+DRIVERFILES += %D%/at91rm9200.c
 endif
 if GW16012
-DRIVERFILES += gw16012.c
+DRIVERFILES += %D%/gw16012.c
 endif
 if BITQ
-DRIVERFILES += bitq.c
+DRIVERFILES += %D%/bitq.c
 endif
-if PRESTO_DRIVER
-DRIVERFILES += presto.c
+if PRESTO
+DRIVERFILES += %D%/presto.c
 endif
 if USBPROG
-DRIVERFILES += usbprog.c
+DRIVERFILES += %D%/usbprog.c
 endif
 if RLINK
-DRIVERFILES += rlink.c rlink_speed_table.c
+DRIVERFILES += %D%/rlink.c %D%/rlink_speed_table.c
 endif
 if ULINK
-DRIVERFILES += ulink.c
+DRIVERFILES += %D%/ulink.c
 ulinkdir = $(pkgdatadir)/OpenULINK
 dist_ulink_DATA = $(ULINK_FIRMWARE)/ulink_firmware.hex
+%C%_libocdjtagdrivers_la_LIBADD += -lm
 endif
 if VSLLINK
-DRIVERFILES += versaloon/usbtoxxx/usbtogpio.c
-DRIVERFILES += versaloon/usbtoxxx/usbtojtagraw.c
-DRIVERFILES += versaloon/usbtoxxx/usbtoswd.c
-DRIVERFILES += versaloon/usbtoxxx/usbtopwr.c
-DRIVERFILES += versaloon/usbtoxxx/usbtoxxx.c
-DRIVERFILES += versaloon/versaloon.c
-DRIVERFILES += vsllink.c
+DRIVERFILES += %D%/versaloon/usbtoxxx/usbtogpio.c
+DRIVERFILES += %D%/versaloon/usbtoxxx/usbtojtagraw.c
+DRIVERFILES += %D%/versaloon/usbtoxxx/usbtoswd.c
+DRIVERFILES += %D%/versaloon/usbtoxxx/usbtopwr.c
+DRIVERFILES += %D%/versaloon/usbtoxxx/usbtoxxx.c
+DRIVERFILES += %D%/versaloon/versaloon.c
+DRIVERFILES += %D%/vsllink.c
 endif
 if ARMJTAGEW
-DRIVERFILES += arm-jtag-ew.c
+DRIVERFILES += %D%/arm-jtag-ew.c
 endif
 if BUSPIRATE
-DRIVERFILES += buspirate.c
+DRIVERFILES += %D%/buspirate.c
 endif
 if REMOTE_BITBANG
-DRIVERFILES += remote_bitbang.c
+DRIVERFILES += %D%/remote_bitbang.c
 endif
 if HLADAPTER
-DRIVERFILES += stlink_usb.c
-DRIVERFILES += ti_icdi_usb.c
+DRIVERFILES += %D%/stlink_usb.c
+DRIVERFILES += %D%/ti_icdi_usb.c
 endif
 if OSBDM
-DRIVERFILES += osbdm.c
+DRIVERFILES += %D%/osbdm.c
 endif
 if OPENDOUS
-DRIVERFILES += opendous.c
+DRIVERFILES += %D%/opendous.c
 endif
 if SYSFSGPIO
-DRIVERFILES += sysfsgpio.c
+DRIVERFILES += %D%/sysfsgpio.c
 endif
 if BCM2835GPIO
-DRIVERFILES += bcm2835gpio.c
+DRIVERFILES += %D%/bcm2835gpio.c
 endif
-
 if OPENJTAG
-DRIVERFILES += openjtag.c
+DRIVERFILES += %D%/openjtag.c
 endif
-
 if CMSIS_DAP
-DRIVERFILES += cmsis_dap_usb.c
-endif
-
-noinst_HEADERS = \
-	bitbang.h \
-	bitq.h \
-	ftd2xx_common.h \
-	libusb0_common.h \
-	libusb1_common.h \
-	libusb_common.h \
-	minidriver_imp.h \
-	mpsse.h \
-	rlink.h \
-	rlink_dtc_cmd.h \
-	rlink_ep1_cmd.h \
-	rlink_st7.h \
-	usb_common.h \
-	versaloon/usbtoxxx/usbtoxxx.h \
-	versaloon/usbtoxxx/usbtoxxx_internal.h \
-	versaloon/versaloon.h \
-	versaloon/versaloon_include.h \
-	versaloon/versaloon_internal.h
-
-DIST_SUBDIRS = usb_blaster
-
-if INTERNAL_LIBJAYLINK
-DIST_SUBDIRS += libjaylink
-endif
+DRIVERFILES += %D%/cmsis_dap_usb.c
+endif
+if IMX_GPIO
+DRIVERFILES += %D%/imx_gpio.c
+endif
+
+if KITPROG
+DRIVERFILES += %D%/kitprog.c
+endif
+
+DRIVERHEADERS = \
+	%D%/bitbang.h \
+	%D%/bitq.h \
+	%D%/libusb0_common.h \
+	%D%/libusb1_common.h \
+	%D%/libusb_common.h \
+	%D%/minidriver_imp.h \
+	%D%/mpsse.h \
+	%D%/rlink.h \
+	%D%/rlink_dtc_cmd.h \
+	%D%/rlink_ep1_cmd.h \
+	%D%/rlink_st7.h \
+	%D%/usb_common.h \
+	%D%/versaloon/usbtoxxx/usbtoxxx.h \
+	%D%/versaloon/usbtoxxx/usbtoxxx_internal.h \
+	%D%/versaloon/versaloon.h \
+	%D%/versaloon/versaloon_include.h \
+	%D%/versaloon/versaloon_internal.h
 
-MAINTAINERCLEANFILES = $(srcdir)/Makefile.in
diff --git a/src/jtag/drivers/bcm2835gpio.c b/src/jtag/drivers/bcm2835gpio.c
index 1622b22..a41caf0 100644
--- a/src/jtag/drivers/bcm2835gpio.c
+++ b/src/jtag/drivers/bcm2835gpio.c
@@ -468,8 +468,8 @@ static int bcm2835gpio_init(void)
 		return ERROR_JTAG_INIT_FAILED;
 	}
 
-	/* set 16mA drive strength */
-	pads_base[BCM2835_PADS_GPIO_0_27_OFFSET] = 0x5a000018 + 7;
+	/* set 4mA drive strength, slew rate limited, hysteresis on */
+	pads_base[BCM2835_PADS_GPIO_0_27_OFFSET] = 0x5a000008 + 1;
 
 	tdo_gpio_mode = MODE_GPIO(tdo_gpio);
 	tdi_gpio_mode = MODE_GPIO(tdi_gpio);
diff --git a/src/jtag/drivers/cmsis_dap_usb.c b/src/jtag/drivers/cmsis_dap_usb.c
index a07064b..86f7968 100644
--- a/src/jtag/drivers/cmsis_dap_usb.c
+++ b/src/jtag/drivers/cmsis_dap_usb.c
@@ -118,6 +118,13 @@ static bool swd_mode;
  * Bit 7: nRESET
  */
 
+#define SWJ_PIN_TCK               (1<<0)
+#define SWJ_PIN_TMS               (1<<1)
+#define SWJ_PIN_TDI               (1<<2)
+#define SWJ_PIN_TDO               (1<<3)
+#define SWJ_PIN_TRST              (1<<5)
+#define SWJ_PIN_SRST              (1<<7)
+
 /* CMSIS-DAP SWD Commands */
 #define CMD_DAP_SWD_CONFIGURE     0x13
 
@@ -309,9 +316,11 @@ static int cmsis_dap_usb_open(void)
 	int packet_size = PACKET_SIZE;
 
 	/* atmel cmsis-dap uses 512 byte reports */
+	/* except when it doesn't e.g. with mEDBG on SAMD10 Xplained
+	 * board */
 	/* TODO: HID report descriptor should be parsed instead of
 	 * hardcoding a match by VID */
-	if (target_vid == 0x03eb)
+	if (target_vid == 0x03eb && target_pid != 0x2145)
 		packet_size = 512 + 1;
 
 	cmsis_dap_handle->packet_buffer = malloc(packet_size);
@@ -764,12 +773,12 @@ static int cmsis_dap_get_status(void)
 
 	if (retval == ERROR_OK) {
 		LOG_INFO("SWCLK/TCK = %d SWDIO/TMS = %d TDI = %d TDO = %d nTRST = %d nRESET = %d",
-			(d & (0x01 << 0)) ? 1 : 0,	/* Bit 0: SWCLK/TCK */
-			(d & (0x01 << 1)) ? 1 : 0,	/* Bit 1: SWDIO/TMS */
-			(d & (0x01 << 2)) ? 1 : 0,	/* Bit 2: TDI */
-			(d & (0x01 << 3)) ? 1 : 0,	/* Bit 3: TDO */
-			(d & (0x01 << 5)) ? 1 : 0,	/* Bit 5: nTRST */
-			(d & (0x01 << 7)) ? 1 : 0);	/* Bit 7: nRESET */
+			(d & SWJ_PIN_TCK) ? 1 : 0,
+			(d & SWJ_PIN_TMS) ? 1 : 0,
+			(d & SWJ_PIN_TDI) ? 1 : 0,
+			(d & SWJ_PIN_TDO) ? 1 : 0,
+			(d & SWJ_PIN_TRST) ? 1 : 0,
+			(d & SWJ_PIN_SRST) ? 1 : 0);
 	}
 
 	return retval;
@@ -812,7 +821,13 @@ static int cmsis_dap_swd_switch_seq(enum swd_special_seq seq)
 		return ERROR_FAIL;
 	}
 
-	return cmsis_dap_cmd_DAP_SWJ_Sequence(s_len, s);
+	retval = cmsis_dap_cmd_DAP_SWJ_Sequence(s_len, s);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Atmel EDBG needs renew clock setting after SWJ_Sequence
+	 * otherwise default frequency is used */
+	return cmsis_dap_cmd_DAP_SWJ_Clock(jtag_get_speed_khz());
 }
 
 static int cmsis_dap_swd_open(void)
@@ -990,8 +1005,17 @@ static int cmsis_dap_quit(void)
 
 static void cmsis_dap_execute_reset(struct jtag_command *cmd)
 {
-	int retval = cmsis_dap_cmd_DAP_SWJ_Pins(cmd->cmd.reset->srst ? 0 : (1 << 7), \
-			(1 << 7), 0, NULL);
+	/* Set both TRST and SRST even if they're not enabled as
+	 * there's no way to tristate them */
+	uint8_t pins = 0;
+
+	if (!cmd->cmd.reset->srst)
+		pins |= SWJ_PIN_SRST;
+	if (!cmd->cmd.reset->trst)
+		pins |= SWJ_PIN_TRST;
+
+	int retval = cmsis_dap_cmd_DAP_SWJ_Pins(pins,
+			SWJ_PIN_TRST | SWJ_PIN_SRST, 0, NULL);
 	if (retval != ERROR_OK)
 		LOG_ERROR("CMSIS-DAP: Interface reset failed");
 }
@@ -1475,13 +1499,11 @@ static int cmsis_dap_execute_queue(void)
 
 static int cmsis_dap_speed(int speed)
 {
-	if (speed > DAP_MAX_CLOCK) {
-		LOG_INFO("reduce speed request: %dkHz to %dkHz maximum", speed, DAP_MAX_CLOCK);
-		speed = DAP_MAX_CLOCK;
-	}
+	if (speed > DAP_MAX_CLOCK)
+		LOG_INFO("High speed (adapter_khz %d) may be limited by adapter firmware.", speed);
 
 	if (speed == 0) {
-		LOG_INFO("RTCK not supported");
+		LOG_ERROR("RTCK not supported. Set nonzero adapter_khz.");
 		return ERROR_JTAG_NOT_IMPLEMENTED;
 	}
 
diff --git a/src/jtag/drivers/ft2232.c b/src/jtag/drivers/ft2232.c
deleted file mode 100644
index 621da8e..0000000
--- a/src/jtag/drivers/ft2232.c
+++ /dev/null
@@ -1,4308 +0,0 @@
-/***************************************************************************
-*   Copyright (C) 2009 by Øyvind Harboe                                   *
-*	Øyvind Harboe <oyvind.harboe@zylin.com>                               *
-*                                                                         *
-*   Copyright (C) 2009 by SoftPLC Corporation.  http://softplc.com        *
-*	Dick Hollenbeck <dick@softplc.com>                                    *
-*                                                                         *
-*   Copyright (C) 2004, 2006 by Dominic Rath                              *
-*   Dominic.Rath@gmx.de                                                   *
-*                                                                         *
-*   Copyright (C) 2008 by Spencer Oliver                                  *
-*   spen@spen-soft.co.uk                                                  *
-*                                                                         *
-*   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/>. *
-***************************************************************************/
-
-/**
- * @file
- * JTAG adapters based on the FT2232 full and high speed USB parts are
- * popular low cost JTAG debug solutions.  Many FT2232 based JTAG adapters
- * are discrete, but development boards may integrate them as alternatives
- * to more capable (and expensive) third party JTAG pods.
- *
- * JTAG uses only one of the two communications channels ("MPSSE engines")
- * on these devices.  Adapters based on FT4232 parts have four ports/channels
- * (A/B/C/D), instead of just two (A/B).
- *
- * Especially on development boards integrating one of these chips (as
- * opposed to discrete pods/dongles), the additional channels can be used
- * for a variety of purposes, but OpenOCD only uses one channel at a time.
- *
- *  - As a USB-to-serial adapter for the target's console UART ...
- *    which may be able to support ROM boot loaders that load initial
- *    firmware images to flash (or SRAM).
- *
- *  - On systems which support ARM's SWD in addition to JTAG, or instead
- *    of it, that second port can be used for reading SWV/SWO trace data.
- *
- *  - Additional JTAG links, e.g. to a CPLD or * FPGA.
- *
- * FT2232 based JTAG adapters are "dumb" not "smart", because most JTAG
- * request/response interactions involve round trips over the USB link.
- * A "smart" JTAG adapter has intelligence close to the scan chain, so it
- * can for example poll quickly for a status change (usually taking on the
- * order of microseconds not milliseconds) before beginning a queued
- * transaction which require the previous one to have completed.
- *
- * There are dozens of adapters of this type, differing in details which
- * this driver needs to understand.  Those "layout" details are required
- * as part of FT2232 driver configuration.
- *
- * This code uses information contained in the MPSSE specification which was
- * found here:
- * http://www.ftdichip.com/Documents/AppNotes/AN2232C-01_MPSSE_Cmnd.pdf
- * Hereafter this is called the "MPSSE Spec".
- *
- * The datasheet for the ftdichip.com's FT2232D part is here:
- * http://www.ftdichip.com/Documents/DataSheets/DS_FT2232D.pdf
- *
- * Also note the issue with code 0x4b (clock data to TMS) noted in
- * http://developer.intra2net.com/mailarchive/html/libftdi/2009/msg00292.html
- * which can affect longer JTAG state paths.
- */
-
-#ifdef HAVE_CONFIG_H
-#include "config.h"
-#endif
-
-/* project specific includes */
-#include <jtag/interface.h>
-#include <transport/transport.h>
-#include <helper/time_support.h>
-
-#if IS_CYGWIN == 1
-#include <windows.h>
-#endif
-
-#include <assert.h>
-
-#if (BUILD_FT2232_FTD2XX == 1 && BUILD_FT2232_LIBFTDI == 1)
-#error "BUILD_FT2232_FTD2XX && BUILD_FT2232_LIBFTDI are mutually exclusive"
-#elif (BUILD_FT2232_FTD2XX != 1 && BUILD_FT2232_LIBFTDI != 1)
-#error "BUILD_FT2232_FTD2XX || BUILD_FT2232_LIBFTDI must be chosen"
-#endif
-
-/* FT2232 access library includes */
-#if BUILD_FT2232_FTD2XX == 1
-#include <ftd2xx.h>
-#include "ftd2xx_common.h"
-
-enum ftdi_interface {
-	INTERFACE_ANY = 0,
-	INTERFACE_A   = 1,
-	INTERFACE_B   = 2,
-	INTERFACE_C   = 3,
-	INTERFACE_D   = 4
-};
-
-#elif BUILD_FT2232_LIBFTDI == 1
-#include <ftdi.h>
-#endif
-
-/* max TCK for the high speed devices 30000 kHz */
-#define	FTDI_x232H_MAX_TCK	30000
-/* max TCK for the full speed devices 6000 kHz */
-#define FTDI_2232C_MAX_TCK 6000
-/* this speed value tells that RTCK is requested */
-#define RTCK_SPEED -1
-
-/*
- * On my Athlon XP 1900+ EHCI host with FT2232H JTAG dongle I get read timeout
- * errors with a retry count of 100. Increasing it solves the problem for me.
- *	- Dimitar
- *
- * FIXME There's likely an issue with the usb_read_timeout from libftdi.
- * Fix that (libusb? kernel? libftdi? here?) and restore the retry count
- * to something sane.
- */
-#define LIBFTDI_READ_RETRY_COUNT		2000
-
-#ifndef BUILD_FT2232_HIGHSPEED
- #if BUILD_FT2232_FTD2XX == 1
-	enum { FT_DEVICE_2232H = 6, FT_DEVICE_4232H, FT_DEVICE_232H };
- #elif BUILD_FT2232_LIBFTDI == 1
-	enum ftdi_chip_type { TYPE_2232H = 4, TYPE_4232H = 5, TYPE_232H = 6 };
- #endif
-#endif
-
-/**
- * Send out \a num_cycles on the TCK line while the TAP(s) are in a
- * stable state.  Calling code must ensure that current state is stable,
- * that verification is not done in here.
- *
- * @param num_cycles The number of clocks cycles to send.
- * @param cmd The command to send.
- *
- * @returns ERROR_OK on success, or ERROR_JTAG_QUEUE_FAILED on failure.
- */
-static int ft2232_stableclocks(int num_cycles, struct jtag_command *cmd);
-
-static char *ft2232_device_desc_A;
-static char *ft2232_device_desc;
-static char *ft2232_serial;
-static uint8_t ft2232_latency = 2;
-static unsigned ft2232_max_tck = FTDI_2232C_MAX_TCK;
-static int ft2232_channel = INTERFACE_ANY;
-
-#define MAX_USB_IDS 8
-/* vid = pid = 0 marks the end of the list */
-static uint16_t ft2232_vid[MAX_USB_IDS + 1] = { 0x0403, 0 };
-static uint16_t ft2232_pid[MAX_USB_IDS + 1] = { 0x6010, 0 };
-
-struct ft2232_layout {
-	const char *name;
-	int (*init)(void);
-	void (*reset)(int trst, int srst);
-	void (*blink)(void);
-	int channel;
-};
-
-/* init procedures for supported layouts */
-static int usbjtag_init(void);
-static int jtagkey_init(void);
-static int lm3s811_jtag_init(void);
-static int icdi_jtag_init(void);
-static int olimex_jtag_init(void);
-static int flyswatter1_init(void);
-static int flyswatter2_init(void);
-static int minimodule_init(void);
-static int turtle_init(void);
-static int comstick_init(void);
-static int stm32stick_init(void);
-static int axm0432_jtag_init(void);
-static int sheevaplug_init(void);
-static int icebear_jtag_init(void);
-static int cortino_jtag_init(void);
-static int signalyzer_init(void);
-static int signalyzer_h_init(void);
-static int ktlink_init(void);
-static int redbee_init(void);
-static int lisa_l_init(void);
-static int flossjtag_init(void);
-static int xds100v2_init(void);
-static int digilent_hs1_init(void);
-
-/* reset procedures for supported layouts */
-static void ftx23_reset(int trst, int srst);
-static void jtagkey_reset(int trst, int srst);
-static void olimex_jtag_reset(int trst, int srst);
-static void flyswatter1_reset(int trst, int srst);
-static void flyswatter2_reset(int trst, int srst);
-static void minimodule_reset(int trst, int srst);
-static void turtle_reset(int trst, int srst);
-static void comstick_reset(int trst, int srst);
-static void stm32stick_reset(int trst, int srst);
-static void axm0432_jtag_reset(int trst, int srst);
-static void sheevaplug_reset(int trst, int srst);
-static void icebear_jtag_reset(int trst, int srst);
-static void signalyzer_h_reset(int trst, int srst);
-static void ktlink_reset(int trst, int srst);
-static void redbee_reset(int trst, int srst);
-static void xds100v2_reset(int trst, int srst);
-static void digilent_hs1_reset(int trst, int srst);
-
-/* blink procedures for layouts that support a blinking led */
-static void olimex_jtag_blink(void);
-static void flyswatter1_jtag_blink(void);
-static void flyswatter2_jtag_blink(void);
-static void turtle_jtag_blink(void);
-static void signalyzer_h_blink(void);
-static void ktlink_blink(void);
-static void lisa_l_blink(void);
-static void flossjtag_blink(void);
-
-/* common transport support options */
-
-/* static const char *jtag_and_swd[] = { "jtag", "swd", NULL }; */
-
-static const struct ft2232_layout  ft2232_layouts[] = {
-	{ .name = "usbjtag",
-		.init = usbjtag_init,
-		.reset = ftx23_reset,
-	},
-	{ .name = "jtagkey",
-		.init = jtagkey_init,
-		.reset = jtagkey_reset,
-	},
-	{ .name = "jtagkey_prototype_v1",
-		.init = jtagkey_init,
-		.reset = jtagkey_reset,
-	},
-	{ .name = "oocdlink",
-		.init = jtagkey_init,
-		.reset = jtagkey_reset,
-	},
-	{ .name = "signalyzer",
-		.init = signalyzer_init,
-		.reset = ftx23_reset,
-	},
-	{ .name = "evb_lm3s811",
-		.init = lm3s811_jtag_init,
-		.reset = ftx23_reset,
-	},
-	{ .name = "luminary_icdi",
-		.init = icdi_jtag_init,
-		.reset = ftx23_reset,
-	},
-	{ .name = "olimex-jtag",
-		.init = olimex_jtag_init,
-		.reset = olimex_jtag_reset,
-		.blink = olimex_jtag_blink
-	},
-	{ .name = "flyswatter",
-		.init = flyswatter1_init,
-		.reset = flyswatter1_reset,
-		.blink = flyswatter1_jtag_blink
-	},
-	{ .name = "flyswatter2",
-		.init = flyswatter2_init,
-		.reset = flyswatter2_reset,
-		.blink = flyswatter2_jtag_blink
-	},
-	{ .name = "minimodule",
-		.init = minimodule_init,
-		.reset = minimodule_reset,
-	},
-	{ .name = "turtelizer2",
-		.init = turtle_init,
-		.reset = turtle_reset,
-		.blink = turtle_jtag_blink
-	},
-	{ .name = "comstick",
-		.init = comstick_init,
-		.reset = comstick_reset,
-	},
-	{ .name = "stm32stick",
-		.init = stm32stick_init,
-		.reset = stm32stick_reset,
-	},
-	{ .name = "axm0432_jtag",
-		.init = axm0432_jtag_init,
-		.reset = axm0432_jtag_reset,
-	},
-	{ .name = "sheevaplug",
-		.init = sheevaplug_init,
-		.reset = sheevaplug_reset,
-	},
-	{ .name = "icebear",
-		.init = icebear_jtag_init,
-		.reset = icebear_jtag_reset,
-	},
-	{ .name = "cortino",
-		.init = cortino_jtag_init,
-		.reset = comstick_reset,
-	},
-	{ .name = "signalyzer-h",
-		.init = signalyzer_h_init,
-		.reset = signalyzer_h_reset,
-		.blink = signalyzer_h_blink
-	},
-	{ .name = "ktlink",
-		.init = ktlink_init,
-		.reset = ktlink_reset,
-		.blink = ktlink_blink
-	},
-	{ .name = "redbee-econotag",
-		.init = redbee_init,
-		.reset = redbee_reset,
-	},
-	{ .name = "redbee-usb",
-		.init = redbee_init,
-		.reset = redbee_reset,
-		.channel = INTERFACE_B,
-	},
-	{ .name = "lisa-l",
-		.init = lisa_l_init,
-		.reset = ftx23_reset,
-		.blink = lisa_l_blink,
-		.channel = INTERFACE_B,
-	},
-	{ .name = "flossjtag",
-		.init = flossjtag_init,
-		.reset = ftx23_reset,
-		.blink = flossjtag_blink,
-	},
-	{ .name = "xds100v2",
-		.init = xds100v2_init,
-		.reset = xds100v2_reset,
-	},
-	{ .name = "digilent-hs1",
-		.init = digilent_hs1_init,
-		.reset = digilent_hs1_reset,
-		.channel = INTERFACE_A,
-	},
-	{ .name = NULL, /* END OF TABLE */ },
-};
-
-/* bitmask used to drive nTRST; usually a GPIOLx signal */
-static uint8_t nTRST;
-static uint8_t nTRSTnOE;
-/* bitmask used to drive nSRST; usually a GPIOLx signal */
-static uint8_t nSRST;
-static uint8_t nSRSTnOE;
-
-/** the layout being used with this debug session */
-static const struct ft2232_layout *layout;
-
-/** default bitmask values driven on DBUS: TCK/TDI/TDO/TMS and GPIOL(0..4) */
-static uint8_t low_output;
-
-/* note that direction bit == 1 means that signal is an output */
-
-/** default direction bitmask for DBUS: TCK/TDI/TDO/TMS and GPIOL(0..4) */
-static uint8_t low_direction;
-/** default value bitmask for CBUS GPIOH(0..4) */
-static uint8_t high_output;
-/** default direction bitmask for CBUS GPIOH(0..4) */
-static uint8_t high_direction;
-
-#if BUILD_FT2232_FTD2XX == 1
-static FT_HANDLE ftdih;
-static FT_DEVICE ftdi_device;
-#elif BUILD_FT2232_LIBFTDI == 1
-static struct ftdi_context ftdic;
-static enum ftdi_chip_type ftdi_device;
-#endif
-
-static struct jtag_command *first_unsent;	/* next command that has to be sent */
-static int require_send;
-
-/*	http://urjtag.wiki.sourceforge.net/Cable + FT2232 says:
-
-	"There is a significant difference between libftdi and libftd2xx. The latter
-	one allows to schedule up to 64*64 bytes of result data while libftdi fails
-	with more than 4*64. As a consequence, the FT2232 driver is forced to
-	perform around 16x more USB transactions for long command streams with TDO
-	capture when running with libftdi."
-
-	No idea how we get
-	#define FT2232_BUFFER_SIZE 131072
-	a comment would have been nice.
-*/
-
-#if BUILD_FT2232_FTD2XX == 1
-#define FT2232_BUFFER_READ_QUEUE_SIZE (64*64)
-#else
-#define FT2232_BUFFER_READ_QUEUE_SIZE (64*4)
-#endif
-
-#define FT2232_BUFFER_SIZE 131072
-
-static uint8_t *ft2232_buffer;
-static int ft2232_buffer_size;
-static int ft2232_read_pointer;
-static int ft2232_expect_read;
-
-/**
- * Function buffer_write
- * writes a byte into the byte buffer, "ft2232_buffer", which must be sent later.
- * @param val is the byte to send.
- */
-static inline void buffer_write(uint8_t val)
-{
-	assert(ft2232_buffer);
-	assert((unsigned) ft2232_buffer_size < (unsigned) FT2232_BUFFER_SIZE);
-	ft2232_buffer[ft2232_buffer_size++] = val;
-}
-
-/**
- * Function buffer_read
- * returns a byte from the byte buffer.
- */
-static inline uint8_t buffer_read(void)
-{
-	assert(ft2232_buffer);
-	assert(ft2232_read_pointer < ft2232_buffer_size);
-	return ft2232_buffer[ft2232_read_pointer++];
-}
-
-/**
- * Clocks out \a bit_count bits on the TMS line, starting with the least
- * significant bit of tms_bits and progressing to more significant bits.
- * Rigorous state transition logging is done here via tap_set_state().
- *
- * @param mpsse_cmd One of the MPSSE TMS oriented commands such as
- *	0x4b or 0x6b.  See the MPSSE spec referenced above for their
- *	functionality. The MPSSE command "Clock Data to TMS/CS Pin (no Read)"
- *	is often used for this, 0x4b.
- *
- * @param tms_bits Holds the sequence of bits to send.
- * @param tms_count Tells how many bits in the sequence.
- * @param tdi_bit A single bit to pass on to TDI before the first TCK
- *	cycle and held static for the duration of TMS clocking.
- *
- * See the MPSSE spec referenced above.
- */
-static void clock_tms(uint8_t mpsse_cmd, int tms_bits, int tms_count, bool tdi_bit)
-{
-	uint8_t tms_byte;
-	int i;
-	int tms_ndx;	/* bit index into tms_byte */
-
-	assert(tms_count > 0);
-
-	DEBUG_JTAG_IO("mpsse cmd=%02x, tms_bits = 0x%08x, bit_count=%d",
-		mpsse_cmd, tms_bits, tms_count);
-
-	for (tms_byte = tms_ndx = i = 0; i < tms_count; ++i, tms_bits >>= 1) {
-		bool bit = tms_bits & 1;
-
-		if (bit)
-			tms_byte |= (1 << tms_ndx);
-
-		/* always do state transitions in public view */
-		tap_set_state(tap_state_transition(tap_get_state(), bit));
-
-		/*	we wrote a bit to tms_byte just above, increment bit index.  if bit was zero
-		 * also increment.
-		*/
-		++tms_ndx;
-
-		if (tms_ndx == 7 || i == tms_count-1) {
-			buffer_write(mpsse_cmd);
-			buffer_write(tms_ndx - 1);
-
-			/*	Bit 7 of the byte is passed on to TDI/DO before the first TCK/SK of
-			 * TMS/CS and is held static for the duration of TMS/CS clocking.
-			*/
-			buffer_write(tms_byte | (tdi_bit << 7));
-		}
-	}
-}
-
-/**
- * Function get_tms_buffer_requirements
- * returns what clock_tms() will consume if called with
- * same \a bit_count.
- */
-static inline int get_tms_buffer_requirements(int bit_count)
-{
-	return ((bit_count + 6)/7) * 3;
-}
-
-/**
- * Function move_to_state
- * moves the TAP controller from the current state to a
- * \a goal_state through a path given by tap_get_tms_path().  State transition
- * logging is performed by delegation to clock_tms().
- *
- * @param goal_state is the destination state for the move.
- */
-static void move_to_state(tap_state_t goal_state)
-{
-	tap_state_t start_state = tap_get_state();
-
-	/*	goal_state is 1/2 of a tuple/pair of states which allow convenient
-	 * lookup of the required TMS pattern to move to this state from the start state.
-	*/
-
-	/* do the 2 lookups */
-	int tms_bits  = tap_get_tms_path(start_state, goal_state);
-	int tms_count = tap_get_tms_path_len(start_state, goal_state);
-
-	DEBUG_JTAG_IO("start=%s goal=%s", tap_state_name(start_state), tap_state_name(goal_state));
-
-	clock_tms(0x4b,  tms_bits, tms_count, 0);
-}
-
-static int ft2232_write(uint8_t *buf, int size, uint32_t *bytes_written)
-{
-#if BUILD_FT2232_FTD2XX == 1
-	FT_STATUS status;
-	DWORD dw_bytes_written = 0;
-	status = FT_Write(ftdih, buf, size, &dw_bytes_written);
-	if (status != FT_OK) {
-		*bytes_written = dw_bytes_written;
-		LOG_ERROR("FT_Write returned: %s", ftd2xx_status_string(status));
-		return ERROR_JTAG_DEVICE_ERROR;
-	} else
-		*bytes_written = dw_bytes_written;
-
-#elif BUILD_FT2232_LIBFTDI == 1
-	int retval = ftdi_write_data(&ftdic, buf, size);
-	if (retval < 0) {
-		*bytes_written = 0;
-		LOG_ERROR("ftdi_write_data: %s", ftdi_get_error_string(&ftdic));
-		return ERROR_JTAG_DEVICE_ERROR;
-	} else
-		*bytes_written = retval;
-
-#endif
-
-	if (*bytes_written != (uint32_t)size)
-		return ERROR_JTAG_DEVICE_ERROR;
-
-	return ERROR_OK;
-}
-
-static int ft2232_read(uint8_t *buf, uint32_t size, uint32_t *bytes_read)
-{
-#if BUILD_FT2232_FTD2XX == 1
-	DWORD dw_bytes_read;
-	FT_STATUS status;
-	int timeout = 5;
-	*bytes_read = 0;
-
-	while ((*bytes_read < size) && timeout--) {
-		status = FT_Read(ftdih, buf + *bytes_read, size -
-				*bytes_read, &dw_bytes_read);
-		if (status != FT_OK) {
-			*bytes_read = 0;
-			LOG_ERROR("FT_Read returned: %s", ftd2xx_status_string(status));
-			return ERROR_JTAG_DEVICE_ERROR;
-		}
-		*bytes_read += dw_bytes_read;
-	}
-
-#elif BUILD_FT2232_LIBFTDI == 1
-	int retval;
-	int timeout = LIBFTDI_READ_RETRY_COUNT;
-	*bytes_read = 0;
-
-	while ((*bytes_read < size) && timeout--) {
-		retval = ftdi_read_data(&ftdic, buf + *bytes_read, size - *bytes_read);
-		if (retval < 0) {
-			*bytes_read = 0;
-			LOG_ERROR("ftdi_read_data: %s", ftdi_get_error_string(&ftdic));
-			return ERROR_JTAG_DEVICE_ERROR;
-		}
-		*bytes_read += retval;
-	}
-
-#endif
-
-	if (*bytes_read < size) {
-		LOG_ERROR("couldn't read enough bytes from "
-			"FT2232 device (%i < %i)",
-			(unsigned)*bytes_read,
-			(unsigned)size);
-		return ERROR_JTAG_DEVICE_ERROR;
-	}
-
-	return ERROR_OK;
-}
-
-static bool ft2232_device_is_highspeed(void)
-{
-#if BUILD_FT2232_FTD2XX == 1
-	return (ftdi_device == FT_DEVICE_2232H) || (ftdi_device == FT_DEVICE_4232H)
- #ifdef HAS_ENUM_FT232H
-		|| (ftdi_device == FT_DEVICE_232H)
- #endif
-	;
-#elif BUILD_FT2232_LIBFTDI == 1
-	return (ftdi_device == TYPE_2232H || ftdi_device == TYPE_4232H
- #ifdef HAS_ENUM_FT232H
-		|| ftdi_device == TYPE_232H
- #endif
-	);
-#endif
-}
-
-/*
- * Commands that only apply to the highspeed FTx232H devices (FT2232H, FT4232H, FT232H).
- * See chapter 6 in http://www.ftdichip.com/Documents/AppNotes/
- * AN_108_Command_Processor_for_MPSSE_and_MCU_Host_Bus_Emulation_Modes.pdf
- */
-
-static int ftx232h_adaptive_clocking(bool enable)
-{
-	uint8_t buf = enable ? 0x96 : 0x97;
-	LOG_DEBUG("%2.2x", buf);
-
-	uint32_t bytes_written;
-	int retval;
-
-	retval = ft2232_write(&buf, sizeof(buf), &bytes_written);
-	if (retval != ERROR_OK) {
-		LOG_ERROR("couldn't write command to %s adaptive clocking"
-			, enable ? "enable" : "disable");
-		return retval;
-	}
-
-	return ERROR_OK;
-}
-
-/**
- * Enable/disable the clk divide by 5 of the 60MHz master clock.
- * This result in a JTAG clock speed range of 91.553Hz-6MHz
- * respective 457.763Hz-30MHz.
- */
-static int ftx232h_clk_divide_by_5(bool enable)
-{
-	uint32_t bytes_written;
-	uint8_t buf = enable ?  0x8b : 0x8a;
-
-	if (ft2232_write(&buf, sizeof(buf), &bytes_written) != ERROR_OK) {
-		LOG_ERROR("couldn't write command to %s clk divide by 5"
-			, enable ? "enable" : "disable");
-		return ERROR_JTAG_INIT_FAILED;
-	}
-	ft2232_max_tck = enable ? FTDI_2232C_MAX_TCK : FTDI_x232H_MAX_TCK;
-	LOG_INFO("max TCK change to: %u kHz", ft2232_max_tck);
-
-	return ERROR_OK;
-}
-
-static int ft2232_speed(int speed)
-{
-	uint8_t buf[3];
-	int retval;
-	uint32_t bytes_written;
-
-	retval = ERROR_OK;
-	bool enable_adaptive_clocking = (RTCK_SPEED == speed);
-	if (ft2232_device_is_highspeed())
-		retval = ftx232h_adaptive_clocking(enable_adaptive_clocking);
-	else if (enable_adaptive_clocking) {
-		LOG_ERROR("ft2232 device %lu does not support RTCK"
-			, (long unsigned int)ftdi_device);
-		return ERROR_FAIL;
-	}
-
-	if ((enable_adaptive_clocking) || (ERROR_OK != retval))
-		return retval;
-
-	buf[0] = 0x86;					/* command "set divisor" */
-	buf[1] = speed & 0xff;			/* valueL (0 = 6MHz, 1 = 3MHz, 2 = 2.0MHz, ...*/
-	buf[2] = (speed >> 8) & 0xff;	/* valueH */
-
-	LOG_DEBUG("%2.2x %2.2x %2.2x", buf[0], buf[1], buf[2]);
-	retval = ft2232_write(buf, sizeof(buf), &bytes_written);
-	if (retval != ERROR_OK) {
-		LOG_ERROR("couldn't set FT2232 TCK speed");
-		return retval;
-	}
-
-	return ERROR_OK;
-}
-
-static int ft2232_speed_div(int speed, int *khz)
-{
-	/* Take a look in the FT2232 manual,
-	 * AN2232C-01 Command Processor for
-	 * MPSSE and MCU Host Bus. Chapter 3.8 */
-
-	*khz = (RTCK_SPEED == speed) ? 0 : ft2232_max_tck / (1 + speed);
-
-	return ERROR_OK;
-}
-
-static int ft2232_khz(int khz, int *jtag_speed)
-{
-	if (khz == 0) {
-		if (ft2232_device_is_highspeed()) {
-			*jtag_speed = RTCK_SPEED;
-			return ERROR_OK;
-		} else {
-			LOG_DEBUG("RCLK not supported");
-			return ERROR_FAIL;
-		}
-	}
-
-	/* Take a look in the FT2232 manual,
-	 * AN2232C-01 Command Processor for
-	 * MPSSE and MCU Host Bus. Chapter 3.8
-	 *
-	 * We will calc here with a multiplier
-	 * of 10 for better rounding later. */
-
-	/* Calc speed, (ft2232_max_tck / khz) - 1
-	 * Use 65000 for better rounding */
-	*jtag_speed = ((ft2232_max_tck*10) / khz) - 10;
-
-	/* Add 0.9 for rounding */
-	*jtag_speed += 9;
-
-	/* Calc real speed */
-	*jtag_speed = *jtag_speed / 10;
-
-	/* Check if speed is greater than 0 */
-	if (*jtag_speed < 0)
-		*jtag_speed = 0;
-
-	/* Check max value */
-	if (*jtag_speed > 0xFFFF)
-		*jtag_speed = 0xFFFF;
-
-	return ERROR_OK;
-}
-
-static void ft2232_end_state(tap_state_t state)
-{
-	if (tap_is_state_stable(state))
-		tap_set_end_state(state);
-	else {
-		LOG_ERROR("BUG: %s is not a stable end state", tap_state_name(state));
-		exit(-1);
-	}
-}
-
-static void ft2232_read_scan(enum scan_type type, uint8_t *buffer, int scan_size)
-{
-	int num_bytes = (scan_size + 7) / 8;
-	int bits_left = scan_size;
-	int cur_byte  = 0;
-
-	while (num_bytes-- > 1) {
-		buffer[cur_byte++] = buffer_read();
-		bits_left -= 8;
-	}
-
-	buffer[cur_byte] = 0x0;
-
-	/* There is one more partial byte left from the clock data in/out instructions */
-	if (bits_left > 1)
-		buffer[cur_byte] = buffer_read() >> 1;
-	/* This shift depends on the length of the
-	 *clock data to tms instruction, insterted
-	 *at end of the scan, now fixed to a two
-	 *step transition in ft2232_add_scan */
-	buffer[cur_byte] = (buffer[cur_byte] | (((buffer_read()) << 1) & 0x80)) >> (8 - bits_left);
-}
-
-static void ft2232_debug_dump_buffer(void)
-{
-	int i;
-	char line[256];
-	char *line_p = line;
-
-	for (i = 0; i < ft2232_buffer_size; i++) {
-		line_p += snprintf(line_p,
-				sizeof(line) - (line_p - line),
-				"%2.2x ",
-				ft2232_buffer[i]);
-		if (i % 16 == 15) {
-			LOG_DEBUG("%s", line);
-			line_p = line;
-		}
-	}
-
-	if (line_p != line)
-		LOG_DEBUG("%s", line);
-}
-
-static int ft2232_send_and_recv(struct jtag_command *first, struct jtag_command *last)
-{
-	struct jtag_command *cmd;
-	uint8_t *buffer;
-	int scan_size;
-	enum scan_type type;
-	int retval;
-	uint32_t bytes_written = 0;
-	uint32_t bytes_read = 0;
-
-#ifdef _DEBUG_USB_IO_
-	struct timeval start, inter, inter2, end;
-	struct timeval d_inter, d_inter2, d_end;
-#endif
-
-#ifdef _DEBUG_USB_COMMS_
-	LOG_DEBUG("write buffer (size %i):", ft2232_buffer_size);
-	ft2232_debug_dump_buffer();
-#endif
-
-#ifdef _DEBUG_USB_IO_
-	gettimeofday(&start, NULL);
-#endif
-
-	retval = ft2232_write(ft2232_buffer, ft2232_buffer_size, &bytes_written);
-	if (retval != ERROR_OK) {
-		LOG_ERROR("couldn't write MPSSE commands to FT2232");
-		return retval;
-	}
-
-#ifdef _DEBUG_USB_IO_
-	gettimeofday(&inter, NULL);
-#endif
-
-	if (ft2232_expect_read) {
-		/* FIXME this "timeout" is never changed ... */
-		int timeout = LIBFTDI_READ_RETRY_COUNT;
-		ft2232_buffer_size = 0;
-
-#ifdef _DEBUG_USB_IO_
-		gettimeofday(&inter2, NULL);
-#endif
-
-		retval = ft2232_read(ft2232_buffer, ft2232_expect_read, &bytes_read);
-		if (retval != ERROR_OK) {
-			LOG_ERROR("couldn't read from FT2232");
-			return retval;
-		}
-
-#ifdef _DEBUG_USB_IO_
-		gettimeofday(&end, NULL);
-
-		timeval_subtract(&d_inter, &inter, &start);
-		timeval_subtract(&d_inter2, &inter2, &start);
-		timeval_subtract(&d_end, &end, &start);
-
-		LOG_INFO("inter: %u.%06u, inter2: %u.%06u end: %u.%06u",
-			(unsigned)d_inter.tv_sec, (unsigned)d_inter.tv_usec,
-			(unsigned)d_inter2.tv_sec, (unsigned)d_inter2.tv_usec,
-			(unsigned)d_end.tv_sec, (unsigned)d_end.tv_usec);
-#endif
-
-		ft2232_buffer_size = bytes_read;
-
-		if (ft2232_expect_read != ft2232_buffer_size) {
-			LOG_ERROR("ft2232_expect_read (%i) != "
-				"ft2232_buffer_size (%i) "
-				"(%i retries)",
-				ft2232_expect_read,
-				ft2232_buffer_size,
-				LIBFTDI_READ_RETRY_COUNT - timeout);
-			ft2232_debug_dump_buffer();
-
-			exit(-1);
-		}
-
-#ifdef _DEBUG_USB_COMMS_
-		LOG_DEBUG("read buffer (%i retries): %i bytes",
-			LIBFTDI_READ_RETRY_COUNT - timeout,
-			ft2232_buffer_size);
-		ft2232_debug_dump_buffer();
-#endif
-	}
-
-	ft2232_expect_read  = 0;
-	ft2232_read_pointer = 0;
-
-	/* return ERROR_OK, unless a jtag_read_buffer returns a failed check
-	 * that wasn't handled by a caller-provided error handler
-	 */
-	retval = ERROR_OK;
-
-	cmd = first;
-	while (cmd != last) {
-		switch (cmd->type) {
-			case JTAG_SCAN:
-				type = jtag_scan_type(cmd->cmd.scan);
-				if (type != SCAN_OUT) {
-					scan_size = jtag_scan_size(cmd->cmd.scan);
-					buffer = calloc(DIV_ROUND_UP(scan_size, 8), 1);
-					ft2232_read_scan(type, buffer, scan_size);
-					if (jtag_read_buffer(buffer, cmd->cmd.scan) != ERROR_OK)
-						retval = ERROR_JTAG_QUEUE_FAILED;
-					free(buffer);
-				}
-				break;
-
-			default:
-				break;
-		}
-
-		cmd = cmd->next;
-	}
-
-	ft2232_buffer_size = 0;
-
-	return retval;
-}
-
-/**
- * Function ft2232_add_pathmove
- * moves the TAP controller from the current state to a new state through the
- * given path, where path is an array of tap_state_t's.
- *
- * @param path is an array of tap_stat_t which gives the states to traverse through
- *   ending with the last state at path[num_states-1]
- * @param num_states is the count of state steps to move through
- */
-static void ft2232_add_pathmove(tap_state_t *path, int num_states)
-{
-	int state_count = 0;
-
-	assert((unsigned) num_states <= 32u);		/* tms_bits only holds 32 bits */
-
-	DEBUG_JTAG_IO("-");
-
-	/* this loop verifies that the path is legal and logs each state in the path */
-	while (num_states) {
-		unsigned char tms_byte = 0;		/* zero this on each MPSSE batch */
-		int bit_count = 0;
-		int num_states_batch = num_states > 7 ? 7 : num_states;
-
-		/* command "Clock Data to TMS/CS Pin (no Read)" */
-		buffer_write(0x4b);
-
-		/* number of states remaining */
-		buffer_write(num_states_batch - 1);
-
-		while (num_states_batch--) {
-			/* either TMS=0 or TMS=1 must work ... */
-			if (tap_state_transition(tap_get_state(), false) == path[state_count])
-				buf_set_u32(&tms_byte, bit_count++, 1, 0x0);
-			else if (tap_state_transition(tap_get_state(), true) == path[state_count])
-				buf_set_u32(&tms_byte, bit_count++, 1, 0x1);
-
-			/* ... or else the caller goofed BADLY */
-			else {
-				LOG_ERROR("BUG: %s -> %s isn't a valid "
-					"TAP state transition",
-					tap_state_name(tap_get_state()),
-					tap_state_name(path[state_count]));
-				exit(-1);
-			}
-
-			tap_set_state(path[state_count]);
-			state_count++;
-			num_states--;
-		}
-
-		buffer_write(tms_byte);
-	}
-	tap_set_end_state(tap_get_state());
-}
-
-static void ft2232_add_scan(bool ir_scan, enum scan_type type, uint8_t *buffer, int scan_size)
-{
-	int num_bytes = (scan_size + 7) / 8;
-	int bits_left = scan_size;
-	int cur_byte  = 0;
-	int last_bit;
-
-	if (!ir_scan) {
-		if (tap_get_state() != TAP_DRSHIFT)
-			move_to_state(TAP_DRSHIFT);
-	} else {
-		if (tap_get_state() != TAP_IRSHIFT)
-			move_to_state(TAP_IRSHIFT);
-	}
-
-	/* add command for complete bytes */
-	while (num_bytes > 1) {
-		int thisrun_bytes;
-		if (type == SCAN_IO) {
-			/* Clock Data Bytes In and Out LSB First */
-			buffer_write(0x39);
-			/* LOG_DEBUG("added TDI bytes (io %i)", num_bytes); */
-		} else if (type == SCAN_OUT) {
-			/* Clock Data Bytes Out on -ve Clock Edge LSB First (no Read) */
-			buffer_write(0x19);
-			/* LOG_DEBUG("added TDI bytes (o)"); */
-		} else if (type == SCAN_IN) {
-			/* Clock Data Bytes In on +ve Clock Edge LSB First (no Write) */
-			buffer_write(0x28);
-			/* LOG_DEBUG("added TDI bytes (i %i)", num_bytes); */
-		}
-
-		thisrun_bytes = (num_bytes > 65537) ? 65536 : (num_bytes - 1);
-		num_bytes -= thisrun_bytes;
-
-		buffer_write((uint8_t) (thisrun_bytes - 1));
-		buffer_write((uint8_t) ((thisrun_bytes - 1) >> 8));
-
-		if (type != SCAN_IN) {
-			/* add complete bytes */
-			while (thisrun_bytes-- > 0) {
-				buffer_write(buffer[cur_byte++]);
-				bits_left -= 8;
-			}
-		} else /* (type == SCAN_IN) */
-			bits_left -= 8 * (thisrun_bytes);
-	}
-
-	/* the most signifcant bit is scanned during TAP movement */
-	if (type != SCAN_IN)
-		last_bit = (buffer[cur_byte] >> (bits_left - 1)) & 0x1;
-	else
-		last_bit = 0;
-
-	/* process remaining bits but the last one */
-	if (bits_left > 1) {
-		if (type == SCAN_IO) {
-			/* Clock Data Bits In and Out LSB First */
-			buffer_write(0x3b);
-			/* LOG_DEBUG("added TDI bits (io) %i", bits_left - 1); */
-		} else if (type == SCAN_OUT) {
-			/* Clock Data Bits Out on -ve Clock Edge LSB First (no Read) */
-			buffer_write(0x1b);
-			/* LOG_DEBUG("added TDI bits (o)"); */
-		} else if (type == SCAN_IN) {
-			/* Clock Data Bits In on +ve Clock Edge LSB First (no Write) */
-			buffer_write(0x2a);
-			/* LOG_DEBUG("added TDI bits (i %i)", bits_left - 1); */
-		}
-
-		buffer_write(bits_left - 2);
-		if (type != SCAN_IN)
-			buffer_write(buffer[cur_byte]);
-	}
-
-	if ((ir_scan && (tap_get_end_state() == TAP_IRSHIFT))
-			|| (!ir_scan && (tap_get_end_state() == TAP_DRSHIFT))) {
-		if (type == SCAN_IO) {
-			/* Clock Data Bits In and Out LSB First */
-			buffer_write(0x3b);
-			/* LOG_DEBUG("added TDI bits (io) %i", bits_left - 1); */
-		} else if (type == SCAN_OUT) {
-			/* Clock Data Bits Out on -ve Clock Edge LSB First (no Read) */
-			buffer_write(0x1b);
-			/* LOG_DEBUG("added TDI bits (o)"); */
-		} else if (type == SCAN_IN) {
-			/* Clock Data Bits In on +ve Clock Edge LSB First (no Write) */
-			buffer_write(0x2a);
-			/* LOG_DEBUG("added TDI bits (i %i)", bits_left - 1); */
-		}
-		buffer_write(0x0);
-		if (type != SCAN_IN)
-			buffer_write(last_bit);
-	} else {
-		int tms_bits;
-		int tms_count;
-		uint8_t mpsse_cmd;
-
-		/* move from Shift-IR/DR to end state */
-		if (type != SCAN_OUT) {
-			/* We always go to the PAUSE state in two step at the end of an IN or IO
-			 *scan
-			 * This must be coordinated with the bit shifts in ft2232_read_scan    */
-			tms_bits  = 0x01;
-			tms_count = 2;
-			/* Clock Data to TMS/CS Pin with Read */
-			mpsse_cmd = 0x6b;
-		} else {
-			tms_bits  = tap_get_tms_path(tap_get_state(), tap_get_end_state());
-			tms_count = tap_get_tms_path_len(tap_get_state(), tap_get_end_state());
-			/* Clock Data to TMS/CS Pin (no Read) */
-			mpsse_cmd = 0x4b;
-		}
-
-		DEBUG_JTAG_IO("finish %s", (type == SCAN_OUT) ? "without read" : "via PAUSE");
-		clock_tms(mpsse_cmd, tms_bits, tms_count, last_bit);
-	}
-
-	if (tap_get_state() != tap_get_end_state())
-		move_to_state(tap_get_end_state());
-}
-
-static int ft2232_large_scan(struct scan_command *cmd,
-	enum scan_type type,
-	uint8_t *buffer,
-	int scan_size)
-{
-	int num_bytes = (scan_size + 7) / 8;
-	int bits_left = scan_size;
-	int cur_byte  = 0;
-	int last_bit;
-	uint8_t *receive_buffer  = malloc(DIV_ROUND_UP(scan_size, 8));
-	uint8_t *receive_pointer = receive_buffer;
-	uint32_t bytes_written;
-	uint32_t bytes_read;
-	int retval;
-	int thisrun_read = 0;
-
-	if (!receive_buffer) {
-		LOG_ERROR("failed to allocate memory");
-		exit(-1);
-	}
-
-	if (cmd->ir_scan) {
-		LOG_ERROR("BUG: large IR scans are not supported");
-		exit(-1);
-	}
-
-	if (tap_get_state() != TAP_DRSHIFT)
-		move_to_state(TAP_DRSHIFT);
-
-	retval = ft2232_write(ft2232_buffer, ft2232_buffer_size, &bytes_written);
-	if (retval != ERROR_OK) {
-		LOG_ERROR("couldn't write MPSSE commands to FT2232");
-		exit(-1);
-	}
-	LOG_DEBUG("ft2232_buffer_size: %i, bytes_written: %i",
-		ft2232_buffer_size, (int)bytes_written);
-	ft2232_buffer_size = 0;
-
-	/* add command for complete bytes */
-	while (num_bytes > 1) {
-		int thisrun_bytes;
-
-		if (type == SCAN_IO) {
-			/* Clock Data Bytes In and Out LSB First */
-			buffer_write(0x39);
-			/* LOG_DEBUG("added TDI bytes (io %i)", num_bytes); */
-		} else if (type == SCAN_OUT) {
-			/* Clock Data Bytes Out on -ve Clock Edge LSB First (no Read) */
-			buffer_write(0x19);
-			/* LOG_DEBUG("added TDI bytes (o)"); */
-		} else if (type == SCAN_IN) {
-			/* Clock Data Bytes In on +ve Clock Edge LSB First (no Write) */
-			buffer_write(0x28);
-			/* LOG_DEBUG("added TDI bytes (i %i)", num_bytes); */
-		}
-
-		thisrun_bytes = (num_bytes > 65537) ? 65536 : (num_bytes - 1);
-		thisrun_read  = thisrun_bytes;
-		num_bytes    -= thisrun_bytes;
-		buffer_write((uint8_t) (thisrun_bytes - 1));
-		buffer_write((uint8_t) ((thisrun_bytes - 1) >> 8));
-
-		if (type != SCAN_IN) {
-			/* add complete bytes */
-			while (thisrun_bytes-- > 0) {
-				buffer_write(buffer[cur_byte]);
-				cur_byte++;
-				bits_left -= 8;
-			}
-		} else /* (type == SCAN_IN) */
-			bits_left -= 8 * (thisrun_bytes);
-
-		retval = ft2232_write(ft2232_buffer, ft2232_buffer_size, &bytes_written);
-		if (retval != ERROR_OK) {
-			LOG_ERROR("couldn't write MPSSE commands to FT2232");
-			exit(-1);
-		}
-		LOG_DEBUG("ft2232_buffer_size: %i, bytes_written: %i",
-			ft2232_buffer_size,
-			(int)bytes_written);
-		ft2232_buffer_size = 0;
-
-		if (type != SCAN_OUT) {
-			retval = ft2232_read(receive_pointer, thisrun_read, &bytes_read);
-			if (retval != ERROR_OK) {
-				LOG_ERROR("couldn't read from FT2232");
-				exit(-1);
-			}
-			LOG_DEBUG("thisrun_read: %i, bytes_read: %i",
-				thisrun_read,
-				(int)bytes_read);
-			receive_pointer += bytes_read;
-		}
-	}
-
-	thisrun_read = 0;
-
-	/* the most signifcant bit is scanned during TAP movement */
-	if (type != SCAN_IN)
-		last_bit = (buffer[cur_byte] >> (bits_left - 1)) & 0x1;
-	else
-		last_bit = 0;
-
-	/* process remaining bits but the last one */
-	if (bits_left > 1) {
-		if (type == SCAN_IO) {
-			/* Clock Data Bits In and Out LSB First */
-			buffer_write(0x3b);
-			/* LOG_DEBUG("added TDI bits (io) %i", bits_left - 1); */
-		} else if (type == SCAN_OUT) {
-			/* Clock Data Bits Out on -ve Clock Edge LSB First (no Read) */
-			buffer_write(0x1b);
-			/* LOG_DEBUG("added TDI bits (o)"); */
-		} else if (type == SCAN_IN) {
-			/* Clock Data Bits In on +ve Clock Edge LSB First (no Write) */
-			buffer_write(0x2a);
-			/* LOG_DEBUG("added TDI bits (i %i)", bits_left - 1); */
-		}
-		buffer_write(bits_left - 2);
-		if (type != SCAN_IN)
-			buffer_write(buffer[cur_byte]);
-
-		if (type != SCAN_OUT)
-			thisrun_read += 2;
-	}
-
-	if (tap_get_end_state() == TAP_DRSHIFT) {
-		if (type == SCAN_IO) {
-			/* Clock Data Bits In and Out LSB First */
-			buffer_write(0x3b);
-			/* LOG_DEBUG("added TDI bits (io) %i", bits_left - 1); */
-		} else if (type == SCAN_OUT) {
-			/* Clock Data Bits Out on -ve Clock Edge LSB First (no Read) */
-			buffer_write(0x1b);
-			/* LOG_DEBUG("added TDI bits (o)"); */
-		} else if (type == SCAN_IN) {
-			/* Clock Data Bits In on +ve Clock Edge LSB First (no Write) */
-			buffer_write(0x2a);
-			/* LOG_DEBUG("added TDI bits (i %i)", bits_left - 1); */
-		}
-		buffer_write(0x0);
-		buffer_write(last_bit);
-	} else {
-		int tms_bits  = tap_get_tms_path(tap_get_state(), tap_get_end_state());
-		int tms_count = tap_get_tms_path_len(tap_get_state(), tap_get_end_state());
-		uint8_t mpsse_cmd;
-
-		/* move from Shift-IR/DR to end state */
-		if (type != SCAN_OUT) {
-			/* Clock Data to TMS/CS Pin with Read */
-			mpsse_cmd = 0x6b;
-			/* LOG_DEBUG("added TMS scan (read)"); */
-		} else {
-			/* Clock Data to TMS/CS Pin (no Read) */
-			mpsse_cmd = 0x4b;
-			/* LOG_DEBUG("added TMS scan (no read)"); */
-		}
-
-		DEBUG_JTAG_IO("finish, %s", (type == SCAN_OUT) ? "no read" : "read");
-		clock_tms(mpsse_cmd, tms_bits, tms_count, last_bit);
-	}
-
-	if (type != SCAN_OUT)
-		thisrun_read += 1;
-
-	retval = ft2232_write(ft2232_buffer, ft2232_buffer_size, &bytes_written);
-	if (retval != ERROR_OK) {
-		LOG_ERROR("couldn't write MPSSE commands to FT2232");
-		exit(-1);
-	}
-	LOG_DEBUG("ft2232_buffer_size: %i, bytes_written: %i",
-		ft2232_buffer_size,
-		(int)bytes_written);
-	ft2232_buffer_size = 0;
-
-	if (type != SCAN_OUT) {
-		retval = ft2232_read(receive_pointer, thisrun_read, &bytes_read);
-		if (retval != ERROR_OK) {
-			LOG_ERROR("couldn't read from FT2232");
-			exit(-1);
-		}
-		LOG_DEBUG("thisrun_read: %i, bytes_read: %i",
-			thisrun_read,
-			(int)bytes_read);
-	}
-
-	free(receive_buffer);
-
-	return ERROR_OK;
-}
-
-static int ft2232_predict_scan_out(int scan_size, enum scan_type type)
-{
-	int predicted_size = 3;
-	int num_bytes = (scan_size - 1) / 8;
-
-	if (tap_get_state() != TAP_DRSHIFT)
-		predicted_size += get_tms_buffer_requirements(
-				tap_get_tms_path_len(tap_get_state(), TAP_DRSHIFT));
-
-	if (type == SCAN_IN) {	/* only from device to host */
-		/* complete bytes */
-		predicted_size += DIV_ROUND_UP(num_bytes, 65536) * 3;
-
-		/* remaining bits - 1 (up to 7) */
-		predicted_size += ((scan_size - 1) % 8) ? 2 : 0;
-	} else {/* host to device, or bidirectional
-		 * complete bytes */
-		predicted_size += num_bytes + DIV_ROUND_UP(num_bytes, 65536) * 3;
-
-		/* remaining bits -1 (up to 7) */
-		predicted_size += ((scan_size - 1) % 8) ? 3 : 0;
-	}
-
-	return predicted_size;
-}
-
-static int ft2232_predict_scan_in(int scan_size, enum scan_type type)
-{
-	int predicted_size = 0;
-
-	if (type != SCAN_OUT) {
-		/* complete bytes */
-		predicted_size +=
-			(DIV_ROUND_UP(scan_size, 8) > 1) ? (DIV_ROUND_UP(scan_size, 8) - 1) : 0;
-
-		/* remaining bits - 1 */
-		predicted_size += ((scan_size - 1) % 8) ? 1 : 0;
-
-		/* last bit (from TMS scan) */
-		predicted_size += 1;
-	}
-
-	/* LOG_DEBUG("scan_size: %i, predicted_size: %i", scan_size, predicted_size); */
-
-	return predicted_size;
-}
-
-/* semi-generic FT2232/FT4232 reset code */
-static void ftx23_reset(int trst, int srst)
-{
-	enum reset_types jtag_reset_config = jtag_get_reset_config();
-	if (trst == 1) {
-		if (jtag_reset_config & RESET_TRST_OPEN_DRAIN)
-			low_direction |= nTRSTnOE;	/* switch to output pin (output is low) */
-		else
-			low_output &= ~nTRST;		/* switch output low */
-	} else if (trst == 0) {
-		if (jtag_reset_config & RESET_TRST_OPEN_DRAIN)
-			low_direction &= ~nTRSTnOE;	/* switch to input pin (high-Z + internal
-							 *and external pullup) */
-		else
-			low_output |= nTRST;		/* switch output high */
-	}
-
-	if (srst == 1) {
-		if (jtag_reset_config & RESET_SRST_PUSH_PULL)
-			low_output &= ~nSRST;		/* switch output low */
-		else
-			low_direction |= nSRSTnOE;	/* switch to output pin (output is low) */
-	} else if (srst == 0) {
-		if (jtag_reset_config & RESET_SRST_PUSH_PULL)
-			low_output |= nSRST;		/* switch output high */
-		else
-			low_direction &= ~nSRSTnOE;	/* switch to input pin (high-Z) */
-	}
-
-	/* command "set data bits low byte" */
-	buffer_write(0x80);
-	buffer_write(low_output);
-	buffer_write(low_direction);
-}
-
-static void jtagkey_reset(int trst, int srst)
-{
-	enum reset_types jtag_reset_config = jtag_get_reset_config();
-	if (trst == 1) {
-		if (jtag_reset_config & RESET_TRST_OPEN_DRAIN)
-			high_output &= ~nTRSTnOE;
-		else
-			high_output &= ~nTRST;
-	} else if (trst == 0) {
-		if (jtag_reset_config & RESET_TRST_OPEN_DRAIN)
-			high_output |= nTRSTnOE;
-		else
-			high_output |= nTRST;
-	}
-
-	if (srst == 1) {
-		if (jtag_reset_config & RESET_SRST_PUSH_PULL)
-			high_output &= ~nSRST;
-		else
-			high_output &= ~nSRSTnOE;
-	} else if (srst == 0) {
-		if (jtag_reset_config & RESET_SRST_PUSH_PULL)
-			high_output |= nSRST;
-		else
-			high_output |= nSRSTnOE;
-	}
-
-	/* command "set data bits high byte" */
-	buffer_write(0x82);
-	buffer_write(high_output);
-	buffer_write(high_direction);
-	LOG_DEBUG("trst: %i, srst: %i, high_output: 0x%2.2x, high_direction: 0x%2.2x",
-		trst,
-		srst,
-		high_output,
-		high_direction);
-}
-
-static void olimex_jtag_reset(int trst, int srst)
-{
-	enum reset_types jtag_reset_config = jtag_get_reset_config();
-	if (trst == 1) {
-		if (jtag_reset_config & RESET_TRST_OPEN_DRAIN)
-			high_output &= ~nTRSTnOE;
-		else
-			high_output &= ~nTRST;
-	} else if (trst == 0) {
-		if (jtag_reset_config & RESET_TRST_OPEN_DRAIN)
-			high_output |= nTRSTnOE;
-		else
-			high_output |= nTRST;
-	}
-
-	if (srst == 1)
-		high_output |= nSRST;
-	else if (srst == 0)
-		high_output &= ~nSRST;
-
-	/* command "set data bits high byte" */
-	buffer_write(0x82);
-	buffer_write(high_output);
-	buffer_write(high_direction);
-	LOG_DEBUG("trst: %i, srst: %i, high_output: 0x%2.2x, high_direction: 0x%2.2x",
-		trst,
-		srst,
-		high_output,
-		high_direction);
-}
-
-static void axm0432_jtag_reset(int trst, int srst)
-{
-	if (trst == 1) {
-		tap_set_state(TAP_RESET);
-		high_output &= ~nTRST;
-	} else if (trst == 0)
-		high_output |= nTRST;
-
-	if (srst == 1)
-		high_output &= ~nSRST;
-	else if (srst == 0)
-		high_output |= nSRST;
-
-	/* command "set data bits low byte" */
-	buffer_write(0x82);
-	buffer_write(high_output);
-	buffer_write(high_direction);
-	LOG_DEBUG("trst: %i, srst: %i, high_output: 0x%2.2x, high_direction: 0x%2.2x",
-		trst,
-		srst,
-		high_output,
-		high_direction);
-}
-
-static void flyswatter_reset(int trst, int srst)
-{
-	if (trst == 1)
-		low_output &= ~nTRST;
-	else if (trst == 0)
-		low_output |= nTRST;
-
-	if (srst == 1)
-		low_output |= nSRST;
-	else if (srst == 0)
-		low_output &= ~nSRST;
-
-	/* command "set data bits low byte" */
-	buffer_write(0x80);
-	buffer_write(low_output);
-	buffer_write(low_direction);
-	LOG_DEBUG("trst: %i, srst: %i, low_output: 0x%2.2x, low_direction: 0x%2.2x",
-		trst,
-		srst,
-		low_output,
-		low_direction);
-}
-
-static void flyswatter1_reset(int trst, int srst)
-{
-	flyswatter_reset(trst, srst);
-}
-
-static void flyswatter2_reset(int trst, int srst)
-{
-	flyswatter_reset(trst, !srst);
-}
-
-static void minimodule_reset(int trst, int srst)
-{
-	if (srst == 1)
-		low_output &= ~nSRST;
-	else if (srst == 0)
-		low_output |= nSRST;
-
-	/* command "set data bits low byte" */
-	buffer_write(0x80);
-	buffer_write(low_output);
-	buffer_write(low_direction);
-	LOG_DEBUG("trst: %i, srst: %i, low_output: 0x%2.2x, low_direction: 0x%2.2x",
-		trst,
-		srst,
-		low_output,
-		low_direction);
-}
-
-static void turtle_reset(int trst, int srst)
-{
-	if (trst == 1)
-		LOG_ERROR("Can't assert TRST: the adapter lacks this signal");
-
-	if (srst == 1)
-		low_output |= nSRST;
-	else if (srst == 0)
-		low_output &= ~nSRST;
-
-	/* command "set data bits low byte" */
-	buffer_write(0x80);
-	buffer_write(low_output);
-	buffer_write(low_direction);
-	LOG_DEBUG("srst: %i, low_output: 0x%2.2x, low_direction: 0x%2.2x",
-		srst,
-		low_output,
-		low_direction);
-}
-
-static void comstick_reset(int trst, int srst)
-{
-	if (trst == 1)
-		high_output &= ~nTRST;
-	else if (trst == 0)
-		high_output |= nTRST;
-
-	if (srst == 1)
-		high_output &= ~nSRST;
-	else if (srst == 0)
-		high_output |= nSRST;
-
-	/* command "set data bits high byte" */
-	buffer_write(0x82);
-	buffer_write(high_output);
-	buffer_write(high_direction);
-	LOG_DEBUG("trst: %i, srst: %i, high_output: 0x%2.2x, high_direction: 0x%2.2x",
-		trst,
-		srst,
-		high_output,
-		high_direction);
-}
-
-static void stm32stick_reset(int trst, int srst)
-{
-	if (trst == 1)
-		high_output &= ~nTRST;
-	else if (trst == 0)
-		high_output |= nTRST;
-
-	if (srst == 1)
-		low_output &= ~nSRST;
-	else if (srst == 0)
-		low_output |= nSRST;
-
-	/* command "set data bits low byte" */
-	buffer_write(0x80);
-	buffer_write(low_output);
-	buffer_write(low_direction);
-
-	/* command "set data bits high byte" */
-	buffer_write(0x82);
-	buffer_write(high_output);
-	buffer_write(high_direction);
-	LOG_DEBUG("trst: %i, srst: %i, high_output: 0x%2.2x, high_direction: 0x%2.2x",
-		trst,
-		srst,
-		high_output,
-		high_direction);
-}
-
-static void sheevaplug_reset(int trst, int srst)
-{
-	if (trst == 1)
-		high_output &= ~nTRST;
-	else if (trst == 0)
-		high_output |= nTRST;
-
-	if (srst == 1)
-		high_output &= ~nSRSTnOE;
-	else if (srst == 0)
-		high_output |= nSRSTnOE;
-
-	/* command "set data bits high byte" */
-	buffer_write(0x82);
-	buffer_write(high_output);
-	buffer_write(high_direction);
-	LOG_DEBUG("trst: %i, srst: %i, high_output: 0x%2.2x, high_direction: 0x%2.2x",
-		trst,
-		srst,
-		high_output,
-		high_direction);
-}
-
-static void redbee_reset(int trst, int srst)
-{
-	if (trst == 1) {
-		tap_set_state(TAP_RESET);
-		high_output &= ~nTRST;
-	} else if (trst == 0)
-		high_output |= nTRST;
-
-	if (srst == 1)
-		high_output &= ~nSRST;
-	else if (srst == 0)
-		high_output |= nSRST;
-
-	/* command "set data bits low byte" */
-	buffer_write(0x82);
-	buffer_write(high_output);
-	buffer_write(high_direction);
-	LOG_DEBUG("trst: %i, srst: %i, high_output: 0x%2.2x, "
-		"high_direction: 0x%2.2x", trst, srst, high_output,
-		high_direction);
-}
-
-static void xds100v2_reset(int trst, int srst)
-{
-	if (trst == 1) {
-		tap_set_state(TAP_RESET);
-		high_output &= ~nTRST;
-	} else if (trst == 0)
-		high_output |= nTRST;
-
-	if (srst == 1)
-		high_output |= nSRST;
-	else if (srst == 0)
-		high_output &= ~nSRST;
-
-	/* command "set data bits low byte" */
-	buffer_write(0x82);
-	buffer_write(high_output);
-	buffer_write(high_direction);
-	LOG_DEBUG("trst: %i, srst: %i, high_output: 0x%2.2x, "
-		"high_direction: 0x%2.2x", trst, srst, high_output,
-		high_direction);
-}
-
-static int ft2232_execute_runtest(struct jtag_command *cmd)
-{
-	int retval;
-	int i;
-	int predicted_size = 0;
-	retval = ERROR_OK;
-
-	DEBUG_JTAG_IO("runtest %i cycles, end in %s",
-		cmd->cmd.runtest->num_cycles,
-		tap_state_name(cmd->cmd.runtest->end_state));
-
-	/* only send the maximum buffer size that FT2232C can handle */
-	predicted_size = 0;
-	if (tap_get_state() != TAP_IDLE)
-		predicted_size += 3;
-	predicted_size += 3 * DIV_ROUND_UP(cmd->cmd.runtest->num_cycles, 7);
-	if (cmd->cmd.runtest->end_state != TAP_IDLE)
-		predicted_size += 3;
-	if (tap_get_end_state() != TAP_IDLE)
-		predicted_size += 3;
-	if (ft2232_buffer_size + predicted_size + 1 > FT2232_BUFFER_SIZE) {
-		if (ft2232_send_and_recv(first_unsent, cmd) != ERROR_OK)
-			retval = ERROR_JTAG_QUEUE_FAILED;
-		require_send = 0;
-		first_unsent = cmd;
-	}
-	if (tap_get_state() != TAP_IDLE) {
-		move_to_state(TAP_IDLE);
-		require_send = 1;
-	}
-	i = cmd->cmd.runtest->num_cycles;
-	while (i > 0) {
-		/* there are no state transitions in this code, so omit state tracking */
-
-		/* command "Clock Data to TMS/CS Pin (no Read)" */
-		buffer_write(0x4b);
-
-		/* scan 7 bits */
-		buffer_write((i > 7) ? 6 : (i - 1));
-
-		/* TMS data bits */
-		buffer_write(0x0);
-
-		i -= (i > 7) ? 7 : i;
-		/* LOG_DEBUG("added TMS scan (no read)"); */
-	}
-
-	ft2232_end_state(cmd->cmd.runtest->end_state);
-
-	if (tap_get_state() != tap_get_end_state())
-		move_to_state(tap_get_end_state());
-
-	require_send = 1;
-	DEBUG_JTAG_IO("runtest: %i, end in %s",
-		cmd->cmd.runtest->num_cycles,
-		tap_state_name(tap_get_end_state()));
-	return retval;
-}
-
-static int ft2232_execute_statemove(struct jtag_command *cmd)
-{
-	int predicted_size = 0;
-	int retval = ERROR_OK;
-
-	DEBUG_JTAG_IO("statemove end in %s",
-		tap_state_name(cmd->cmd.statemove->end_state));
-
-	/* only send the maximum buffer size that FT2232C can handle */
-	predicted_size = 3;
-	if (ft2232_buffer_size + predicted_size + 1 > FT2232_BUFFER_SIZE) {
-		if (ft2232_send_and_recv(first_unsent, cmd) != ERROR_OK)
-			retval = ERROR_JTAG_QUEUE_FAILED;
-		require_send = 0;
-		first_unsent = cmd;
-	}
-	ft2232_end_state(cmd->cmd.statemove->end_state);
-
-	/* For TAP_RESET, ignore the current recorded state.  It's often
-	 * wrong at server startup, and this transation is critical whenever
-	 * it's requested.
-	 */
-	if (tap_get_end_state() == TAP_RESET) {
-		clock_tms(0x4b,  0xff, 5, 0);
-		require_send = 1;
-
-		/* shortest-path move to desired end state */
-	} else if (tap_get_state() != tap_get_end_state()) {
-		move_to_state(tap_get_end_state());
-		require_send = 1;
-	}
-
-	return retval;
-}
-
-/**
- * Clock a bunch of TMS (or SWDIO) transitions, to change the JTAG
- * (or SWD) state machine.
- */
-static int ft2232_execute_tms(struct jtag_command *cmd)
-{
-	int retval = ERROR_OK;
-	unsigned num_bits = cmd->cmd.tms->num_bits;
-	const uint8_t *bits = cmd->cmd.tms->bits;
-	unsigned count;
-
-	DEBUG_JTAG_IO("TMS: %d bits", num_bits);
-
-	/* only send the maximum buffer size that FT2232C can handle */
-	count = 3 * DIV_ROUND_UP(num_bits, 4);
-	if (ft2232_buffer_size + 3*count + 1 > FT2232_BUFFER_SIZE) {
-		if (ft2232_send_and_recv(first_unsent, cmd) != ERROR_OK)
-			retval = ERROR_JTAG_QUEUE_FAILED;
-
-		require_send = 0;
-		first_unsent = cmd;
-	}
-
-	/* Shift out in batches of at most 6 bits; there's a report of an
-	 * FT2232 bug in this area, where shifting exactly 7 bits can make
-	 * problems with TMS signaling for the last clock cycle:
-	 *
-	 *    http://developer.intra2net.com/mailarchive/html/
-	 *		libftdi/2009/msg00292.html
-	 *
-	 * Command 0x4b is: "Clock Data to TMS/CS Pin (no Read)"
-	 *
-	 * Note that pathmoves in JTAG are not often seven bits, so that
-	 * isn't a particularly likely situation outside of "special"
-	 * signaling such as switching between JTAG and SWD modes.
-	 */
-	while (num_bits) {
-		if (num_bits <= 6) {
-			buffer_write(0x4b);
-			buffer_write(num_bits - 1);
-			buffer_write(*bits & 0x3f);
-			break;
-		}
-
-		/* Yes, this is lazy ... we COULD shift out more data
-		 * bits per operation, but doing it in nybbles is easy
-		 */
-		buffer_write(0x4b);
-		buffer_write(3);
-		buffer_write(*bits & 0xf);
-		num_bits -= 4;
-
-		count  = (num_bits > 4) ? 4 : num_bits;
-
-		buffer_write(0x4b);
-		buffer_write(count - 1);
-		buffer_write((*bits >> 4) & 0xf);
-		num_bits -= count;
-
-		bits++;
-	}
-
-	require_send = 1;
-	return retval;
-}
-
-static int ft2232_execute_pathmove(struct jtag_command *cmd)
-{
-	int predicted_size = 0;
-	int retval = ERROR_OK;
-
-	tap_state_t *path = cmd->cmd.pathmove->path;
-	int num_states    = cmd->cmd.pathmove->num_states;
-
-	DEBUG_JTAG_IO("pathmove: %i states, current: %s  end: %s", num_states,
-		tap_state_name(tap_get_state()),
-		tap_state_name(path[num_states-1]));
-
-	/* only send the maximum buffer size that FT2232C can handle */
-	predicted_size = 3 * DIV_ROUND_UP(num_states, 7);
-	if (ft2232_buffer_size + predicted_size + 1 > FT2232_BUFFER_SIZE) {
-		if (ft2232_send_and_recv(first_unsent, cmd) != ERROR_OK)
-			retval = ERROR_JTAG_QUEUE_FAILED;
-
-		require_send = 0;
-		first_unsent = cmd;
-	}
-
-	ft2232_add_pathmove(path, num_states);
-	require_send = 1;
-
-	return retval;
-}
-
-static int ft2232_execute_scan(struct jtag_command *cmd)
-{
-	uint8_t *buffer;
-	int scan_size;				/* size of IR or DR scan */
-	int predicted_size = 0;
-	int retval = ERROR_OK;
-
-	enum scan_type type = jtag_scan_type(cmd->cmd.scan);
-
-	DEBUG_JTAG_IO("%s type:%d", cmd->cmd.scan->ir_scan ? "IRSCAN" : "DRSCAN", type);
-
-	scan_size = jtag_build_buffer(cmd->cmd.scan, &buffer);
-
-	predicted_size = ft2232_predict_scan_out(scan_size, type);
-	if ((predicted_size + 1) > FT2232_BUFFER_SIZE) {
-		LOG_DEBUG("oversized ft2232 scan (predicted_size > FT2232_BUFFER_SIZE)");
-		/* unsent commands before this */
-		if (first_unsent != cmd)
-			if (ft2232_send_and_recv(first_unsent, cmd) != ERROR_OK)
-				retval = ERROR_JTAG_QUEUE_FAILED;
-
-		/* current command */
-		ft2232_end_state(cmd->cmd.scan->end_state);
-		ft2232_large_scan(cmd->cmd.scan, type, buffer, scan_size);
-		require_send = 0;
-		first_unsent = cmd->next;
-		if (buffer)
-			free(buffer);
-		return retval;
-	} else if (ft2232_buffer_size + predicted_size + 1 > FT2232_BUFFER_SIZE) {
-		LOG_DEBUG(
-			"ft2232 buffer size reached, sending queued commands (first_unsent: %p, cmd: %p)",
-			first_unsent,
-			cmd);
-		if (ft2232_send_and_recv(first_unsent, cmd) != ERROR_OK)
-			retval = ERROR_JTAG_QUEUE_FAILED;
-		require_send = 0;
-		first_unsent = cmd;
-	}
-	ft2232_expect_read += ft2232_predict_scan_in(scan_size, type);
-	/* LOG_DEBUG("new read size: %i", ft2232_expect_read); */
-	ft2232_end_state(cmd->cmd.scan->end_state);
-	ft2232_add_scan(cmd->cmd.scan->ir_scan, type, buffer, scan_size);
-	require_send = 1;
-	if (buffer)
-		free(buffer);
-	DEBUG_JTAG_IO("%s scan, %i bits, end in %s",
-		(cmd->cmd.scan->ir_scan) ? "IR" : "DR", scan_size,
-		tap_state_name(tap_get_end_state()));
-	return retval;
-
-}
-
-static int ft2232_execute_reset(struct jtag_command *cmd)
-{
-	int retval;
-	int predicted_size = 0;
-	retval = ERROR_OK;
-
-	DEBUG_JTAG_IO("reset trst: %i srst %i",
-		cmd->cmd.reset->trst, cmd->cmd.reset->srst);
-
-	/* only send the maximum buffer size that FT2232C can handle */
-	predicted_size = 3;
-	if (ft2232_buffer_size + predicted_size + 1 > FT2232_BUFFER_SIZE) {
-		if (ft2232_send_and_recv(first_unsent, cmd) != ERROR_OK)
-			retval = ERROR_JTAG_QUEUE_FAILED;
-		require_send = 0;
-		first_unsent = cmd;
-	}
-
-	if ((cmd->cmd.reset->trst == 1) ||
-	    (cmd->cmd.reset->srst && (jtag_get_reset_config() & RESET_SRST_PULLS_TRST)))
-		tap_set_state(TAP_RESET);
-
-	layout->reset(cmd->cmd.reset->trst, cmd->cmd.reset->srst);
-	require_send = 1;
-
-	DEBUG_JTAG_IO("trst: %i, srst: %i",
-		cmd->cmd.reset->trst, cmd->cmd.reset->srst);
-	return retval;
-}
-
-static int ft2232_execute_sleep(struct jtag_command *cmd)
-{
-	int retval;
-	retval = ERROR_OK;
-
-	DEBUG_JTAG_IO("sleep %" PRIi32, cmd->cmd.sleep->us);
-
-	if (ft2232_send_and_recv(first_unsent, cmd) != ERROR_OK)
-		retval = ERROR_JTAG_QUEUE_FAILED;
-	first_unsent = cmd->next;
-	jtag_sleep(cmd->cmd.sleep->us);
-	DEBUG_JTAG_IO("sleep %" PRIi32 " usec while in %s",
-		cmd->cmd.sleep->us,
-		tap_state_name(tap_get_state()));
-	return retval;
-}
-
-static int ft2232_execute_stableclocks(struct jtag_command *cmd)
-{
-	int retval;
-	retval = ERROR_OK;
-
-	/* this is only allowed while in a stable state.  A check for a stable
-	 * state was done in jtag_add_clocks()
-	 */
-	if (ft2232_stableclocks(cmd->cmd.stableclocks->num_cycles, cmd) != ERROR_OK)
-		retval = ERROR_JTAG_QUEUE_FAILED;
-	DEBUG_JTAG_IO("clocks %i while in %s",
-		cmd->cmd.stableclocks->num_cycles,
-		tap_state_name(tap_get_state()));
-	return retval;
-}
-
-static int ft2232_execute_command(struct jtag_command *cmd)
-{
-	int retval;
-
-	switch (cmd->type) {
-		case JTAG_RESET:
-			retval = ft2232_execute_reset(cmd);
-			break;
-		case JTAG_RUNTEST:
-			retval = ft2232_execute_runtest(cmd);
-			break;
-		case JTAG_TLR_RESET:
-			retval = ft2232_execute_statemove(cmd);
-			break;
-		case JTAG_PATHMOVE:
-			retval = ft2232_execute_pathmove(cmd);
-			break;
-		case JTAG_SCAN:
-			retval = ft2232_execute_scan(cmd);
-			break;
-		case JTAG_SLEEP:
-			retval = ft2232_execute_sleep(cmd);
-			break;
-		case JTAG_STABLECLOCKS:
-			retval = ft2232_execute_stableclocks(cmd);
-			break;
-		case JTAG_TMS:
-			retval = ft2232_execute_tms(cmd);
-			break;
-		default:
-			LOG_ERROR("BUG: unknown JTAG command type encountered");
-			retval = ERROR_JTAG_QUEUE_FAILED;
-			break;
-	}
-	return retval;
-}
-
-static int ft2232_execute_queue(void)
-{
-	struct jtag_command *cmd = jtag_command_queue;	/* currently processed command */
-	int retval;
-
-	first_unsent = cmd;		/* next command that has to be sent */
-	require_send = 0;
-
-	/* return ERROR_OK, unless ft2232_send_and_recv reports a failed check
-	 * that wasn't handled by a caller-provided error handler
-	 */
-	retval = ERROR_OK;
-
-	ft2232_buffer_size = 0;
-	ft2232_expect_read = 0;
-
-	/* blink, if the current layout has that feature */
-	if (layout->blink)
-		layout->blink();
-
-	while (cmd) {
-		/* fill the write buffer with the desired command */
-		if (ft2232_execute_command(cmd) != ERROR_OK)
-			retval = ERROR_JTAG_QUEUE_FAILED;
-		/* Start reading input before FT2232 TX buffer fills up.
-		 * Sometimes this happens because we don't know the
-		 * length of the last command before we execute it. So
-		 * we simple inform the user.
-		 */
-		cmd = cmd->next;
-
-		if (ft2232_expect_read >= FT2232_BUFFER_READ_QUEUE_SIZE) {
-			if (ft2232_expect_read > (FT2232_BUFFER_READ_QUEUE_SIZE+1))
-				LOG_DEBUG("read buffer size looks too high %d/%d",
-					ft2232_expect_read,
-					(FT2232_BUFFER_READ_QUEUE_SIZE+1));
-			if (ft2232_send_and_recv(first_unsent, cmd) != ERROR_OK)
-				retval = ERROR_JTAG_QUEUE_FAILED;
-			first_unsent = cmd;
-		}
-	}
-
-	if (require_send > 0)
-		if (ft2232_send_and_recv(first_unsent, cmd) != ERROR_OK)
-			retval = ERROR_JTAG_QUEUE_FAILED;
-
-	return retval;
-}
-
-#if BUILD_FT2232_FTD2XX == 1
-static int ft2232_init_ftd2xx(uint16_t vid, uint16_t pid, int more, int *try_more)
-{
-	FT_STATUS status;
-	DWORD deviceID;
-	char SerialNumber[16];
-	char Description[64];
-	DWORD openex_flags  = 0;
-	char *openex_string = NULL;
-	uint8_t latency_timer;
-
-	if (layout == NULL) {
-		LOG_WARNING("No ft2232 layout specified'");
-		return ERROR_JTAG_INIT_FAILED;
-	}
-
-	LOG_DEBUG("'ft2232' interface using FTD2XX with '%s' layout (%4.4x:%4.4x)",
-			layout->name, vid, pid);
-
-#if IS_WIN32 == 0
-	/* Add non-standard Vid/Pid to the linux driver */
-	status = FT_SetVIDPID(vid, pid);
-	if (status != FT_OK)
-		LOG_WARNING("couldn't add %4.4x:%4.4x", vid, pid);
-
-#endif
-
-	if (ft2232_device_desc && ft2232_serial) {
-		LOG_WARNING(
-			"can't open by device description and serial number, giving precedence to serial");
-		ft2232_device_desc = NULL;
-	}
-
-	if (ft2232_device_desc) {
-		openex_string = ft2232_device_desc;
-		openex_flags  = FT_OPEN_BY_DESCRIPTION;
-	} else if (ft2232_serial) {
-		openex_string = ft2232_serial;
-		openex_flags  = FT_OPEN_BY_SERIAL_NUMBER;
-	} else {
-		LOG_ERROR("neither device description nor serial number specified");
-		LOG_ERROR(
-			"please add \"ft2232_device_desc <string>\" or \"ft2232_serial <string>\" to your .cfg file");
-
-		return ERROR_JTAG_INIT_FAILED;
-	}
-
-	status = FT_OpenEx(openex_string, openex_flags, &ftdih);
-	if (status != FT_OK) {
-		/* under Win32, the FTD2XX driver appends an "A" to the end
-		 * of the description, if we tried by the desc, then
-		 * try by the alternate "A" description. */
-		if (openex_string == ft2232_device_desc) {
-			/* Try the alternate method. */
-			openex_string = ft2232_device_desc_A;
-			status = FT_OpenEx(openex_string, openex_flags, &ftdih);
-			if (status == FT_OK) {
-				/* yea, the "alternate" method worked! */
-			} else {
-				/* drat, give the user a meaningfull message.
-				 * telling the use we tried *BOTH* methods. */
-				LOG_WARNING("Unable to open FTDI Device tried: '%s' and '%s'",
-					ft2232_device_desc,
-					ft2232_device_desc_A);
-			}
-		}
-	}
-
-	if (status != FT_OK) {
-		DWORD num_devices;
-
-		if (more) {
-			LOG_WARNING("unable to open ftdi device (trying more): %s",
-				ftd2xx_status_string(status));
-			*try_more = 1;
-			return ERROR_JTAG_INIT_FAILED;
-		}
-		LOG_ERROR("unable to open ftdi device: %s",
-			ftd2xx_status_string(status));
-		status = FT_ListDevices(&num_devices, NULL, FT_LIST_NUMBER_ONLY);
-		if (status == FT_OK) {
-			char **desc_array = malloc(sizeof(char *) * (num_devices + 1));
-			uint32_t i;
-
-			for (i = 0; i < num_devices; i++)
-				desc_array[i] = malloc(64);
-
-			desc_array[num_devices] = NULL;
-
-			status = FT_ListDevices(desc_array, &num_devices, FT_LIST_ALL | openex_flags);
-
-			if (status == FT_OK) {
-				LOG_ERROR("ListDevices: %" PRIu32, (uint32_t)num_devices);
-				for (i = 0; i < num_devices; i++)
-					LOG_ERROR("%" PRIu32 ": \"%s\"", i, desc_array[i]);
-			}
-
-			for (i = 0; i < num_devices; i++)
-				free(desc_array[i]);
-
-			free(desc_array);
-		} else
-			LOG_ERROR("ListDevices: NONE");
-		return ERROR_JTAG_INIT_FAILED;
-	}
-
-	status = FT_SetLatencyTimer(ftdih, ft2232_latency);
-	if (status != FT_OK) {
-		LOG_ERROR("unable to set latency timer: %s",
-			ftd2xx_status_string(status));
-		return ERROR_JTAG_INIT_FAILED;
-	}
-
-	status = FT_GetLatencyTimer(ftdih, &latency_timer);
-	if (status != FT_OK) {
-		/* ftd2xx 1.04 (linux) has a bug when calling FT_GetLatencyTimer
-		 * so ignore errors if using this driver version */
-		DWORD dw_version;
-
-		status = FT_GetDriverVersion(ftdih, &dw_version);
-		LOG_ERROR("unable to get latency timer: %s",
-			ftd2xx_status_string(status));
-
-		if ((status == FT_OK) && (dw_version == 0x10004)) {
-			LOG_ERROR("ftd2xx 1.04 detected - this has known issues " \
-				"with FT_GetLatencyTimer, upgrade to a newer version");
-		} else
-			return ERROR_JTAG_INIT_FAILED;
-	} else
-		LOG_DEBUG("current latency timer: %i", latency_timer);
-
-	status = FT_SetTimeouts(ftdih, 5000, 5000);
-	if (status != FT_OK) {
-		LOG_ERROR("unable to set timeouts: %s",
-			ftd2xx_status_string(status));
-		return ERROR_JTAG_INIT_FAILED;
-	}
-
-	status = FT_SetBitMode(ftdih, 0x0b, 2);
-	if (status != FT_OK) {
-		LOG_ERROR("unable to enable bit i/o mode: %s",
-			ftd2xx_status_string(status));
-		return ERROR_JTAG_INIT_FAILED;
-	}
-
-	status = FT_GetDeviceInfo(ftdih, &ftdi_device, &deviceID,
-			SerialNumber, Description, NULL);
-	if (status != FT_OK) {
-		LOG_ERROR("unable to get FT_GetDeviceInfo: %s",
-			ftd2xx_status_string(status));
-		return ERROR_JTAG_INIT_FAILED;
-	} else {
-		static const char *type_str[] = {
-			"BM", "AM", "100AX", "UNKNOWN", "2232C", "232R", "2232H", "4232H", "232H"
-		};
-		unsigned no_of_known_types = ARRAY_SIZE(type_str) - 1;
-		unsigned type_index = ((unsigned)ftdi_device <= no_of_known_types)
-			? ftdi_device : FT_DEVICE_UNKNOWN;
-		LOG_INFO("device: %" PRIu32 " \"%s\"", (uint32_t)ftdi_device, type_str[type_index]);
-		LOG_INFO("deviceID: %" PRIu32, (uint32_t)deviceID);
-		LOG_INFO("SerialNumber: %s", SerialNumber);
-		LOG_INFO("Description: %s", Description);
-	}
-
-	return ERROR_OK;
-}
-
-static int ft2232_purge_ftd2xx(void)
-{
-	FT_STATUS status;
-
-	status = FT_Purge(ftdih, FT_PURGE_RX | FT_PURGE_TX);
-	if (status != FT_OK) {
-		LOG_ERROR("error purging ftd2xx device: %s",
-			ftd2xx_status_string(status));
-		return ERROR_JTAG_INIT_FAILED;
-	}
-
-	return ERROR_OK;
-}
-
-#endif	/* BUILD_FT2232_FTD2XX == 1 */
-
-#if BUILD_FT2232_LIBFTDI == 1
-static int ft2232_init_libftdi(uint16_t vid, uint16_t pid, int more, int *try_more, int channel)
-{
-	uint8_t latency_timer;
-
-	if (layout == NULL) {
-		LOG_WARNING("No ft2232 layout specified'");
-		return ERROR_JTAG_INIT_FAILED;
-	}
-
-	LOG_DEBUG("'ft2232' interface using libftdi with '%s' layout (%4.4x:%4.4x)",
-		layout->name, vid, pid);
-
-	if (ftdi_init(&ftdic) < 0)
-		return ERROR_JTAG_INIT_FAILED;
-
-	/* default to INTERFACE_A */
-	if (channel == INTERFACE_ANY)
-		channel = INTERFACE_A;
-	if (ftdi_set_interface(&ftdic, channel) < 0) {
-		LOG_ERROR("unable to select FT2232 channel A: %s", ftdic.error_str);
-		return ERROR_JTAG_INIT_FAILED;
-	}
-
-	/* context, vendor id, product id */
-	if (ftdi_usb_open_desc(&ftdic, vid, pid, ft2232_device_desc, ft2232_serial) < 0) {
-		if (more)
-			LOG_WARNING("unable to open ftdi device (trying more): %s",
-				ftdic.error_str);
-		else
-			LOG_ERROR("unable to open ftdi device: %s", ftdic.error_str);
-		*try_more = 1;
-		return ERROR_JTAG_INIT_FAILED;
-	}
-
-	/* There is already a reset in ftdi_usb_open_desc, this should be redundant */
-	if (ftdi_usb_reset(&ftdic) < 0) {
-		LOG_ERROR("unable to reset ftdi device");
-		return ERROR_JTAG_INIT_FAILED;
-	}
-
-	if (ftdi_set_latency_timer(&ftdic, ft2232_latency) < 0) {
-		LOG_ERROR("unable to set latency timer");
-		return ERROR_JTAG_INIT_FAILED;
-	}
-
-	if (ftdi_get_latency_timer(&ftdic, &latency_timer) < 0) {
-		LOG_ERROR("unable to get latency timer");
-		return ERROR_JTAG_INIT_FAILED;
-	} else
-		LOG_DEBUG("current latency timer: %i", latency_timer);
-
-	ftdi_set_bitmode(&ftdic, 0x0b, 2);	/* ctx, JTAG I/O mask */
-
-	ftdi_device = ftdic.type;
-	static const char *type_str[] = {
-		"AM", "BM", "2232C", "R", "2232H", "4232H", "232H", "Unknown"
-	};
-	unsigned no_of_known_types = ARRAY_SIZE(type_str) - 1;
-	unsigned type_index = ((unsigned)ftdi_device < no_of_known_types)
-		? ftdi_device : no_of_known_types;
-	LOG_DEBUG("FTDI chip type: %i \"%s\"", (int)ftdi_device, type_str[type_index]);
-	return ERROR_OK;
-}
-
-static int ft2232_purge_libftdi(void)
-{
-	if (ftdi_usb_purge_buffers(&ftdic) < 0) {
-		LOG_ERROR("ftdi_purge_buffers: %s", ftdic.error_str);
-		return ERROR_JTAG_INIT_FAILED;
-	}
-
-	return ERROR_OK;
-}
-
-#endif	/* BUILD_FT2232_LIBFTDI == 1 */
-
-static int ft2232_set_data_bits_low_byte(uint8_t value, uint8_t direction)
-{
-	uint8_t buf[3];
-	uint32_t bytes_written;
-
-	buf[0] = 0x80;		/* command "set data bits low byte" */
-	buf[1] = value;		/* value */
-	buf[2] = direction;	/* direction */
-
-	LOG_DEBUG("%2.2x %2.2x %2.2x", buf[0], buf[1], buf[2]);
-
-	if (ft2232_write(buf, sizeof(buf), &bytes_written) != ERROR_OK) {
-		LOG_ERROR("couldn't initialize data bits low byte");
-		return ERROR_JTAG_INIT_FAILED;
-	}
-
-	return ERROR_OK;
-}
-
-static int ft2232_set_data_bits_high_byte(uint8_t value, uint8_t direction)
-{
-	uint8_t buf[3];
-	uint32_t bytes_written;
-
-	buf[0] = 0x82;		/* command "set data bits high byte" */
-	buf[1] = value;		/* value */
-	buf[2] = direction;	/* direction */
-
-	LOG_DEBUG("%2.2x %2.2x %2.2x", buf[0], buf[1], buf[2]);
-
-	if (ft2232_write(buf, sizeof(buf), &bytes_written) != ERROR_OK) {
-		LOG_ERROR("couldn't initialize data bits high byte");
-		return ERROR_JTAG_INIT_FAILED;
-	}
-
-	return ERROR_OK;
-}
-
-static int ft2232_init(void)
-{
-	uint8_t buf[1];
-	int retval;
-	uint32_t bytes_written;
-
-	LOG_WARNING("Using DEPRECATED interface driver 'ft2232'");
-#if BUILD_FTDI
-	LOG_INFO("Consider using the 'ftdi' interface driver, with configuration files in interface/ftdi/...");
-#endif
-
-	if (tap_get_tms_path_len(TAP_IRPAUSE, TAP_IRPAUSE) == 7)
-		LOG_DEBUG("ft2232 interface using 7 step jtag state transitions");
-	else
-		LOG_DEBUG("ft2232 interface using shortest path jtag state transitions");
-	if (layout == NULL) {
-		LOG_WARNING("No ft2232 layout specified'");
-		return ERROR_JTAG_INIT_FAILED;
-	}
-
-	for (int i = 0; 1; i++) {
-		/*
-		 * "more indicates that there are more IDs to try, so we should
-		 * not print an error for an ID mismatch (but for anything
-		 * else, we should).
-		 *
-		 * try_more indicates that the error code returned indicates an
-		 * ID mismatch (and nothing else) and that we should proceeed
-		 * with the next ID pair.
-		 */
-		int more = ft2232_vid[i + 1] || ft2232_pid[i + 1];
-		int try_more = 0;
-
-#if BUILD_FT2232_FTD2XX == 1
-		retval = ft2232_init_ftd2xx(ft2232_vid[i], ft2232_pid[i],
-				more, &try_more);
-#elif BUILD_FT2232_LIBFTDI == 1
-		retval = ft2232_init_libftdi(ft2232_vid[i], ft2232_pid[i],
-				more, &try_more, ft2232_channel);
-#endif
-		if (retval >= 0)
-			break;
-		if (!more || !try_more)
-			return retval;
-	}
-
-	ft2232_buffer_size = 0;
-	ft2232_buffer = malloc(FT2232_BUFFER_SIZE);
-
-	if (layout->init() != ERROR_OK)
-		return ERROR_JTAG_INIT_FAILED;
-
-	if (ft2232_device_is_highspeed()) {
-#ifndef BUILD_FT2232_HIGHSPEED
- #if BUILD_FT2232_FTD2XX == 1
-		LOG_WARNING(
-			"High Speed device found - You need a newer FTD2XX driver (version 2.04.16 or later)");
- #elif BUILD_FT2232_LIBFTDI == 1
-		LOG_WARNING(
-			"High Speed device found - You need a newer libftdi version (0.16 or later)");
- #endif
-#endif
-		/* make sure the legacy mode is disabled */
-		if (ftx232h_clk_divide_by_5(false) != ERROR_OK)
-			return ERROR_JTAG_INIT_FAILED;
-	}
-
-	buf[0] = 0x85;	/* Disconnect TDI/DO to TDO/DI for Loopback */
-	retval = ft2232_write(buf, 1, &bytes_written);
-	if (retval != ERROR_OK) {
-		LOG_ERROR("couldn't write to FT2232 to disable loopback");
-		return ERROR_JTAG_INIT_FAILED;
-	}
-
-#if BUILD_FT2232_FTD2XX == 1
-	return ft2232_purge_ftd2xx();
-#elif BUILD_FT2232_LIBFTDI == 1
-	return ft2232_purge_libftdi();
-#endif
-
-	return ERROR_OK;
-}
-
-/** Updates defaults for DBUS signals:  the four JTAG signals
- * (TCK, TDI, TDO, TMS) and * the four GPIOL signals.
- */
-static inline void ftx232_dbus_init(void)
-{
-	low_output    = 0x08;
-	low_direction = 0x0b;
-}
-
-/** Initializes DBUS signals:  the four JTAG signals (TCK, TDI, TDO, TMS),
- * the four GPIOL signals.  Initialization covers value and direction,
- * as customized for each layout.
- */
-static int ftx232_dbus_write(void)
-{
-	enum reset_types jtag_reset_config = jtag_get_reset_config();
-	if (jtag_reset_config & RESET_TRST_OPEN_DRAIN) {
-		low_direction &= ~nTRSTnOE;	/* nTRST input */
-		low_output &= ~nTRST;	/* nTRST = 0 */
-	} else {
-		low_direction |= nTRSTnOE;	/* nTRST output */
-		low_output |= nTRST;		/* nTRST = 1 */
-	}
-
-	if (jtag_reset_config & RESET_SRST_PUSH_PULL) {
-		low_direction |= nSRSTnOE;	/* nSRST output */
-		low_output |= nSRST;		/* nSRST = 1 */
-	} else {
-		low_direction &= ~nSRSTnOE;	/* nSRST input */
-		low_output &= ~nSRST;	/* nSRST = 0 */
-	}
-
-	/* initialize low byte for jtag */
-	if (ft2232_set_data_bits_low_byte(low_output, low_direction) != ERROR_OK) {
-		LOG_ERROR("couldn't initialize FT2232 DBUS");
-		return ERROR_JTAG_INIT_FAILED;
-	}
-
-	return ERROR_OK;
-}
-
-static int usbjtag_init(void)
-{
-	/*
-	 * NOTE:  This is now _specific_ to the "usbjtag" layout.
-	 * Don't try cram any more layouts into this.
-	 */
-	ftx232_dbus_init();
-
-	nTRST    = 0x10;
-	nTRSTnOE = 0x10;
-	nSRST    = 0x40;
-	nSRSTnOE = 0x40;
-
-	return ftx232_dbus_write();
-}
-
-static int lm3s811_jtag_init(void)
-{
-	ftx232_dbus_init();
-
-	/* There are multiple revisions of LM3S811 eval boards:
-	 * - Rev B (and older?) boards have no SWO trace support.
-	 * - Rev C boards add ADBUS_6 DBG_ENn and BDBUS_4 SWO_EN;
-	 *   they should use the "luminary_icdi" layout instead.
-	 */
-	nTRST = 0x0;
-	nTRSTnOE = 0x00;
-	nSRST = 0x20;
-	nSRSTnOE = 0x20;
-	low_output    = 0x88;
-	low_direction = 0x8b;
-
-	return ftx232_dbus_write();
-}
-
-static int icdi_jtag_init(void)
-{
-	ftx232_dbus_init();
-
-	/* Most Luminary eval boards support SWO trace output,
-	 * and should use this "luminary_icdi" layout.
-	 *
-	 * ADBUS 0..3 are used for JTAG as usual.  GPIOs are used
-	 * to switch between JTAG and SWD, or switch the ft2232 UART
-	 * on the second MPSSE channel/interface (BDBUS)
-	 * between (i) the stellaris UART (on Luminary boards)
-	 * or (ii) SWO trace data (generic).
-	 *
-	 * We come up in JTAG mode and may switch to SWD later (with
-	 * SWO/trace option if SWD is active).
-	 *
-	 * DBUS == GPIO-Lx
-	 * CBUS == GPIO-Hx
-	 */
-
-
-#define ICDI_JTAG_EN (1 << 7)		/* ADBUS 7 (a.k.a. DBGMOD) */
-#define ICDI_DBG_ENn (1 << 6)		/* ADBUS 6 */
-#define ICDI_SRST (1 << 5)		/* ADBUS 5 */
-
-
-	/* GPIOs on second channel/interface (UART) ... */
-#define ICDI_SWO_EN (1 << 4)		/* BDBUS 4 */
-#define ICDI_TX_SWO (1 << 1)		/* BDBUS 1 */
-#define ICDI_VCP_RX (1 << 0)		/* BDBUS 0 (to stellaris UART) */
-
-	nTRST = 0x0;
-	nTRSTnOE = 0x00;
-	nSRST = ICDI_SRST;
-	nSRSTnOE = ICDI_SRST;
-
-	low_direction |= ICDI_JTAG_EN | ICDI_DBG_ENn;
-	low_output    |= ICDI_JTAG_EN;
-	low_output    &= ~ICDI_DBG_ENn;
-
-	return ftx232_dbus_write();
-}
-
-static int signalyzer_init(void)
-{
-	ftx232_dbus_init();
-
-	nTRST    = 0x10;
-	nTRSTnOE = 0x10;
-	nSRST    = 0x20;
-	nSRSTnOE = 0x20;
-	return ftx232_dbus_write();
-}
-
-static int axm0432_jtag_init(void)
-{
-	low_output    = 0x08;
-	low_direction = 0x2b;
-
-	/* initialize low byte for jtag */
-	if (ft2232_set_data_bits_low_byte(low_output, low_direction) != ERROR_OK) {
-		LOG_ERROR("couldn't initialize FT2232 with 'JTAGkey' layout");
-		return ERROR_JTAG_INIT_FAILED;
-	}
-
-	if (strcmp(layout->name, "axm0432_jtag") == 0) {
-		nTRST    = 0x08;
-		nTRSTnOE = 0x0;		/* No output enable for TRST*/
-		nSRST    = 0x04;
-		nSRSTnOE = 0x0;		/* No output enable for SRST*/
-	} else {
-		LOG_ERROR("BUG: axm0432_jtag_init called for non axm0432 layout");
-		exit(-1);
-	}
-
-	high_output    = 0x0;
-	high_direction = 0x0c;
-
-	enum reset_types jtag_reset_config = jtag_get_reset_config();
-	if (jtag_reset_config & RESET_TRST_OPEN_DRAIN)
-		LOG_ERROR("can't set nTRSTOE to push-pull on the Dicarlo jtag");
-	else
-		high_output |= nTRST;
-
-	if (jtag_reset_config & RESET_SRST_PUSH_PULL)
-		LOG_ERROR("can't set nSRST to push-pull on the Dicarlo jtag");
-	else
-		high_output |= nSRST;
-
-	/* initialize high byte for jtag */
-	if (ft2232_set_data_bits_high_byte(high_output, high_direction) != ERROR_OK) {
-		LOG_ERROR("couldn't initialize FT2232 with 'Dicarlo' layout");
-		return ERROR_JTAG_INIT_FAILED;
-	}
-
-	return ERROR_OK;
-}
-
-static int redbee_init(void)
-{
-	low_output    = 0x08;
-	low_direction = 0x2b;
-
-	/* initialize low byte for jtag */
-	if (ft2232_set_data_bits_low_byte(low_output, low_direction) != ERROR_OK) {
-		LOG_ERROR("couldn't initialize FT2232 with 'redbee' layout");
-		return ERROR_JTAG_INIT_FAILED;
-	}
-
-	nTRST    = 0x08;
-	nTRSTnOE = 0x0;		/* No output enable for TRST*/
-	nSRST    = 0x04;
-	nSRSTnOE = 0x0;		/* No output enable for SRST*/
-
-	high_output    = 0x0;
-	high_direction = 0x0c;
-
-	enum reset_types jtag_reset_config = jtag_get_reset_config();
-	if (jtag_reset_config & RESET_TRST_OPEN_DRAIN)
-		LOG_ERROR("can't set nTRSTOE to push-pull on redbee");
-	else
-		high_output |= nTRST;
-
-	if (jtag_reset_config & RESET_SRST_PUSH_PULL)
-		LOG_ERROR("can't set nSRST to push-pull on redbee");
-	else
-		high_output |= nSRST;
-
-	/* initialize high byte for jtag */
-	if (ft2232_set_data_bits_high_byte(high_output, high_direction) != ERROR_OK) {
-		LOG_ERROR("couldn't initialize FT2232 with 'redbee' layout");
-		return ERROR_JTAG_INIT_FAILED;
-	}
-
-	return ERROR_OK;
-}
-
-static int jtagkey_init(void)
-{
-	low_output    = 0x08;
-	low_direction = 0x1b;
-
-	/* initialize low byte for jtag */
-	if (ft2232_set_data_bits_low_byte(low_output, low_direction) != ERROR_OK) {
-		LOG_ERROR("couldn't initialize FT2232 with 'JTAGkey' layout");
-		return ERROR_JTAG_INIT_FAILED;
-	}
-
-	if (strcmp(layout->name, "jtagkey") == 0) {
-		nTRST    = 0x01;
-		nTRSTnOE = 0x4;
-		nSRST    = 0x02;
-		nSRSTnOE = 0x08;
-	} else if ((strcmp(layout->name, "jtagkey_prototype_v1") == 0)
-		   || (strcmp(layout->name, "oocdlink") == 0)) {
-		nTRST    = 0x02;
-		nTRSTnOE = 0x1;
-		nSRST    = 0x08;
-		nSRSTnOE = 0x04;
-	} else {
-		LOG_ERROR("BUG: jtagkey_init called for non jtagkey layout");
-		exit(-1);
-	}
-
-	high_output    = 0x0;
-	high_direction = 0x0f;
-
-	enum reset_types jtag_reset_config = jtag_get_reset_config();
-	if (jtag_reset_config & RESET_TRST_OPEN_DRAIN) {
-		high_output |= nTRSTnOE;
-		high_output &= ~nTRST;
-	} else {
-		high_output &= ~nTRSTnOE;
-		high_output |= nTRST;
-	}
-
-	if (jtag_reset_config & RESET_SRST_PUSH_PULL) {
-		high_output &= ~nSRSTnOE;
-		high_output |= nSRST;
-	} else {
-		high_output |= nSRSTnOE;
-		high_output &= ~nSRST;
-	}
-
-	/* initialize high byte for jtag */
-	if (ft2232_set_data_bits_high_byte(high_output, high_direction) != ERROR_OK) {
-		LOG_ERROR("couldn't initialize FT2232 with 'JTAGkey' layout");
-		return ERROR_JTAG_INIT_FAILED;
-	}
-
-	return ERROR_OK;
-}
-
-static int olimex_jtag_init(void)
-{
-	low_output    = 0x08;
-	low_direction = 0x1b;
-
-	/* initialize low byte for jtag */
-	if (ft2232_set_data_bits_low_byte(low_output, low_direction) != ERROR_OK) {
-		LOG_ERROR("couldn't initialize FT2232 with 'Olimex' layout");
-		return ERROR_JTAG_INIT_FAILED;
-	}
-
-	nTRST    = 0x01;
-	nTRSTnOE = 0x4;
-	nSRST    = 0x02;
-	nSRSTnOE = 0x00;/* no output enable for nSRST */
-
-	high_output    = 0x0;
-	high_direction = 0x0f;
-
-	enum reset_types jtag_reset_config = jtag_get_reset_config();
-	if (jtag_reset_config & RESET_TRST_OPEN_DRAIN) {
-		high_output |= nTRSTnOE;
-		high_output &= ~nTRST;
-	} else {
-		high_output &= ~nTRSTnOE;
-		high_output |= nTRST;
-	}
-
-	if (jtag_reset_config & RESET_SRST_PUSH_PULL)
-		LOG_ERROR("can't set nSRST to push-pull on the Olimex ARM-USB-OCD");
-	else
-		high_output &= ~nSRST;
-
-	/* turn red LED on */
-	high_output |= 0x08;
-
-	/* initialize high byte for jtag */
-	if (ft2232_set_data_bits_high_byte(high_output, high_direction) != ERROR_OK) {
-		LOG_ERROR("couldn't initialize FT2232 with 'Olimex' layout");
-		return ERROR_JTAG_INIT_FAILED;
-	}
-
-	return ERROR_OK;
-}
-
-static int flyswatter_init(int rev)
-{
-	low_output    = 0x18;
-	low_direction = 0x7b;
-
-	if ((rev < 0) || (rev > 3)) {
-		LOG_ERROR("bogus 'flyswatter' revision supplied (%i)", rev);
-		return ERROR_JTAG_INIT_FAILED;
-	}
-
-	if (rev == 1)
-		low_direction |= 1 << 7;
-
-	/* initialize low byte for jtag */
-	if (ft2232_set_data_bits_low_byte(low_output, low_direction) != ERROR_OK) {
-		LOG_ERROR("couldn't initialize FT2232 with 'flyswatter' layout");
-		return ERROR_JTAG_INIT_FAILED;
-	}
-
-	nTRST    = 0x10;
-	nTRSTnOE = 0x0;		/* not output enable for nTRST */
-	nSRST    = 0x20;
-	nSRSTnOE = 0x00;	/* no output enable for nSRST */
-
-	high_output    = 0x00;
-
-	if (rev == 1)
-		high_direction = 0x0c;
-	else
-		high_direction = 0x01;
-
-	/* turn red LED3 on, LED2 off */
-	high_output |= 0x08;
-
-	/* initialize high byte for jtag */
-	if (ft2232_set_data_bits_high_byte(high_output, high_direction) != ERROR_OK) {
-		LOG_ERROR("couldn't initialize FT2232 with 'flyswatter' layout");
-		return ERROR_JTAG_INIT_FAILED;
-	}
-
-	return ERROR_OK;
-}
-
-static int flyswatter1_init(void)
-{
-	return flyswatter_init(1);
-}
-
-static int flyswatter2_init(void)
-{
-	return flyswatter_init(2);
-}
-
-static int minimodule_init(void)
-{
-	low_output    = 0x18;	/* check if srst should be 1 or 0 initially. (0x08) (flyswatter was
-				 * 0x18) */
-	low_direction = 0xfb;	/* 0xfb; */
-
-	/* initialize low byte for jtag */
-	if (ft2232_set_data_bits_low_byte(low_output, low_direction) != ERROR_OK) {
-		LOG_ERROR("couldn't initialize FT2232 with 'minimodule' layout");
-		return ERROR_JTAG_INIT_FAILED;
-	}
-
-
-	nSRST    = 0x20;
-
-	high_output    = 0x00;
-	high_direction = 0x05;
-
-	/* turn red LED3 on, LED2 off */
-	/* high_output |= 0x08; */
-
-	/* initialize high byte for jtag */
-	if (ft2232_set_data_bits_high_byte(high_output, high_direction) != ERROR_OK) {
-		LOG_ERROR("couldn't initialize FT2232 with 'minimodule' layout");
-		return ERROR_JTAG_INIT_FAILED;
-	}
-
-	return ERROR_OK;
-}
-
-static int turtle_init(void)
-{
-	low_output    = 0x08;
-	low_direction = 0x5b;
-
-	/* initialize low byte for jtag */
-	if (ft2232_set_data_bits_low_byte(low_output, low_direction) != ERROR_OK) {
-		LOG_ERROR("couldn't initialize FT2232 with 'turtelizer2' layout");
-		return ERROR_JTAG_INIT_FAILED;
-	}
-
-	nSRST = 0x40;
-
-	high_output    = 0x00;
-	high_direction = 0x0C;
-
-	/* initialize high byte for jtag */
-	if (ft2232_set_data_bits_high_byte(high_output, high_direction) != ERROR_OK) {
-		LOG_ERROR("couldn't initialize FT2232 with 'turtelizer2' layout");
-		return ERROR_JTAG_INIT_FAILED;
-	}
-
-	return ERROR_OK;
-}
-
-static int comstick_init(void)
-{
-	low_output    = 0x08;
-	low_direction = 0x0b;
-
-	/* initialize low byte for jtag */
-	if (ft2232_set_data_bits_low_byte(low_output, low_direction) != ERROR_OK) {
-		LOG_ERROR("couldn't initialize FT2232 with 'comstick' layout");
-		return ERROR_JTAG_INIT_FAILED;
-	}
-
-	nTRST    = 0x01;
-	nTRSTnOE = 0x00;	/* no output enable for nTRST */
-	nSRST    = 0x02;
-	nSRSTnOE = 0x00;	/* no output enable for nSRST */
-
-	high_output    = 0x03;
-	high_direction = 0x03;
-
-	/* initialize high byte for jtag */
-	if (ft2232_set_data_bits_high_byte(high_output, high_direction) != ERROR_OK) {
-		LOG_ERROR("couldn't initialize FT2232 with 'comstick' layout");
-		return ERROR_JTAG_INIT_FAILED;
-	}
-
-	return ERROR_OK;
-}
-
-static int stm32stick_init(void)
-{
-	low_output    = 0x88;
-	low_direction = 0x8b;
-
-	/* initialize low byte for jtag */
-	if (ft2232_set_data_bits_low_byte(low_output, low_direction) != ERROR_OK) {
-		LOG_ERROR("couldn't initialize FT2232 with 'stm32stick' layout");
-		return ERROR_JTAG_INIT_FAILED;
-	}
-
-	nTRST    = 0x01;
-	nTRSTnOE = 0x00;	/* no output enable for nTRST */
-	nSRST    = 0x80;
-	nSRSTnOE = 0x00;	/* no output enable for nSRST */
-
-	high_output    = 0x01;
-	high_direction = 0x03;
-
-	/* initialize high byte for jtag */
-	if (ft2232_set_data_bits_high_byte(high_output, high_direction) != ERROR_OK) {
-		LOG_ERROR("couldn't initialize FT2232 with 'stm32stick' layout");
-		return ERROR_JTAG_INIT_FAILED;
-	}
-
-	return ERROR_OK;
-}
-
-static int sheevaplug_init(void)
-{
-	low_output = 0x08;
-	low_direction = 0x1b;
-
-	/* initialize low byte for jtag */
-	if (ft2232_set_data_bits_low_byte(low_output, low_direction) != ERROR_OK) {
-		LOG_ERROR("couldn't initialize FT2232 with 'sheevaplug' layout");
-		return ERROR_JTAG_INIT_FAILED;
-	}
-
-	nTRSTnOE = 0x1;
-	nTRST = 0x02;
-	nSRSTnOE = 0x4;
-	nSRST = 0x08;
-
-	high_output = 0x0;
-	high_direction = 0x0f;
-
-	/* nTRST is always push-pull */
-	high_output &= ~nTRSTnOE;
-	high_output |= nTRST;
-
-	/* nSRST is always open-drain */
-	high_output |= nSRSTnOE;
-	high_output &= ~nSRST;
-
-	/* initialize high byte for jtag */
-	if (ft2232_set_data_bits_high_byte(high_output, high_direction) != ERROR_OK) {
-		LOG_ERROR("couldn't initialize FT2232 with 'sheevaplug' layout");
-		return ERROR_JTAG_INIT_FAILED;
-	}
-
-	return ERROR_OK;
-}
-
-static int cortino_jtag_init(void)
-{
-	low_output    = 0x08;
-	low_direction = 0x1b;
-
-	/* initialize low byte for jtag */
-	if (ft2232_set_data_bits_low_byte(low_output, low_direction) != ERROR_OK) {
-		LOG_ERROR("couldn't initialize FT2232 with 'cortino' layout");
-		return ERROR_JTAG_INIT_FAILED;
-	}
-
-	nTRST    = 0x01;
-	nTRSTnOE = 0x00;	/* no output enable for nTRST */
-	nSRST    = 0x02;
-	nSRSTnOE = 0x00;	/* no output enable for nSRST */
-
-	high_output    = 0x03;
-	high_direction = 0x03;
-
-	/* initialize high byte for jtag */
-	if (ft2232_set_data_bits_high_byte(high_output, high_direction) != ERROR_OK) {
-		LOG_ERROR("couldn't initialize FT2232 with 'cortino' layout");
-		return ERROR_JTAG_INIT_FAILED;
-	}
-
-	return ERROR_OK;
-}
-
-static int lisa_l_init(void)
-{
-	ftx232_dbus_init();
-
-	nTRST    = 0x10;
-	nTRSTnOE = 0x10;
-	nSRST    = 0x40;
-	nSRSTnOE = 0x40;
-
-	high_output = 0x00;
-	high_direction = 0x18;
-
-	/* initialize high byte for jtag */
-	if (ft2232_set_data_bits_high_byte(high_output, high_direction) != ERROR_OK) {
-		LOG_ERROR("couldn't initialize FT2232 with 'lisa_l' layout");
-		return ERROR_JTAG_INIT_FAILED;
-	}
-
-	return ftx232_dbus_write();
-}
-
-static int flossjtag_init(void)
-{
-	ftx232_dbus_init();
-
-	nTRST    = 0x10;
-	nTRSTnOE = 0x10;
-	nSRST    = 0x40;
-	nSRSTnOE = 0x40;
-
-	high_output = 0x00;
-	high_direction = 0x18;
-
-	/* initialize high byte for jtag */
-	if (ft2232_set_data_bits_high_byte(high_output, high_direction) != ERROR_OK) {
-		LOG_ERROR("couldn't initialize FT2232 with 'Floss-JTAG' layout");
-		return ERROR_JTAG_INIT_FAILED;
-	}
-
-	return ftx232_dbus_write();
-}
-
-/*
- * The reference schematic from TI for the XDS100v2 has a CPLD on which opens
- * the door for a number of different configurations
- *
- * Known Implementations:
- * http://processors.wiki.ti.com/images/9/93/TMS570LS20216_USB_STICK_Schematic.pdf
- *
- * http://processors.wiki.ti.com/index.php/XDS100 (rev2)
- *	* CLPD logic: Rising edge to enable outputs (XDS100_PWR_RST)
- *		* ACBUS3 to transition 0->1 (OE rising edge)
- *	* CPLD logic: Put the EMU0/1 pins in Hi-Z:
- *		* ADBUS5/GPIOL1 = EMU_EN = 1
- *		* ADBUS6/GPIOL2 = EMU0 = 0
- *		* ACBUS4/SPARE0 = EMU1 = 0
- *	* CPLD logic: Disable loopback
- *		* ACBUS6/SPARE2 = LOOPBACK = 0
- */
-#define XDS100_nEMU_EN  (1<<5)
-#define XDS100_nEMU0    (1<<6)
-
-#define XDS100_PWR_RST  (1<<3)
-#define XDS100_nEMU1    (1<<4)
-#define XDS100_LOOPBACK (1<<6)
-static int xds100v2_init(void)
-{
-	/* These are in the lower byte */
-	nTRST    = 0x10;
-	nTRSTnOE = 0x10;
-
-	/* These aren't actually used on 14 pin connectors
-	 * These are in the upper byte */
-	nSRST    = 0x01;
-	nSRSTnOE = 0x01;
-
-	low_output    = 0x08 | nTRST | XDS100_nEMU_EN;
-	low_direction = 0x0b | nTRSTnOE | XDS100_nEMU_EN | XDS100_nEMU0;
-
-	if (ft2232_set_data_bits_low_byte(low_output, low_direction) != ERROR_OK) {
-		LOG_ERROR("couldn't initialize FT2232 with 'xds100v2' layout");
-		return ERROR_JTAG_INIT_FAILED;
-	}
-
-	high_output = 0;
-	high_direction = nSRSTnOE | XDS100_LOOPBACK | XDS100_PWR_RST | XDS100_nEMU1;
-
-	/* initialize high byte for jtag */
-	if (ft2232_set_data_bits_high_byte(high_output, high_direction) != ERROR_OK) {
-		LOG_ERROR("couldn't put CPLD in to reset with 'xds100v2' layout");
-		return ERROR_JTAG_INIT_FAILED;
-	}
-
-	high_output |= XDS100_PWR_RST;
-
-	/* initialize high byte for jtag */
-	if (ft2232_set_data_bits_high_byte(high_output, high_direction) != ERROR_OK) {
-		LOG_ERROR("couldn't bring CPLD out of reset with 'xds100v2' layout");
-		return ERROR_JTAG_INIT_FAILED;
-	}
-
-	return ERROR_OK;
-}
-
-static void olimex_jtag_blink(void)
-{
-	/* Olimex ARM-USB-OCD has a LED connected to ACBUS3
-	 * ACBUS3 is bit 3 of the GPIOH port
-	 */
-	high_output ^= 0x08;
-
-	buffer_write(0x82);
-	buffer_write(high_output);
-	buffer_write(high_direction);
-}
-
-static void flyswatter_jtag_blink(unsigned char led)
-{
-	buffer_write(0x82);
-	buffer_write(high_output ^ led);
-	buffer_write(high_direction);
-}
-
-static void flyswatter1_jtag_blink(void)
-{
-	/*
-	 * Flyswatter has two LEDs connected to ACBUS2 and ACBUS3
-	 */
-	flyswatter_jtag_blink(0xc);
-}
-
-static void flyswatter2_jtag_blink(void)
-{
-	/*
-	 * Flyswatter2 only has one LED connected to ACBUS2
-	 */
-	flyswatter_jtag_blink(0x4);
-}
-
-static void turtle_jtag_blink(void)
-{
-	/*
-	 * Turtelizer2 has two LEDs connected to ACBUS2 and ACBUS3
-	 */
-	if (high_output & 0x08)
-		high_output = 0x04;
-	else
-		high_output = 0x08;
-
-	buffer_write(0x82);
-	buffer_write(high_output);
-	buffer_write(high_direction);
-}
-
-static void lisa_l_blink(void)
-{
-	/*
-	 * Lisa/L has two LEDs connected to BCBUS3 and BCBUS4
-	 */
-	if (high_output & 0x10)
-		high_output = 0x08;
-	else
-		high_output = 0x10;
-
-	buffer_write(0x82);
-	buffer_write(high_output);
-	buffer_write(high_direction);
-}
-
-static void flossjtag_blink(void)
-{
-	/*
-	 * Floss-JTAG has two LEDs connected to ACBUS3 and ACBUS4
-	 */
-	if (high_output & 0x10)
-		high_output = 0x08;
-	else
-		high_output = 0x10;
-
-	buffer_write(0x82);
-	buffer_write(high_output);
-	buffer_write(high_direction);
-}
-
-static int ft2232_quit(void)
-{
-#if BUILD_FT2232_FTD2XX == 1
-
-	FT_Close(ftdih);
-#elif BUILD_FT2232_LIBFTDI == 1
-	ftdi_usb_close(&ftdic);
-
-	ftdi_deinit(&ftdic);
-#endif
-
-	free(ft2232_buffer);
-	ft2232_buffer = NULL;
-
-	return ERROR_OK;
-}
-
-COMMAND_HANDLER(ft2232_handle_device_desc_command)
-{
-	char *cp;
-	char buf[200];
-	if (CMD_ARGC == 1) {
-		ft2232_device_desc = strdup(CMD_ARGV[0]);
-		cp = strchr(ft2232_device_desc, 0);
-		/* under Win32, the FTD2XX driver appends an "A" to the end
-		 * of the description, this examines the given desc
-		 * and creates the 'missing' _A or non_A variable. */
-		if ((cp[-1] == 'A') && (cp[-2] == ' ')) {
-			/* it was, so make this the "A" version. */
-			ft2232_device_desc_A = ft2232_device_desc;
-			/* and *CREATE* the non-A version. */
-			strcpy(buf, ft2232_device_desc);
-			cp = strchr(buf, 0);
-			cp[-2] = 0;
-			ft2232_device_desc = strdup(buf);
-		} else {
-			/* <space > A not defined
-			 * so create it */
-			sprintf(buf, "%s A", ft2232_device_desc);
-			ft2232_device_desc_A = strdup(buf);
-		}
-	} else
-		LOG_ERROR("expected exactly one argument to ft2232_device_desc <description>");
-
-	return ERROR_OK;
-}
-
-COMMAND_HANDLER(ft2232_handle_serial_command)
-{
-	if (CMD_ARGC == 1)
-		ft2232_serial = strdup(CMD_ARGV[0]);
-	else
-		return ERROR_COMMAND_SYNTAX_ERROR;
-
-	return ERROR_OK;
-}
-
-COMMAND_HANDLER(ft2232_handle_layout_command)
-{
-	if (CMD_ARGC != 1)
-		return ERROR_COMMAND_SYNTAX_ERROR;
-
-	if (layout) {
-		LOG_ERROR("already specified ft2232_layout %s",
-			layout->name);
-		return (strcmp(layout->name, CMD_ARGV[0]) != 0)
-		       ? ERROR_FAIL
-		       : ERROR_OK;
-	}
-
-	for (const struct ft2232_layout *l = ft2232_layouts; l->name; l++) {
-		if (strcmp(l->name, CMD_ARGV[0]) == 0) {
-			layout = l;
-			ft2232_channel = l->channel;
-			return ERROR_OK;
-		}
-	}
-
-	LOG_ERROR("No FT2232 layout '%s' found", CMD_ARGV[0]);
-	return ERROR_FAIL;
-}
-
-COMMAND_HANDLER(ft2232_handle_vid_pid_command)
-{
-	if (CMD_ARGC > MAX_USB_IDS * 2) {
-		LOG_WARNING("ignoring extra IDs in ft2232_vid_pid "
-			"(maximum is %d pairs)", MAX_USB_IDS);
-		CMD_ARGC = MAX_USB_IDS * 2;
-	}
-	if (CMD_ARGC < 2 || (CMD_ARGC & 1)) {
-		LOG_WARNING("incomplete ft2232_vid_pid configuration directive");
-		if (CMD_ARGC < 2)
-			return ERROR_COMMAND_SYNTAX_ERROR;
-		/* remove the incomplete trailing id */
-		CMD_ARGC -= 1;
-	}
-
-	unsigned i;
-	for (i = 0; i < CMD_ARGC; i += 2) {
-		COMMAND_PARSE_NUMBER(u16, CMD_ARGV[i], ft2232_vid[i >> 1]);
-		COMMAND_PARSE_NUMBER(u16, CMD_ARGV[i + 1], ft2232_pid[i >> 1]);
-	}
-
-	/*
-	 * Explicitly terminate, in case there are multiples instances of
-	 * ft2232_vid_pid.
-	 */
-	ft2232_vid[i >> 1] = ft2232_pid[i >> 1] = 0;
-
-	return ERROR_OK;
-}
-
-COMMAND_HANDLER(ft2232_handle_latency_command)
-{
-	if (CMD_ARGC == 1)
-		ft2232_latency = atoi(CMD_ARGV[0]);
-	else
-		return ERROR_COMMAND_SYNTAX_ERROR;
-
-	return ERROR_OK;
-}
-
-COMMAND_HANDLER(ft2232_handle_channel_command)
-{
-	if (CMD_ARGC == 1) {
-		ft2232_channel = atoi(CMD_ARGV[0]);
-		if (ft2232_channel < 0 || ft2232_channel > 4)
-			LOG_ERROR("ft2232_channel must be in the 0 to 4 range");
-	} else
-		LOG_ERROR("expected exactly one argument to ft2232_channel <ch>");
-
-	return ERROR_OK;
-}
-
-static int ft2232_stableclocks(int num_cycles, struct jtag_command *cmd)
-{
-	int retval = 0;
-
-	/* 7 bits of either ones or zeros. */
-	uint8_t tms = (tap_get_state() == TAP_RESET ? 0x7F : 0x00);
-
-	while (num_cycles > 0) {
-		/* the command 0x4b, "Clock Data to TMS/CS Pin (no Read)" handles
-		 * at most 7 bits per invocation.  Here we invoke it potentially
-		 * several times.
-		 */
-		int bitcount_per_command = (num_cycles > 7) ? 7 : num_cycles;
-
-		if (ft2232_buffer_size + 3 >= FT2232_BUFFER_SIZE) {
-			if (ft2232_send_and_recv(first_unsent, cmd) != ERROR_OK)
-				retval = ERROR_JTAG_QUEUE_FAILED;
-
-			first_unsent = cmd;
-		}
-
-		/* there are no state transitions in this code, so omit state tracking */
-
-		/* command "Clock Data to TMS/CS Pin (no Read)" */
-		buffer_write(0x4b);
-
-		/* scan 7 bit */
-		buffer_write(bitcount_per_command - 1);
-
-		/* TMS data bits are either all zeros or ones to stay in the current stable state */
-		buffer_write(tms);
-
-		require_send = 1;
-
-		num_cycles -= bitcount_per_command;
-	}
-
-	return retval;
-}
-
-/* ---------------------------------------------------------------------
- * Support for IceBear JTAG adapter from Section5:
- *	http://section5.ch/icebear
- *
- * Author: Sten, debian@sansys-electronic.com
- */
-
-/* Icebear pin layout
- *
- * ADBUS5 (nEMU) nSRST	| 2   1|	GND (10k->VCC)
- * GND GND		| 4   3|	n.c.
- * ADBUS3 TMS		| 6   5|	ADBUS6 VCC
- * ADBUS0 TCK		| 8   7|	ADBUS7 (GND)
- * ADBUS4 nTRST		|10   9|	ACBUS0 (GND)
- * ADBUS1 TDI		|12  11|	ACBUS1 (GND)
- * ADBUS2 TDO		|14  13|	GND GND
- *
- * ADBUS0 O L TCK		ACBUS0 GND
- * ADBUS1 O L TDI		ACBUS1 GND
- * ADBUS2 I   TDO		ACBUS2 n.c.
- * ADBUS3 O H TMS		ACBUS3 n.c.
- * ADBUS4 O H nTRST
- * ADBUS5 O H nSRST
- * ADBUS6 -   VCC
- * ADBUS7 -   GND
- */
-static int icebear_jtag_init(void)
-{
-	low_direction   = 0x0b;	/* output: TCK TDI TMS; input: TDO */
-	low_output      = 0x08;	/* high: TMS; low: TCK TDI */
-	nTRST           = 0x10;
-	nSRST           = 0x20;
-
-	enum reset_types jtag_reset_config = jtag_get_reset_config();
-	if ((jtag_reset_config & RESET_TRST_OPEN_DRAIN) != 0)
-		low_direction   &= ~nTRST;	/* nTRST high impedance */
-	else {
-		low_direction   |= nTRST;
-		low_output      |= nTRST;
-	}
-
-	low_direction   |= nSRST;
-	low_output      |= nSRST;
-
-	/* initialize low byte for jtag */
-	if (ft2232_set_data_bits_low_byte(low_output, low_direction) != ERROR_OK) {
-		LOG_ERROR("couldn't initialize FT2232 with 'IceBear' layout (low)");
-		return ERROR_JTAG_INIT_FAILED;
-	}
-
-	high_output    = 0x0;
-	high_direction = 0x00;
-
-	/* initialize high byte for jtag */
-	if (ft2232_set_data_bits_high_byte(high_output, high_direction) != ERROR_OK) {
-		LOG_ERROR("couldn't initialize FT2232 with 'IceBear' layout (high)");
-		return ERROR_JTAG_INIT_FAILED;
-	}
-
-	return ERROR_OK;
-}
-
-static void icebear_jtag_reset(int trst, int srst)
-{
-	if (trst == 1) {
-		low_direction   |= nTRST;
-		low_output      &= ~nTRST;
-	} else if (trst == 0) {
-		enum reset_types jtag_reset_config = jtag_get_reset_config();
-		if ((jtag_reset_config & RESET_TRST_OPEN_DRAIN) != 0)
-			low_direction   &= ~nTRST;
-		else
-			low_output      |= nTRST;
-	}
-
-	if (srst == 1)
-		low_output &= ~nSRST;
-	else if (srst == 0)
-		low_output |= nSRST;
-
-	/* command "set data bits low byte" */
-	buffer_write(0x80);
-	buffer_write(low_output);
-	buffer_write(low_direction);
-
-	LOG_DEBUG("trst: %i, srst: %i, low_output: 0x%2.2x, low_direction: 0x%2.2x",
-		trst,
-		srst,
-		low_output,
-		low_direction);
-}
-
-/* ---------------------------------------------------------------------
- * Support for Signalyzer H2 and Signalyzer H4
- * JTAG adapter from Xverve Technologies Inc.
- * http://www.signalyzer.com or http://www.xverve.com
- *
- * Author: Oleg Seiljus, oleg@signalyzer.com
- */
-static unsigned char signalyzer_h_side;
-static unsigned int signalyzer_h_adapter_type;
-
-static int signalyzer_h_ctrl_write(int address, unsigned short value);
-
-#if BUILD_FT2232_FTD2XX == 1
-static int signalyzer_h_ctrl_read(int address, unsigned short *value);
-#endif
-
-#define SIGNALYZER_COMMAND_ADDR					128
-#define SIGNALYZER_DATA_BUFFER_ADDR				129
-
-#define SIGNALYZER_COMMAND_VERSION				0x41
-#define SIGNALYZER_COMMAND_RESET				0x42
-#define SIGNALYZER_COMMAND_POWERCONTROL_GET		0x50
-#define SIGNALYZER_COMMAND_POWERCONTROL_SET		0x51
-#define SIGNALYZER_COMMAND_PWM_SET				0x52
-#define SIGNALYZER_COMMAND_LED_SET				0x53
-#define SIGNALYZER_COMMAND_ADC					0x54
-#define SIGNALYZER_COMMAND_GPIO_STATE			0x55
-#define SIGNALYZER_COMMAND_GPIO_MODE			0x56
-#define SIGNALYZER_COMMAND_GPIO_PORT			0x57
-#define SIGNALYZER_COMMAND_I2C					0x58
-
-#define SIGNALYZER_CHAN_A						1
-#define SIGNALYZER_CHAN_B						2
-/* LEDS use channel C */
-#define SIGNALYZER_CHAN_C						4
-
-#define SIGNALYZER_LED_GREEN					1
-#define SIGNALYZER_LED_RED						2
-
-#define SIGNALYZER_MODULE_TYPE_EM_LT16_A		0x0301
-#define SIGNALYZER_MODULE_TYPE_EM_ARM_JTAG		0x0302
-#define SIGNALYZER_MODULE_TYPE_EM_JTAG			0x0303
-#define SIGNALYZER_MODULE_TYPE_EM_ARM_JTAG_P	0x0304
-#define SIGNALYZER_MODULE_TYPE_EM_JTAG_P		0x0305
-
-
-static int signalyzer_h_ctrl_write(int address, unsigned short value)
-{
-#if BUILD_FT2232_FTD2XX == 1
-	return FT_WriteEE(ftdih, address, value);
-#elif BUILD_FT2232_LIBFTDI == 1
-	return 0;
-#endif
-}
-
-#if BUILD_FT2232_FTD2XX == 1
-static int signalyzer_h_ctrl_read(int address, unsigned short *value)
-{
-	return FT_ReadEE(ftdih, address, value);
-}
-#endif
-
-static int signalyzer_h_led_set(unsigned char channel, unsigned char led,
-	int on_time_ms, int off_time_ms, unsigned char cycles)
-{
-	unsigned char on_time;
-	unsigned char off_time;
-
-	if (on_time_ms < 0xFFFF)
-		on_time = (unsigned char)(on_time_ms / 62);
-	else
-		on_time = 0xFF;
-
-	off_time = (unsigned char)(off_time_ms / 62);
-
-#if BUILD_FT2232_FTD2XX == 1
-	FT_STATUS status;
-
-	status = signalyzer_h_ctrl_write(SIGNALYZER_DATA_BUFFER_ADDR,
-			((uint32_t)(channel << 8) | led));
-	if (status != FT_OK) {
-		LOG_ERROR("signalyzer_h_ctrl_write  returned: %s",
-			ftd2xx_status_string(status));
-		return ERROR_JTAG_DEVICE_ERROR;
-	}
-
-	status = signalyzer_h_ctrl_write((SIGNALYZER_DATA_BUFFER_ADDR + 1),
-			((uint32_t)(on_time << 8) | off_time));
-	if (status != FT_OK) {
-		LOG_ERROR("signalyzer_h_ctrl_write  returned: %s",
-			ftd2xx_status_string(status));
-		return ERROR_JTAG_DEVICE_ERROR;
-	}
-
-	status = signalyzer_h_ctrl_write((SIGNALYZER_DATA_BUFFER_ADDR + 2),
-			((uint32_t)cycles));
-	if (status != FT_OK) {
-		LOG_ERROR("signalyzer_h_ctrl_write  returned: %s",
-			ftd2xx_status_string(status));
-		return ERROR_JTAG_DEVICE_ERROR;
-	}
-
-	status = signalyzer_h_ctrl_write(SIGNALYZER_COMMAND_ADDR,
-				SIGNALYZER_COMMAND_LED_SET);
-	if (status != FT_OK) {
-		LOG_ERROR("signalyzer_h_ctrl_write  returned: %s",
-			ftd2xx_status_string(status));
-		return ERROR_JTAG_DEVICE_ERROR;
-	}
-
-	return ERROR_OK;
-#elif BUILD_FT2232_LIBFTDI == 1
-	int retval;
-
-	retval = signalyzer_h_ctrl_write(SIGNALYZER_DATA_BUFFER_ADDR,
-				((uint32_t)(channel << 8) | led));
-	if (retval < 0) {
-		LOG_ERROR("signalyzer_h_ctrl_write returned: %s",
-			ftdi_get_error_string(&ftdic));
-		return ERROR_JTAG_DEVICE_ERROR;
-	}
-
-	retval = signalyzer_h_ctrl_write((SIGNALYZER_DATA_BUFFER_ADDR + 1),
-			((uint32_t)(on_time << 8) | off_time));
-	if (retval < 0) {
-		LOG_ERROR("signalyzer_h_ctrl_write returned: %s",
-			ftdi_get_error_string(&ftdic));
-		return ERROR_JTAG_DEVICE_ERROR;
-	}
-
-	retval = signalyzer_h_ctrl_write((SIGNALYZER_DATA_BUFFER_ADDR + 2),
-			(uint32_t)cycles);
-	if (retval < 0) {
-		LOG_ERROR("signalyzer_h_ctrl_write returned: %s",
-			ftdi_get_error_string(&ftdic));
-		return ERROR_JTAG_DEVICE_ERROR;
-	}
-
-	retval = signalyzer_h_ctrl_write(SIGNALYZER_COMMAND_ADDR,
-			SIGNALYZER_COMMAND_LED_SET);
-	if (retval < 0) {
-		LOG_ERROR("signalyzer_h_ctrl_write returned: %s",
-			ftdi_get_error_string(&ftdic));
-		return ERROR_JTAG_DEVICE_ERROR;
-	}
-
-	return ERROR_OK;
-#endif
-}
-
-static int signalyzer_h_init(void)
-{
-#if BUILD_FT2232_FTD2XX == 1
-	FT_STATUS status;
-	int i;
-#endif
-
-	char *end_of_desc;
-
-	uint16_t read_buf[12] = { 0 };
-
-	/* turn on center green led */
-	signalyzer_h_led_set(SIGNALYZER_CHAN_C, SIGNALYZER_LED_GREEN,
-		0xFFFF, 0x00, 0x00);
-
-	/* determine what channel config wants to open
-	 * TODO: change me... current implementation is made to work
-	 * with openocd description parsing.
-	 */
-	end_of_desc = strrchr(ft2232_device_desc, 0x00);
-
-	if (end_of_desc) {
-		signalyzer_h_side = *(end_of_desc - 1);
-		if (signalyzer_h_side == 'B')
-			signalyzer_h_side = SIGNALYZER_CHAN_B;
-		else
-			signalyzer_h_side = SIGNALYZER_CHAN_A;
-	} else {
-		LOG_ERROR("No Channel was specified");
-		return ERROR_FAIL;
-	}
-
-	signalyzer_h_led_set(signalyzer_h_side, SIGNALYZER_LED_GREEN,
-		1000, 1000, 0xFF);
-
-#if BUILD_FT2232_FTD2XX == 1
-	/* read signalyzer versionining information */
-	status = signalyzer_h_ctrl_write(SIGNALYZER_COMMAND_ADDR,
-			SIGNALYZER_COMMAND_VERSION);
-	if (status != FT_OK) {
-		LOG_ERROR("signalyzer_h_ctrl_write returned: %s",
-			ftd2xx_status_string(status));
-		return ERROR_JTAG_DEVICE_ERROR;
-	}
-
-	for (i = 0; i < 10; i++) {
-		status = signalyzer_h_ctrl_read((SIGNALYZER_DATA_BUFFER_ADDR + i),
-				&read_buf[i]);
-		if (status != FT_OK) {
-			LOG_ERROR("signalyzer_h_ctrl_read returned: %s",
-				ftd2xx_status_string(status));
-			return ERROR_JTAG_DEVICE_ERROR;
-		}
-	}
-
-	LOG_INFO("Signalyzer: ID info: { %.4x %.4x %.4x %.4x %.4x %.4x %.4x }",
-		read_buf[0], read_buf[1], read_buf[2], read_buf[3],
-		read_buf[4], read_buf[5], read_buf[6]);
-
-	/* set gpio register */
-	status = signalyzer_h_ctrl_write(SIGNALYZER_DATA_BUFFER_ADDR,
-			(uint32_t)(signalyzer_h_side << 8));
-	if (status != FT_OK) {
-		LOG_ERROR("signalyzer_h_ctrl_write returned: %s",
-			ftd2xx_status_string(status));
-		return ERROR_JTAG_DEVICE_ERROR;
-	}
-
-	status = signalyzer_h_ctrl_write(SIGNALYZER_DATA_BUFFER_ADDR + 1, 0x0404);
-	if (status != FT_OK) {
-		LOG_ERROR("signalyzer_h_ctrl_write returned: %s",
-			ftd2xx_status_string(status));
-		return ERROR_JTAG_DEVICE_ERROR;
-	}
-
-	status = signalyzer_h_ctrl_write(SIGNALYZER_COMMAND_ADDR,
-			SIGNALYZER_COMMAND_GPIO_STATE);
-	if (status != FT_OK) {
-		LOG_ERROR("signalyzer_h_ctrl_write returned: %s",
-			ftd2xx_status_string(status));
-		return ERROR_JTAG_DEVICE_ERROR;
-	}
-
-	/* read adapter type information */
-	status = signalyzer_h_ctrl_write(SIGNALYZER_DATA_BUFFER_ADDR,
-			((uint32_t)(signalyzer_h_side << 8) | 0x01));
-	if (status != FT_OK) {
-		LOG_ERROR("signalyzer_h_ctrl_write returned: %s",
-			ftd2xx_status_string(status));
-		return ERROR_JTAG_DEVICE_ERROR;
-	}
-
-	status = signalyzer_h_ctrl_write(
-			(SIGNALYZER_DATA_BUFFER_ADDR + 1), 0xA000);
-	if (status != FT_OK) {
-		LOG_ERROR("signalyzer_h_ctrl_write returned: %s",
-			ftd2xx_status_string(status));
-		return ERROR_JTAG_DEVICE_ERROR;
-	}
-
-	status = signalyzer_h_ctrl_write(
-			(SIGNALYZER_DATA_BUFFER_ADDR + 2), 0x0008);
-	if (status != FT_OK) {
-		LOG_ERROR("signalyzer_h_ctrl_write returned: %s",
-			ftd2xx_status_string(status));
-		return ERROR_JTAG_DEVICE_ERROR;
-	}
-
-	status = signalyzer_h_ctrl_write(SIGNALYZER_COMMAND_ADDR,
-			SIGNALYZER_COMMAND_I2C);
-	if (status != FT_OK) {
-		LOG_ERROR("signalyzer_h_ctrl_write returned: %s",
-			ftd2xx_status_string(status));
-		return ERROR_JTAG_DEVICE_ERROR;
-	}
-
-	usleep(100000);
-
-	status = signalyzer_h_ctrl_read(SIGNALYZER_COMMAND_ADDR, &read_buf[0]);
-	if (status != FT_OK) {
-		LOG_ERROR("signalyzer_h_ctrl_read returned: %s",
-			ftd2xx_status_string(status));
-		return ERROR_JTAG_DEVICE_ERROR;
-	}
-
-	if (read_buf[0] != 0x0498)
-		signalyzer_h_adapter_type = 0x0000;
-	else {
-		for (i = 0; i < 4; i++) {
-			status = signalyzer_h_ctrl_read((SIGNALYZER_DATA_BUFFER_ADDR + i), &read_buf[i]);
-			if (status != FT_OK) {
-				LOG_ERROR("signalyzer_h_ctrl_read returned: %s",
-					ftd2xx_status_string(status));
-				return ERROR_JTAG_DEVICE_ERROR;
-			}
-		}
-
-		signalyzer_h_adapter_type = read_buf[0];
-	}
-
-#elif BUILD_FT2232_LIBFTDI == 1
-	/* currently libftdi does not allow reading individual eeprom
-	 * locations, therefore adapter type cannot be detected.
-	 * override with most common type
-	 */
-	signalyzer_h_adapter_type = SIGNALYZER_MODULE_TYPE_EM_ARM_JTAG;
-#endif
-
-	enum reset_types jtag_reset_config = jtag_get_reset_config();
-
-	/* ADAPTOR: EM_LT16_A */
-	if (signalyzer_h_adapter_type == SIGNALYZER_MODULE_TYPE_EM_LT16_A) {
-		LOG_INFO("Signalyzer: EM-LT (16-channel level translator) "
-			"detected. (HW: %2x).", (read_buf[1] >> 8));
-
-		nTRST    = 0x10;
-		nTRSTnOE = 0x10;
-		nSRST    = 0x20;
-		nSRSTnOE = 0x20;
-
-		low_output     = 0x08;
-		low_direction  = 0x1b;
-
-		high_output    = 0x0;
-		high_direction = 0x0;
-
-		if (jtag_reset_config & RESET_TRST_OPEN_DRAIN) {
-			low_direction &= ~nTRSTnOE;	/* nTRST input */
-			low_output    &= ~nTRST;	/* nTRST = 0 */
-		} else {
-			low_direction |= nTRSTnOE;	/* nTRST output */
-			low_output    |= nTRST;		/* nTRST = 1 */
-		}
-
-		if (jtag_reset_config & RESET_SRST_PUSH_PULL) {
-			low_direction |= nSRSTnOE;	/* nSRST output */
-			low_output    |= nSRST;		/* nSRST = 1 */
-		} else {
-			low_direction &= ~nSRSTnOE;	/* nSRST input */
-			low_output    &= ~nSRST;	/* nSRST = 0 */
-		}
-
-#if BUILD_FT2232_FTD2XX == 1
-		/* enable power to the module */
-		status = signalyzer_h_ctrl_write(SIGNALYZER_DATA_BUFFER_ADDR,
-				((uint32_t)(signalyzer_h_side << 8) | 0x01));
-		if (status != FT_OK) {
-			LOG_ERROR("signalyzer_h_ctrl_write returned: %s",
-				ftd2xx_status_string(status));
-			return ERROR_JTAG_DEVICE_ERROR;
-		}
-
-		status = signalyzer_h_ctrl_write(SIGNALYZER_COMMAND_ADDR,
-				SIGNALYZER_COMMAND_POWERCONTROL_SET);
-		if (status != FT_OK) {
-			LOG_ERROR("signalyzer_h_ctrl_write returned: %s",
-				ftd2xx_status_string(status));
-			return ERROR_JTAG_DEVICE_ERROR;
-		}
-
-		/* set gpio mode register */
-		status = signalyzer_h_ctrl_write(SIGNALYZER_DATA_BUFFER_ADDR,
-				(uint32_t)(signalyzer_h_side << 8));
-		if (status != FT_OK) {
-			LOG_ERROR("signalyzer_h_ctrl_write returned: %s",
-				ftd2xx_status_string(status));
-			return ERROR_JTAG_DEVICE_ERROR;
-		}
-
-		status = signalyzer_h_ctrl_write(SIGNALYZER_DATA_BUFFER_ADDR + 1, 0x0000);
-		if (status != FT_OK) {
-			LOG_ERROR("signalyzer_h_ctrl_write returned: %s",
-				ftd2xx_status_string(status));
-			return ERROR_JTAG_DEVICE_ERROR;
-		}
-
-		status = signalyzer_h_ctrl_write(SIGNALYZER_COMMAND_ADDR, SIGNALYZER_COMMAND_GPIO_MODE);
-		if (status != FT_OK) {
-			LOG_ERROR("signalyzer_h_ctrl_write returned: %s",
-				ftd2xx_status_string(status));
-			return ERROR_JTAG_DEVICE_ERROR;
-		}
-
-		/* set gpio register */
-		status = signalyzer_h_ctrl_write(SIGNALYZER_DATA_BUFFER_ADDR,
-				(uint32_t)(signalyzer_h_side << 8));
-		if (status != FT_OK) {
-			LOG_ERROR("signalyzer_h_ctrl_write returned: %s",
-				ftd2xx_status_string(status));
-			return ERROR_JTAG_DEVICE_ERROR;
-		}
-
-		status = signalyzer_h_ctrl_write(SIGNALYZER_DATA_BUFFER_ADDR + 1, 0x4040);
-		if (status != FT_OK) {
-			LOG_ERROR("signalyzer_h_ctrl_write returned: %s",
-				ftd2xx_status_string(status));
-			return ERROR_JTAG_DEVICE_ERROR;
-		}
-
-		status = signalyzer_h_ctrl_write(SIGNALYZER_COMMAND_ADDR,
-				SIGNALYZER_COMMAND_GPIO_STATE);
-		if (status != FT_OK) {
-			LOG_ERROR("signalyzer_h_ctrl_write returned: %s",
-				ftd2xx_status_string(status));
-			return ERROR_JTAG_DEVICE_ERROR;
-		}
-#endif
-	}
-	/* ADAPTOR: EM_ARM_JTAG, EM_ARM_JTAG_P, EM_JTAG, EM_JTAG_P */
-	else if ((signalyzer_h_adapter_type == SIGNALYZER_MODULE_TYPE_EM_ARM_JTAG) ||
-		 (signalyzer_h_adapter_type == SIGNALYZER_MODULE_TYPE_EM_ARM_JTAG_P) ||
-		 (signalyzer_h_adapter_type == SIGNALYZER_MODULE_TYPE_EM_JTAG)  ||
-		 (signalyzer_h_adapter_type == SIGNALYZER_MODULE_TYPE_EM_JTAG_P)) {
-		if (signalyzer_h_adapter_type
-		    == SIGNALYZER_MODULE_TYPE_EM_ARM_JTAG)
-			LOG_INFO("Signalyzer: EM-ARM-JTAG (ARM JTAG) "
-				"detected. (HW: %2x).", (read_buf[1] >> 8));
-		else if (signalyzer_h_adapter_type
-			 == SIGNALYZER_MODULE_TYPE_EM_ARM_JTAG_P)
-			LOG_INFO("Signalyzer: EM-ARM-JTAG_P "
-				"(ARM JTAG with PSU) detected. (HW: %2x).",
-				(read_buf[1] >> 8));
-		else if (signalyzer_h_adapter_type
-			 == SIGNALYZER_MODULE_TYPE_EM_JTAG)
-			LOG_INFO("Signalyzer: EM-JTAG (Generic JTAG) "
-				"detected. (HW: %2x).", (read_buf[1] >> 8));
-		else if (signalyzer_h_adapter_type
-			 == SIGNALYZER_MODULE_TYPE_EM_JTAG_P)
-			LOG_INFO("Signalyzer: EM-JTAG-P "
-				"(Generic JTAG with PSU) detected. (HW: %2x).",
-				(read_buf[1] >> 8));
-
-		nTRST          = 0x02;
-		nTRSTnOE       = 0x04;
-		nSRST          = 0x08;
-		nSRSTnOE       = 0x10;
-
-		low_output     = 0x08;
-		low_direction  = 0x1b;
-
-		high_output    = 0x0;
-		high_direction = 0x1f;
-
-		if (jtag_reset_config & RESET_TRST_OPEN_DRAIN) {
-			high_output |= nTRSTnOE;
-			high_output &= ~nTRST;
-		} else {
-			high_output &= ~nTRSTnOE;
-			high_output |= nTRST;
-		}
-
-		if (jtag_reset_config & RESET_SRST_PUSH_PULL) {
-			high_output &= ~nSRSTnOE;
-			high_output |= nSRST;
-		} else {
-			high_output |= nSRSTnOE;
-			high_output &= ~nSRST;
-		}
-
-#if BUILD_FT2232_FTD2XX == 1
-		/* enable power to the module */
-		status = signalyzer_h_ctrl_write(SIGNALYZER_DATA_BUFFER_ADDR,
-				((uint32_t)(signalyzer_h_side << 8) | 0x01));
-		if (status != FT_OK) {
-			LOG_ERROR("signalyzer_h_ctrl_write returned: %s",
-				ftd2xx_status_string(status));
-			return ERROR_JTAG_DEVICE_ERROR;
-		}
-
-		status = signalyzer_h_ctrl_write(SIGNALYZER_COMMAND_ADDR,
-				SIGNALYZER_COMMAND_POWERCONTROL_SET);
-		if (status != FT_OK) {
-			LOG_ERROR("signalyzer_h_ctrl_write returned: %s",
-				ftd2xx_status_string(status));
-			return ERROR_JTAG_DEVICE_ERROR;
-		}
-
-		/* set gpio mode register (IO_16 and IO_17 set as analog
-		 * inputs, other is gpio)
-		 */
-		status = signalyzer_h_ctrl_write(SIGNALYZER_DATA_BUFFER_ADDR,
-				(uint32_t)(signalyzer_h_side << 8));
-		if (status != FT_OK) {
-			LOG_ERROR("signalyzer_h_ctrl_write returned: %s",
-				ftd2xx_status_string(status));
-			return ERROR_JTAG_DEVICE_ERROR;
-		}
-
-		status = signalyzer_h_ctrl_write(SIGNALYZER_DATA_BUFFER_ADDR + 1, 0x0060);
-		if (status != FT_OK) {
-			LOG_ERROR("signalyzer_h_ctrl_write returned: %s",
-				ftd2xx_status_string(status));
-			return ERROR_JTAG_DEVICE_ERROR;
-		}
-
-		status = signalyzer_h_ctrl_write(SIGNALYZER_COMMAND_ADDR, SIGNALYZER_COMMAND_GPIO_MODE);
-		if (status != FT_OK) {
-			LOG_ERROR("signalyzer_h_ctrl_write returned: %s",
-				ftd2xx_status_string(status));
-			return ERROR_JTAG_DEVICE_ERROR;
-		}
-
-		/* set gpio register (all inputs, for -P modules,
-		 * PSU will be turned off)
-		 */
-		status = signalyzer_h_ctrl_write(SIGNALYZER_DATA_BUFFER_ADDR,
-				(uint32_t)(signalyzer_h_side << 8));
-		if (status != FT_OK) {
-			LOG_ERROR("signalyzer_h_ctrl_write returned: %s",
-				ftd2xx_status_string(status));
-			return ERROR_JTAG_DEVICE_ERROR;
-		}
-
-		status = signalyzer_h_ctrl_write(SIGNALYZER_DATA_BUFFER_ADDR + 1, 0x0000);
-		if (status != FT_OK) {
-			LOG_ERROR("signalyzer_h_ctrl_write returned: %s",
-				ftd2xx_status_string(status));
-			return ERROR_JTAG_DEVICE_ERROR;
-		}
-
-		status = signalyzer_h_ctrl_write(SIGNALYZER_COMMAND_ADDR, SIGNALYZER_COMMAND_GPIO_STATE);
-		if (status != FT_OK) {
-			LOG_ERROR("signalyzer_h_ctrl_write returned: %s",
-				ftd2xx_status_string(status));
-			return ERROR_JTAG_DEVICE_ERROR;
-		}
-#endif
-	} else if (signalyzer_h_adapter_type == 0x0000) {
-		LOG_INFO("Signalyzer: No external modules were detected.");
-
-		nTRST    = 0x10;
-		nTRSTnOE = 0x10;
-		nSRST    = 0x20;
-		nSRSTnOE = 0x20;
-
-		low_output     = 0x08;
-		low_direction  = 0x1b;
-
-		high_output    = 0x0;
-		high_direction = 0x0;
-
-		if (jtag_reset_config & RESET_TRST_OPEN_DRAIN) {
-			low_direction &= ~nTRSTnOE;	/* nTRST input */
-			low_output    &= ~nTRST;	/* nTRST = 0 */
-		} else {
-			low_direction |= nTRSTnOE;	/* nTRST output */
-			low_output    |= nTRST;		/* nTRST = 1 */
-		}
-
-		if (jtag_reset_config & RESET_SRST_PUSH_PULL) {
-			low_direction |= nSRSTnOE;	/* nSRST output */
-			low_output    |= nSRST;		/* nSRST = 1 */
-		} else {
-			low_direction &= ~nSRSTnOE;	/* nSRST input */
-			low_output    &= ~nSRST;	/* nSRST = 0 */
-		}
-	} else {
-		LOG_ERROR("Unknown module type is detected: %.4x",
-			signalyzer_h_adapter_type);
-		return ERROR_JTAG_DEVICE_ERROR;
-	}
-
-	/* initialize low byte of controller for jtag operation */
-	if (ft2232_set_data_bits_low_byte(low_output, low_direction) != ERROR_OK) {
-		LOG_ERROR("couldn't initialize Signalyzer-H layout");
-		return ERROR_JTAG_INIT_FAILED;
-	}
-
-#if BUILD_FT2232_FTD2XX == 1
-	if (ftdi_device == FT_DEVICE_2232H) {
-		/* initialize high byte of controller for jtag operation */
-		if (ft2232_set_data_bits_high_byte(high_output, high_direction) != ERROR_OK) {
-			LOG_ERROR("couldn't initialize Signalyzer-H layout");
-			return ERROR_JTAG_INIT_FAILED;
-		}
-	}
-#elif BUILD_FT2232_LIBFTDI == 1
-	if (ftdi_device == TYPE_2232H) {
-		/* initialize high byte of controller for jtag operation */
-		if (ft2232_set_data_bits_high_byte(high_output, high_direction) != ERROR_OK) {
-			LOG_ERROR("couldn't initialize Signalyzer-H layout");
-			return ERROR_JTAG_INIT_FAILED;
-		}
-	}
-#endif
-	return ERROR_OK;
-}
-
-static void signalyzer_h_reset(int trst, int srst)
-{
-	enum reset_types jtag_reset_config = jtag_get_reset_config();
-
-	/* ADAPTOR: EM_LT16_A */
-	if (signalyzer_h_adapter_type == SIGNALYZER_MODULE_TYPE_EM_LT16_A) {
-		if (trst == 1) {
-			if (jtag_reset_config & RESET_TRST_OPEN_DRAIN)
-				/* switch to output pin (output is low) */
-				low_direction |= nTRSTnOE;
-			else
-				/* switch output low */
-				low_output &= ~nTRST;
-		} else if (trst == 0) {
-			if (jtag_reset_config & RESET_TRST_OPEN_DRAIN)
-				/* switch to input pin (high-Z + internal
-				 * and external pullup) */
-				low_direction &= ~nTRSTnOE;
-			else
-				/* switch output high */
-				low_output |= nTRST;
-		}
-
-		if (srst == 1) {
-			if (jtag_reset_config & RESET_SRST_PUSH_PULL)
-				/* switch output low */
-				low_output &= ~nSRST;
-			else
-				/* switch to output pin (output is low) */
-				low_direction |= nSRSTnOE;
-		} else if (srst == 0) {
-			if (jtag_reset_config & RESET_SRST_PUSH_PULL)
-				/* switch output high */
-				low_output |= nSRST;
-			else
-				/* switch to input pin (high-Z) */
-				low_direction &= ~nSRSTnOE;
-		}
-
-		/* command "set data bits low byte" */
-		buffer_write(0x80);
-		buffer_write(low_output);
-		buffer_write(low_direction);
-		LOG_DEBUG("trst: %i, srst: %i, low_output: 0x%2.2x, "
-			"low_direction: 0x%2.2x",
-			trst, srst, low_output, low_direction);
-	}
-	/* ADAPTOR: EM_ARM_JTAG,  EM_ARM_JTAG_P, EM_JTAG, EM_JTAG_P */
-	else if ((signalyzer_h_adapter_type == SIGNALYZER_MODULE_TYPE_EM_ARM_JTAG) ||
-		 (signalyzer_h_adapter_type == SIGNALYZER_MODULE_TYPE_EM_ARM_JTAG_P) ||
-		 (signalyzer_h_adapter_type == SIGNALYZER_MODULE_TYPE_EM_JTAG)  ||
-		 (signalyzer_h_adapter_type == SIGNALYZER_MODULE_TYPE_EM_JTAG_P)) {
-		if (trst == 1) {
-			if (jtag_reset_config & RESET_TRST_OPEN_DRAIN)
-				high_output &= ~nTRSTnOE;
-			else
-				high_output &= ~nTRST;
-		} else if (trst == 0) {
-			if (jtag_reset_config & RESET_TRST_OPEN_DRAIN)
-				high_output |= nTRSTnOE;
-			else
-				high_output |= nTRST;
-		}
-
-		if (srst == 1) {
-			if (jtag_reset_config & RESET_SRST_PUSH_PULL)
-				high_output &= ~nSRST;
-			else
-				high_output &= ~nSRSTnOE;
-		} else if (srst == 0) {
-			if (jtag_reset_config & RESET_SRST_PUSH_PULL)
-				high_output |= nSRST;
-			else
-				high_output |= nSRSTnOE;
-		}
-
-		/* command "set data bits high byte" */
-		buffer_write(0x82);
-		buffer_write(high_output);
-		buffer_write(high_direction);
-		LOG_INFO("trst: %i, srst: %i, high_output: 0x%2.2x, "
-			"high_direction: 0x%2.2x",
-			trst, srst, high_output, high_direction);
-	} else if (signalyzer_h_adapter_type == 0x0000) {
-		if (trst == 1) {
-			if (jtag_reset_config & RESET_TRST_OPEN_DRAIN)
-				/* switch to output pin (output is low) */
-				low_direction |= nTRSTnOE;
-			else
-				/* switch output low */
-				low_output &= ~nTRST;
-		} else if (trst == 0) {
-			if (jtag_reset_config & RESET_TRST_OPEN_DRAIN)
-				/* switch to input pin (high-Z + internal
-				 * and external pullup) */
-				low_direction &= ~nTRSTnOE;
-			else
-				/* switch output high */
-				low_output |= nTRST;
-		}
-
-		if (srst == 1) {
-			if (jtag_reset_config & RESET_SRST_PUSH_PULL)
-				/* switch output low */
-				low_output &= ~nSRST;
-			else
-				/* switch to output pin (output is low) */
-				low_direction |= nSRSTnOE;
-		} else if (srst == 0) {
-			if (jtag_reset_config & RESET_SRST_PUSH_PULL)
-				/* switch output high */
-				low_output |= nSRST;
-			else
-				/* switch to input pin (high-Z) */
-				low_direction &= ~nSRSTnOE;
-		}
-
-		/* command "set data bits low byte" */
-		buffer_write(0x80);
-		buffer_write(low_output);
-		buffer_write(low_direction);
-		LOG_DEBUG("trst: %i, srst: %i, low_output: 0x%2.2x, "
-			"low_direction: 0x%2.2x",
-			trst, srst, low_output, low_direction);
-	}
-}
-
-static void signalyzer_h_blink(void)
-{
-	signalyzer_h_led_set(signalyzer_h_side, SIGNALYZER_LED_RED, 100, 0, 1);
-}
-
-/********************************************************************
- * Support for KT-LINK
- * JTAG adapter from KRISTECH
- * http://www.kristech.eu
- *******************************************************************/
-static int ktlink_init(void)
-{
-	uint8_t swd_en = 0x20;	/* 0x20 SWD disable, 0x00 SWD enable (ADBUS5) */
-
-	low_output    = 0x08 | swd_en;	/* value; TMS=1,TCK=0,TDI=0,SWD=swd_en */
-	low_direction = 0x3B;		/* out=1; TCK/TDI/TMS=out,TDO=in,SWD=out,RTCK=in,SRSTIN=in */
-
-	/* initialize low byte for jtag */
-	if (ft2232_set_data_bits_low_byte(low_output, low_direction) != ERROR_OK) {
-		LOG_ERROR("couldn't initialize FT2232 with 'ktlink' layout");
-		return ERROR_JTAG_INIT_FAILED;
-	}
-
-	nTRST    = 0x01;
-	nSRST    = 0x02;
-	nTRSTnOE = 0x04;
-	nSRSTnOE = 0x08;
-
-	high_output    = 0x80;	/* turn LED on */
-	high_direction = 0xFF;	/* all outputs */
-
-	enum reset_types jtag_reset_config = jtag_get_reset_config();
-
-	if (jtag_reset_config & RESET_TRST_OPEN_DRAIN) {
-		high_output |= nTRSTnOE;
-		high_output &= ~nTRST;
-	} else {
-		high_output &= ~nTRSTnOE;
-		high_output |= nTRST;
-	}
-
-	if (jtag_reset_config & RESET_SRST_PUSH_PULL) {
-		high_output &= ~nSRSTnOE;
-		high_output |= nSRST;
-	} else {
-		high_output |= nSRSTnOE;
-		high_output &= ~nSRST;
-	}
-
-	/* initialize high byte for jtag */
-	if (ft2232_set_data_bits_high_byte(high_output, high_direction) != ERROR_OK) {
-		LOG_ERROR("couldn't initialize FT2232 with 'ktlink' layout");
-		return ERROR_JTAG_INIT_FAILED;
-	}
-
-	return ERROR_OK;
-}
-
-static void ktlink_reset(int trst, int srst)
-{
-	enum reset_types jtag_reset_config = jtag_get_reset_config();
-
-	if (trst == 1) {
-		if (jtag_reset_config & RESET_TRST_OPEN_DRAIN)
-			high_output &= ~nTRSTnOE;
-		else
-			high_output &= ~nTRST;
-	} else if (trst == 0) {
-		if (jtag_reset_config & RESET_TRST_OPEN_DRAIN)
-			high_output |= nTRSTnOE;
-		else
-			high_output |= nTRST;
-	}
-
-	if (srst == 1) {
-		if (jtag_reset_config & RESET_SRST_PUSH_PULL)
-			high_output &= ~nSRST;
-		else
-			high_output &= ~nSRSTnOE;
-	} else if (srst == 0) {
-		if (jtag_reset_config & RESET_SRST_PUSH_PULL)
-			high_output |= nSRST;
-		else
-			high_output |= nSRSTnOE;
-	}
-
-	buffer_write(0x82);	/* command "set data bits high byte" */
-	buffer_write(high_output);
-	buffer_write(high_direction);
-	LOG_DEBUG("trst: %i, srst: %i, high_output: 0x%2.2x, high_direction: 0x%2.2x",
-		trst,
-		srst,
-		high_output,
-		high_direction);
-}
-
-static void ktlink_blink(void)
-{
-	/* LED connected to ACBUS7 */
-	high_output ^= 0x80;
-
-	buffer_write(0x82);	/* command "set data bits high byte" */
-	buffer_write(high_output);
-	buffer_write(high_direction);
-}
-
-/********************************************************************
- * Support for Digilent HS-1
- * JTAG adapter from Digilent
- * http://www.digilent.com
- * Author: Stephane Bonnet bonnetst@hds.utc.fr
- *******************************************************************/
-
-static int digilent_hs1_init(void)
-{
-	/* the adapter only supports the base JTAG signals, no nTRST
-	   nor nSRST */
-	low_output	= 0x88;
-	low_direction	= 0x8b;
-
-	/* initialize low byte for jtag */
-	if (ft2232_set_data_bits_low_byte(low_output, low_direction) != ERROR_OK) {
-		LOG_ERROR("couldn't initialize FT2232 with 'digilent_hs1' layout");
-		return ERROR_JTAG_INIT_FAILED;
-	}
-	return ERROR_OK;
-}
-
-static void digilent_hs1_reset(int trst, int srst)
-{
-	/* Dummy function, no reset signals supported. */
-}
-
-static const struct command_registration ft2232_command_handlers[] = {
-	{
-		.name = "ft2232_device_desc",
-		.handler = &ft2232_handle_device_desc_command,
-		.mode = COMMAND_CONFIG,
-		.help = "set the USB device description of the FTDI FT2232 device",
-		.usage = "description_string",
-	},
-	{
-		.name = "ft2232_serial",
-		.handler = &ft2232_handle_serial_command,
-		.mode = COMMAND_CONFIG,
-		.help = "set the serial number of the FTDI FT2232 device",
-		.usage = "serial_string",
-	},
-	{
-		.name = "ft2232_layout",
-		.handler = &ft2232_handle_layout_command,
-		.mode = COMMAND_CONFIG,
-		.help = "set the layout of the FT2232 GPIO signals used "
-			"to control output-enables and reset signals",
-		.usage = "layout_name",
-	},
-	{
-		.name = "ft2232_vid_pid",
-		.handler = &ft2232_handle_vid_pid_command,
-		.mode = COMMAND_CONFIG,
-		.help = "the vendor ID and product ID of the FTDI FT2232 device",
-		.usage = "(vid pid)* ",
-	},
-	{
-		.name = "ft2232_latency",
-		.handler = &ft2232_handle_latency_command,
-		.mode = COMMAND_CONFIG,
-		.help = "set the FT2232 latency timer to a new value",
-		.usage = "value",
-	},
-	{
-		.name = "ft2232_channel",
-		.handler = &ft2232_handle_channel_command,
-		.mode = COMMAND_CONFIG,
-		.help = "set the FT2232 channel to a new value",
-		.usage = "value",
-	},
-	COMMAND_REGISTRATION_DONE
-};
-
-struct jtag_interface ft2232_interface = {
-	.name = "ft2232",
-	.supported = DEBUG_CAP_TMS_SEQ,
-	.commands = ft2232_command_handlers,
-	.transports = jtag_only,
-
-	.init = ft2232_init,
-	.quit = ft2232_quit,
-	.speed = ft2232_speed,
-	.speed_div = ft2232_speed_div,
-	.khz = ft2232_khz,
-	.execute_queue = ft2232_execute_queue,
-};
diff --git a/src/jtag/drivers/ftd2xx_common.h b/src/jtag/drivers/ftd2xx_common.h
deleted file mode 100644
index 14b8455..0000000
--- a/src/jtag/drivers/ftd2xx_common.h
+++ /dev/null
@@ -1,55 +0,0 @@
-/***************************************************************************
- *   Copyright (C) 2011 by Spencer Oliver <spen@spen-soft.co.uk>           *
- *                                                                         *
- *   Written by Arnim Laeuger, 2008 (from urjtag)                          *
- *                                                                         *
- *   This program is free software; you can redistribute it and/or modify  *
- *   it under the terms of the GNU General Public License as published by  *
- *   the Free Software Foundation; either version 2 of the License, or     *
- *   (at your option) any later version.                                   *
- *                                                                         *
- *   This program is distributed in the hope that it will be useful,       *
- *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
- *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
- *   GNU General Public License for more details.                          *
- *                                                                         *
- *   You should have received a copy of the GNU General Public License     *
- *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
- ***************************************************************************/
-
-#ifndef OPENOCD_JTAG_DRIVERS_FTD2XX_COMMON_H
-#define OPENOCD_JTAG_DRIVERS_FTD2XX_COMMON_H
-
-#if ((BUILD_FT2232_FTD2XX == 1) || (BUILD_PRESTO_FTD2XX == 1) || (BUILD_USB_BLASTER_FTD2XX == 1))
-#include <ftd2xx.h>
-
-static const char *ftd2xx_status_string(FT_STATUS status)
-{
-	switch (status) {
-		case FT_OK:							return "OK";
-		case FT_INVALID_HANDLE:				return "invalid handle";
-		case FT_DEVICE_NOT_FOUND:			return "device not found";
-		case FT_DEVICE_NOT_OPENED:			return "device not opened";
-		case FT_IO_ERROR:					return "io error";
-		case FT_INSUFFICIENT_RESOURCES:		return "insufficient resources";
-		case FT_INVALID_PARAMETER:			return "invalid parameter";
-		case FT_INVALID_BAUD_RATE:			return "invalid baud rate";
-
-		case FT_DEVICE_NOT_OPENED_FOR_ERASE: return "device not opened for erase";
-		case FT_DEVICE_NOT_OPENED_FOR_WRITE: return "device not opened for write";
-		case FT_FAILED_TO_WRITE_DEVICE:		return "failed to write device";
-		case FT_EEPROM_READ_FAILED:			return "eeprom read failed";
-		case FT_EEPROM_WRITE_FAILED:		return "eeprom write failed";
-		case FT_EEPROM_ERASE_FAILED:		return "eeprom erase failed";
-		case FT_EEPROM_NOT_PRESENT:			return "eeprom not present";
-		case FT_EEPROM_NOT_PROGRAMMED:		return "eeprom not programmed";
-		case FT_INVALID_ARGS:				return "invalid args";
-		case FT_NOT_SUPPORTED:				return "not supported";
-		case FT_OTHER_ERROR:				return "other error";
-	}
-
-	return "undefined FTD2xx error";
-}
-
-#endif
-#endif /* OPENOCD_JTAG_DRIVERS_FTD2XX_COMMON_H */
diff --git a/src/jtag/drivers/imx_gpio.c b/src/jtag/drivers/imx_gpio.c
new file mode 100644
index 0000000..f33d109
--- /dev/null
+++ b/src/jtag/drivers/imx_gpio.c
@@ -0,0 +1,552 @@
+/***************************************************************************
+ *   Copyright (C) 2017 by Grzegorz Kostka, kostka.grzegorz@gmail.com      *
+ *                                                                         *
+ *   Based on bcm2835gpio.c                                                 *
+ *                                                                         *
+ *   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 <jtag/interface.h>
+#include "bitbang.h"
+
+#include <sys/mman.h>
+
+#define IMX_GPIO_BASE 0x0209c000
+#define IMX_GPIO_SIZE 0x00004000
+#define IMX_GPIO_REGS_COUNT 8
+
+static uint32_t imx_gpio_peri_base = IMX_GPIO_BASE;
+
+struct imx_gpio_regs {
+	uint32_t dr;
+	uint32_t gdir;
+	uint32_t psr;
+	uint32_t icr1;
+	uint32_t icr2;
+	uint32_t imr;
+	uint32_t isr;
+	uint32_t edge_sel;
+} __attribute__((aligned(IMX_GPIO_SIZE)));
+
+static int dev_mem_fd;
+static volatile struct imx_gpio_regs *pio_base;
+
+/* GPIO setup functions */
+static inline bool gpio_mode_get(int g)
+{
+	return pio_base[g / 32].gdir >> (g & 0x1F) & 1;
+}
+
+static inline void gpio_mode_input_set(int g)
+{
+	pio_base[g / 32].gdir &=  ~(1u << (g & 0x1F));
+}
+
+static inline void gpio_mode_output_set(int g)
+{
+	pio_base[g / 32].gdir |=  (1u << (g & 0x1F));
+}
+
+static inline void gpio_mode_set(int g, int m)
+{
+	(m) ? gpio_mode_output_set(g) : gpio_mode_input_set(g);
+}
+
+static inline void gpio_set(int g)
+{
+	pio_base[g / 32].dr |=  (1u << (g & 0x1F));
+}
+
+static inline void gpio_clear(int g)
+{
+	pio_base[g / 32].dr &=  ~(1u << (g & 0x1F));
+}
+
+static inline bool gpio_level(int g)
+{
+	return pio_base[g / 32].dr >> (g & 0x1F) & 1;
+}
+
+static int imx_gpio_read(void);
+static void imx_gpio_write(int tck, int tms, int tdi);
+static void imx_gpio_reset(int trst, int srst);
+
+static int imx_gpio_swdio_read(void);
+static void imx_gpio_swdio_drive(bool is_output);
+
+static int imx_gpio_init(void);
+static int imx_gpio_quit(void);
+
+static struct bitbang_interface imx_gpio_bitbang = {
+	.read = imx_gpio_read,
+	.write = imx_gpio_write,
+	.reset = imx_gpio_reset,
+	.swdio_read = imx_gpio_swdio_read,
+	.swdio_drive = imx_gpio_swdio_drive,
+	.blink = NULL
+};
+
+/* GPIO numbers for each signal. Negative values are invalid */
+static int tck_gpio = -1;
+static int tck_gpio_mode;
+static int tms_gpio = -1;
+static int tms_gpio_mode;
+static int tdi_gpio = -1;
+static int tdi_gpio_mode;
+static int tdo_gpio = -1;
+static int tdo_gpio_mode;
+static int trst_gpio = -1;
+static int trst_gpio_mode;
+static int srst_gpio = -1;
+static int srst_gpio_mode;
+static int swclk_gpio = -1;
+static int swclk_gpio_mode;
+static int swdio_gpio = -1;
+static int swdio_gpio_mode;
+
+/* Transition delay coefficients. Tuned for IMX6UL 528MHz. Adjusted
+ * experimentally for:10kHz, 100Khz, 500KHz. Speeds above 800Khz are impossible
+ * to reach via memory mapped method (at least for IMX6UL@528MHz).
+ * Measured mmap raw GPIO toggling speed on IMX6UL@528MHz: 1.3MHz.
+ */
+static int speed_coeff = 50000;
+static int speed_offset = 100;
+static unsigned int jtag_delay;
+
+static int imx_gpio_read(void)
+{
+	return gpio_level(tdo_gpio);
+}
+
+static void imx_gpio_write(int tck, int tms, int tdi)
+{
+	tms ? gpio_set(tms_gpio) : gpio_clear(tms_gpio);
+	tdi ? gpio_set(tdi_gpio) : gpio_clear(tdi_gpio);
+	tck ? gpio_set(tck_gpio) : gpio_clear(tck_gpio);
+
+	for (unsigned int i = 0; i < jtag_delay; i++)
+		asm volatile ("");
+}
+
+static void imx_gpio_swd_write(int tck, int tms, int tdi)
+{
+	tdi ? gpio_set(swdio_gpio) : gpio_clear(swdio_gpio);
+	tck ? gpio_set(swclk_gpio) : gpio_clear(swclk_gpio);
+
+	for (unsigned int i = 0; i < jtag_delay; i++)
+		asm volatile ("");
+}
+
+/* (1) assert or (0) deassert reset lines */
+static void imx_gpio_reset(int trst, int srst)
+{
+	if (trst_gpio != -1)
+		trst ? gpio_set(trst_gpio) : gpio_clear(trst_gpio);
+
+	if (srst_gpio != -1)
+		srst ? gpio_set(srst_gpio) : gpio_clear(srst_gpio);
+}
+
+static void imx_gpio_swdio_drive(bool is_output)
+{
+	if (is_output)
+		gpio_mode_output_set(swdio_gpio);
+	else
+		gpio_mode_input_set(swdio_gpio);
+}
+
+static int imx_gpio_swdio_read(void)
+{
+	return gpio_level(swdio_gpio);
+}
+
+static int imx_gpio_khz(int khz, int *jtag_speed)
+{
+	if (!khz) {
+		LOG_DEBUG("RCLK not supported");
+		return ERROR_FAIL;
+	}
+	*jtag_speed = speed_coeff/khz - speed_offset;
+	if (*jtag_speed < 0)
+		*jtag_speed = 0;
+	return ERROR_OK;
+}
+
+static int imx_gpio_speed_div(int speed, int *khz)
+{
+	*khz = speed_coeff/(speed + speed_offset);
+	return ERROR_OK;
+}
+
+static int imx_gpio_speed(int speed)
+{
+	jtag_delay = speed;
+	return ERROR_OK;
+}
+
+static int is_gpio_valid(int gpio)
+{
+	return gpio >= 0 && gpio < 32 * IMX_GPIO_REGS_COUNT;
+}
+
+COMMAND_HANDLER(imx_gpio_handle_jtag_gpionums)
+{
+	if (CMD_ARGC == 4) {
+		COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], tck_gpio);
+		COMMAND_PARSE_NUMBER(int, CMD_ARGV[1], tms_gpio);
+		COMMAND_PARSE_NUMBER(int, CMD_ARGV[2], tdi_gpio);
+		COMMAND_PARSE_NUMBER(int, CMD_ARGV[3], tdo_gpio);
+	} else if (CMD_ARGC != 0) {
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	command_print(CMD_CTX,
+			"imx_gpio GPIO config: tck = %d, tms = %d, tdi = %d, tdo = %d",
+			tck_gpio, tms_gpio, tdi_gpio, tdo_gpio);
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(imx_gpio_handle_jtag_gpionum_tck)
+{
+	if (CMD_ARGC == 1)
+		COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], tck_gpio);
+
+	command_print(CMD_CTX, "imx_gpio GPIO config: tck = %d", tck_gpio);
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(imx_gpio_handle_jtag_gpionum_tms)
+{
+	if (CMD_ARGC == 1)
+		COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], tms_gpio);
+
+	command_print(CMD_CTX, "imx_gpio GPIO config: tms = %d", tms_gpio);
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(imx_gpio_handle_jtag_gpionum_tdo)
+{
+	if (CMD_ARGC == 1)
+		COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], tdo_gpio);
+
+	command_print(CMD_CTX, "imx_gpio GPIO config: tdo = %d", tdo_gpio);
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(imx_gpio_handle_jtag_gpionum_tdi)
+{
+	if (CMD_ARGC == 1)
+		COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], tdi_gpio);
+
+	command_print(CMD_CTX, "imx_gpio GPIO config: tdi = %d", tdi_gpio);
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(imx_gpio_handle_jtag_gpionum_srst)
+{
+	if (CMD_ARGC == 1)
+		COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], srst_gpio);
+
+	command_print(CMD_CTX, "imx_gpio GPIO config: srst = %d", srst_gpio);
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(imx_gpio_handle_jtag_gpionum_trst)
+{
+	if (CMD_ARGC == 1)
+		COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], trst_gpio);
+
+	command_print(CMD_CTX, "imx_gpio GPIO config: trst = %d", trst_gpio);
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(imx_gpio_handle_swd_gpionums)
+{
+	if (CMD_ARGC == 2) {
+		COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], swclk_gpio);
+		COMMAND_PARSE_NUMBER(int, CMD_ARGV[1], swdio_gpio);
+	} else if (CMD_ARGC != 0) {
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	command_print(CMD_CTX,
+			"imx_gpio GPIO nums: swclk = %d, swdio = %d",
+			swclk_gpio, swdio_gpio);
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(imx_gpio_handle_swd_gpionum_swclk)
+{
+	if (CMD_ARGC == 1)
+		COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], swclk_gpio);
+
+	command_print(CMD_CTX, "imx_gpio num: swclk = %d", swclk_gpio);
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(imx_gpio_handle_swd_gpionum_swdio)
+{
+	if (CMD_ARGC == 1)
+		COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], swdio_gpio);
+
+	command_print(CMD_CTX, "imx_gpio num: swdio = %d", swdio_gpio);
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(imx_gpio_handle_speed_coeffs)
+{
+	if (CMD_ARGC == 2) {
+		COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], speed_coeff);
+		COMMAND_PARSE_NUMBER(int, CMD_ARGV[1], speed_offset);
+	}
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(imx_gpio_handle_peripheral_base)
+{
+	if (CMD_ARGC == 1)
+		COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], imx_gpio_peri_base);
+	return ERROR_OK;
+}
+
+static const struct command_registration imx_gpio_command_handlers[] = {
+	{
+		.name = "imx_gpio_jtag_nums",
+		.handler = &imx_gpio_handle_jtag_gpionums,
+		.mode = COMMAND_CONFIG,
+		.help = "gpio numbers for tck, tms, tdi, tdo. (in that order)",
+		.usage = "(tck tms tdi tdo)* ",
+	},
+	{
+		.name = "imx_gpio_tck_num",
+		.handler = &imx_gpio_handle_jtag_gpionum_tck,
+		.mode = COMMAND_CONFIG,
+		.help = "gpio number for tck.",
+	},
+	{
+		.name = "imx_gpio_tms_num",
+		.handler = &imx_gpio_handle_jtag_gpionum_tms,
+		.mode = COMMAND_CONFIG,
+		.help = "gpio number for tms.",
+	},
+	{
+		.name = "imx_gpio_tdo_num",
+		.handler = &imx_gpio_handle_jtag_gpionum_tdo,
+		.mode = COMMAND_CONFIG,
+		.help = "gpio number for tdo.",
+	},
+	{
+		.name = "imx_gpio_tdi_num",
+		.handler = &imx_gpio_handle_jtag_gpionum_tdi,
+		.mode = COMMAND_CONFIG,
+		.help = "gpio number for tdi.",
+	},
+	{
+		.name = "imx_gpio_swd_nums",
+		.handler = &imx_gpio_handle_swd_gpionums,
+		.mode = COMMAND_CONFIG,
+		.help = "gpio numbers for swclk, swdio. (in that order)",
+		.usage = "(swclk swdio)* ",
+	},
+	{
+		.name = "imx_gpio_swclk_num",
+		.handler = &imx_gpio_handle_swd_gpionum_swclk,
+		.mode = COMMAND_CONFIG,
+		.help = "gpio number for swclk.",
+	},
+	{
+		.name = "imx_gpio_swdio_num",
+		.handler = &imx_gpio_handle_swd_gpionum_swdio,
+		.mode = COMMAND_CONFIG,
+		.help = "gpio number for swdio.",
+	},
+	{
+		.name = "imx_gpio_srst_num",
+		.handler = &imx_gpio_handle_jtag_gpionum_srst,
+		.mode = COMMAND_CONFIG,
+		.help = "gpio number for srst.",
+	},
+	{
+		.name = "imx_gpio_trst_num",
+		.handler = &imx_gpio_handle_jtag_gpionum_trst,
+		.mode = COMMAND_CONFIG,
+		.help = "gpio number for trst.",
+	},
+	{
+		.name = "imx_gpio_speed_coeffs",
+		.handler = &imx_gpio_handle_speed_coeffs,
+		.mode = COMMAND_CONFIG,
+		.help = "SPEED_COEFF and SPEED_OFFSET for delay calculations.",
+	},
+	{
+		.name = "imx_gpio_peripheral_base",
+		.handler = &imx_gpio_handle_peripheral_base,
+		.mode = COMMAND_CONFIG,
+		.help = "peripheral base to access GPIOs (0x0209c000 for most IMX).",
+	},
+
+	COMMAND_REGISTRATION_DONE
+};
+
+static const char * const imx_gpio_transports[] = { "jtag", "swd", NULL };
+
+struct jtag_interface imx_gpio_interface = {
+	.name = "imx_gpio",
+	.supported = DEBUG_CAP_TMS_SEQ,
+	.execute_queue = bitbang_execute_queue,
+	.transports = imx_gpio_transports,
+	.swd = &bitbang_swd,
+	.speed = imx_gpio_speed,
+	.khz = imx_gpio_khz,
+	.speed_div = imx_gpio_speed_div,
+	.commands = imx_gpio_command_handlers,
+	.init = imx_gpio_init,
+	.quit = imx_gpio_quit,
+};
+
+static bool imx_gpio_jtag_mode_possible(void)
+{
+	if (!is_gpio_valid(tck_gpio))
+		return 0;
+	if (!is_gpio_valid(tms_gpio))
+		return 0;
+	if (!is_gpio_valid(tdi_gpio))
+		return 0;
+	if (!is_gpio_valid(tdo_gpio))
+		return 0;
+	return 1;
+}
+
+static bool imx_gpio_swd_mode_possible(void)
+{
+	if (!is_gpio_valid(swclk_gpio))
+		return 0;
+	if (!is_gpio_valid(swdio_gpio))
+		return 0;
+	return 1;
+}
+
+static int imx_gpio_init(void)
+{
+	bitbang_interface = &imx_gpio_bitbang;
+
+	LOG_INFO("imx_gpio GPIO JTAG/SWD bitbang driver");
+
+	if (imx_gpio_jtag_mode_possible()) {
+		if (imx_gpio_swd_mode_possible())
+			LOG_INFO("JTAG and SWD modes enabled");
+		else
+			LOG_INFO("JTAG only mode enabled (specify swclk and swdio gpio to add SWD mode)");
+	} else if (imx_gpio_swd_mode_possible()) {
+		LOG_INFO("SWD only mode enabled (specify tck, tms, tdi and tdo gpios to add JTAG mode)");
+	} else {
+		LOG_ERROR("Require tck, tms, tdi and tdo gpios for JTAG mode and/or swclk and swdio gpio for SWD mode");
+		return ERROR_JTAG_INIT_FAILED;
+	}
+
+	dev_mem_fd = open("/dev/mem", O_RDWR | O_SYNC);
+	if (dev_mem_fd < 0) {
+		perror("open");
+		return ERROR_JTAG_INIT_FAILED;
+	}
+
+
+	LOG_INFO("imx_gpio mmap: pagesize: %u, regionsize: %u",
+			sysconf(_SC_PAGE_SIZE), IMX_GPIO_REGS_COUNT * IMX_GPIO_SIZE);
+	pio_base = mmap(NULL, IMX_GPIO_REGS_COUNT * IMX_GPIO_SIZE,
+				PROT_READ | PROT_WRITE,
+				MAP_SHARED, dev_mem_fd, imx_gpio_peri_base);
+
+	if (pio_base == MAP_FAILED) {
+		perror("mmap");
+		close(dev_mem_fd);
+		return ERROR_JTAG_INIT_FAILED;
+	}
+
+	/*
+	 * Configure TDO as an input, and TDI, TCK, TMS, TRST, SRST
+	 * as outputs.  Drive TDI and TCK low, and TMS/TRST/SRST high.
+	 */
+	if (imx_gpio_jtag_mode_possible()) {
+		tdo_gpio_mode = gpio_mode_get(tdo_gpio);
+		tdi_gpio_mode = gpio_mode_get(tdi_gpio);
+		tck_gpio_mode = gpio_mode_get(tck_gpio);
+		tms_gpio_mode = gpio_mode_get(tms_gpio);
+
+		gpio_clear(tdi_gpio);
+		gpio_clear(tck_gpio);
+		gpio_set(tms_gpio);
+
+		gpio_mode_input_set(tdo_gpio);
+		gpio_mode_output_set(tdi_gpio);
+		gpio_mode_output_set(tck_gpio);
+		gpio_mode_output_set(tms_gpio);
+	}
+	if (imx_gpio_swd_mode_possible()) {
+		swclk_gpio_mode = gpio_mode_get(swclk_gpio);
+		swdio_gpio_mode = gpio_mode_get(swdio_gpio);
+
+		gpio_clear(swdio_gpio);
+		gpio_clear(swclk_gpio);
+		gpio_mode_output_set(swclk_gpio);
+		gpio_mode_output_set(swdio_gpio);
+	}
+	if (trst_gpio != -1) {
+		trst_gpio_mode = gpio_mode_get(trst_gpio);
+		gpio_set(trst_gpio);
+		gpio_mode_output_set(trst_gpio);
+	}
+	if (srst_gpio != -1) {
+		srst_gpio_mode = gpio_mode_get(srst_gpio);
+		gpio_set(srst_gpio);
+		gpio_mode_output_set(srst_gpio);
+	}
+
+	LOG_DEBUG("saved pinmux settings: tck %d tms %d tdi %d "
+		  "tdo %d trst %d srst %d", tck_gpio_mode, tms_gpio_mode,
+		  tdi_gpio_mode, tdo_gpio_mode, trst_gpio_mode, srst_gpio_mode);
+
+	if (swd_mode) {
+		imx_gpio_bitbang.write = imx_gpio_swd_write;
+		bitbang_switch_to_swd();
+	}
+
+	return ERROR_OK;
+}
+
+static int imx_gpio_quit(void)
+{
+	if (imx_gpio_jtag_mode_possible()) {
+		gpio_mode_set(tdo_gpio, tdo_gpio_mode);
+		gpio_mode_set(tdi_gpio, tdi_gpio_mode);
+		gpio_mode_set(tck_gpio, tck_gpio_mode);
+		gpio_mode_set(tms_gpio, tms_gpio_mode);
+	}
+	if (imx_gpio_swd_mode_possible()) {
+		gpio_mode_set(swclk_gpio, swclk_gpio_mode);
+		gpio_mode_set(swdio_gpio, swdio_gpio_mode);
+	}
+	if (trst_gpio != -1)
+		gpio_mode_set(trst_gpio, trst_gpio_mode);
+	if (srst_gpio != -1)
+		gpio_mode_set(srst_gpio, srst_gpio_mode);
+
+	return ERROR_OK;
+}
diff --git a/src/jtag/drivers/jlink.c b/src/jtag/drivers/jlink.c
index 3009058..bd3c5e0 100644
--- a/src/jtag/drivers/jlink.c
+++ b/src/jtag/drivers/jlink.c
@@ -314,12 +314,11 @@ static int jlink_execute_queue(void)
 static int jlink_speed(int speed)
 {
 	int ret;
-	uint32_t freq;
-	uint16_t divider;
+	struct jaylink_speed tmp;
 	int max_speed;
 
 	if (jaylink_has_cap(caps, JAYLINK_DEV_CAP_GET_SPEEDS)) {
-		ret = jaylink_get_speeds(devh, &freq, &divider);
+		ret = jaylink_get_speeds(devh, &tmp);
 
 		if (ret != JAYLINK_OK) {
 			LOG_ERROR("jaylink_get_speeds() failed: %s.",
@@ -327,8 +326,8 @@ static int jlink_speed(int speed)
 			return ERROR_JTAG_DEVICE_ERROR;
 		}
 
-		freq = freq / 1000;
-		max_speed = freq / divider;
+		tmp.freq /= 1000;
+		max_speed = tmp.freq / tmp.div;
 	} else {
 		max_speed = JLINK_MAX_SPEED;
 	}
@@ -433,15 +432,16 @@ static int select_interface(void)
 static int jlink_register(void)
 {
 	int ret;
-	int i;
+	size_t i;
 	bool handle_found;
+	size_t count;
 
 	if (!jaylink_has_cap(caps, JAYLINK_DEV_CAP_REGISTER))
 		return ERROR_OK;
 
-	ret = jaylink_register(devh, &conn, connlist, NULL, NULL);
+	ret = jaylink_register(devh, &conn, connlist, &count);
 
-	if (ret < 0) {
+	if (ret != JAYLINK_OK) {
 		LOG_ERROR("jaylink_register() failed: %s.",
 			jaylink_strerror_name(ret));
 		return ERROR_FAIL;
@@ -449,7 +449,7 @@ static int jlink_register(void)
 
 	handle_found = false;
 
-	for (i = 0; i < ret; i++) {
+	for (i = 0; i < count; i++) {
 		if (connlist[i].handle == conn.handle) {
 			handle_found = true;
 			break;
@@ -502,6 +502,36 @@ static bool adjust_swd_buffer_size(void)
 	return true;
 }
 
+static int jaylink_log_handler(const struct jaylink_context *ctx,
+		enum jaylink_log_level level, const char *format, va_list args,
+		void *user_data)
+{
+	enum log_levels tmp;
+
+	switch (level) {
+	case JAYLINK_LOG_LEVEL_ERROR:
+		tmp = LOG_LVL_ERROR;
+		break;
+	case JAYLINK_LOG_LEVEL_WARNING:
+		tmp = LOG_LVL_WARNING;
+		break;
+	/*
+	 * Forward info messages to the debug output because they are more verbose
+	 * than info messages of OpenOCD.
+	 */
+	case JAYLINK_LOG_LEVEL_INFO:
+	case JAYLINK_LOG_LEVEL_DEBUG:
+		tmp = LOG_LVL_DEBUG;
+		break;
+	default:
+		tmp = LOG_LVL_WARNING;
+	}
+
+	log_vprintf_lf(tmp, __FILE__, __LINE__, __func__, format, args);
+
+	return 0;
+}
+
 static int jlink_init(void)
 {
 	int ret;
@@ -515,6 +545,9 @@ static int jlink_init(void)
 	enum jaylink_usb_address address;
 	size_t length;
 
+	LOG_DEBUG("Using libjaylink %s (compiled with %s).",
+		jaylink_version_package_get_string(), JAYLINK_VERSION_PACKAGE_STRING);
+
 	ret = jaylink_init(&jayctx);
 
 	if (ret != JAYLINK_OK) {
@@ -523,10 +556,28 @@ static int jlink_init(void)
 		return ERROR_JTAG_INIT_FAILED;
 	}
 
-	ret = jaylink_get_device_list(jayctx, &devs);
+	ret = jaylink_log_set_callback(jayctx, &jaylink_log_handler, NULL);
 
-	if (ret < 0) {
-		LOG_ERROR("jaylink_get_device_list() failed: %s.",
+	if (ret != JAYLINK_OK) {
+		LOG_ERROR("jaylink_log_set_callback() failed: %s.",
+			jaylink_strerror_name(ret));
+		jaylink_exit(jayctx);
+		return ERROR_JTAG_INIT_FAILED;
+	}
+
+	ret = jaylink_discovery_scan(jayctx, 0);
+
+	if (ret != JAYLINK_OK) {
+		LOG_ERROR("jaylink_discovery_scan() failed: %s.",
+			jaylink_strerror_name(ret));
+		jaylink_exit(jayctx);
+		return ERROR_JTAG_INIT_FAILED;
+	}
+
+	ret = jaylink_get_devices(jayctx, &devs, NULL);
+
+	if (ret != JAYLINK_OK) {
+		LOG_ERROR("jaylink_get_devices() failed: %s.",
 			jaylink_strerror_name(ret));
 		jaylink_exit(jayctx);
 		return ERROR_JTAG_INIT_FAILED;
@@ -576,7 +627,7 @@ static int jlink_init(void)
 		LOG_ERROR("Failed to open device: %s.", jaylink_strerror_name(ret));
 	}
 
-	jaylink_free_device_list(devs, 1);
+	jaylink_free_devices(devs, true);
 
 	if (!found_device) {
 		LOG_ERROR("No J-Link device found.");
@@ -626,7 +677,7 @@ static int jlink_init(void)
 		}
 	}
 
-	jtag_command_version = JAYLINK_JTAG_V2;
+	jtag_command_version = JAYLINK_JTAG_VERSION_2;
 
 	if (jaylink_has_cap(caps, JAYLINK_DEV_CAP_GET_HW_VERSION)) {
 		ret = jaylink_get_hardware_version(devh, &hwver);
@@ -642,7 +693,7 @@ static int jlink_init(void)
 		LOG_INFO("Hardware version: %u.%02u", hwver.major, hwver.minor);
 
 		if (hwver.major >= 5)
-			jtag_command_version = JAYLINK_JTAG_V3;
+			jtag_command_version = JAYLINK_JTAG_VERSION_3;
 	}
 
 	if (iface == JAYLINK_TIF_SWD) {
@@ -685,7 +736,7 @@ static int jlink_init(void)
 
 	conn.handle = 0;
 	conn.pid = 0;
-	conn.hid = 0;
+	strcpy(conn.hid, "0.0.0.0");
 	conn.iid = 0;
 	conn.cid = 0;
 
@@ -729,6 +780,7 @@ static int jlink_init(void)
 static int jlink_quit(void)
 {
 	int ret;
+	size_t count;
 
 	if (trace_enabled) {
 		ret = jaylink_swo_stop(devh);
@@ -739,9 +791,9 @@ static int jlink_quit(void)
 	}
 
 	if (jaylink_has_cap(caps, JAYLINK_DEV_CAP_REGISTER)) {
-		ret = jaylink_unregister(devh, &conn, connlist, NULL, NULL);
+		ret = jaylink_unregister(devh, &conn, connlist, &count);
 
-		if (ret < 0)
+		if (ret != JAYLINK_OK)
 			LOG_ERROR("jaylink_unregister() failed: %s.",
 				jaylink_strerror_name(ret));
 	}
@@ -878,14 +930,22 @@ COMMAND_HANDLER(jlink_usb_command)
 
 COMMAND_HANDLER(jlink_serial_command)
 {
+	int ret;
+
 	if (CMD_ARGC != 1) {
 		command_print(CMD_CTX, "Need exactly one argument for jlink serial.");
 		return ERROR_COMMAND_SYNTAX_ERROR;
 	}
 
-	if (sscanf(CMD_ARGV[0], "%" SCNd32, &serial_number) != 1) {
+	ret = jaylink_parse_serial_number(CMD_ARGV[0], &serial_number);
+
+	if (ret == JAYLINK_ERR) {
 		command_print(CMD_CTX, "Invalid serial number: %s.", CMD_ARGV[0]);
 		return ERROR_FAIL;
+	} else if (ret != JAYLINK_OK) {
+		command_print(CMD_CTX, "jaylink_parse_serial_number() failed: %s.",
+			jaylink_strerror_name(ret));
+		return ERROR_FAIL;
 	}
 
 	use_serial_number = true;
@@ -951,10 +1011,10 @@ COMMAND_HANDLER(jlink_handle_jlink_jtag_command)
 
 	if (!CMD_ARGC) {
 		switch (jtag_command_version) {
-			case JAYLINK_JTAG_V2:
+			case JAYLINK_JTAG_VERSION_2:
 				version = 2;
 				break;
-			case JAYLINK_JTAG_V3:
+			case JAYLINK_JTAG_VERSION_3:
 				version = 3;
 				break;
 			default:
@@ -970,10 +1030,10 @@ COMMAND_HANDLER(jlink_handle_jlink_jtag_command)
 
 		switch (tmp) {
 			case 2:
-				jtag_command_version = JAYLINK_JTAG_V2;
+				jtag_command_version = JAYLINK_JTAG_VERSION_2;
 				break;
 			case 3:
-				jtag_command_version = JAYLINK_JTAG_V3;
+				jtag_command_version = JAYLINK_JTAG_VERSION_3;
 				break;
 			default:
 				command_print(CMD_CTX, "Invalid argument: %s.", CMD_ARGV[0]);
@@ -1545,6 +1605,125 @@ COMMAND_HANDLER(jlink_handle_config_command)
 	return ERROR_OK;
 }
 
+COMMAND_HANDLER(jlink_handle_emucom_write_command)
+{
+	int ret;
+	size_t tmp;
+	uint32_t channel;
+	uint32_t length;
+	uint8_t *buf;
+	size_t dummy;
+
+	if (CMD_ARGC != 2)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	if (!jaylink_has_cap(caps, JAYLINK_DEV_CAP_EMUCOM)) {
+		LOG_ERROR("Device does not support EMUCOM.");
+		return ERROR_FAIL;
+	}
+
+	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], channel);
+
+	tmp = strlen(CMD_ARGV[1]);
+
+	if (tmp % 2 != 0) {
+		LOG_ERROR("Data must be encoded as hexadecimal pairs.");
+		return ERROR_COMMAND_ARGUMENT_INVALID;
+	}
+
+	buf = malloc(tmp / 2);
+
+	if (!buf) {
+		LOG_ERROR("Failed to allocate buffer.");
+		return ERROR_FAIL;
+	}
+
+	dummy = unhexify(buf, CMD_ARGV[1], tmp / 2);
+
+	if (dummy != (tmp / 2)) {
+		LOG_ERROR("Data must be encoded as hexadecimal pairs.");
+		free(buf);
+		return ERROR_COMMAND_ARGUMENT_INVALID;
+	}
+
+	length = tmp / 2;
+	ret = jaylink_emucom_write(devh, channel, buf, &length);
+
+	free(buf);
+
+	if (ret == JAYLINK_ERR_DEV_NOT_SUPPORTED) {
+		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));
+		return ERROR_FAIL;
+	}
+
+	if (length != (tmp / 2))
+		LOG_WARNING("Only %" PRIu32 " bytes written to the channel.", length);
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(jlink_handle_emucom_read_command)
+{
+	int ret;
+	uint32_t channel;
+	uint32_t length;
+	uint8_t *buf;
+	size_t tmp;
+
+	if (CMD_ARGC != 2)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	if (!jaylink_has_cap(caps, JAYLINK_DEV_CAP_EMUCOM)) {
+		LOG_ERROR("Device does not support EMUCOM.");
+		return ERROR_FAIL;
+	}
+
+	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], channel);
+	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], length);
+
+	buf = malloc(length * 3 + 1);
+
+	if (!buf) {
+		LOG_ERROR("Failed to allocate buffer.");
+		return ERROR_FAIL;
+	}
+
+	ret = jaylink_emucom_read(devh, channel, buf, &length);
+
+	if (ret == JAYLINK_ERR_DEV_NOT_SUPPORTED) {
+		LOG_ERROR("Channel is not supported by the device.");
+		free(buf);
+		return ERROR_FAIL;
+	} else if (ret == JAYLINK_ERR_DEV_NOT_AVAILABLE) {
+		LOG_ERROR("Channel is not available for the requested amount of data. "
+			"%" PRIu32 " bytes are avilable.", length);
+		free(buf);
+		return ERROR_FAIL;
+	} else if (ret != JAYLINK_OK) {
+		LOG_ERROR("Failed to read from channel: %s.",
+			jaylink_strerror_name(ret));
+		free(buf);
+		return ERROR_FAIL;
+	}
+
+	tmp = hexify((char *)buf + length, buf, length, 2 * length + 1);
+
+	if (tmp != 2 * length) {
+		LOG_ERROR("Failed to convert data into hexadecimal string.");
+		free(buf);
+		return ERROR_FAIL;
+	}
+
+	command_print(CMD_CTX, "%s", buf + length);
+	free(buf);
+
+	return ERROR_OK;
+}
+
 static const struct command_registration jlink_config_subcommand_handlers[] = {
 	{
 		.name = "usb",
@@ -1590,6 +1769,24 @@ static const struct command_registration jlink_config_subcommand_handlers[] = {
 	COMMAND_REGISTRATION_DONE
 };
 
+static const struct command_registration jlink_emucom_subcommand_handlers[] = {
+	{
+		.name = "write",
+		.handler = &jlink_handle_emucom_write_command,
+		.mode = COMMAND_EXEC,
+		.help = "write to a channel",
+		.usage = "<channel> <data>",
+	},
+	{
+		.name = "read",
+		.handler = &jlink_handle_emucom_read_command,
+		.mode = COMMAND_EXEC,
+		.help = "read from a channel",
+		.usage = "<channel> <length>"
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
 static const struct command_registration jlink_subcommand_handlers[] = {
 	{
 		.name = "jtag",
@@ -1639,6 +1836,12 @@ static const struct command_registration jlink_subcommand_handlers[] = {
 			"this will show the device configuration",
 		.chain = jlink_config_subcommand_handlers,
 	},
+	{
+		.name = "emucom",
+		.mode = COMMAND_EXEC,
+		.help = "access EMUCOM channel",
+		.chain = jlink_emucom_subcommand_handlers
+	},
 	COMMAND_REGISTRATION_DONE
 };
 
diff --git a/src/jtag/drivers/kitprog.c b/src/jtag/drivers/kitprog.c
new file mode 100644
index 0000000..c689848
--- /dev/null
+++ b/src/jtag/drivers/kitprog.c
@@ -0,0 +1,967 @@
+/***************************************************************************
+ *   Copyright (C) 2007 by Juergen Stuber <juergen@jstuber.net>            *
+ *   based on Dominic Rath's and Benedikt Sauter's usbprog.c               *
+ *                                                                         *
+ *   Copyright (C) 2008 by Spencer Oliver                                  *
+ *   spen@spen-soft.co.uk                                                  *
+ *                                                                         *
+ *   Copyright (C) 2011 by Jean-Christophe PLAGNIOL-VIILARD                *
+ *   plagnioj@jcrosoft.com                                                 *
+ *                                                                         *
+ *   Copyright (C) 2015 by Marc Schink                                     *
+ *   openocd-dev@marcschink.de                                             *
+ *                                                                         *
+ *   Copyright (C) 2015 by Paul Fertser                                    *
+ *   fercerpav@gmail.com                                                   *
+ *                                                                         *
+ *   Copyright (C) 2015-2017 by Forest Crossman                            *
+ *   cyrozap@gmail.com                                                     *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
+ ***************************************************************************/
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include <stdint.h>
+
+#include <hidapi.h>
+
+#include <jtag/interface.h>
+#include <jtag/swd.h>
+#include <jtag/commands.h>
+
+#include "libusb_common.h"
+
+#define VID 0x04b4
+#define PID 0xf139
+
+#define BULK_EP_IN  1
+#define BULK_EP_OUT 2
+
+#define CONTROL_TYPE_READ  0x01
+#define CONTROL_TYPE_WRITE 0x02
+
+#define CONTROL_COMMAND_PROGRAM 0x07
+
+#define CONTROL_MODE_POLL_PROGRAMMER_STATUS  0x01
+#define CONTROL_MODE_RESET_TARGET            0x04
+#define CONTROL_MODE_SET_PROGRAMMER_PROTOCOL 0x40
+#define CONTROL_MODE_SYNCHRONIZE_TRANSFER    0x41
+#define CONTROL_MODE_ACQUIRE_SWD_TARGET      0x42
+#define CONTROL_MODE_SEND_SWD_SEQUENCE       0x43
+
+#define PROTOCOL_JTAG 0x00
+#define PROTOCOL_SWD  0x01
+
+#define DEVICE_PSOC4   0x00
+#define DEVICE_PSOC3   0x01
+#define DEVICE_UNKNOWN 0x02
+#define DEVICE_PSOC5   0x03
+
+#define ACQUIRE_MODE_RESET       0x00
+#define ACQUIRE_MODE_POWER_CYCLE 0x01
+
+#define SEQUENCE_LINE_RESET  0x00
+#define SEQUENCE_JTAG_TO_SWD 0x01
+
+#define PROGRAMMER_NOK_NACK 0x00
+#define PROGRAMMER_OK_ACK   0x01
+
+#define HID_TYPE_WRITE 0x00
+#define HID_TYPE_READ  0x01
+#define HID_TYPE_START 0x02
+
+#define HID_COMMAND_POWER      0x80
+#define HID_COMMAND_VERSION    0x81
+#define HID_COMMAND_RESET      0x82
+#define HID_COMMAND_CONFIGURE  0x8f
+#define HID_COMMAND_BOOTLOADER 0xa0
+
+/* 512 bytes seems to work reliably */
+#define SWD_MAX_BUFFER_LENGTH 512
+
+struct kitprog {
+	hid_device *hid_handle;
+	struct jtag_libusb_device_handle *usb_handle;
+	uint16_t packet_size;
+	uint16_t packet_index;
+	uint8_t *packet_buffer;
+	char *serial;
+	uint8_t hardware_version;
+	uint8_t minor_version;
+	uint8_t major_version;
+	uint16_t millivolts;
+
+	bool supports_jtag_to_swd;
+};
+
+struct pending_transfer_result {
+	uint8_t cmd;
+	uint32_t data;
+	void *buffer;
+};
+
+static char *kitprog_serial;
+static bool kitprog_init_acquire_psoc;
+
+static int pending_transfer_count, pending_queue_len;
+static struct pending_transfer_result *pending_transfers;
+
+static int queued_retval;
+
+static struct kitprog *kitprog_handle;
+
+static int kitprog_usb_open(void);
+static void kitprog_usb_close(void);
+
+static int kitprog_hid_command(uint8_t *command, size_t command_length,
+		uint8_t *data, size_t data_length);
+static int kitprog_get_version(void);
+static int kitprog_get_millivolts(void);
+static int kitprog_get_info(void);
+static int kitprog_set_protocol(uint8_t protocol);
+static int kitprog_get_status(void);
+static int kitprog_set_unknown(void);
+static int kitprog_acquire_psoc(uint8_t psoc_type, uint8_t acquire_mode,
+		uint8_t max_attempts);
+static int kitprog_reset_target(void);
+static int kitprog_swd_sync(void);
+static int kitprog_swd_seq(uint8_t seq_type);
+
+static int kitprog_generic_acquire(void);
+
+static int kitprog_swd_run_queue(void);
+static void kitprog_swd_queue_cmd(uint8_t cmd, uint32_t *dst, uint32_t data);
+static int kitprog_swd_switch_seq(enum swd_special_seq seq);
+
+
+static inline int mm_to_version(uint8_t major, uint8_t minor)
+{
+	return (major << 8) | minor;
+}
+
+static int kitprog_init(void)
+{
+	int retval;
+
+	kitprog_handle = malloc(sizeof(struct kitprog));
+	if (kitprog_handle == NULL) {
+		LOG_ERROR("Failed to allocate memory");
+		return ERROR_FAIL;
+	}
+
+	if (kitprog_usb_open() != ERROR_OK) {
+		LOG_ERROR("Can't find a KitProg device! Please check device connections and permissions.");
+		return ERROR_JTAG_INIT_FAILED;
+	}
+
+	/* Get the current KitProg version and target voltage */
+	if (kitprog_get_info() != ERROR_OK)
+		return ERROR_FAIL;
+
+	/* Compatibility check */
+	kitprog_handle->supports_jtag_to_swd = true;
+	int kitprog_version = mm_to_version(kitprog_handle->major_version, kitprog_handle->minor_version);
+	if (kitprog_version < mm_to_version(2, 14)) {
+		LOG_WARNING("KitProg firmware versions below v2.14 do not support sending JTAG to SWD sequences. These sequences will be substituted with SWD line resets.");
+		kitprog_handle->supports_jtag_to_swd = false;
+	}
+
+	/* I have no idea what this does */
+	if (kitprog_set_unknown() != ERROR_OK)
+		return ERROR_FAIL;
+
+	/* SWD won't work unless we do this */
+	if (kitprog_swd_sync() != ERROR_OK)
+		return ERROR_FAIL;
+
+	/* Set the protocol to SWD */
+	if (kitprog_set_protocol(PROTOCOL_SWD) != ERROR_OK)
+		return ERROR_FAIL;
+
+	/* Reset the SWD bus */
+	if (kitprog_swd_seq(SEQUENCE_LINE_RESET) != ERROR_OK)
+		return ERROR_FAIL;
+
+	if (kitprog_init_acquire_psoc) {
+		/* Try to acquire any device that will respond */
+		retval = kitprog_generic_acquire();
+		if (retval != ERROR_OK) {
+			LOG_ERROR("No PSoC devices found");
+			return retval;
+		}
+	}
+
+	/* Allocate packet buffers and queues */
+	kitprog_handle->packet_size = SWD_MAX_BUFFER_LENGTH;
+	kitprog_handle->packet_buffer = malloc(SWD_MAX_BUFFER_LENGTH);
+	if (kitprog_handle->packet_buffer == NULL) {
+		LOG_ERROR("Failed to allocate memory for the packet buffer");
+		return ERROR_FAIL;
+	}
+
+	pending_queue_len = SWD_MAX_BUFFER_LENGTH / 5;
+	pending_transfers = malloc(pending_queue_len * sizeof(*pending_transfers));
+	if (pending_transfers == NULL) {
+		LOG_ERROR("Failed to allocate memory for the SWD transfer queue");
+		return ERROR_FAIL;
+	}
+
+	return ERROR_OK;
+}
+
+static int kitprog_quit(void)
+{
+	kitprog_usb_close();
+
+	if (kitprog_handle->packet_buffer != NULL)
+		free(kitprog_handle->packet_buffer);
+	if (kitprog_handle->serial != NULL)
+		free(kitprog_handle->serial);
+	if (kitprog_handle != NULL)
+		free(kitprog_handle);
+
+	if (kitprog_serial != NULL)
+		free(kitprog_serial);
+
+	if (pending_transfers != NULL)
+		free(pending_transfers);
+
+	return ERROR_OK;
+}
+
+/*************** kitprog usb functions *********************/
+
+static int kitprog_get_usb_serial(void)
+{
+	int retval;
+	const uint8_t str_index = 128; /* This seems to be a constant */
+	char desc_string[256+1]; /* Max size of string descriptor */
+
+	retval = libusb_get_string_descriptor_ascii(kitprog_handle->usb_handle,
+			str_index, (unsigned char *)desc_string, sizeof(desc_string)-1);
+	if (retval < 0) {
+		LOG_ERROR("libusb_get_string_descriptor_ascii() failed with %d", retval);
+		return ERROR_FAIL;
+	}
+
+	/* Null terminate descriptor string */
+	desc_string[retval] = '\0';
+
+	/* Allocate memory for the serial number */
+	kitprog_handle->serial = calloc(retval + 1, sizeof(char));
+	if (kitprog_handle->serial == NULL) {
+		LOG_ERROR("Failed to allocate memory for the serial number");
+		return ERROR_FAIL;
+	}
+
+	/* Store the serial number */
+	strncpy(kitprog_handle->serial, desc_string, retval + 1);
+
+	return ERROR_OK;
+}
+
+static int kitprog_usb_open(void)
+{
+	const uint16_t vids[] = { VID, 0 };
+	const uint16_t pids[] = { PID, 0 };
+
+	if (jtag_libusb_open(vids, pids, kitprog_serial,
+			&kitprog_handle->usb_handle) != ERROR_OK) {
+		LOG_ERROR("Failed to open or find the device");
+		return ERROR_FAIL;
+	}
+
+	/* Get the serial number for the device */
+	if (kitprog_get_usb_serial() != ERROR_OK)
+		LOG_WARNING("Failed to get KitProg serial number");
+
+	/* Convert the ASCII serial number into a (wchar_t *) */
+	size_t len = strlen(kitprog_handle->serial);
+	wchar_t *hid_serial = calloc(len + 1, sizeof(wchar_t));
+	if (hid_serial == NULL) {
+		LOG_ERROR("Failed to allocate memory for the serial number");
+		return ERROR_FAIL;
+	}
+	if (mbstowcs(hid_serial, kitprog_handle->serial, len + 1) == (size_t)-1) {
+		free(hid_serial);
+		LOG_ERROR("Failed to convert serial number");
+		return ERROR_FAIL;
+	}
+
+	/* Use HID for the KitBridge interface */
+	kitprog_handle->hid_handle = hid_open(VID, PID, hid_serial);
+	free(hid_serial);
+	if (kitprog_handle->hid_handle == NULL) {
+		LOG_ERROR("Failed to open KitBridge (HID) interface");
+		return ERROR_FAIL;
+	}
+
+	/* Claim the KitProg Programmer (bulk transfer) interface */
+	if (jtag_libusb_claim_interface(kitprog_handle->usb_handle, 1) != ERROR_OK) {
+		LOG_ERROR("Failed to claim KitProg Programmer (bulk transfer) interface");
+		return ERROR_FAIL;
+	}
+
+	return ERROR_OK;
+}
+
+static void kitprog_usb_close(void)
+{
+	if (kitprog_handle->hid_handle != NULL) {
+		hid_close(kitprog_handle->hid_handle);
+		hid_exit();
+	}
+
+	jtag_libusb_close(kitprog_handle->usb_handle);
+}
+
+/*************** kitprog lowlevel functions *********************/
+
+static int kitprog_hid_command(uint8_t *command, size_t command_length,
+		uint8_t *data, size_t data_length)
+{
+	int ret;
+
+	ret = hid_write(kitprog_handle->hid_handle, command, command_length);
+	if (ret < 0) {
+		LOG_DEBUG("HID write returned %i", ret);
+		return ERROR_FAIL;
+	}
+
+	ret = hid_read(kitprog_handle->hid_handle, data, data_length);
+	if (ret < 0) {
+		LOG_DEBUG("HID read returned %i", ret);
+		return ERROR_FAIL;
+	}
+
+	return ERROR_OK;
+}
+
+static int kitprog_get_version(void)
+{
+	int ret;
+
+	unsigned char command[3] = {HID_TYPE_START | HID_TYPE_WRITE, 0x00, HID_COMMAND_VERSION};
+	unsigned char data[64];
+
+	ret = kitprog_hid_command(command, sizeof command, data, sizeof data);
+	if (ret != ERROR_OK)
+		return ret;
+
+	kitprog_handle->hardware_version = data[1];
+	kitprog_handle->minor_version = data[2];
+	kitprog_handle->major_version = data[3];
+
+	return ERROR_OK;
+}
+
+static int kitprog_get_millivolts(void)
+{
+	int ret;
+
+	unsigned char command[3] = {HID_TYPE_START | HID_TYPE_READ, 0x00, HID_COMMAND_POWER};
+	unsigned char data[64];
+
+	ret = kitprog_hid_command(command, sizeof command, data, sizeof data);
+	if (ret != ERROR_OK)
+		return ret;
+
+	kitprog_handle->millivolts = (data[4] << 8) | data[3];
+
+	return ERROR_OK;
+}
+
+static int kitprog_get_info(void)
+{
+	/* Get the device version information */
+	if (kitprog_get_version() == ERROR_OK) {
+		LOG_INFO("KitProg v%u.%02u",
+			kitprog_handle->major_version, kitprog_handle->minor_version);
+		LOG_INFO("Hardware version: %u",
+			kitprog_handle->hardware_version);
+	} else {
+		LOG_ERROR("Failed to get KitProg version");
+		return ERROR_FAIL;
+	}
+
+	/* Get the current reported target voltage */
+	if (kitprog_get_millivolts() == ERROR_OK) {
+		LOG_INFO("VTARG = %u.%03u V",
+			kitprog_handle->millivolts / 1000, kitprog_handle->millivolts % 1000);
+	} else {
+		LOG_ERROR("Failed to get target voltage");
+		return ERROR_FAIL;
+	}
+
+	return ERROR_OK;
+}
+
+static int kitprog_set_protocol(uint8_t protocol)
+{
+	int transferred;
+	char status = PROGRAMMER_NOK_NACK;
+
+	transferred = jtag_libusb_control_transfer(kitprog_handle->usb_handle,
+		LIBUSB_ENDPOINT_IN | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE,
+		CONTROL_TYPE_WRITE,
+		(CONTROL_MODE_SET_PROGRAMMER_PROTOCOL << 8) | CONTROL_COMMAND_PROGRAM,
+		protocol, &status, 1, 0);
+
+	if (transferred == 0) {
+		LOG_DEBUG("Zero bytes transferred");
+		return ERROR_FAIL;
+	}
+
+	if (status != PROGRAMMER_OK_ACK) {
+		LOG_DEBUG("Programmer did not respond OK");
+		return ERROR_FAIL;
+	}
+
+	return ERROR_OK;
+}
+
+static int kitprog_get_status(void)
+{
+	int transferred = 0;
+	char status = PROGRAMMER_NOK_NACK;
+
+	/* Try a maximum of three times */
+	for (int i = 0; (i < 3) && (transferred == 0); i++) {
+		transferred = jtag_libusb_control_transfer(kitprog_handle->usb_handle,
+			LIBUSB_ENDPOINT_IN | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE,
+			CONTROL_TYPE_READ,
+			(CONTROL_MODE_POLL_PROGRAMMER_STATUS << 8) | CONTROL_COMMAND_PROGRAM,
+			0, &status, 1, 0);
+		jtag_sleep(1000);
+	}
+
+	if (transferred == 0) {
+		LOG_DEBUG("Zero bytes transferred");
+		return ERROR_FAIL;
+	}
+
+	if (status != PROGRAMMER_OK_ACK) {
+		LOG_DEBUG("Programmer did not respond OK");
+		return ERROR_FAIL;
+	}
+
+	return ERROR_OK;
+}
+
+static int kitprog_set_unknown(void)
+{
+	int transferred;
+	char status = PROGRAMMER_NOK_NACK;
+
+	transferred = jtag_libusb_control_transfer(kitprog_handle->usb_handle,
+		LIBUSB_ENDPOINT_IN | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE,
+		CONTROL_TYPE_WRITE,
+		(0x03 << 8) | 0x04,
+		0, &status, 1, 0);
+
+	if (transferred == 0) {
+		LOG_DEBUG("Zero bytes transferred");
+		return ERROR_FAIL;
+	}
+
+	if (status != PROGRAMMER_OK_ACK) {
+		LOG_DEBUG("Programmer did not respond OK");
+		return ERROR_FAIL;
+	}
+
+	return ERROR_OK;
+}
+
+static int kitprog_acquire_psoc(uint8_t psoc_type, uint8_t acquire_mode,
+		uint8_t max_attempts)
+{
+	int transferred;
+	char status = PROGRAMMER_NOK_NACK;
+
+	transferred = jtag_libusb_control_transfer(kitprog_handle->usb_handle,
+		LIBUSB_ENDPOINT_IN | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE,
+		CONTROL_TYPE_WRITE,
+		(CONTROL_MODE_ACQUIRE_SWD_TARGET << 8) | CONTROL_COMMAND_PROGRAM,
+		(max_attempts << 8) | (acquire_mode << 4) | psoc_type, &status, 1, 0);
+
+	if (transferred == 0) {
+		LOG_DEBUG("Zero bytes transferred");
+		return ERROR_FAIL;
+	}
+
+	if (status != PROGRAMMER_OK_ACK) {
+		LOG_DEBUG("Programmer did not respond OK");
+		return ERROR_FAIL;
+	}
+
+	return ERROR_OK;
+}
+
+static int kitprog_reset_target(void)
+{
+	int transferred;
+	char status = PROGRAMMER_NOK_NACK;
+
+	transferred = jtag_libusb_control_transfer(kitprog_handle->usb_handle,
+		LIBUSB_ENDPOINT_IN | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE,
+		CONTROL_TYPE_WRITE,
+		(CONTROL_MODE_RESET_TARGET << 8) | CONTROL_COMMAND_PROGRAM,
+		0, &status, 1, 0);
+
+	if (transferred == 0) {
+		LOG_DEBUG("Zero bytes transferred");
+		return ERROR_FAIL;
+	}
+
+	if (status != PROGRAMMER_OK_ACK) {
+		LOG_DEBUG("Programmer did not respond OK");
+		return ERROR_FAIL;
+	}
+
+	return ERROR_OK;
+}
+
+static int kitprog_swd_sync(void)
+{
+	int transferred;
+	char status = PROGRAMMER_NOK_NACK;
+
+	transferred = jtag_libusb_control_transfer(kitprog_handle->usb_handle,
+		LIBUSB_ENDPOINT_IN | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE,
+		CONTROL_TYPE_WRITE,
+		(CONTROL_MODE_SYNCHRONIZE_TRANSFER << 8) | CONTROL_COMMAND_PROGRAM,
+		0, &status, 1, 0);
+
+	if (transferred == 0) {
+		LOG_DEBUG("Zero bytes transferred");
+		return ERROR_FAIL;
+	}
+
+	if (status != PROGRAMMER_OK_ACK) {
+		LOG_DEBUG("Programmer did not respond OK");
+		return ERROR_FAIL;
+	}
+
+	return ERROR_OK;
+}
+
+static int kitprog_swd_seq(uint8_t seq_type)
+{
+	int transferred;
+	char status = PROGRAMMER_NOK_NACK;
+
+	transferred = jtag_libusb_control_transfer(kitprog_handle->usb_handle,
+		LIBUSB_ENDPOINT_IN | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE,
+		CONTROL_TYPE_WRITE,
+		(CONTROL_MODE_SEND_SWD_SEQUENCE << 8) | CONTROL_COMMAND_PROGRAM,
+		seq_type, &status, 1, 0);
+
+	if (transferred == 0) {
+		LOG_DEBUG("Zero bytes transferred");
+		return ERROR_FAIL;
+	}
+
+	if (status != PROGRAMMER_OK_ACK) {
+		LOG_DEBUG("Programmer did not respond OK");
+		return ERROR_FAIL;
+	}
+
+	return ERROR_OK;
+}
+
+static int kitprog_generic_acquire(void)
+{
+	const uint8_t devices[] = {DEVICE_PSOC4, DEVICE_PSOC3, DEVICE_PSOC5};
+
+	int retval;
+	int acquire_count = 0;
+
+	/* Due to the way the SWD port is shared between the Test Controller (TC)
+	 * and the Cortex-M3 DAP on the PSoC 5LP, the TC is the default SWD target
+	 * after power is applied. To access the DAP, the PSoC 5LP requires at least
+	 * one acquisition sequence to be run (which switches the SWD mux from the
+	 * TC to the DAP). However, after the mux is switched, the Cortex-M3 will be
+	 * held in reset until a series of registers are written to (see section 5.2
+	 * of the PSoC 5LP Device Programming Specifications for details).
+	 *
+	 * Instead of writing the registers in this function, we just do what the
+	 * Cypress tools do and run the acquisition sequence a second time. This
+	 * will take the Cortex-M3 out of reset and enable debugging.
+	 */
+	for (int i = 0; i < 2; i++) {
+		for (uint8_t j = 0; j < sizeof devices && acquire_count == i; j++) {
+			retval = kitprog_acquire_psoc(devices[j], ACQUIRE_MODE_RESET, 3);
+			if (retval != ERROR_OK) {
+				LOG_DEBUG("Aquisition function failed for device 0x%02x.", devices[j]);
+				return retval;
+			}
+
+			if (kitprog_get_status() == ERROR_OK)
+				acquire_count++;
+		}
+
+		jtag_sleep(10);
+	}
+
+	if (acquire_count < 2)
+		return ERROR_FAIL;
+
+	return ERROR_OK;
+}
+
+/*************** swd wrapper functions *********************/
+
+static int kitprog_swd_init(void)
+{
+	return ERROR_OK;
+}
+
+static void kitprog_swd_write_reg(uint8_t cmd, uint32_t value, uint32_t ap_delay_clk)
+{
+	assert(!(cmd & SWD_CMD_RnW));
+	kitprog_swd_queue_cmd(cmd, NULL, value);
+}
+
+static void kitprog_swd_read_reg(uint8_t cmd, uint32_t *value, uint32_t ap_delay_clk)
+{
+	assert(cmd & SWD_CMD_RnW);
+	kitprog_swd_queue_cmd(cmd, value, 0);
+}
+
+/*************** swd lowlevel functions ********************/
+
+static int kitprog_swd_switch_seq(enum swd_special_seq seq)
+{
+	switch (seq) {
+		case JTAG_TO_SWD:
+			if (kitprog_handle->supports_jtag_to_swd) {
+				LOG_DEBUG("JTAG to SWD");
+				if (kitprog_swd_seq(SEQUENCE_JTAG_TO_SWD) != ERROR_OK)
+					return ERROR_FAIL;
+				break;
+			} else {
+				LOG_DEBUG("JTAG to SWD not supported");
+				/* Fall through to fix target reset issue */
+			}
+		case LINE_RESET:
+			LOG_DEBUG("SWD line reset");
+			if (kitprog_swd_seq(SEQUENCE_LINE_RESET) != ERROR_OK)
+				return ERROR_FAIL;
+			break;
+		default:
+			LOG_ERROR("Sequence %d not supported.", seq);
+			return ERROR_FAIL;
+	}
+
+	return ERROR_OK;
+}
+
+static int kitprog_swd_run_queue(void)
+{
+	int ret;
+
+	size_t read_count = 0;
+	size_t read_index = 0;
+	size_t write_count = 0;
+	uint8_t *buffer = kitprog_handle->packet_buffer;
+
+	do {
+		LOG_DEBUG("Executing %d queued transactions", pending_transfer_count);
+
+		if (queued_retval != ERROR_OK) {
+			LOG_DEBUG("Skipping due to previous errors: %d", queued_retval);
+			break;
+		}
+
+		if (!pending_transfer_count)
+			break;
+
+		for (int i = 0; i < pending_transfer_count; i++) {
+			uint8_t cmd = pending_transfers[i].cmd;
+			uint32_t data = pending_transfers[i].data;
+
+			/* When proper WAIT handling is implemented in the
+			 * common SWD framework, this kludge can be
+			 * removed. However, this might lead to minor
+			 * performance degradation as the adapter wouldn't be
+			 * able to automatically retry anything (because ARM
+			 * has forgotten to implement sticky error flags
+			 * clearing). See also comments regarding
+			 * cmsis_dap_cmd_DAP_TFER_Configure() and
+			 * cmsis_dap_cmd_DAP_SWD_Configure() in
+			 * cmsis_dap_init().
+			 */
+			if (!(cmd & SWD_CMD_RnW) &&
+				!(cmd & SWD_CMD_APnDP) &&
+				(cmd & SWD_CMD_A32) >> 1 == DP_CTRL_STAT &&
+				(data & CORUNDETECT)) {
+				LOG_DEBUG("refusing to enable sticky overrun detection");
+				data &= ~CORUNDETECT;
+			}
+
+#if 0
+			LOG_DEBUG("%s %s reg %x %"PRIx32,
+					cmd & SWD_CMD_APnDP ? "AP" : "DP",
+					cmd & SWD_CMD_RnW ? "read" : "write",
+				  (cmd & SWD_CMD_A32) >> 1, data);
+#endif
+
+			buffer[write_count++] = (cmd | SWD_CMD_START | SWD_CMD_PARK) & ~SWD_CMD_STOP;
+			read_count++;
+			if (!(cmd & SWD_CMD_RnW)) {
+				buffer[write_count++] = (data) & 0xff;
+				buffer[write_count++] = (data >> 8) & 0xff;
+				buffer[write_count++] = (data >> 16) & 0xff;
+				buffer[write_count++] = (data >> 24) & 0xff;
+			} else {
+				read_count += 4;
+			}
+		}
+
+		ret = jtag_libusb_bulk_write(kitprog_handle->usb_handle,
+				BULK_EP_OUT, (char *)buffer, write_count, 0);
+		if (ret > 0) {
+			queued_retval = ERROR_OK;
+		} else {
+			LOG_ERROR("Bulk write failed");
+			queued_retval = ERROR_FAIL;
+			break;
+		}
+
+		/* We use the maximum buffer size here because the KitProg sometimes
+		 * doesn't like bulk reads of fewer than 62 bytes. (?!?!)
+		 */
+		ret = jtag_libusb_bulk_read(kitprog_handle->usb_handle,
+				BULK_EP_IN | LIBUSB_ENDPOINT_IN, (char *)buffer,
+				SWD_MAX_BUFFER_LENGTH, 0);
+		if (ret > 0) {
+			/* Handle garbage data by offsetting the initial read index */
+			if ((unsigned int)ret > read_count)
+				read_index = ret - read_count;
+			queued_retval = ERROR_OK;
+		} else {
+			LOG_ERROR("Bulk read failed");
+			queued_retval = ERROR_FAIL;
+			break;
+		}
+
+		for (int i = 0; i < pending_transfer_count; i++) {
+			if (pending_transfers[i].cmd & SWD_CMD_RnW) {
+				uint32_t data = le_to_h_u32(&buffer[read_index]);
+
+#if 0
+				LOG_DEBUG("Read result: %"PRIx32, data);
+#endif
+
+				if (pending_transfers[i].buffer)
+					*(uint32_t *)pending_transfers[i].buffer = data;
+
+				read_index += 4;
+			}
+
+			uint8_t ack = buffer[read_index] & 0x07;
+			if (ack != SWD_ACK_OK || (buffer[read_index] & 0x08)) {
+				LOG_DEBUG("SWD ack not OK: %d %s", i,
+					  ack == SWD_ACK_WAIT ? "WAIT" : ack == SWD_ACK_FAULT ? "FAULT" : "JUNK");
+				queued_retval = ack == SWD_ACK_WAIT ? ERROR_WAIT : ERROR_FAIL;
+				break;
+			}
+			read_index++;
+		}
+	} while (0);
+
+	pending_transfer_count = 0;
+	int retval = queued_retval;
+	queued_retval = ERROR_OK;
+
+	return retval;
+}
+
+static void kitprog_swd_queue_cmd(uint8_t cmd, uint32_t *dst, uint32_t data)
+{
+	if (pending_transfer_count == pending_queue_len) {
+		/* Not enough room in the queue. Run the queue. */
+		queued_retval = kitprog_swd_run_queue();
+	}
+
+	if (queued_retval != ERROR_OK)
+		return;
+
+	pending_transfers[pending_transfer_count].data = data;
+	pending_transfers[pending_transfer_count].cmd = cmd;
+	if (cmd & SWD_CMD_RnW) {
+		/* Queue a read transaction */
+		pending_transfers[pending_transfer_count].buffer = dst;
+	}
+	pending_transfer_count++;
+}
+
+/*************** jtag lowlevel functions ********************/
+
+static void kitprog_execute_reset(struct jtag_command *cmd)
+{
+	int retval = ERROR_OK;
+
+	if (cmd->cmd.reset->srst == 1) {
+		retval = kitprog_reset_target();
+		/* Since the previous command also disables SWCLK output, we need to send an
+		 * SWD bus reset command to re-enable it. For some reason, running
+		 * kitprog_swd_seq() immediately after kitprog_reset_target() won't
+		 * actually fix this. Instead, kitprog_swd_seq() will be run once OpenOCD
+		 * tries to send a JTAG-to-SWD sequence, which should happen during
+		 * swd_check_reconnect (see the JTAG_TO_SWD case in kitprog_swd_switch_seq).
+		 */
+	}
+
+	if (retval != ERROR_OK)
+		LOG_ERROR("KitProg: Interface reset failed");
+}
+
+static void kitprog_execute_sleep(struct jtag_command *cmd)
+{
+	jtag_sleep(cmd->cmd.sleep->us);
+}
+
+static void kitprog_execute_command(struct jtag_command *cmd)
+{
+	switch (cmd->type) {
+		case JTAG_RESET:
+			kitprog_execute_reset(cmd);
+			break;
+		case JTAG_SLEEP:
+			kitprog_execute_sleep(cmd);
+			break;
+		default:
+			LOG_ERROR("BUG: unknown JTAG command type encountered");
+			exit(-1);
+	}
+}
+
+static int kitprog_execute_queue(void)
+{
+	struct jtag_command *cmd = jtag_command_queue;
+
+	while (cmd != NULL) {
+		kitprog_execute_command(cmd);
+		cmd = cmd->next;
+	}
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(kitprog_handle_info_command)
+{
+	int retval = kitprog_get_info();
+
+	return retval;
+}
+
+
+COMMAND_HANDLER(kitprog_handle_acquire_psoc_command)
+{
+	int retval = kitprog_generic_acquire();
+
+	return retval;
+}
+
+COMMAND_HANDLER(kitprog_handle_serial_command)
+{
+	if (CMD_ARGC == 1) {
+		size_t len = strlen(CMD_ARGV[0]);
+		kitprog_serial = calloc(len + 1, sizeof(char));
+		if (kitprog_serial == NULL) {
+			LOG_ERROR("Failed to allocate memory for the serial number");
+			return ERROR_FAIL;
+		}
+		strncpy(kitprog_serial, CMD_ARGV[0], len + 1);
+	} else {
+		LOG_ERROR("expected exactly one argument to kitprog_serial <serial-number>");
+		return ERROR_FAIL;
+	}
+
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(kitprog_handle_init_acquire_psoc_command)
+{
+	kitprog_init_acquire_psoc = true;
+
+	return ERROR_OK;
+}
+
+static const struct command_registration kitprog_subcommand_handlers[] = {
+	{
+		.name = "info",
+		.handler = &kitprog_handle_info_command,
+		.mode = COMMAND_EXEC,
+		.usage = "",
+		.help = "show KitProg info",
+	},
+	{
+		.name = "acquire_psoc",
+		.handler = &kitprog_handle_acquire_psoc_command,
+		.mode = COMMAND_EXEC,
+		.usage = "",
+		.help = "try to acquire a PSoC",
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+static const struct command_registration kitprog_command_handlers[] = {
+	{
+		.name = "kitprog",
+		.mode = COMMAND_ANY,
+		.help = "perform KitProg management",
+		.usage = "<cmd>",
+		.chain = kitprog_subcommand_handlers,
+	},
+	{
+		.name = "kitprog_serial",
+		.handler = &kitprog_handle_serial_command,
+		.mode = COMMAND_CONFIG,
+		.help = "set the serial number of the adapter",
+		.usage = "serial_string",
+	},
+	{
+		.name = "kitprog_init_acquire_psoc",
+		.handler = &kitprog_handle_init_acquire_psoc_command,
+		.mode = COMMAND_CONFIG,
+		.help = "try to acquire a PSoC during init",
+		.usage = "",
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+static const struct swd_driver kitprog_swd = {
+	.init = kitprog_swd_init,
+	.switch_seq = kitprog_swd_switch_seq,
+	.read_reg = kitprog_swd_read_reg,
+	.write_reg = kitprog_swd_write_reg,
+	.run = kitprog_swd_run_queue,
+};
+
+static const char * const kitprog_transports[] = { "swd", NULL };
+
+struct jtag_interface kitprog_interface = {
+	.name = "kitprog",
+	.commands = kitprog_command_handlers,
+	.transports = kitprog_transports,
+	.swd = &kitprog_swd,
+	.execute_queue = kitprog_execute_queue,
+	.init = kitprog_init,
+	.quit = kitprog_quit
+};
diff --git a/src/jtag/drivers/libjaylink b/src/jtag/drivers/libjaylink
-Subproject d57dee67bc756291b7d8b51d350d1c6213e514f
+Subproject 699b7001d34a79c8e7064503dde1bede786fd7f
diff --git a/src/jtag/drivers/libusb0_common.c b/src/jtag/drivers/libusb0_common.c
index e319751..1825543 100644
--- a/src/jtag/drivers/libusb0_common.c
+++ b/src/jtag/drivers/libusb0_common.c
@@ -146,7 +146,7 @@ int jtag_libusb_set_configuration(jtag_libusb_device_handle *devh,
 int jtag_libusb_choose_interface(struct jtag_libusb_device_handle *devh,
 		unsigned int *usb_read_ep,
 		unsigned int *usb_write_ep,
-		int bclass, int subclass, int protocol)
+		int bclass, int subclass, int protocol, int trans_type)
 {
 	struct jtag_libusb_device *udev = jtag_libusb_get_device(devh);
 	struct usb_interface *iface = udev->config->interface;
@@ -157,7 +157,8 @@ int jtag_libusb_choose_interface(struct jtag_libusb_device_handle *devh,
 	for (int i = 0; i < desc->bNumEndpoints; i++) {
 		if ((bclass > 0 && desc->bInterfaceClass != bclass) ||
 		    (subclass > 0 && desc->bInterfaceSubClass != subclass) ||
-		    (protocol > 0 && desc->bInterfaceProtocol != protocol))
+		    (protocol > 0 && desc->bInterfaceProtocol != protocol) ||
+		    (trans_type > 0 && (desc->endpoint[i].bmAttributes & 0x3) != trans_type))
 			continue;
 
 		uint8_t epnum = desc->endpoint[i].bEndpointAddress;
diff --git a/src/jtag/drivers/libusb0_common.h b/src/jtag/drivers/libusb0_common.h
index 7163b43..baa9e3c 100644
--- a/src/jtag/drivers/libusb0_common.h
+++ b/src/jtag/drivers/libusb0_common.h
@@ -67,7 +67,7 @@ int jtag_libusb_set_configuration(jtag_libusb_device_handle *devh,
 int jtag_libusb_choose_interface(struct jtag_libusb_device_handle *devh,
 		unsigned int *usb_read_ep,
 		unsigned int *usb_write_ep,
-		int bclass, int subclass, int protocol);
+		int bclass, int subclass, int protocol, int trans_type);
 int jtag_libusb_get_pid(struct jtag_libusb_device *dev, uint16_t *pid);
 
 #endif /* OPENOCD_JTAG_DRIVERS_LIBUSB0_COMMON_H */
diff --git a/src/jtag/drivers/libusb1_common.c b/src/jtag/drivers/libusb1_common.c
index f8b7c75..89f8092 100644
--- a/src/jtag/drivers/libusb1_common.c
+++ b/src/jtag/drivers/libusb1_common.c
@@ -187,7 +187,7 @@ int jtag_libusb_set_configuration(jtag_libusb_device_handle *devh,
 int jtag_libusb_choose_interface(struct jtag_libusb_device_handle *devh,
 		unsigned int *usb_read_ep,
 		unsigned int *usb_write_ep,
-		int bclass, int subclass, int protocol)
+		int bclass, int subclass, int protocol, int trans_type)
 {
 	struct jtag_libusb_device *udev = jtag_libusb_get_device(devh);
 	const struct libusb_interface *inter;
@@ -210,6 +210,8 @@ int jtag_libusb_choose_interface(struct jtag_libusb_device_handle *devh,
 				continue;
 
 			epdesc = &interdesc->endpoint[k];
+			if (trans_type > 0 && (epdesc->bmAttributes & 0x3) != trans_type)
+				continue;
 
 			uint8_t epnum = epdesc->bEndpointAddress;
 			bool is_input = epnum & 0x80;
diff --git a/src/jtag/drivers/libusb1_common.h b/src/jtag/drivers/libusb1_common.h
index fc6526a..7c73d29 100644
--- a/src/jtag/drivers/libusb1_common.h
+++ b/src/jtag/drivers/libusb1_common.h
@@ -69,12 +69,13 @@ int jtag_libusb_set_configuration(jtag_libusb_device_handle *devh,
  * @param bclass `bInterfaceClass` to match, or -1 to ignore this field.
  * @param subclass `bInterfaceSubClass` to match, or -1 to ignore this field.
  * @param protocol `bInterfaceProtocol` to match, or -1 to ignore this field.
+ * @param trans_type `bmAttributes Bits 0..1 Transfer type` to match, or -1 to ignore this field.
  * @returns Returns ERROR_OK on success, ERROR_FAIL otherwise.
  */
 int jtag_libusb_choose_interface(struct jtag_libusb_device_handle *devh,
 		unsigned int *usb_read_ep,
 		unsigned int *usb_write_ep,
-		int bclass, int subclass, int protocol);
+		int bclass, int subclass, int protocol, int trans_type);
 int jtag_libusb_get_pid(struct jtag_libusb_device *dev, uint16_t *pid);
 
 #endif /* OPENOCD_JTAG_DRIVERS_LIBUSB1_COMMON_H */
diff --git a/src/jtag/drivers/libusb_common.h b/src/jtag/drivers/libusb_common.h
index 563af10..599a0a9 100644
--- a/src/jtag/drivers/libusb_common.h
+++ b/src/jtag/drivers/libusb_common.h
@@ -19,9 +19,9 @@
 #define OPENOCD_JTAG_DRIVERS_LIBUSB_COMMON_H
 
 #ifdef HAVE_LIBUSB1
-#include <libusb1_common.h>
+#include "libusb1_common.h"
 #else
-#include <libusb0_common.h>
+#include "libusb0_common.h"
 #endif
 
 #endif /* OPENOCD_JTAG_DRIVERS_LIBUSB_COMMON_H */
diff --git a/src/jtag/drivers/mpsse.c b/src/jtag/drivers/mpsse.c
index 0ef88ba..924c974 100644
--- a/src/jtag/drivers/mpsse.c
+++ b/src/jtag/drivers/mpsse.c
@@ -247,8 +247,8 @@ static bool open_matching_device(struct mpsse_ctx *ctx, const uint16_t *vid, con
 	err = libusb_detach_kernel_driver(ctx->usb_dev, ctx->interface);
 	if (err != LIBUSB_SUCCESS && err != LIBUSB_ERROR_NOT_FOUND
 			&& err != LIBUSB_ERROR_NOT_SUPPORTED) {
-		LOG_ERROR("libusb_detach_kernel_driver() failed with %s", libusb_error_name(err));
-		goto error;
+		LOG_WARNING("libusb_detach_kernel_driver() failed with %s, trying to continue anyway",
+			libusb_error_name(err));
 	}
 
 	err = libusb_claim_interface(ctx->usb_dev, ctx->interface);
@@ -872,6 +872,8 @@ int mpsse_flush(struct mpsse_ctx *ctx)
 	libusb_fill_bulk_transfer(write_transfer, ctx->usb_dev, ctx->out_ep, ctx->write_buffer,
 		ctx->write_count, write_cb, &write_result, ctx->usb_write_timeout);
 	retval = libusb_submit_transfer(write_transfer);
+	if (retval != LIBUSB_SUCCESS)
+		goto error_check;
 
 	if (ctx->read_count) {
 		read_transfer = libusb_alloc_transfer(0);
@@ -879,22 +881,36 @@ int mpsse_flush(struct mpsse_ctx *ctx)
 			ctx->read_chunk_size, read_cb, &read_result,
 			ctx->usb_read_timeout);
 		retval = libusb_submit_transfer(read_transfer);
+		if (retval != LIBUSB_SUCCESS)
+			goto error_check;
 	}
 
 	/* Polling loop, more or less taken from libftdi */
 	while (!write_result.done || !read_result.done) {
-		retval = libusb_handle_events(ctx->usb_ctx);
+		struct timeval timeout_usb;
+
+		timeout_usb.tv_sec = 1;
+		timeout_usb.tv_usec = 0;
+
+		retval = libusb_handle_events_timeout_completed(ctx->usb_ctx, &timeout_usb, NULL);
 		keep_alive();
-		if (retval != LIBUSB_SUCCESS && retval != LIBUSB_ERROR_INTERRUPTED) {
+		if (retval == LIBUSB_ERROR_NO_DEVICE || retval == LIBUSB_ERROR_INTERRUPTED)
+			break;
+
+		if (retval != LIBUSB_SUCCESS) {
 			libusb_cancel_transfer(write_transfer);
 			if (read_transfer)
 				libusb_cancel_transfer(read_transfer);
-			while (!write_result.done || !read_result.done)
-				if (libusb_handle_events(ctx->usb_ctx) != LIBUSB_SUCCESS)
+			while (!write_result.done || !read_result.done) {
+				retval = libusb_handle_events_timeout_completed(ctx->usb_ctx,
+								&timeout_usb, NULL);
+				if (retval != LIBUSB_SUCCESS)
 					break;
+			}
 		}
 	}
 
+error_check:
 	if (retval != LIBUSB_SUCCESS) {
 		LOG_ERROR("libusb_handle_events() failed with %s", libusb_error_name(retval));
 		retval = ERROR_FAIL;
diff --git a/src/jtag/drivers/openjtag.c b/src/jtag/drivers/openjtag.c
index 157020e..8f11b4b 100644
--- a/src/jtag/drivers/openjtag.c
+++ b/src/jtag/drivers/openjtag.c
@@ -2,6 +2,10 @@
  *   Driver for OpenJTAG Project (www.openjtag.org)                            *
  *   Compatible with libftdi and ftd2xx drivers.                               *
  *                                                                             *
+ *   Cypress CY7C65215 support                                                 *
+ *   Copyright (C) 2015 Vianney le Clément de Saint-Marcq, Essensium NV        *
+ *                      <vianney.leclement@essensium.com>                      *
+ *                                                                             *
  *   Copyright (C) 2010 by Ivan Meleca <mileca@gmail.com>                      *
  *                                                                             *
  *   Copyright (C) 2013 by Ryan Corbin, GlueLogix Inc. <corbin.ryan@gmail.com> *
@@ -41,7 +45,18 @@
 
 #include <jtag/interface.h>
 #include <jtag/commands.h>
-#include "usb_common.h"
+#include "libusb_common.h"
+
+static enum {
+	OPENJTAG_VARIANT_STANDARD,
+	OPENJTAG_VARIANT_CY7C65215,
+} openjtag_variant = OPENJTAG_VARIANT_STANDARD;
+
+static const char * const openjtag_variant_names[] = {
+	"standard",
+	"cy7c65215",
+	NULL
+};
 
 /*
  * OpenJTAG-OpenOCD state conversion
@@ -66,19 +81,8 @@ typedef enum openjtag_tap_state {
 	OPENJTAG_TAP_UPDATE_IR  = 15,
 } openjtag_tap_state_t;
 
-#if (BUILD_OPENJTAG_FTD2XX == 1 && BUILD_OPENJTAG_LIBFTDI == 1)
-#error "BUILD_OPENJTAG_FTD2XX && BUILD_OPENJTAG_LIBFTDI "
-	   "are mutually exclusive"
-#elif (BUILD_OPENJTAG_FTD2XX != 1 && BUILD_OPENJTAG_LIBFTDI != 1)
-#error "BUILD_OPENJTAG_FTD2XX || BUILD_OPENJTAG_LIBFTDI must be chosen"
-#endif
-
 /* OPENJTAG access library includes */
-#if BUILD_OPENJTAG_FTD2XX == 1
-#include <ftd2xx.h>
-#elif BUILD_OPENJTAG_LIBFTDI == 1
 #include <ftdi.h>
-#endif
 
 /* OpenJTAG vid/pid */
 static uint16_t openjtag_vid = 0x0403;
@@ -86,12 +90,7 @@ static uint16_t openjtag_pid = 0x6001;
 
 static char *openjtag_device_desc;
 
-#if BUILD_OPENJTAG_FTD2XX == 1
-static FT_HANDLE ftdih;
-
-#elif BUILD_OPENJTAG_LIBFTDI == 1
 static struct ftdi_context ftdic;
-#endif
 
 #define OPENJTAG_BUFFER_SIZE        504
 #define OPENJTAG_MAX_PENDING_RESULTS    256
@@ -112,10 +111,24 @@ static uint8_t usb_rx_buf[OPENJTAG_BUFFER_SIZE];
 static struct openjtag_scan_result openjtag_scan_result_buffer[OPENJTAG_MAX_PENDING_RESULTS];
 static int openjtag_scan_result_count;
 
-/* Openocd usb handler */
-struct openocd {
-	struct usb_dev_handle *usb_handle;
-};
+static jtag_libusb_device_handle *usbh;
+
+/* CY7C65215 model only */
+#define CY7C65215_JTAG_REQUEST  0x40  /* bmRequestType: vendor host-to-device */
+#define CY7C65215_JTAG_ENABLE   0xD0  /* bRequest: enable JTAG */
+#define CY7C65215_JTAG_DISABLE  0xD1  /* bRequest: disable JTAG */
+#define CY7C65215_JTAG_READ     0xD2  /* bRequest: read buffer */
+#define CY7C65215_JTAG_WRITE    0xD3  /* bRequest: write buffer */
+
+#define CY7C65215_USB_TIMEOUT   100
+
+static const uint16_t cy7c65215_vids[] = {0x04b4, 0};
+static const uint16_t cy7c65215_pids[] = {0x0007, 0};
+
+#define CY7C65215_JTAG_CLASS     0xff
+#define CY7C65215_JTAG_SUBCLASS  0x04
+
+static unsigned int ep_in, ep_out;
 
 #ifdef _DEBUG_USB_COMMS_
 
@@ -201,26 +214,9 @@ static int8_t openjtag_get_tap_state(int8_t state)
 	}
 }
 
-static int openjtag_buf_write(
+static int openjtag_buf_write_standard(
 	uint8_t *buf, int size, uint32_t *bytes_written)
 {
-#if BUILD_OPENJTAG_FTD2XX == 1
-	FT_STATUS status;
-	DWORD dw_bytes_written;
-
-#ifdef _DEBUG_USB_COMMS_
-	openjtag_debug_buffer(buf, size, DEBUG_TYPE_WRITE);
-#endif
-
-	status = FT_Write(ftdih, buf, size, &dw_bytes_written);
-	if (status != FT_OK) {
-		*bytes_written = dw_bytes_written;
-		LOG_ERROR("FT_Write returned: %u", status);
-		return ERROR_JTAG_DEVICE_ERROR;
-	}
-	*bytes_written = dw_bytes_written;
-	return ERROR_OK;
-#elif BUILD_OPENJTAG_LIBFTDI == 1
 	int retval;
 #ifdef _DEBUG_USB_COMMS_
 	openjtag_debug_buffer(buf, size, DEBUG_TYPE_WRITE);
@@ -236,36 +232,56 @@ static int openjtag_buf_write(
 	*bytes_written += retval;
 
 	return ERROR_OK;
-#endif
 }
 
-static int openjtag_buf_read(uint8_t *buf, uint32_t qty, uint32_t *bytes_read)
+static int openjtag_buf_write_cy7c65215(
+	uint8_t *buf, int size, uint32_t *bytes_written)
 {
+	int ret;
 
-#if BUILD_OPENJTAG_FTD2XX == 1
-	DWORD dw_bytes_read;
-	FT_STATUS status;
-	int timeout = 50;
+#ifdef _DEBUG_USB_COMMS_
+	openjtag_debug_buffer(buf, size, DEBUG_TYPE_WRITE);
+#endif
 
-	*bytes_read = 0;
-	while (qty && (*bytes_read < qty) && timeout--) {
+	if (size == 0) {
+		*bytes_written = 0;
+		return ERROR_OK;
+	}
 
-		status = FT_Read(ftdih, buf + *bytes_read,
-				qty - *bytes_read, &dw_bytes_read);
-		if (status != FT_OK) {
-			*bytes_read = dw_bytes_read;
-			LOG_ERROR("FT_Read returned: %u", status);
-			return ERROR_JTAG_DEVICE_ERROR;
-		}
-		*bytes_read += dw_bytes_read;
+	ret = jtag_libusb_control_transfer(usbh, CY7C65215_JTAG_REQUEST,
+									   CY7C65215_JTAG_WRITE, size, 0,
+									   NULL, 0, CY7C65215_USB_TIMEOUT);
+	if (ret < 0) {
+		LOG_ERROR("vendor command failed, error %d", ret);
+		return ERROR_JTAG_DEVICE_ERROR;
 	}
 
-#ifdef _DEBUG_USB_COMMS_
-	openjtag_debug_buffer(buf, *bytes_read, DEBUG_TYPE_READ);
-#endif
+	ret = jtag_libusb_bulk_write(usbh, ep_out, (char *)buf, size,
+								 CY7C65215_USB_TIMEOUT);
+	if (ret < 0) {
+		LOG_ERROR("bulk write failed, error %d", ret);
+		return ERROR_JTAG_DEVICE_ERROR;
+	}
+	*bytes_written = ret;
 
 	return ERROR_OK;
-#elif BUILD_OPENJTAG_LIBFTDI == 1
+}
+
+static int openjtag_buf_write(
+	uint8_t *buf, int size, uint32_t *bytes_written)
+{
+	switch (openjtag_variant) {
+	case OPENJTAG_VARIANT_CY7C65215:
+		return openjtag_buf_write_cy7c65215(buf, size, bytes_written);
+	default:
+		return openjtag_buf_write_standard(buf, size, bytes_written);
+	}
+}
+
+static int openjtag_buf_read_standard(
+	uint8_t *buf, uint32_t qty, uint32_t *bytes_read)
+{
+
 	int retval;
 	int timeout = 5;
 
@@ -287,10 +303,53 @@ static int openjtag_buf_read(uint8_t *buf, uint32_t qty, uint32_t *bytes_read)
 	openjtag_debug_buffer(buf, *bytes_read, DEBUG_TYPE_READ);
 #endif
 
+	return ERROR_OK;
+}
+
+static int openjtag_buf_read_cy7c65215(
+	uint8_t *buf, uint32_t qty, uint32_t *bytes_read)
+{
+	int ret;
+
+	if (qty == 0) {
+		*bytes_read = 0;
+		goto out;
+	}
+
+	ret = jtag_libusb_control_transfer(usbh, CY7C65215_JTAG_REQUEST,
+									   CY7C65215_JTAG_READ, qty, 0,
+									   NULL, 0, CY7C65215_USB_TIMEOUT);
+	if (ret < 0) {
+		LOG_ERROR("vendor command failed, error %d", ret);
+		return ERROR_JTAG_DEVICE_ERROR;
+	}
+
+	ret = jtag_libusb_bulk_read(usbh, ep_in, (char *)buf, qty,
+								CY7C65215_USB_TIMEOUT);
+	if (ret < 0) {
+		LOG_ERROR("bulk read failed, error %d", ret);
+		return ERROR_JTAG_DEVICE_ERROR;
+	}
+	*bytes_read = ret;
+
+out:
+#ifdef _DEBUG_USB_COMMS_
+	openjtag_debug_buffer(buf, *bytes_read, DEBUG_TYPE_READ);
 #endif
+
 	return ERROR_OK;
 }
 
+static int openjtag_buf_read(uint8_t *buf, uint32_t qty, uint32_t *bytes_read)
+{
+	switch (openjtag_variant) {
+	case OPENJTAG_VARIANT_CY7C65215:
+		return openjtag_buf_read_cy7c65215(buf, qty, bytes_read);
+	default:
+		return openjtag_buf_read_standard(buf, qty, bytes_read);
+	}
+}
+
 static int openjtag_sendcommand(uint8_t cmd)
 {
 	uint32_t written;
@@ -335,109 +394,17 @@ static int openjtag_speed(int speed)
 	return ERROR_OK;
 }
 
-static int openjtag_init(void)
+static int openjtag_init_standard(void)
 {
 	uint8_t latency_timer;
 
-#if BUILD_OPENJTAG_FTD2XX == 1
-	FT_STATUS status;
-#endif
-
-usb_tx_buf_offs = 0;
-usb_rx_buf_len = 0;
-openjtag_scan_result_count = 0;
-
-#if BUILD_OPENJTAG_FTD2XX == 1
-	LOG_DEBUG("'openjtag' interface using FTD2XX");
-#elif BUILD_OPENJTAG_LIBFTDI == 1
-	LOG_DEBUG("'openjtag' interface using libftdi");
-#endif
-
-/* Open by device description */
-if (openjtag_device_desc == NULL) {
-	LOG_WARNING("no openjtag device description specified, "
+	/* Open by device description */
+	if (openjtag_device_desc == NULL) {
+		LOG_WARNING("no openjtag device description specified, "
 				"using default 'Open JTAG Project'");
-	openjtag_device_desc = "Open JTAG Project";
-}
-
-#if BUILD_OPENJTAG_FTD2XX == 1
-
-#if IS_WIN32 == 0
-	/* Add non-standard Vid/Pid to the linux driver */
-	status = FT_SetVIDPID(openjtag_vid, openjtag_pid);
-	if (status != FT_OK) {
-		LOG_WARNING("couldn't add %4.4x:%4.4x",
-			openjtag_vid, openjtag_pid);
-	}
-#endif
-
-	status = FT_OpenEx(openjtag_device_desc, FT_OPEN_BY_DESCRIPTION,
-			&ftdih);
-	if (status != FT_OK) {
-		DWORD num_devices;
-
-		LOG_ERROR("unable to open ftdi device: %u", status);
-		status = FT_ListDevices(&num_devices, NULL,
-				FT_LIST_NUMBER_ONLY);
-		if (status == FT_OK) {
-			char **desc_array = malloc(sizeof(char *)
-						* (num_devices + 1));
-			unsigned int i;
-
-			for (i = 0; i < num_devices; i++)
-				desc_array[i] = malloc(64);
-			desc_array[num_devices] = NULL;
-
-			status = FT_ListDevices(desc_array, &num_devices,
-				FT_LIST_ALL | FT_OPEN_BY_DESCRIPTION);
-
-			if (status == FT_OK) {
-				LOG_ERROR("ListDevices: %u\n", num_devices);
-				for (i = 0; i < num_devices; i++)
-					LOG_ERROR("%i: %s", i, desc_array[i]);
-			}
-
-			for (i = 0; i < num_devices; i++)
-				free(desc_array[i]);
-			free(desc_array);
-		} else {
-			LOG_ERROR("ListDevices: NONE\n");
-		}
-		return ERROR_JTAG_INIT_FAILED;
-	}
-
-	status = FT_SetLatencyTimer(ftdih, 2);
-	if (status != FT_OK) {
-		LOG_ERROR("unable to set latency timer: %u", status);
-		return ERROR_JTAG_INIT_FAILED;
-	}
-
-	status = FT_GetLatencyTimer(ftdih, &latency_timer);
-	if (status != FT_OK) {
-		LOG_ERROR("unable to get latency timer: %u", status);
-		return ERROR_JTAG_INIT_FAILED;
-	}
-	LOG_DEBUG("current latency timer: %i", latency_timer);
-
-	status = FT_SetBitMode(ftdih, 0x00, 0x40);
-	if (status != FT_OK) {
-		LOG_ERROR("unable to disable bit i/o mode: %u", status);
-		return ERROR_JTAG_INIT_FAILED;
-	}
-
-	status = FT_SetTimeouts(ftdih, 50, 0);
-	if (status != FT_OK) {
-		LOG_ERROR("unable to set timeouts: %u", status);
-		return ERROR_JTAG_INIT_FAILED;
-	}
-
-	status = FT_Purge(ftdih, FT_PURGE_RX | FT_PURGE_TX);
-	if (status != FT_OK) {
-		LOG_ERROR("unable to FT_Purge() %u", status);
-		return ERROR_JTAG_INIT_FAILED;
+		openjtag_device_desc = "Open JTAG Project";
 	}
 
-#elif BUILD_OPENJTAG_LIBFTDI == 1
 	if (ftdi_init(&ftdic) < 0)
 		return ERROR_JTAG_INIT_FAILED;
 
@@ -470,40 +437,109 @@ if (openjtag_device_desc == NULL) {
 			ftdi_get_error_string(&ftdic));
 		return ERROR_JTAG_DEVICE_ERROR;
 	}
-#endif
 
-#if BUILD_OPENJTAG_FTD2XX == 1
-	status = FT_Purge(ftdih, FT_PURGE_RX | FT_PURGE_TX);
-	if (status != FT_OK)
-		return ERROR_JTAG_INIT_FAILED;
-#elif BUILD_OPENJTAG_LIBFTDI == 1
 	if (ftdi_usb_purge_buffers(&ftdic) < 0) {
 		LOG_ERROR("ftdi_purge_buffers: %s", ftdic.error_str);
 		return ERROR_JTAG_INIT_FAILED;
 	}
-#endif
 
-	/* OpenJTAG speed */
-	openjtag_sendcommand(0xE0); /*Start at slowest adapter speed*/
+	return ERROR_OK;
+}
+
+static int openjtag_init_cy7c65215(void)
+{
+	int ret;
+
+	usbh = NULL;
+	ret = jtag_libusb_open(cy7c65215_vids, cy7c65215_pids, NULL, &usbh);
+	if (ret != ERROR_OK) {
+		LOG_ERROR("unable to open cy7c65215 device");
+		goto err;
+	}
+
+	ret = jtag_libusb_choose_interface(usbh, &ep_in, &ep_out,
+									   CY7C65215_JTAG_CLASS,
+									   CY7C65215_JTAG_SUBCLASS, -1, LIBUSB_TRANSFER_TYPE_BULK);
+	if (ret != ERROR_OK) {
+		LOG_ERROR("unable to claim JTAG interface");
+		goto err;
+	}
 
-	/* MSB */
-	openjtag_sendcommand(0x75);
+	ret = jtag_libusb_control_transfer(usbh,
+									   CY7C65215_JTAG_REQUEST,
+									   CY7C65215_JTAG_ENABLE,
+									   0, 0, NULL, 0, CY7C65215_USB_TIMEOUT);
+	if (ret < 0) {
+		LOG_ERROR("could not enable JTAG module");
+		goto err;
+	}
 
 	return ERROR_OK;
+
+err:
+	if (usbh != NULL)
+		jtag_libusb_close(usbh);
+	return ERROR_JTAG_INIT_FAILED;
 }
 
-static int openjtag_quit(void)
+static int openjtag_init(void)
+{
+	int ret;
+
+	usb_tx_buf_offs = 0;
+	usb_rx_buf_len = 0;
+	openjtag_scan_result_count = 0;
+
+	switch (openjtag_variant) {
+	case OPENJTAG_VARIANT_CY7C65215:
+		ret = openjtag_init_cy7c65215();
+		break;
+	default:
+		ret = openjtag_init_standard();
+	}
+	if (ret != ERROR_OK)
+		return ret;
+
+	openjtag_speed(375); /* Start at slowest adapter speed */
+	openjtag_sendcommand(0x75); /* MSB */
+
+	return ERROR_OK;
+}
+
+static int openjtag_quit_standard(void)
 {
-#if BUILD_OPENJTAG_FTD2XX == 1
-	FT_Close(ftdih);
-#elif BUILD_OPENJTAG_LIBFTDI == 1
 	ftdi_usb_close(&ftdic);
 	ftdi_deinit(&ftdic);
-#endif
 
 	return ERROR_OK;
 }
 
+static int openjtag_quit_cy7c65215(void)
+{
+	int ret;
+
+	ret = jtag_libusb_control_transfer(usbh,
+									   CY7C65215_JTAG_REQUEST,
+									   CY7C65215_JTAG_DISABLE,
+									   0, 0, NULL, 0, CY7C65215_USB_TIMEOUT);
+	if (ret < 0)
+		LOG_WARNING("could not disable JTAG module");
+
+	jtag_libusb_close(usbh);
+
+	return ERROR_OK;
+}
+
+static int openjtag_quit(void)
+{
+	switch (openjtag_variant) {
+	case OPENJTAG_VARIANT_CY7C65215:
+		return openjtag_quit_cy7c65215();
+	default:
+		return openjtag_quit_standard();
+	}
+}
+
 static void openjtag_write_tap_buffer(void)
 {
 	uint32_t written;
@@ -536,8 +572,8 @@ static int openjtag_execute_tap_queue(void)
 
 			uint8_t *buffer = openjtag_scan_result_buffer[res_count].buffer;
 
-			while (len) {
-				if (len <= 8) {
+			while (len > 0) {
+				if (len <= 8 && openjtag_variant != OPENJTAG_VARIANT_CY7C65215) {
 					DEBUG_JTAG_IO("bits < 8 buf = 0x%X, will be 0x%X",
 						usb_rx_buf[rx_offs], usb_rx_buf[rx_offs] >> (8 - len));
 					buffer[count] = usb_rx_buf[rx_offs] >> (8 - len);
@@ -724,11 +760,14 @@ static void openjtag_execute_runtest(struct jtag_command *cmd)
 	if (cmd->cmd.runtest->num_cycles > 16)
 		LOG_WARNING("num_cycles > 16 on run test");
 
-	uint8_t command;
-	command = 7;
-	command |= ((cmd->cmd.runtest->num_cycles - 1) & 0x0F) << 4;
+	if (openjtag_variant != OPENJTAG_VARIANT_CY7C65215 ||
+		cmd->cmd.runtest->num_cycles) {
+		uint8_t command;
+		command = 7;
+		command |= ((cmd->cmd.runtest->num_cycles - 1) & 0x0F) << 4;
 
-	openjtag_add_byte(command);
+		openjtag_add_byte(command);
+	}
 
 	tap_set_end_state(end_state);
 	if (tap_get_end_state() != tap_get_state()) {
@@ -816,6 +855,24 @@ COMMAND_HANDLER(openjtag_handle_device_desc_command)
 	return ERROR_OK;
 }
 
+COMMAND_HANDLER(openjtag_handle_variant_command)
+{
+	if (CMD_ARGC == 1) {
+		const char * const *name = openjtag_variant_names;
+		int variant = 0;
+		for (; *name; name++, variant++) {
+			if (strcasecmp(CMD_ARGV[0], *name) == 0) {
+				openjtag_variant = variant;
+				return ERROR_OK;
+			}
+		}
+		LOG_ERROR("unknown openjtag variant '%s'", CMD_ARGV[0]);
+	} else {
+		LOG_ERROR("require exactly one argument to "
+				"openjtag_variant <variant>");
+	}
+	return ERROR_OK;
+}
 
 static const struct command_registration openjtag_command_handlers[] = {
 	{
@@ -825,6 +882,13 @@ static const struct command_registration openjtag_command_handlers[] = {
 		.help = "set the USB device description of the OpenJTAG",
 		.usage = "description-string",
 	},
+	{
+		.name = "openjtag_variant",
+		.handler = openjtag_handle_variant_command,
+		.mode = COMMAND_CONFIG,
+		.help = "set the OpenJTAG variant",
+		.usage = "variant-string",
+	},
 	COMMAND_REGISTRATION_DONE
 };
 
diff --git a/src/jtag/drivers/presto.c b/src/jtag/drivers/presto.c
index 0a18dc9..49caa67 100644
--- a/src/jtag/drivers/presto.c
+++ b/src/jtag/drivers/presto.c
@@ -34,14 +34,7 @@
 #include "bitq.h"
 
 /* PRESTO access library includes */
-#if BUILD_PRESTO_FTD2XX == 1
-#include <ftd2xx.h>
-#include "ftd2xx_common.h"
-#elif BUILD_PRESTO_LIBFTDI == 1
 #include <ftdi.h>
-#else
-#error "BUG: either FTD2XX and LIBFTDI has to be used"
-#endif
 
 /* -------------------------------------------------------------------------- */
 
@@ -55,13 +48,8 @@
 #define BUFFER_SIZE (64*62)
 
 struct presto {
-#if BUILD_PRESTO_FTD2XX == 1
-	FT_HANDLE handle;
-	FT_STATUS status;
-#elif BUILD_PRESTO_LIBFTDI == 1
 	struct ftdi_context ftdic;
 	int retval;
-#endif
 
 	char serial[FT_DEVICE_SERNUM_LEN];
 
@@ -95,15 +83,6 @@ static uint8_t presto_init_seq[] = {
 
 static int presto_write(uint8_t *buf, uint32_t size)
 {
-#if BUILD_PRESTO_FTD2XX == 1
-	DWORD ftbytes;
-	presto->status = FT_Write(presto->handle, buf, size, &ftbytes);
-	if (presto->status != FT_OK) {
-		LOG_ERROR("FT_Write returned: %s", ftd2xx_status_string(presto->status));
-		return ERROR_JTAG_DEVICE_ERROR;
-	}
-
-#elif BUILD_PRESTO_LIBFTDI == 1
 	uint32_t ftbytes;
 	presto->retval = ftdi_write_data(&presto->ftdic, buf, size);
 	if (presto->retval < 0) {
@@ -111,7 +90,6 @@ static int presto_write(uint8_t *buf, uint32_t size)
 		return ERROR_JTAG_DEVICE_ERROR;
 	}
 	ftbytes = presto->retval;
-#endif
 
 	if (ftbytes != size) {
 		LOG_ERROR("couldn't write the requested number of bytes to PRESTO (%u < %u)",
@@ -124,15 +102,6 @@ static int presto_write(uint8_t *buf, uint32_t size)
 
 static int presto_read(uint8_t *buf, uint32_t size)
 {
-#if BUILD_PRESTO_FTD2XX == 1
-	DWORD ftbytes;
-	presto->status = FT_Read(presto->handle, buf, size, &ftbytes);
-	if (presto->status != FT_OK) {
-		LOG_ERROR("FT_Read returned: %s", ftd2xx_status_string(presto->status));
-		return ERROR_JTAG_DEVICE_ERROR;
-	}
-
-#elif BUILD_PRESTO_LIBFTDI == 1
 	uint32_t ftbytes = 0;
 
 	struct timeval timeout, now;
@@ -152,7 +121,6 @@ static int presto_read(uint8_t *buf, uint32_t size)
 				((now.tv_sec == timeout.tv_sec) && (now.tv_usec > timeout.tv_usec)))
 			break;
 	}
-#endif
 
 	if (ftbytes != size) {
 		/* this is just a warning, there might have been timeout when detecting PRESTO,
@@ -165,150 +133,6 @@ static int presto_read(uint8_t *buf, uint32_t size)
 	return ERROR_OK;
 }
 
-#if BUILD_PRESTO_FTD2XX == 1
-static int presto_open_ftd2xx(char *req_serial)
-{
-	uint32_t i;
-	DWORD numdevs;
-	DWORD vidpid;
-	char devname[FT_DEVICE_NAME_LEN];
-	FT_DEVICE device;
-
-	BYTE presto_data;
-	DWORD ftbytes;
-
-	presto->handle = (FT_HANDLE)INVALID_HANDLE_VALUE;
-
-#if IS_WIN32 == 0
-	/* Add non-standard Vid/Pid to the linux driver */
-	presto->status = FT_SetVIDPID(PRESTO_VID, PRESTO_PID);
-	if (presto->status != FT_OK) {
-		LOG_ERROR("couldn't add PRESTO VID/PID");
-		exit(-1);
-	}
-#endif
-
-	presto->status = FT_ListDevices(&numdevs, NULL, FT_LIST_NUMBER_ONLY);
-	if (presto->status != FT_OK) {
-		LOG_ERROR("FT_ListDevices failed: %s", ftd2xx_status_string(presto->status));
-		return ERROR_JTAG_DEVICE_ERROR;
-	}
-
-	LOG_DEBUG("FTDI devices available: %" PRIu32, (uint32_t)numdevs);
-	for (i = 0; i < numdevs; i++) {
-		presto->status = FT_Open(i, &(presto->handle));
-		if (presto->status != FT_OK) {
-			/* this is not fatal, the device may be legitimately open by other process,
-			 *hence debug message only */
-			LOG_DEBUG("FT_Open failed: %s", ftd2xx_status_string(presto->status));
-			continue;
-		}
-		LOG_DEBUG("FTDI device %i open", (int)i);
-
-		presto->status = FT_GetDeviceInfo(presto->handle, &device,
-				&vidpid, presto->serial, devname, NULL);
-		if (presto->status == FT_OK) {
-			if (vidpid == PRESTO_VID_PID && (req_serial == NULL ||
-					!strcmp(presto->serial, req_serial)))
-				break;
-		} else
-			LOG_DEBUG("FT_GetDeviceInfo failed: %s", ftd2xx_status_string(
-					presto->status));
-
-		LOG_DEBUG("FTDI device %i does not match, closing", (int)i);
-		FT_Close(presto->handle);
-		presto->handle = (FT_HANDLE)INVALID_HANDLE_VALUE;
-	}
-
-	if (presto->handle == (FT_HANDLE)INVALID_HANDLE_VALUE)
-		return ERROR_JTAG_DEVICE_ERROR;	/* presto not open, return */
-
-	presto->status = FT_SetLatencyTimer(presto->handle, 1);
-	if (presto->status != FT_OK)
-		return ERROR_JTAG_DEVICE_ERROR;
-
-	presto->status = FT_SetTimeouts(presto->handle, 100, 0);
-	if (presto->status != FT_OK)
-		return ERROR_JTAG_DEVICE_ERROR;
-
-	presto->status = FT_Purge(presto->handle, FT_PURGE_TX | FT_PURGE_RX);
-	if (presto->status != FT_OK)
-		return ERROR_JTAG_DEVICE_ERROR;
-
-	presto_data = 0xD0;
-	presto->status = FT_Write(presto->handle, &presto_data, 1, &ftbytes);
-	if (presto->status != FT_OK)
-		return ERROR_JTAG_DEVICE_ERROR;
-
-	/* delay between first write/read turnaround (after purge?) necessary
-	 * under Linux for unknown reason,
-	 * probably a bug in library threading */
-	usleep(100000);
-	presto->status = FT_Read(presto->handle, &presto_data, 1, &ftbytes);
-	if (presto->status != FT_OK)
-		return ERROR_JTAG_DEVICE_ERROR;
-
-	if (ftbytes != 1) {
-		LOG_DEBUG("PRESTO reset");
-
-		presto->status = FT_Purge(presto->handle, FT_PURGE_TX | FT_PURGE_RX);
-		if (presto->status != FT_OK)
-			return ERROR_JTAG_DEVICE_ERROR;
-		presto->status = FT_SetBitMode(presto->handle, 0x80, 1);
-		if (presto->status != FT_OK)
-			return ERROR_JTAG_DEVICE_ERROR;
-		presto->status = FT_SetBaudRate(presto->handle, 9600);
-		if (presto->status != FT_OK)
-			return ERROR_JTAG_DEVICE_ERROR;
-
-		presto_data = 0;
-		for (i = 0; i < 4 * 62; i++) {
-			presto->status = FT_Write(presto->handle, &presto_data, 1, &ftbytes);
-			if (presto->status != FT_OK)
-				return ERROR_JTAG_DEVICE_ERROR;
-		}
-		usleep(100000);
-
-		presto->status = FT_SetBitMode(presto->handle, 0x00, 0);
-		if (presto->status != FT_OK)
-			return ERROR_JTAG_DEVICE_ERROR;
-
-		presto->status = FT_Purge(presto->handle, FT_PURGE_TX | FT_PURGE_RX);
-		if (presto->status != FT_OK)
-			return ERROR_JTAG_DEVICE_ERROR;
-
-		presto_data = 0xD0;
-		presto->status = FT_Write(presto->handle, &presto_data, 1, &ftbytes);
-		if (presto->status != FT_OK)
-			return ERROR_JTAG_DEVICE_ERROR;
-
-		/* delay between first write/read turnaround (after purge?) necessary under Linux for unknown reason,
-		   probably a bug in library threading */
-		usleep(100000);
-		presto->status = FT_Read(presto->handle, &presto_data, 1, &ftbytes);
-		if (presto->status != FT_OK)
-			return ERROR_JTAG_DEVICE_ERROR;
-
-		if (ftbytes != 1) {
-			LOG_DEBUG("PRESTO not responding");
-			return ERROR_JTAG_DEVICE_ERROR;
-		}
-	}
-
-	presto->status = FT_SetTimeouts(presto->handle, 0, 0);
-	if (presto->status != FT_OK)
-		return ERROR_JTAG_DEVICE_ERROR;
-
-	presto->status = FT_Write(presto->handle, &presto_init_seq,
-			sizeof(presto_init_seq), &ftbytes);
-
-	if (presto->status != FT_OK || ftbytes != sizeof(presto_init_seq))
-		return ERROR_JTAG_DEVICE_ERROR;
-
-	return ERROR_OK;
-}
-
-#elif BUILD_PRESTO_LIBFTDI == 1
 static int presto_open_libftdi(char *req_serial)
 {
 	uint8_t presto_data;
@@ -371,7 +195,6 @@ static int presto_open_libftdi(char *req_serial)
 
 	return ERROR_OK;
 }
-#endif	/* BUILD_PRESTO_LIBFTDI == 1 */
 
 static int presto_open(char *req_serial)
 {
@@ -391,11 +214,7 @@ static int presto_open(char *req_serial)
 
 	presto->jtag_speed = 0;
 
-#if BUILD_PRESTO_FTD2XX == 1
-	return presto_open_ftd2xx(req_serial);
-#elif BUILD_PRESTO_LIBFTDI == 1
 	return presto_open_libftdi(req_serial);
-#endif
 }
 
 static int presto_close(void)
@@ -403,35 +222,6 @@ static int presto_close(void)
 
 	int result = ERROR_OK;
 
-#if BUILD_PRESTO_FTD2XX == 1
-	DWORD ftbytes;
-
-	if (presto->handle == (FT_HANDLE)INVALID_HANDLE_VALUE)
-		return result;
-
-	presto->status = FT_Purge(presto->handle, FT_PURGE_TX | FT_PURGE_RX);
-	if (presto->status != FT_OK)
-		result = ERROR_JTAG_DEVICE_ERROR;
-
-	presto->status = FT_Write(presto->handle,
-			&presto_init_seq,
-			sizeof(presto_init_seq),
-			&ftbytes);
-	if (presto->status != FT_OK || ftbytes != sizeof(presto_init_seq))
-		result = ERROR_JTAG_DEVICE_ERROR;
-
-	presto->status = FT_SetLatencyTimer(presto->handle, 16);
-	if (presto->status != FT_OK)
-		result = ERROR_JTAG_DEVICE_ERROR;
-
-	presto->status = FT_Close(presto->handle);
-	if (presto->status != FT_OK)
-		result = ERROR_JTAG_DEVICE_ERROR;
-	else
-		presto->handle = (FT_HANDLE)INVALID_HANDLE_VALUE;
-
-#elif BUILD_PRESTO_LIBFTDI == 1
-
 	presto->retval = ftdi_write_data(&presto->ftdic, presto_init_seq, sizeof(presto_init_seq));
 	if (presto->retval != sizeof(presto_init_seq))
 		result = ERROR_JTAG_DEVICE_ERROR;
@@ -445,7 +235,6 @@ static int presto_close(void)
 		result = ERROR_JTAG_DEVICE_ERROR;
 	else
 		ftdi_deinit(&presto->ftdic);
-#endif
 
 	return result;
 }
@@ -455,11 +244,7 @@ static int presto_flush(void)
 	if (presto->buff_out_pos == 0)
 		return ERROR_OK;
 
-#if BUILD_PRESTO_FTD2XX == 1
-	if (presto->status != FT_OK) {
-#elif BUILD_PRESTO_LIBFTDI == 1
 	if (presto->retval < 0) {
-#endif
 		LOG_DEBUG("error in previous communication, canceling I/O operation");
 		return ERROR_JTAG_DEVICE_ERROR;
 	}
@@ -502,13 +287,9 @@ static int presto_sendbyte(int data)
 	} else
 		return ERROR_JTAG_DEVICE_ERROR;
 
-#if BUILD_PRESTO_FTD2XX == 1
-	if (presto->buff_out_pos >= BUFFER_SIZE)
-#elif BUILD_PRESTO_LIBFTDI == 1
 	/* libftdi does not do background read, be sure that USB IN buffer does not overflow (128
 	 *bytes only!) */
 	if (presto->buff_out_pos >= BUFFER_SIZE || presto->buff_in_exp == 128)
-#endif
 		return presto_flush();
 
 	return ERROR_OK;
diff --git a/src/jtag/drivers/stlink_usb.c b/src/jtag/drivers/stlink_usb.c
index 699c41f..0bdcd31 100644
--- a/src/jtag/drivers/stlink_usb.c
+++ b/src/jtag/drivers/stlink_usb.c
@@ -216,7 +216,7 @@ struct stlink_usb_handle_s {
 #define STLINK_DEBUG_APIV2_DRIVE_NRST_HIGH  0x01
 #define STLINK_DEBUG_APIV2_DRIVE_NRST_PULSE 0x02
 
-#define STLINK_TRACE_SIZE               1024
+#define STLINK_TRACE_SIZE               4096
 #define STLINK_TRACE_MAX_HZ             2000000
 #define STLINK_TRACE_MIN_VERSION        13
 
diff --git a/src/jtag/drivers/ti_icdi_usb.c b/src/jtag/drivers/ti_icdi_usb.c
index 3db03b0..171ac66 100644
--- a/src/jtag/drivers/ti_icdi_usb.c
+++ b/src/jtag/drivers/ti_icdi_usb.c
@@ -242,7 +242,8 @@ static int icdi_send_remote_cmd(void *handle, const char *data)
 	struct icdi_usb_handle_s *h = handle;
 
 	size_t cmd_len = sprintf(h->write_buffer, PACKET_START "qRcmd,");
-	cmd_len += hexify(h->write_buffer + cmd_len, data, 0, h->max_packet - cmd_len);
+	cmd_len += hexify(h->write_buffer + cmd_len, (const uint8_t *)data,
+		strlen(data), h->max_packet - cmd_len);
 
 	return icdi_send_packet(handle, cmd_len);
 }
@@ -266,7 +267,7 @@ static int icdi_get_cmd_result(void *handle)
 
 	if (h->read_buffer[offset] == 'E') {
 		/* get error code */
-		char result;
+		uint8_t result;
 		if (unhexify(&result, h->read_buffer + offset + 1, 1) != 1)
 			return ERROR_FAIL;
 		return result;
@@ -328,7 +329,7 @@ static int icdi_usb_version(void *handle)
 	}
 
 	/* convert reply */
-	if (unhexify(version, h->read_buffer + 2, 4) != 4) {
+	if (unhexify((uint8_t *)version, h->read_buffer + 2, 4) != 4) {
 		LOG_WARNING("unable to get ICDI version");
 		return ERROR_OK;
 	}
@@ -495,7 +496,7 @@ static int icdi_usb_read_reg(void *handle, int num, uint32_t *val)
 
 	/* convert result */
 	uint8_t buf[4];
-	if (unhexify((char *)buf, h->read_buffer + 2, 4) != 4) {
+	if (unhexify(buf, h->read_buffer + 2, 4) != 4) {
 		LOG_ERROR("failed to convert result");
 		return ERROR_FAIL;
 	}
@@ -512,7 +513,7 @@ static int icdi_usb_write_reg(void *handle, int num, uint32_t val)
 	h_u32_to_le(buf, val);
 
 	int cmd_len = snprintf(cmd, sizeof(cmd), "P%x=", num);
-	hexify(cmd + cmd_len, (const char *)buf, 4, sizeof(cmd));
+	hexify(cmd + cmd_len, buf, 4, sizeof(cmd));
 
 	result = icdi_send_cmd(handle, cmd);
 	if (result != ERROR_OK)
diff --git a/src/jtag/drivers/ulink.c b/src/jtag/drivers/ulink.c
index c319a11..550df1c 100644
--- a/src/jtag/drivers/ulink.c
+++ b/src/jtag/drivers/ulink.c
@@ -2066,7 +2066,7 @@ static int ulink_khz(int khz, int *jtag_speed)
 	}
 
 #ifdef _DEBUG_JTAG_IO_
-	long f_tck, f_tms, f_scan_in, f_scan_out, f_scan_io;
+	long f_tck = 0, f_tms = 0, f_scan_in = 0, f_scan_out = 0, f_scan_io = 0;
 
 	ulink_calculate_frequency(DELAY_CLOCK_TCK, ulink_handle->delay_clock_tck,
 		&f_tck);
diff --git a/src/jtag/drivers/usb_blaster/Makefile.am b/src/jtag/drivers/usb_blaster/Makefile.am
index 0fb4700..a6694c5 100644
--- a/src/jtag/drivers/usb_blaster/Makefile.am
+++ b/src/jtag/drivers/usb_blaster/Makefile.am
@@ -1,24 +1,13 @@
-include $(top_srcdir)/common.mk
+noinst_LTLIBRARIES += %D%/libocdusbblaster.la
+%C%_libocdusbblaster_la_SOURCES = $(USB_BLASTER_SRC)
+%C%_libocdusbblaster_la_CPPFLAGS = -I$(top_srcdir)/src/jtag/drivers $(AM_CPPFLAGS) $(LIBUSB1_CFLAGS) $(LIBFTDI_CFLAGS)
 
-AM_CPPFLAGS += -I$(top_srcdir)/src/jtag/drivers $(LIBUSB1_CFLAGS) $(LIBFTDI_CFLAGS)
+USB_BLASTER_SRC = %D%/usb_blaster.c %D%/ublast_access.h
 
-noinst_LTLIBRARIES = libocdusbblaster.la
-libocdusbblaster_la_SOURCES = $(USB_BLASTER_SRC)
-
-USB_BLASTER_SRC = usb_blaster.c
-
-if USB_BLASTER_LIBFTDI
-USB_BLASTER_SRC += ublast_access_ftdi.c
-endif
-
-if USB_BLASTER_FTD2XX
-USB_BLASTER_SRC += ublast_access_ftd2xx.c
+if USB_BLASTER
+USB_BLASTER_SRC += %D%/ublast_access_ftdi.c
 endif
 
 if USB_BLASTER_2
-USB_BLASTER_SRC += ublast2_access_libusb.c
+USB_BLASTER_SRC += %D%/ublast2_access_libusb.c
 endif
-
-noinst_HEADERS = ublast_access.h
-
-MAINTAINERCLEANFILES = $(srcdir)/Makefile.in
diff --git a/src/jtag/drivers/usb_blaster/ublast_access.h b/src/jtag/drivers/usb_blaster/ublast_access.h
index 00349d2..252f003 100644
--- a/src/jtag/drivers/usb_blaster/ublast_access.h
+++ b/src/jtag/drivers/usb_blaster/ublast_access.h
@@ -56,19 +56,16 @@ struct ublast_lowlevel {
 
 /**
  * ublast_register_ftdi - get a lowlevel USB Blaster driver
- * ublast_register_ftd2xx - get a lowlevel USB Blaster driver
  * ublast2_register_libusb - get a lowlevel USB Blaster II driver
  *
- * Get a lowlevel USB-Blaster driver. In the current implementation, there are 3
+ * Get a lowlevel USB-Blaster driver. In the current implementation, there are 2
  * possible lowlevel drivers :
- *  - one based on libftdi from ftdichip.com
- *  - one based on libftdxx, the free alternative
+ *  - one based on libftdi,
  *  - one based on libusb, specific to the USB-Blaster II
  *
  * Returns the lowlevel driver structure.
  */
 extern struct ublast_lowlevel *ublast_register_ftdi(void);
-extern struct ublast_lowlevel *ublast_register_ftd2xx(void);
 extern struct ublast_lowlevel *ublast2_register_libusb(void);
 
 #endif /* OPENOCD_JTAG_DRIVERS_USB_BLASTER_UBLAST_ACCESS_H */
diff --git a/src/jtag/drivers/usb_blaster/ublast_access_ftd2xx.c b/src/jtag/drivers/usb_blaster/ublast_access_ftd2xx.c
deleted file mode 100644
index ffcf312..0000000
--- a/src/jtag/drivers/usb_blaster/ublast_access_ftd2xx.c
+++ /dev/null
@@ -1,180 +0,0 @@
-/*
- *   Driver for USB-JTAG, Altera USB-Blaster and compatibles
- *
- *   Inspired from original code from Kolja Waschk's USB-JTAG project
- *   (http://www.ixo.de/info/usb_jtag/), and from openocd project.
- *
- *   Copyright (C) 2012 Robert Jarzmik robert.jarzmik@free.fr
- *   Copyright (C) 2011 Ali Lown ali@lown.me.uk
- *   Copyright (C) 2009 Catalin Patulea cat@vv.carleton.ca
- *   Copyright (C) 2006 Kolja Waschk usbjtag@ixo.de
- *
- *   This program is free software; you can redistribute it and/or modify
- *   it under the terms of the GNU General Public License as published by
- *   the Free Software Foundation; either version 2 of the License, or
- *   (at your option) any later version.
- *
- *   This program is distributed in the hope that it will be useful,
- *   but WITHOUT ANY WARRANTY; without even the implied warranty of
- *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- *   GNU General Public License for more details.
- *
- *   You should have received a copy of the GNU General Public License
- *   along with this program.  If not, see <http://www.gnu.org/licenses/>.
- *
- */
-
-#ifdef HAVE_CONFIG_H
-#include <config.h>
-#endif
-#include <jtag/interface.h>
-#include <jtag/commands.h>
-
-#include "ublast_access.h"
-
-#include <ftd2xx.h>
-#include "jtag/drivers/ftd2xx_common.h"
-
-static FT_HANDLE *ublast_getftdih(struct ublast_lowlevel *low)
-{
-	return low->priv;
-}
-
-static int ublast_ftd2xx_write(struct ublast_lowlevel *low, uint8_t *buf, int size,
-			      uint32_t *bytes_written)
-{
-	FT_STATUS status;
-	DWORD dw_bytes_written;
-	FT_HANDLE *ftdih = ublast_getftdih(low);
-
-	status = FT_Write(*ftdih, buf, size, &dw_bytes_written);
-	if (status != FT_OK) {
-		*bytes_written = dw_bytes_written;
-		LOG_ERROR("FT_Write returned: %s", ftd2xx_status_string(status));
-		return ERROR_JTAG_DEVICE_ERROR;
-	}
-	*bytes_written = dw_bytes_written;
-	return ERROR_OK;
-}
-
-static int ublast_ftd2xx_read(struct ublast_lowlevel *low, uint8_t *buf,
-			     unsigned size, uint32_t *bytes_read)
-{
-	DWORD dw_bytes_read;
-	FT_STATUS status;
-	FT_HANDLE *ftdih = ublast_getftdih(low);
-
-	status = FT_Read(*ftdih, buf, size, &dw_bytes_read);
-	if (status != FT_OK) {
-		*bytes_read = dw_bytes_read;
-		LOG_ERROR("FT_Read returned: %s", ftd2xx_status_string(status));
-		return ERROR_JTAG_DEVICE_ERROR;
-	}
-	*bytes_read = dw_bytes_read;
-	return ERROR_OK;
-}
-
-static int ublast_ftd2xx_init(struct ublast_lowlevel *low)
-{
-	FT_STATUS status;
-	FT_HANDLE *ftdih = ublast_getftdih(low);
-	uint8_t latency_timer;
-
-	LOG_INFO("usb blaster interface using FTD2XX");
-	/* Open by device description */
-	if (low->ublast_device_desc == NULL) {
-		LOG_WARNING("no usb blaster device description specified, "
-			    "using default 'USB-Blaster'");
-		low->ublast_device_desc = "USB-Blaster";
-	}
-
-#if IS_WIN32 == 0
-	/* Add non-standard Vid/Pid to the linux driver */
-	status = FT_SetVIDPID(low->ublast_vid, low->ublast_pid);
-	if (status != FT_OK) {
-		LOG_WARNING("couldn't add %4.4x:%4.4x",
-			    low->ublast_vid, low->ublast_pid);
-	}
-#endif
-	status = FT_OpenEx(low->ublast_device_desc, FT_OPEN_BY_DESCRIPTION,
-			   ftdih);
-	if (status != FT_OK) {
-		DWORD num_devices;
-
-		LOG_ERROR("unable to open ftdi device: %s",
-			  ftd2xx_status_string(status));
-		status = FT_ListDevices(&num_devices, NULL, FT_LIST_NUMBER_ONLY);
-		if (status == FT_OK) {
-			char **desc_array =
-				malloc(sizeof(char *) * (num_devices + 1));
-			unsigned int i;
-
-			for (i = 0; i < num_devices; i++)
-				desc_array[i] = malloc(64);
-			desc_array[num_devices] = NULL;
-
-			status = FT_ListDevices(desc_array, &num_devices,
-						FT_LIST_ALL | FT_OPEN_BY_DESCRIPTION);
-
-			if (status == FT_OK) {
-				LOG_ERROR("ListDevices: %" PRIu32, (uint32_t)num_devices);
-				for (i = 0; i < num_devices; i++)
-					LOG_ERROR("%i: %s", i, desc_array[i]);
-			}
-
-			for (i = 0; i < num_devices; i++)
-				free(desc_array[i]);
-			free(desc_array);
-		} else {
-			printf("ListDevices: NONE\n");
-		}
-		return ERROR_JTAG_INIT_FAILED;
-	}
-
-	status = FT_SetLatencyTimer(*ftdih, 2);
-	if (status != FT_OK) {
-		LOG_ERROR("unable to set latency timer: %s",
-				ftd2xx_status_string(status));
-		return ERROR_JTAG_INIT_FAILED;
-	}
-
-	status = FT_GetLatencyTimer(*ftdih, &latency_timer);
-	if (status != FT_OK)
-		LOG_ERROR("unable to get latency timer: %s",
-				ftd2xx_status_string(status));
-	else
-		LOG_DEBUG("current latency timer: %i", latency_timer);
-
-	status = FT_SetBitMode(*ftdih, 0x00, 0);
-	if (status != FT_OK) {
-		LOG_ERROR("unable to disable bit i/o mode: %s",
-				ftd2xx_status_string(status));
-		return ERROR_JTAG_INIT_FAILED;
-	}
-	return ERROR_OK;
-}
-
-static int ublast_ftd2xx_quit(struct ublast_lowlevel *low)
-{
-	FT_HANDLE *ftdih = ublast_getftdih(low);
-
-	FT_Close(*ftdih);
-	return ERROR_OK;
-}
-
-static struct ublast_lowlevel_priv {
-	FT_HANDLE ftdih;
-} info;
-
-static struct ublast_lowlevel low = {
-	.open = ublast_ftd2xx_init,
-	.close = ublast_ftd2xx_quit,
-	.read = ublast_ftd2xx_read,
-	.write = ublast_ftd2xx_write,
-	.priv = &info,
-};
-
-struct ublast_lowlevel *ublast_register_ftd2xx(void)
-{
-	return &low;
-}
diff --git a/src/jtag/drivers/usb_blaster/usb_blaster.c b/src/jtag/drivers/usb_blaster/usb_blaster.c
index 8f3f327..a975bd1 100644
--- a/src/jtag/drivers/usb_blaster/usb_blaster.c
+++ b/src/jtag/drivers/usb_blaster/usb_blaster.c
@@ -147,12 +147,9 @@ struct drvs_map {
 };
 
 static struct drvs_map lowlevel_drivers_map[] = {
-#if BUILD_USB_BLASTER_LIBFTDI
+#if BUILD_USB_BLASTER
 	{ .name = "ftdi", .drv_register = ublast_register_ftdi },
 #endif
-#if BUILD_USB_BLASTER_FTD2XX
-	{ .name = "ftd2xx", .drv_register = ublast_register_ftd2xx },
-#endif
 #if BUILD_USB_BLASTER_2
 	{ .name = "ublast2", .drv_register = ublast2_register_libusb },
 #endif
@@ -1048,8 +1045,8 @@ static const struct command_registration ublast_command_handlers[] = {
 		.name = "usb_blaster_lowlevel_driver",
 		.handler = ublast_handle_lowlevel_drv_command,
 		.mode = COMMAND_CONFIG,
-		.help = "set the lowlevel access for the USB Blaster (ftdi, ftd2xx, ublast2)",
-		.usage = "(ftdi|ftd2xx|ublast2)",
+		.help = "set the lowlevel access for the USB Blaster (ftdi, ublast2)",
+		.usage = "(ftdi|ublast2)",
 	},
 	{
 		.name = "usb_blaster_pin",
diff --git a/src/jtag/hla/Makefile.am b/src/jtag/hla/Makefile.am
index 4fbc70e..6bb2960 100644
--- a/src/jtag/hla/Makefile.am
+++ b/src/jtag/hla/Makefile.am
@@ -1,23 +1,11 @@
-include $(top_srcdir)/common.mk
-
-noinst_LTLIBRARIES = libocdhla.la
-
-libocdhla_la_SOURCES = \
-	$(HLFILES)
-
-HLFILES =
-
-if HLADAPTER
-HLFILES += hla_transport.c
-HLFILES += hla_tcl.c
-HLFILES += hla_interface.c
-HLFILES += hla_layout.c
-endif
-
-noinst_HEADERS = \
-	hla_interface.h \
-	hla_layout.h \
-	hla_tcl.h \
-	hla_transport.h
-
-MAINTAINERCLEANFILES = $(srcdir)/Makefile.in
+noinst_LTLIBRARIES += %D%/libocdhla.la
+
+%C%_libocdhla_la_SOURCES = \
+	%D%/hla_transport.c \
+	%D%/hla_tcl.c \
+	%D%/hla_interface.c \
+	%D%/hla_layout.c \
+	%D%/hla_transport.h \
+	%D%/hla_interface.h \
+	%D%/hla_layout.h \
+	%D%/hla_tcl.h
diff --git a/src/jtag/interfaces.c b/src/jtag/interfaces.c
index 64fdd71..174c63a 100644
--- a/src/jtag/interfaces.c
+++ b/src/jtag/interfaces.c
@@ -51,16 +51,10 @@ extern struct jtag_interface parport_interface;
 #if BUILD_DUMMY == 1
 extern struct jtag_interface dummy_interface;
 #endif
-#if BUILD_FT2232_FTD2XX == 1
-extern struct jtag_interface ft2232_interface;
-#endif
-#if BUILD_FT2232_LIBFTDI == 1
-extern struct jtag_interface ft2232_interface;
-#endif
 #if BUILD_FTDI == 1
 extern struct jtag_interface ftdi_interface;
 #endif
-#if BUILD_USB_BLASTER_LIBFTDI == 1 || BUILD_USB_BLASTER_FTD2XX == 1 || BUILD_USB_BLASTER_2 == 1
+#if BUILD_USB_BLASTER == 1 || BUILD_USB_BLASTER_2 == 1
 extern struct jtag_interface usb_blaster_interface;
 #endif
 #if BUILD_JTAG_VPI == 1
@@ -78,7 +72,7 @@ extern struct jtag_interface at91rm9200_interface;
 #if BUILD_GW16012 == 1
 extern struct jtag_interface gw16012_interface;
 #endif
-#if BUILD_PRESTO_LIBFTDI == 1 || BUILD_PRESTO_FTD2XX == 1
+#if BUILD_PRESTO
 extern struct jtag_interface presto_interface;
 #endif
 #if BUILD_USBPROG == 1
@@ -129,6 +123,12 @@ extern struct jtag_interface bcm2835gpio_interface;
 #if BUILD_CMSIS_DAP == 1
 extern struct jtag_interface cmsis_dap_interface;
 #endif
+#if BUILD_KITPROG == 1
+extern struct jtag_interface kitprog_interface;
+#endif
+#if BUILD_IMX_GPIO == 1
+extern struct jtag_interface imx_gpio_interface;
+#endif
 #endif /* standard drivers */
 
 /**
@@ -150,16 +150,10 @@ struct jtag_interface *jtag_interfaces[] = {
 #if BUILD_DUMMY == 1
 		&dummy_interface,
 #endif
-#if BUILD_FT2232_FTD2XX == 1
-		&ft2232_interface,
-#endif
-#if BUILD_FT2232_LIBFTDI == 1
-		&ft2232_interface,
-#endif
 #if BUILD_FTDI == 1
 		&ftdi_interface,
 #endif
-#if BUILD_USB_BLASTER_LIBFTDI == 1 || BUILD_USB_BLASTER_FTD2XX == 1 || BUILD_USB_BLASTER_2 == 1
+#if BUILD_USB_BLASTER || BUILD_USB_BLASTER_2 == 1
 		&usb_blaster_interface,
 #endif
 #if BUILD_JTAG_VPI == 1
@@ -177,7 +171,7 @@ struct jtag_interface *jtag_interfaces[] = {
 #if BUILD_GW16012 == 1
 		&gw16012_interface,
 #endif
-#if BUILD_PRESTO_LIBFTDI == 1 || BUILD_PRESTO_FTD2XX == 1
+#if BUILD_PRESTO
 		&presto_interface,
 #endif
 #if BUILD_USBPROG == 1
@@ -228,6 +222,12 @@ struct jtag_interface *jtag_interfaces[] = {
 #if BUILD_CMSIS_DAP == 1
 		&cmsis_dap_interface,
 #endif
+#if BUILD_KITPROG == 1
+		&kitprog_interface,
+#endif
+#if BUILD_IMX_GPIO == 1
+		&imx_gpio_interface,
+#endif
 #endif /* standard drivers */
 		NULL,
 	};
diff --git a/src/openocd.c b/src/openocd.c
index b5bb44b..83329b5 100644
--- a/src/openocd.c
+++ b/src/openocd.c
@@ -45,8 +45,13 @@
 #include <strings.h>
 #endif
 
+#ifdef PKGBLDDATE
 #define OPENOCD_VERSION	\
 	"Open On-Chip Debugger " VERSION RELSTR " (" PKGBLDDATE ")"
+#else
+#define OPENOCD_VERSION	\
+	"Open On-Chip Debugger " VERSION RELSTR
+#endif
 
 static const char openocd_startup_tcl[] = {
 #include "startup_tcl.inc"
@@ -292,8 +297,10 @@ static int openocd_thread(int argc, char *argv[], struct command_context *cmd_ct
 
 	if (init_at_startup) {
 		ret = command_run_line(cmd_ctx, "init");
-		if (ERROR_OK != ret)
+		if (ERROR_OK != ret) {
+			server_quit();
 			return ERROR_FAIL;
+		}
 	}
 
 	ret = server_loop(cmd_ctx);
diff --git a/src/pld/Makefile.am b/src/pld/Makefile.am
index 93b79f4..7f3a554 100644
--- a/src/pld/Makefile.am
+++ b/src/pld/Makefile.am
@@ -1,8 +1,8 @@
-include $(top_srcdir)/common.mk
-
-METASOURCES = AUTO
-noinst_LTLIBRARIES = libpld.la
-noinst_HEADERS = pld.h xilinx_bit.h virtex2.h
-libpld_la_SOURCES = pld.c xilinx_bit.c virtex2.c
-
-MAINTAINERCLEANFILES = $(srcdir)/Makefile.in
+noinst_LTLIBRARIES += %D%/libpld.la
+%C%_libpld_la_SOURCES = \
+	%D%/pld.c \
+	%D%/xilinx_bit.c \
+	%D%/virtex2.c \
+	%D%/pld.h \
+	%D%/xilinx_bit.h \
+	%D%/virtex2.h
diff --git a/src/rtos/ChibiOS.c b/src/rtos/ChibiOS.c
index 00e9df7..1bc1af8 100644
--- a/src/rtos/ChibiOS.c
+++ b/src/rtos/ChibiOS.c
@@ -440,11 +440,11 @@ static int ChibiOS_update_threads(struct rtos *rtos)
 		if (threadState < CHIBIOS_NUM_STATES)
 			state_desc = ChibiOS_thread_states[threadState];
 		else
-			state_desc = "Unknown state";
+			state_desc = "Unknown";
 
 		curr_thrd_details->extra_info_str = malloc(strlen(
-					state_desc)+1);
-		strcpy(curr_thrd_details->extra_info_str, state_desc);
+					state_desc)+8);
+		sprintf(curr_thrd_details->extra_info_str, "State: %s", state_desc);
 
 		curr_thrd_details->exists = true;
 
diff --git a/src/rtos/FreeRTOS.c b/src/rtos/FreeRTOS.c
index 52cebad..83961eb 100644
--- a/src/rtos/FreeRTOS.c
+++ b/src/rtos/FreeRTOS.c
@@ -362,7 +362,7 @@ static int FreeRTOS_update_threads(struct rtos *rtos)
 			rtos->thread_details[tasks_found].exists = true;
 
 			if (rtos->thread_details[tasks_found].threadid == rtos->current_thread) {
-				char running_str[] = "Running";
+				char running_str[] = "State: Running";
 				rtos->thread_details[tasks_found].extra_info_str = malloc(
 						sizeof(running_str));
 				strcpy(rtos->thread_details[tasks_found].extra_info_str,
diff --git a/src/rtos/Makefile.am b/src/rtos/Makefile.am
index a7dab00..22f7da5 100644
--- a/src/rtos/Makefile.am
+++ b/src/rtos/Makefile.am
@@ -1,32 +1,34 @@
-# ***************************************************************************
-# *   Copyright (C) 2011 by Broadcom Corporation                            *
-# *   Evan Hunter - ehunter@broadcom.com                                    *
-# *                                                                         *
-# *   This program is free software; you can redistribute it and/or modify  *
-# *   it under the terms of the GNU General Public License as published by  *
-# *   the Free Software Foundation; either version 2 of the License, or     *
-# *   (at your option) any later version.                                   *
-# *                                                                         *
-# *   This program is distributed in the hope that it will be useful,       *
-# *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
-# *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
-# *   GNU General Public License for more details.                          *
-# *                                                                         *
-# *   You should have received a copy of the GNU General Public License     *
-# *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
-# ***************************************************************************
+noinst_LTLIBRARIES += %D%/librtos.la
+%C%_librtos_la_SOURCES = \
+	%D%/rtos.c \
+	%D%/rtos_standard_stackings.c \
+	%D%/rtos_ecos_stackings.c  \
+	%D%/rtos_chibios_stackings.c \
+	%D%/rtos_embkernel_stackings.c \
+	%D%/rtos_mqx_stackings.c \
+	%D%/rtos_ucos_iii_stackings.c \
+	%D%/FreeRTOS.c \
+	%D%/ThreadX.c \
+	%D%/eCos.c \
+	%D%/linux.c \
+	%D%/ChibiOS.c \
+	%D%/embKernel.c \
+	%D%/mqx.c \
+	%D%/riscv_debug.c \
+	%D%/uCOS-III.c \
+	%D%/rtos.h \
+	%D%/rtos_standard_stackings.h \
+	%D%/rtos_ecos_stackings.h \
+	%D%/linux_header.h \
+	%D%/rtos_chibios_stackings.h \
+	%D%/rtos_embkernel_stackings.h \
+	%D%/rtos_mqx_stackings.h \
+	%D%/rtos_ucos_iii_stackings.h \
+	%D%/riscv_debug.h
 
-include $(top_srcdir)/common.mk
+%C%_librtos_la_CFLAGS = $(AM_CFLAGS)
 
-METASOURCES = AUTO
-noinst_LTLIBRARIES = librtos.la
-noinst_HEADERS = rtos.h rtos_standard_stackings.h rtos_ecos_stackings.h linux_header.h rtos_chibios_stackings.h rtos_embkernel_stackings.h rtos_mqx_stackings.h riscv_debug.h
-librtos_la_SOURCES = rtos.c rtos_standard_stackings.c rtos_ecos_stackings.c  rtos_chibios_stackings.c rtos_embkernel_stackings.c rtos_mqx_stackings.c FreeRTOS.c ThreadX.c eCos.c linux.c ChibiOS.c embKernel.c mqx.c riscv_debug.c
-
-librtos_la_CFLAGS =
 if IS_MINGW
 # FD_* macros are sloppy with their signs on MinGW32 platform
-librtos_la_CFLAGS += -Wno-sign-compare
+%C%_librtos_la_CFLAGS += -Wno-sign-compare
 endif
-
-MAINTAINERCLEANFILES = $(srcdir)/Makefile.in
diff --git a/src/rtos/ThreadX.c b/src/rtos/ThreadX.c
index 8267b9f..ab8a66e 100644
--- a/src/rtos/ThreadX.c
+++ b/src/rtos/ThreadX.c
@@ -408,8 +408,8 @@ static int ThreadX_update_threads(struct rtos *rtos)
 			state_desc = "Unknown state";
 
 		rtos->thread_details[tasks_found].extra_info_str = malloc(strlen(
-					state_desc)+1);
-		strcpy(rtos->thread_details[tasks_found].extra_info_str, state_desc);
+					state_desc)+8);
+		sprintf(rtos->thread_details[tasks_found].extra_info_str, "State: %s", state_desc);
 
 		rtos->thread_details[tasks_found].exists = true;
 
diff --git a/src/rtos/eCos.c b/src/rtos/eCos.c
index e38e11f..edc3d8b 100644
--- a/src/rtos/eCos.c
+++ b/src/rtos/eCos.c
@@ -261,8 +261,8 @@ static int eCos_update_threads(struct rtos *rtos)
 			state_desc = "Unknown state";
 
 		rtos->thread_details[tasks_found].extra_info_str = malloc(strlen(
-					state_desc)+1);
-		strcpy(rtos->thread_details[tasks_found].extra_info_str, state_desc);
+					state_desc)+8);
+		sprintf(rtos->thread_details[tasks_found].extra_info_str, "State: %s", state_desc);
 
 		rtos->thread_details[tasks_found].exists = true;
 
diff --git a/src/rtos/embKernel.c b/src/rtos/embKernel.c
index ceb313f..e515383 100644
--- a/src/rtos/embKernel.c
+++ b/src/rtos/embKernel.c
@@ -168,11 +168,11 @@ static int embKernel_get_tasks_details(struct rtos *rtos, int64_t iterable, cons
 		return retval;
 	details->extra_info_str = malloc(EMBKERNEL_MAX_THREAD_NAME_STR_SIZE);
 	if (task == rtos->current_thread) {
-		snprintf(details->extra_info_str, EMBKERNEL_MAX_THREAD_NAME_STR_SIZE, "Pri=%u, Running",
+		snprintf(details->extra_info_str, EMBKERNEL_MAX_THREAD_NAME_STR_SIZE, "State: Running, Priority: %u",
 				(unsigned int) priority);
 	} else {
-		snprintf(details->extra_info_str, EMBKERNEL_MAX_THREAD_NAME_STR_SIZE, "Pri=%u, %s", (unsigned int) priority,
-				state_str);
+		snprintf(details->extra_info_str, EMBKERNEL_MAX_THREAD_NAME_STR_SIZE, "State: %s, Priority: %u",
+				state_str, (unsigned int) priority);
 	}
 
 	LOG_OUTPUT("Getting task details: iterable=0x%08X, task=0x%08X, name=%s\n", (unsigned int)iterable,
diff --git a/src/rtos/linux.c b/src/rtos/linux.c
index 8c150af..3efaab1 100644
--- a/src/rtos/linux.c
+++ b/src/rtos/linux.c
@@ -105,11 +105,11 @@ static int linux_os_dummy_update(struct rtos *rtos)
 	return 0;
 }
 
-static int linux_compute_virt2phys(struct target *target, uint32_t address)
+static int linux_compute_virt2phys(struct target *target, target_addr_t address)
 {
 	struct linux_os *linux_os = (struct linux_os *)
 		target->rtos->rtos_specific_params;
-	uint32_t pa = 0;
+	target_addr_t pa = 0;
 	int retval = target->type->virt2phys(target, address, &pa);
 	if (retval != ERROR_OK) {
 		LOG_ERROR("Cannot compute linux virt2phys translation");
@@ -1213,7 +1213,7 @@ int linux_thread_extra_info(struct target *target,
 		if (temp->threadid == threadid) {
 			char *pid = " PID: ";
 			char *pid_current = "*PID: ";
-			char *name = "NAME: ";
+			char *name = "Name: ";
 			int str_size = strlen(pid) + strlen(name);
 			char *tmp_str = calloc(1, str_size + 50);
 			char *tmp_str_ptr = tmp_str;
@@ -1225,13 +1225,12 @@ int linux_thread_extra_info(struct target *target,
 			else
 				tmp_str_ptr += sprintf(tmp_str_ptr, "%s", pid);
 
-			tmp_str_ptr +=
-				sprintf(tmp_str_ptr, "%d", (int)temp->pid);
-			tmp_str_ptr += sprintf(tmp_str_ptr, "%s", " | ");
+			tmp_str_ptr += sprintf(tmp_str_ptr, "%d, ", (int)temp->pid);
 			sprintf(tmp_str_ptr, "%s", name);
 			sprintf(tmp_str_ptr, "%s", temp->name);
 			char *hex_str = calloc(1, strlen(tmp_str) * 2 + 1);
-			int pkt_len = hexify(hex_str, tmp_str, 0, strlen(tmp_str) * 2 + 1);
+			size_t pkt_len = hexify(hex_str, (const uint8_t *)tmp_str,
+				strlen(tmp_str), strlen(tmp_str) * 2 + 1);
 			gdb_put_packet(connection, hex_str, pkt_len);
 			free(hex_str);
 			free(tmp_str);
diff --git a/src/rtos/mqx.c b/src/rtos/mqx.c
index b8095a0..63a48c5 100644
--- a/src/rtos/mqx.c
+++ b/src/rtos/mqx.c
@@ -353,7 +353,7 @@ static int mqx_update_threads(
 		uint32_t task_name_addr = 0, task_id = 0, task_errno = 0;
 		uint32_t state_index = 0, state_max = 0;
 		uint32_t extra_info_length = 0;
-		char *state_name = "unknown state";
+		char *state_name = "Unknown";
 
 		/* set current taskpool address */
 		if (ERROR_OK != mqx_get_member(
@@ -435,13 +435,13 @@ static int mqx_update_threads(
 		 * calculate length as:
 		 * state length + address length + errno length + formatter length
 		 */
-		extra_info_length += strlen((void *)state_name) + 8 + 8 + 8;
+		extra_info_length += strlen((void *)state_name) + 7 + 13 + 8 + 15 + 8;
 		rtos->thread_details[i].extra_info_str = malloc(extra_info_length + 1);
 		if (NULL == rtos->thread_details[i].extra_info_str)
 			return ERROR_FAIL;
-		snprintf(
-			rtos->thread_details[i].extra_info_str, extra_info_length, "%s : 0x%"PRIx32 " : %" PRIu32,
-			state_name, task_addr, task_errno
+		snprintf(rtos->thread_details[i].extra_info_str, extra_info_length,
+			 "State: %s, Address: 0x%" PRIx32 ",  Error Code: %" PRIu32,
+			 state_name, task_addr, task_errno
 		);
 		/* set active thread */
 		if (active_td_addr == task_addr)
diff --git a/src/rtos/rtos.c b/src/rtos/rtos.c
index 9bf218e..5e8ce48 100644
--- a/src/rtos/rtos.c
+++ b/src/rtos/rtos.c
@@ -34,6 +34,7 @@ extern struct rtos_type Linux_os;
 extern struct rtos_type ChibiOS_rtos;
 extern struct rtos_type embKernel_rtos;
 extern struct rtos_type mqx_rtos;
+extern struct rtos_type uCOS_III_rtos;
 extern struct rtos_type riscv_rtos;
 
 static struct rtos_type *rtos_types[] = {
@@ -44,6 +45,7 @@ static struct rtos_type *rtos_types[] = {
 	&ChibiOS_rtos,
 	&embKernel_rtos,
 	&mqx_rtos,
+	&uCOS_III_rtos,
 	&riscv_rtos,
 	NULL
 };
@@ -107,6 +109,7 @@ int rtos_create(Jim_GetOptInfo *goi, struct target *target)
 	int x;
 	const char *cp;
 	struct Jim_Obj *res;
+	int e;
 
 	if (!goi->isconfigure && goi->argc != 0) {
 		Jim_WrongNumArgs(goi->interp, goi->argc, goi->argv, "NO PARAMS");
@@ -115,7 +118,9 @@ int rtos_create(Jim_GetOptInfo *goi, struct target *target)
 
 	os_free(target);
 
-	Jim_GetOpt_String(goi, &cp, NULL);
+	e = Jim_GetOpt_String(goi, &cp, NULL);
+	if (e != JIM_OK)
+		return e;
 
 	if (0 == strcmp(cp, "auto")) {
 		/* Auto detect tries to look up all symbols for each RTOS,
@@ -216,7 +221,7 @@ int rtos_qsymbol(struct connection *connection, char const *packet, int packet_s
 		goto done;
 
 	/* Decode any symbol name in the packet*/
-	int len = unhexify(cur_sym, strchr(packet + 8, ':') + 1, strlen(strchr(packet + 8, ':') + 1));
+	size_t len = unhexify((uint8_t *)cur_sym, strchr(packet + 8, ':') + 1, strlen(strchr(packet + 8, ':') + 1));
 	cur_sym[len] = 0;
 
 	if ((strcmp(packet, "qSymbol::") != 0) &&               /* GDB is not offering symbol lookup for the first time */
@@ -264,7 +269,9 @@ int rtos_qsymbol(struct connection *connection, char const *packet, int packet_s
 	}
 
 	reply_len = snprintf(reply, sizeof(reply), "qSymbol:");
-	reply_len += hexify(reply + reply_len, next_sym->symbol_name, 0, sizeof(reply) - reply_len);
+	reply_len += hexify(reply + reply_len,
+		(const uint8_t *)next_sym->symbol_name, strlen(next_sym->symbol_name),
+		sizeof(reply) - reply_len);
 
 done:
 	gdb_put_packet(connection, reply, reply_len);
@@ -304,14 +311,14 @@ int rtos_thread_packet(struct connection *connection, char const *packet, int pa
 			if (detail->extra_info_str != NULL)
 				str_size += strlen(detail->extra_info_str);
 
-			char *tmp_str = calloc(str_size + 4, sizeof(char));
+			char *tmp_str = calloc(str_size + 9, sizeof(char));
 			char *tmp_str_ptr = tmp_str;
 
 			if (detail->thread_name_str != NULL)
-				tmp_str_ptr += sprintf(tmp_str_ptr, "%s", detail->thread_name_str);
+				tmp_str_ptr += sprintf(tmp_str_ptr, "Name: %s", detail->thread_name_str);
 			if (detail->extra_info_str != NULL) {
 				if (tmp_str_ptr != tmp_str)
-					tmp_str_ptr += sprintf(tmp_str_ptr, " : ");
+					tmp_str_ptr += sprintf(tmp_str_ptr, ", ");
 				tmp_str_ptr += sprintf(tmp_str_ptr, "%s", detail->extra_info_str);
 			}
 
@@ -319,7 +326,8 @@ int rtos_thread_packet(struct connection *connection, char const *packet, int pa
 				(size_t) (tmp_str_ptr - tmp_str));
 
 			char *hex_str = malloc(strlen(tmp_str) * 2 + 1);
-			int pkt_len = hexify(hex_str, tmp_str, 0, strlen(tmp_str) * 2 + 1);
+			size_t pkt_len = hexify(hex_str, (const uint8_t *)tmp_str,
+				strlen(tmp_str), strlen(tmp_str) * 2 + 1);
 
 			gdb_put_packet(connection, hex_str, pkt_len);
 			free(hex_str);
@@ -400,9 +408,14 @@ int rtos_thread_packet(struct connection *connection, char const *packet, int pa
 	} else if (packet[0] == 'H') {	/* Set current thread ( 'c' for step and continue, 'g' for
 					 * all other operations ) */
 		if ((packet[1] == 'g') && (target->rtos != NULL)) {
-			sscanf(packet, "Hg%16" SCNx64, &target->rtos->current_threadid);
-			LOG_DEBUG("RTOS: GDB requested to set current thread to 0x%" PRIx64 "\r\n",
-										target->rtos->current_threadid);
+			threadid_t threadid;
+			sscanf(packet, "Hg%16" SCNx64, &threadid);
+			LOG_DEBUG("RTOS: GDB requested to set current thread to 0x%" PRIx64, threadid);
+			/* threadid of 0 indicates target should choose */
+			if (threadid == 0)
+				target->rtos->current_threadid = target->rtos->current_thread;
+			else
+				target->rtos->current_threadid = threadid;
 		}
 		gdb_put_packet(connection, "OK", 2);
 		return ERROR_OK;
@@ -426,9 +439,13 @@ int rtos_get_gdb_reg_list(struct connection *connection)
 										current_threadid,
 										target->rtos->current_thread);
 
-		target->rtos->type->get_thread_reg_list(target->rtos,
-			current_threadid,
-			&hex_reg_list);
+		int retval = target->rtos->type->get_thread_reg_list(target->rtos,
+				current_threadid,
+				&hex_reg_list);
+		if (retval != ERROR_OK) {
+			LOG_ERROR("RTOS: failed to get register list");
+			return retval;
+		}
 
 		if (hex_reg_list != NULL) {
 			gdb_put_packet(connection, hex_reg_list, strlen(hex_reg_list));
@@ -546,5 +563,7 @@ void rtos_free_threadlist(struct rtos *rtos)
 		free(rtos->thread_details);
 		rtos->thread_details = NULL;
 		rtos->thread_count = 0;
+		rtos->current_threadid = -1;
+		rtos->current_thread = 0;
 	}
 }
diff --git a/src/rtos/rtos_ucos_iii_stackings.c b/src/rtos/rtos_ucos_iii_stackings.c
new file mode 100644
index 0000000..f2f5564
--- /dev/null
+++ b/src/rtos/rtos_ucos_iii_stackings.c
@@ -0,0 +1,53 @@
+/***************************************************************************
+ *   Copyright (C) 2016 by Square, Inc.                                    *
+ *   Steven Stallion <stallion@squareup.com>                               *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
+ ***************************************************************************/
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "rtos.h"
+#include "rtos_standard_stackings.h"
+#include "target/armv7m.h"
+
+static const struct stack_register_offset rtos_uCOS_III_Cortex_M_stack_offsets[ARMV7M_NUM_CORE_REGS] = {
+	{ 0x20, 32 },		/* r0   */
+	{ 0x24, 32 },		/* r1   */
+	{ 0x28, 32 },		/* r2   */
+	{ 0x2c, 32 },		/* r3   */
+	{ 0x00, 32 },		/* r4   */
+	{ 0x04, 32 },		/* r5   */
+	{ 0x08, 32 },		/* r6   */
+	{ 0x0c, 32 },		/* r7   */
+	{ 0x10, 32 },		/* r8   */
+	{ 0x14, 32 },		/* r9   */
+	{ 0x18, 32 },		/* r10  */
+	{ 0x1c, 32 },		/* r11  */
+	{ 0x30, 32 },		/* r12  */
+	{ -2,   32 },		/* sp   */
+	{ 0x34, 32 },		/* lr   */
+	{ 0x38, 32 },		/* pc   */
+	{ 0x3c, 32 },		/* xPSR */
+};
+
+const struct rtos_register_stacking rtos_uCOS_III_Cortex_M_stacking = {
+	0x40,					/* stack_registers_size */
+	-1,					/* stack_growth_direction */
+	ARMV7M_NUM_CORE_REGS,			/* num_output_registers */
+	rtos_generic_stack_align8,		/* stack_alignment */
+	rtos_uCOS_III_Cortex_M_stack_offsets	/* register_offsets */
+};
diff --git a/src/target/xscale/protocol.h b/src/rtos/rtos_ucos_iii_stackings.h
index cb01655..c462cd7 100644
--- a/src/target/xscale/protocol.h
+++ b/src/rtos/rtos_ucos_iii_stackings.h
@@ -1,6 +1,6 @@
 /***************************************************************************
- *   Copyright (C) 2006 by Dominic Rath                                    *
- *   Dominic.Rath@gmx.de                                                   *
+ *   Copyright (C) 2016 by Square, Inc.                                    *
+ *   Steven Stallion <stallion@squareup.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  *
@@ -16,53 +16,15 @@
  *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
  ***************************************************************************/
 
-#define REG_R0 0
-#define REG_R1 1
-#define REG_R2 2
-#define REG_R3 3
-#define REG_R4 4
-#define REG_R5 5
-#define REG_R6 6
-#define REG_R7 7
-#define REG_R8 8
-#define REG_R9 9
-#define REG_R10 10
-#define REG_R11 11
-#define REG_R12 12
-#define REG_R13 13
-#define REG_R14 14
-#define REG_R15 15
-#define REG_CPSR 16
-#define REG_SPSR 17
+#ifndef OPENOCD_RTOS_RTOS_UCOS_III_STACKINGS_H
+#define OPENOCD_RTOS_RTOS_UCOS_III_STACKINGS_H
 
-#define MODE_USR 0x10
-#define MODE_FIQ 0x11
-#define MODE_IRQ 0x12
-#define MODE_SVC 0x13
-#define MODE_ABT 0x17
-#define MODE_UND 0x1b
-#define MODE_SYS 0x1f
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
 
-#define MODE_ANY 0x40
-#define MODE_CURRENT 0x80
+#include "rtos.h"
 
-#define MODE_MASK 0x1f
-#define PSR_I 0x80
-#define PSR_F 0x40
-#define PSR_T 0x20
+extern const struct rtos_register_stacking rtos_uCOS_III_Cortex_M_stacking;
 
-#define XSCALE_DBG_MAINID 0x0
-#define XSCALE_DBG_CACHETYPE 0x1
-#define XSCALE_DBG_CTRL 0x2
-#define XSCALE_DBG_AUXCTRL 0x3
-#define XSCALE_DBG_TTB 0x4
-#define XSCALE_DBG_DAC 0x5
-#define XSCALE_DBG_FSR 0x6
-#define XSCALE_DBG_FAR 0x7
-#define XSCALE_DBG_PID 0x8
-#define XSCALE_DBG_CPACCESS 0x9
-#define XSCALE_DBG_IBCR0 0xa
-#define XSCALE_DBG_IBCR1 0xb
-#define XSCALE_DBG_DBR0 0xc
-#define XSCALE_DBG_DBR1 0xd
-#define XSCALE_DBG_DBCON 0xe
+#endif /* OPENOCD_RTOS_RTOS_UCOS_III_STACKINGS_H */
diff --git a/src/rtos/uCOS-III.c b/src/rtos/uCOS-III.c
new file mode 100644
index 0000000..75cfe52
--- /dev/null
+++ b/src/rtos/uCOS-III.c
@@ -0,0 +1,509 @@
+/***************************************************************************
+ *   Copyright (C) 2016 by Square, Inc.                                    *
+ *   Steven Stallion <stallion@squareup.com>                               *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
+ ***************************************************************************/
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include <helper/time_support.h>
+#include <jtag/jtag.h>
+#include "target/target.h"
+#include "target/target_type.h"
+#include "rtos.h"
+#include "helper/log.h"
+#include "helper/types.h"
+#include "rtos/rtos_ucos_iii_stackings.h"
+
+#ifndef UCOS_III_MAX_STRLEN
+#define UCOS_III_MAX_STRLEN 64
+#endif
+
+#ifndef UCOS_III_MAX_THREADS
+#define UCOS_III_MAX_THREADS 256
+#endif
+
+struct uCOS_III_params {
+	const char *target_name;
+	const unsigned char pointer_width;
+	symbol_address_t thread_stack_offset;
+	symbol_address_t thread_name_offset;
+	symbol_address_t thread_state_offset;
+	symbol_address_t thread_priority_offset;
+	symbol_address_t thread_prev_offset;
+	symbol_address_t thread_next_offset;
+	bool thread_offsets_updated;
+	size_t threadid_start;
+	const struct rtos_register_stacking *stacking_info;
+	size_t num_threads;
+	symbol_address_t threads[];
+};
+
+static const struct uCOS_III_params uCOS_III_params_list[] = {
+	{
+		"cortex_m",				/* target_name */
+		sizeof(uint32_t),			/* pointer_width */
+		0,					/* thread_stack_offset */
+		0,					/* thread_name_offset */
+		0,					/* thread_state_offset */
+		0,					/* thread_priority_offset */
+		0,					/* thread_prev_offset */
+		0,					/* thread_next_offset */
+		false,					/* thread_offsets_updated */
+		1,					/* threadid_start */
+		&rtos_uCOS_III_Cortex_M_stacking,	/* stacking_info */
+		0,					/* num_threads */
+	},
+};
+
+static const char * const uCOS_III_symbol_list[] = {
+	"OSRunning",
+	"OSTCBCurPtr",
+	"OSTaskDbgListPtr",
+	"OSTaskQty",
+
+	/* also see: contrib/rtos-helpers/uCOS-III-openocd.c */
+	"openocd_OS_TCB_StkPtr_offset",
+	"openocd_OS_TCB_NamePtr_offset",
+	"openocd_OS_TCB_TaskState_offset",
+	"openocd_OS_TCB_Prio_offset",
+	"openocd_OS_TCB_DbgPrevPtr_offset",
+	"openocd_OS_TCB_DbgNextPtr_offset",
+	NULL
+};
+
+enum uCOS_III_symbol_values {
+	uCOS_III_VAL_OSRunning,
+	uCOS_III_VAL_OSTCBCurPtr,
+	uCOS_III_VAL_OSTaskDbgListPtr,
+	uCOS_III_VAL_OSTaskQty,
+
+	/* also see: contrib/rtos-helpers/uCOS-III-openocd.c */
+	uCOS_III_VAL_OS_TCB_StkPtr_offset,
+	uCOS_III_VAL_OS_TCB_NamePtr_offset,
+	uCOS_III_VAL_OS_TCB_TaskState_offset,
+	uCOS_III_VAL_OS_TCB_Prio_offset,
+	uCOS_III_VAL_OS_TCB_DbgPrevPtr_offset,
+	uCOS_III_VAL_OS_TCB_DbgNextPtr_offset,
+};
+
+static const char * const uCOS_III_thread_state_list[] = {
+	"Ready",
+	"Delay",
+	"Pend",
+	"Pend Timeout",
+	"Suspended",
+	"Delay Suspended",
+	"Pend Suspended",
+	"Pend Timeout Suspended",
+};
+
+static int uCOS_III_find_or_create_thread(struct rtos *rtos, symbol_address_t thread_address,
+		threadid_t *threadid)
+{
+	struct uCOS_III_params *params = rtos->rtos_specific_params;
+	size_t thread_index;
+
+	for (thread_index = 0; thread_index < params->num_threads; thread_index++)
+		if (params->threads[thread_index] == thread_address)
+			goto found;
+
+	if (params->num_threads == UCOS_III_MAX_THREADS) {
+		LOG_WARNING("uCOS-III: too many threads; increase UCOS_III_MAX_THREADS");
+		return ERROR_FAIL;
+	}
+
+	params->threads[thread_index] = thread_address;
+	params->num_threads++;
+found:
+	*threadid = thread_index + params->threadid_start;
+	return ERROR_OK;
+}
+
+static int uCOS_III_find_thread_address(struct rtos *rtos, threadid_t threadid,
+		symbol_address_t *thread_address)
+{
+	struct uCOS_III_params *params = rtos->rtos_specific_params;
+	size_t thread_index;
+
+	thread_index = threadid - params->threadid_start;
+	if (thread_index >= params->num_threads) {
+		LOG_ERROR("uCOS-III: failed to find thread address");
+		return ERROR_FAIL;
+	}
+
+	*thread_address = params->threads[thread_index];
+	return ERROR_OK;
+}
+
+static int uCOS_III_find_last_thread_address(struct rtos *rtos, symbol_address_t *thread_address)
+{
+	struct uCOS_III_params *params = rtos->rtos_specific_params;
+	int retval;
+
+	/* read the thread list head */
+	symbol_address_t thread_list_address = 0;
+
+	retval = target_read_memory(rtos->target,
+				    rtos->symbols[uCOS_III_VAL_OSTaskDbgListPtr].address,
+				    params->pointer_width,
+				    1,
+				    (void *)&thread_list_address);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("uCOS-III: failed to read thread list address");
+		return retval;
+	}
+
+	/* advance to end of thread list */
+	do {
+		*thread_address = thread_list_address;
+
+		retval = target_read_memory(rtos->target,
+					    thread_list_address + params->thread_next_offset,
+					    params->pointer_width,
+					    1,
+					    (void *)&thread_list_address);
+		if (retval != ERROR_OK) {
+			LOG_ERROR("uCOS-III: failed to read next thread address");
+			return retval;
+		}
+	} while (thread_list_address != 0);
+
+	return ERROR_OK;
+}
+
+static int uCOS_III_update_thread_offsets(struct rtos *rtos)
+{
+	struct uCOS_III_params *params = rtos->rtos_specific_params;
+
+	if (params->thread_offsets_updated)
+		return ERROR_OK;
+
+	const struct thread_offset_map {
+		enum uCOS_III_symbol_values symbol_value;
+		symbol_address_t *thread_offset;
+	} thread_offset_maps[] = {
+		{
+			uCOS_III_VAL_OS_TCB_StkPtr_offset,
+			&params->thread_stack_offset,
+		},
+		{
+			uCOS_III_VAL_OS_TCB_NamePtr_offset,
+			&params->thread_name_offset,
+		},
+		{
+			uCOS_III_VAL_OS_TCB_TaskState_offset,
+			&params->thread_state_offset,
+		},
+		{
+			uCOS_III_VAL_OS_TCB_Prio_offset,
+			&params->thread_priority_offset,
+		},
+		{
+			uCOS_III_VAL_OS_TCB_DbgPrevPtr_offset,
+			&params->thread_prev_offset,
+		},
+		{
+			uCOS_III_VAL_OS_TCB_DbgNextPtr_offset,
+			&params->thread_next_offset,
+		},
+	};
+
+	for (size_t i = 0; i < ARRAY_SIZE(thread_offset_maps); i++) {
+		const struct thread_offset_map *thread_offset_map = &thread_offset_maps[i];
+
+		int retval = target_read_memory(rtos->target,
+						rtos->symbols[thread_offset_map->symbol_value].address,
+						params->pointer_width,
+						1,
+						(void *)thread_offset_map->thread_offset);
+		if (retval != ERROR_OK) {
+			LOG_ERROR("uCOS-III: failed to read thread offset");
+			return retval;
+		}
+	}
+
+	params->thread_offsets_updated = true;
+	return ERROR_OK;
+}
+
+static int uCOS_III_detect_rtos(struct target *target)
+{
+	return target->rtos->symbols != NULL &&
+	    target->rtos->symbols[uCOS_III_VAL_OSRunning].address != 0;
+}
+
+static int uCOS_III_reset_handler(struct target *target, enum target_reset_mode reset_mode, void *priv)
+{
+	struct uCOS_III_params *params = target->rtos->rtos_specific_params;
+
+	params->thread_offsets_updated = false;
+	params->num_threads = 0;
+
+	return ERROR_OK;
+}
+
+static int uCOS_III_create(struct target *target)
+{
+	struct uCOS_III_params *params;
+
+	for (size_t i = 0; i < ARRAY_SIZE(uCOS_III_params_list); i++)
+		if (strcmp(uCOS_III_params_list[i].target_name, target->type->name) == 0) {
+			params = malloc(sizeof(*params) +
+			    UCOS_III_MAX_THREADS * sizeof(*params->threads));
+			if (params == NULL) {
+				LOG_ERROR("uCOS-III: out of memory");
+				return ERROR_FAIL;
+			}
+
+			memcpy(params, &uCOS_III_params_list[i], sizeof(uCOS_III_params_list[i]));
+			target->rtos->rtos_specific_params = (void *)params;
+
+			target_register_reset_callback(uCOS_III_reset_handler, NULL);
+
+			return ERROR_OK;
+		}
+
+	LOG_ERROR("uCOS-III: target not supported: %s", target->type->name);
+	return ERROR_FAIL;
+}
+
+static int uCOS_III_update_threads(struct rtos *rtos)
+{
+	struct uCOS_III_params *params = rtos->rtos_specific_params;
+	int retval;
+
+	/* free previous thread details */
+	rtos_free_threadlist(rtos);
+
+	/* verify RTOS is running */
+	uint8_t rtos_running;
+
+	retval = target_read_u8(rtos->target,
+				rtos->symbols[uCOS_III_VAL_OSRunning].address,
+				&rtos_running);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("uCOS-III: failed to read RTOS running");
+		return retval;
+	}
+
+	if (!rtos_running) {
+		rtos->thread_details = calloc(1, sizeof(struct thread_detail));
+		if (rtos->thread_details == NULL) {
+			LOG_ERROR("uCOS-III: out of memory");
+			return ERROR_FAIL;
+		}
+
+		rtos->thread_count = 1;
+		rtos->thread_details->threadid = 0;
+		rtos->thread_details->exists = true;
+		rtos->current_thread = 0;
+
+		return ERROR_OK;
+	}
+
+	/* update thread offsets */
+	retval = uCOS_III_update_thread_offsets(rtos);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("uCOS-III: failed to update thread offsets");
+		return retval;
+	}
+
+	/* read current thread address */
+	symbol_address_t current_thread_address = 0;
+
+	retval = target_read_memory(rtos->target,
+				    rtos->symbols[uCOS_III_VAL_OSTCBCurPtr].address,
+				    params->pointer_width,
+				    1,
+				    (void *)&current_thread_address);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("uCOS-III: failed to read current thread address");
+		return retval;
+	}
+
+	/* read number of tasks */
+	retval = target_read_u16(rtos->target,
+				 rtos->symbols[uCOS_III_VAL_OSTaskQty].address,
+				 (void *)&rtos->thread_count);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("uCOS-III: failed to read thread count");
+		return retval;
+	}
+
+	rtos->thread_details = calloc(rtos->thread_count, sizeof(struct thread_detail));
+	if (rtos->thread_details == NULL) {
+		LOG_ERROR("uCOS-III: out of memory");
+		return ERROR_FAIL;
+	}
+
+	/*
+	 * uC/OS-III adds tasks in LIFO order; advance to the end of the
+	 * list and work backwards to preserve the intended order.
+	 */
+	symbol_address_t thread_address = 0;
+
+	retval = uCOS_III_find_last_thread_address(rtos, &thread_address);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("uCOS-III: failed to find last thread address");
+		return retval;
+	}
+
+	for (int i = 0; i < rtos->thread_count; i++) {
+		struct thread_detail *thread_detail = &rtos->thread_details[i];
+		char thread_str_buffer[UCOS_III_MAX_STRLEN + 1];
+
+		/* find or create new threadid */
+		retval = uCOS_III_find_or_create_thread(rtos,
+							thread_address,
+							&thread_detail->threadid);
+		if (retval != ERROR_OK) {
+			LOG_ERROR("uCOS-III: failed to find or create thread");
+			return retval;
+		}
+
+		if (thread_address == current_thread_address)
+			rtos->current_thread = thread_detail->threadid;
+
+		thread_detail->exists = true;
+
+		/* read thread name */
+		symbol_address_t thread_name_address = 0;
+
+		retval = target_read_memory(rtos->target,
+					    thread_address + params->thread_name_offset,
+					    params->pointer_width,
+					    1,
+					    (void *)&thread_name_address);
+		if (retval != ERROR_OK) {
+			LOG_ERROR("uCOS-III: failed to name address");
+			return retval;
+		}
+
+		retval = target_read_buffer(rtos->target,
+					    thread_name_address,
+					    sizeof(thread_str_buffer),
+					    (void *)thread_str_buffer);
+		if (retval != ERROR_OK) {
+			LOG_ERROR("uCOS-III: failed to read thread name");
+			return retval;
+		}
+
+		thread_str_buffer[sizeof(thread_str_buffer) - 1] = '\0';
+		thread_detail->thread_name_str = strdup(thread_str_buffer);
+
+		/* read thread extra info */
+		uint8_t thread_state;
+		uint8_t thread_priority;
+
+		retval = target_read_u8(rtos->target,
+					thread_address + params->thread_state_offset,
+					&thread_state);
+		if (retval != ERROR_OK) {
+			LOG_ERROR("uCOS-III: failed to read thread state");
+			return retval;
+		}
+
+		retval = target_read_u8(rtos->target,
+					thread_address + params->thread_priority_offset,
+					&thread_priority);
+		if (retval != ERROR_OK) {
+			LOG_ERROR("uCOS-III: failed to read thread priority");
+			return retval;
+		}
+
+		const char *thread_state_str;
+
+		if (thread_state < ARRAY_SIZE(uCOS_III_thread_state_list))
+			thread_state_str = uCOS_III_thread_state_list[thread_state];
+		else
+			thread_state_str = "Unknown";
+
+		snprintf(thread_str_buffer, sizeof(thread_str_buffer), "State: %s, Priority: %d",
+			 thread_state_str, thread_priority);
+		thread_detail->extra_info_str = strdup(thread_str_buffer);
+
+		/* read previous thread address */
+		retval = target_read_memory(rtos->target,
+					    thread_address + params->thread_prev_offset,
+					    params->pointer_width,
+					    1,
+					    (void *)&thread_address);
+		if (retval != ERROR_OK) {
+			LOG_ERROR("uCOS-III: failed to read previous thread address");
+			return retval;
+		}
+	}
+
+	return ERROR_OK;
+}
+
+static int uCOS_III_get_thread_reg_list(struct rtos *rtos, threadid_t threadid, char **hex_reg_list)
+{
+	struct uCOS_III_params *params = rtos->rtos_specific_params;
+	int retval;
+
+	/* find thread address for threadid */
+	symbol_address_t thread_address = 0;
+
+	retval = uCOS_III_find_thread_address(rtos, threadid, &thread_address);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("uCOS-III: failed to find thread address");
+		return retval;
+	}
+
+	/* read thread stack address */
+	symbol_address_t stack_address = 0;
+
+	retval = target_read_memory(rtos->target,
+				    thread_address + params->thread_stack_offset,
+				    params->pointer_width,
+				    1,
+				    (void *)&stack_address);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("uCOS-III: failed to read stack address");
+		return retval;
+	}
+
+	return rtos_generic_stack_read(rtos->target,
+				       params->stacking_info,
+				       stack_address,
+				       hex_reg_list);
+}
+
+static int uCOS_III_get_symbol_list_to_lookup(symbol_table_elem_t *symbol_list[])
+{
+	*symbol_list = calloc(ARRAY_SIZE(uCOS_III_symbol_list), sizeof(symbol_table_elem_t));
+	if (*symbol_list == NULL) {
+		LOG_ERROR("uCOS-III: out of memory");
+		return ERROR_FAIL;
+	}
+
+	for (size_t i = 0; i < ARRAY_SIZE(uCOS_III_symbol_list); i++)
+		(*symbol_list)[i].symbol_name = uCOS_III_symbol_list[i];
+
+	return ERROR_OK;
+}
+
+const struct rtos_type uCOS_III_rtos = {
+	.name = "uCOS-III",
+	.detect_rtos = uCOS_III_detect_rtos,
+	.create = uCOS_III_create,
+	.update_threads = uCOS_III_update_threads,
+	.get_thread_reg_list = uCOS_III_get_thread_reg_list,
+	.get_symbol_list_to_lookup = uCOS_III_get_symbol_list_to_lookup,
+};
diff --git a/src/server/Makefile.am b/src/server/Makefile.am
index 04d3035..804efac 100644
--- a/src/server/Makefile.am
+++ b/src/server/Makefile.am
@@ -1,23 +1,19 @@
-include $(top_srcdir)/common.mk
+noinst_LTLIBRARIES += %D%/libserver.la
+%C%_libserver_la_SOURCES = \
+	%D%/server.c \
+	%D%/telnet_server.c \
+	%D%/gdb_server.c \
+	%D%/server.h \
+	%D%/telnet_server.h \
+	%D%/gdb_server.h \
+	%D%/server_stubs.c \
+	%D%/tcl_server.c \
+	%D%/tcl_server.h
 
-METASOURCES = AUTO
-noinst_LTLIBRARIES = libserver.la
-noinst_HEADERS = server.h telnet_server.h gdb_server.h
-libserver_la_SOURCES = server.c telnet_server.c gdb_server.c
-
-libserver_la_SOURCES += server_stubs.c
-
-libserver_la_CFLAGS =
+%C%_libserver_la_CFLAGS = $(AM_CFLAGS)
 if IS_MINGW
 # FD_* macros are sloppy with their signs on MinGW32 platform
-libserver_la_CFLAGS += -Wno-sign-compare
+%C%_libserver_la_CFLAGS += -Wno-sign-compare
 endif
 
-# tcl server addons
-noinst_HEADERS += tcl_server.h
-libserver_la_SOURCES += tcl_server.c
-
-EXTRA_DIST = \
-	startup.tcl
-
-MAINTAINERCLEANFILES = $(srcdir)/Makefile.in
+STARTUP_TCL_SRCS += %D%/startup.tcl
diff --git a/src/server/gdb_server.c b/src/server/gdb_server.c
index 4828c21..e7fe657 100644
--- a/src/server/gdb_server.c
+++ b/src/server/gdb_server.c
@@ -90,6 +90,8 @@ struct gdb_connection {
 	bool attached;
 	/* temporarily used for target description support */
 	struct target_desc_format target_desc;
+	/* temporarily used for thread list support */
+	char *thread_list;
 };
 
 #if 0
@@ -695,7 +697,8 @@ static int gdb_output_con(struct connection *connection, const char *line)
 		return ERROR_GDB_BUFFER_TOO_SMALL;
 
 	hex_buffer[0] = 'O';
-	int pkt_len = hexify(hex_buffer + 1, line, bin_size, bin_size * 2 + 1);
+	size_t pkt_len = hexify(hex_buffer + 1, (const uint8_t *)line, bin_size,
+		bin_size * 2 + 1);
 	int retval = gdb_put_packet(connection, hex_buffer, pkt_len + 1);
 
 	free(hex_buffer);
@@ -732,22 +735,22 @@ static void gdb_signal_reply(struct target *target, struct connection *connectio
 		stop_reason[0] = '\0';
 		if (target->debug_reason == DBG_REASON_WATCHPOINT) {
 			enum watchpoint_rw hit_wp_type;
-			uint32_t hit_wp_address;
+			target_addr_t hit_wp_address;
 
 			if (watchpoint_hit(target, &hit_wp_type, &hit_wp_address) == ERROR_OK) {
 
 				switch (hit_wp_type) {
 					case WPT_WRITE:
 						snprintf(stop_reason, sizeof(stop_reason),
-								"watch:%08" PRIx32 ";", hit_wp_address);
+								"watch:%08" TARGET_PRIxADDR ";", hit_wp_address);
 						break;
 					case WPT_READ:
 						snprintf(stop_reason, sizeof(stop_reason),
-								"rwatch:%08" PRIx32 ";", hit_wp_address);
+								"rwatch:%08" TARGET_PRIxADDR ";", hit_wp_address);
 						break;
 					case WPT_ACCESS:
 						snprintf(stop_reason, sizeof(stop_reason),
-								"awatch:%08" PRIx32 ";", hit_wp_address);
+								"awatch:%08" TARGET_PRIxADDR ";", hit_wp_address);
 						break;
 					default:
 						break;
@@ -934,6 +937,7 @@ static int gdb_new_connection(struct connection *connection)
 	gdb_connection->attached = true;
 	gdb_connection->target_desc.tdesc = NULL;
 	gdb_connection->target_desc.tdesc_length = 0;
+	gdb_connection->thread_list = NULL;
 
 	/* send ACK to GDB for debug request */
 	gdb_write(connection, "+", 1);
@@ -1376,7 +1380,7 @@ static int gdb_read_memory_packet(struct connection *connection,
 {
 	struct target *target = get_target_from_connection(connection);
 	char *separator;
-	uint32_t addr = 0;
+	uint64_t addr = 0;
 	uint32_t len = 0;
 
 	uint8_t *buffer;
@@ -1387,7 +1391,7 @@ static int gdb_read_memory_packet(struct connection *connection,
 	/* skip command character */
 	packet++;
 
-	addr = strtoul(packet, &separator, 16);
+	addr = strtoull(packet, &separator, 16);
 
 	if (*separator != ',') {
 		LOG_ERROR("incomplete read memory packet received, dropping connection");
@@ -1404,7 +1408,7 @@ static int gdb_read_memory_packet(struct connection *connection,
 
 	buffer = malloc(len);
 
-	LOG_DEBUG("addr: 0x%8.8" PRIx32 ", len: 0x%8.8" PRIx32 "", addr, len);
+	LOG_DEBUG("addr: 0x%16.16" PRIx64 ", len: 0x%8.8" PRIx32 "", addr, len);
 
 	retval = target_read_buffer(target, addr, len, buffer);
 
@@ -1429,7 +1433,7 @@ static int gdb_read_memory_packet(struct connection *connection,
 	if (retval == ERROR_OK) {
 		hex_buffer = malloc(len * 2 + 1);
 
-		int pkt_len = hexify(hex_buffer, (char *)buffer, len, len * 2 + 1);
+		size_t pkt_len = hexify(hex_buffer, buffer, len, len * 2 + 1);
 
 		gdb_put_packet(connection, hex_buffer, pkt_len);
 
@@ -1447,7 +1451,7 @@ static int gdb_write_memory_packet(struct connection *connection,
 {
 	struct target *target = get_target_from_connection(connection);
 	char *separator;
-	uint32_t addr = 0;
+	uint64_t addr = 0;
 	uint32_t len = 0;
 
 	uint8_t *buffer;
@@ -1456,7 +1460,7 @@ static int gdb_write_memory_packet(struct connection *connection,
 	/* skip command character */
 	packet++;
 
-	addr = strtoul(packet, &separator, 16);
+	addr = strtoull(packet, &separator, 16);
 
 	if (*separator != ',') {
 		LOG_ERROR("incomplete write memory packet received, dropping connection");
@@ -1472,9 +1476,9 @@ static int gdb_write_memory_packet(struct connection *connection,
 
 	buffer = malloc(len);
 
-	LOG_DEBUG("addr: 0x%8.8" PRIx32 ", len: 0x%8.8" PRIx32 "", addr, len);
+	LOG_DEBUG("addr: 0x%" PRIx64 ", len: 0x%8.8" PRIx32 "", addr, len);
 
-	if (unhexify((char *)buffer, separator, len) != (int)len)
+	if (unhexify(buffer, separator, len) != len)
 		LOG_ERROR("unable to decode memory packet");
 
 	retval = target_write_buffer(target, addr, len, buffer);
@@ -1494,15 +1498,19 @@ static int gdb_write_memory_binary_packet(struct connection *connection,
 {
 	struct target *target = get_target_from_connection(connection);
 	char *separator;
-	uint32_t addr = 0;
+	uint64_t addr = 0;
 	uint32_t len = 0;
 
 	int retval = ERROR_OK;
+	/* Packets larger than fast_limit bytes will be acknowledged instantly on
+	 * the assumption that we're in a download and it's important to go as fast
+	 * as possible. */
+	uint32_t fast_limit = 8;
 
 	/* skip command character */
 	packet++;
 
-	addr = strtoul(packet, &separator, 16);
+	addr = strtoull(packet, &separator, 16);
 
 	if (*separator != ',') {
 		LOG_ERROR("incomplete write memory binary packet received, dropping connection");
@@ -1518,31 +1526,44 @@ static int gdb_write_memory_binary_packet(struct connection *connection,
 
 	struct gdb_connection *gdb_connection = connection->priv;
 
-	if (gdb_connection->mem_write_error) {
+	if (gdb_connection->mem_write_error)
 		retval = ERROR_FAIL;
-		/* now that we have reported the memory write error, we can clear the condition */
-		gdb_connection->mem_write_error = false;
-	}
 
-	/* By replying the packet *immediately* GDB will send us a new packet
-	 * while we write the last one to the target.
-	 */
-	if (retval == ERROR_OK)
-		gdb_put_packet(connection, "OK", 2);
-	else {
+	if (retval == ERROR_OK) {
+		if (len >= fast_limit) {
+			/* By replying the packet *immediately* GDB will send us a new packet
+			 * while we write the last one to the target.
+			 * We only do this for larger writes, so that users who do something like:
+			 * p *((int*)0xdeadbeef)=8675309
+			 * will get immediate feedback that that write failed.
+			 */
+			gdb_put_packet(connection, "OK", 2);
+		}
+	} else {
 		retval = gdb_error(connection, retval);
+		/* now that we have reported the memory write error, we can clear the condition */
+		gdb_connection->mem_write_error = false;
 		if (retval != ERROR_OK)
 			return retval;
 	}
 
 	if (len) {
-		LOG_DEBUG("addr: 0x%8.8" PRIx32 ", len: 0x%8.8" PRIx32 "", addr, len);
+		LOG_DEBUG("addr: 0x%" PRIx64 ", len: 0x%8.8" PRIx32 "", addr, len);
 
 		retval = target_write_buffer(target, addr, len, (uint8_t *)separator);
 		if (retval != ERROR_OK)
 			gdb_connection->mem_write_error = true;
 	}
 
+	if (len < fast_limit) {
+		if (retval != ERROR_OK) {
+			gdb_error(connection, retval);
+			gdb_connection->mem_write_error = false;
+		} else {
+			gdb_put_packet(connection, "OK", 2);
+		}
+	}
+
 	return ERROR_OK;
 }
 
@@ -1551,13 +1572,13 @@ static int gdb_step_continue_packet(struct connection *connection,
 {
 	struct target *target = get_target_from_connection(connection);
 	int current = 0;
-	uint32_t address = 0x0;
+	uint64_t address = 0x0;
 	int retval = ERROR_OK;
 
 	LOG_DEBUG("-");
 
 	if (packet_size > 1)
-		address = strtoul(packet + 1, NULL, 16);
+		address = strtoull(packet + 1, NULL, 16);
 	else
 		current = 1;
 
@@ -1581,7 +1602,7 @@ static int gdb_breakpoint_watchpoint_packet(struct connection *connection,
 	int type;
 	enum breakpoint_type bp_type = BKPT_SOFT /* dummy init to avoid warning */;
 	enum watchpoint_rw wp_type = WPT_READ /* dummy init to avoid warning */;
-	uint32_t address;
+	uint64_t address;
 	uint32_t size;
 	char *separator;
 	int retval;
@@ -1613,7 +1634,7 @@ static int gdb_breakpoint_watchpoint_packet(struct connection *connection,
 		return ERROR_SERVER_REMOTE_CLOSED;
 	}
 
-	address = strtoul(separator + 1, &separator, 16);
+	address = strtoull(separator + 1, &separator, 16);
 
 	if (*separator != ',') {
 		LOG_ERROR("incomplete breakpoint/watchpoint packet received, dropping connection");
@@ -2291,6 +2312,95 @@ error:
 	return retval;
 }
 
+static int gdb_generate_thread_list(struct target *target, char **thread_list_out)
+{
+	struct rtos *rtos = target->rtos;
+	int retval = ERROR_OK;
+	char *thread_list = NULL;
+	int pos = 0;
+	int size = 0;
+
+	xml_printf(&retval, &thread_list, &pos, &size,
+		   "<?xml version=\"1.0\"?>\n"
+		   "<threads>\n");
+
+	if (rtos != NULL) {
+		for (int i = 0; i < rtos->thread_count; i++) {
+			struct thread_detail *thread_detail = &rtos->thread_details[i];
+
+			if (!thread_detail->exists)
+				continue;
+
+			xml_printf(&retval, &thread_list, &pos, &size,
+				   "<thread id=\"%" PRIx64 "\">", thread_detail->threadid);
+
+			if (thread_detail->thread_name_str != NULL)
+				xml_printf(&retval, &thread_list, &pos, &size,
+					   "Name: %s", thread_detail->thread_name_str);
+
+			if (thread_detail->extra_info_str != NULL) {
+				if (thread_detail->thread_name_str != NULL)
+					xml_printf(&retval, &thread_list, &pos, &size,
+						   ", ");
+				xml_printf(&retval, &thread_list, &pos, &size,
+					   thread_detail->extra_info_str);
+			}
+
+			xml_printf(&retval, &thread_list, &pos, &size,
+				   "</thread>\n");
+		}
+	}
+
+	xml_printf(&retval, &thread_list, &pos, &size,
+		   "</threads>\n");
+
+	if (retval == ERROR_OK)
+		*thread_list_out = thread_list;
+	else
+		free(thread_list);
+
+	return retval;
+}
+
+static int gdb_get_thread_list_chunk(struct target *target, char **thread_list,
+		char **chunk, int32_t offset, uint32_t length)
+{
+	if (*thread_list == NULL) {
+		int retval = gdb_generate_thread_list(target, thread_list);
+		if (retval != ERROR_OK) {
+			LOG_ERROR("Unable to Generate Thread List");
+			return ERROR_FAIL;
+		}
+	}
+
+	size_t thread_list_length = strlen(*thread_list);
+	char transfer_type;
+
+	length = MIN(length, thread_list_length - offset);
+	if (length < (thread_list_length - offset))
+		transfer_type = 'm';
+	else
+		transfer_type = 'l';
+
+	*chunk = malloc(length + 2);
+	if (*chunk == NULL) {
+		LOG_ERROR("Unable to allocate memory");
+		return ERROR_FAIL;
+	}
+
+	(*chunk)[0] = transfer_type;
+	strncpy((*chunk) + 1, (*thread_list) + offset, length);
+	(*chunk)[1 + length] = '\0';
+
+	/* After gdb-server sends out last chunk, invalidate thread list. */
+	if (transfer_type == 'l') {
+		free(*thread_list);
+		*thread_list = NULL;
+	}
+
+	return ERROR_OK;
+}
+
 static int gdb_query_packet(struct connection *connection,
 		char const *packet, int packet_size)
 {
@@ -2302,7 +2412,7 @@ static int gdb_query_packet(struct connection *connection,
 		if (packet_size > 6) {
 			char *cmd;
 			cmd = malloc((packet_size - 6) / 2 + 1);
-			int len = unhexify(cmd, packet + 6, (packet_size - 6) / 2);
+			size_t len = unhexify((uint8_t *)cmd, packet + 6, (packet_size - 6) / 2);
 			cmd[len] = 0;
 
 			/* We want to print all debug output to GDB connection */
@@ -2380,7 +2490,7 @@ static int gdb_query_packet(struct connection *connection,
 			&buffer,
 			&pos,
 			&size,
-			"PacketSize=%x;qXfer:memory-map:read%c;qXfer:features:read%c;QStartNoAckMode+",
+			"PacketSize=%x;qXfer:memory-map:read%c;qXfer:features:read%c;qXfer:threads:read+;QStartNoAckMode+",
 			(GDB_BUFFER_SIZE - 1),
 			((gdb_use_memory_map == 1) && (flash_get_bank_count() > 0)) ? '+' : '-',
 			(gdb_target_desc_supported == 1) ? '+' : '-');
@@ -2428,6 +2538,37 @@ static int gdb_query_packet(struct connection *connection,
 
 		free(xml);
 		return ERROR_OK;
+	} else if (strncmp(packet, "qXfer:threads:read:", 19) == 0) {
+		char *xml = NULL;
+		int retval = ERROR_OK;
+
+		int offset;
+		unsigned int length;
+
+		/* skip command character */
+		packet += 19;
+
+		if (decode_xfer_read(packet, NULL, &offset, &length) < 0) {
+			gdb_send_error(connection, 01);
+			return ERROR_OK;
+		}
+
+		/* Target should prepare correct thread list for annex.
+		 * The first character of returned xml is 'm' or 'l'. 'm' for
+		 * there are *more* chunks to transfer. 'l' for it is the *last*
+		 * chunk of target description.
+		 */
+		retval = gdb_get_thread_list_chunk(target, &gdb_connection->thread_list,
+						   &xml, offset, length);
+		if (retval != ERROR_OK) {
+			gdb_error(connection, retval);
+			return retval;
+		}
+
+		gdb_put_packet(connection, xml, strlen(xml));
+
+		free(xml);
+		return ERROR_OK;
 	} else if (strncmp(packet, "QStartNoAckMode", 15) == 0) {
 		gdb_connection->noack_mode = 1;
 		gdb_put_packet(connection, "OK", 2);
@@ -2953,6 +3094,11 @@ static int gdb_target_start(struct target *target, const char *port)
 
 static int gdb_target_add_one(struct target *target)
 {
+	if (strcmp(gdb_port, "disabled") == 0) {
+		LOG_INFO("gdb port disabled");
+		return ERROR_OK;
+	}
+
 	/*  one gdb instance per smp list */
 	if ((target->smp) && (target->gdb_service))
 		return ERROR_OK;
@@ -3146,7 +3292,7 @@ static const struct command_registration gdb_command_handlers[] = {
 			"server listens for the next port number after the "
 			"base port number specified. "
 			"No arguments reports GDB port. \"pipe\" means listen to stdin "
-			"output to stdout, an integer is base port number, \"disable\" disables "
+			"output to stdout, an integer is base port number, \"disabled\" disables "
 			"port. Any other string is are interpreted as named pipe to listen to. "
 			"Output pipe is the same name as input pipe, but with 'o' appended.",
 		.usage = "[port_num]",
diff --git a/src/server/server.c b/src/server/server.c
index f6889a0..8009d40 100644
--- a/src/server/server.c
+++ b/src/server/server.c
@@ -132,7 +132,9 @@ static int add_connection(struct service *service, struct command_context *cmd_c
 		free(out_file);
 		if (c->fd_out == -1) {
 			LOG_ERROR("could not open %s", service->port);
-			exit(1);
+			command_done(c->cmd_ctx);
+			free(c);
+			return ERROR_FAIL;
 		}
 
 		LOG_INFO("accepting '%s' connection from pipe %s", service->name, service->port);
@@ -191,7 +193,13 @@ static int remove_connection(struct service *service, struct connection *connect
 	return ERROR_OK;
 }
 
-/* FIX! make service return error instead of invoking exit() */
+static void free_service(struct service *c)
+{
+	free(c->name);
+	free(c->port);
+	free(c);
+}
+
 int add_service(char *name,
 	const char *port,
 	int max_connections,
@@ -235,7 +243,8 @@ int add_service(char *name,
 		c->fd = socket(AF_INET, SOCK_STREAM, 0);
 		if (c->fd == -1) {
 			LOG_ERROR("error creating socket: %s", strerror(errno));
-			exit(-1);
+			free_service(c);
+			return ERROR_FAIL;
 		}
 
 		setsockopt(c->fd,
@@ -255,7 +264,9 @@ int add_service(char *name,
 			hp = gethostbyname(bindto_name);
 			if (hp == NULL) {
 				LOG_ERROR("couldn't resolve bindto address: %s", bindto_name);
-				exit(-1);
+				close_socket(c->fd);
+				free_service(c);
+				return ERROR_FAIL;
 			}
 			memcpy(&c->sin.sin_addr, hp->h_addr_list[0], hp->h_length);
 		}
@@ -263,7 +274,9 @@ int add_service(char *name,
 
 		if (bind(c->fd, (struct sockaddr *)&c->sin, sizeof(c->sin)) == -1) {
 			LOG_ERROR("couldn't bind %s to socket: %s", name, strerror(errno));
-			exit(-1);
+			close_socket(c->fd);
+			free_service(c);
+			return ERROR_FAIL;
 		}
 
 #ifndef _WIN32
@@ -281,7 +294,9 @@ int add_service(char *name,
 
 		if (listen(c->fd, 1) == -1) {
 			LOG_ERROR("couldn't listen on socket: %s", strerror(errno));
-			exit(-1);
+			close_socket(c->fd);
+			free_service(c);
+			return ERROR_FAIL;
 		}
 	} else if (c->type == CONNECTION_STDINOUT) {
 		c->fd = fileno(stdin);
@@ -302,13 +317,15 @@ int add_service(char *name,
 		/* we currenty do not support named pipes under win32
 		 * so exit openocd for now */
 		LOG_ERROR("Named pipes currently not supported under this os");
-		exit(1);
+		free_service(c);
+		return ERROR_FAIL;
 #else
 		/* Pipe we're reading from */
 		c->fd = open(c->port, O_RDONLY | O_NONBLOCK);
 		if (c->fd == -1) {
 			LOG_ERROR("could not open %s", c->port);
-			exit(1);
+			free_service(c);
+			return ERROR_FAIL;
 		}
 #endif
 	}
@@ -425,7 +442,7 @@ int server_loop(struct command_context *command_context)
 				FD_ZERO(&read_fds);
 			else {
 				LOG_ERROR("error during select: %s", strerror(errno));
-				exit(-1);
+				return ERROR_FAIL;
 			}
 #else
 
@@ -433,7 +450,7 @@ int server_loop(struct command_context *command_context)
 				FD_ZERO(&read_fds);
 			else {
 				LOG_ERROR("error during select: %s", strerror(errno));
-				exit(-1);
+				return ERROR_FAIL;
 			}
 #endif
 		}
@@ -552,7 +569,7 @@ int server_preinit(void)
 
 	if (WSAStartup(wVersionRequested, &wsaData) != 0) {
 		LOG_ERROR("Failed to Open Winsock");
-		exit(-1);
+		return ERROR_FAIL;
 	}
 
 	/* register ctrl-c handler */
@@ -570,10 +587,18 @@ int server_preinit(void)
 int server_init(struct command_context *cmd_ctx)
 {
 	int ret = tcl_init();
-	if (ERROR_OK != ret)
+
+	if (ret != ERROR_OK)
 		return ret;
 
-	return telnet_init("Open On-Chip Debugger");
+	ret = telnet_init("Open On-Chip Debugger");
+
+	if (ret != ERROR_OK) {
+		remove_services();
+		return ret;
+	}
+
+	return ERROR_OK;
 }
 
 int server_quit(void)
diff --git a/src/server/tcl_server.c b/src/server/tcl_server.c
index 15a8736..0077339 100644
--- a/src/server/tcl_server.c
+++ b/src/server/tcl_server.c
@@ -105,7 +105,7 @@ static int tcl_target_callback_trace_handler(struct target *target,
 	if (tclc->tc_trace) {
 		hex = malloc(hex_len);
 		buf = malloc(max_len);
-		hexify(hex, (const char *)data, len, hex_len);
+		hexify(hex, data, len, hex_len);
 		snprintf(buf, max_len, "%s%s%s", header, hex, trailer);
 		tcl_output(connection, buf, strlen(buf));
 		free(hex);
diff --git a/src/server/telnet_server.c b/src/server/telnet_server.c
index 0f5769a..e33188b 100644
--- a/src/server/telnet_server.c
+++ b/src/server/telnet_server.c
@@ -573,7 +573,7 @@ static int telnet_input(struct connection *connection)
 				break;
 			default:
 				LOG_ERROR("unknown telnet state");
-				exit(-1);
+				return ERROR_FAIL;
 		}
 
 		bytes_read--;
@@ -624,9 +624,8 @@ int telnet_init(char *banner)
 		return ERROR_OK;
 	}
 
-	struct telnet_service *telnet_service;
-
-	telnet_service = malloc(sizeof(struct telnet_service));
+	struct telnet_service *telnet_service =
+		malloc(sizeof(struct telnet_service));
 
 	if (!telnet_service) {
 		LOG_ERROR("Failed to allocate telnet service.");
@@ -635,13 +634,16 @@ int telnet_init(char *banner)
 
 	telnet_service->banner = banner;
 
-	return add_service("telnet",
-		telnet_port,
-		CONNECTION_LIMIT_UNLIMITED,
-		telnet_new_connection,
-		telnet_input,
-		telnet_connection_closed,
+	int ret = add_service("telnet", telnet_port, CONNECTION_LIMIT_UNLIMITED,
+		telnet_new_connection, telnet_input, telnet_connection_closed,
 		telnet_service);
+
+	if (ret != ERROR_OK) {
+		free(telnet_service);
+		return ret;
+	}
+
+	return ERROR_OK;
 }
 
 /* daemon configuration command telnet_port */
diff --git a/src/svf/Makefile.am b/src/svf/Makefile.am
index 3a14d20..5603d53 100644
--- a/src/svf/Makefile.am
+++ b/src/svf/Makefile.am
@@ -1,8 +1,2 @@
-include $(top_srcdir)/common.mk
-
-METASOURCES = AUTO
-noinst_LTLIBRARIES = libsvf.la
-noinst_HEADERS = svf.h
-libsvf_la_SOURCES = svf.c
-
-MAINTAINERCLEANFILES = $(srcdir)/Makefile.in
+noinst_LTLIBRARIES += %D%/libsvf.la
+%C%_libsvf_la_SOURCES = %D%/svf.c %D%/svf.h
diff --git a/src/target/Makefile.am b/src/target/Makefile.am
index ba15300..1e81f4a 100644
--- a/src/target/Makefile.am
+++ b/src/target/Makefile.am
@@ -1,31 +1,15 @@
-include $(top_srcdir)/common.mk
-
 if OOCD_TRACE
-OOCD_TRACE_FILES = oocd_trace.c
+OOCD_TRACE_FILES = %D%/oocd_trace.c
 else
 OOCD_TRACE_FILES =
 endif
 
-SUBDIRS = openrisc
-libtarget_la_LIBADD = $(top_builddir)/src/target/openrisc/libopenrisc.la
-
-BIN2C		= $(top_srcdir)/src/helper/bin2char.sh
-
-DEBUG_HANDLER	= $(srcdir)/xscale/debug_handler.bin
-EXTRA_DIST = \
-	startup.tcl \
-	$(wildcard $(srcdir)/xscale/*)
+%C%_libtarget_la_LIBADD = %D%/openrisc/libopenrisc.la
 
-DEBUG_HEADER	= xscale_debug.inc
-BUILT_SOURCES = $(DEBUG_HEADER)
-CLEANFILES = $(DEBUG_HEADER)
+STARTUP_TCL_SRCS += %D%/startup.tcl
 
-$(DEBUG_HEADER): $(DEBUG_HANDLER) $(BIN2C)
-	$(BIN2C) < $< > $@ || { rm -f $@; false; }
-
-METASOURCES = AUTO
-noinst_LTLIBRARIES = libtarget.la
-libtarget_la_SOURCES = \
+noinst_LTLIBRARIES += %D%/libtarget.la
+%C%_libtarget_la_SOURCES = \
 	$(TARGET_CORE_SRC) \
 	$(ARM_DEBUG_SRC) \
 	$(ARMV4_5_SRC) \
@@ -37,185 +21,200 @@ libtarget_la_SOURCES = \
 	$(NDS32_SRC) \
 	$(INTEL_IA32_SRC) \
 	$(RISCV_SRC) \
-	avrt.c \
-	dsp563xx.c \
-	dsp563xx_once.c \
-	dsp5680xx.c \
-	hla_target.c
+	%D%/avrt.c \
+	%D%/dsp563xx.c \
+	%D%/dsp563xx_once.c \
+	%D%/dsp5680xx.c \
+	%D%/hla_target.c
+
+if TARGET64
+%C%_libtarget_la_SOURCES +=$(ARMV8_SRC)
+endif
 
 TARGET_CORE_SRC = \
-	algorithm.c \
-	register.c \
-	image.c \
-	breakpoints.c \
-	target.c \
-	target_request.c \
-	testee.c \
-	smp.c
+	%D%/algorithm.c \
+	%D%/register.c \
+	%D%/image.c \
+	%D%/breakpoints.c \
+	%D%/target.c \
+	%D%/target_request.c \
+	%D%/testee.c \
+	%D%/smp.c
 
 ARMV4_5_SRC = \
-	armv4_5.c \
-	armv4_5_mmu.c \
-	armv4_5_cache.c \
+	%D%/armv4_5.c \
+	%D%/armv4_5_mmu.c \
+	%D%/armv4_5_cache.c \
 	$(ARM7_9_SRC)
 
 ARM7_9_SRC = \
-	arm7_9_common.c \
-	arm7tdmi.c \
-	arm720t.c \
-	arm9tdmi.c \
-	arm920t.c \
-	arm966e.c \
-	arm946e.c \
-	arm926ejs.c \
-	feroceon.c
+	%D%/arm7_9_common.c \
+	%D%/arm7tdmi.c \
+	%D%/arm720t.c \
+	%D%/arm9tdmi.c \
+	%D%/arm920t.c \
+	%D%/arm966e.c \
+	%D%/arm946e.c \
+	%D%/arm926ejs.c \
+	%D%/feroceon.c
 
 ARM_MISC_SRC = \
-	fa526.c \
-	xscale.c
+	%D%/fa526.c \
+	%D%/xscale.c
 
 ARMV6_SRC = \
-	arm11.c \
-	arm11_dbgtap.c
+	%D%/arm11.c \
+	%D%/arm11_dbgtap.c
 
 ARMV7_SRC = \
-	armv7m.c \
-	armv7m_trace.c \
-	cortex_m.c \
-	armv7a.c \
-	cortex_a.c \
-	ls1_sap.c
+	%D%/armv7m.c \
+	%D%/armv7m_trace.c \
+	%D%/cortex_m.c \
+	%D%/armv7a.c \
+	%D%/cortex_a.c \
+	%D%/ls1_sap.c
+
+ARMV8_SRC = \
+	%D%/armv8_dpm.c \
+	%D%/armv8_opcodes.c \
+	%D%/aarch64.c \
+	%D%/armv8.c \
+	%D%/armv8_cache.c
 
 ARM_DEBUG_SRC = \
-	arm_dpm.c \
-	arm_jtag.c \
-	arm_disassembler.c \
-	arm_simulator.c \
-	arm_semihosting.c \
-	arm_adi_v5.c \
-	armv7a_cache.c \
-	armv7a_cache_l2x.c \
-	adi_v5_jtag.c \
-	adi_v5_swd.c \
-	embeddedice.c \
-	trace.c \
-	etb.c \
-	etm.c \
+	%D%/arm_dpm.c \
+	%D%/arm_jtag.c \
+	%D%/arm_disassembler.c \
+	%D%/arm_simulator.c \
+	%D%/arm_semihosting.c \
+	%D%/arm_adi_v5.c \
+	%D%/armv7a_cache.c \
+	%D%/armv7a_cache_l2x.c \
+	%D%/adi_v5_jtag.c \
+	%D%/adi_v5_swd.c \
+	%D%/embeddedice.c \
+	%D%/trace.c \
+	%D%/etb.c \
+	%D%/etm.c \
 	$(OOCD_TRACE_FILES) \
-	etm_dummy.c
+	%D%/etm_dummy.c \
+	%D%/arm_cti.c
 
 AVR32_SRC = \
-	avr32_ap7k.c \
-	avr32_jtag.c \
-	avr32_mem.c \
-	avr32_regs.c
+	%D%/avr32_ap7k.c \
+	%D%/avr32_jtag.c \
+	%D%/avr32_mem.c \
+	%D%/avr32_regs.c
 
 MIPS32_SRC = \
-	mips32.c \
-	mips_m4k.c \
-	mips32_pracc.c \
-	mips32_dmaacc.c \
-	mips_ejtag.c
+	%D%/mips32.c \
+	%D%/mips_m4k.c \
+	%D%/mips32_pracc.c \
+	%D%/mips32_dmaacc.c \
+	%D%/mips_ejtag.c
 
 NDS32_SRC = \
-	nds32.c \
-	nds32_reg.c \
-	nds32_cmd.c \
-	nds32_disassembler.c \
-	nds32_tlb.c \
-	nds32_v2.c \
-	nds32_v3_common.c \
-	nds32_v3.c \
-	nds32_v3m.c \
-	nds32_aice.c
+	%D%/nds32.c \
+	%D%/nds32_reg.c \
+	%D%/nds32_cmd.c \
+	%D%/nds32_disassembler.c \
+	%D%/nds32_tlb.c \
+	%D%/nds32_v2.c \
+	%D%/nds32_v3_common.c \
+	%D%/nds32_v3.c \
+	%D%/nds32_v3m.c \
+	%D%/nds32_aice.c
 
 INTEL_IA32_SRC = \
-	quark_x10xx.c \
-	quark_d20xx.c \
-	lakemont.c \
-	x86_32_common.c
+	%D%/quark_x10xx.c \
+	%D%/quark_d20xx.c \
+	%D%/lakemont.c \
+	%D%/x86_32_common.c
 
 RISCV_SRC = \
-	riscv/riscv-011.c \
-	riscv/riscv-013.c \
-	riscv/riscv.c \
-	riscv/program.c \
-	riscv/batch.c
-
-noinst_HEADERS = \
-	algorithm.h \
-	arm.h \
-	arm_dpm.h \
-	arm_jtag.h \
-	arm_adi_v5.h \
-	armv7a_cache.h \
-	armv7a_cache_l2x.h \
-	arm_disassembler.h \
-	arm_opcodes.h \
-	arm_simulator.h \
-	arm_semihosting.h \
-	arm7_9_common.h \
-	arm7tdmi.h \
-	arm720t.h \
-	arm9tdmi.h \
-	arm920t.h \
-	arm926ejs.h \
-	arm966e.h \
-	arm946e.h \
-	arm11.h \
-	arm11_dbgtap.h \
-	armv4_5.h \
-	armv4_5_mmu.h \
-	armv4_5_cache.h \
-	armv7a.h \
-	armv7m.h \
-	armv7m_trace.h \
-	avrt.h \
-	dsp563xx.h \
-	dsp563xx_once.h \
-	dsp5680xx.h \
-	breakpoints.h \
-	cortex_m.h \
-	cortex_a.h \
-	embeddedice.h \
-	etb.h \
-	etm.h \
-	etm_dummy.h \
-	image.h \
-	mips32.h \
-	mips_m4k.h \
-	mips_ejtag.h \
-	mips32_pracc.h \
-	mips32_dmaacc.h \
-	oocd_trace.h \
-	register.h \
-	target.h \
-	target_type.h \
-	trace.h \
-	target_request.h \
-	trace.h \
-	xscale.h \
-	smp.h \
-	avr32_ap7k.h \
-	avr32_jtag.h \
-	avr32_mem.h \
-	avr32_regs.h \
-	nds32.h \
-	nds32_cmd.h \
-	nds32_disassembler.h \
-	nds32_edm.h \
-	nds32_insn.h \
-	nds32_reg.h \
-	nds32_tlb.h \
-	nds32_v2.h \
-	nds32_v3_common.h \
-	nds32_v3.h \
-	nds32_v3m.h \
-	nds32_aice.h \
-	lakemont.h \
-	x86_32_common.h
-
-ocddatadir = $(pkglibdir)
-nobase_dist_ocddata_DATA =
-
-MAINTAINERCLEANFILES = $(srcdir)/Makefile.in
+	%D%/riscv/riscv-011.c \
+	%D%/riscv/riscv-013.c \
+	%D%/riscv/riscv.c \
+	%D%/riscv/program.c \
+	%D%/riscv/batch.c
+
+%C%_libtarget_la_SOURCES += \
+	%D%/algorithm.h \
+	%D%/arm.h \
+	%D%/arm_dpm.h \
+	%D%/arm_jtag.h \
+	%D%/arm_adi_v5.h \
+	%D%/armv7a_cache.h \
+	%D%/armv7a_cache_l2x.h \
+	%D%/arm_disassembler.h \
+	%D%/arm_opcodes.h \
+	%D%/arm_simulator.h \
+	%D%/arm_semihosting.h \
+	%D%/arm7_9_common.h \
+	%D%/arm7tdmi.h \
+	%D%/arm720t.h \
+	%D%/arm9tdmi.h \
+	%D%/arm920t.h \
+	%D%/arm926ejs.h \
+	%D%/arm966e.h \
+	%D%/arm946e.h \
+	%D%/arm11.h \
+	%D%/arm11_dbgtap.h \
+	%D%/armv4_5.h \
+	%D%/armv4_5_mmu.h \
+	%D%/armv4_5_cache.h \
+	%D%/armv7a.h \
+	%D%/armv7m.h \
+	%D%/armv7m_trace.h \
+	%D%/armv8.h \
+	%D%/armv8_dpm.h \
+	%D%/armv8_opcodes.h \
+	%D%/armv8_cache.h \
+	%D%/avrt.h \
+	%D%/dsp563xx.h \
+	%D%/dsp563xx_once.h \
+	%D%/dsp5680xx.h \
+	%D%/breakpoints.h \
+	%D%/cortex_m.h \
+	%D%/cortex_a.h \
+	%D%/aarch64.h \
+	%D%/embeddedice.h \
+	%D%/etb.h \
+	%D%/etm.h \
+	%D%/etm_dummy.h \
+	%D%/image.h \
+	%D%/mips32.h \
+	%D%/mips_m4k.h \
+	%D%/mips_ejtag.h \
+	%D%/mips32_pracc.h \
+	%D%/mips32_dmaacc.h \
+	%D%/oocd_trace.h \
+	%D%/register.h \
+	%D%/target.h \
+	%D%/target_type.h \
+	%D%/trace.h \
+	%D%/target_request.h \
+	%D%/trace.h \
+	%D%/xscale.h \
+	%D%/smp.h \
+	%D%/avr32_ap7k.h \
+	%D%/avr32_jtag.h \
+	%D%/avr32_mem.h \
+	%D%/avr32_regs.h \
+	%D%/nds32.h \
+	%D%/nds32_cmd.h \
+	%D%/nds32_disassembler.h \
+	%D%/nds32_edm.h \
+	%D%/nds32_insn.h \
+	%D%/nds32_reg.h \
+	%D%/nds32_tlb.h \
+	%D%/nds32_v2.h \
+	%D%/nds32_v3_common.h \
+	%D%/nds32_v3.h \
+	%D%/nds32_v3m.h \
+	%D%/nds32_aice.h \
+	%D%/lakemont.h \
+	%D%/x86_32_common.h \
+	%D%/arm_cti.h
+
+include %D%/openrisc/Makefile.am
diff --git a/src/target/aarch64.c b/src/target/aarch64.c
new file mode 100644
index 0000000..5e5d3fc
--- /dev/null
+++ b/src/target/aarch64.c
@@ -0,0 +1,2452 @@
+/***************************************************************************
+ *   Copyright (C) 2015 by David Ung                                       *
+ *                                                                         *
+ *   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, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *                                                                         *
+ ***************************************************************************/
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "breakpoints.h"
+#include "aarch64.h"
+#include "register.h"
+#include "target_request.h"
+#include "target_type.h"
+#include "armv8_opcodes.h"
+#include "armv8_cache.h"
+#include <helper/time_support.h>
+
+enum restart_mode {
+	RESTART_LAZY,
+	RESTART_SYNC,
+};
+
+enum halt_mode {
+	HALT_LAZY,
+	HALT_SYNC,
+};
+
+static int aarch64_poll(struct target *target);
+static int aarch64_debug_entry(struct target *target);
+static int aarch64_restore_context(struct target *target, bool bpwp);
+static int aarch64_set_breakpoint(struct target *target,
+	struct breakpoint *breakpoint, uint8_t matchmode);
+static int aarch64_set_context_breakpoint(struct target *target,
+	struct breakpoint *breakpoint, uint8_t matchmode);
+static int aarch64_set_hybrid_breakpoint(struct target *target,
+	struct breakpoint *breakpoint);
+static int aarch64_unset_breakpoint(struct target *target,
+	struct breakpoint *breakpoint);
+static int aarch64_mmu(struct target *target, int *enabled);
+static int aarch64_virt2phys(struct target *target,
+	target_addr_t virt, target_addr_t *phys);
+static int aarch64_read_apb_ap_memory(struct target *target,
+	uint64_t address, uint32_t size, uint32_t count, uint8_t *buffer);
+
+#define foreach_smp_target(pos, head) \
+	for (pos = head; (pos != NULL); pos = pos->next)
+
+static int aarch64_restore_system_control_reg(struct target *target)
+{
+	enum arm_mode target_mode = ARM_MODE_ANY;
+	int retval = ERROR_OK;
+	uint32_t instr;
+
+	struct aarch64_common *aarch64 = target_to_aarch64(target);
+	struct armv8_common *armv8 = target_to_armv8(target);
+
+	if (aarch64->system_control_reg != aarch64->system_control_reg_curr) {
+		aarch64->system_control_reg_curr = aarch64->system_control_reg;
+		/* LOG_INFO("cp15_control_reg: %8.8" PRIx32, cortex_v8->cp15_control_reg); */
+
+		switch (armv8->arm.core_mode) {
+		case ARMV8_64_EL0T:
+			target_mode = ARMV8_64_EL1H;
+			/* fall through */
+		case ARMV8_64_EL1T:
+		case ARMV8_64_EL1H:
+			instr = ARMV8_MSR_GP(SYSTEM_SCTLR_EL1, 0);
+			break;
+		case ARMV8_64_EL2T:
+		case ARMV8_64_EL2H:
+			instr = ARMV8_MSR_GP(SYSTEM_SCTLR_EL2, 0);
+			break;
+		case ARMV8_64_EL3H:
+		case ARMV8_64_EL3T:
+			instr = ARMV8_MSR_GP(SYSTEM_SCTLR_EL3, 0);
+			break;
+
+		case ARM_MODE_SVC:
+		case ARM_MODE_ABT:
+		case ARM_MODE_FIQ:
+		case ARM_MODE_IRQ:
+			instr = ARMV4_5_MCR(15, 0, 0, 1, 0, 0);
+			break;
+
+		default:
+			LOG_INFO("cannot read system control register in this mode");
+			return ERROR_FAIL;
+		}
+
+		if (target_mode != ARM_MODE_ANY)
+			armv8_dpm_modeswitch(&armv8->dpm, target_mode);
+
+		retval = armv8->dpm.instr_write_data_r0(&armv8->dpm, instr, aarch64->system_control_reg);
+		if (retval != ERROR_OK)
+			return retval;
+
+		if (target_mode != ARM_MODE_ANY)
+			armv8_dpm_modeswitch(&armv8->dpm, ARM_MODE_ANY);
+	}
+
+	return retval;
+}
+
+/*  modify system_control_reg in order to enable or disable mmu for :
+ *  - virt2phys address conversion
+ *  - read or write memory in phys or virt address */
+static int aarch64_mmu_modify(struct target *target, int enable)
+{
+	struct aarch64_common *aarch64 = target_to_aarch64(target);
+	struct armv8_common *armv8 = &aarch64->armv8_common;
+	int retval = ERROR_OK;
+	uint32_t instr = 0;
+
+	if (enable) {
+		/*	if mmu enabled at target stop and mmu not enable */
+		if (!(aarch64->system_control_reg & 0x1U)) {
+			LOG_ERROR("trying to enable mmu on target stopped with mmu disable");
+			return ERROR_FAIL;
+		}
+		if (!(aarch64->system_control_reg_curr & 0x1U))
+			aarch64->system_control_reg_curr |= 0x1U;
+	} else {
+		if (aarch64->system_control_reg_curr & 0x4U) {
+			/*  data cache is active */
+			aarch64->system_control_reg_curr &= ~0x4U;
+			/* flush data cache armv8 function to be called */
+			if (armv8->armv8_mmu.armv8_cache.flush_all_data_cache)
+				armv8->armv8_mmu.armv8_cache.flush_all_data_cache(target);
+		}
+		if ((aarch64->system_control_reg_curr & 0x1U)) {
+			aarch64->system_control_reg_curr &= ~0x1U;
+		}
+	}
+
+	switch (armv8->arm.core_mode) {
+	case ARMV8_64_EL0T:
+	case ARMV8_64_EL1T:
+	case ARMV8_64_EL1H:
+		instr = ARMV8_MSR_GP(SYSTEM_SCTLR_EL1, 0);
+		break;
+	case ARMV8_64_EL2T:
+	case ARMV8_64_EL2H:
+		instr = ARMV8_MSR_GP(SYSTEM_SCTLR_EL2, 0);
+		break;
+	case ARMV8_64_EL3H:
+	case ARMV8_64_EL3T:
+		instr = ARMV8_MSR_GP(SYSTEM_SCTLR_EL3, 0);
+		break;
+	default:
+		LOG_DEBUG("unknown cpu state 0x%x" PRIx32, armv8->arm.core_state);
+		break;
+	}
+
+	retval = armv8->dpm.instr_write_data_r0(&armv8->dpm, instr,
+				aarch64->system_control_reg_curr);
+	return retval;
+}
+
+/*
+ * Basic debug access, very low level assumes state is saved
+ */
+static int aarch64_init_debug_access(struct target *target)
+{
+	struct armv8_common *armv8 = target_to_armv8(target);
+	int retval;
+	uint32_t dummy;
+
+	LOG_DEBUG(" ");
+
+	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUV8_DBG_OSLAR, 0);
+	if (retval != ERROR_OK) {
+		LOG_DEBUG("Examine %s failed", "oslock");
+		return retval;
+	}
+
+	/* Clear Sticky Power Down status Bit in PRSR to enable access to
+	   the registers in the Core Power Domain */
+	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUV8_DBG_PRSR, &dummy);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/*
+	 * Static CTI configuration:
+	 * Channel 0 -> trigger outputs HALT request to PE
+	 * Channel 1 -> trigger outputs Resume request to PE
+	 * Gate all channel trigger events from entering the CTM
+	 */
+
+	/* Enable CTI */
+	retval = arm_cti_enable(armv8->cti, true);
+	/* By default, gate all channel events to and from the CTM */
+	if (retval == ERROR_OK)
+		retval = arm_cti_write_reg(armv8->cti, CTI_GATE, 0);
+	/* output halt requests to PE on channel 0 event */
+	if (retval == ERROR_OK)
+		retval = arm_cti_write_reg(armv8->cti, CTI_OUTEN0, CTI_CHNL(0));
+	/* output restart requests to PE on channel 1 event */
+	if (retval == ERROR_OK)
+		retval = arm_cti_write_reg(armv8->cti, CTI_OUTEN1, CTI_CHNL(1));
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Resync breakpoint registers */
+
+	return ERROR_OK;
+}
+
+/* Write to memory mapped registers directly with no cache or mmu handling */
+static int aarch64_dap_write_memap_register_u32(struct target *target,
+	uint32_t address,
+	uint32_t value)
+{
+	int retval;
+	struct armv8_common *armv8 = target_to_armv8(target);
+
+	retval = mem_ap_write_atomic_u32(armv8->debug_ap, address, value);
+
+	return retval;
+}
+
+static int aarch64_dpm_setup(struct aarch64_common *a8, uint64_t debug)
+{
+	struct arm_dpm *dpm = &a8->armv8_common.dpm;
+	int retval;
+
+	dpm->arm = &a8->armv8_common.arm;
+	dpm->didr = debug;
+
+	retval = armv8_dpm_setup(dpm);
+	if (retval == ERROR_OK)
+		retval = armv8_dpm_initialize(dpm);
+
+	return retval;
+}
+
+static int aarch64_set_dscr_bits(struct target *target, unsigned long bit_mask, unsigned long value)
+{
+	struct armv8_common *armv8 = target_to_armv8(target);
+	return armv8_set_dbgreg_bits(armv8, CPUV8_DBG_DSCR, bit_mask, value);
+}
+
+static int aarch64_check_state_one(struct target *target,
+		uint32_t mask, uint32_t val, int *p_result, uint32_t *p_prsr)
+{
+	struct armv8_common *armv8 = target_to_armv8(target);
+	uint32_t prsr;
+	int retval;
+
+	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUV8_DBG_PRSR, &prsr);
+	if (retval != ERROR_OK)
+		return retval;
+
+	if (p_prsr)
+		*p_prsr = prsr;
+
+	if (p_result)
+		*p_result = (prsr & mask) == (val & mask);
+
+	return ERROR_OK;
+}
+
+static int aarch64_wait_halt_one(struct target *target)
+{
+	int retval = ERROR_OK;
+	uint32_t prsr;
+
+	int64_t then = timeval_ms();
+	for (;;) {
+		int halted;
+
+		retval = aarch64_check_state_one(target, PRSR_HALT, PRSR_HALT, &halted, &prsr);
+		if (retval != ERROR_OK || halted)
+			break;
+
+		if (timeval_ms() > then + 1000) {
+			retval = ERROR_TARGET_TIMEOUT;
+			LOG_DEBUG("target %s timeout, prsr=0x%08"PRIx32, target_name(target), prsr);
+			break;
+		}
+	}
+	return retval;
+}
+
+static int aarch64_prepare_halt_smp(struct target *target, bool exc_target, struct target **p_first)
+{
+	int retval = ERROR_OK;
+	struct target_list *head = target->head;
+	struct target *first = NULL;
+
+	LOG_DEBUG("target %s exc %i", target_name(target), exc_target);
+
+	while (head != NULL) {
+		struct target *curr = head->target;
+		struct armv8_common *armv8 = target_to_armv8(curr);
+		head = head->next;
+
+		if (exc_target && curr == target)
+			continue;
+		if (!target_was_examined(curr))
+			continue;
+		if (curr->state != TARGET_RUNNING)
+			continue;
+
+		/* HACK: mark this target as prepared for halting */
+		curr->debug_reason = DBG_REASON_DBGRQ;
+
+		/* open the gate for channel 0 to let HALT requests pass to the CTM */
+		retval = arm_cti_ungate_channel(armv8->cti, 0);
+		if (retval == ERROR_OK)
+			retval = aarch64_set_dscr_bits(curr, DSCR_HDE, DSCR_HDE);
+		if (retval != ERROR_OK)
+			break;
+
+		LOG_DEBUG("target %s prepared", target_name(curr));
+
+		if (first == NULL)
+			first = curr;
+	}
+
+	if (p_first) {
+		if (exc_target && first)
+			*p_first = first;
+		else
+			*p_first = target;
+	}
+
+	return retval;
+}
+
+static int aarch64_halt_one(struct target *target, enum halt_mode mode)
+{
+	int retval = ERROR_OK;
+	struct armv8_common *armv8 = target_to_armv8(target);
+
+	LOG_DEBUG("%s", target_name(target));
+
+	/* allow Halting Debug Mode */
+	retval = aarch64_set_dscr_bits(target, DSCR_HDE, DSCR_HDE);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* trigger an event on channel 0, this outputs a halt request to the PE */
+	retval = arm_cti_pulse_channel(armv8->cti, 0);
+	if (retval != ERROR_OK)
+		return retval;
+
+	if (mode == HALT_SYNC) {
+		retval = aarch64_wait_halt_one(target);
+		if (retval != ERROR_OK) {
+			if (retval == ERROR_TARGET_TIMEOUT)
+				LOG_ERROR("Timeout waiting for target %s halt", target_name(target));
+			return retval;
+		}
+	}
+
+	return ERROR_OK;
+}
+
+static int aarch64_halt_smp(struct target *target, bool exc_target)
+{
+	struct target *next = target;
+	int retval;
+
+	/* prepare halt on all PEs of the group */
+	retval = aarch64_prepare_halt_smp(target, exc_target, &next);
+
+	if (exc_target && next == target)
+		return retval;
+
+	/* halt the target PE */
+	if (retval == ERROR_OK)
+		retval = aarch64_halt_one(next, HALT_LAZY);
+
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* wait for all PEs to halt */
+	int64_t then = timeval_ms();
+	for (;;) {
+		bool all_halted = true;
+		struct target_list *head;
+		struct target *curr;
+
+		foreach_smp_target(head, target->head) {
+			int halted;
+
+			curr = head->target;
+
+			if (!target_was_examined(curr))
+				continue;
+
+			retval = aarch64_check_state_one(curr, PRSR_HALT, PRSR_HALT, &halted, NULL);
+			if (retval != ERROR_OK || !halted) {
+				all_halted = false;
+				break;
+			}
+		}
+
+		if (all_halted)
+			break;
+
+		if (timeval_ms() > then + 1000) {
+			retval = ERROR_TARGET_TIMEOUT;
+			break;
+		}
+
+		/*
+		 * HACK: on Hi6220 there are 8 cores organized in 2 clusters
+		 * and it looks like the CTI's are not connected by a common
+		 * trigger matrix. It seems that we need to halt one core in each
+		 * cluster explicitly. So if we find that a core has not halted
+		 * yet, we trigger an explicit halt for the second cluster.
+		 */
+		retval = aarch64_halt_one(curr, HALT_LAZY);
+		if (retval != ERROR_OK)
+			break;
+	}
+
+	return retval;
+}
+
+static int update_halt_gdb(struct target *target, enum target_debug_reason debug_reason)
+{
+	struct target *gdb_target = NULL;
+	struct target_list *head;
+	struct target *curr;
+
+	if (debug_reason == DBG_REASON_NOTHALTED) {
+		LOG_INFO("Halting remaining targets in SMP group");
+		aarch64_halt_smp(target, true);
+	}
+
+	/* poll all targets in the group, but skip the target that serves GDB */
+	foreach_smp_target(head, target->head) {
+		curr = head->target;
+		/* skip calling context */
+		if (curr == target)
+			continue;
+		if (!target_was_examined(curr))
+			continue;
+		/* skip targets that were already halted */
+		if (curr->state == TARGET_HALTED)
+			continue;
+		/* remember the gdb_service->target */
+		if (curr->gdb_service != NULL)
+			gdb_target = curr->gdb_service->target;
+		/* skip it */
+		if (curr == gdb_target)
+			continue;
+
+		/* avoid recursion in aarch64_poll() */
+		curr->smp = 0;
+		aarch64_poll(curr);
+		curr->smp = 1;
+	}
+
+	/* after all targets were updated, poll the gdb serving target */
+	if (gdb_target != NULL && gdb_target != target)
+		aarch64_poll(gdb_target);
+
+	return ERROR_OK;
+}
+
+/*
+ * Aarch64 Run control
+ */
+
+static int aarch64_poll(struct target *target)
+{
+	enum target_state prev_target_state;
+	int retval = ERROR_OK;
+	int halted;
+
+	retval = aarch64_check_state_one(target,
+				PRSR_HALT, PRSR_HALT, &halted, NULL);
+	if (retval != ERROR_OK)
+		return retval;
+
+	if (halted) {
+		prev_target_state = target->state;
+		if (prev_target_state != TARGET_HALTED) {
+			enum target_debug_reason debug_reason = target->debug_reason;
+
+			/* We have a halting debug event */
+			target->state = TARGET_HALTED;
+			LOG_DEBUG("Target %s halted", target_name(target));
+			retval = aarch64_debug_entry(target);
+			if (retval != ERROR_OK)
+				return retval;
+
+			if (target->smp)
+				update_halt_gdb(target, debug_reason);
+
+			switch (prev_target_state) {
+			case TARGET_RUNNING:
+			case TARGET_UNKNOWN:
+			case TARGET_RESET:
+				target_call_event_callbacks(target, TARGET_EVENT_HALTED);
+				break;
+			case TARGET_DEBUG_RUNNING:
+				target_call_event_callbacks(target, TARGET_EVENT_DEBUG_HALTED);
+				break;
+			default:
+				break;
+			}
+		}
+	} else
+		target->state = TARGET_RUNNING;
+
+	return retval;
+}
+
+static int aarch64_halt(struct target *target)
+{
+	if (target->smp)
+		return aarch64_halt_smp(target, false);
+
+	return aarch64_halt_one(target, HALT_SYNC);
+}
+
+static int aarch64_restore_one(struct target *target, int current,
+	uint64_t *address, int handle_breakpoints, int debug_execution)
+{
+	struct armv8_common *armv8 = target_to_armv8(target);
+	struct arm *arm = &armv8->arm;
+	int retval;
+	uint64_t resume_pc;
+
+	LOG_DEBUG("%s", target_name(target));
+
+	if (!debug_execution)
+		target_free_all_working_areas(target);
+
+	/* current = 1: continue on current pc, otherwise continue at <address> */
+	resume_pc = buf_get_u64(arm->pc->value, 0, 64);
+	if (!current)
+		resume_pc = *address;
+	else
+		*address = resume_pc;
+
+	/* Make sure that the Armv7 gdb thumb fixups does not
+	 * kill the return address
+	 */
+	switch (arm->core_state) {
+		case ARM_STATE_ARM:
+			resume_pc &= 0xFFFFFFFC;
+			break;
+		case ARM_STATE_AARCH64:
+			resume_pc &= 0xFFFFFFFFFFFFFFFC;
+			break;
+		case ARM_STATE_THUMB:
+		case ARM_STATE_THUMB_EE:
+			/* When the return address is loaded into PC
+			 * bit 0 must be 1 to stay in Thumb state
+			 */
+			resume_pc |= 0x1;
+			break;
+		case ARM_STATE_JAZELLE:
+			LOG_ERROR("How do I resume into Jazelle state??");
+			return ERROR_FAIL;
+	}
+	LOG_DEBUG("resume pc = 0x%016" PRIx64, resume_pc);
+	buf_set_u64(arm->pc->value, 0, 64, resume_pc);
+	arm->pc->dirty = 1;
+	arm->pc->valid = 1;
+
+	/* called it now before restoring context because it uses cpu
+	 * register r0 for restoring system control register */
+	retval = aarch64_restore_system_control_reg(target);
+	if (retval == ERROR_OK)
+		retval = aarch64_restore_context(target, handle_breakpoints);
+
+	return retval;
+}
+
+/**
+ * prepare single target for restart
+ *
+ *
+ */
+static int aarch64_prepare_restart_one(struct target *target)
+{
+	struct armv8_common *armv8 = target_to_armv8(target);
+	int retval;
+	uint32_t dscr;
+	uint32_t tmp;
+
+	LOG_DEBUG("%s", target_name(target));
+
+	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
+	if (retval != ERROR_OK)
+		return retval;
+
+	if ((dscr & DSCR_ITE) == 0)
+		LOG_ERROR("DSCR.ITE must be set before leaving debug!");
+	if ((dscr & DSCR_ERR) != 0)
+		LOG_ERROR("DSCR.ERR must be cleared before leaving debug!");
+
+	/* acknowledge a pending CTI halt event */
+	retval = arm_cti_ack_events(armv8->cti, CTI_TRIG(HALT));
+	/*
+	 * open the CTI gate for channel 1 so that the restart events
+	 * get passed along to all PEs. Also close gate for channel 0
+	 * to isolate the PE from halt events.
+	 */
+	if (retval == ERROR_OK)
+		retval = arm_cti_ungate_channel(armv8->cti, 1);
+	if (retval == ERROR_OK)
+		retval = arm_cti_gate_channel(armv8->cti, 0);
+
+	/* make sure that DSCR.HDE is set */
+	if (retval == ERROR_OK) {
+		dscr |= DSCR_HDE;
+		retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUV8_DBG_DSCR, dscr);
+	}
+
+	/* clear sticky bits in PRSR, SDR is now 0 */
+	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUV8_DBG_PRSR, &tmp);
+
+	return retval;
+}
+
+static int aarch64_do_restart_one(struct target *target, enum restart_mode mode)
+{
+	struct armv8_common *armv8 = target_to_armv8(target);
+	int retval;
+
+	LOG_DEBUG("%s", target_name(target));
+
+	/* trigger an event on channel 1, generates a restart request to the PE */
+	retval = arm_cti_pulse_channel(armv8->cti, 1);
+	if (retval != ERROR_OK)
+		return retval;
+
+	if (mode == RESTART_SYNC) {
+		int64_t then = timeval_ms();
+		for (;;) {
+			int resumed;
+			/*
+			 * if PRSR.SDR is set now, the target did restart, even
+			 * if it's now already halted again (e.g. due to breakpoint)
+			 */
+			retval = aarch64_check_state_one(target,
+						PRSR_SDR, PRSR_SDR, &resumed, NULL);
+			if (retval != ERROR_OK || resumed)
+				break;
+
+			if (timeval_ms() > then + 1000) {
+				LOG_ERROR("%s: Timeout waiting for resume"PRIx32, target_name(target));
+				retval = ERROR_TARGET_TIMEOUT;
+				break;
+			}
+		}
+	}
+
+	if (retval != ERROR_OK)
+		return retval;
+
+	target->debug_reason = DBG_REASON_NOTHALTED;
+	target->state = TARGET_RUNNING;
+
+	return ERROR_OK;
+}
+
+static int aarch64_restart_one(struct target *target, enum restart_mode mode)
+{
+	int retval;
+
+	LOG_DEBUG("%s", target_name(target));
+
+	retval = aarch64_prepare_restart_one(target);
+	if (retval == ERROR_OK)
+		retval = aarch64_do_restart_one(target, mode);
+
+	return retval;
+}
+
+/*
+ * prepare all but the current target for restart
+ */
+static int aarch64_prep_restart_smp(struct target *target, int handle_breakpoints, struct target **p_first)
+{
+	int retval = ERROR_OK;
+	struct target_list *head;
+	struct target *first = NULL;
+	uint64_t address;
+
+	foreach_smp_target(head, target->head) {
+		struct target *curr = head->target;
+
+		/* skip calling target */
+		if (curr == target)
+			continue;
+		if (!target_was_examined(curr))
+			continue;
+		if (curr->state != TARGET_HALTED)
+			continue;
+
+		/*  resume at current address, not in step mode */
+		retval = aarch64_restore_one(curr, 1, &address, handle_breakpoints, 0);
+		if (retval == ERROR_OK)
+			retval = aarch64_prepare_restart_one(curr);
+		if (retval != ERROR_OK) {
+			LOG_ERROR("failed to restore target %s", target_name(curr));
+			break;
+		}
+		/* remember the first valid target in the group */
+		if (first == NULL)
+			first = curr;
+	}
+
+	if (p_first)
+		*p_first = first;
+
+	return retval;
+}
+
+
+static int aarch64_step_restart_smp(struct target *target)
+{
+	int retval = ERROR_OK;
+	struct target_list *head;
+	struct target *first = NULL;
+
+	LOG_DEBUG("%s", target_name(target));
+
+	retval = aarch64_prep_restart_smp(target, 0, &first);
+	if (retval != ERROR_OK)
+		return retval;
+
+	if (first != NULL)
+		retval = aarch64_do_restart_one(first, RESTART_LAZY);
+	if (retval != ERROR_OK) {
+		LOG_DEBUG("error restarting target %s", target_name(first));
+		return retval;
+	}
+
+	int64_t then = timeval_ms();
+	for (;;) {
+		struct target *curr = target;
+		bool all_resumed = true;
+
+		foreach_smp_target(head, target->head) {
+			uint32_t prsr;
+			int resumed;
+
+			curr = head->target;
+
+			if (curr == target)
+				continue;
+
+			retval = aarch64_check_state_one(curr,
+					PRSR_SDR, PRSR_SDR, &resumed, &prsr);
+			if (retval != ERROR_OK || (!resumed && (prsr & PRSR_HALT))) {
+				all_resumed = false;
+				break;
+			}
+
+			if (curr->state != TARGET_RUNNING) {
+				curr->state = TARGET_RUNNING;
+				curr->debug_reason = DBG_REASON_NOTHALTED;
+				target_call_event_callbacks(curr, TARGET_EVENT_RESUMED);
+			}
+		}
+
+		if (all_resumed)
+			break;
+
+		if (timeval_ms() > then + 1000) {
+			LOG_ERROR("%s: timeout waiting for target resume", __func__);
+			retval = ERROR_TARGET_TIMEOUT;
+			break;
+		}
+		/*
+		 * HACK: on Hi6220 there are 8 cores organized in 2 clusters
+		 * and it looks like the CTI's are not connected by a common
+		 * trigger matrix. It seems that we need to halt one core in each
+		 * cluster explicitly. So if we find that a core has not halted
+		 * yet, we trigger an explicit resume for the second cluster.
+		 */
+		retval = aarch64_do_restart_one(curr, RESTART_LAZY);
+		if (retval != ERROR_OK)
+			break;
+}
+
+	return retval;
+}
+
+static int aarch64_resume(struct target *target, int current,
+	target_addr_t address, int handle_breakpoints, int debug_execution)
+{
+	int retval = 0;
+	uint64_t addr = address;
+
+	if (target->state != TARGET_HALTED)
+		return ERROR_TARGET_NOT_HALTED;
+
+	/*
+	 * If this target is part of a SMP group, prepare the others
+	 * targets for resuming. This involves restoring the complete
+	 * target register context and setting up CTI gates to accept
+	 * resume events from the trigger matrix.
+	 */
+	if (target->smp) {
+		retval = aarch64_prep_restart_smp(target, handle_breakpoints, NULL);
+		if (retval != ERROR_OK)
+			return retval;
+	}
+
+	/* all targets prepared, restore and restart the current target */
+	retval = aarch64_restore_one(target, current, &addr, handle_breakpoints,
+				 debug_execution);
+	if (retval == ERROR_OK)
+		retval = aarch64_restart_one(target, RESTART_SYNC);
+	if (retval != ERROR_OK)
+		return retval;
+
+	if (target->smp) {
+		int64_t then = timeval_ms();
+		for (;;) {
+			struct target *curr = target;
+			struct target_list *head;
+			bool all_resumed = true;
+
+			foreach_smp_target(head, target->head) {
+				uint32_t prsr;
+				int resumed;
+
+				curr = head->target;
+				if (curr == target)
+					continue;
+				if (!target_was_examined(curr))
+					continue;
+
+				retval = aarch64_check_state_one(curr,
+						PRSR_SDR, PRSR_SDR, &resumed, &prsr);
+				if (retval != ERROR_OK || (!resumed && (prsr & PRSR_HALT))) {
+					all_resumed = false;
+					break;
+				}
+
+				if (curr->state != TARGET_RUNNING) {
+					curr->state = TARGET_RUNNING;
+					curr->debug_reason = DBG_REASON_NOTHALTED;
+					target_call_event_callbacks(curr, TARGET_EVENT_RESUMED);
+				}
+			}
+
+			if (all_resumed)
+				break;
+
+			if (timeval_ms() > then + 1000) {
+				LOG_ERROR("%s: timeout waiting for target %s to resume", __func__, target_name(curr));
+				retval = ERROR_TARGET_TIMEOUT;
+				break;
+			}
+
+			/*
+			 * HACK: on Hi6220 there are 8 cores organized in 2 clusters
+			 * and it looks like the CTI's are not connected by a common
+			 * trigger matrix. It seems that we need to halt one core in each
+			 * cluster explicitly. So if we find that a core has not halted
+			 * yet, we trigger an explicit resume for the second cluster.
+			 */
+			retval = aarch64_do_restart_one(curr, RESTART_LAZY);
+			if (retval != ERROR_OK)
+				break;
+		}
+	}
+
+	if (retval != ERROR_OK)
+		return retval;
+
+	target->debug_reason = DBG_REASON_NOTHALTED;
+
+	if (!debug_execution) {
+		target->state = TARGET_RUNNING;
+		target_call_event_callbacks(target, TARGET_EVENT_RESUMED);
+		LOG_DEBUG("target resumed at 0x%" PRIx64, addr);
+	} else {
+		target->state = TARGET_DEBUG_RUNNING;
+		target_call_event_callbacks(target, TARGET_EVENT_DEBUG_RESUMED);
+		LOG_DEBUG("target debug resumed at 0x%" PRIx64, addr);
+	}
+
+	return ERROR_OK;
+}
+
+static int aarch64_debug_entry(struct target *target)
+{
+	int retval = ERROR_OK;
+	struct armv8_common *armv8 = target_to_armv8(target);
+	struct arm_dpm *dpm = &armv8->dpm;
+	enum arm_state core_state;
+	uint32_t dscr;
+
+	/* make sure to clear all sticky errors */
+	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUV8_DBG_DRCR, DRCR_CSE);
+	if (retval == ERROR_OK)
+		retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
+	if (retval == ERROR_OK)
+		retval = arm_cti_ack_events(armv8->cti, CTI_TRIG(HALT));
+
+	if (retval != ERROR_OK)
+		return retval;
+
+	LOG_DEBUG("%s dscr = 0x%08" PRIx32, target_name(target), dscr);
+
+	dpm->dscr = dscr;
+	core_state = armv8_dpm_get_core_state(dpm);
+	armv8_select_opcodes(armv8, core_state == ARM_STATE_AARCH64);
+	armv8_select_reg_access(armv8, core_state == ARM_STATE_AARCH64);
+
+	/* close the CTI gate for all events */
+	if (retval == ERROR_OK)
+		retval = arm_cti_write_reg(armv8->cti, CTI_GATE, 0);
+	/* discard async exceptions */
+	if (retval == ERROR_OK)
+		retval = dpm->instr_cpsr_sync(dpm);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Examine debug reason */
+	armv8_dpm_report_dscr(dpm, dscr);
+
+	/* save address of instruction that triggered the watchpoint? */
+	if (target->debug_reason == DBG_REASON_WATCHPOINT) {
+		uint32_t tmp;
+		uint64_t wfar = 0;
+
+		retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUV8_DBG_WFAR1,
+				&tmp);
+		if (retval != ERROR_OK)
+			return retval;
+		wfar = tmp;
+		wfar = (wfar << 32);
+		retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUV8_DBG_WFAR0,
+				&tmp);
+		if (retval != ERROR_OK)
+			return retval;
+		wfar |= tmp;
+		armv8_dpm_report_wfar(&armv8->dpm, wfar);
+	}
+
+	retval = armv8_dpm_read_current_registers(&armv8->dpm);
+
+	if (retval == ERROR_OK && armv8->post_debug_entry)
+		retval = armv8->post_debug_entry(target);
+
+	return retval;
+}
+
+static int aarch64_post_debug_entry(struct target *target)
+{
+	struct aarch64_common *aarch64 = target_to_aarch64(target);
+	struct armv8_common *armv8 = &aarch64->armv8_common;
+	int retval;
+	enum arm_mode target_mode = ARM_MODE_ANY;
+	uint32_t instr;
+
+	switch (armv8->arm.core_mode) {
+	case ARMV8_64_EL0T:
+		target_mode = ARMV8_64_EL1H;
+		/* fall through */
+	case ARMV8_64_EL1T:
+	case ARMV8_64_EL1H:
+		instr = ARMV8_MRS(SYSTEM_SCTLR_EL1, 0);
+		break;
+	case ARMV8_64_EL2T:
+	case ARMV8_64_EL2H:
+		instr = ARMV8_MRS(SYSTEM_SCTLR_EL2, 0);
+		break;
+	case ARMV8_64_EL3H:
+	case ARMV8_64_EL3T:
+		instr = ARMV8_MRS(SYSTEM_SCTLR_EL3, 0);
+		break;
+
+	case ARM_MODE_SVC:
+	case ARM_MODE_ABT:
+	case ARM_MODE_FIQ:
+	case ARM_MODE_IRQ:
+		instr = ARMV4_5_MRC(15, 0, 0, 1, 0, 0);
+		break;
+
+	default:
+		LOG_INFO("cannot read system control register in this mode");
+		return ERROR_FAIL;
+	}
+
+	if (target_mode != ARM_MODE_ANY)
+		armv8_dpm_modeswitch(&armv8->dpm, target_mode);
+
+	retval = armv8->dpm.instr_read_data_r0(&armv8->dpm, instr, &aarch64->system_control_reg);
+	if (retval != ERROR_OK)
+		return retval;
+
+	if (target_mode != ARM_MODE_ANY)
+		armv8_dpm_modeswitch(&armv8->dpm, ARM_MODE_ANY);
+
+	LOG_DEBUG("System_register: %8.8" PRIx32, aarch64->system_control_reg);
+	aarch64->system_control_reg_curr = aarch64->system_control_reg;
+
+	if (armv8->armv8_mmu.armv8_cache.info == -1) {
+		armv8_identify_cache(armv8);
+		armv8_read_mpidr(armv8);
+	}
+
+	armv8->armv8_mmu.mmu_enabled =
+			(aarch64->system_control_reg & 0x1U) ? 1 : 0;
+	armv8->armv8_mmu.armv8_cache.d_u_cache_enabled =
+		(aarch64->system_control_reg & 0x4U) ? 1 : 0;
+	armv8->armv8_mmu.armv8_cache.i_cache_enabled =
+		(aarch64->system_control_reg & 0x1000U) ? 1 : 0;
+	return ERROR_OK;
+}
+
+/*
+ * single-step a target
+ */
+static int aarch64_step(struct target *target, int current, target_addr_t address,
+	int handle_breakpoints)
+{
+	struct armv8_common *armv8 = target_to_armv8(target);
+	int saved_retval = ERROR_OK;
+	int retval;
+	uint32_t edecr;
+
+	if (target->state != TARGET_HALTED) {
+		LOG_WARNING("target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUV8_DBG_EDECR, &edecr);
+	/* make sure EDECR.SS is not set when restoring the register */
+
+	if (retval == ERROR_OK) {
+		edecr &= ~0x4;
+		/* set EDECR.SS to enter hardware step mode */
+		retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUV8_DBG_EDECR, (edecr|0x4));
+	}
+	/* disable interrupts while stepping */
+	if (retval == ERROR_OK)
+		retval = aarch64_set_dscr_bits(target, 0x3 << 22, 0x3 << 22);
+	/* bail out if stepping setup has failed */
+	if (retval != ERROR_OK)
+		return retval;
+
+	if (target->smp && !handle_breakpoints) {
+		/*
+		 * isolate current target so that it doesn't get resumed
+		 * together with the others
+		 */
+		retval = arm_cti_gate_channel(armv8->cti, 1);
+		/* resume all other targets in the group */
+		if (retval == ERROR_OK)
+			retval = aarch64_step_restart_smp(target);
+		if (retval != ERROR_OK) {
+			LOG_ERROR("Failed to restart non-stepping targets in SMP group");
+			return retval;
+		}
+		LOG_DEBUG("Restarted all non-stepping targets in SMP group");
+	}
+
+	/* all other targets running, restore and restart the current target */
+	retval = aarch64_restore_one(target, current, &address, 0, 0);
+	if (retval == ERROR_OK)
+		retval = aarch64_restart_one(target, RESTART_LAZY);
+
+	if (retval != ERROR_OK)
+		return retval;
+
+	LOG_DEBUG("target step-resumed at 0x%" PRIx64, address);
+	if (!handle_breakpoints)
+		target_call_event_callbacks(target, TARGET_EVENT_RESUMED);
+
+	int64_t then = timeval_ms();
+	for (;;) {
+		int stepped;
+		uint32_t prsr;
+
+		retval = aarch64_check_state_one(target,
+					PRSR_SDR|PRSR_HALT, PRSR_SDR|PRSR_HALT, &stepped, &prsr);
+		if (retval != ERROR_OK || stepped)
+			break;
+
+		if (timeval_ms() > then + 1000) {
+			LOG_ERROR("timeout waiting for target %s halt after step",
+					target_name(target));
+			retval = ERROR_TARGET_TIMEOUT;
+			break;
+		}
+	}
+
+	if (retval == ERROR_TARGET_TIMEOUT)
+		saved_retval = retval;
+
+	/* restore EDECR */
+	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUV8_DBG_EDECR, edecr);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* restore interrupts */
+	retval = aarch64_set_dscr_bits(target, 0x3 << 22, 0);
+	if (retval != ERROR_OK)
+		return ERROR_OK;
+
+	if (saved_retval != ERROR_OK)
+		return saved_retval;
+
+	return aarch64_poll(target);
+}
+
+static int aarch64_restore_context(struct target *target, bool bpwp)
+{
+	struct armv8_common *armv8 = target_to_armv8(target);
+	struct arm *arm = &armv8->arm;
+
+	int retval;
+
+	LOG_DEBUG("%s", target_name(target));
+
+	if (armv8->pre_restore_context)
+		armv8->pre_restore_context(target);
+
+	retval = armv8_dpm_write_dirty_registers(&armv8->dpm, bpwp);
+	if (retval == ERROR_OK) {
+		/* registers are now invalid */
+		register_cache_invalidate(arm->core_cache);
+		register_cache_invalidate(arm->core_cache->next);
+	}
+
+	return retval;
+}
+
+/*
+ * Cortex-A8 Breakpoint and watchpoint functions
+ */
+
+/* Setup hardware Breakpoint Register Pair */
+static int aarch64_set_breakpoint(struct target *target,
+	struct breakpoint *breakpoint, uint8_t matchmode)
+{
+	int retval;
+	int brp_i = 0;
+	uint32_t control;
+	uint8_t byte_addr_select = 0x0F;
+	struct aarch64_common *aarch64 = target_to_aarch64(target);
+	struct armv8_common *armv8 = &aarch64->armv8_common;
+	struct aarch64_brp *brp_list = aarch64->brp_list;
+
+	if (breakpoint->set) {
+		LOG_WARNING("breakpoint already set");
+		return ERROR_OK;
+	}
+
+	if (breakpoint->type == BKPT_HARD) {
+		int64_t bpt_value;
+		while (brp_list[brp_i].used && (brp_i < aarch64->brp_num))
+			brp_i++;
+		if (brp_i >= aarch64->brp_num) {
+			LOG_ERROR("ERROR Can not find free Breakpoint Register Pair");
+			return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+		}
+		breakpoint->set = brp_i + 1;
+		if (breakpoint->length == 2)
+			byte_addr_select = (3 << (breakpoint->address & 0x02));
+		control = ((matchmode & 0x7) << 20)
+			| (1 << 13)
+			| (byte_addr_select << 5)
+			| (3 << 1) | 1;
+		brp_list[brp_i].used = 1;
+		brp_list[brp_i].value = breakpoint->address & 0xFFFFFFFFFFFFFFFC;
+		brp_list[brp_i].control = control;
+		bpt_value = brp_list[brp_i].value;
+
+		retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
+				+ CPUV8_DBG_BVR_BASE + 16 * brp_list[brp_i].BRPn,
+				(uint32_t)(bpt_value & 0xFFFFFFFF));
+		if (retval != ERROR_OK)
+			return retval;
+		retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
+				+ CPUV8_DBG_BVR_BASE + 4 + 16 * brp_list[brp_i].BRPn,
+				(uint32_t)(bpt_value >> 32));
+		if (retval != ERROR_OK)
+			return retval;
+
+		retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
+				+ CPUV8_DBG_BCR_BASE + 16 * brp_list[brp_i].BRPn,
+				brp_list[brp_i].control);
+		if (retval != ERROR_OK)
+			return retval;
+		LOG_DEBUG("brp %i control 0x%0" PRIx32 " value 0x%" TARGET_PRIxADDR, brp_i,
+			brp_list[brp_i].control,
+			brp_list[brp_i].value);
+
+	} else if (breakpoint->type == BKPT_SOFT) {
+		uint8_t code[4];
+
+		buf_set_u32(code, 0, 32, armv8_opcode(armv8, ARMV8_OPC_HLT));
+		retval = target_read_memory(target,
+				breakpoint->address & 0xFFFFFFFFFFFFFFFE,
+				breakpoint->length, 1,
+				breakpoint->orig_instr);
+		if (retval != ERROR_OK)
+			return retval;
+
+		armv8_cache_d_inner_flush_virt(armv8,
+				breakpoint->address & 0xFFFFFFFFFFFFFFFE,
+				breakpoint->length);
+
+		retval = target_write_memory(target,
+				breakpoint->address & 0xFFFFFFFFFFFFFFFE,
+				breakpoint->length, 1, code);
+		if (retval != ERROR_OK)
+			return retval;
+
+		armv8_cache_d_inner_flush_virt(armv8,
+				breakpoint->address & 0xFFFFFFFFFFFFFFFE,
+				breakpoint->length);
+
+		armv8_cache_i_inner_inval_virt(armv8,
+				breakpoint->address & 0xFFFFFFFFFFFFFFFE,
+				breakpoint->length);
+
+		breakpoint->set = 0x11;	/* Any nice value but 0 */
+	}
+
+	/* Ensure that halting debug mode is enable */
+	retval = aarch64_set_dscr_bits(target, DSCR_HDE, DSCR_HDE);
+	if (retval != ERROR_OK) {
+		LOG_DEBUG("Failed to set DSCR.HDE");
+		return retval;
+	}
+
+	return ERROR_OK;
+}
+
+static int aarch64_set_context_breakpoint(struct target *target,
+	struct breakpoint *breakpoint, uint8_t matchmode)
+{
+	int retval = ERROR_FAIL;
+	int brp_i = 0;
+	uint32_t control;
+	uint8_t byte_addr_select = 0x0F;
+	struct aarch64_common *aarch64 = target_to_aarch64(target);
+	struct armv8_common *armv8 = &aarch64->armv8_common;
+	struct aarch64_brp *brp_list = aarch64->brp_list;
+
+	if (breakpoint->set) {
+		LOG_WARNING("breakpoint already set");
+		return retval;
+	}
+	/*check available context BRPs*/
+	while ((brp_list[brp_i].used ||
+		(brp_list[brp_i].type != BRP_CONTEXT)) && (brp_i < aarch64->brp_num))
+		brp_i++;
+
+	if (brp_i >= aarch64->brp_num) {
+		LOG_ERROR("ERROR Can not find free Breakpoint Register Pair");
+		return ERROR_FAIL;
+	}
+
+	breakpoint->set = brp_i + 1;
+	control = ((matchmode & 0x7) << 20)
+		| (1 << 13)
+		| (byte_addr_select << 5)
+		| (3 << 1) | 1;
+	brp_list[brp_i].used = 1;
+	brp_list[brp_i].value = (breakpoint->asid);
+	brp_list[brp_i].control = control;
+	retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
+			+ CPUV8_DBG_BVR_BASE + 16 * brp_list[brp_i].BRPn,
+			brp_list[brp_i].value);
+	if (retval != ERROR_OK)
+		return retval;
+	retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
+			+ CPUV8_DBG_BCR_BASE + 16 * brp_list[brp_i].BRPn,
+			brp_list[brp_i].control);
+	if (retval != ERROR_OK)
+		return retval;
+	LOG_DEBUG("brp %i control 0x%0" PRIx32 " value 0x%" TARGET_PRIxADDR, brp_i,
+		brp_list[brp_i].control,
+		brp_list[brp_i].value);
+	return ERROR_OK;
+
+}
+
+static int aarch64_set_hybrid_breakpoint(struct target *target, struct breakpoint *breakpoint)
+{
+	int retval = ERROR_FAIL;
+	int brp_1 = 0;	/* holds the contextID pair */
+	int brp_2 = 0;	/* holds the IVA pair */
+	uint32_t control_CTX, control_IVA;
+	uint8_t CTX_byte_addr_select = 0x0F;
+	uint8_t IVA_byte_addr_select = 0x0F;
+	uint8_t CTX_machmode = 0x03;
+	uint8_t IVA_machmode = 0x01;
+	struct aarch64_common *aarch64 = target_to_aarch64(target);
+	struct armv8_common *armv8 = &aarch64->armv8_common;
+	struct aarch64_brp *brp_list = aarch64->brp_list;
+
+	if (breakpoint->set) {
+		LOG_WARNING("breakpoint already set");
+		return retval;
+	}
+	/*check available context BRPs*/
+	while ((brp_list[brp_1].used ||
+		(brp_list[brp_1].type != BRP_CONTEXT)) && (brp_1 < aarch64->brp_num))
+		brp_1++;
+
+	printf("brp(CTX) found num: %d\n", brp_1);
+	if (brp_1 >= aarch64->brp_num) {
+		LOG_ERROR("ERROR Can not find free Breakpoint Register Pair");
+		return ERROR_FAIL;
+	}
+
+	while ((brp_list[brp_2].used ||
+		(brp_list[brp_2].type != BRP_NORMAL)) && (brp_2 < aarch64->brp_num))
+		brp_2++;
+
+	printf("brp(IVA) found num: %d\n", brp_2);
+	if (brp_2 >= aarch64->brp_num) {
+		LOG_ERROR("ERROR Can not find free Breakpoint Register Pair");
+		return ERROR_FAIL;
+	}
+
+	breakpoint->set = brp_1 + 1;
+	breakpoint->linked_BRP = brp_2;
+	control_CTX = ((CTX_machmode & 0x7) << 20)
+		| (brp_2 << 16)
+		| (0 << 14)
+		| (CTX_byte_addr_select << 5)
+		| (3 << 1) | 1;
+	brp_list[brp_1].used = 1;
+	brp_list[brp_1].value = (breakpoint->asid);
+	brp_list[brp_1].control = control_CTX;
+	retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
+			+ CPUV8_DBG_BVR_BASE + 16 * brp_list[brp_1].BRPn,
+			brp_list[brp_1].value);
+	if (retval != ERROR_OK)
+		return retval;
+	retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
+			+ CPUV8_DBG_BCR_BASE + 16 * brp_list[brp_1].BRPn,
+			brp_list[brp_1].control);
+	if (retval != ERROR_OK)
+		return retval;
+
+	control_IVA = ((IVA_machmode & 0x7) << 20)
+		| (brp_1 << 16)
+		| (1 << 13)
+		| (IVA_byte_addr_select << 5)
+		| (3 << 1) | 1;
+	brp_list[brp_2].used = 1;
+	brp_list[brp_2].value = breakpoint->address & 0xFFFFFFFFFFFFFFFC;
+	brp_list[brp_2].control = control_IVA;
+	retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
+			+ CPUV8_DBG_BVR_BASE + 16 * brp_list[brp_2].BRPn,
+			brp_list[brp_2].value & 0xFFFFFFFF);
+	if (retval != ERROR_OK)
+		return retval;
+	retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
+			+ CPUV8_DBG_BVR_BASE + 4 + 16 * brp_list[brp_2].BRPn,
+			brp_list[brp_2].value >> 32);
+	if (retval != ERROR_OK)
+		return retval;
+	retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
+			+ CPUV8_DBG_BCR_BASE + 16 * brp_list[brp_2].BRPn,
+			brp_list[brp_2].control);
+	if (retval != ERROR_OK)
+		return retval;
+
+	return ERROR_OK;
+}
+
+static int aarch64_unset_breakpoint(struct target *target, struct breakpoint *breakpoint)
+{
+	int retval;
+	struct aarch64_common *aarch64 = target_to_aarch64(target);
+	struct armv8_common *armv8 = &aarch64->armv8_common;
+	struct aarch64_brp *brp_list = aarch64->brp_list;
+
+	if (!breakpoint->set) {
+		LOG_WARNING("breakpoint not set");
+		return ERROR_OK;
+	}
+
+	if (breakpoint->type == BKPT_HARD) {
+		if ((breakpoint->address != 0) && (breakpoint->asid != 0)) {
+			int brp_i = breakpoint->set - 1;
+			int brp_j = breakpoint->linked_BRP;
+			if ((brp_i < 0) || (brp_i >= aarch64->brp_num)) {
+				LOG_DEBUG("Invalid BRP number in breakpoint");
+				return ERROR_OK;
+			}
+			LOG_DEBUG("rbp %i control 0x%0" PRIx32 " value 0x%" TARGET_PRIxADDR, brp_i,
+				brp_list[brp_i].control, brp_list[brp_i].value);
+			brp_list[brp_i].used = 0;
+			brp_list[brp_i].value = 0;
+			brp_list[brp_i].control = 0;
+			retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
+					+ CPUV8_DBG_BCR_BASE + 16 * brp_list[brp_i].BRPn,
+					brp_list[brp_i].control);
+			if (retval != ERROR_OK)
+				return retval;
+			retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
+					+ CPUV8_DBG_BVR_BASE + 16 * brp_list[brp_i].BRPn,
+					(uint32_t)brp_list[brp_i].value);
+			if (retval != ERROR_OK)
+				return retval;
+			retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
+					+ CPUV8_DBG_BVR_BASE + 4 + 16 * brp_list[brp_i].BRPn,
+					(uint32_t)brp_list[brp_i].value);
+			if (retval != ERROR_OK)
+				return retval;
+			if ((brp_j < 0) || (brp_j >= aarch64->brp_num)) {
+				LOG_DEBUG("Invalid BRP number in breakpoint");
+				return ERROR_OK;
+			}
+			LOG_DEBUG("rbp %i control 0x%0" PRIx32 " value 0x%0" PRIx64, brp_j,
+				brp_list[brp_j].control, brp_list[brp_j].value);
+			brp_list[brp_j].used = 0;
+			brp_list[brp_j].value = 0;
+			brp_list[brp_j].control = 0;
+			retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
+					+ CPUV8_DBG_BCR_BASE + 16 * brp_list[brp_j].BRPn,
+					brp_list[brp_j].control);
+			if (retval != ERROR_OK)
+				return retval;
+			retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
+					+ CPUV8_DBG_BVR_BASE + 16 * brp_list[brp_j].BRPn,
+					(uint32_t)brp_list[brp_j].value);
+			if (retval != ERROR_OK)
+				return retval;
+			retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
+					+ CPUV8_DBG_BVR_BASE + 4 + 16 * brp_list[brp_j].BRPn,
+					(uint32_t)brp_list[brp_j].value);
+			if (retval != ERROR_OK)
+				return retval;
+
+			breakpoint->linked_BRP = 0;
+			breakpoint->set = 0;
+			return ERROR_OK;
+
+		} else {
+			int brp_i = breakpoint->set - 1;
+			if ((brp_i < 0) || (brp_i >= aarch64->brp_num)) {
+				LOG_DEBUG("Invalid BRP number in breakpoint");
+				return ERROR_OK;
+			}
+			LOG_DEBUG("rbp %i control 0x%0" PRIx32 " value 0x%0" PRIx64, brp_i,
+				brp_list[brp_i].control, brp_list[brp_i].value);
+			brp_list[brp_i].used = 0;
+			brp_list[brp_i].value = 0;
+			brp_list[brp_i].control = 0;
+			retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
+					+ CPUV8_DBG_BCR_BASE + 16 * brp_list[brp_i].BRPn,
+					brp_list[brp_i].control);
+			if (retval != ERROR_OK)
+				return retval;
+			retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
+					+ CPUV8_DBG_BVR_BASE + 16 * brp_list[brp_i].BRPn,
+					brp_list[brp_i].value);
+			if (retval != ERROR_OK)
+				return retval;
+
+			retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
+					+ CPUV8_DBG_BVR_BASE + 4 + 16 * brp_list[brp_i].BRPn,
+					(uint32_t)brp_list[brp_i].value);
+			if (retval != ERROR_OK)
+				return retval;
+			breakpoint->set = 0;
+			return ERROR_OK;
+		}
+	} else {
+		/* restore original instruction (kept in target endianness) */
+
+		armv8_cache_d_inner_flush_virt(armv8,
+				breakpoint->address & 0xFFFFFFFFFFFFFFFE,
+				breakpoint->length);
+
+		if (breakpoint->length == 4) {
+			retval = target_write_memory(target,
+					breakpoint->address & 0xFFFFFFFFFFFFFFFE,
+					4, 1, breakpoint->orig_instr);
+			if (retval != ERROR_OK)
+				return retval;
+		} else {
+			retval = target_write_memory(target,
+					breakpoint->address & 0xFFFFFFFFFFFFFFFE,
+					2, 1, breakpoint->orig_instr);
+			if (retval != ERROR_OK)
+				return retval;
+		}
+
+		armv8_cache_d_inner_flush_virt(armv8,
+				breakpoint->address & 0xFFFFFFFFFFFFFFFE,
+				breakpoint->length);
+
+		armv8_cache_i_inner_inval_virt(armv8,
+				breakpoint->address & 0xFFFFFFFFFFFFFFFE,
+				breakpoint->length);
+	}
+	breakpoint->set = 0;
+
+	return ERROR_OK;
+}
+
+static int aarch64_add_breakpoint(struct target *target,
+	struct breakpoint *breakpoint)
+{
+	struct aarch64_common *aarch64 = target_to_aarch64(target);
+
+	if ((breakpoint->type == BKPT_HARD) && (aarch64->brp_num_available < 1)) {
+		LOG_INFO("no hardware breakpoint available");
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	}
+
+	if (breakpoint->type == BKPT_HARD)
+		aarch64->brp_num_available--;
+
+	return aarch64_set_breakpoint(target, breakpoint, 0x00);	/* Exact match */
+}
+
+static int aarch64_add_context_breakpoint(struct target *target,
+	struct breakpoint *breakpoint)
+{
+	struct aarch64_common *aarch64 = target_to_aarch64(target);
+
+	if ((breakpoint->type == BKPT_HARD) && (aarch64->brp_num_available < 1)) {
+		LOG_INFO("no hardware breakpoint available");
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	}
+
+	if (breakpoint->type == BKPT_HARD)
+		aarch64->brp_num_available--;
+
+	return aarch64_set_context_breakpoint(target, breakpoint, 0x02);	/* asid match */
+}
+
+static int aarch64_add_hybrid_breakpoint(struct target *target,
+	struct breakpoint *breakpoint)
+{
+	struct aarch64_common *aarch64 = target_to_aarch64(target);
+
+	if ((breakpoint->type == BKPT_HARD) && (aarch64->brp_num_available < 1)) {
+		LOG_INFO("no hardware breakpoint available");
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	}
+
+	if (breakpoint->type == BKPT_HARD)
+		aarch64->brp_num_available--;
+
+	return aarch64_set_hybrid_breakpoint(target, breakpoint);	/* ??? */
+}
+
+
+static int aarch64_remove_breakpoint(struct target *target, struct breakpoint *breakpoint)
+{
+	struct aarch64_common *aarch64 = target_to_aarch64(target);
+
+#if 0
+/* It is perfectly possible to remove breakpoints while the target is running */
+	if (target->state != TARGET_HALTED) {
+		LOG_WARNING("target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+#endif
+
+	if (breakpoint->set) {
+		aarch64_unset_breakpoint(target, breakpoint);
+		if (breakpoint->type == BKPT_HARD)
+			aarch64->brp_num_available++;
+	}
+
+	return ERROR_OK;
+}
+
+/*
+ * Cortex-A8 Reset functions
+ */
+
+static int aarch64_assert_reset(struct target *target)
+{
+	struct armv8_common *armv8 = target_to_armv8(target);
+
+	LOG_DEBUG(" ");
+
+	/* FIXME when halt is requested, make it work somehow... */
+
+	/* Issue some kind of warm reset. */
+	if (target_has_event_action(target, TARGET_EVENT_RESET_ASSERT))
+		target_handle_event(target, TARGET_EVENT_RESET_ASSERT);
+	else if (jtag_get_reset_config() & RESET_HAS_SRST) {
+		/* REVISIT handle "pulls" cases, if there's
+		 * hardware that needs them to work.
+		 */
+		jtag_add_reset(0, 1);
+	} else {
+		LOG_ERROR("%s: how to reset?", target_name(target));
+		return ERROR_FAIL;
+	}
+
+	/* registers are now invalid */
+	if (target_was_examined(target)) {
+		register_cache_invalidate(armv8->arm.core_cache);
+		register_cache_invalidate(armv8->arm.core_cache->next);
+	}
+
+	target->state = TARGET_RESET;
+
+	return ERROR_OK;
+}
+
+static int aarch64_deassert_reset(struct target *target)
+{
+	int retval;
+
+	LOG_DEBUG(" ");
+
+	/* be certain SRST is off */
+	jtag_add_reset(0, 0);
+
+	if (!target_was_examined(target))
+		return ERROR_OK;
+
+	retval = aarch64_poll(target);
+	if (retval != ERROR_OK)
+		return retval;
+
+	if (target->reset_halt) {
+		if (target->state != TARGET_HALTED) {
+			LOG_WARNING("%s: ran after reset and before halt ...",
+				target_name(target));
+			retval = target_halt(target);
+			if (retval != ERROR_OK)
+				return retval;
+		}
+	}
+
+	return aarch64_init_debug_access(target);
+}
+
+static int aarch64_write_apb_ap_memory(struct target *target,
+	uint64_t address, uint32_t size,
+	uint32_t count, const uint8_t *buffer)
+{
+	/* write memory through APB-AP */
+	int retval = ERROR_COMMAND_SYNTAX_ERROR;
+	struct armv8_common *armv8 = target_to_armv8(target);
+	struct arm_dpm *dpm = &armv8->dpm;
+	struct arm *arm = &armv8->arm;
+	int total_bytes = count * size;
+	int total_u32;
+	int start_byte = address & 0x3;
+	int end_byte   = (address + total_bytes) & 0x3;
+	struct reg *reg;
+	uint32_t dscr;
+	uint8_t *tmp_buff = NULL;
+
+	if (target->state != TARGET_HALTED) {
+		LOG_WARNING("target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	total_u32 = DIV_ROUND_UP((address & 3) + total_bytes, 4);
+
+	/* Mark register R0 as dirty, as it will be used
+	 * for transferring the data.
+	 * It will be restored automatically when exiting
+	 * debug mode
+	 */
+	reg = armv8_reg_current(arm, 1);
+	reg->dirty = true;
+
+	reg = armv8_reg_current(arm, 0);
+	reg->dirty = true;
+
+	/* This algorithm comes from DDI0487A.g, chapter J9.1 */
+
+	/* The algorithm only copies 32 bit words, so the buffer
+	 * should be expanded to include the words at either end.
+	 * The first and last words will be read first to avoid
+	 * corruption if needed.
+	 */
+	tmp_buff = malloc(total_u32 * 4);
+
+	if ((start_byte != 0) && (total_u32 > 1)) {
+		/* First bytes not aligned - read the 32 bit word to avoid corrupting
+		 * the other bytes in the word.
+		 */
+		retval = aarch64_read_apb_ap_memory(target, (address & ~0x3), 4, 1, tmp_buff);
+		if (retval != ERROR_OK)
+			goto error_free_buff_w;
+	}
+
+	/* If end of write is not aligned, or the write is less than 4 bytes */
+	if ((end_byte != 0) ||
+		((total_u32 == 1) && (total_bytes != 4))) {
+
+		/* Read the last word to avoid corruption during 32 bit write */
+		int mem_offset = (total_u32-1) * 4;
+		retval = aarch64_read_apb_ap_memory(target, (address & ~0x3) + mem_offset, 4, 1, &tmp_buff[mem_offset]);
+		if (retval != ERROR_OK)
+			goto error_free_buff_w;
+	}
+
+	/* Copy the write buffer over the top of the temporary buffer */
+	memcpy(&tmp_buff[start_byte], buffer, total_bytes);
+
+	/* We now have a 32 bit aligned buffer that can be written */
+
+	/* Read DSCR */
+	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
+	if (retval != ERROR_OK)
+		goto error_free_buff_w;
+
+	/* Set Normal access mode  */
+	dscr = (dscr & ~DSCR_MA);
+	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUV8_DBG_DSCR, dscr);
+
+	if (arm->core_state == ARM_STATE_AARCH64) {
+		/* Write X0 with value 'address' using write procedure */
+		/* Step 1.a+b - Write the address for read access into DBGDTR_EL0 */
+		/* Step 1.c   - Copy value from DTR to R0 using instruction mrs DBGDTR_EL0, x0 */
+		retval = dpm->instr_write_data_dcc_64(dpm,
+				ARMV8_MRS(SYSTEM_DBG_DBGDTR_EL0, 0), address & ~0x3ULL);
+	} else {
+		/* Write R0 with value 'address' using write procedure */
+		/* Step 1.a+b - Write the address for read access into DBGDTRRX */
+		/* Step 1.c   - Copy value from DTR to R0 using instruction mrc DBGDTRTXint, r0 */
+		dpm->instr_write_data_dcc(dpm,
+				ARMV4_5_MRC(14, 0, 0, 0, 5, 0), address & ~0x3ULL);
+
+	}
+	/* Step 1.d   - Change DCC to memory mode */
+	dscr = dscr | DSCR_MA;
+	retval +=  mem_ap_write_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUV8_DBG_DSCR, dscr);
+	if (retval != ERROR_OK)
+		goto error_unset_dtr_w;
+
+
+	/* Step 2.a   - Do the write */
+	retval = mem_ap_write_buf_noincr(armv8->debug_ap,
+					tmp_buff, 4, total_u32, armv8->debug_base + CPUV8_DBG_DTRRX);
+	if (retval != ERROR_OK)
+		goto error_unset_dtr_w;
+
+	/* Step 3.a   - Switch DTR mode back to Normal mode */
+	dscr = (dscr & ~DSCR_MA);
+	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUV8_DBG_DSCR, dscr);
+	if (retval != ERROR_OK)
+		goto error_unset_dtr_w;
+
+	/* Check for sticky abort flags in the DSCR */
+	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
+	if (retval != ERROR_OK)
+		goto error_free_buff_w;
+
+	dpm->dscr = dscr;
+	if (dscr & (DSCR_ERR | DSCR_SYS_ERROR_PEND)) {
+		/* Abort occurred - clear it and exit */
+		LOG_ERROR("abort occurred - dscr = 0x%08" PRIx32, dscr);
+		armv8_dpm_handle_exception(dpm);
+		goto error_free_buff_w;
+	}
+
+	/* Done */
+	free(tmp_buff);
+	return ERROR_OK;
+
+error_unset_dtr_w:
+	/* Unset DTR mode */
+	mem_ap_read_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
+	dscr = (dscr & ~DSCR_MA);
+	mem_ap_write_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUV8_DBG_DSCR, dscr);
+error_free_buff_w:
+	LOG_ERROR("error");
+	free(tmp_buff);
+	return ERROR_FAIL;
+}
+
+static int aarch64_read_apb_ap_memory(struct target *target,
+	target_addr_t address, uint32_t size,
+	uint32_t count, uint8_t *buffer)
+{
+	/* read memory through APB-AP */
+	int retval = ERROR_COMMAND_SYNTAX_ERROR;
+	struct armv8_common *armv8 = target_to_armv8(target);
+	struct arm_dpm *dpm = &armv8->dpm;
+	struct arm *arm = &armv8->arm;
+	int total_bytes = count * size;
+	int total_u32;
+	int start_byte = address & 0x3;
+	int end_byte   = (address + total_bytes) & 0x3;
+	struct reg *reg;
+	uint32_t dscr;
+	uint8_t *tmp_buff = NULL;
+	uint8_t *u8buf_ptr;
+	uint32_t value;
+
+	if (target->state != TARGET_HALTED) {
+		LOG_WARNING("target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	total_u32 = DIV_ROUND_UP((address & 3) + total_bytes, 4);
+	/* Mark register X0, X1 as dirty, as it will be used
+	 * for transferring the data.
+	 * It will be restored automatically when exiting
+	 * debug mode
+	 */
+	reg = armv8_reg_current(arm, 1);
+	reg->dirty = true;
+
+	reg = armv8_reg_current(arm, 0);
+	reg->dirty = true;
+
+	/* Read DSCR */
+	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
+
+	/* This algorithm comes from DDI0487A.g, chapter J9.1 */
+
+	/* Set Normal access mode  */
+	dscr = (dscr & ~DSCR_MA);
+	retval +=  mem_ap_write_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUV8_DBG_DSCR, dscr);
+
+	if (arm->core_state == ARM_STATE_AARCH64) {
+		/* Write X0 with value 'address' using write procedure */
+		/* Step 1.a+b - Write the address for read access into DBGDTR_EL0 */
+		/* Step 1.c   - Copy value from DTR to R0 using instruction mrs DBGDTR_EL0, x0 */
+		retval += dpm->instr_write_data_dcc_64(dpm,
+				ARMV8_MRS(SYSTEM_DBG_DBGDTR_EL0, 0), address & ~0x3ULL);
+		/* Step 1.d - Dummy operation to ensure EDSCR.Txfull == 1 */
+		retval += dpm->instr_execute(dpm, ARMV8_MSR_GP(SYSTEM_DBG_DBGDTR_EL0, 0));
+		/* Step 1.e - Change DCC to memory mode */
+		dscr = dscr | DSCR_MA;
+		retval +=  mem_ap_write_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUV8_DBG_DSCR, dscr);
+		/* Step 1.f - read DBGDTRTX and discard the value */
+		retval += mem_ap_read_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUV8_DBG_DTRTX, &value);
+	} else {
+		/* Write R0 with value 'address' using write procedure */
+		/* Step 1.a+b - Write the address for read access into DBGDTRRXint */
+		/* Step 1.c   - Copy value from DTR to R0 using instruction mrc DBGDTRTXint, r0 */
+		retval += dpm->instr_write_data_dcc(dpm,
+				ARMV4_5_MRC(14, 0, 0, 0, 5, 0), address & ~0x3ULL);
+		/* Step 1.d - Dummy operation to ensure EDSCR.Txfull == 1 */
+		retval += dpm->instr_execute(dpm, ARMV4_5_MCR(14, 0, 0, 0, 5, 0));
+		/* Step 1.e - Change DCC to memory mode */
+		dscr = dscr | DSCR_MA;
+		retval +=  mem_ap_write_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUV8_DBG_DSCR, dscr);
+		/* Step 1.f - read DBGDTRTX and discard the value */
+		retval += mem_ap_read_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUV8_DBG_DTRTX, &value);
+
+	}
+	if (retval != ERROR_OK)
+		goto error_unset_dtr_r;
+
+	/* Optimize the read as much as we can, either way we read in a single pass  */
+	if ((start_byte) || (end_byte)) {
+		/* The algorithm only copies 32 bit words, so the buffer
+		 * should be expanded to include the words at either end.
+		 * The first and last words will be read into a temp buffer
+		 * to avoid corruption
+		 */
+		tmp_buff = malloc(total_u32 * 4);
+		if (!tmp_buff)
+			goto error_unset_dtr_r;
+
+		/* use the tmp buffer to read the entire data */
+		u8buf_ptr = tmp_buff;
+	} else
+		/* address and read length are aligned so read directly into the passed buffer */
+		u8buf_ptr = buffer;
+
+	/* Read the data - Each read of the DTRTX register causes the instruction to be reissued
+	 * Abort flags are sticky, so can be read at end of transactions
+	 *
+	 * This data is read in aligned to 32 bit boundary.
+	 */
+
+	/* Step 2.a - Loop n-1 times, each read of DBGDTRTX reads the data from [X0] and
+	 * increments X0 by 4. */
+	retval = mem_ap_read_buf_noincr(armv8->debug_ap, u8buf_ptr, 4, total_u32-1,
+									armv8->debug_base + CPUV8_DBG_DTRTX);
+	if (retval != ERROR_OK)
+			goto error_unset_dtr_r;
+
+	/* Step 3.a - set DTR access mode back to Normal mode	*/
+	dscr = (dscr & ~DSCR_MA);
+	retval =  mem_ap_write_atomic_u32(armv8->debug_ap,
+					armv8->debug_base + CPUV8_DBG_DSCR, dscr);
+	if (retval != ERROR_OK)
+		goto error_free_buff_r;
+
+	/* Step 3.b - read DBGDTRTX for the final value */
+	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUV8_DBG_DTRTX, &value);
+	memcpy(u8buf_ptr + (total_u32-1) * 4, &value, 4);
+
+	/* Check for sticky abort flags in the DSCR */
+	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
+	if (retval != ERROR_OK)
+		goto error_free_buff_r;
+
+	dpm->dscr = dscr;
+
+	if (dscr & (DSCR_ERR | DSCR_SYS_ERROR_PEND)) {
+		/* Abort occurred - clear it and exit */
+		LOG_ERROR("abort occurred - dscr = 0x%08" PRIx32, dscr);
+		armv8_dpm_handle_exception(dpm);
+		goto error_free_buff_r;
+	}
+
+	/* check if we need to copy aligned data by applying any shift necessary */
+	if (tmp_buff) {
+		memcpy(buffer, tmp_buff + start_byte, total_bytes);
+		free(tmp_buff);
+	}
+
+	/* Done */
+	return ERROR_OK;
+
+error_unset_dtr_r:
+	/* Unset DTR mode */
+	mem_ap_read_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
+	dscr = (dscr & ~DSCR_MA);
+	mem_ap_write_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUV8_DBG_DSCR, dscr);
+error_free_buff_r:
+	LOG_ERROR("error");
+	free(tmp_buff);
+	return ERROR_FAIL;
+}
+
+static int aarch64_read_phys_memory(struct target *target,
+	target_addr_t address, uint32_t size,
+	uint32_t count, uint8_t *buffer)
+{
+	int retval = ERROR_COMMAND_SYNTAX_ERROR;
+
+	if (count && buffer) {
+		/* read memory through APB-AP */
+		retval = aarch64_mmu_modify(target, 0);
+		if (retval != ERROR_OK)
+			return retval;
+		retval = aarch64_read_apb_ap_memory(target, address, size, count, buffer);
+	}
+	return retval;
+}
+
+static int aarch64_read_memory(struct target *target, target_addr_t address,
+	uint32_t size, uint32_t count, uint8_t *buffer)
+{
+	int mmu_enabled = 0;
+	int retval;
+
+	/* determine if MMU was enabled on target stop */
+	retval = aarch64_mmu(target, &mmu_enabled);
+	if (retval != ERROR_OK)
+		return retval;
+
+	if (mmu_enabled) {
+		/* enable MMU as we could have disabled it for phys access */
+		retval = aarch64_mmu_modify(target, 1);
+		if (retval != ERROR_OK)
+			return retval;
+	}
+	return aarch64_read_apb_ap_memory(target, address, size, count, buffer);
+}
+
+static int aarch64_write_phys_memory(struct target *target,
+	target_addr_t address, uint32_t size,
+	uint32_t count, const uint8_t *buffer)
+{
+	int retval = ERROR_COMMAND_SYNTAX_ERROR;
+
+	if (count && buffer) {
+		/* write memory through APB-AP */
+		retval = aarch64_mmu_modify(target, 0);
+		if (retval != ERROR_OK)
+			return retval;
+		return aarch64_write_apb_ap_memory(target, address, size, count, buffer);
+	}
+
+	return retval;
+}
+
+static int aarch64_write_memory(struct target *target, target_addr_t address,
+	uint32_t size, uint32_t count, const uint8_t *buffer)
+{
+	int mmu_enabled = 0;
+	int retval;
+
+	/* determine if MMU was enabled on target stop */
+	retval = aarch64_mmu(target, &mmu_enabled);
+	if (retval != ERROR_OK)
+		return retval;
+
+	if (mmu_enabled) {
+		/* enable MMU as we could have disabled it for phys access */
+		retval = aarch64_mmu_modify(target, 1);
+		if (retval != ERROR_OK)
+			return retval;
+	}
+	return aarch64_write_apb_ap_memory(target, address, size, count, buffer);
+}
+
+static int aarch64_handle_target_request(void *priv)
+{
+	struct target *target = priv;
+	struct armv8_common *armv8 = target_to_armv8(target);
+	int retval;
+
+	if (!target_was_examined(target))
+		return ERROR_OK;
+	if (!target->dbg_msg_enabled)
+		return ERROR_OK;
+
+	if (target->state == TARGET_RUNNING) {
+		uint32_t request;
+		uint32_t dscr;
+		retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
+
+		/* check if we have data */
+		while ((dscr & DSCR_DTR_TX_FULL) && (retval == ERROR_OK)) {
+			retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+					armv8->debug_base + CPUV8_DBG_DTRTX, &request);
+			if (retval == ERROR_OK) {
+				target_request(target, request);
+				retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+						armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
+			}
+		}
+	}
+
+	return ERROR_OK;
+}
+
+static int aarch64_examine_first(struct target *target)
+{
+	struct aarch64_common *aarch64 = target_to_aarch64(target);
+	struct armv8_common *armv8 = &aarch64->armv8_common;
+	struct adiv5_dap *swjdp = armv8->arm.dap;
+	uint32_t cti_base;
+	int i;
+	int retval = ERROR_OK;
+	uint64_t debug, ttypr;
+	uint32_t cpuid;
+	uint32_t tmp0, tmp1;
+	debug = ttypr = cpuid = 0;
+
+	retval = dap_dp_init(swjdp);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Search for the APB-AB - it is needed for access to debug registers */
+	retval = dap_find_ap(swjdp, AP_TYPE_APB_AP, &armv8->debug_ap);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("Could not find APB-AP for debug access");
+		return retval;
+	}
+
+	retval = mem_ap_init(armv8->debug_ap);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("Could not initialize the APB-AP");
+		return retval;
+	}
+
+	armv8->debug_ap->memaccess_tck = 10;
+
+	if (!target->dbgbase_set) {
+		uint32_t dbgbase;
+		/* Get ROM Table base */
+		uint32_t apid;
+		int32_t coreidx = target->coreid;
+		retval = dap_get_debugbase(armv8->debug_ap, &dbgbase, &apid);
+		if (retval != ERROR_OK)
+			return retval;
+		/* Lookup 0x15 -- Processor DAP */
+		retval = dap_lookup_cs_component(armv8->debug_ap, dbgbase, 0x15,
+				&armv8->debug_base, &coreidx);
+		if (retval != ERROR_OK)
+			return retval;
+		LOG_DEBUG("Detected core %" PRId32 " dbgbase: %08" PRIx32
+				" apid: %08" PRIx32, coreidx, armv8->debug_base, apid);
+	} else
+		armv8->debug_base = target->dbgbase;
+
+	uint32_t prsr;
+	int64_t then = timeval_ms();
+	do {
+		retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUV8_DBG_PRSR, &prsr);
+		if (retval == ERROR_OK) {
+			retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+					armv8->debug_base + CPUV8_DBG_PRCR, PRCR_COREPURQ|PRCR_CORENPDRQ);
+			if (retval != ERROR_OK) {
+				LOG_DEBUG("write to PRCR failed");
+				break;
+			}
+		}
+
+		if (timeval_ms() > then + 1000) {
+			retval = ERROR_TARGET_TIMEOUT;
+			break;
+		}
+
+	} while ((prsr & PRSR_PU) == 0);
+
+	if (retval != ERROR_OK) {
+		LOG_ERROR("target %s: failed to set power state of the core.", target_name(target));
+		return retval;
+	}
+
+	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUV8_DBG_OSLAR, 0);
+	if (retval != ERROR_OK) {
+		LOG_DEBUG("Examine %s failed", "oslock");
+		return retval;
+	}
+
+	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUV8_DBG_MAINID0, &cpuid);
+	if (retval != ERROR_OK) {
+		LOG_DEBUG("Examine %s failed", "CPUID");
+		return retval;
+	}
+
+	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUV8_DBG_MEMFEATURE0, &tmp0);
+	retval += mem_ap_read_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUV8_DBG_MEMFEATURE0 + 4, &tmp1);
+	if (retval != ERROR_OK) {
+		LOG_DEBUG("Examine %s failed", "Memory Model Type");
+		return retval;
+	}
+	ttypr |= tmp1;
+	ttypr = (ttypr << 32) | tmp0;
+
+	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUV8_DBG_DBGFEATURE0, &tmp0);
+	retval += mem_ap_read_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUV8_DBG_DBGFEATURE0 + 4, &tmp1);
+	if (retval != ERROR_OK) {
+		LOG_DEBUG("Examine %s failed", "ID_AA64DFR0_EL1");
+		return retval;
+	}
+	debug |= tmp1;
+	debug = (debug << 32) | tmp0;
+
+	LOG_DEBUG("cpuid = 0x%08" PRIx32, cpuid);
+	LOG_DEBUG("ttypr = 0x%08" PRIx64, ttypr);
+	LOG_DEBUG("debug = 0x%08" PRIx64, debug);
+
+	if (target->ctibase == 0) {
+		/* assume a v8 rom table layout */
+		cti_base = armv8->debug_base + 0x10000;
+		LOG_INFO("Target ctibase is not set, assuming 0x%0" PRIx32, cti_base);
+	} else
+		cti_base = target->ctibase;
+
+	armv8->cti = arm_cti_create(armv8->debug_ap, cti_base);
+	if (armv8->cti == NULL)
+		return ERROR_FAIL;
+
+	retval = aarch64_dpm_setup(aarch64, debug);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Setup Breakpoint Register Pairs */
+	aarch64->brp_num = (uint32_t)((debug >> 12) & 0x0F) + 1;
+	aarch64->brp_num_context = (uint32_t)((debug >> 28) & 0x0F) + 1;
+	aarch64->brp_num_available = aarch64->brp_num;
+	aarch64->brp_list = calloc(aarch64->brp_num, sizeof(struct aarch64_brp));
+	for (i = 0; i < aarch64->brp_num; i++) {
+		aarch64->brp_list[i].used = 0;
+		if (i < (aarch64->brp_num-aarch64->brp_num_context))
+			aarch64->brp_list[i].type = BRP_NORMAL;
+		else
+			aarch64->brp_list[i].type = BRP_CONTEXT;
+		aarch64->brp_list[i].value = 0;
+		aarch64->brp_list[i].control = 0;
+		aarch64->brp_list[i].BRPn = i;
+	}
+
+	LOG_DEBUG("Configured %i hw breakpoints", aarch64->brp_num);
+
+	target->state = TARGET_RUNNING;
+	target->debug_reason = DBG_REASON_NOTHALTED;
+
+	target_set_examined(target);
+	return ERROR_OK;
+}
+
+static int aarch64_examine(struct target *target)
+{
+	int retval = ERROR_OK;
+
+	/* don't re-probe hardware after each reset */
+	if (!target_was_examined(target))
+		retval = aarch64_examine_first(target);
+
+	/* Configure core debug access */
+	if (retval == ERROR_OK)
+		retval = aarch64_init_debug_access(target);
+
+	return retval;
+}
+
+/*
+ *	Cortex-A8 target creation and initialization
+ */
+
+static int aarch64_init_target(struct command_context *cmd_ctx,
+	struct target *target)
+{
+	/* examine_first() does a bunch of this */
+	return ERROR_OK;
+}
+
+static int aarch64_init_arch_info(struct target *target,
+	struct aarch64_common *aarch64, struct jtag_tap *tap)
+{
+	struct armv8_common *armv8 = &aarch64->armv8_common;
+
+	/* Setup struct aarch64_common */
+	aarch64->common_magic = AARCH64_COMMON_MAGIC;
+	/*  tap has no dap initialized */
+	if (!tap->dap) {
+		tap->dap = dap_init();
+		tap->dap->tap = tap;
+	}
+	armv8->arm.dap = tap->dap;
+
+	/* register arch-specific functions */
+	armv8->examine_debug_reason = NULL;
+	armv8->post_debug_entry = aarch64_post_debug_entry;
+	armv8->pre_restore_context = NULL;
+	armv8->armv8_mmu.read_physical_memory = aarch64_read_phys_memory;
+
+	armv8_init_arch_info(target, armv8);
+	target_register_timer_callback(aarch64_handle_target_request, 1, 1, target);
+
+	return ERROR_OK;
+}
+
+static int aarch64_target_create(struct target *target, Jim_Interp *interp)
+{
+	struct aarch64_common *aarch64 = calloc(1, sizeof(struct aarch64_common));
+
+	return aarch64_init_arch_info(target, aarch64, target->tap);
+}
+
+static int aarch64_mmu(struct target *target, int *enabled)
+{
+	if (target->state != TARGET_HALTED) {
+		LOG_ERROR("%s: target %s not halted", __func__, target_name(target));
+		return ERROR_TARGET_INVALID;
+	}
+
+	*enabled = target_to_aarch64(target)->armv8_common.armv8_mmu.mmu_enabled;
+	return ERROR_OK;
+}
+
+static int aarch64_virt2phys(struct target *target, target_addr_t virt,
+			     target_addr_t *phys)
+{
+	return armv8_mmu_translate_va_pa(target, virt, phys, 1);
+}
+
+COMMAND_HANDLER(aarch64_handle_cache_info_command)
+{
+	struct target *target = get_current_target(CMD_CTX);
+	struct armv8_common *armv8 = target_to_armv8(target);
+
+	return armv8_handle_cache_info_command(CMD_CTX,
+			&armv8->armv8_mmu.armv8_cache);
+}
+
+
+COMMAND_HANDLER(aarch64_handle_dbginit_command)
+{
+	struct target *target = get_current_target(CMD_CTX);
+	if (!target_was_examined(target)) {
+		LOG_ERROR("target not examined yet");
+		return ERROR_FAIL;
+	}
+
+	return aarch64_init_debug_access(target);
+}
+COMMAND_HANDLER(aarch64_handle_smp_off_command)
+{
+	struct target *target = get_current_target(CMD_CTX);
+	/* check target is an smp target */
+	struct target_list *head;
+	struct target *curr;
+	head = target->head;
+	target->smp = 0;
+	if (head != (struct target_list *)NULL) {
+		while (head != (struct target_list *)NULL) {
+			curr = head->target;
+			curr->smp = 0;
+			head = head->next;
+		}
+		/*  fixes the target display to the debugger */
+		target->gdb_service->target = target;
+	}
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(aarch64_handle_smp_on_command)
+{
+	struct target *target = get_current_target(CMD_CTX);
+	struct target_list *head;
+	struct target *curr;
+	head = target->head;
+	if (head != (struct target_list *)NULL) {
+		target->smp = 1;
+		while (head != (struct target_list *)NULL) {
+			curr = head->target;
+			curr->smp = 1;
+			head = head->next;
+		}
+	}
+	return ERROR_OK;
+}
+
+static const struct command_registration aarch64_exec_command_handlers[] = {
+	{
+		.name = "cache_info",
+		.handler = aarch64_handle_cache_info_command,
+		.mode = COMMAND_EXEC,
+		.help = "display information about target caches",
+		.usage = "",
+	},
+	{
+		.name = "dbginit",
+		.handler = aarch64_handle_dbginit_command,
+		.mode = COMMAND_EXEC,
+		.help = "Initialize core debug",
+		.usage = "",
+	},
+	{	.name = "smp_off",
+		.handler = aarch64_handle_smp_off_command,
+		.mode = COMMAND_EXEC,
+		.help = "Stop smp handling",
+		.usage = "",
+	},
+	{
+		.name = "smp_on",
+		.handler = aarch64_handle_smp_on_command,
+		.mode = COMMAND_EXEC,
+		.help = "Restart smp handling",
+		.usage = "",
+	},
+
+	COMMAND_REGISTRATION_DONE
+};
+static const struct command_registration aarch64_command_handlers[] = {
+	{
+		.chain = armv8_command_handlers,
+	},
+	{
+		.name = "aarch64",
+		.mode = COMMAND_ANY,
+		.help = "Aarch64 command group",
+		.usage = "",
+		.chain = aarch64_exec_command_handlers,
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+struct target_type aarch64_target = {
+	.name = "aarch64",
+
+	.poll = aarch64_poll,
+	.arch_state = armv8_arch_state,
+
+	.halt = aarch64_halt,
+	.resume = aarch64_resume,
+	.step = aarch64_step,
+
+	.assert_reset = aarch64_assert_reset,
+	.deassert_reset = aarch64_deassert_reset,
+
+	/* REVISIT allow exporting VFP3 registers ... */
+	.get_gdb_reg_list = armv8_get_gdb_reg_list,
+
+	.read_memory = aarch64_read_memory,
+	.write_memory = aarch64_write_memory,
+
+	.add_breakpoint = aarch64_add_breakpoint,
+	.add_context_breakpoint = aarch64_add_context_breakpoint,
+	.add_hybrid_breakpoint = aarch64_add_hybrid_breakpoint,
+	.remove_breakpoint = aarch64_remove_breakpoint,
+	.add_watchpoint = NULL,
+	.remove_watchpoint = NULL,
+
+	.commands = aarch64_command_handlers,
+	.target_create = aarch64_target_create,
+	.init_target = aarch64_init_target,
+	.examine = aarch64_examine,
+
+	.read_phys_memory = aarch64_read_phys_memory,
+	.write_phys_memory = aarch64_write_phys_memory,
+	.mmu = aarch64_mmu,
+	.virt2phys = aarch64_virt2phys,
+};
diff --git a/src/target/aarch64.h b/src/target/aarch64.h
new file mode 100644
index 0000000..c9ec02d
--- /dev/null
+++ b/src/target/aarch64.h
@@ -0,0 +1,69 @@
+/***************************************************************************
+ *   Copyright (C) 2015 by David Ung                                       *
+ *                                                                         *
+ *   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, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ ***************************************************************************/
+
+#ifndef OPENOCD_TARGET_AARCH64_H
+#define OPENOCD_TARGET_AARCH64_H
+
+#include "armv8.h"
+
+#define AARCH64_COMMON_MAGIC 0x411fc082
+
+#define CPUDBG_CPUID	0xD00
+#define CPUDBG_CTYPR	0xD04
+#define CPUDBG_TTYPR	0xD0C
+#define ID_AA64PFR0_EL1	0xD20
+#define ID_AA64DFR0_EL1	0xD28
+#define CPUDBG_LOCKACCESS 0xFB0
+#define CPUDBG_LOCKSTATUS 0xFB4
+
+#define BRP_NORMAL 0
+#define BRP_CONTEXT 1
+
+#define AARCH64_PADDRDBG_CPU_SHIFT 13
+
+struct aarch64_brp {
+	int used;
+	int type;
+	target_addr_t value;
+	uint32_t control;
+	uint8_t BRPn;
+};
+
+struct aarch64_common {
+	int common_magic;
+
+	/* Context information */
+	uint32_t system_control_reg;
+	uint32_t system_control_reg_curr;
+
+	/* Breakpoint register pairs */
+	int brp_num_context;
+	int brp_num;
+	int brp_num_available;
+	struct aarch64_brp *brp_list;
+
+	struct armv8_common armv8_common;
+};
+
+static inline struct aarch64_common *
+target_to_aarch64(struct target *target)
+{
+	return container_of(target->arch_info, struct aarch64_common, armv8_common.arm);
+}
+
+#endif /* OPENOCD_TARGET_AARCH64_H */
diff --git a/src/target/adi_v5_jtag.c b/src/target/adi_v5_jtag.c
index 2717c9e..c7dc4f7 100644
--- a/src/target/adi_v5_jtag.c
+++ b/src/target/adi_v5_jtag.c
@@ -574,8 +574,6 @@ static int jtagdp_transaction_endcheck(struct adiv5_dap *dap)
 		if ((ctrlstat & (CDBGPWRUPREQ | CDBGPWRUPACK | CSYSPWRUPREQ | CSYSPWRUPACK)) !=
 						(CDBGPWRUPREQ | CDBGPWRUPACK | CSYSPWRUPREQ | CSYSPWRUPACK)) {
 			LOG_ERROR("Debug regions are unpowered, an unexpected reset might have happened");
-			retval = ERROR_JTAG_DEVICE_ERROR;
-			goto done;
 		}
 
 		if (ctrlstat & SSTICKYERR)
@@ -590,10 +588,7 @@ static int jtagdp_transaction_endcheck(struct adiv5_dap *dap)
 		if (retval != ERROR_OK)
 			goto done;
 
-		if (ctrlstat & SSTICKYERR) {
-			retval = ERROR_JTAG_DEVICE_ERROR;
-			goto done;
-		}
+		retval = ERROR_JTAG_DEVICE_ERROR;
 	}
 
  done:
diff --git a/src/target/algorithm.h b/src/target/algorithm.h
index d216a82..8894241 100644
--- a/src/target/algorithm.h
+++ b/src/target/algorithm.h
@@ -26,7 +26,7 @@ enum param_direction {
 };
 
 struct mem_param {
-	uint32_t address;
+	target_addr_t address;
 	uint32_t size;
 	uint8_t *value;
 	enum param_direction direction;
diff --git a/src/target/arm.h b/src/target/arm.h
index 163db23..d63ead2 100644
--- a/src/target/arm.h
+++ b/src/target/arm.h
@@ -66,6 +66,15 @@ enum arm_mode {
 	ARM_MODE_USER_THREAD = 1,
 	ARM_MODE_HANDLER = 2,
 
+	/* shift left 4 bits for armv8 64 */
+	ARMV8_64_EL0T = 0x0F,
+	ARMV8_64_EL1T = 0x4F,
+	ARMV8_64_EL1H = 0x5F,
+	ARMV8_64_EL2T = 0x8F,
+	ARMV8_64_EL2H = 0x9F,
+	ARMV8_64_EL3T = 0xCF,
+	ARMV8_64_EL3H = 0xDF,
+
 	ARM_MODE_ANY = -1
 };
 
@@ -78,6 +87,7 @@ enum arm_state {
 	ARM_STATE_THUMB,
 	ARM_STATE_JAZELLE,
 	ARM_STATE_THUMB_EE,
+	ARM_STATE_AARCH64,
 };
 
 #define ARM_COMMON_MAGIC 0x0A450A45
@@ -130,6 +140,18 @@ struct arm {
 	/** Flag reporting whether semihosting is active. */
 	bool is_semihosting;
 
+	/** Flag reporting whether semihosting fileio is active. */
+	bool is_semihosting_fileio;
+
+	/** Flag reporting whether semihosting fileio operation is active. */
+	bool semihosting_hit_fileio;
+
+	/** Current semihosting operation. */
+	int semihosting_op;
+
+	/** Current semihosting result. */
+	int semihosting_result;
+
 	/** Value to be returned by semihosting SYS_ERRNO request. */
 	int semihosting_errno;
 
@@ -201,10 +223,11 @@ struct arm_reg {
 	enum arm_mode mode;
 	struct target *target;
 	struct arm *arm;
-	uint8_t value[4];
+	uint8_t value[8];
 };
 
 struct reg_cache *arm_build_reg_cache(struct target *target, struct arm *arm);
+struct reg_cache *armv8_build_reg_cache(struct target *target);
 
 extern const struct command_registration arm_command_handlers[];
 
@@ -212,6 +235,9 @@ int arm_arch_state(struct target *target);
 int arm_get_gdb_reg_list(struct target *target,
 		struct reg **reg_list[], int *reg_list_size,
 		enum target_register_class reg_class);
+int armv8_get_gdb_reg_list(struct target *target,
+		struct reg **reg_list[], int *reg_list_size,
+		enum target_register_class reg_class);
 
 int arm_init_arch_info(struct target *target, struct arm *arm);
 
@@ -219,7 +245,7 @@ int arm_init_arch_info(struct target *target, struct arm *arm);
 int armv4_5_run_algorithm(struct target *target,
 		int num_mem_params, struct mem_param *mem_params,
 		int num_reg_params, struct reg_param *reg_params,
-		uint32_t entry_point, uint32_t exit_point,
+		target_addr_t entry_point, target_addr_t exit_point,
 		int timeout_ms, void *arch_info);
 int armv4_5_run_algorithm_inner(struct target *target,
 		int num_mem_params, struct mem_param *mem_params,
@@ -230,12 +256,13 @@ int armv4_5_run_algorithm_inner(struct target *target,
 				int timeout_ms, void *arch_info));
 
 int arm_checksum_memory(struct target *target,
-		uint32_t address, uint32_t count, uint32_t *checksum);
+		target_addr_t address, uint32_t count, uint32_t *checksum);
 int arm_blank_check_memory(struct target *target,
-		uint32_t address, uint32_t count, uint32_t *blank);
+		target_addr_t address, uint32_t count, uint32_t *blank, uint8_t erased_value);
 
 void arm_set_cpsr(struct arm *arm, uint32_t cpsr);
 struct reg *arm_reg_current(struct arm *arm, unsigned regnum);
+struct reg *armv8_reg_current(struct arm *arm, unsigned regnum);
 
 extern struct reg arm_gdb_dummy_fp_reg;
 extern struct reg arm_gdb_dummy_fps_reg;
diff --git a/src/target/arm11.c b/src/target/arm11.c
index cbe4d45..13fbd20 100644
--- a/src/target/arm11.c
+++ b/src/target/arm11.c
@@ -42,7 +42,7 @@
 
 
 static int arm11_step(struct target *target, int current,
-		uint32_t address, int handle_breakpoints);
+		target_addr_t address, int handle_breakpoints);
 
 
 /** Check and if necessary take control of the system
@@ -449,7 +449,7 @@ static uint32_t arm11_nextpc(struct arm11_common *arm11, int current, uint32_t a
 }
 
 static int arm11_resume(struct target *target, int current,
-	uint32_t address, int handle_breakpoints, int debug_execution)
+	target_addr_t address, int handle_breakpoints, int debug_execution)
 {
 	/*	  LOG_DEBUG("current %d  address %08x  handle_breakpoints %d  debug_execution %d", */
 	/*	current, address, handle_breakpoints, debug_execution); */
@@ -467,7 +467,7 @@ static int arm11_resume(struct target *target, int current,
 
 	address = arm11_nextpc(arm11, current, address);
 
-	LOG_DEBUG("RESUME PC %08" PRIx32 "%s", address, !current ? "!" : "");
+	LOG_DEBUG("RESUME PC %08" TARGET_PRIxADDR "%s", address, !current ? "!" : "");
 
 	/* clear breakpoints/watchpoints and VCR*/
 	CHECK_RETVAL(arm11_sc7_clear_vbw(arm11));
@@ -481,7 +481,7 @@ static int arm11_resume(struct target *target, int current,
 
 		for (bp = target->breakpoints; bp; bp = bp->next) {
 			if (bp->address == address) {
-				LOG_DEBUG("must step over %08" PRIx32 "", bp->address);
+				LOG_DEBUG("must step over %08" TARGET_PRIxADDR "", bp->address);
 				arm11_step(target, 1, 0, 0);
 				break;
 			}
@@ -507,7 +507,7 @@ static int arm11_resume(struct target *target, int current,
 
 			CHECK_RETVAL(arm11_sc7_run(arm11, brp, ARRAY_SIZE(brp)));
 
-			LOG_DEBUG("Add BP %d at %08" PRIx32, brp_num,
+			LOG_DEBUG("Add BP %d at %08" TARGET_PRIxADDR, brp_num,
 				bp->address);
 
 			brp_num++;
@@ -557,7 +557,7 @@ static int arm11_resume(struct target *target, int current,
 }
 
 static int arm11_step(struct target *target, int current,
-	uint32_t address, int handle_breakpoints)
+	target_addr_t address, int handle_breakpoints)
 {
 	LOG_DEBUG("target->state: %s",
 		target_state_name(target));
@@ -571,7 +571,7 @@ static int arm11_step(struct target *target, int current,
 
 	address = arm11_nextpc(arm11, current, address);
 
-	LOG_DEBUG("STEP PC %08" PRIx32 "%s", address, !current ? "!" : "");
+	LOG_DEBUG("STEP PC %08" TARGET_PRIxADDR "%s", address, !current ? "!" : "");
 
 
 	/** \todo TODO: Thumb not supported here */
@@ -583,13 +583,13 @@ static int arm11_step(struct target *target, int current,
 	/* skip over BKPT */
 	if ((next_instruction & 0xFFF00070) == 0xe1200070) {
 		address = arm11_nextpc(arm11, 0, address + 4);
-		LOG_DEBUG("Skipping BKPT %08" PRIx32, address);
+		LOG_DEBUG("Skipping BKPT %08" TARGET_PRIxADDR, address);
 	}
 	/* skip over Wait for interrupt / Standby
 	 * mcr	15, 0, r?, cr7, cr0, {4} */
 	else if ((next_instruction & 0xFFFF0FFF) == 0xee070f90) {
 		address = arm11_nextpc(arm11, 0, address + 4);
-		LOG_DEBUG("Skipping WFI %08" PRIx32, address);
+		LOG_DEBUG("Skipping WFI %08" TARGET_PRIxADDR, address);
 	}
 	/* ignore B to self */
 	else if ((next_instruction & 0xFEFFFFFF) == 0xeafffffe)
@@ -887,7 +887,7 @@ static int arm11_read_memory_inner(struct target *target,
 }
 
 static int arm11_read_memory(struct target *target,
-	uint32_t address,
+	target_addr_t address,
 	uint32_t size,
 	uint32_t count,
 	uint8_t *buffer)
@@ -1043,7 +1043,7 @@ static int arm11_write_memory_inner(struct target *target,
 }
 
 static int arm11_write_memory(struct target *target,
-	uint32_t address, uint32_t size,
+	target_addr_t address, uint32_t size,
 	uint32_t count, const uint8_t *buffer)
 {
 	/* pointer increment matters only for multi-unit writes ...
diff --git a/src/target/arm720t.c b/src/target/arm720t.c
index 3991e19..bcbfa9d 100644
--- a/src/target/arm720t.c
+++ b/src/target/arm720t.c
@@ -271,7 +271,7 @@ static int arm720_mmu(struct target *target, int *enabled)
 }
 
 static int arm720_virt2phys(struct target *target,
-		uint32_t virtual, uint32_t *physical)
+		target_addr_t virtual, target_addr_t *physical)
 {
 	uint32_t cb;
 	struct arm720t_common *arm720t = target_to_arm720(target);
@@ -286,7 +286,7 @@ static int arm720_virt2phys(struct target *target,
 }
 
 static int arm720t_read_memory(struct target *target,
-		uint32_t address, uint32_t size, uint32_t count, uint8_t *buffer)
+		target_addr_t address, uint32_t size, uint32_t count, uint8_t *buffer)
 {
 	int retval;
 	struct arm720t_common *arm720t = target_to_arm720(target);
@@ -309,7 +309,7 @@ static int arm720t_read_memory(struct target *target,
 }
 
 static int arm720t_read_phys_memory(struct target *target,
-		uint32_t address, uint32_t size, uint32_t count, uint8_t *buffer)
+		target_addr_t address, uint32_t size, uint32_t count, uint8_t *buffer)
 {
 	struct arm720t_common *arm720t = target_to_arm720(target);
 
@@ -317,7 +317,7 @@ static int arm720t_read_phys_memory(struct target *target,
 }
 
 static int arm720t_write_phys_memory(struct target *target,
-		uint32_t address, uint32_t size, uint32_t count, const uint8_t *buffer)
+		target_addr_t address, uint32_t size, uint32_t count, const uint8_t *buffer)
 {
 	struct arm720t_common *arm720t = target_to_arm720(target);
 
diff --git a/src/target/arm7_9_common.c b/src/target/arm7_9_common.c
index c1d5c79..7fd1ed9 100644
--- a/src/target/arm7_9_common.c
+++ b/src/target/arm7_9_common.c
@@ -101,7 +101,8 @@ static void arm7_9_assign_wp(struct arm7_9_common *arm7_9, struct breakpoint *br
 		arm7_9->wp_available--;
 	} else
 		LOG_ERROR("BUG: no hardware comparator available");
-	LOG_DEBUG("BPID: %" PRId32 " (0x%08" PRIx32 ") using hw wp: %d",
+
+	LOG_DEBUG("BPID: %" PRId32 " (0x%08" TARGET_PRIxADDR ") using hw wp: %d",
 			breakpoint->unique_id,
 			breakpoint->address,
 			breakpoint->set);
@@ -187,7 +188,7 @@ static int arm7_9_set_breakpoint(struct target *target, struct breakpoint *break
 	struct arm7_9_common *arm7_9 = target_to_arm7_9(target);
 	int retval = ERROR_OK;
 
-	LOG_DEBUG("BPID: %" PRId32 ", Address: 0x%08" PRIx32 ", Type: %d",
+	LOG_DEBUG("BPID: %" PRId32 ", Address: 0x%08" TARGET_PRIxADDR ", Type: %d",
 		breakpoint->unique_id,
 		breakpoint->address,
 		breakpoint->type);
@@ -244,7 +245,7 @@ static int arm7_9_set_breakpoint(struct target *target, struct breakpoint *break
 			if (retval != ERROR_OK)
 				return retval;
 			if (verify != arm7_9->arm_bkpt) {
-				LOG_ERROR("Unable to set 32 bit software breakpoint at address %08" PRIx32
+				LOG_ERROR("Unable to set 32 bit software breakpoint at address %08" TARGET_PRIxADDR
 						" - check that memory is read/writable", breakpoint->address);
 				return ERROR_OK;
 			}
@@ -264,7 +265,7 @@ static int arm7_9_set_breakpoint(struct target *target, struct breakpoint *break
 			if (retval != ERROR_OK)
 				return retval;
 			if (verify != arm7_9->thumb_bkpt) {
-				LOG_ERROR("Unable to set thumb software breakpoint at address %08" PRIx32
+				LOG_ERROR("Unable to set thumb software breakpoint at address %08" TARGET_PRIxADDR
 						" - check that memory is read/writable", breakpoint->address);
 				return ERROR_OK;
 			}
@@ -299,7 +300,7 @@ static int arm7_9_unset_breakpoint(struct target *target, struct breakpoint *bre
 	int retval = ERROR_OK;
 	struct arm7_9_common *arm7_9 = target_to_arm7_9(target);
 
-	LOG_DEBUG("BPID: %" PRId32 ", Address: 0x%08" PRIx32,
+	LOG_DEBUG("BPID: %" PRId32 ", Address: 0x%08" TARGET_PRIxADDR,
 		breakpoint->unique_id,
 		breakpoint->address);
 
@@ -1692,7 +1693,7 @@ static void arm7_9_enable_breakpoints(struct target *target)
 
 int arm7_9_resume(struct target *target,
 	int current,
-	uint32_t address,
+	target_addr_t address,
 	int handle_breakpoints,
 	int debug_execution)
 {
@@ -1724,7 +1725,7 @@ int arm7_9_resume(struct target *target,
 		breakpoint = breakpoint_find(target,
 				buf_get_u32(arm->pc->value, 0, 32));
 		if (breakpoint != NULL) {
-			LOG_DEBUG("unset breakpoint at 0x%8.8" PRIx32 " (id: %" PRId32,
+			LOG_DEBUG("unset breakpoint at 0x%8.8" TARGET_PRIxADDR " (id: %" PRId32,
 				breakpoint->address,
 				breakpoint->unique_id);
 			retval = arm7_9_unset_breakpoint(target, breakpoint);
@@ -1783,7 +1784,7 @@ int arm7_9_resume(struct target *target,
 			LOG_DEBUG("new PC after step: 0x%8.8" PRIx32,
 				buf_get_u32(arm->pc->value, 0, 32));
 
-			LOG_DEBUG("set breakpoint at 0x%8.8" PRIx32 "", breakpoint->address);
+			LOG_DEBUG("set breakpoint at 0x%8.8" TARGET_PRIxADDR "", breakpoint->address);
 			retval = arm7_9_set_breakpoint(target, breakpoint);
 			if (retval != ERROR_OK)
 				return retval;
@@ -1894,7 +1895,7 @@ void arm7_9_disable_eice_step(struct target *target)
 	embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_VALUE]);
 }
 
-int arm7_9_step(struct target *target, int current, uint32_t address, int handle_breakpoints)
+int arm7_9_step(struct target *target, int current, target_addr_t address, int handle_breakpoints)
 {
 	struct arm7_9_common *arm7_9 = target_to_arm7_9(target);
 	struct arm *arm = &arm7_9->arm;
@@ -2094,7 +2095,7 @@ static int arm7_9_write_core_reg(struct target *target, struct reg *r,
 }
 
 int arm7_9_read_memory(struct target *target,
-	uint32_t address,
+	target_addr_t address,
 	uint32_t size,
 	uint32_t count,
 	uint8_t *buffer)
@@ -2109,7 +2110,7 @@ int arm7_9_read_memory(struct target *target,
 	int retval;
 	int last_reg = 0;
 
-	LOG_DEBUG("address: 0x%8.8" PRIx32 ", size: 0x%8.8" PRIx32 ", count: 0x%8.8" PRIx32 "",
+	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) {
@@ -2247,7 +2248,8 @@ int arm7_9_read_memory(struct target *target,
 
 	if (((cpsr & 0x1f) == ARM_MODE_ABT) && (arm->core_mode != ARM_MODE_ABT)) {
 		LOG_WARNING(
-			"memory read caused data abort (address: 0x%8.8" PRIx32 ", size: 0x%" PRIx32 ", count: 0x%" PRIx32 ")",
+			"memory read caused data abort "
+			"(address: 0x%8.8" TARGET_PRIxADDR ", size: 0x%" PRIx32 ", count: 0x%" PRIx32 ")",
 			address,
 			size,
 			count);
@@ -2263,7 +2265,7 @@ int arm7_9_read_memory(struct target *target,
 }
 
 int arm7_9_write_memory(struct target *target,
-	uint32_t address,
+	target_addr_t address,
 	uint32_t size,
 	uint32_t count,
 	const uint8_t *buffer)
@@ -2460,7 +2462,8 @@ int arm7_9_write_memory(struct target *target,
 
 	if (((cpsr & 0x1f) == ARM_MODE_ABT) && (arm->core_mode != ARM_MODE_ABT)) {
 		LOG_WARNING(
-			"memory write caused data abort (address: 0x%8.8" PRIx32 ", size: 0x%" PRIx32 ", count: 0x%" PRIx32 ")",
+			"memory write caused data abort "
+			"(address: 0x%8.8" TARGET_PRIxADDR ", size: 0x%" PRIx32 ", count: 0x%" PRIx32 ")",
 			address,
 			size,
 			count);
@@ -2476,7 +2479,7 @@ int arm7_9_write_memory(struct target *target,
 }
 
 int arm7_9_write_memory_opt(struct target *target,
-	uint32_t address,
+	target_addr_t address,
 	uint32_t size,
 	uint32_t count,
 	const uint8_t *buffer)
@@ -2576,7 +2579,7 @@ static const uint32_t dcc_code[] = {
 };
 
 int arm7_9_bulk_write_memory(struct target *target,
-	uint32_t address,
+	target_addr_t address,
 	uint32_t count,
 	const uint8_t *buffer)
 {
@@ -2632,7 +2635,7 @@ int arm7_9_bulk_write_memory(struct target *target,
 		uint32_t endaddress = buf_get_u32(reg_params[0].value, 0, 32);
 		if (endaddress != (address + count*4)) {
 			LOG_ERROR(
-				"DCC write failed, expected end address 0x%08" PRIx32 " got 0x%0" PRIx32 "",
+				"DCC write failed, expected end address 0x%08" TARGET_PRIxADDR " got 0x%0" PRIx32 "",
 				(address + count*4),
 				endaddress);
 			retval = ERROR_FAIL;
diff --git a/src/target/arm7_9_common.h b/src/target/arm7_9_common.h
index 044384b..811f9c5 100644
--- a/src/target/arm7_9_common.h
+++ b/src/target/arm7_9_common.h
@@ -122,13 +122,13 @@ struct arm7_9_common {
 	 * Used as a fallback when bulk writes are unavailable, or for writing data needed to
 	 * do the bulk writes.
 	 */
-	int (*write_memory)(struct target *target, uint32_t address,
+	int (*write_memory)(struct target *target, target_addr_t address,
 			uint32_t size, uint32_t count, const uint8_t *buffer);
 	/**
 	 * Write target memory in multiples of 4 bytes, optimized for
 	 * writing large quantities of data.
 	 */
-	int (*bulk_write_memory)(struct target *target, uint32_t address,
+	int (*bulk_write_memory)(struct target *target, target_addr_t address,
 			uint32_t count, const uint8_t *buffer);
 };
 
@@ -155,19 +155,19 @@ int arm7_9_early_halt(struct target *target);
 int arm7_9_soft_reset_halt(struct target *target);
 
 int arm7_9_halt(struct target *target);
-int arm7_9_resume(struct target *target, int current, uint32_t address,
+int arm7_9_resume(struct target *target, int current, target_addr_t address,
 		int handle_breakpoints, int debug_execution);
-int arm7_9_step(struct target *target, int current, uint32_t address,
+int arm7_9_step(struct target *target, int current, target_addr_t address,
 		int handle_breakpoints);
-int arm7_9_read_memory(struct target *target, uint32_t address,
+int arm7_9_read_memory(struct target *target, target_addr_t address,
 		uint32_t size, uint32_t count, uint8_t *buffer);
-int arm7_9_write_memory(struct target *target, uint32_t address,
+int arm7_9_write_memory(struct target *target, target_addr_t address,
 		uint32_t size, uint32_t count, const uint8_t *buffer);
-int arm7_9_write_memory_opt(struct target *target, uint32_t address,
+int arm7_9_write_memory_opt(struct target *target, target_addr_t address,
 		uint32_t size, uint32_t count, const uint8_t *buffer);
 int arm7_9_write_memory_no_opt(struct target *target, uint32_t address,
 		uint32_t size, uint32_t count, const uint8_t *buffer);
-int arm7_9_bulk_write_memory(struct target *target, uint32_t address,
+int arm7_9_bulk_write_memory(struct target *target, target_addr_t address,
 		uint32_t count, const uint8_t *buffer);
 
 int arm7_9_run_algorithm(struct target *target, int num_mem_params,
diff --git a/src/target/arm7tdmi.c b/src/target/arm7tdmi.c
index 58ab027..9dcb302 100644
--- a/src/target/arm7tdmi.c
+++ b/src/target/arm7tdmi.c
@@ -30,6 +30,7 @@
 #include "target_type.h"
 #include "register.h"
 #include "arm_opcodes.h"
+#include "arm_semihosting.h"
 
 /*
  * For information about ARM7TDMI, see ARM DDI 0210C (r4p1)
@@ -615,7 +616,7 @@ static void arm7tdmi_build_reg_cache(struct target *target)
 int arm7tdmi_init_target(struct command_context *cmd_ctx, struct target *target)
 {
 	arm7tdmi_build_reg_cache(target);
-
+	arm_semihosting_init(target);
 	return ERROR_OK;
 }
 
diff --git a/src/target/arm920t.c b/src/target/arm920t.c
index 2c96d19..7927a2b 100644
--- a/src/target/arm920t.c
+++ b/src/target/arm920t.c
@@ -553,7 +553,7 @@ static int arm920_mmu(struct target *target, int *enabled)
 }
 
 static int arm920_virt2phys(struct target *target,
-	uint32_t virt, uint32_t *phys)
+	target_addr_t virt, target_addr_t *phys)
 {
 	uint32_t cb;
 	struct arm920t_common *arm920t = target_to_arm920(target);
@@ -568,7 +568,7 @@ static int arm920_virt2phys(struct target *target,
 }
 
 /** Reads a buffer, in the specified word size, with current MMU settings. */
-int arm920t_read_memory(struct target *target, uint32_t address,
+int arm920t_read_memory(struct target *target, target_addr_t address,
 	uint32_t size, uint32_t count, uint8_t *buffer)
 {
 	int retval;
@@ -580,7 +580,7 @@ int arm920t_read_memory(struct target *target, uint32_t address,
 
 
 static int arm920t_read_phys_memory(struct target *target,
-	uint32_t address, uint32_t size,
+	target_addr_t address, uint32_t size,
 	uint32_t count, uint8_t *buffer)
 {
 	struct arm920t_common *arm920t = target_to_arm920(target);
@@ -590,7 +590,7 @@ static int arm920t_read_phys_memory(struct target *target,
 }
 
 static int arm920t_write_phys_memory(struct target *target,
-	uint32_t address, uint32_t size,
+	target_addr_t address, uint32_t size,
 	uint32_t count, const uint8_t *buffer)
 {
 	struct arm920t_common *arm920t = target_to_arm920(target);
@@ -600,7 +600,7 @@ static int arm920t_write_phys_memory(struct target *target,
 }
 
 /** Writes a buffer, in the specified word size, with current MMU settings. */
-int arm920t_write_memory(struct target *target, uint32_t address,
+int arm920t_write_memory(struct target *target, target_addr_t address,
 	uint32_t size, uint32_t count, const uint8_t *buffer)
 {
 	int retval;
diff --git a/src/target/arm920t.h b/src/target/arm920t.h
index 3401b09..2e3b08c 100644
--- a/src/target/arm920t.h
+++ b/src/target/arm920t.h
@@ -55,9 +55,9 @@ struct arm920t_tlb_entry {
 int arm920t_arch_state(struct target *target);
 int arm920t_soft_reset_halt(struct target *target);
 int arm920t_read_memory(struct target *target,
-	uint32_t address, uint32_t size, uint32_t count, uint8_t *buffer);
+	target_addr_t address, uint32_t size, uint32_t count, uint8_t *buffer);
 int arm920t_write_memory(struct target *target,
-	uint32_t address, uint32_t size, uint32_t count, const uint8_t *buffer);
+	target_addr_t address, uint32_t size, uint32_t count, const uint8_t *buffer);
 int arm920t_post_debug_entry(struct target *target);
 void arm920t_pre_restore_context(struct target *target);
 int arm920t_get_ttb(struct target *target, uint32_t *result);
diff --git a/src/target/arm926ejs.c b/src/target/arm926ejs.c
index d7c043e..58de778 100644
--- a/src/target/arm926ejs.c
+++ b/src/target/arm926ejs.c
@@ -594,7 +594,7 @@ int arm926ejs_soft_reset_halt(struct target *target)
 }
 
 /** Writes a buffer, in the specified word size, with current MMU settings. */
-int arm926ejs_write_memory(struct target *target, uint32_t address,
+int arm926ejs_write_memory(struct target *target, target_addr_t address,
 		uint32_t size, uint32_t count, const uint8_t *buffer)
 {
 	int retval;
@@ -623,7 +623,7 @@ int arm926ejs_write_memory(struct target *target, uint32_t address,
 				return retval;
 		}
 
-		uint32_t pa;
+		target_addr_t pa;
 		retval = target->type->virt2phys(target, address, &pa);
 		if (retval != ERROR_OK)
 			return retval;
@@ -655,7 +655,7 @@ int arm926ejs_write_memory(struct target *target, uint32_t address,
 }
 
 static int arm926ejs_write_phys_memory(struct target *target,
-		uint32_t address, uint32_t size,
+		target_addr_t address, uint32_t size,
 		uint32_t count, const uint8_t *buffer)
 {
 	struct arm926ejs_common *arm926ejs = target_to_arm926(target);
@@ -665,7 +665,7 @@ static int arm926ejs_write_phys_memory(struct target *target,
 }
 
 static int arm926ejs_read_phys_memory(struct target *target,
-		uint32_t address, uint32_t size,
+		target_addr_t address, uint32_t size,
 		uint32_t count, uint8_t *buffer)
 {
 	struct arm926ejs_common *arm926ejs = target_to_arm926(target);
@@ -736,7 +736,7 @@ COMMAND_HANDLER(arm926ejs_handle_cache_info_command)
 	return armv4_5_handle_cache_info_command(CMD_CTX, &arm926ejs->armv4_5_mmu.armv4_5_cache);
 }
 
-static int arm926ejs_virt2phys(struct target *target, uint32_t virtual, uint32_t *physical)
+static int arm926ejs_virt2phys(struct target *target, target_addr_t virtual, target_addr_t *physical)
 {
 	uint32_t cb;
 	struct arm926ejs_common *arm926ejs = target_to_arm926(target);
diff --git a/src/target/arm926ejs.h b/src/target/arm926ejs.h
index 02b4ef8..d4fd0cb 100644
--- a/src/target/arm926ejs.h
+++ b/src/target/arm926ejs.h
@@ -47,7 +47,7 @@ int arm926ejs_init_arch_info(struct target *target,
 		struct arm926ejs_common *arm926ejs, struct jtag_tap *tap);
 int arm926ejs_arch_state(struct target *target);
 int arm926ejs_write_memory(struct target *target,
-		uint32_t address, uint32_t size, uint32_t count, const uint8_t *buffer);
+		target_addr_t address, uint32_t size, uint32_t count, const uint8_t *buffer);
 int arm926ejs_soft_reset_halt(struct target *target);
 
 extern const struct command_registration arm926ejs_command_handlers[];
diff --git a/src/target/arm946e.c b/src/target/arm946e.c
index 5ee31cc..06c9fc3 100644
--- a/src/target/arm946e.c
+++ b/src/target/arm946e.c
@@ -487,7 +487,7 @@ uint32_t arm946e_invalidate_icache(struct target *target, uint32_t address,
 }
 
 /** Writes a buffer, in the specified word size, with current MMU settings. */
-int arm946e_write_memory(struct target *target, uint32_t address,
+int arm946e_write_memory(struct target *target, target_addr_t address,
 	uint32_t size, uint32_t count, const uint8_t *buffer)
 {
 	int retval;
@@ -535,7 +535,7 @@ int arm946e_write_memory(struct target *target, uint32_t address,
 
 }
 
-int arm946e_read_memory(struct target *target, uint32_t address,
+int arm946e_read_memory(struct target *target, target_addr_t address,
 	uint32_t size, uint32_t count, uint8_t *buffer)
 {
 	int retval;
diff --git a/src/target/arm9tdmi.c b/src/target/arm9tdmi.c
index eed965a..82b430f 100644
--- a/src/target/arm9tdmi.c
+++ b/src/target/arm9tdmi.c
@@ -30,6 +30,7 @@
 #include "target_type.h"
 #include "register.h"
 #include "arm_opcodes.h"
+#include "arm_semihosting.h"
 
 /*
  * NOTE:  this holds code that's used with multiple ARM9 processors:
@@ -714,6 +715,7 @@ int arm9tdmi_init_target(struct command_context *cmd_ctx,
 		struct target *target)
 {
 	arm9tdmi_build_reg_cache(target);
+	arm_semihosting_init(target);
 	return ERROR_OK;
 }
 
diff --git a/src/target/arm_adi_v5.c b/src/target/arm_adi_v5.c
index f58afdc..eafc2dd 100644
--- a/src/target/arm_adi_v5.c
+++ b/src/target/arm_adi_v5.c
@@ -612,60 +612,58 @@ int dap_dp_init(struct adiv5_dap *dap)
 	dap->select = DP_SELECT_INVALID;
 	dap->last_read = NULL;
 
-	for (size_t i = 0; i < 10; i++) {
+	for (size_t i = 0; i < 30; i++) {
 		/* DP initialization */
 
-		retval = dap_queue_dp_read(dap, DP_CTRL_STAT, NULL);
-		if (retval != ERROR_OK)
-			continue;
-
-		retval = dap_queue_dp_write(dap, DP_CTRL_STAT, SSTICKYERR);
-		if (retval != ERROR_OK)
-			continue;
+		retval = dap_dp_read_atomic(dap, DP_CTRL_STAT, NULL);
+		if (retval == ERROR_OK)
+			break;
+	}
 
-		retval = dap_queue_dp_read(dap, DP_CTRL_STAT, NULL);
-		if (retval != ERROR_OK)
-			continue;
+	retval = dap_queue_dp_write(dap, DP_CTRL_STAT, SSTICKYERR);
+	if (retval != ERROR_OK)
+		return retval;
 
-		dap->dp_ctrl_stat = CDBGPWRUPREQ | CSYSPWRUPREQ;
-		retval = dap_queue_dp_write(dap, DP_CTRL_STAT, dap->dp_ctrl_stat);
-		if (retval != ERROR_OK)
-			continue;
+	retval = dap_queue_dp_read(dap, DP_CTRL_STAT, NULL);
+	if (retval != ERROR_OK)
+		return retval;
 
-		/* Check that we have debug power domains activated */
-		LOG_DEBUG("DAP: wait CDBGPWRUPACK");
-		retval = dap_dp_poll_register(dap, DP_CTRL_STAT,
-					      CDBGPWRUPACK, CDBGPWRUPACK,
-					      DAP_POWER_DOMAIN_TIMEOUT);
-		if (retval != ERROR_OK)
-			continue;
+	dap->dp_ctrl_stat = CDBGPWRUPREQ | CSYSPWRUPREQ;
+	retval = dap_queue_dp_write(dap, DP_CTRL_STAT, dap->dp_ctrl_stat);
+	if (retval != ERROR_OK)
+		return retval;
 
-		LOG_DEBUG("DAP: wait CSYSPWRUPACK");
-		retval = dap_dp_poll_register(dap, DP_CTRL_STAT,
-					      CSYSPWRUPACK, CSYSPWRUPACK,
-					      DAP_POWER_DOMAIN_TIMEOUT);
-		if (retval != ERROR_OK)
-			continue;
+	/* Check that we have debug power domains activated */
+	LOG_DEBUG("DAP: wait CDBGPWRUPACK");
+	retval = dap_dp_poll_register(dap, DP_CTRL_STAT,
+				      CDBGPWRUPACK, CDBGPWRUPACK,
+				      DAP_POWER_DOMAIN_TIMEOUT);
+	if (retval != ERROR_OK)
+		return retval;
 
-		retval = dap_queue_dp_read(dap, DP_CTRL_STAT, NULL);
-		if (retval != ERROR_OK)
-			continue;
+	LOG_DEBUG("DAP: wait CSYSPWRUPACK");
+	retval = dap_dp_poll_register(dap, DP_CTRL_STAT,
+				      CSYSPWRUPACK, CSYSPWRUPACK,
+				      DAP_POWER_DOMAIN_TIMEOUT);
+	if (retval != ERROR_OK)
+		return retval;
 
-		/* With debug power on we can activate OVERRUN checking */
-		dap->dp_ctrl_stat = CDBGPWRUPREQ | CSYSPWRUPREQ | CORUNDETECT;
-		retval = dap_queue_dp_write(dap, DP_CTRL_STAT, dap->dp_ctrl_stat);
-		if (retval != ERROR_OK)
-			continue;
-		retval = dap_queue_dp_read(dap, DP_CTRL_STAT, NULL);
-		if (retval != ERROR_OK)
-			continue;
+	retval = dap_queue_dp_read(dap, DP_CTRL_STAT, NULL);
+	if (retval != ERROR_OK)
+		return retval;
 
-		retval = dap_run(dap);
-		if (retval != ERROR_OK)
-			continue;
+	/* With debug power on we can activate OVERRUN checking */
+	dap->dp_ctrl_stat = CDBGPWRUPREQ | CSYSPWRUPREQ | CORUNDETECT;
+	retval = dap_queue_dp_write(dap, DP_CTRL_STAT, dap->dp_ctrl_stat);
+	if (retval != ERROR_OK)
+		return retval;
+	retval = dap_queue_dp_read(dap, DP_CTRL_STAT, NULL);
+	if (retval != ERROR_OK)
+		return retval;
 
-		break;
-	}
+	retval = dap_run(dap);
+	if (retval != ERROR_OK)
+		return retval;
 
 	return retval;
 }
@@ -1361,6 +1359,41 @@ static int dap_info_command(struct command_context *cmd_ctx,
 	return ERROR_OK;
 }
 
+int adiv5_jim_configure(struct target *target, Jim_GetOptInfo *goi)
+{
+	struct adiv5_private_config *pc;
+	const char *arg;
+	jim_wide ap_num;
+	int e;
+
+	/* check if argv[0] is for us */
+	arg = Jim_GetString(goi->argv[0], NULL);
+	if (strcmp(arg, "-ap-num"))
+		return JIM_CONTINUE;
+
+	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, "-ap-num ?ap-number? ...");
+		return JIM_ERR;
+	}
+
+	e = Jim_GetOpt_Wide(goi, &ap_num);
+	if (e != JIM_OK)
+		return e;
+
+	if (target->private_config == NULL) {
+		pc = calloc(1, sizeof(struct adiv5_private_config));
+		target->private_config = pc;
+		pc->ap_num = ap_num;
+	}
+
+
+	return JIM_OK;
+}
+
 COMMAND_HANDLER(handle_dap_info_command)
 {
 	struct target *target = get_current_target(CMD_CTX);
diff --git a/src/target/arm_adi_v5.h b/src/target/arm_adi_v5.h
index 3220d8b..bf9cb5c 100644
--- a/src/target/arm_adi_v5.h
+++ b/src/target/arm_adi_v5.h
@@ -504,4 +504,10 @@ int dap_to_jtag(struct target *target);
 
 extern const struct command_registration dap_command_handlers[];
 
+struct adiv5_private_config {
+	int ap_num;
+};
+
+extern int adiv5_jim_configure(struct target *target, Jim_GetOptInfo *goi);
+
 #endif /* OPENOCD_TARGET_ARM_ADI_V5_H */
diff --git a/src/target/arm_cti.c b/src/target/arm_cti.c
new file mode 100644
index 0000000..75169b2
--- /dev/null
+++ b/src/target/arm_cti.c
@@ -0,0 +1,148 @@
+/***************************************************************************
+ *   Copyright (C) 2016 by Matthias Welwarsky                              *
+ *                                                                         *
+ *   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, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *                                                                         *
+ ***************************************************************************/
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include <stdlib.h>
+#include <stdint.h>
+#include "target/arm_adi_v5.h"
+#include "target/arm_cti.h"
+#include "target/target.h"
+#include "helper/time_support.h"
+
+struct arm_cti {
+	uint32_t base;
+	struct adiv5_ap *ap;
+};
+
+struct arm_cti *arm_cti_create(struct adiv5_ap *ap, uint32_t base)
+{
+	struct arm_cti *self = calloc(1, sizeof(struct arm_cti));
+	if (!self)
+		return NULL;
+
+	self->base = base;
+	self->ap = ap;
+	return self;
+}
+
+static int arm_cti_mod_reg_bits(struct arm_cti *self, unsigned int reg, uint32_t mask, uint32_t value)
+{
+	uint32_t tmp;
+
+	/* Read register */
+	int retval = mem_ap_read_atomic_u32(self->ap, self->base + reg, &tmp);
+	if (ERROR_OK != retval)
+		return retval;
+
+	/* clear bitfield */
+	tmp &= ~mask;
+	/* put new value */
+	tmp |= value & mask;
+
+	/* write new value */
+	return mem_ap_write_atomic_u32(self->ap, self->base + reg, tmp);
+}
+
+int arm_cti_enable(struct arm_cti *self, bool enable)
+{
+	uint32_t val = enable ? 1 : 0;
+
+	return mem_ap_write_atomic_u32(self->ap, self->base + CTI_CTR, val);
+}
+
+int arm_cti_ack_events(struct arm_cti *self, uint32_t event)
+{
+	int retval;
+	uint32_t tmp;
+
+	retval = mem_ap_write_atomic_u32(self->ap, self->base + CTI_INACK, event);
+	if (retval == ERROR_OK) {
+		int64_t then = timeval_ms();
+		for (;;) {
+			retval = mem_ap_read_atomic_u32(self->ap, self->base + CTI_TROUT_STATUS, &tmp);
+			if (retval != ERROR_OK)
+				break;
+			if ((tmp & event) == 0)
+				break;
+			if (timeval_ms() > then + 1000) {
+				LOG_ERROR("timeout waiting for target");
+				retval = ERROR_TARGET_TIMEOUT;
+				break;
+			}
+		}
+	}
+
+	return retval;
+}
+
+int arm_cti_gate_channel(struct arm_cti *self, uint32_t channel)
+{
+	if (channel > 31)
+		return ERROR_COMMAND_ARGUMENT_INVALID;
+
+	return arm_cti_mod_reg_bits(self, CTI_GATE, CTI_CHNL(channel), 0);
+}
+
+int arm_cti_ungate_channel(struct arm_cti *self, uint32_t channel)
+{
+	if (channel > 31)
+		return ERROR_COMMAND_ARGUMENT_INVALID;
+
+	return arm_cti_mod_reg_bits(self, CTI_GATE, CTI_CHNL(channel), 0xFFFFFFFF);
+}
+
+int arm_cti_write_reg(struct arm_cti *self, unsigned int reg, uint32_t value)
+{
+	return mem_ap_write_atomic_u32(self->ap, self->base + reg, value);
+}
+
+int arm_cti_read_reg(struct arm_cti *self, unsigned int reg, uint32_t *p_value)
+{
+	if (p_value == NULL)
+		return ERROR_COMMAND_ARGUMENT_INVALID;
+
+	return mem_ap_read_atomic_u32(self->ap, self->base + reg, p_value);
+}
+
+int arm_cti_pulse_channel(struct arm_cti *self, uint32_t channel)
+{
+	if (channel > 31)
+		return ERROR_COMMAND_ARGUMENT_INVALID;
+
+	return arm_cti_write_reg(self, CTI_APPPULSE, CTI_CHNL(channel));
+}
+
+int arm_cti_set_channel(struct arm_cti *self, uint32_t channel)
+{
+	if (channel > 31)
+		return ERROR_COMMAND_ARGUMENT_INVALID;
+
+	return arm_cti_write_reg(self, CTI_APPSET, CTI_CHNL(channel));
+}
+
+int arm_cti_clear_channel(struct arm_cti *self, uint32_t channel)
+{
+	if (channel > 31)
+		return ERROR_COMMAND_ARGUMENT_INVALID;
+
+	return arm_cti_write_reg(self, CTI_APPCLEAR, CTI_CHNL(channel));
+}
diff --git a/src/target/arm_cti.h b/src/target/arm_cti.h
new file mode 100644
index 0000000..99724c4
--- /dev/null
+++ b/src/target/arm_cti.h
@@ -0,0 +1,73 @@
+/***************************************************************************
+ *   Copyright (C) 2016 by Matthias Welwarsky                              *
+ *                                                                         *
+ *   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, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *                                                                         *
+ ***************************************************************************/
+
+#ifndef OPENOCD_TARGET_ARM_CTI_H
+#define OPENOCD_TARGET_ARM_CTI_H
+
+/*define CTI(cross trigger interface)*/
+#define CTI_CTR				0x0
+#define CTI_INACK			0x10
+#define CTI_APPSET			0x14
+#define CTI_APPCLEAR		0x18
+#define CTI_APPPULSE		0x1C
+#define CTI_INEN0			0x20
+#define CTI_INEN1			0x24
+#define CTI_INEN2			0x28
+#define CTI_INEN3			0x2C
+#define CTI_INEN4			0x30
+#define CTI_INEN5			0x34
+#define CTI_INEN6			0x38
+#define CTI_INEN7			0x3C
+#define CTI_INEN(n)			(0x20 + 4 * n)
+#define CTI_OUTEN0			0xA0
+#define CTI_OUTEN1			0xA4
+#define CTI_OUTEN2			0xA8
+#define CTI_OUTEN3			0xAC
+#define CTI_OUTEN4			0xB0
+#define CTI_OUTEN5			0xB4
+#define CTI_OUTEN6			0xB8
+#define CTI_OUTEN7			0xBC
+#define CTI_OUTEN(n)		(0xA0 + 4 * n)
+#define CTI_TRIN_STATUS		0x130
+#define CTI_TROUT_STATUS	0x134
+#define CTI_CHIN_STATUS		0x138
+#define CTI_CHOU_STATUS		0x13C
+#define CTI_GATE			0x140
+#define CTI_UNLOCK			0xFB0
+
+#define CTI_CHNL(x)			(1 << x)
+#define CTI_TRIG_HALT		0
+#define CTI_TRIG_RESUME		1
+#define CTI_TRIG(n)			(1 << CTI_TRIG_##n)
+
+/* forward-declare arm_cti struct */
+struct arm_cti;
+
+extern struct arm_cti *arm_cti_create(struct adiv5_ap *ap, uint32_t base);
+extern int arm_cti_enable(struct arm_cti *self, bool enable);
+extern int arm_cti_ack_events(struct arm_cti *self, uint32_t event);
+extern int arm_cti_gate_channel(struct arm_cti *self, uint32_t channel);
+extern int arm_cti_ungate_channel(struct arm_cti *self, uint32_t channel);
+extern int arm_cti_write_reg(struct arm_cti *self, unsigned int reg, uint32_t value);
+extern int arm_cti_read_reg(struct arm_cti *self, unsigned int reg, uint32_t *value);
+extern int arm_cti_pulse_channel(struct arm_cti *self, uint32_t channel);
+extern int arm_cti_set_channel(struct arm_cti *self, uint32_t channel);
+extern int arm_cti_clear_channel(struct arm_cti *self, uint32_t channel);
+
+#endif /* OPENOCD_TARGET_ARM_CTI_H */
diff --git a/src/target/arm_dpm.c b/src/target/arm_dpm.c
index 8ad6575..3e8180c 100644
--- a/src/target/arm_dpm.c
+++ b/src/target/arm_dpm.c
@@ -21,6 +21,7 @@
 
 #include "arm.h"
 #include "arm_dpm.h"
+#include "armv8_dpm.h"
 #include <jtag/jtag.h>
 #include "register.h"
 #include "breakpoints.h"
@@ -165,6 +166,9 @@ static int dpm_read_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
 					/* core-specific ... ? */
 					LOG_WARNING("Jazelle PC adjustment unknown");
 					break;
+				default:
+					LOG_WARNING("unknow core state");
+					break;
 			}
 			break;
 		default:
@@ -433,20 +437,20 @@ int arm_dpm_write_dirty_registers(struct arm_dpm *dpm, bool bpwp)
 
 				/* cope with special cases */
 				switch (regnum) {
-					case 8 ... 12:
-						/* r8..r12 "anything but FIQ" case;
-						 * we "know" core mode is accurate
-						 * since we haven't changed it yet
-						 */
-						if (arm->core_mode == ARM_MODE_FIQ
-							&& ARM_MODE_ANY
-							!= mode)
-							tmode = ARM_MODE_USR;
-						break;
-					case 16:
-						/* SPSR */
-						regnum++;
-						break;
+				case 8 ... 12:
+					/* r8..r12 "anything but FIQ" case;
+					 * we "know" core mode is accurate
+					 * since we haven't changed it yet
+					 */
+					if (arm->core_mode == ARM_MODE_FIQ
+					    && ARM_MODE_ANY
+					    != mode)
+						tmode = ARM_MODE_USR;
+					break;
+				case 16:
+					/* SPSR */
+					regnum++;
+					break;
 				}
 
 				/* REVISIT error checks */
@@ -460,8 +464,8 @@ int arm_dpm_write_dirty_registers(struct arm_dpm *dpm, bool bpwp)
 				continue;
 
 			retval = dpm_write_reg(dpm,
-					&cache->reg_list[i],
-					regnum);
+					       &cache->reg_list[i],
+					       regnum);
 			if (retval != ERROR_OK)
 				goto done;
 		}
@@ -905,6 +909,7 @@ void arm_dpm_report_wfar(struct arm_dpm *dpm, uint32_t addr)
 			addr -= 4;
 			break;
 		case ARM_STATE_JAZELLE:
+		case ARM_STATE_AARCH64:
 			/* ?? */
 			break;
 	}
@@ -925,20 +930,16 @@ void arm_dpm_report_dscr(struct arm_dpm *dpm, uint32_t dscr)
 
 	/* Examine debug reason */
 	switch (DSCR_ENTRY(dscr)) {
-		case 6:	/* Data abort (v6 only) */
-		case 7:	/* Prefetch abort (v6 only) */
-		/* FALL THROUGH -- assume a v6 core in abort mode */
-		case 0:	/* HALT request from debugger */
-		case 4:	/* EDBGRQ */
+		case DSCR_ENTRY_HALT_REQ:	/* HALT request from debugger */
+		case DSCR_ENTRY_EXT_DBG_REQ:	/* EDBGRQ */
 			target->debug_reason = DBG_REASON_DBGRQ;
 			break;
-		case 1:	/* HW breakpoint */
-		case 3:	/* SW BKPT */
-		case 5:	/* vector catch */
+		case DSCR_ENTRY_BREAKPOINT:	/* HW breakpoint */
+		case DSCR_ENTRY_BKPT_INSTR:	/* vector catch */
 			target->debug_reason = DBG_REASON_BREAKPOINT;
 			break;
-		case 2:	/* asynch watchpoint */
-		case 10:/* precise watchpoint */
+		case DSCR_ENTRY_IMPRECISE_WATCHPT:	/* asynch watchpoint */
+		case DSCR_ENTRY_PRECISE_WATCHPT:/* precise watchpoint */
 			target->debug_reason = DBG_REASON_WATCHPOINT;
 			break;
 		default:
@@ -963,7 +964,7 @@ int arm_dpm_setup(struct arm_dpm *dpm)
 {
 	struct arm *arm = dpm->arm;
 	struct target *target = arm->target;
-	struct reg_cache *cache;
+	struct reg_cache *cache = 0;
 
 	arm->dpm = dpm;
 
@@ -972,11 +973,13 @@ int arm_dpm_setup(struct arm_dpm *dpm)
 	arm->read_core_reg = arm_dpm_read_core_reg;
 	arm->write_core_reg = arm_dpm_write_core_reg;
 
-	cache = arm_build_reg_cache(target, arm);
-	if (!cache)
-		return ERROR_FAIL;
+	if (arm->core_cache == NULL) {
+		cache = arm_build_reg_cache(target, arm);
+		if (!cache)
+			return ERROR_FAIL;
 
-	*register_get_last_cache_p(&target->reg_cache) = cache;
+		*register_get_last_cache_p(&target->reg_cache) = cache;
+	}
 
 	/* coprocessor access setup */
 	arm->mrc = dpm_mrc;
@@ -995,9 +998,8 @@ int arm_dpm_setup(struct arm_dpm *dpm)
 	/* FIXME add vector catch support */
 
 	dpm->nbp = 1 + ((dpm->didr >> 24) & 0xf);
-	dpm->dbp = calloc(dpm->nbp, sizeof *dpm->dbp);
-
 	dpm->nwp = 1 + ((dpm->didr >> 28) & 0xf);
+	dpm->dbp = calloc(dpm->nbp, sizeof *dpm->dbp);
 	dpm->dwp = calloc(dpm->nwp, sizeof *dpm->dwp);
 
 	if (!dpm->dbp || !dpm->dwp) {
diff --git a/src/target/arm_dpm.h b/src/target/arm_dpm.h
index fa87baf..f8d1248 100644
--- a/src/target/arm_dpm.h
+++ b/src/target/arm_dpm.h
@@ -59,7 +59,7 @@ struct arm_dpm {
 	struct arm *arm;
 
 	/** Cache of DIDR */
-	uint32_t didr;
+	uint64_t didr;
 
 	/** Invoke before a series of instruction operations */
 	int (*prepare)(struct arm_dpm *);
@@ -67,16 +67,26 @@ struct arm_dpm {
 	/** Invoke after a series of instruction operations */
 	int (*finish)(struct arm_dpm *);
 
+	/** Runs one instruction. */
+	int (*instr_execute)(struct arm_dpm *, uint32_t opcode);
+
 	/* WRITE TO CPU */
 
 	/** Runs one instruction, writing data to DCC before execution. */
 	int (*instr_write_data_dcc)(struct arm_dpm *,
 			uint32_t opcode, uint32_t data);
 
+	int (*instr_write_data_dcc_64)(struct arm_dpm *,
+			uint32_t opcode, uint64_t data);
+
 	/** Runs one instruction, writing data to R0 before execution. */
 	int (*instr_write_data_r0)(struct arm_dpm *,
 			uint32_t opcode, uint32_t data);
 
+	/** Runs one instruction, writing data to R0 before execution. */
+	int (*instr_write_data_r0_64)(struct arm_dpm *,
+			uint32_t opcode, uint64_t data);
+
 	/** Optional core-specific operation invoked after CPSR writes. */
 	int (*instr_cpsr_sync)(struct arm_dpm *dpm);
 
@@ -86,10 +96,19 @@ struct arm_dpm {
 	int (*instr_read_data_dcc)(struct arm_dpm *,
 			uint32_t opcode, uint32_t *data);
 
+	int (*instr_read_data_dcc_64)(struct arm_dpm *,
+			uint32_t opcode, uint64_t *data);
+
 	/** Runs one instruction, reading data from r0 after execution. */
 	int (*instr_read_data_r0)(struct arm_dpm *,
 			uint32_t opcode, uint32_t *data);
 
+	int (*instr_read_data_r0_64)(struct arm_dpm *,
+			uint32_t opcode, uint64_t *data);
+
+	struct reg *(*arm_reg_current)(struct arm *arm,
+			unsigned regnum);
+
 	/* BREAKPOINT/WATCHPOINT SUPPORT */
 
 	/**
@@ -119,11 +138,14 @@ struct arm_dpm {
 	struct dpm_wp *dwp;
 
 	/** Address of the instruction which triggered a watchpoint. */
-	uint32_t wp_pc;
+	target_addr_t wp_pc;
 
 	/** Recent value of DSCR. */
 	uint32_t dscr;
 
+	/** Recent exception level on armv8 */
+	unsigned int last_el;
+
 	/* FIXME -- read/write DCSR methods and symbols */
 };
 
@@ -133,7 +155,6 @@ int arm_dpm_initialize(struct arm_dpm *dpm);
 int arm_dpm_read_current_registers(struct arm_dpm *);
 int dpm_modeswitch(struct arm_dpm *dpm, enum arm_mode mode);
 
-
 int arm_dpm_write_dirty_registers(struct arm_dpm *, bool bpwp);
 
 void arm_dpm_report_wfar(struct arm_dpm *, uint32_t wfar);
@@ -166,21 +187,21 @@ void arm_dpm_report_wfar(struct arm_dpm *, uint32_t wfar);
 #define DSCR_DTR_TX_FULL            (0x1 << 29)
 #define DSCR_DTR_RX_FULL            (0x1 << 30) /* bit 31 is reserved */
 
-#define DSCR_ENTRY(dscr)            (((dscr) >> 2) & 0xf)
-#define DSCR_RUN_MODE(dscr)         ((dscr) & (DSCR_CORE_HALTED | DSCR_CORE_RESTARTED))
+#define DSCR_ENTRY(dscr)            ((dscr) & 0x3f)
+#define DSCR_RUN_MODE(dscr)         ((dscr) & 0x03)
 
 
 /* Methods of entry into debug mode */
-#define DSCR_ENTRY_HALT_REQ           (0x0 << 2)
-#define DSCR_ENTRY_BREAKPOINT         (0x1 << 2)
-#define DSCR_ENTRY_IMPRECISE_WATCHPT  (0x2 << 2)
-#define DSCR_ENTRY_BKPT_INSTR         (0x3 << 2)
-#define DSCR_ENTRY_EXT_DBG_REQ        (0x4 << 2)
-#define DSCR_ENTRY_VECT_CATCH         (0x5 << 2)
-#define DSCR_ENTRY_D_SIDE_ABORT       (0x6 << 2)  /* v6 only */
-#define DSCR_ENTRY_I_SIDE_ABORT       (0x7 << 2)  /* v6 only */
-#define DSCR_ENTRY_OS_UNLOCK          (0x8 << 2)
-#define DSCR_ENTRY_PRECISE_WATCHPT    (0xA << 2)
+#define DSCR_ENTRY_HALT_REQ           (0x03)
+#define DSCR_ENTRY_BREAKPOINT         (0x07)
+#define DSCR_ENTRY_IMPRECISE_WATCHPT  (0x0B)
+#define DSCR_ENTRY_BKPT_INSTR         (0x0F)
+#define DSCR_ENTRY_EXT_DBG_REQ        (0x13)
+#define DSCR_ENTRY_VECT_CATCH         (0x17)
+#define DSCR_ENTRY_D_SIDE_ABORT       (0x1B)  /* v6 only */
+#define DSCR_ENTRY_I_SIDE_ABORT       (0x1F)  /* v6 only */
+#define DSCR_ENTRY_OS_UNLOCK          (0x23)
+#define DSCR_ENTRY_PRECISE_WATCHPT    (0x2B)
 
 /* DTR modes */
 #define DSCR_EXT_DCC_NON_BLOCKING     (0x0 << 20)
@@ -198,4 +219,25 @@ void arm_dpm_report_wfar(struct arm_dpm *, uint32_t wfar);
 
 void arm_dpm_report_dscr(struct arm_dpm *dpm, uint32_t dcsr);
 
+/* PRCR (Device Power-down and Reset Control Register) bits */
+#define PRCR_DEBUG_NO_POWER_DOWN         (1 << 0)
+#define PRCR_WARM_RESET                  (1 << 1)
+#define PRCR_HOLD_NON_DEBUG_RESET        (1 << 2)
+
+/* PRSR (Device Power-down and Reset Status Register) bits */
+#define PRSR_POWERUP_STATUS              (1 << 0)
+#define PRSR_STICKY_POWERDOWN_STATUS     (1 << 1)
+#define PRSR_RESET_STATUS                (1 << 2)
+#define PRSR_STICKY_RESET_STATUS         (1 << 3)
+#define PRSR_HALTED                      (1 << 4)  /* v7.1 Debug only */
+#define PRSR_OSLK                        (1 << 5)  /* v7.1 Debug only */
+#define PRSR_DLK                         (1 << 6)  /* v7.1 Debug only */
+
+/* OSLSR (OS Lock Status Register) bits */
+#define OSLSR_OSLM0                      (1 << 0)
+#define OSLSR_OSLK                       (1 << 1)
+#define OSLSR_nTT                        (1 << 2)
+#define OSLSR_OSLM1                      (1 << 3)
+#define OSLSR_OSLM                       (OSLSR_OSLM0|OSLSR_OSLM1)
+
 #endif /* OPENOCD_TARGET_ARM_DPM_H */
diff --git a/src/target/arm_semihosting.c b/src/target/arm_semihosting.c
index 2fd6580..2525119 100644
--- a/src/target/arm_semihosting.c
+++ b/src/target/arm_semihosting.c
@@ -5,6 +5,9 @@
  *   Copyright (C) 2010 by Spencer Oliver                                  *
  *   spen@spen-soft.co.uk                                                  *
  *                                                                         *
+ *   Copyright (C) 2016 by Square, Inc.                                    *
+ *   Steven Stallion <stallion@squareup.com>                               *
+ *                                                                         *
  *   This program is free software; you can redistribute it and/or modify  *
  *   it under the terms of the GNU General Public License as published by  *
  *   the Free Software Foundation; either version 2 of the License, or     *
@@ -39,8 +42,11 @@
 #include "armv4_5.h"
 #include "arm7_9_common.h"
 #include "armv7m.h"
+#include "armv7a.h"
 #include "cortex_m.h"
 #include "register.h"
+#include "arm_opcodes.h"
+#include "target_type.h"
 #include "arm_semihosting.h"
 #include <helper/binarybuffer.h>
 #include <helper/log.h>
@@ -61,13 +67,59 @@ static const int open_modeflags[12] = {
 	O_RDWR | O_CREAT | O_APPEND | O_BINARY
 };
 
+static int post_result(struct target *target)
+{
+	struct arm *arm = target_to_arm(target);
+
+	/* REVISIT this looks wrong ... ARM11 and Cortex-A8
+	 * should work this way at least sometimes.
+	 */
+	if (is_arm7_9(target_to_arm7_9(target)) ||
+	    is_armv7a(target_to_armv7a(target))) {
+		uint32_t spsr;
+
+		/* return value in R0 */
+		buf_set_u32(arm->core_cache->reg_list[0].value, 0, 32, arm->semihosting_result);
+		arm->core_cache->reg_list[0].dirty = 1;
+
+		/* LR --> PC */
+		buf_set_u32(arm->core_cache->reg_list[15].value, 0, 32,
+			buf_get_u32(arm_reg_current(arm, 14)->value, 0, 32));
+		arm->core_cache->reg_list[15].dirty = 1;
+
+		/* saved PSR --> current PSR */
+		spsr = buf_get_u32(arm->spsr->value, 0, 32);
+
+		/* REVISIT should this be arm_set_cpsr(arm, spsr)
+		 * instead of a partially unrolled version?
+		 */
+
+		buf_set_u32(arm->cpsr->value, 0, 32, spsr);
+		arm->cpsr->dirty = 1;
+		arm->core_mode = spsr & 0x1f;
+		if (spsr & 0x20)
+			arm->core_state = ARM_STATE_THUMB;
+
+	} else {
+		/* resume execution, this will be pc+2 to skip over the
+		 * bkpt instruction */
+
+		/* return result in R0 */
+		buf_set_u32(arm->core_cache->reg_list[0].value, 0, 32, arm->semihosting_result);
+		arm->core_cache->reg_list[0].dirty = 1;
+	}
+
+	return ERROR_OK;
+}
+
 static int do_semihosting(struct target *target)
 {
 	struct arm *arm = target_to_arm(target);
+	struct gdb_fileio_info *fileio_info = target->fileio_info;
 	uint32_t r0 = buf_get_u32(arm->core_cache->reg_list[0].value, 0, 32);
 	uint32_t r1 = buf_get_u32(arm->core_cache->reg_list[1].value, 0, 32);
 	uint8_t params[16];
-	int retval, result;
+	int retval;
 
 	/*
 	 * TODO: lots of security issues are not considered yet, such as:
@@ -75,10 +127,10 @@ static int do_semihosting(struct target *target)
 	 * - no safety checks on opened/deleted/renamed file paths
 	 * Beware the target app you use this support with.
 	 *
-	 * TODO: explore mapping requests to GDB's "File-I/O Remote
-	 * Protocol Extension" ... when GDB is active.
+	 * TODO: unsupported semihosting fileio operations could be
+	 * implemented if we had a small working area at our disposal.
 	 */
-	switch (r0) {
+	switch ((arm->semihosting_op = r0)) {
 	case 0x01:	/* SYS_OPEN */
 		retval = target_read_memory(target, r1, 4, 3, params);
 		if (retval != ERROR_OK)
@@ -87,27 +139,40 @@ static int do_semihosting(struct target *target)
 			uint32_t a = target_buffer_get_u32(target, params+0);
 			uint32_t m = target_buffer_get_u32(target, params+4);
 			uint32_t l = target_buffer_get_u32(target, params+8);
-			if (l <= 255 && m <= 11) {
-				uint8_t fn[256];
-				retval = target_read_memory(target, a, 1, l, fn);
-				if (retval != ERROR_OK)
-					return retval;
-				fn[l] = 0;
-				if (strcmp((char *)fn, ":tt") == 0) {
-					if (m < 4)
-						result = dup(STDIN_FILENO);
-					else
-						result = dup(STDOUT_FILENO);
-				} else {
-					/* cygwin requires the permission setting
-					 * otherwise it will fail to reopen a previously
-					 * written file */
-					result = open((char *)fn, open_modeflags[m], 0644);
+			uint8_t fn[256];
+			retval = target_read_memory(target, a, 1, l, fn);
+			if (retval != ERROR_OK)
+				return retval;
+			fn[l] = 0;
+			if (arm->is_semihosting_fileio) {
+				if (strcmp((char *)fn, ":tt") == 0)
+					arm->semihosting_result = 0;
+				else {
+					arm->semihosting_hit_fileio = true;
+					fileio_info->identifier = "open";
+					fileio_info->param_1 = a;
+					fileio_info->param_2 = l;
+					fileio_info->param_3 = open_modeflags[m];
+					fileio_info->param_4 = 0644;
 				}
-				arm->semihosting_errno =  errno;
 			} else {
-				result = -1;
-				arm->semihosting_errno = EINVAL;
+				if (l <= 255 && m <= 11) {
+					if (strcmp((char *)fn, ":tt") == 0) {
+						if (m < 4)
+							arm->semihosting_result = dup(STDIN_FILENO);
+						else
+							arm->semihosting_result = dup(STDOUT_FILENO);
+					} else {
+						/* cygwin requires the permission setting
+						 * otherwise it will fail to reopen a previously
+						 * written file */
+						arm->semihosting_result = open((char *)fn, open_modeflags[m], 0644);
+					}
+					arm->semihosting_errno =  errno;
+				} else {
+					arm->semihosting_result = -1;
+					arm->semihosting_errno = EINVAL;
+				}
 			}
 		}
 		break;
@@ -118,33 +183,63 @@ static int do_semihosting(struct target *target)
 			return retval;
 		else {
 			int fd = target_buffer_get_u32(target, params+0);
-			result = close(fd);
-			arm->semihosting_errno = errno;
+			if (arm->is_semihosting_fileio) {
+				arm->semihosting_hit_fileio = true;
+				fileio_info->identifier = "close";
+				fileio_info->param_1 = fd;
+			} else {
+				arm->semihosting_result = close(fd);
+				arm->semihosting_errno = errno;
+			}
 		}
 		break;
 
 	case 0x03:	/* SYS_WRITEC */
-		{
+		if (arm->is_semihosting_fileio) {
+			arm->semihosting_hit_fileio = true;
+			fileio_info->identifier = "write";
+			fileio_info->param_1 = 1;
+			fileio_info->param_2 = r1;
+			fileio_info->param_3 = 1;
+		} else {
 			unsigned char c;
 			retval = target_read_memory(target, r1, 1, 1, &c);
 			if (retval != ERROR_OK)
 				return retval;
 			putchar(c);
-			result = 0;
+			arm->semihosting_result = 0;
 		}
 		break;
 
 	case 0x04:	/* SYS_WRITE0 */
-		do {
-			unsigned char c;
-			retval = target_read_memory(target, r1++, 1, 1, &c);
-			if (retval != ERROR_OK)
-				return retval;
-			if (!c)
-				break;
-			putchar(c);
-		} while (1);
-		result = 0;
+		if (arm->is_semihosting_fileio) {
+			size_t count = 0;
+			for (uint32_t a = r1;; a++) {
+				unsigned char c;
+				retval = target_read_memory(target, a, 1, 1, &c);
+				if (retval != ERROR_OK)
+					return retval;
+				if (c == '\0')
+					break;
+				count++;
+			}
+			arm->semihosting_hit_fileio = true;
+			fileio_info->identifier = "write";
+			fileio_info->param_1 = 1;
+			fileio_info->param_2 = r1;
+			fileio_info->param_3 = count;
+		} else {
+			do {
+				unsigned char c;
+				retval = target_read_memory(target, r1++, 1, 1, &c);
+				if (retval != ERROR_OK)
+					return retval;
+				if (!c)
+					break;
+				putchar(c);
+			} while (1);
+			arm->semihosting_result = 0;
+		}
 		break;
 
 	case 0x05:	/* SYS_WRITE */
@@ -155,21 +250,29 @@ static int do_semihosting(struct target *target)
 			int fd = target_buffer_get_u32(target, params+0);
 			uint32_t a = target_buffer_get_u32(target, params+4);
 			size_t l = target_buffer_get_u32(target, params+8);
-			uint8_t *buf = malloc(l);
-			if (!buf) {
-				result = -1;
-				arm->semihosting_errno = ENOMEM;
+			if (arm->is_semihosting_fileio) {
+				arm->semihosting_hit_fileio = true;
+				fileio_info->identifier = "write";
+				fileio_info->param_1 = fd;
+				fileio_info->param_2 = a;
+				fileio_info->param_3 = l;
 			} else {
-				retval = target_read_buffer(target, a, l, buf);
-				if (retval != ERROR_OK) {
+				uint8_t *buf = malloc(l);
+				if (!buf) {
+					arm->semihosting_result = -1;
+					arm->semihosting_errno = ENOMEM;
+				} else {
+					retval = target_read_buffer(target, a, l, buf);
+					if (retval != ERROR_OK) {
+						free(buf);
+						return retval;
+					}
+					arm->semihosting_result = write(fd, buf, l);
+					arm->semihosting_errno = errno;
+					if (arm->semihosting_result >= 0)
+						arm->semihosting_result = l - arm->semihosting_result;
 					free(buf);
-					return retval;
 				}
-				result = write(fd, buf, l);
-				arm->semihosting_errno = errno;
-				if (result >= 0)
-					result = l - result;
-				free(buf);
 			}
 		}
 		break;
@@ -182,42 +285,60 @@ static int do_semihosting(struct target *target)
 			int fd = target_buffer_get_u32(target, params+0);
 			uint32_t a = target_buffer_get_u32(target, params+4);
 			ssize_t l = target_buffer_get_u32(target, params+8);
-			uint8_t *buf = malloc(l);
-			if (!buf) {
-				result = -1;
-				arm->semihosting_errno = ENOMEM;
+			if (arm->is_semihosting_fileio) {
+				arm->semihosting_hit_fileio = true;
+				fileio_info->identifier = "read";
+				fileio_info->param_1 = fd;
+				fileio_info->param_2 = a;
+				fileio_info->param_3 = l;
 			} else {
-				result = read(fd, buf, l);
-				arm->semihosting_errno = errno;
-				if (result >= 0) {
-					retval = target_write_buffer(target, a, result, buf);
-					if (retval != ERROR_OK) {
-						free(buf);
-						return retval;
+				uint8_t *buf = malloc(l);
+				if (!buf) {
+					arm->semihosting_result = -1;
+					arm->semihosting_errno = ENOMEM;
+				} else {
+					arm->semihosting_result = read(fd, buf, l);
+					arm->semihosting_errno = errno;
+					if (arm->semihosting_result >= 0) {
+						retval = target_write_buffer(target, a, arm->semihosting_result, buf);
+						if (retval != ERROR_OK) {
+							free(buf);
+							return retval;
+						}
+						arm->semihosting_result = l - arm->semihosting_result;
 					}
-					result = l - result;
+					free(buf);
 				}
-				free(buf);
 			}
 		}
 		break;
 
 	case 0x07:	/* SYS_READC */
-		result = getchar();
+		if (arm->is_semihosting_fileio) {
+			LOG_ERROR("SYS_READC not supported by semihosting fileio");
+			return ERROR_FAIL;
+		}
+		arm->semihosting_result = getchar();
 		break;
 
 	case 0x08:	/* SYS_ISERROR */
 		retval = target_read_memory(target, r1, 4, 1, params);
 		if (retval != ERROR_OK)
 			return retval;
-		result = (target_buffer_get_u32(target, params+0) != 0);
+		arm->semihosting_result = (target_buffer_get_u32(target, params+0) != 0);
 		break;
 
 	case 0x09:	/* SYS_ISTTY */
-		retval = target_read_memory(target, r1, 4, 1, params);
-		if (retval != ERROR_OK)
-			return retval;
-		result = isatty(target_buffer_get_u32(target, params+0));
+		if (arm->is_semihosting_fileio) {
+			arm->semihosting_hit_fileio = true;
+			fileio_info->identifier = "isatty";
+			fileio_info->param_1 = r1;
+		} else {
+			retval = target_read_memory(target, r1, 4, 1, params);
+			if (retval != ERROR_OK)
+				return retval;
+			arm->semihosting_result = isatty(target_buffer_get_u32(target, params+0));
+		}
 		break;
 
 	case 0x0a:	/* SYS_SEEK */
@@ -227,27 +348,39 @@ static int do_semihosting(struct target *target)
 		else {
 			int fd = target_buffer_get_u32(target, params+0);
 			off_t pos = target_buffer_get_u32(target, params+4);
-			result = lseek(fd, pos, SEEK_SET);
-			arm->semihosting_errno = errno;
-			if (result == pos)
-				result = 0;
+			if (arm->is_semihosting_fileio) {
+				arm->semihosting_hit_fileio = true;
+				fileio_info->identifier = "lseek";
+				fileio_info->param_1 = fd;
+				fileio_info->param_2 = pos;
+				fileio_info->param_3 = SEEK_SET;
+			} else {
+				arm->semihosting_result = lseek(fd, pos, SEEK_SET);
+				arm->semihosting_errno = errno;
+				if (arm->semihosting_result == pos)
+					arm->semihosting_result = 0;
+			}
 		}
 		break;
 
 	case 0x0c:	/* SYS_FLEN */
+		if (arm->is_semihosting_fileio) {
+			LOG_ERROR("SYS_FLEN not supported by semihosting fileio");
+			return ERROR_FAIL;
+		}
 		retval = target_read_memory(target, r1, 4, 1, params);
 		if (retval != ERROR_OK)
 			return retval;
 		else {
 			int fd = target_buffer_get_u32(target, params+0);
 			struct stat buf;
-			result = fstat(fd, &buf);
-			if (result == -1) {
+			arm->semihosting_result = fstat(fd, &buf);
+			if (arm->semihosting_result == -1) {
 				arm->semihosting_errno = errno;
-				result = -1;
+				arm->semihosting_result = -1;
 				break;
 			}
-			result = buf.st_size;
+			arm->semihosting_result = buf.st_size;
 		}
 		break;
 
@@ -258,17 +391,24 @@ 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);
-			if (l <= 255) {
-				uint8_t fn[256];
-				retval = target_read_memory(target, a, 1, l, fn);
-				if (retval != ERROR_OK)
-					return retval;
-				fn[l] = 0;
-				result = remove((char *)fn);
-				arm->semihosting_errno =  errno;
+			if (arm->is_semihosting_fileio) {
+				arm->semihosting_hit_fileio = true;
+				fileio_info->identifier = "unlink";
+				fileio_info->param_1 = a;
+				fileio_info->param_2 = l;
 			} else {
-				result = -1;
-				arm->semihosting_errno = EINVAL;
+				if (l <= 255) {
+					uint8_t fn[256];
+					retval = target_read_memory(target, a, 1, l, fn);
+					if (retval != ERROR_OK)
+						return retval;
+					fn[l] = 0;
+					arm->semihosting_result = remove((char *)fn);
+					arm->semihosting_errno =  errno;
+				} else {
+					arm->semihosting_result = -1;
+					arm->semihosting_errno = EINVAL;
+				}
 			}
 		}
 		break;
@@ -282,31 +422,40 @@ static int do_semihosting(struct target *target)
 			uint32_t l1 = target_buffer_get_u32(target, params+4);
 			uint32_t a2 = target_buffer_get_u32(target, params+8);
 			uint32_t l2 = target_buffer_get_u32(target, params+12);
-			if (l1 <= 255 && l2 <= 255) {
-				uint8_t fn1[256], fn2[256];
-				retval = target_read_memory(target, a1, 1, l1, fn1);
-				if (retval != ERROR_OK)
-					return retval;
-				retval = target_read_memory(target, a2, 1, l2, fn2);
-				if (retval != ERROR_OK)
-					return retval;
-				fn1[l1] = 0;
-				fn2[l2] = 0;
-				result = rename((char *)fn1, (char *)fn2);
-				arm->semihosting_errno =  errno;
+			if (arm->is_semihosting_fileio) {
+				arm->semihosting_hit_fileio = true;
+				fileio_info->identifier = "rename";
+				fileio_info->param_1 = a1;
+				fileio_info->param_2 = l1;
+				fileio_info->param_3 = a2;
+				fileio_info->param_4 = l2;
 			} else {
-				result = -1;
-				arm->semihosting_errno = EINVAL;
+				if (l1 <= 255 && l2 <= 255) {
+					uint8_t fn1[256], fn2[256];
+					retval = target_read_memory(target, a1, 1, l1, fn1);
+					if (retval != ERROR_OK)
+						return retval;
+					retval = target_read_memory(target, a2, 1, l2, fn2);
+					if (retval != ERROR_OK)
+						return retval;
+					fn1[l1] = 0;
+					fn2[l2] = 0;
+					arm->semihosting_result = rename((char *)fn1, (char *)fn2);
+					arm->semihosting_errno =  errno;
+				} else {
+					arm->semihosting_result = -1;
+					arm->semihosting_errno = EINVAL;
+				}
 			}
 		}
 		break;
 
 	case 0x11:	/* SYS_TIME */
-		result = time(NULL);
+		arm->semihosting_result = time(NULL);
 		break;
 
 	case 0x13:	/* SYS_ERRNO */
-		result = arm->semihosting_errno;
+		arm->semihosting_result = arm->semihosting_errno;
 		break;
 
 	case 0x15:	/* SYS_GET_CMDLINE */
@@ -319,12 +468,12 @@ static int do_semihosting(struct target *target)
 			char *arg = "foobar";
 			uint32_t s = strlen(arg) + 1;
 			if (l < s)
-				result = -1;
+				arm->semihosting_result = -1;
 			else {
 				retval = target_write_buffer(target, a, s, (uint8_t *)arg);
 				if (retval != ERROR_OK)
 					return retval;
-				result = 0;
+				arm->semihosting_result = 0;
 			}
 		}
 		break;
@@ -340,7 +489,7 @@ static int do_semihosting(struct target *target)
 			retval = target_write_memory(target, a, 4, 4, params);
 			if (retval != ERROR_OK)
 				return retval;
-			result = 0;
+			arm->semihosting_result = 0;
 		}
 		break;
 
@@ -384,17 +533,24 @@ static int do_semihosting(struct target *target)
 		else {
 			uint32_t len = target_buffer_get_u32(target, params+4);
 			uint32_t c_ptr = target_buffer_get_u32(target, params);
-			uint8_t cmd[256];
-			if (len > 255) {
-				result = -1;
-				arm->semihosting_errno = EINVAL;
+			if (arm->is_semihosting_fileio) {
+				arm->semihosting_hit_fileio = true;
+				fileio_info->identifier = "system";
+				fileio_info->param_1 = c_ptr;
+				fileio_info->param_2 = len;
 			} else {
-				memset(cmd, 0x0, 256);
-				retval = target_read_memory(target, c_ptr, 1, len, cmd);
-				if (retval != ERROR_OK)
-					return retval;
-				else
-					result = system((const char *)cmd);
+				uint8_t cmd[256];
+				if (len > 255) {
+					arm->semihosting_result = -1;
+					arm->semihosting_errno = EINVAL;
+				} else {
+					memset(cmd, 0x0, 256);
+					retval = target_read_memory(target, c_ptr, 1, len, cmd);
+					if (retval != ERROR_OK)
+						return retval;
+					else
+						arm->semihosting_result = system((const char *)cmd);
+				}
 			}
 		}
 		break;
@@ -406,50 +562,84 @@ static int do_semihosting(struct target *target)
 	default:
 		fprintf(stderr, "semihosting: unsupported call %#x\n",
 				(unsigned) r0);
-		result = -1;
+		arm->semihosting_result = -1;
 		arm->semihosting_errno = ENOTSUP;
 	}
 
-	/* resume execution to the original mode */
+	return ERROR_OK;
+}
 
-	/* REVISIT this looks wrong ... ARM11 and Cortex-A8
-	 * should work this way at least sometimes.
+static int get_gdb_fileio_info(struct target *target, struct gdb_fileio_info *fileio_info)
+{
+	struct arm *arm = target_to_arm(target);
+
+	/* To avoid uneccessary duplication, semihosting prepares the
+	 * fileio_info structure out-of-band when the target halts. See
+	 * do_semihosting for more detail.
 	 */
-	if (is_arm7_9(target_to_arm7_9(target))) {
-		uint32_t spsr;
+	if (!arm->is_semihosting_fileio || !arm->semihosting_hit_fileio)
+		return ERROR_FAIL;
 
-		/* return value in R0 */
-		buf_set_u32(arm->core_cache->reg_list[0].value, 0, 32, result);
-		arm->core_cache->reg_list[0].dirty = 1;
+	return ERROR_OK;
+}
 
-		/* LR --> PC */
-		buf_set_u32(arm->core_cache->reg_list[15].value, 0, 32,
-			buf_get_u32(arm_reg_current(arm, 14)->value, 0, 32));
-		arm->core_cache->reg_list[15].dirty = 1;
+static int gdb_fileio_end(struct target *target, int result, int fileio_errno, bool ctrl_c)
+{
+	struct arm *arm = target_to_arm(target);
+	struct gdb_fileio_info *fileio_info = target->fileio_info;
 
-		/* saved PSR --> current PSR */
-		spsr = buf_get_u32(arm->spsr->value, 0, 32);
+	/* clear pending status */
+	arm->semihosting_hit_fileio = false;
 
-		/* REVISIT should this be arm_set_cpsr(arm, spsr)
-		 * instead of a partially unrolled version?
-		 */
+	arm->semihosting_result = result;
+	arm->semihosting_errno = fileio_errno;
 
-		buf_set_u32(arm->cpsr->value, 0, 32, spsr);
-		arm->cpsr->dirty = 1;
-		arm->core_mode = spsr & 0x1f;
-		if (spsr & 0x20)
-			arm->core_state = ARM_STATE_THUMB;
+	/* Some fileio results do not match up with what the semihosting
+	 * operation expects; for these operations, we munge the results
+	 * below:
+	 */
+	switch (arm->semihosting_op) {
+	case 0x05:	/* SYS_WRITE */
+		if (result < 0)
+			arm->semihosting_result = fileio_info->param_3;
+		else
+			arm->semihosting_result = 0;
+		break;
 
-	} else {
-		/* resume execution, this will be pc+2 to skip over the
-		 * bkpt instruction */
+	case 0x06:	/* SYS_READ */
+		if (result == (int)fileio_info->param_3)
+			arm->semihosting_result = 0;
+		if (result <= 0)
+			arm->semihosting_result = fileio_info->param_3;
+		break;
 
-		/* return result in R0 */
-		buf_set_u32(arm->core_cache->reg_list[0].value, 0, 32, result);
-		arm->core_cache->reg_list[0].dirty = 1;
+	case 0x0a:	/* SYS_SEEK */
+		if (result > 0)
+			arm->semihosting_result = 0;
+		break;
 	}
 
-	return target_resume(target, 1, 0, 0, 0);
+	return post_result(target);
+}
+
+/**
+ * Initialize ARM semihosting support.
+ *
+ * @param target Pointer to the ARM target to initialize.
+ * @return An error status if there is a problem during initialization.
+ */
+int arm_semihosting_init(struct target *target)
+{
+	target->fileio_info = malloc(sizeof(*target->fileio_info));
+	if (target->fileio_info == NULL) {
+		LOG_ERROR("out of memory");
+		return ERROR_FAIL;
+	}
+
+	target->type->get_gdb_fileio_info = get_gdb_fileio_info;
+	target->type->gdb_fileio_end = gdb_fileio_end;
+
+	return ERROR_OK;
 }
 
 /**
@@ -468,20 +658,42 @@ static int do_semihosting(struct target *target)
 int arm_semihosting(struct target *target, int *retval)
 {
 	struct arm *arm = target_to_arm(target);
+	struct armv7a_common *armv7a = target_to_armv7a(target);
 	uint32_t pc, lr, spsr;
 	struct reg *r;
 
 	if (!arm->is_semihosting)
 		return 0;
 
-	if (is_arm7_9(target_to_arm7_9(target))) {
+	if (is_arm7_9(target_to_arm7_9(target)) ||
+	    is_armv7a(armv7a)) {
+		uint32_t vbar = 0x00000000;
+
 		if (arm->core_mode != ARM_MODE_SVC)
 			return 0;
 
+		if (is_armv7a(armv7a)) {
+			struct arm_dpm *dpm = armv7a->arm.dpm;
+
+			*retval = dpm->prepare(dpm);
+			if (*retval == ERROR_OK) {
+				*retval = dpm->instr_read_data_r0(dpm,
+								 ARMV4_5_MRC(15, 0, 0, 12, 0, 0),
+								 &vbar);
+
+				dpm->finish(dpm);
+
+				if (*retval != ERROR_OK)
+					return 1;
+			} else {
+				return 1;
+			}
+		}
+
 		/* Check for PC == 0x00000008 or 0xffff0008: Supervisor Call vector. */
 		r = arm->pc;
 		pc = buf_get_u32(r->value, 0, 32);
-		if (pc != 0x00000008 && pc != 0xffff0008)
+		if (pc != (vbar + 0x00000008) && pc != 0xffff0008)
 			return 0;
 
 		r = arm_reg_current(arm, 14);
@@ -551,6 +763,35 @@ int arm_semihosting(struct target *target, int *retval)
 		return 0;
 	}
 
-	*retval = do_semihosting(target);
-	return 1;
+	/* Perform semihosting if we are not waiting on a fileio
+	 * operation to complete.
+	 */
+	if (!arm->semihosting_hit_fileio) {
+		*retval = do_semihosting(target);
+		if (*retval != ERROR_OK) {
+			LOG_ERROR("Failed semihosting operation");
+			return 0;
+		}
+	}
+
+	/* Post result to target if we are not waiting on a fileio
+	 * operation to complete:
+	 */
+	if (!arm->semihosting_hit_fileio) {
+		*retval = post_result(target);
+		if (*retval != ERROR_OK) {
+			LOG_ERROR("Failed to post semihosting result");
+			return 0;
+		}
+
+		*retval = target_resume(target, 1, 0, 0, 0);
+		if (*retval != ERROR_OK) {
+			LOG_ERROR("Failed to resume target");
+			return 0;
+		}
+
+		return 1;
+	}
+
+	return 0;
 }
diff --git a/src/target/arm_semihosting.h b/src/target/arm_semihosting.h
index 7b5c0b2..011f19f 100644
--- a/src/target/arm_semihosting.h
+++ b/src/target/arm_semihosting.h
@@ -19,6 +19,7 @@
 #ifndef OPENOCD_TARGET_ARM_SEMIHOSTING_H
 #define OPENOCD_TARGET_ARM_SEMIHOSTING_H
 
+int arm_semihosting_init(struct target *target);
 int arm_semihosting(struct target *target, int *retval);
 
 #endif /* OPENOCD_TARGET_ARM_SEMIHOSTING_H */
diff --git a/src/target/armv4_5.c b/src/target/armv4_5.c
index e6ecfc8..2029ca9 100644
--- a/src/target/armv4_5.c
+++ b/src/target/armv4_5.c
@@ -666,14 +666,19 @@ int arm_arch_state(struct target *target)
 		return ERROR_FAIL;
 	}
 
+	/* avoid filling log waiting for fileio reply */
+	if (arm->semihosting_hit_fileio)
+		return ERROR_OK;
+
 	LOG_USER("target halted in %s state due to %s, current mode: %s\n"
-		"cpsr: 0x%8.8" PRIx32 " pc: 0x%8.8" PRIx32 "%s",
+		"cpsr: 0x%8.8" PRIx32 " pc: 0x%8.8" PRIx32 "%s%s",
 		arm_state_strings[arm->core_state],
 		debug_reason_name(target),
 		arm_mode_name(arm->core_mode),
 		buf_get_u32(arm->cpsr->value, 0, 32),
 		buf_get_u32(arm->pc->value, 0, 32),
-		arm->is_semihosting ? ", semihosting" : "");
+		arm->is_semihosting ? ", semihosting" : "",
+		arm->is_semihosting_fileio ? " fileio" : "");
 
 	return ERROR_OK;
 }
@@ -811,7 +816,7 @@ COMMAND_HANDLER(handle_arm_disassemble_command)
 	}
 
 	struct arm *arm = target_to_arm(target);
-	uint32_t address;
+	target_addr_t address;
 	int count = 1;
 	int thumb = 0;
 
@@ -835,7 +840,7 @@ COMMAND_HANDLER(handle_arm_disassemble_command)
 			COMMAND_PARSE_NUMBER(int, CMD_ARGV[1], count);
 		/* FALL THROUGH */
 		case 1:
-			COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], address);
+			COMMAND_PARSE_ADDRESS(CMD_ARGV[0], address);
 			if (address & 0x01) {
 				if (!thumb) {
 					command_print(CMD_CTX, "Disassemble as Thumb");
@@ -1055,6 +1060,37 @@ COMMAND_HANDLER(handle_arm_semihosting_command)
 	return ERROR_OK;
 }
 
+COMMAND_HANDLER(handle_arm_semihosting_fileio_command)
+{
+	struct target *target = get_current_target(CMD_CTX);
+
+	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->is_semihosting) {
+		command_print(CMD_CTX, "semihosting is not enabled");
+		return ERROR_FAIL;
+	}
+
+	if (CMD_ARGC > 0)
+		COMMAND_PARSE_ENABLE(CMD_ARGV[0], arm->is_semihosting_fileio);
+
+	command_print(CMD_CTX, "semihosting fileio is %s",
+		arm->is_semihosting_fileio
+		? "enabled" : "disabled");
+
+	return ERROR_OK;
+}
+
 static const struct command_registration arm_exec_command_handlers[] = {
 	{
 		.name = "reg",
@@ -1097,6 +1133,13 @@ static const struct command_registration arm_exec_command_handlers[] = {
 		.usage = "['enable'|'disable']",
 		.help = "activate support for semihosting operations",
 	},
+	{
+		"semihosting_fileio",
+		.handler = handle_arm_semihosting_fileio_command,
+		.mode = COMMAND_EXEC,
+		.usage = "['enable'|'disable']",
+		.help = "activate support for semihosting fileio operations",
+	},
 
 	COMMAND_REGISTRATION_DONE
 };
@@ -1391,8 +1434,8 @@ int armv4_5_run_algorithm(struct target *target,
 	struct mem_param *mem_params,
 	int num_reg_params,
 	struct reg_param *reg_params,
-	uint32_t entry_point,
-	uint32_t exit_point,
+	target_addr_t entry_point,
+	target_addr_t exit_point,
 	int timeout_ms,
 	void *arch_info)
 {
@@ -1401,8 +1444,8 @@ int armv4_5_run_algorithm(struct target *target,
 			mem_params,
 			num_reg_params,
 			reg_params,
-			entry_point,
-			exit_point,
+			(uint32_t)entry_point,
+			(uint32_t)exit_point,
 			timeout_ms,
 			arch_info,
 			armv4_5_run_algorithm_completion);
@@ -1413,7 +1456,7 @@ int armv4_5_run_algorithm(struct target *target,
  *
  */
 int arm_checksum_memory(struct target *target,
-	uint32_t address, uint32_t count, uint32_t *checksum)
+	target_addr_t address, uint32_t count, uint32_t *checksum)
 {
 	struct working_area *crc_algorithm;
 	struct arm_algorithm arm_algo;
@@ -1486,7 +1529,7 @@ cleanup:
  *
  */
 int arm_blank_check_memory(struct target *target,
-	uint32_t address, uint32_t count, uint32_t *blank)
+	target_addr_t address, uint32_t count, uint32_t *blank, uint8_t erased_value)
 {
 	struct working_area *check_algorithm;
 	struct reg_param reg_params[3];
@@ -1502,6 +1545,12 @@ int arm_blank_check_memory(struct target *target,
 
 	assert(sizeof(check_code_le) % 4 == 0);
 
+	if (erased_value != 0xff) {
+		LOG_ERROR("Erase value 0x%02" PRIx8 " not yet supported for ARMv4/v5 targets",
+			erased_value);
+		return ERROR_FAIL;
+	}
+
 	/* make sure we have a working area */
 	retval = target_alloc_working_area(target,
 			sizeof(check_code_le), &check_algorithm);
@@ -1529,7 +1578,7 @@ int arm_blank_check_memory(struct target *target,
 	buf_set_u32(reg_params[1].value, 0, 32, count);
 
 	init_reg_param(&reg_params[2], "r2", 32, PARAM_IN_OUT);
-	buf_set_u32(reg_params[2].value, 0, 32, 0xff);
+	buf_set_u32(reg_params[2].value, 0, 32, erased_value);
 
 	/* armv4 must exit using a hardware breakpoint */
 	if (arm->is_armv4)
diff --git a/src/target/armv4_5_mmu.h b/src/target/armv4_5_mmu.h
index 0f52121..7beaf4e 100644
--- a/src/target/armv4_5_mmu.h
+++ b/src/target/armv4_5_mmu.h
@@ -25,8 +25,9 @@ struct target;
 
 struct armv4_5_mmu_common {
 	int (*get_ttb)(struct target *target, uint32_t *result);
-	int (*read_memory)(struct target *target, uint32_t address, uint32_t size, uint32_t count, uint8_t *buffer);
-	int (*write_memory)(struct target *target, uint32_t address, uint32_t size, uint32_t count, const uint8_t *buffer);
+	int (*read_memory)(struct target *target, target_addr_t address, uint32_t size, uint32_t count, uint8_t *buffer);
+	int (*write_memory)(struct target *target, target_addr_t address,
+			    uint32_t size, uint32_t count, const uint8_t *buffer);
 	int (*disable_mmu_caches)(struct target *target, int mmu, int d_u_cache, int i_cache);
 	int (*enable_mmu_caches)(struct target *target, int mmu, int d_u_cache, int i_cache);
 	struct armv4_5_cache_common armv4_5_cache;
diff --git a/src/target/armv7a.c b/src/target/armv7a.c
index 37eb1b5..6021def 100644
--- a/src/target/armv7a.c
+++ b/src/target/armv7a.c
@@ -679,11 +679,40 @@ done:
 
 }
 
+static int armv7a_setup_semihosting(struct target *target, int enable)
+{
+	struct armv7a_common *armv7a = target_to_armv7a(target);
+	uint32_t vcr;
+	int ret;
+
+	ret = mem_ap_read_atomic_u32(armv7a->debug_ap,
+					 armv7a->debug_base + CPUDBG_VCR,
+					 &vcr);
+	if (ret < 0) {
+		LOG_ERROR("Failed to read VCR register\n");
+		return ret;
+	}
+
+	if (enable)
+		vcr |= DBG_VCR_SVC_MASK;
+	else
+		vcr &= ~DBG_VCR_SVC_MASK;
+
+	ret = mem_ap_write_atomic_u32(armv7a->debug_ap,
+					  armv7a->debug_base + CPUDBG_VCR,
+					  vcr);
+	if (ret < 0)
+		LOG_ERROR("Failed to write VCR register\n");
+
+	return ret;
+}
+
 int armv7a_init_arch_info(struct target *target, struct armv7a_common *armv7a)
 {
 	struct arm *arm = &armv7a->arm;
 	arm->arch_info = armv7a;
 	target->arch_info = &armv7a->arm;
+	arm->setup_semihosting = armv7a_setup_semihosting;
 	/*  target is useful in all function arm v4 5 compatible */
 	armv7a->arm.target = target;
 	armv7a->arm.common_magic = ARM_COMMON_MAGIC;
diff --git a/src/target/armv7a.h b/src/target/armv7a.h
index 6461ba9..14112e4 100644
--- a/src/target/armv7a.h
+++ b/src/target/armv7a.h
@@ -48,7 +48,6 @@ struct armv7a_l2x_cache {
 };
 
 struct armv7a_cachesize {
-	uint32_t level_num;
 	/*  cache dimensionning */
 	uint32_t linelen;
 	uint32_t associativity;
@@ -91,7 +90,7 @@ struct armv7a_mmu_common {
 	uint32_t ttbr_mask[2];
 	uint32_t ttbr_range[2];
 
-	int (*read_physical_memory)(struct target *target, uint32_t address, uint32_t size,
+	int (*read_physical_memory)(struct target *target, target_addr_t address, uint32_t size,
 			uint32_t count, uint8_t *buffer);
 	struct armv7a_cache_common armv7a_cache;
 	uint32_t mmu_enabled;
@@ -134,6 +133,12 @@ target_to_armv7a(struct target *target)
 	return container_of(target->arch_info, struct armv7a_common, arm);
 }
 
+static inline bool is_armv7a(struct armv7a_common *armv7a)
+{
+	return armv7a->common_magic == ARMV7_COMMON_MAGIC;
+}
+
+
 /* register offsets from armv7a.debug_base */
 
 /* See ARMv7a arch spec section C10.2 */
@@ -172,6 +177,13 @@ target_to_armv7a(struct target *target)
 /* See ARMv7a arch spec section C10.8 */
 #define CPUDBG_AUTHSTATUS	0xFB8
 
+/* Masks for Vector Catch register */
+#define DBG_VCR_FIQ_MASK	((1 << 31) | (1 << 7))
+#define DBG_VCR_IRQ_MASK	((1 << 30) | (1 << 6))
+#define DBG_VCR_DATA_ABORT_MASK	((1 << 28) | (1 << 4))
+#define DBG_VCR_PREF_ABORT_MASK	((1 << 27) | (1 << 3))
+#define DBG_VCR_SVC_MASK	((1 << 26) | (1 << 2))
+
 int armv7a_arch_state(struct target *target);
 int armv7a_identify_cache(struct target *target);
 int armv7a_init_arch_info(struct target *target, struct armv7a_common *armv7a);
diff --git a/src/target/armv7a_cache_l2x.c b/src/target/armv7a_cache_l2x.c
index 7988438..e181f26 100644
--- a/src/target/armv7a_cache_l2x.c
+++ b/src/target/armv7a_cache_l2x.c
@@ -63,12 +63,12 @@ int arm7a_l2x_flush_all_data(struct target *target)
 
 	l2_way_val = (1 << l2x_cache->way) - 1;
 
-	return target_write_phys_memory(target,
+	return target_write_phys_u32(target,
 			l2x_cache->base + L2X0_CLEAN_INV_WAY,
-			4, 1, (uint8_t *)&l2_way_val);
+			l2_way_val);
 }
 
-int armv7a_l2x_cache_flush_virt(struct target *target, uint32_t virt,
+int armv7a_l2x_cache_flush_virt(struct target *target, target_addr_t virt,
 					uint32_t size)
 {
 	struct armv7a_common *armv7a = target_to_armv7a(target);
@@ -83,16 +83,15 @@ int armv7a_l2x_cache_flush_virt(struct target *target, uint32_t virt,
 		return retval;
 
 	for (i = 0; i < size; i += linelen) {
-		uint32_t pa, offs = virt + i;
+		target_addr_t pa, offs = virt + i;
 
 		/* FIXME: use less verbose virt2phys? */
 		retval = target->type->virt2phys(target, offs, &pa);
 		if (retval != ERROR_OK)
 			goto done;
 
-		retval = target_write_phys_memory(target,
-				l2x_cache->base + L2X0_CLEAN_INV_LINE_PA,
-				4, 1, (uint8_t *)&pa);
+		retval = target_write_phys_u32(target,
+				l2x_cache->base + L2X0_CLEAN_INV_LINE_PA, pa);
 		if (retval != ERROR_OK)
 			goto done;
 	}
@@ -104,7 +103,7 @@ done:
 	return retval;
 }
 
-static int armv7a_l2x_cache_inval_virt(struct target *target, uint32_t virt,
+static int armv7a_l2x_cache_inval_virt(struct target *target, target_addr_t virt,
 					uint32_t size)
 {
 	struct armv7a_common *armv7a = target_to_armv7a(target);
@@ -119,16 +118,15 @@ static int armv7a_l2x_cache_inval_virt(struct target *target, uint32_t virt,
 		return retval;
 
 	for (i = 0; i < size; i += linelen) {
-		uint32_t pa, offs = virt + i;
+		target_addr_t pa, offs = virt + i;
 
 		/* FIXME: use less verbose virt2phys? */
 		retval = target->type->virt2phys(target, offs, &pa);
 		if (retval != ERROR_OK)
 			goto done;
 
-		retval = target_write_phys_memory(target,
-				l2x_cache->base + L2X0_INV_LINE_PA,
-				4, 1, (uint8_t *)&pa);
+		retval = target_write_phys_u32(target,
+				l2x_cache->base + L2X0_INV_LINE_PA, pa);
 		if (retval != ERROR_OK)
 			goto done;
 	}
@@ -140,7 +138,7 @@ done:
 	return retval;
 }
 
-static int armv7a_l2x_cache_clean_virt(struct target *target, uint32_t virt,
+static int armv7a_l2x_cache_clean_virt(struct target *target, target_addr_t virt,
 					unsigned int size)
 {
 	struct armv7a_common *armv7a = target_to_armv7a(target);
@@ -155,16 +153,15 @@ static int armv7a_l2x_cache_clean_virt(struct target *target, uint32_t virt,
 		return retval;
 
 	for (i = 0; i < size; i += linelen) {
-		uint32_t pa, offs = virt + i;
+		target_addr_t pa, offs = virt + i;
 
 		/* FIXME: use less verbose virt2phys? */
 		retval = target->type->virt2phys(target, offs, &pa);
 		if (retval != ERROR_OK)
 			goto done;
 
-		retval = target_write_phys_memory(target,
-				l2x_cache->base + L2X0_CLEAN_LINE_PA,
-				4, 1, (uint8_t *)&pa);
+		retval = target_write_phys_u32(target,
+				l2x_cache->base + L2X0_CLEAN_LINE_PA, pa);
 		if (retval != ERROR_OK)
 			goto done;
 	}
@@ -252,7 +249,8 @@ COMMAND_HANDLER(arm7a_l2x_cache_flush_all_command)
 COMMAND_HANDLER(arm7a_l2x_cache_flush_virt_cmd)
 {
 	struct target *target = get_current_target(CMD_CTX);
-	uint32_t virt, size;
+	target_addr_t virt;
+	uint32_t size;
 
 	if (CMD_ARGC == 0 || CMD_ARGC > 2)
 		return ERROR_COMMAND_SYNTAX_ERROR;
@@ -262,7 +260,7 @@ COMMAND_HANDLER(arm7a_l2x_cache_flush_virt_cmd)
 	else
 		size = 1;
 
-	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], virt);
+	COMMAND_PARSE_ADDRESS(CMD_ARGV[0], virt);
 
 	return armv7a_l2x_cache_flush_virt(target, virt, size);
 }
@@ -270,7 +268,8 @@ COMMAND_HANDLER(arm7a_l2x_cache_flush_virt_cmd)
 COMMAND_HANDLER(arm7a_l2x_cache_inval_virt_cmd)
 {
 	struct target *target = get_current_target(CMD_CTX);
-	uint32_t virt, size;
+	target_addr_t virt;
+	uint32_t size;
 
 	if (CMD_ARGC == 0 || CMD_ARGC > 2)
 		return ERROR_COMMAND_SYNTAX_ERROR;
@@ -280,7 +279,7 @@ COMMAND_HANDLER(arm7a_l2x_cache_inval_virt_cmd)
 	else
 		size = 1;
 
-	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], virt);
+	COMMAND_PARSE_ADDRESS(CMD_ARGV[0], virt);
 
 	return armv7a_l2x_cache_inval_virt(target, virt, size);
 }
@@ -288,7 +287,8 @@ COMMAND_HANDLER(arm7a_l2x_cache_inval_virt_cmd)
 COMMAND_HANDLER(arm7a_l2x_cache_clean_virt_cmd)
 {
 	struct target *target = get_current_target(CMD_CTX);
-	uint32_t virt, size;
+	target_addr_t virt;
+	uint32_t size;
 
 	if (CMD_ARGC == 0 || CMD_ARGC > 2)
 		return ERROR_COMMAND_SYNTAX_ERROR;
@@ -298,7 +298,7 @@ COMMAND_HANDLER(arm7a_l2x_cache_clean_virt_cmd)
 	else
 		size = 1;
 
-	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], virt);
+	COMMAND_PARSE_ADDRESS(CMD_ARGV[0], virt);
 
 	return armv7a_l2x_cache_clean_virt(target, virt, size);
 }
diff --git a/src/target/armv7a_cache_l2x.h b/src/target/armv7a_cache_l2x.h
index 3d9ad81..f98b554 100644
--- a/src/target/armv7a_cache_l2x.h
+++ b/src/target/armv7a_cache_l2x.h
@@ -151,7 +151,7 @@ struct l2c_init_data {
 
 extern const struct command_registration arm7a_l2x_cache_command_handler[];
 
-int armv7a_l2x_cache_flush_virt(struct target *target, uint32_t virt,
+int armv7a_l2x_cache_flush_virt(struct target *target, target_addr_t virt,
 					uint32_t size);
 int arm7a_l2x_flush_all_data(struct target *target);
 
diff --git a/src/target/armv7m.c b/src/target/armv7m.c
index e2f710f..e0911c3 100644
--- a/src/target/armv7m.c
+++ b/src/target/armv7m.c
@@ -318,7 +318,7 @@ int armv7m_get_gdb_reg_list(struct target *target, struct reg **reg_list[],
 int armv7m_run_algorithm(struct target *target,
 	int num_mem_params, struct mem_param *mem_params,
 	int num_reg_params, struct reg_param *reg_params,
-	uint32_t entry_point, uint32_t exit_point,
+	target_addr_t entry_point, target_addr_t exit_point,
 	int timeout_ms, void *arch_info)
 {
 	int retval;
@@ -343,7 +343,7 @@ int armv7m_run_algorithm(struct target *target,
 int armv7m_start_algorithm(struct target *target,
 	int num_mem_params, struct mem_param *mem_params,
 	int num_reg_params, struct reg_param *reg_params,
-	uint32_t entry_point, uint32_t exit_point,
+	target_addr_t entry_point, target_addr_t exit_point,
 	void *arch_info)
 {
 	struct armv7m_common *armv7m = target_to_armv7m(target);
@@ -431,7 +431,7 @@ int armv7m_start_algorithm(struct target *target,
 int armv7m_wait_algorithm(struct target *target,
 	int num_mem_params, struct mem_param *mem_params,
 	int num_reg_params, struct reg_param *reg_params,
-	uint32_t exit_point, int timeout_ms,
+	target_addr_t exit_point, int timeout_ms,
 	void *arch_info)
 {
 	struct armv7m_common *armv7m = target_to_armv7m(target);
@@ -461,7 +461,7 @@ int armv7m_wait_algorithm(struct target *target,
 
 	armv7m->load_core_reg_u32(target, 15, &pc);
 	if (exit_point && (pc != exit_point)) {
-		LOG_DEBUG("failed algorithm halted at 0x%" PRIx32 ", expected 0x%" PRIx32,
+		LOG_DEBUG("failed algorithm halted at 0x%" PRIx32 ", expected 0x%" TARGET_PRIxADDR,
 			pc,
 			exit_point);
 		return ERROR_TARGET_TIMEOUT;
@@ -536,11 +536,15 @@ int armv7m_arch_state(struct target *target)
 	struct arm *arm = &armv7m->arm;
 	uint32_t ctrl, sp;
 
+	/* avoid filling log waiting for fileio reply */
+	if (arm->semihosting_hit_fileio)
+		return ERROR_OK;
+
 	ctrl = buf_get_u32(arm->core_cache->reg_list[ARMV7M_CONTROL].value, 0, 32);
 	sp = buf_get_u32(arm->core_cache->reg_list[ARMV7M_R13].value, 0, 32);
 
 	LOG_USER("target halted due to %s, current mode: %s %s\n"
-		"xPSR: %#8.8" PRIx32 " pc: %#8.8" PRIx32 " %csp: %#8.8" PRIx32 "%s",
+		"xPSR: %#8.8" PRIx32 " pc: %#8.8" PRIx32 " %csp: %#8.8" PRIx32 "%s%s",
 		debug_reason_name(target),
 		arm_mode_name(arm->core_mode),
 		armv7m_exception_string(armv7m->exception_number),
@@ -548,7 +552,8 @@ int armv7m_arch_state(struct target *target)
 		buf_get_u32(arm->pc->value, 0, 32),
 		(ctrl & 0x02) ? 'p' : 'm',
 		sp,
-		arm->is_semihosting ? ", semihosting" : "");
+		arm->is_semihosting ? ", semihosting" : "",
+		arm->is_semihosting_fileio ? " fileio" : "");
 
 	return ERROR_OK;
 }
@@ -677,7 +682,7 @@ int armv7m_init_arch_info(struct target *target, struct armv7m_common *armv7m)
 
 /** Generates a CRC32 checksum of a memory region. */
 int armv7m_checksum_memory(struct target *target,
-	uint32_t address, uint32_t count, uint32_t *checksum)
+	target_addr_t address, uint32_t count, uint32_t *checksum)
 {
 	struct working_area *crc_algorithm;
 	struct armv7m_algorithm armv7m_info;
@@ -726,26 +731,42 @@ cleanup:
 	return retval;
 }
 
-/** Checks whether a memory region is zeroed. */
+/** Checks whether a memory region is erased. */
 int armv7m_blank_check_memory(struct target *target,
-	uint32_t address, uint32_t count, uint32_t *blank)
+	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[3];
 	struct armv7m_algorithm armv7m_info;
+	const uint8_t *code;
+	uint32_t code_size;
 	int retval;
 
 	static const uint8_t erase_check_code[] = {
 #include "../../contrib/loaders/erase_check/armv7m_erase_check.inc"
 	};
+	static const uint8_t zero_erase_check_code[] = {
+#include "../../contrib/loaders/erase_check/armv7m_0_erase_check.inc"
+	};
+
+	switch (erased_value) {
+	case 0x00:
+		code = zero_erase_check_code;
+		code_size = sizeof(zero_erase_check_code);
+		break;
+	case 0xff:
+	default:
+		code = erase_check_code;
+		code_size = sizeof(erase_check_code);
+	}
 
 	/* make sure we have a working area */
-	if (target_alloc_working_area(target, sizeof(erase_check_code),
+	if (target_alloc_working_area(target, code_size,
 		&erase_check_algorithm) != ERROR_OK)
 		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 
 	retval = target_write_buffer(target, erase_check_algorithm->address,
-			sizeof(erase_check_code), (uint8_t *)erase_check_code);
+			code_size, code);
 	if (retval != ERROR_OK)
 		goto cleanup;
 
@@ -759,7 +780,7 @@ int armv7m_blank_check_memory(struct target *target,
 	buf_set_u32(reg_params[1].value, 0, 32, count);
 
 	init_reg_param(&reg_params[2], "r2", 32, PARAM_IN_OUT);
-	buf_set_u32(reg_params[2].value, 0, 32, 0xff);
+	buf_set_u32(reg_params[2].value, 0, 32, erased_value);
 
 	retval = target_run_algorithm(target,
 			0,
@@ -767,7 +788,7 @@ int armv7m_blank_check_memory(struct target *target,
 			3,
 			reg_params,
 			erase_check_algorithm->address,
-			erase_check_algorithm->address + (sizeof(erase_check_code) - 2),
+			erase_check_algorithm->address + (code_size - 2),
 			10000,
 			&armv7m_info);
 
diff --git a/src/target/armv7m.h b/src/target/armv7m.h
index 90cad00..284bb9c 100644
--- a/src/target/armv7m.h
+++ b/src/target/armv7m.h
@@ -203,19 +203,19 @@ int armv7m_init_arch_info(struct target *target, struct armv7m_common *armv7m);
 int armv7m_run_algorithm(struct target *target,
 		int num_mem_params, struct mem_param *mem_params,
 		int num_reg_params, struct reg_param *reg_params,
-		uint32_t entry_point, uint32_t exit_point,
+		target_addr_t entry_point, target_addr_t exit_point,
 		int timeout_ms, void *arch_info);
 
 int armv7m_start_algorithm(struct target *target,
 		int num_mem_params, struct mem_param *mem_params,
 		int num_reg_params, struct reg_param *reg_params,
-		uint32_t entry_point, uint32_t exit_point,
+		target_addr_t entry_point, target_addr_t exit_point,
 		void *arch_info);
 
 int armv7m_wait_algorithm(struct target *target,
 		int num_mem_params, struct mem_param *mem_params,
 		int num_reg_params, struct reg_param *reg_params,
-		uint32_t exit_point, int timeout_ms,
+		target_addr_t exit_point, int timeout_ms,
 		void *arch_info);
 
 int armv7m_invalidate_core_regs(struct target *target);
@@ -223,9 +223,9 @@ int armv7m_invalidate_core_regs(struct target *target);
 int armv7m_restore_context(struct target *target);
 
 int armv7m_checksum_memory(struct target *target,
-		uint32_t address, uint32_t count, uint32_t *checksum);
+		target_addr_t address, uint32_t count, uint32_t *checksum);
 int armv7m_blank_check_memory(struct target *target,
-		uint32_t address, uint32_t count, uint32_t *blank);
+		target_addr_t address, uint32_t count, uint32_t *blank, uint8_t erased_value);
 
 int armv7m_maybe_skip_bkpt_inst(struct target *target, bool *inst_found);
 
diff --git a/src/target/armv8.c b/src/target/armv8.c
new file mode 100644
index 0000000..df5e251
--- /dev/null
+++ b/src/target/armv8.c
@@ -0,0 +1,1308 @@
+/***************************************************************************
+ *   Copyright (C) 2015 by David Ung                                       *
+ *                                                                         *
+ *   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, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ ***************************************************************************/
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include <helper/replacements.h>
+
+#include "armv8.h"
+#include "arm_disassembler.h"
+
+#include "register.h"
+#include <helper/binarybuffer.h>
+#include <helper/command.h>
+
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+
+#include "armv8_opcodes.h"
+#include "target.h"
+#include "target_type.h"
+
+static const char * const armv8_state_strings[] = {
+	"AArch32", "Thumb", "Jazelle", "ThumbEE", "AArch64",
+};
+
+static const struct {
+	const char *name;
+	unsigned psr;
+} armv8_mode_data[] = {
+	/* These special modes are currently only supported
+	 * by ARMv6M and ARMv7M profiles */
+	{
+		.name = "USR",
+		.psr = ARM_MODE_USR,
+	},
+	{
+		.name = "FIQ",
+		.psr = ARM_MODE_FIQ,
+	},
+	{
+		.name = "IRQ",
+		.psr = ARM_MODE_IRQ,
+	},
+	{
+		.name = "SVC",
+		.psr = ARM_MODE_SVC,
+	},
+	{
+		.name = "MON",
+		.psr = ARM_MODE_MON,
+	},
+	{
+		.name = "ABT",
+		.psr = ARM_MODE_ABT,
+	},
+	{
+		.name = "EL0T",
+		.psr = ARMV8_64_EL0T,
+	},
+	{
+		.name = "EL1T",
+		.psr = ARMV8_64_EL1T,
+	},
+	{
+		.name = "EL1H",
+		.psr = ARMV8_64_EL1H,
+	},
+	{
+		.name = "EL2T",
+		.psr = ARMV8_64_EL2T,
+	},
+	{
+		.name = "EL2H",
+		.psr = ARMV8_64_EL2H,
+	},
+	{
+		.name = "EL3T",
+		.psr = ARMV8_64_EL3T,
+	},
+	{
+		.name = "EL3H",
+		.psr = ARMV8_64_EL3H,
+	},
+};
+
+/** Map PSR mode bits to the name of an ARM processor operating mode. */
+const char *armv8_mode_name(unsigned psr_mode)
+{
+	for (unsigned i = 0; i < ARRAY_SIZE(armv8_mode_data); i++) {
+		if (armv8_mode_data[i].psr == psr_mode)
+			return armv8_mode_data[i].name;
+	}
+	LOG_ERROR("unrecognized psr mode: %#02x", psr_mode);
+	return "UNRECOGNIZED";
+}
+
+int armv8_mode_to_number(enum arm_mode mode)
+{
+	switch (mode) {
+		case ARM_MODE_ANY:
+		/* map MODE_ANY to user mode */
+		case ARM_MODE_USR:
+			return 0;
+		case ARM_MODE_FIQ:
+			return 1;
+		case ARM_MODE_IRQ:
+			return 2;
+		case ARM_MODE_SVC:
+			return 3;
+		case ARM_MODE_ABT:
+			return 4;
+		case ARM_MODE_UND:
+			return 5;
+		case ARM_MODE_SYS:
+			return 6;
+		case ARM_MODE_MON:
+			return 7;
+		case ARMV8_64_EL0T:
+			return 8;
+		case ARMV8_64_EL1T:
+			return 9;
+		case ARMV8_64_EL1H:
+			return 10;
+		case ARMV8_64_EL2T:
+			return 11;
+		case ARMV8_64_EL2H:
+			return 12;
+		case ARMV8_64_EL3T:
+			return 13;
+		case ARMV8_64_EL3H:
+			return 14;
+
+		default:
+			LOG_ERROR("invalid mode value encountered %d", mode);
+			return -1;
+	}
+}
+
+static int armv8_read_reg(struct armv8_common *armv8, int regnum, uint64_t *regval)
+{
+	struct arm_dpm *dpm = &armv8->dpm;
+	int retval;
+	uint32_t value;
+	uint64_t value_64;
+
+	switch (regnum) {
+	case 0 ... 30:
+		retval = dpm->instr_read_data_dcc_64(dpm,
+				ARMV8_MSR_GP(SYSTEM_DBG_DBGDTR_EL0, regnum), &value_64);
+		break;
+	case ARMV8_SP:
+		retval = dpm->instr_read_data_r0_64(dpm,
+				ARMV8_MOVFSP_64(0), &value_64);
+		break;
+	case ARMV8_PC:
+		retval = dpm->instr_read_data_r0_64(dpm,
+				ARMV8_MRS_DLR(0), &value_64);
+		break;
+	case ARMV8_xPSR:
+		retval = dpm->instr_read_data_r0(dpm,
+				ARMV8_MRS_DSPSR(0), &value);
+		value_64 = value;
+		break;
+	case ARMV8_ELR_EL1:
+		retval = dpm->instr_read_data_r0_64(dpm,
+				ARMV8_MRS(SYSTEM_ELR_EL1, 0), &value_64);
+		break;
+	case ARMV8_ELR_EL2:
+		retval = dpm->instr_read_data_r0_64(dpm,
+				ARMV8_MRS(SYSTEM_ELR_EL2, 0), &value_64);
+		break;
+	case ARMV8_ELR_EL3:
+		retval = dpm->instr_read_data_r0_64(dpm,
+				ARMV8_MRS(SYSTEM_ELR_EL3, 0), &value_64);
+		break;
+	case ARMV8_ESR_EL1:
+		retval = dpm->instr_read_data_r0(dpm,
+				ARMV8_MRS(SYSTEM_ESR_EL1, 0), &value);
+		value_64 = value;
+		break;
+	case ARMV8_ESR_EL2:
+		retval = dpm->instr_read_data_r0(dpm,
+				ARMV8_MRS(SYSTEM_ESR_EL2, 0), &value);
+		value_64 = value;
+		break;
+	case ARMV8_ESR_EL3:
+		retval = dpm->instr_read_data_r0(dpm,
+				ARMV8_MRS(SYSTEM_ESR_EL3, 0), &value);
+		value_64 = value;
+		break;
+	case ARMV8_SPSR_EL1:
+		retval = dpm->instr_read_data_r0(dpm,
+				ARMV8_MRS(SYSTEM_SPSR_EL1, 0), &value);
+		value_64 = value;
+		break;
+	case ARMV8_SPSR_EL2:
+		retval = dpm->instr_read_data_r0(dpm,
+				ARMV8_MRS(SYSTEM_SPSR_EL2, 0), &value);
+		value_64 = value;
+		break;
+	case ARMV8_SPSR_EL3:
+		retval = dpm->instr_read_data_r0(dpm,
+				ARMV8_MRS(SYSTEM_SPSR_EL3, 0), &value);
+		value_64 = value;
+		break;
+	default:
+		retval = ERROR_FAIL;
+		break;
+	}
+
+	if (retval == ERROR_OK && regval != NULL)
+		*regval = value_64;
+
+	return retval;
+}
+
+static int armv8_write_reg(struct armv8_common *armv8, int regnum, uint64_t value_64)
+{
+	struct arm_dpm *dpm = &armv8->dpm;
+	int retval;
+	uint32_t value;
+
+	switch (regnum) {
+	case 0 ... 30:
+		retval = dpm->instr_write_data_dcc_64(dpm,
+			ARMV8_MRS(SYSTEM_DBG_DBGDTR_EL0, regnum),
+			value_64);
+		break;
+	case ARMV8_SP:
+		retval = dpm->instr_write_data_r0_64(dpm,
+			ARMV8_MOVTSP_64(0),
+			value_64);
+		break;
+	case ARMV8_PC:
+		retval = dpm->instr_write_data_r0_64(dpm,
+			ARMV8_MSR_DLR(0),
+			value_64);
+		break;
+	case ARMV8_xPSR:
+		value = value_64;
+		retval = dpm->instr_write_data_r0(dpm,
+			ARMV8_MSR_DSPSR(0),
+			value);
+		break;
+	/* registers clobbered by taking exception in debug state */
+	case ARMV8_ELR_EL1:
+		retval = dpm->instr_write_data_r0_64(dpm,
+				ARMV8_MSR_GP(SYSTEM_ELR_EL1, 0), value_64);
+		break;
+	case ARMV8_ELR_EL2:
+		retval = dpm->instr_write_data_r0_64(dpm,
+				ARMV8_MSR_GP(SYSTEM_ELR_EL2, 0), value_64);
+		break;
+	case ARMV8_ELR_EL3:
+		retval = dpm->instr_write_data_r0_64(dpm,
+				ARMV8_MSR_GP(SYSTEM_ELR_EL3, 0), value_64);
+		break;
+	case ARMV8_ESR_EL1:
+		value = value_64;
+		retval = dpm->instr_write_data_r0(dpm,
+				ARMV8_MSR_GP(SYSTEM_ESR_EL1, 0), value);
+		break;
+	case ARMV8_ESR_EL2:
+		value = value_64;
+		retval = dpm->instr_write_data_r0(dpm,
+				ARMV8_MSR_GP(SYSTEM_ESR_EL2, 0), value);
+		break;
+	case ARMV8_ESR_EL3:
+		value = value_64;
+		retval = dpm->instr_write_data_r0(dpm,
+				ARMV8_MSR_GP(SYSTEM_ESR_EL3, 0), value);
+		break;
+	case ARMV8_SPSR_EL1:
+		value = value_64;
+		retval = dpm->instr_write_data_r0(dpm,
+				ARMV8_MSR_GP(SYSTEM_SPSR_EL1, 0), value);
+		break;
+	case ARMV8_SPSR_EL2:
+		value = value_64;
+		retval = dpm->instr_write_data_r0(dpm,
+				ARMV8_MSR_GP(SYSTEM_SPSR_EL2, 0), value);
+		break;
+	case ARMV8_SPSR_EL3:
+		value = value_64;
+		retval = dpm->instr_write_data_r0(dpm,
+				ARMV8_MSR_GP(SYSTEM_SPSR_EL3, 0), value);
+		break;
+	default:
+		retval = ERROR_FAIL;
+		break;
+	}
+
+	return retval;
+}
+
+static int armv8_read_reg32(struct armv8_common *armv8, int regnum, uint64_t *regval)
+{
+	struct arm_dpm *dpm = &armv8->dpm;
+	uint32_t value = 0;
+	int retval;
+
+	switch (regnum) {
+	case ARMV8_R0 ... ARMV8_R14:
+		/* return via DCC:  "MCR p14, 0, Rnum, c0, c5, 0" */
+		retval = dpm->instr_read_data_dcc(dpm,
+			ARMV4_5_MCR(14, 0, regnum, 0, 5, 0),
+			&value);
+		break;
+	case ARMV8_SP:
+		retval = dpm->instr_read_data_dcc(dpm,
+			ARMV4_5_MCR(14, 0, 13, 0, 5, 0),
+			&value);
+		break;
+	case ARMV8_PC:
+		retval = dpm->instr_read_data_r0(dpm,
+			ARMV8_MRC_DLR(0),
+			&value);
+		break;
+	case ARMV8_xPSR:
+		retval = dpm->instr_read_data_r0(dpm,
+			ARMV8_MRC_DSPSR(0),
+			&value);
+		break;
+	case ARMV8_ELR_EL1: /* mapped to LR_svc */
+		retval = dpm->instr_read_data_dcc(dpm,
+				ARMV4_5_MCR(14, 0, 14, 0, 5, 0),
+				&value);
+		break;
+	case ARMV8_ELR_EL2: /* mapped to ELR_hyp */
+		retval = dpm->instr_read_data_r0(dpm,
+				ARMV8_MRS_T1(0, 14, 0, 1),
+				&value);
+		break;
+	case ARMV8_ELR_EL3: /* mapped to LR_mon */
+		retval = dpm->instr_read_data_dcc(dpm,
+				ARMV4_5_MCR(14, 0, 14, 0, 5, 0),
+				&value);
+		break;
+	case ARMV8_ESR_EL1: /* mapped to DFSR */
+		retval = dpm->instr_read_data_r0(dpm,
+				ARMV4_5_MRC(15, 0, 0, 5, 0, 0),
+				&value);
+		break;
+	case ARMV8_ESR_EL2: /* mapped to HSR */
+		retval = dpm->instr_read_data_r0(dpm,
+				ARMV4_5_MRC(15, 4, 0, 5, 2, 0),
+				&value);
+		break;
+	case ARMV8_ESR_EL3: /* FIXME: no equivalent in aarch32? */
+		retval = ERROR_FAIL;
+		break;
+	case ARMV8_SPSR_EL1: /* mapped to SPSR_svc */
+		retval = dpm->instr_read_data_r0(dpm,
+				ARMV8_MRS_xPSR_T1(1, 0),
+				&value);
+		break;
+	case ARMV8_SPSR_EL2: /* mapped to SPSR_hyp */
+		retval = dpm->instr_read_data_r0(dpm,
+				ARMV8_MRS_xPSR_T1(1, 0),
+				&value);
+		break;
+	case ARMV8_SPSR_EL3: /* mapped to SPSR_mon */
+		retval = dpm->instr_read_data_r0(dpm,
+				ARMV8_MRS_xPSR_T1(1, 0),
+				&value);
+		break;
+	default:
+		retval = ERROR_FAIL;
+		break;
+	}
+
+	if (retval == ERROR_OK && regval != NULL)
+		*regval = value;
+
+	return retval;
+}
+
+static int armv8_write_reg32(struct armv8_common *armv8, int regnum, uint64_t value)
+{
+	struct arm_dpm *dpm = &armv8->dpm;
+	int retval;
+
+	switch (regnum) {
+	case ARMV8_R0 ... ARMV8_R14:
+		/* load register from DCC:  "MRC p14, 0, Rnum, c0, c5, 0" */
+		retval = dpm->instr_write_data_dcc(dpm,
+				ARMV4_5_MRC(14, 0, regnum, 0, 5, 0), value);
+		break;
+	case ARMV8_SP:
+		retval = dpm->instr_write_data_dcc(dpm,
+				ARMV4_5_MRC(14, 0, 13, 0, 5, 0), value);
+			break;
+	case ARMV8_PC:/* PC
+		 * read r0 from DCC; then "MOV pc, r0" */
+		retval = dpm->instr_write_data_r0(dpm,
+				ARMV8_MCR_DLR(0), value);
+		break;
+	case ARMV8_xPSR: /* CPSR */
+		/* read r0 from DCC, then "MCR r0, DSPSR" */
+		retval = dpm->instr_write_data_r0(dpm,
+				ARMV8_MCR_DSPSR(0), value);
+		break;
+	case ARMV8_ELR_EL1: /* mapped to LR_svc */
+		retval = dpm->instr_write_data_dcc(dpm,
+				ARMV4_5_MRC(14, 0, 14, 0, 5, 0),
+				value);
+		break;
+	case ARMV8_ELR_EL2: /* mapped to ELR_hyp */
+		retval = dpm->instr_write_data_r0(dpm,
+				ARMV8_MSR_GP_T1(0, 14, 0, 1),
+				value);
+		break;
+	case ARMV8_ELR_EL3: /* mapped to LR_mon */
+		retval = dpm->instr_write_data_dcc(dpm,
+				ARMV4_5_MRC(14, 0, 14, 0, 5, 0),
+				value);
+		break;
+	case ARMV8_ESR_EL1: /* mapped to DFSR */
+		retval = dpm->instr_write_data_r0(dpm,
+				ARMV4_5_MCR(15, 0, 0, 5, 0, 0),
+				value);
+		break;
+	case ARMV8_ESR_EL2: /* mapped to HSR */
+		retval = dpm->instr_write_data_r0(dpm,
+				ARMV4_5_MCR(15, 4, 0, 5, 2, 0),
+				value);
+		break;
+	case ARMV8_ESR_EL3: /* FIXME: no equivalent in aarch32? */
+		retval = ERROR_FAIL;
+		break;
+	case ARMV8_SPSR_EL1: /* mapped to SPSR_svc */
+		retval = dpm->instr_write_data_r0(dpm,
+				ARMV8_MSR_GP_xPSR_T1(1, 0, 15),
+				value);
+		break;
+	case ARMV8_SPSR_EL2: /* mapped to SPSR_hyp */
+		retval = dpm->instr_write_data_r0(dpm,
+				ARMV8_MSR_GP_xPSR_T1(1, 0, 15),
+				value);
+		break;
+	case ARMV8_SPSR_EL3: /* mapped to SPSR_mon */
+		retval = dpm->instr_write_data_r0(dpm,
+				ARMV8_MSR_GP_xPSR_T1(1, 0, 15),
+				value);
+		break;
+	default:
+		retval = ERROR_FAIL;
+		break;
+	}
+
+	return retval;
+
+}
+
+void armv8_select_reg_access(struct armv8_common *armv8, bool is_aarch64)
+{
+	if (is_aarch64) {
+		armv8->read_reg_u64 = armv8_read_reg;
+		armv8->write_reg_u64 = armv8_write_reg;
+	} else {
+		armv8->read_reg_u64 = armv8_read_reg32;
+		armv8->write_reg_u64 = armv8_write_reg32;
+	}
+}
+
+/*  retrieve core id cluster id  */
+int armv8_read_mpidr(struct armv8_common *armv8)
+{
+	int retval = ERROR_FAIL;
+	struct arm_dpm *dpm = armv8->arm.dpm;
+	uint32_t mpidr;
+
+	retval = dpm->prepare(dpm);
+	if (retval != ERROR_OK)
+		goto done;
+
+	retval = dpm->instr_read_data_r0(dpm, armv8_opcode(armv8, READ_REG_MPIDR), &mpidr);
+	if (retval != ERROR_OK)
+		goto done;
+	if (mpidr & 1<<31) {
+		armv8->multi_processor_system = (mpidr >> 30) & 1;
+		armv8->cluster_id = (mpidr >> 8) & 0xf;
+		armv8->cpu_id = mpidr & 0x3;
+		LOG_INFO("%s cluster %x core %x %s", target_name(armv8->arm.target),
+			armv8->cluster_id,
+			armv8->cpu_id,
+			armv8->multi_processor_system == 0 ? "multi core" : "single core");
+	} else
+		LOG_ERROR("mpidr not in multiprocessor format");
+
+done:
+	dpm->finish(dpm);
+	return retval;
+}
+
+/**
+ * Configures host-side ARM records to reflect the specified CPSR.
+ * Later, code can use arm_reg_current() to map register numbers
+ * according to how they are exposed by this mode.
+ */
+void armv8_set_cpsr(struct arm *arm, uint32_t cpsr)
+{
+	uint32_t mode = cpsr & 0x1F;
+
+	/* NOTE:  this may be called very early, before the register
+	 * cache is set up.  We can't defend against many errors, in
+	 * particular against CPSRs that aren't valid *here* ...
+	 */
+	if (arm->cpsr) {
+		buf_set_u32(arm->cpsr->value, 0, 32, cpsr);
+		arm->cpsr->valid = 1;
+		arm->cpsr->dirty = 0;
+	}
+
+	/* Older ARMs won't have the J bit */
+	enum arm_state state = 0xFF;
+
+	if (((cpsr & 0x10) >> 4) == 0) {
+		state = ARM_STATE_AARCH64;
+	} else {
+		if (cpsr & (1 << 5)) {	/* T */
+			if (cpsr & (1 << 24)) { /* J */
+				LOG_WARNING("ThumbEE -- incomplete support");
+				state = ARM_STATE_THUMB_EE;
+			} else
+				state = ARM_STATE_THUMB;
+		} else {
+			if (cpsr & (1 << 24)) { /* J */
+				LOG_ERROR("Jazelle state handling is BROKEN!");
+				state = ARM_STATE_JAZELLE;
+			} else
+				state = ARM_STATE_ARM;
+		}
+	}
+	arm->core_state = state;
+	if (arm->core_state == ARM_STATE_AARCH64)
+		arm->core_mode = (mode << 4) | 0xf;
+	else
+		arm->core_mode = mode;
+
+	LOG_DEBUG("set CPSR %#8.8x: %s mode, %s state", (unsigned) cpsr,
+		armv8_mode_name(arm->core_mode),
+		armv8_state_strings[arm->core_state]);
+}
+
+static void armv8_show_fault_registers32(struct armv8_common *armv8)
+{
+	uint32_t dfsr, ifsr, dfar, ifar;
+	struct arm_dpm *dpm = armv8->arm.dpm;
+	int retval;
+
+	retval = dpm->prepare(dpm);
+	if (retval != ERROR_OK)
+		return;
+
+	/* ARMV4_5_MRC(cpnum, op1, r0, CRn, CRm, op2) */
+
+	/* c5/c0 - {data, instruction} fault status registers */
+	retval = dpm->instr_read_data_r0(dpm,
+			ARMV4_5_MRC(15, 0, 0, 5, 0, 0),
+			&dfsr);
+	if (retval != ERROR_OK)
+		goto done;
+
+	retval = dpm->instr_read_data_r0(dpm,
+			ARMV4_5_MRC(15, 0, 0, 5, 0, 1),
+			&ifsr);
+	if (retval != ERROR_OK)
+		goto done;
+
+	/* c6/c0 - {data, instruction} fault address registers */
+	retval = dpm->instr_read_data_r0(dpm,
+			ARMV4_5_MRC(15, 0, 0, 6, 0, 0),
+			&dfar);
+	if (retval != ERROR_OK)
+		goto done;
+
+	retval = dpm->instr_read_data_r0(dpm,
+			ARMV4_5_MRC(15, 0, 0, 6, 0, 2),
+			&ifar);
+	if (retval != ERROR_OK)
+		goto done;
+
+	LOG_USER("Data fault registers        DFSR: %8.8" PRIx32
+		", DFAR: %8.8" PRIx32, dfsr, dfar);
+	LOG_USER("Instruction fault registers IFSR: %8.8" PRIx32
+		", IFAR: %8.8" PRIx32, ifsr, ifar);
+
+done:
+	/* (void) */ dpm->finish(dpm);
+}
+
+static __attribute__((unused)) void armv8_show_fault_registers(struct target *target)
+{
+	struct armv8_common *armv8 = target_to_armv8(target);
+
+	if (armv8->arm.core_state != ARM_STATE_AARCH64)
+		armv8_show_fault_registers32(armv8);
+}
+
+static uint8_t armv8_pa_size(uint32_t ps)
+{
+	uint8_t ret = 0;
+	switch (ps) {
+		case 0:
+			ret = 32;
+			break;
+		case 1:
+			ret = 36;
+			break;
+		case 2:
+			ret = 40;
+			break;
+		case 3:
+			ret = 42;
+			break;
+		case 4:
+			ret = 44;
+			break;
+		case 5:
+			ret = 48;
+			break;
+		default:
+			LOG_INFO("Unknow physicall address size");
+			break;
+	}
+	return ret;
+}
+
+static __attribute__((unused)) int armv8_read_ttbcr32(struct target *target)
+{
+	struct armv8_common *armv8 = target_to_armv8(target);
+	struct arm_dpm *dpm = armv8->arm.dpm;
+	uint32_t ttbcr, ttbcr_n;
+	int retval = dpm->prepare(dpm);
+	if (retval != ERROR_OK)
+		goto done;
+	/*  MRC p15,0,<Rt>,c2,c0,2 ; Read CP15 Translation Table Base Control Register*/
+	retval = dpm->instr_read_data_r0(dpm,
+			ARMV4_5_MRC(15, 0, 0, 2, 0, 2),
+			&ttbcr);
+	if (retval != ERROR_OK)
+		goto done;
+
+	LOG_DEBUG("ttbcr %" PRIx32, ttbcr);
+
+	ttbcr_n = ttbcr & 0x7;
+	armv8->armv8_mmu.ttbcr = ttbcr;
+
+	/*
+	 * ARM Architecture Reference Manual (ARMv7-A and ARMv7-Redition),
+	 * document # ARM DDI 0406C
+	 */
+	armv8->armv8_mmu.ttbr_range[0]  = 0xffffffff >> ttbcr_n;
+	armv8->armv8_mmu.ttbr_range[1] = 0xffffffff;
+	armv8->armv8_mmu.ttbr_mask[0] = 0xffffffff << (14 - ttbcr_n);
+	armv8->armv8_mmu.ttbr_mask[1] = 0xffffffff << 14;
+
+	LOG_DEBUG("ttbr1 %s, ttbr0_mask %" PRIx32 " ttbr1_mask %" PRIx32,
+		  (ttbcr_n != 0) ? "used" : "not used",
+		  armv8->armv8_mmu.ttbr_mask[0],
+		  armv8->armv8_mmu.ttbr_mask[1]);
+
+done:
+	dpm->finish(dpm);
+	return retval;
+}
+
+static __attribute__((unused)) int armv8_read_ttbcr(struct target *target)
+{
+	struct armv8_common *armv8 = target_to_armv8(target);
+	struct arm_dpm *dpm = armv8->arm.dpm;
+	struct arm *arm = &armv8->arm;
+	uint32_t ttbcr;
+	uint64_t ttbcr_64;
+
+	int retval = dpm->prepare(dpm);
+	if (retval != ERROR_OK)
+		goto done;
+
+	/* claaer ttrr1_used and ttbr0_mask */
+	memset(&armv8->armv8_mmu.ttbr1_used, 0, sizeof(armv8->armv8_mmu.ttbr1_used));
+	memset(&armv8->armv8_mmu.ttbr0_mask, 0, sizeof(armv8->armv8_mmu.ttbr0_mask));
+
+	switch (armv8_curel_from_core_mode(arm->core_mode)) {
+	case SYSTEM_CUREL_EL3:
+		retval = dpm->instr_read_data_r0(dpm,
+				ARMV8_MRS(SYSTEM_TCR_EL3, 0),
+				&ttbcr);
+		retval += dpm->instr_read_data_r0_64(dpm,
+				ARMV8_MRS(SYSTEM_TTBR0_EL3, 0),
+				&armv8->ttbr_base);
+		if (retval != ERROR_OK)
+			goto done;
+		armv8->va_size = 64 - (ttbcr & 0x3F);
+		armv8->pa_size = armv8_pa_size((ttbcr >> 16) & 7);
+		armv8->page_size = (ttbcr >> 14) & 3;
+		break;
+	case SYSTEM_CUREL_EL2:
+		retval = dpm->instr_read_data_r0(dpm,
+				ARMV8_MRS(SYSTEM_TCR_EL2, 0),
+				&ttbcr);
+		retval += dpm->instr_read_data_r0_64(dpm,
+				ARMV8_MRS(SYSTEM_TTBR0_EL2, 0),
+				&armv8->ttbr_base);
+		if (retval != ERROR_OK)
+			goto done;
+		armv8->va_size = 64 - (ttbcr & 0x3F);
+		armv8->pa_size = armv8_pa_size((ttbcr >> 16) & 7);
+		armv8->page_size = (ttbcr >> 14) & 3;
+		break;
+	case SYSTEM_CUREL_EL0:
+		armv8_dpm_modeswitch(dpm, ARMV8_64_EL1H);
+		/* fall through */
+	case SYSTEM_CUREL_EL1:
+		retval = dpm->instr_read_data_r0_64(dpm,
+				ARMV8_MRS(SYSTEM_TCR_EL1, 0),
+				&ttbcr_64);
+		armv8->va_size = 64 - (ttbcr_64 & 0x3F);
+		armv8->pa_size = armv8_pa_size((ttbcr_64 >> 32) & 7);
+		armv8->page_size = (ttbcr_64 >> 14) & 3;
+		armv8->armv8_mmu.ttbr1_used = (((ttbcr_64 >> 16) & 0x3F) != 0) ? 1 : 0;
+		armv8->armv8_mmu.ttbr0_mask  = 0x0000FFFFFFFFFFFF;
+		retval += dpm->instr_read_data_r0_64(dpm,
+				ARMV8_MRS(SYSTEM_TTBR0_EL1 | (armv8->armv8_mmu.ttbr1_used), 0),
+				&armv8->ttbr_base);
+		if (retval != ERROR_OK)
+			goto done;
+		break;
+	default:
+		LOG_ERROR("unknow core state");
+		retval = ERROR_FAIL;
+		break;
+	}
+	if (retval != ERROR_OK)
+		goto done;
+
+	if (armv8->armv8_mmu.ttbr1_used == 1)
+		LOG_INFO("TTBR0 access above %" PRIx64, (uint64_t)(armv8->armv8_mmu.ttbr0_mask));
+
+done:
+	armv8_dpm_modeswitch(dpm, ARM_MODE_ANY);
+	dpm->finish(dpm);
+	return retval;
+}
+
+/*  method adapted to cortex A : reused arm v4 v5 method*/
+int armv8_mmu_translate_va(struct target *target,  target_addr_t va, target_addr_t *val)
+{
+	return ERROR_OK;
+}
+
+/*  V8 method VA TO PA  */
+int armv8_mmu_translate_va_pa(struct target *target, target_addr_t va,
+	target_addr_t *val, int meminfo)
+{
+	struct armv8_common *armv8 = target_to_armv8(target);
+	struct arm *arm = target_to_arm(target);
+	struct arm_dpm *dpm = &armv8->dpm;
+	enum arm_mode target_mode = ARM_MODE_ANY;
+	uint32_t retval;
+	uint32_t instr = 0;
+	uint64_t par;
+
+	static const char * const shared_name[] = {
+			"Non-", "UNDEFINED ", "Outer ", "Inner "
+	};
+
+	static const char * const secure_name[] = {
+			"Secure", "Not Secure"
+	};
+
+	retval = dpm->prepare(dpm);
+	if (retval != ERROR_OK)
+		return retval;
+
+	switch (armv8_curel_from_core_mode(arm->core_mode)) {
+	case SYSTEM_CUREL_EL0:
+		instr = ARMV8_SYS(SYSTEM_ATS12E0R, 0);
+		/* can only execute instruction at EL2 */
+		target_mode = ARMV8_64_EL2H;
+		break;
+	case SYSTEM_CUREL_EL1:
+		instr = ARMV8_SYS(SYSTEM_ATS12E1R, 0);
+		/* can only execute instruction at EL2 */
+		target_mode = ARMV8_64_EL2H;
+		break;
+	case SYSTEM_CUREL_EL2:
+		instr = ARMV8_SYS(SYSTEM_ATS1E2R, 0);
+		break;
+	case SYSTEM_CUREL_EL3:
+		instr = ARMV8_SYS(SYSTEM_ATS1E3R, 0);
+		break;
+
+	default:
+		break;
+	};
+
+	if (target_mode != ARM_MODE_ANY)
+		armv8_dpm_modeswitch(dpm, target_mode);
+
+	/* write VA to R0 and execute translation instruction */
+	retval = dpm->instr_write_data_r0_64(dpm, instr, (uint64_t)va);
+	/* read result from PAR_EL1 */
+	if (retval == ERROR_OK)
+		retval = dpm->instr_read_data_r0_64(dpm, ARMV8_MRS(SYSTEM_PAR_EL1, 0), &par);
+
+	/* switch back to saved PE mode */
+	if (target_mode != ARM_MODE_ANY)
+		armv8_dpm_modeswitch(dpm, ARM_MODE_ANY);
+
+	dpm->finish(dpm);
+
+	if (retval != ERROR_OK)
+		return retval;
+
+	if (par & 1) {
+		LOG_ERROR("Address translation failed at stage %i, FST=%x, PTW=%i",
+				((int)(par >> 9) & 1)+1, (int)(par >> 1) & 0x3f, (int)(par >> 8) & 1);
+
+		*val = 0;
+		retval = ERROR_FAIL;
+	} else {
+		*val = (par & 0xFFFFFFFFF000UL) | (va & 0xFFF);
+		if (meminfo) {
+			int SH = (par >> 7) & 3;
+			int NS = (par >> 9) & 1;
+			int ATTR = (par >> 56) & 0xFF;
+
+			char *memtype = (ATTR & 0xF0) == 0 ? "Device Memory" : "Normal Memory";
+
+			LOG_USER("%sshareable, %s",
+					shared_name[SH], secure_name[NS]);
+			LOG_USER("%s", memtype);
+		}
+	}
+
+	return retval;
+}
+
+int armv8_handle_cache_info_command(struct command_context *cmd_ctx,
+	struct armv8_cache_common *armv8_cache)
+{
+	if (armv8_cache->info == -1) {
+		command_print(cmd_ctx, "cache not yet identified");
+		return ERROR_OK;
+	}
+
+	if (armv8_cache->display_cache_info)
+		armv8_cache->display_cache_info(cmd_ctx, armv8_cache);
+	return ERROR_OK;
+}
+
+int armv8_init_arch_info(struct target *target, struct armv8_common *armv8)
+{
+	struct arm *arm = &armv8->arm;
+	arm->arch_info = armv8;
+	target->arch_info = &armv8->arm;
+	/*  target is useful in all function arm v4 5 compatible */
+	armv8->arm.target = target;
+	armv8->arm.common_magic = ARM_COMMON_MAGIC;
+	armv8->common_magic = ARMV8_COMMON_MAGIC;
+
+	armv8->armv8_mmu.armv8_cache.l2_cache = NULL;
+	armv8->armv8_mmu.armv8_cache.info = -1;
+	armv8->armv8_mmu.armv8_cache.flush_all_data_cache = NULL;
+	armv8->armv8_mmu.armv8_cache.display_cache_info = NULL;
+	return ERROR_OK;
+}
+
+int armv8_aarch64_state(struct target *target)
+{
+	struct arm *arm = target_to_arm(target);
+
+	if (arm->common_magic != ARM_COMMON_MAGIC) {
+		LOG_ERROR("BUG: called for a non-ARM target");
+		return ERROR_FAIL;
+	}
+
+	LOG_USER("target halted in %s state due to %s, current mode: %s\n"
+		"cpsr: 0x%8.8" PRIx32 " pc: 0x%" PRIx64 "%s",
+		armv8_state_strings[arm->core_state],
+		debug_reason_name(target),
+		armv8_mode_name(arm->core_mode),
+		buf_get_u32(arm->cpsr->value, 0, 32),
+		buf_get_u64(arm->pc->value, 0, 64),
+		arm->is_semihosting ? ", semihosting" : "");
+
+	return ERROR_OK;
+}
+
+int armv8_arch_state(struct target *target)
+{
+	static const char * const state[] = {
+		"disabled", "enabled"
+	};
+
+	struct armv8_common *armv8 = target_to_armv8(target);
+	struct arm *arm = &armv8->arm;
+
+	if (armv8->common_magic != ARMV8_COMMON_MAGIC) {
+		LOG_ERROR("BUG: called for a non-Armv8 target");
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	if (arm->core_state == ARM_STATE_AARCH64)
+		armv8_aarch64_state(target);
+	else
+		arm_arch_state(target);
+
+	LOG_USER("MMU: %s, D-Cache: %s, I-Cache: %s",
+		state[armv8->armv8_mmu.mmu_enabled],
+		state[armv8->armv8_mmu.armv8_cache.d_u_cache_enabled],
+		state[armv8->armv8_mmu.armv8_cache.i_cache_enabled]);
+
+	if (arm->core_mode == ARM_MODE_ABT)
+		armv8_show_fault_registers(target);
+
+	if (target->debug_reason == DBG_REASON_WATCHPOINT)
+		LOG_USER("Watchpoint triggered at PC %#08x",
+			(unsigned) armv8->dpm.wp_pc);
+
+	return ERROR_OK;
+}
+
+static const struct {
+	unsigned id;
+	const char *name;
+	unsigned bits;
+	enum arm_mode mode;
+	enum reg_type type;
+	const char *group;
+	const char *feature;
+} armv8_regs[] = {
+	{ ARMV8_R0,  "x0",  64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R1,  "x1",  64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R2,  "x2",  64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R3,  "x3",  64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R4,  "x4",  64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R5,  "x5",  64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R6,  "x6",  64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R7,  "x7",  64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R8,  "x8",  64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R9,  "x9",  64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R10, "x10", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R11, "x11", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R12, "x12", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R13, "x13", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R14, "x14", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R15, "x15", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R16, "x16", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R17, "x17", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R18, "x18", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R19, "x19", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R20, "x20", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R21, "x21", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R22, "x22", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R23, "x23", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R24, "x24", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R25, "x25", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R26, "x26", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R27, "x27", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R28, "x28", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R29, "x29", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R30, "x30", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+
+	{ ARMV8_SP, "sp", 64, ARM_MODE_ANY, REG_TYPE_DATA_PTR, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_PC,  "pc", 64, ARM_MODE_ANY, REG_TYPE_CODE_PTR, "general", "org.gnu.gdb.aarch64.core" },
+
+	{ ARMV8_xPSR, "CPSR", 32, ARM_MODE_ANY, REG_TYPE_UINT32, "general", "org.gnu.gdb.aarch64.core" },
+
+	{ ARMV8_ELR_EL1, "ELR_EL1", 64, ARMV8_64_EL1H, REG_TYPE_CODE_PTR, "banked", "net.sourceforge.openocd.banked" },
+	{ ARMV8_ESR_EL1, "ESR_EL1", 32, ARMV8_64_EL1H, REG_TYPE_UINT32, "banked", "net.sourceforge.openocd.banked" },
+	{ ARMV8_SPSR_EL1, "SPSR_EL1", 32, ARMV8_64_EL1H, REG_TYPE_UINT32, "banked", "net.sourceforge.openocd.banked" },
+
+	{ ARMV8_ELR_EL2, "ELR_EL2", 64, ARMV8_64_EL2H, REG_TYPE_CODE_PTR, "banked", "net.sourceforge.openocd.banked" },
+	{ ARMV8_ESR_EL2, "ESR_EL2", 32, ARMV8_64_EL2H, REG_TYPE_UINT32, "banked", "net.sourceforge.openocd.banked" },
+	{ ARMV8_SPSR_EL2, "SPSR_EL2", 32, ARMV8_64_EL2H, REG_TYPE_UINT32, "banked", "net.sourceforge.openocd.banked" },
+
+	{ ARMV8_ELR_EL3, "ELR_EL3", 64, ARMV8_64_EL3H, REG_TYPE_CODE_PTR, "banked", "net.sourceforge.openocd.banked" },
+	{ ARMV8_ESR_EL3, "ESR_EL3", 32, ARMV8_64_EL3H, REG_TYPE_UINT32, "banked", "net.sourceforge.openocd.banked" },
+	{ ARMV8_SPSR_EL3, "SPSR_EL3", 32, ARMV8_64_EL3H, REG_TYPE_UINT32, "banked", "net.sourceforge.openocd.banked" },
+};
+
+static const struct {
+	unsigned id;
+	const char *name;
+	unsigned bits;
+	enum arm_mode mode;
+	enum reg_type type;
+	const char *group;
+	const char *feature;
+} armv8_regs32[] = {
+	{ ARMV8_R0,  "r0",  32, ARM_MODE_ANY, REG_TYPE_UINT32, "general", "org.gnu.gdb.arm.core" },
+	{ ARMV8_R1,  "r1",  32, ARM_MODE_ANY, REG_TYPE_UINT32, "general", "org.gnu.gdb.arm.core" },
+	{ ARMV8_R2,  "r2",  32, ARM_MODE_ANY, REG_TYPE_UINT32, "general", "org.gnu.gdb.arm.core" },
+	{ ARMV8_R3,  "r3",  32, ARM_MODE_ANY, REG_TYPE_UINT32, "general", "org.gnu.gdb.arm.core" },
+	{ ARMV8_R4,  "r4",  32, ARM_MODE_ANY, REG_TYPE_UINT32, "general", "org.gnu.gdb.arm.core" },
+	{ ARMV8_R5,  "r5",  32, ARM_MODE_ANY, REG_TYPE_UINT32, "general", "org.gnu.gdb.arm.core" },
+	{ ARMV8_R6,  "r6",  32, ARM_MODE_ANY, REG_TYPE_UINT32, "general", "org.gnu.gdb.arm.core" },
+	{ ARMV8_R7,  "r7",  32, ARM_MODE_ANY, REG_TYPE_UINT32, "general", "org.gnu.gdb.arm.core" },
+	{ ARMV8_R8,  "r8",  32, ARM_MODE_ANY, REG_TYPE_UINT32, "general", "org.gnu.gdb.arm.core" },
+	{ ARMV8_R9,  "r9",  32, ARM_MODE_ANY, REG_TYPE_UINT32, "general", "org.gnu.gdb.arm.core" },
+	{ ARMV8_R10, "r10", 32, ARM_MODE_ANY, REG_TYPE_UINT32, "general", "org.gnu.gdb.arm.core" },
+	{ ARMV8_R11, "r11", 32, ARM_MODE_ANY, REG_TYPE_UINT32, "general", "org.gnu.gdb.arm.core" },
+	{ ARMV8_R12, "r12", 32, ARM_MODE_ANY, REG_TYPE_UINT32, "general", "org.gnu.gdb.arm.core" },
+	{ ARMV8_R13, "sp", 32, ARM_MODE_ANY, REG_TYPE_DATA_PTR, "general", "org.gnu.gdb.arm.core" },
+	{ ARMV8_R14, "lr",  32, ARM_MODE_ANY, REG_TYPE_CODE_PTR, "general", "org.gnu.gdb.arm.core" },
+	{ ARMV8_PC, "pc",   32, ARM_MODE_ANY, REG_TYPE_CODE_PTR, "general", "org.gnu.gdb.arm.core" },
+	{ ARMV8_xPSR, "cpsr", 32, ARM_MODE_ANY, REG_TYPE_UINT32, "general", "org.gnu.gdb.arm.core" },
+};
+
+#define ARMV8_NUM_REGS ARRAY_SIZE(armv8_regs)
+#define ARMV8_NUM_REGS32 ARRAY_SIZE(armv8_regs32)
+
+static int armv8_get_core_reg(struct reg *reg)
+{
+	struct arm_reg *armv8_reg = reg->arch_info;
+	struct target *target = armv8_reg->target;
+	struct arm *arm = target_to_arm(target);
+
+	if (target->state != TARGET_HALTED)
+		return ERROR_TARGET_NOT_HALTED;
+
+	return arm->read_core_reg(target, reg, armv8_reg->num, arm->core_mode);
+}
+
+static int armv8_set_core_reg(struct reg *reg, uint8_t *buf)
+{
+	struct arm_reg *armv8_reg = reg->arch_info;
+	struct target *target = armv8_reg->target;
+	struct arm *arm = target_to_arm(target);
+	uint64_t value = buf_get_u64(buf, 0, 64);
+
+	if (target->state != TARGET_HALTED)
+		return ERROR_TARGET_NOT_HALTED;
+
+	if (reg == arm->cpsr) {
+		armv8_set_cpsr(arm, (uint32_t)value);
+	} else {
+		buf_set_u64(reg->value, 0, 64, value);
+		reg->valid = 1;
+	}
+
+	reg->dirty = 1;
+
+	return ERROR_OK;
+}
+
+static const struct reg_arch_type armv8_reg_type = {
+	.get = armv8_get_core_reg,
+	.set = armv8_set_core_reg,
+};
+
+static int armv8_get_core_reg32(struct reg *reg)
+{
+	struct arm_reg *armv8_reg = reg->arch_info;
+	struct target *target = armv8_reg->target;
+	struct arm *arm = target_to_arm(target);
+	struct reg_cache *cache = arm->core_cache;
+	struct reg *reg64;
+	int retval;
+
+	/* get the corresponding Aarch64 register */
+	reg64 = cache->reg_list + armv8_reg->num;
+	if (reg64->valid) {
+		reg->valid = true;
+		return ERROR_OK;
+	}
+
+	retval = arm->read_core_reg(target, reg64, armv8_reg->num, arm->core_mode);
+	if (retval == ERROR_OK)
+		reg->valid = reg64->valid;
+
+	return retval;
+}
+
+static int armv8_set_core_reg32(struct reg *reg, uint8_t *buf)
+{
+	struct arm_reg *armv8_reg = reg->arch_info;
+	struct target *target = armv8_reg->target;
+	struct arm *arm = target_to_arm(target);
+	struct reg_cache *cache = arm->core_cache;
+	struct reg *reg64 = cache->reg_list + armv8_reg->num;
+	uint32_t value = buf_get_u32(buf, 0, 32);
+
+	if (reg64 == arm->cpsr) {
+		armv8_set_cpsr(arm, value);
+	} else {
+		buf_set_u32(reg->value, 0, 32, value);
+		reg->valid = 1;
+		reg64->valid = 1;
+	}
+
+	reg64->dirty = 1;
+
+	return ERROR_OK;
+}
+
+static const struct reg_arch_type armv8_reg32_type = {
+	.get = armv8_get_core_reg32,
+	.set = armv8_set_core_reg32,
+};
+
+/** Builds cache of architecturally defined registers.  */
+struct reg_cache *armv8_build_reg_cache(struct target *target)
+{
+	struct armv8_common *armv8 = target_to_armv8(target);
+	struct arm *arm = &armv8->arm;
+	int num_regs = ARMV8_NUM_REGS;
+	int num_regs32 = ARMV8_NUM_REGS32;
+	struct reg_cache **cache_p = register_get_last_cache_p(&target->reg_cache);
+	struct reg_cache *cache = malloc(sizeof(struct reg_cache));
+	struct reg_cache *cache32 = malloc(sizeof(struct reg_cache));
+	struct reg *reg_list = calloc(num_regs, sizeof(struct reg));
+	struct reg *reg_list32 = calloc(num_regs32, sizeof(struct reg));
+	struct arm_reg *arch_info = calloc(num_regs, sizeof(struct arm_reg));
+	struct reg_feature *feature;
+	int i;
+
+	/* Build the process context cache */
+	cache->name = "Aarch64 registers";
+	cache->next = cache32;
+	cache->reg_list = reg_list;
+	cache->num_regs = num_regs;
+
+	for (i = 0; i < num_regs; i++) {
+		arch_info[i].num = armv8_regs[i].id;
+		arch_info[i].mode = armv8_regs[i].mode;
+		arch_info[i].target = target;
+		arch_info[i].arm = arm;
+
+		reg_list[i].name = armv8_regs[i].name;
+		reg_list[i].size = armv8_regs[i].bits;
+		reg_list[i].value = &arch_info[i].value[0];
+		reg_list[i].type = &armv8_reg_type;
+		reg_list[i].arch_info = &arch_info[i];
+
+		reg_list[i].group = armv8_regs[i].group;
+		reg_list[i].number = i;
+		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 = armv8_regs[i].feature;
+			reg_list[i].feature = feature;
+		} else
+			LOG_ERROR("unable to allocate feature list");
+
+		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 = armv8_regs[i].type;
+		else
+			LOG_ERROR("unable to allocate reg type list");
+	}
+
+	arm->cpsr = reg_list + ARMV8_xPSR;
+	arm->pc = reg_list + ARMV8_PC;
+	arm->core_cache = cache;
+
+	/* shadow cache for ARM mode registers */
+	cache32->name = "Aarch32 registers";
+	cache32->next = NULL;
+	cache32->reg_list = reg_list32;
+	cache32->num_regs = num_regs32;
+
+	for (i = 0; i < num_regs32; i++) {
+		reg_list32[i].name = armv8_regs32[i].name;
+		reg_list32[i].size = armv8_regs32[i].bits;
+		reg_list32[i].value = &arch_info[armv8_regs32[i].id].value[0];
+		reg_list32[i].type = &armv8_reg32_type;
+		reg_list32[i].arch_info = &arch_info[armv8_regs32[i].id];
+		reg_list32[i].group = armv8_regs32[i].group;
+		reg_list32[i].number = i;
+		reg_list32[i].exist = true;
+		reg_list32[i].caller_save = true;
+
+		feature = calloc(1, sizeof(struct reg_feature));
+		if (feature) {
+			feature->name = armv8_regs32[i].feature;
+			reg_list32[i].feature = feature;
+		} else
+			LOG_ERROR("unable to allocate feature list");
+
+		reg_list32[i].reg_data_type = calloc(1, sizeof(struct reg_data_type));
+		if (reg_list32[i].reg_data_type)
+			reg_list32[i].reg_data_type->type = armv8_regs32[i].type;
+		else
+			LOG_ERROR("unable to allocate reg type list");
+	}
+
+	(*cache_p) = cache;
+	return cache;
+}
+
+struct reg *armv8_reg_current(struct arm *arm, unsigned regnum)
+{
+	struct reg *r;
+
+	if (regnum > (ARMV8_LAST_REG - 1))
+		return NULL;
+
+	r = arm->core_cache->reg_list + regnum;
+	return r;
+}
+
+const struct command_registration armv8_command_handlers[] = {
+	{
+		.chain = dap_command_handlers,
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+
+int armv8_get_gdb_reg_list(struct target *target,
+	struct reg **reg_list[], int *reg_list_size,
+	enum target_register_class reg_class)
+{
+	struct arm *arm = target_to_arm(target);
+	int i;
+
+	if (arm->core_state == ARM_STATE_AARCH64) {
+
+		LOG_DEBUG("Creating Aarch64 register list for target %s", target_name(target));
+
+		switch (reg_class) {
+		case REG_CLASS_GENERAL:
+			*reg_list_size = ARMV8_ELR_EL1;
+			*reg_list = malloc(sizeof(struct reg *) * (*reg_list_size));
+
+			for (i = 0; i < *reg_list_size; i++)
+					(*reg_list)[i] = armv8_reg_current(arm, i);
+			return ERROR_OK;
+
+		case REG_CLASS_ALL:
+			*reg_list_size = ARMV8_LAST_REG;
+			*reg_list = malloc(sizeof(struct reg *) * (*reg_list_size));
+
+			for (i = 0; i < *reg_list_size; i++)
+					(*reg_list)[i] = armv8_reg_current(arm, i);
+
+			return ERROR_OK;
+
+		default:
+			LOG_ERROR("not a valid register class type in query.");
+			return ERROR_FAIL;
+		}
+	} else {
+		struct reg_cache *cache32 = arm->core_cache->next;
+
+		LOG_DEBUG("Creating Aarch32 register list for target %s", target_name(target));
+
+		switch (reg_class) {
+		case REG_CLASS_GENERAL:
+		case REG_CLASS_ALL:
+			*reg_list_size = cache32->num_regs;
+			*reg_list = malloc(sizeof(struct reg *) * (*reg_list_size));
+
+			for (i = 0; i < *reg_list_size; i++)
+				(*reg_list)[i] = cache32->reg_list + i;
+
+			return ERROR_OK;
+		default:
+			LOG_ERROR("not a valid register class type in query.");
+			return ERROR_FAIL;
+		}
+	}
+}
+
+int armv8_set_dbgreg_bits(struct armv8_common *armv8, unsigned int reg, unsigned long mask, unsigned long value)
+{
+	uint32_t tmp;
+
+	/* Read register */
+	int retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + reg, &tmp);
+	if (ERROR_OK != retval)
+		return retval;
+
+	/* clear bitfield */
+	tmp &= ~mask;
+	/* put new value */
+	tmp |= value & mask;
+
+	/* write new value */
+	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + reg, tmp);
+	return retval;
+}
diff --git a/src/target/armv8.h b/src/target/armv8.h
new file mode 100644
index 0000000..02663ca
--- /dev/null
+++ b/src/target/armv8.h
@@ -0,0 +1,282 @@
+/***************************************************************************
+ *   Copyright (C) 2015 by David Ung                                       *
+ *                                                                         *
+ *   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, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ ***************************************************************************/
+
+#ifndef OPENOCD_TARGET_ARMV8_H
+#define OPENOCD_TARGET_ARMV8_H
+
+#include "arm_adi_v5.h"
+#include "arm.h"
+#include "armv4_5_mmu.h"
+#include "armv4_5_cache.h"
+#include "armv8_dpm.h"
+#include "arm_cti.h"
+
+enum {
+	ARMV8_R0 = 0,
+	ARMV8_R1,
+	ARMV8_R2,
+	ARMV8_R3,
+	ARMV8_R4,
+	ARMV8_R5,
+	ARMV8_R6,
+	ARMV8_R7,
+	ARMV8_R8,
+	ARMV8_R9,
+	ARMV8_R10,
+	ARMV8_R11,
+	ARMV8_R12,
+	ARMV8_R13,
+	ARMV8_R14,
+	ARMV8_R15,
+	ARMV8_R16,
+	ARMV8_R17,
+	ARMV8_R18,
+	ARMV8_R19,
+	ARMV8_R20,
+	ARMV8_R21,
+	ARMV8_R22,
+	ARMV8_R23,
+	ARMV8_R24,
+	ARMV8_R25,
+	ARMV8_R26,
+	ARMV8_R27,
+	ARMV8_R28,
+	ARMV8_R29,
+	ARMV8_R30,
+
+	ARMV8_SP = 31,
+	ARMV8_PC = 32,
+	ARMV8_xPSR = 33,
+
+	ARMV8_ELR_EL1 = 34,
+	ARMV8_ESR_EL1 = 35,
+	ARMV8_SPSR_EL1 = 36,
+
+	ARMV8_ELR_EL2 = 37,
+	ARMV8_ESR_EL2 = 38,
+	ARMV8_SPSR_EL2 = 39,
+
+	ARMV8_ELR_EL3 = 40,
+	ARMV8_ESR_EL3 = 41,
+	ARMV8_SPSR_EL3 = 42,
+
+	ARMV8_LAST_REG,
+};
+
+
+#define ARMV8_COMMON_MAGIC 0x0A450AAA
+
+/* VA to PA translation operations opc2 values*/
+#define V2PCWPR  0
+#define V2PCWPW  1
+#define V2PCWUR  2
+#define V2PCWUW  3
+#define V2POWPR  4
+#define V2POWPW  5
+#define V2POWUR  6
+#define V2POWUW  7
+/*   L210/L220 cache controller support */
+struct armv8_l2x_cache {
+	uint32_t base;
+	uint32_t way;
+};
+
+struct armv8_cachesize {
+	uint32_t level_num;
+	/*  cache dimensionning */
+	uint32_t linelen;
+	uint32_t associativity;
+	uint32_t nsets;
+	uint32_t cachesize;
+	/* info for set way operation on cache */
+	uint32_t index;
+	uint32_t index_shift;
+	uint32_t way;
+	uint32_t way_shift;
+};
+
+/* information about one architecture cache at any level */
+struct armv8_arch_cache {
+	int ctype;				/* cache type, CLIDR encoding */
+	struct armv8_cachesize d_u_size;	/* data cache */
+	struct armv8_cachesize i_size;		/* instruction cache */
+};
+
+struct armv8_cache_common {
+	int info;
+	int loc;
+	uint32_t iminline;
+	uint32_t dminline;
+	struct armv8_arch_cache arch[6];	/* cache info, L1 - L7 */
+	int i_cache_enabled;
+	int d_u_cache_enabled;
+
+	/* l2 external unified cache if some */
+	void *l2_cache;
+	int (*flush_all_data_cache)(struct target *target);
+	int (*display_cache_info)(struct command_context *cmd_ctx,
+			struct armv8_cache_common *armv8_cache);
+};
+
+struct armv8_mmu_common {
+	/* following field mmu working way */
+	int32_t ttbr1_used; /*  -1 not initialized, 0 no ttbr1 1 ttbr1 used and  */
+	uint64_t ttbr0_mask;/*  masked to be used  */
+
+	uint32_t ttbcr;     /* cache for ttbcr register */
+	uint32_t ttbr_mask[2];
+	uint32_t ttbr_range[2];
+
+	int (*read_physical_memory)(struct target *target, target_addr_t address,
+			uint32_t size, uint32_t count, uint8_t *buffer);
+	struct armv8_cache_common armv8_cache;
+	uint32_t mmu_enabled;
+};
+
+struct armv8_common {
+	struct arm arm;
+	int common_magic;
+	struct reg_cache *core_cache;
+
+	/* Core Debug Unit */
+	struct arm_dpm dpm;
+	uint32_t debug_base;
+	struct adiv5_ap *debug_ap;
+
+	const uint32_t *opcodes;
+
+	/* mdir */
+	uint8_t multi_processor_system;
+	uint8_t cluster_id;
+	uint8_t cpu_id;
+
+	/* armv8 aarch64 need below information for page translation */
+	uint8_t va_size;
+	uint8_t pa_size;
+	uint32_t page_size;
+	uint64_t ttbr_base;
+
+	struct armv8_mmu_common armv8_mmu;
+
+	struct arm_cti *cti;
+
+	/* Direct processor core register read and writes */
+	int (*read_reg_u64)(struct armv8_common *armv8, int num, uint64_t *value);
+	int (*write_reg_u64)(struct armv8_common *armv8, int num, uint64_t value);
+
+	int (*examine_debug_reason)(struct target *target);
+	int (*post_debug_entry)(struct target *target);
+
+	void (*pre_restore_context)(struct target *target);
+};
+
+static inline struct armv8_common *
+target_to_armv8(struct target *target)
+{
+	return container_of(target->arch_info, struct armv8_common, arm);
+}
+
+/* register offsets from armv8.debug_base */
+#define CPUV8_DBG_MAINID0		0xD00
+#define CPUV8_DBG_CPUFEATURE0	0xD20
+#define CPUV8_DBG_DBGFEATURE0	0xD28
+#define CPUV8_DBG_MEMFEATURE0	0xD38
+
+#define CPUV8_DBG_LOCKACCESS 0xFB0
+#define CPUV8_DBG_LOCKSTATUS 0xFB4
+
+#define CPUV8_DBG_EDESR		0x20
+#define CPUV8_DBG_EDECR		0x24
+#define CPUV8_DBG_WFAR0		0x30
+#define CPUV8_DBG_WFAR1		0x34
+#define CPUV8_DBG_DSCR		0x088
+#define CPUV8_DBG_DRCR		0x090
+#define CPUV8_DBG_PRCR		0x310
+#define CPUV8_DBG_PRSR		0x314
+
+#define CPUV8_DBG_DTRRX		0x080
+#define CPUV8_DBG_ITR		0x084
+#define CPUV8_DBG_SCR		0x088
+#define CPUV8_DBG_DTRTX		0x08c
+
+#define CPUV8_DBG_BVR_BASE	0x400
+#define CPUV8_DBG_BCR_BASE	0x408
+#define CPUV8_DBG_WVR_BASE	0x800
+#define CPUV8_DBG_WCR_BASE	0x808
+#define CPUV8_DBG_VCR		0x01C
+
+#define CPUV8_DBG_OSLAR		0x300
+
+#define CPUV8_DBG_AUTHSTATUS	0xFB8
+
+#define PAGE_SIZE_4KB				0x1000
+#define PAGE_SIZE_4KB_LEVEL0_BITS	39
+#define PAGE_SIZE_4KB_LEVEL1_BITS	30
+#define PAGE_SIZE_4KB_LEVEL2_BITS	21
+#define PAGE_SIZE_4KB_LEVEL3_BITS	12
+
+#define PAGE_SIZE_4KB_LEVEL0_MASK	((0x1FFULL) << PAGE_SIZE_4KB_LEVEL0_BITS)
+#define PAGE_SIZE_4KB_LEVEL1_MASK	((0x1FFULL) << PAGE_SIZE_4KB_LEVEL1_BITS)
+#define PAGE_SIZE_4KB_LEVEL2_MASK	((0x1FFULL) << PAGE_SIZE_4KB_LEVEL2_BITS)
+#define PAGE_SIZE_4KB_LEVEL3_MASK	((0x1FFULL) << PAGE_SIZE_4KB_LEVEL3_BITS)
+
+#define PAGE_SIZE_4KB_TRBBASE_MASK	0xFFFFFFFFF000
+
+int armv8_arch_state(struct target *target);
+int armv8_read_mpidr(struct armv8_common *armv8);
+int armv8_identify_cache(struct armv8_common *armv8);
+int armv8_init_arch_info(struct target *target, struct armv8_common *armv8);
+int armv8_mmu_translate_va_pa(struct target *target, target_addr_t va,
+		target_addr_t *val, int meminfo);
+int armv8_mmu_translate_va(struct target *target,  target_addr_t va, target_addr_t *val);
+
+int armv8_handle_cache_info_command(struct command_context *cmd_ctx,
+		struct armv8_cache_common *armv8_cache);
+
+void armv8_set_cpsr(struct arm *arm, uint32_t cpsr);
+
+static inline unsigned int armv8_curel_from_core_mode(enum arm_mode core_mode)
+{
+	switch (core_mode) {
+	/* Aarch32 modes */
+	case ARM_MODE_USR:
+		return 0;
+	case ARM_MODE_SVC:
+	case ARM_MODE_ABT: /* FIXME: EL3? */
+	case ARM_MODE_IRQ: /* FIXME: EL3? */
+	case ARM_MODE_FIQ: /* FIXME: EL3? */
+	case ARM_MODE_UND: /* FIXME: EL3? */
+	case ARM_MODE_SYS: /* FIXME: EL3? */
+		return 1;
+	/* case ARM_MODE_HYP:
+	 *     return 2;
+	 */
+	case ARM_MODE_MON:
+		return 3;
+	/* all Aarch64 modes */
+	default:
+		return (core_mode >> 6) & 3;
+	}
+}
+
+void armv8_select_reg_access(struct armv8_common *armv8, bool is_aarch64);
+int armv8_set_dbgreg_bits(struct armv8_common *armv8, unsigned int reg, unsigned long mask, unsigned long value);
+
+extern const struct command_registration armv8_command_handlers[];
+
+#endif /* OPENOCD_TARGET_ARMV8_H */
diff --git a/src/target/armv8_cache.c b/src/target/armv8_cache.c
new file mode 100644
index 0000000..7f610c9
--- /dev/null
+++ b/src/target/armv8_cache.c
@@ -0,0 +1,423 @@
+/***************************************************************************
+ *   Copyright (C) 2016 by Matthias Welwarsky                              *
+ *   matthias.welwarsky@sysgo.com                                          *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
+ ***************************************************************************/
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "armv8_cache.h"
+#include "armv8_dpm.h"
+#include "armv8_opcodes.h"
+
+/* CLIDR cache types */
+#define CACHE_LEVEL_HAS_UNIFIED_CACHE	0x4
+#define CACHE_LEVEL_HAS_D_CACHE		0x2
+#define CACHE_LEVEL_HAS_I_CACHE		0x1
+
+static int armv8_d_cache_sanity_check(struct armv8_common *armv8)
+{
+	struct armv8_cache_common *armv8_cache = &armv8->armv8_mmu.armv8_cache;
+
+	if (armv8_cache->d_u_cache_enabled)
+		return ERROR_OK;
+
+	return ERROR_TARGET_INVALID;
+}
+
+static int armv8_i_cache_sanity_check(struct armv8_common *armv8)
+{
+	struct armv8_cache_common *armv8_cache = &armv8->armv8_mmu.armv8_cache;
+
+	if (armv8_cache->i_cache_enabled)
+		return ERROR_OK;
+
+	return ERROR_TARGET_INVALID;
+}
+
+static int armv8_cache_d_inner_flush_level(struct armv8_common *armv8, struct armv8_cachesize *size, int cl)
+{
+	struct arm_dpm *dpm = armv8->arm.dpm;
+	int retval = ERROR_OK;
+	int32_t c_way, c_index = size->index;
+
+	LOG_DEBUG("cl %" PRId32, cl);
+	do {
+		c_way = size->way;
+		do {
+			uint32_t value = (c_index << size->index_shift)
+				| (c_way << size->way_shift) | (cl << 1);
+			/*
+			 * DC CISW - Clean and invalidate data cache
+			 * line by Set/Way.
+			 */
+			retval = dpm->instr_write_data_r0(dpm,
+					armv8_opcode(armv8, ARMV8_OPC_DCCISW), value);
+			if (retval != ERROR_OK)
+				goto done;
+			c_way -= 1;
+		} while (c_way >= 0);
+		c_index -= 1;
+	} while (c_index >= 0);
+
+ done:
+	return retval;
+}
+
+static int armv8_cache_d_inner_clean_inval_all(struct armv8_common *armv8)
+{
+	struct armv8_cache_common *cache = &(armv8->armv8_mmu.armv8_cache);
+	struct arm_dpm *dpm = armv8->arm.dpm;
+	int cl;
+	int retval;
+
+	retval = armv8_d_cache_sanity_check(armv8);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = dpm->prepare(dpm);
+	if (retval != ERROR_OK)
+		goto done;
+
+	for (cl = 0; cl < cache->loc; cl++) {
+		/* skip i-only caches */
+		if (cache->arch[cl].ctype < CACHE_LEVEL_HAS_D_CACHE)
+			continue;
+
+		armv8_cache_d_inner_flush_level(armv8, &cache->arch[cl].d_u_size, cl);
+	}
+
+	retval = dpm->finish(dpm);
+	return retval;
+
+done:
+	LOG_ERROR("clean invalidate failed");
+	dpm->finish(dpm);
+
+	return retval;
+}
+
+int armv8_cache_d_inner_flush_virt(struct armv8_common *armv8, target_addr_t va, size_t size)
+{
+	struct arm_dpm *dpm = armv8->arm.dpm;
+	struct armv8_cache_common *armv8_cache = &armv8->armv8_mmu.armv8_cache;
+	uint64_t linelen = armv8_cache->dminline;
+	target_addr_t va_line, va_end;
+	int retval;
+
+	retval = armv8_d_cache_sanity_check(armv8);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = dpm->prepare(dpm);
+	if (retval != ERROR_OK)
+		goto done;
+
+	va_line = va & (-linelen);
+	va_end = va + size;
+
+	while (va_line < va_end) {
+		/* DC CIVAC */
+		/* Aarch32: DCCIMVAC: ARMV4_5_MCR(15, 0, 0, 7, 14, 1) */
+		retval = dpm->instr_write_data_r0_64(dpm,
+				armv8_opcode(armv8, ARMV8_OPC_DCCIVAC), va_line);
+		if (retval != ERROR_OK)
+			goto done;
+		va_line += linelen;
+	}
+
+	dpm->finish(dpm);
+	return retval;
+
+done:
+	LOG_ERROR("d-cache invalidate failed");
+	dpm->finish(dpm);
+
+	return retval;
+}
+
+int armv8_cache_i_inner_inval_virt(struct armv8_common *armv8, target_addr_t va, size_t size)
+{
+	struct arm_dpm *dpm = armv8->arm.dpm;
+	struct armv8_cache_common *armv8_cache = &armv8->armv8_mmu.armv8_cache;
+	uint64_t linelen = armv8_cache->iminline;
+	target_addr_t va_line, va_end;
+	int retval;
+
+	retval = armv8_i_cache_sanity_check(armv8);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = dpm->prepare(dpm);
+	if (retval != ERROR_OK)
+		goto done;
+
+	va_line = va & (-linelen);
+	va_end = va + size;
+
+	while (va_line < va_end) {
+		/* IC IVAU - Invalidate instruction cache by VA to PoU. */
+		retval = dpm->instr_write_data_r0_64(dpm,
+				armv8_opcode(armv8, ARMV8_OPC_ICIVAU), va_line);
+		if (retval != ERROR_OK)
+			goto done;
+		va_line += linelen;
+	}
+
+	dpm->finish(dpm);
+	return retval;
+
+done:
+	LOG_ERROR("d-cache invalidate failed");
+	dpm->finish(dpm);
+
+	return retval;
+}
+
+static int armv8_handle_inner_cache_info_command(struct command_context *cmd_ctx,
+	struct armv8_cache_common *armv8_cache)
+{
+	int cl;
+
+	if (armv8_cache->info == -1) {
+		command_print(cmd_ctx, "cache not yet identified");
+		return ERROR_OK;
+	}
+
+	for (cl = 0; cl < armv8_cache->loc; cl++) {
+		struct armv8_arch_cache *arch = &(armv8_cache->arch[cl]);
+
+		if (arch->ctype & 1) {
+			command_print(cmd_ctx,
+				"L%d I-Cache: linelen %" PRIi32
+				", associativity %" PRIi32
+				", nsets %" PRIi32
+				", cachesize %" PRId32 " KBytes",
+				cl+1,
+				arch->i_size.linelen,
+				arch->i_size.associativity,
+				arch->i_size.nsets,
+				arch->i_size.cachesize);
+		}
+
+		if (arch->ctype >= 2) {
+			command_print(cmd_ctx,
+				"L%d D-Cache: linelen %" PRIi32
+				", associativity %" PRIi32
+				", nsets %" PRIi32
+				", cachesize %" PRId32 " KBytes",
+				cl+1,
+				arch->d_u_size.linelen,
+				arch->d_u_size.associativity,
+				arch->d_u_size.nsets,
+				arch->d_u_size.cachesize);
+		}
+	}
+
+	return ERROR_OK;
+}
+
+static int _armv8_flush_all_data(struct target *target)
+{
+	return armv8_cache_d_inner_clean_inval_all(target_to_armv8(target));
+}
+
+static int  armv8_flush_all_data(struct target *target)
+{
+	int retval = ERROR_FAIL;
+	/*  check that armv8_cache is correctly identify */
+	struct armv8_common *armv8 = target_to_armv8(target);
+	if (armv8->armv8_mmu.armv8_cache.info == -1) {
+		LOG_ERROR("trying to flush un-identified cache");
+		return retval;
+	}
+
+	if (target->smp) {
+		/*  look if all the other target have been flushed in order to flush level
+		 *  2 */
+		struct target_list *head;
+		struct target *curr;
+		head = target->head;
+		while (head != (struct target_list *)NULL) {
+			curr = head->target;
+			if (curr->state == TARGET_HALTED) {
+				LOG_INFO("Wait flushing data l1 on core %" PRId32, curr->coreid);
+				retval = _armv8_flush_all_data(curr);
+			}
+			head = head->next;
+		}
+	} else
+		retval = _armv8_flush_all_data(target);
+	return retval;
+}
+
+static int get_cache_info(struct arm_dpm *dpm, int cl, int ct, uint32_t *cache_reg)
+{
+	struct armv8_common *armv8 = dpm->arm->arch_info;
+	int retval = ERROR_OK;
+
+	/*  select cache level */
+	retval = dpm->instr_write_data_r0(dpm,
+			armv8_opcode(armv8, WRITE_REG_CSSELR),
+			(cl << 1) | (ct == 1 ? 1 : 0));
+	if (retval != ERROR_OK)
+		goto done;
+
+	retval = dpm->instr_read_data_r0(dpm,
+			armv8_opcode(armv8, READ_REG_CCSIDR),
+			cache_reg);
+ done:
+	return retval;
+}
+
+static struct armv8_cachesize decode_cache_reg(uint32_t cache_reg)
+{
+	struct armv8_cachesize size;
+	int i = 0;
+
+	size.linelen = 16 << (cache_reg & 0x7);
+	size.associativity = ((cache_reg >> 3) & 0x3ff) + 1;
+	size.nsets = ((cache_reg >> 13) & 0x7fff) + 1;
+	size.cachesize = size.linelen * size.associativity * size.nsets / 1024;
+
+	/*  compute info for set way operation on cache */
+	size.index_shift = (cache_reg & 0x7) + 4;
+	size.index = (cache_reg >> 13) & 0x7fff;
+	size.way = ((cache_reg >> 3) & 0x3ff);
+
+	while (((size.way << i) & 0x80000000) == 0)
+		i++;
+	size.way_shift = i;
+
+	return size;
+}
+
+int armv8_identify_cache(struct armv8_common *armv8)
+{
+	/*  read cache descriptor */
+	int retval = ERROR_FAIL;
+	struct arm_dpm *dpm = armv8->arm.dpm;
+	uint32_t csselr, clidr, ctr;
+	uint32_t cache_reg;
+	int cl, ctype;
+	struct armv8_cache_common *cache = &(armv8->armv8_mmu.armv8_cache);
+
+	retval = dpm->prepare(dpm);
+	if (retval != ERROR_OK)
+		goto done;
+
+	/* retrieve CTR */
+	retval = dpm->instr_read_data_r0(dpm,
+			armv8_opcode(armv8, READ_REG_CTR), &ctr);
+	if (retval != ERROR_OK)
+		goto done;
+
+	cache->iminline = 4UL << (ctr & 0xf);
+	cache->dminline = 4UL << ((ctr & 0xf0000) >> 16);
+	LOG_DEBUG("ctr %" PRIx32 " ctr.iminline %" PRId32 " ctr.dminline %" PRId32,
+		 ctr, cache->iminline, cache->dminline);
+
+	/*  retrieve CLIDR */
+	retval = dpm->instr_read_data_r0(dpm,
+			armv8_opcode(armv8, READ_REG_CLIDR), &clidr);
+	if (retval != ERROR_OK)
+		goto done;
+
+	cache->loc = (clidr & 0x7000000) >> 24;
+	LOG_DEBUG("Number of cache levels to PoC %" PRId32, cache->loc);
+
+	/*  retrieve selected cache for later restore
+	 *  MRC p15, 2,<Rd>, c0, c0, 0; Read CSSELR */
+	retval = dpm->instr_read_data_r0(dpm,
+			armv8_opcode(armv8, READ_REG_CSSELR), &csselr);
+	if (retval != ERROR_OK)
+		goto done;
+
+	/* retrieve all available inner caches */
+	for (cl = 0; cl < cache->loc; clidr >>= 3, cl++) {
+
+		/* isolate cache type at current level */
+		ctype = clidr & 7;
+
+		/* skip reserved values */
+		if (ctype > CACHE_LEVEL_HAS_UNIFIED_CACHE)
+			continue;
+
+		/* separate d or unified d/i cache at this level ? */
+		if (ctype & (CACHE_LEVEL_HAS_UNIFIED_CACHE | CACHE_LEVEL_HAS_D_CACHE)) {
+			/* retrieve d-cache info */
+			retval = get_cache_info(dpm, cl, 0, &cache_reg);
+			if (retval != ERROR_OK)
+				goto done;
+			cache->arch[cl].d_u_size = decode_cache_reg(cache_reg);
+
+			LOG_DEBUG("data/unified cache index %d << %d, way %d << %d",
+					cache->arch[cl].d_u_size.index,
+					cache->arch[cl].d_u_size.index_shift,
+					cache->arch[cl].d_u_size.way,
+					cache->arch[cl].d_u_size.way_shift);
+
+			LOG_DEBUG("cacheline %d bytes %d KBytes asso %d ways",
+					cache->arch[cl].d_u_size.linelen,
+					cache->arch[cl].d_u_size.cachesize,
+					cache->arch[cl].d_u_size.associativity);
+		}
+
+		/* separate i-cache at this level ? */
+		if (ctype & CACHE_LEVEL_HAS_I_CACHE) {
+			/* retrieve i-cache info */
+			retval = get_cache_info(dpm, cl, 1, &cache_reg);
+			if (retval != ERROR_OK)
+				goto done;
+			cache->arch[cl].i_size = decode_cache_reg(cache_reg);
+
+			LOG_DEBUG("instruction cache index %d << %d, way %d << %d",
+					cache->arch[cl].i_size.index,
+					cache->arch[cl].i_size.index_shift,
+					cache->arch[cl].i_size.way,
+					cache->arch[cl].i_size.way_shift);
+
+			LOG_DEBUG("cacheline %d bytes %d KBytes asso %d ways",
+					cache->arch[cl].i_size.linelen,
+					cache->arch[cl].i_size.cachesize,
+					cache->arch[cl].i_size.associativity);
+		}
+
+		cache->arch[cl].ctype = ctype;
+	}
+
+	/*  restore selected cache  */
+	dpm->instr_write_data_r0(dpm,
+			armv8_opcode(armv8, WRITE_REG_CSSELR), csselr);
+	if (retval != ERROR_OK)
+		goto done;
+
+	armv8->armv8_mmu.armv8_cache.info = 1;
+
+	/*  if no l2 cache initialize l1 data cache flush function function */
+	if (armv8->armv8_mmu.armv8_cache.flush_all_data_cache == NULL) {
+		armv8->armv8_mmu.armv8_cache.display_cache_info =
+			armv8_handle_inner_cache_info_command;
+		armv8->armv8_mmu.armv8_cache.flush_all_data_cache =
+			armv8_flush_all_data;
+	}
+
+done:
+	dpm->finish(dpm);
+	return retval;
+
+}
diff --git a/src/target/armv8_cache.h b/src/target/armv8_cache.h
new file mode 100644
index 0000000..fa46e16
--- /dev/null
+++ b/src/target/armv8_cache.h
@@ -0,0 +1,26 @@
+/***************************************************************************
+ *   Copyright (C) 2016 by Matthias Welwarsky                              *
+ *   matthias.welwarsky@sysgo.com                                          *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
+ ***************************************************************************/
+#ifndef OPENOCD_TARGET_ARMV8_CACHE_H_
+#define OPENOCD_TARGET_ARMV8_CACHE_H_
+
+#include "armv8.h"
+
+extern int armv8_cache_d_inner_flush_virt(struct armv8_common *armv8, target_addr_t va, size_t size);
+extern int armv8_cache_i_inner_inval_virt(struct armv8_common *armv8, target_addr_t va, size_t size);
+
+#endif /* OPENOCD_TARGET_ARMV8_CACHE_H_ */
diff --git a/src/target/armv8_dpm.c b/src/target/armv8_dpm.c
new file mode 100644
index 0000000..f4e7a07
--- /dev/null
+++ b/src/target/armv8_dpm.c
@@ -0,0 +1,1472 @@
+/*
+ * Copyright (C) 2009 by David Brownell
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "arm.h"
+#include "armv8.h"
+#include "armv8_dpm.h"
+#include <jtag/jtag.h>
+#include "register.h"
+#include "breakpoints.h"
+#include "target_type.h"
+#include "armv8_opcodes.h"
+
+#include "helper/time_support.h"
+
+/* T32 ITR format */
+#define T32_FMTITR(instr) (((instr & 0x0000FFFF) << 16) | ((instr & 0xFFFF0000) >> 16))
+
+/**
+ * @file
+ * Implements various ARM DPM operations using architectural debug registers.
+ * These routines layer over core-specific communication methods to cope with
+ * implementation differences between cores like ARM1136 and Cortex-A8.
+ *
+ * The "Debug Programmers' Model" (DPM) for ARMv6 and ARMv7 is defined by
+ * Part C (Debug Architecture) of the ARM Architecture Reference Manual,
+ * ARMv7-A and ARMv7-R edition (ARM DDI 0406B).  In OpenOCD, DPM operations
+ * are abstracted through internal programming interfaces to share code and
+ * to minimize needless differences in debug behavior between cores.
+ */
+
+/**
+ * Get core state from EDSCR, without necessity to retrieve CPSR
+ */
+enum arm_state armv8_dpm_get_core_state(struct arm_dpm *dpm)
+{
+	int el = (dpm->dscr >> 8) & 0x3;
+	int rw = (dpm->dscr >> 10) & 0xF;
+
+	dpm->last_el = el;
+
+	/* In Debug state, each bit gives the current Execution state of each EL */
+	if ((rw >> el) & 0b1)
+		return ARM_STATE_AARCH64;
+
+	return ARM_STATE_ARM;
+}
+
+/*----------------------------------------------------------------------*/
+
+static int dpmv8_write_dcc(struct armv8_common *armv8, uint32_t data)
+{
+	return mem_ap_write_u32(armv8->debug_ap,
+				armv8->debug_base + CPUV8_DBG_DTRRX, data);
+}
+
+static int dpmv8_write_dcc_64(struct armv8_common *armv8, uint64_t data)
+{
+	int ret;
+	ret = mem_ap_write_u32(armv8->debug_ap,
+			       armv8->debug_base + CPUV8_DBG_DTRRX, data);
+	if (ret == ERROR_OK)
+		ret = mem_ap_write_u32(armv8->debug_ap,
+				armv8->debug_base + CPUV8_DBG_DTRTX, data >> 32);
+	return ret;
+}
+
+static int dpmv8_read_dcc(struct armv8_common *armv8, uint32_t *data,
+	uint32_t *dscr_p)
+{
+	uint32_t dscr = DSCR_ITE;
+	int retval;
+
+	if (dscr_p)
+		dscr = *dscr_p;
+
+	/* Wait for DTRRXfull */
+	long long then = timeval_ms();
+	while ((dscr & DSCR_DTR_TX_FULL) == 0) {
+		retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUV8_DBG_DSCR,
+				&dscr);
+		if (retval != ERROR_OK)
+			return retval;
+		if (timeval_ms() > then + 1000) {
+			LOG_ERROR("Timeout waiting for read dcc");
+			return ERROR_FAIL;
+		}
+	}
+
+	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+					    armv8->debug_base + CPUV8_DBG_DTRTX,
+					    data);
+	if (retval != ERROR_OK)
+		return retval;
+
+	if (dscr_p)
+		*dscr_p = dscr;
+
+	return retval;
+}
+
+static int dpmv8_read_dcc_64(struct armv8_common *armv8, uint64_t *data,
+	uint32_t *dscr_p)
+{
+	uint32_t dscr = DSCR_ITE;
+	uint32_t higher;
+	int retval;
+
+	if (dscr_p)
+		dscr = *dscr_p;
+
+	/* Wait for DTRRXfull */
+	long long then = timeval_ms();
+	while ((dscr & DSCR_DTR_TX_FULL) == 0) {
+		retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUV8_DBG_DSCR,
+				&dscr);
+		if (retval != ERROR_OK)
+			return retval;
+		if (timeval_ms() > then + 1000) {
+			LOG_ERROR("Timeout waiting for DTR_TX_FULL, dscr = 0x%08" PRIx32, dscr);
+			return ERROR_FAIL;
+		}
+	}
+
+	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+					    armv8->debug_base + CPUV8_DBG_DTRTX,
+					    (uint32_t *)data);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+					    armv8->debug_base + CPUV8_DBG_DTRRX,
+					    &higher);
+	if (retval != ERROR_OK)
+		return retval;
+
+	*data = *(uint32_t *)data | (uint64_t)higher << 32;
+
+	if (dscr_p)
+		*dscr_p = dscr;
+
+	return retval;
+}
+
+static int dpmv8_dpm_prepare(struct arm_dpm *dpm)
+{
+	struct armv8_common *armv8 = dpm->arm->arch_info;
+	uint32_t dscr;
+	int retval;
+
+	/* set up invariant:  ITE is set after ever DPM operation */
+	long long then = timeval_ms();
+	for (;; ) {
+		retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUV8_DBG_DSCR,
+				&dscr);
+		if (retval != ERROR_OK)
+			return retval;
+		if ((dscr & DSCR_ITE) != 0)
+			break;
+		if (timeval_ms() > then + 1000) {
+			LOG_ERROR("Timeout waiting for dpm prepare");
+			return ERROR_FAIL;
+		}
+	}
+
+	/* update the stored copy of dscr */
+	dpm->dscr = dscr;
+
+	/* this "should never happen" ... */
+	if (dscr & DSCR_DTR_RX_FULL) {
+		LOG_ERROR("DSCR_DTR_RX_FULL, dscr 0x%08" PRIx32, dscr);
+		/* Clear DCCRX */
+		retval = mem_ap_read_u32(armv8->debug_ap,
+			armv8->debug_base + CPUV8_DBG_DTRRX, &dscr);
+		if (retval != ERROR_OK)
+			return retval;
+	}
+
+	return retval;
+}
+
+static int dpmv8_dpm_finish(struct arm_dpm *dpm)
+{
+	/* REVISIT what could be done here? */
+	return ERROR_OK;
+}
+
+static int dpmv8_exec_opcode(struct arm_dpm *dpm,
+	uint32_t opcode, uint32_t *p_dscr)
+{
+	struct armv8_common *armv8 = dpm->arm->arch_info;
+	uint32_t dscr = dpm->dscr;
+	int retval;
+
+	if (p_dscr)
+		dscr = *p_dscr;
+
+	/* Wait for InstrCompl bit to be set */
+	long long then = timeval_ms();
+	while ((dscr & DSCR_ITE) == 0) {
+		retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
+		if (retval != ERROR_OK) {
+			LOG_ERROR("Could not read DSCR register, opcode = 0x%08" PRIx32, opcode);
+			return retval;
+		}
+		if (timeval_ms() > then + 1000) {
+			LOG_ERROR("Timeout waiting for aarch64_exec_opcode");
+			return ERROR_FAIL;
+		}
+	}
+
+	if (armv8_dpm_get_core_state(dpm) != ARM_STATE_AARCH64)
+		opcode = T32_FMTITR(opcode);
+
+	retval = mem_ap_write_u32(armv8->debug_ap,
+			armv8->debug_base + CPUV8_DBG_ITR, opcode);
+	if (retval != ERROR_OK)
+		return retval;
+
+	then = timeval_ms();
+	do {
+		retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
+		if (retval != ERROR_OK) {
+			LOG_ERROR("Could not read DSCR register");
+			return retval;
+		}
+		if (timeval_ms() > then + 1000) {
+			LOG_ERROR("Timeout waiting for aarch64_exec_opcode");
+			return ERROR_FAIL;
+		}
+	} while ((dscr & DSCR_ITE) == 0);	/* Wait for InstrCompl bit to be set */
+
+	/* update dscr and el after each command execution */
+	dpm->dscr = dscr;
+	if (dpm->last_el != ((dscr >> 8) & 3))
+		LOG_DEBUG("EL %i -> %i", dpm->last_el, (dscr >> 8) & 3);
+	dpm->last_el = (dscr >> 8) & 3;
+
+	if (dscr & DSCR_ERR) {
+		LOG_ERROR("Opcode 0x%08"PRIx32", DSCR.ERR=1, DSCR.EL=%i", opcode, dpm->last_el);
+		armv8_dpm_handle_exception(dpm);
+		retval = ERROR_FAIL;
+	}
+
+	if (p_dscr)
+		*p_dscr = dscr;
+
+	return retval;
+}
+
+static int dpmv8_instr_execute(struct arm_dpm *dpm, uint32_t opcode)
+{
+	return dpmv8_exec_opcode(dpm, opcode, NULL);
+}
+
+static int dpmv8_instr_write_data_dcc(struct arm_dpm *dpm,
+	uint32_t opcode, uint32_t data)
+{
+	struct armv8_common *armv8 = dpm->arm->arch_info;
+	int retval;
+
+	retval = dpmv8_write_dcc(armv8, data);
+	if (retval != ERROR_OK)
+		return retval;
+
+	return dpmv8_exec_opcode(dpm, opcode, 0);
+}
+
+static int dpmv8_instr_write_data_dcc_64(struct arm_dpm *dpm,
+	uint32_t opcode, uint64_t data)
+{
+	struct armv8_common *armv8 = dpm->arm->arch_info;
+	int retval;
+
+	retval = dpmv8_write_dcc_64(armv8, data);
+	if (retval != ERROR_OK)
+		return retval;
+
+	return dpmv8_exec_opcode(dpm, opcode, 0);
+}
+
+static int dpmv8_instr_write_data_r0(struct arm_dpm *dpm,
+	uint32_t opcode, uint32_t data)
+{
+	struct armv8_common *armv8 = dpm->arm->arch_info;
+	uint32_t dscr = DSCR_ITE;
+	int retval;
+
+	retval = dpmv8_write_dcc(armv8, data);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = dpmv8_exec_opcode(dpm, armv8_opcode(armv8, READ_REG_DTRRX), &dscr);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* then the opcode, taking data from R0 */
+	return dpmv8_exec_opcode(dpm, opcode, &dscr);
+}
+
+static int dpmv8_instr_write_data_r0_64(struct arm_dpm *dpm,
+	uint32_t opcode, uint64_t data)
+{
+	struct armv8_common *armv8 = dpm->arm->arch_info;
+	int retval;
+
+	if (dpm->arm->core_state != ARM_STATE_AARCH64)
+		return dpmv8_instr_write_data_r0(dpm, opcode, data);
+
+	/* transfer data from DCC to R0 */
+	retval = dpmv8_write_dcc_64(armv8, data);
+	if (retval == ERROR_OK)
+		retval = dpmv8_exec_opcode(dpm, ARMV8_MRS(SYSTEM_DBG_DBGDTR_EL0, 0), &dpm->dscr);
+
+	/* then the opcode, taking data from R0 */
+	if (retval == ERROR_OK)
+		retval = dpmv8_exec_opcode(dpm, opcode, &dpm->dscr);
+
+	return retval;
+}
+
+static int dpmv8_instr_cpsr_sync(struct arm_dpm *dpm)
+{
+	int retval;
+	struct armv8_common *armv8 = dpm->arm->arch_info;
+
+	/* "Prefetch flush" after modifying execution status in CPSR */
+	retval = dpmv8_exec_opcode(dpm, armv8_opcode(armv8, ARMV8_OPC_DSB_SY), &dpm->dscr);
+	if (retval == ERROR_OK)
+		dpmv8_exec_opcode(dpm, armv8_opcode(armv8, ARMV8_OPC_ISB_SY), &dpm->dscr);
+	return retval;
+}
+
+static int dpmv8_instr_read_data_dcc(struct arm_dpm *dpm,
+	uint32_t opcode, uint32_t *data)
+{
+	struct armv8_common *armv8 = dpm->arm->arch_info;
+	int retval;
+
+	/* the opcode, writing data to DCC */
+	retval = dpmv8_exec_opcode(dpm, opcode, &dpm->dscr);
+	if (retval != ERROR_OK)
+		return retval;
+
+	return dpmv8_read_dcc(armv8, data, &dpm->dscr);
+}
+
+static int dpmv8_instr_read_data_dcc_64(struct arm_dpm *dpm,
+	uint32_t opcode, uint64_t *data)
+{
+	struct armv8_common *armv8 = dpm->arm->arch_info;
+	int retval;
+
+	/* the opcode, writing data to DCC */
+	retval = dpmv8_exec_opcode(dpm, opcode, &dpm->dscr);
+	if (retval != ERROR_OK)
+		return retval;
+
+	return dpmv8_read_dcc_64(armv8, data, &dpm->dscr);
+}
+
+static int dpmv8_instr_read_data_r0(struct arm_dpm *dpm,
+	uint32_t opcode, uint32_t *data)
+{
+	struct armv8_common *armv8 = dpm->arm->arch_info;
+	int retval;
+
+	/* the opcode, writing data to R0 */
+	retval = dpmv8_exec_opcode(dpm, opcode, &dpm->dscr);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* write R0 to DCC */
+	retval = dpmv8_exec_opcode(dpm, armv8_opcode(armv8, WRITE_REG_DTRTX), &dpm->dscr);
+	if (retval != ERROR_OK)
+		return retval;
+
+	return dpmv8_read_dcc(armv8, data, &dpm->dscr);
+}
+
+static int dpmv8_instr_read_data_r0_64(struct arm_dpm *dpm,
+	uint32_t opcode, uint64_t *data)
+{
+	struct armv8_common *armv8 = dpm->arm->arch_info;
+	int retval;
+
+	if (dpm->arm->core_state != ARM_STATE_AARCH64) {
+		uint32_t tmp;
+		retval = dpmv8_instr_read_data_r0(dpm, opcode, &tmp);
+		if (retval == ERROR_OK)
+			*data = tmp;
+		return retval;
+	}
+
+	/* the opcode, writing data to R0 */
+	retval = dpmv8_exec_opcode(dpm, opcode, &dpm->dscr);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* write R0 to DCC */
+	retval = dpmv8_exec_opcode(dpm, ARMV8_MSR_GP(SYSTEM_DBG_DBGDTR_EL0, 0), &dpm->dscr);
+	if (retval != ERROR_OK)
+		return retval;
+
+	return dpmv8_read_dcc_64(armv8, data, &dpm->dscr);
+}
+
+#if 0
+static int dpmv8_bpwp_enable(struct arm_dpm *dpm, unsigned index_t,
+	target_addr_t addr, uint32_t control)
+{
+	struct armv8_common *armv8 = dpm->arm->arch_info;
+	uint32_t vr = armv8->debug_base;
+	uint32_t cr = armv8->debug_base;
+	int retval;
+
+	switch (index_t) {
+		case 0 ... 15:	/* breakpoints */
+			vr += CPUV8_DBG_BVR_BASE;
+			cr += CPUV8_DBG_BCR_BASE;
+			break;
+		case 16 ... 31:	/* watchpoints */
+			vr += CPUV8_DBG_WVR_BASE;
+			cr += CPUV8_DBG_WCR_BASE;
+			index_t -= 16;
+			break;
+		default:
+			return ERROR_FAIL;
+	}
+	vr += 16 * index_t;
+	cr += 16 * index_t;
+
+	LOG_DEBUG("A8: bpwp enable, vr %08x cr %08x",
+		(unsigned) vr, (unsigned) cr);
+
+	retval = mem_ap_write_atomic_u32(armv8->debug_ap, vr, addr);
+	if (retval != ERROR_OK)
+		return retval;
+	return mem_ap_write_atomic_u32(armv8->debug_ap, cr, control);
+}
+#endif
+
+static int dpmv8_bpwp_disable(struct arm_dpm *dpm, unsigned index_t)
+{
+	struct armv8_common *armv8 = dpm->arm->arch_info;
+	uint32_t cr;
+
+	switch (index_t) {
+		case 0 ... 15:
+			cr = armv8->debug_base + CPUV8_DBG_BCR_BASE;
+			break;
+		case 16 ... 31:
+			cr = armv8->debug_base + CPUV8_DBG_WCR_BASE;
+			index_t -= 16;
+			break;
+		default:
+			return ERROR_FAIL;
+	}
+	cr += 16 * index_t;
+
+	LOG_DEBUG("A: bpwp disable, cr %08x", (unsigned) cr);
+
+	/* clear control register */
+	return mem_ap_write_atomic_u32(armv8->debug_ap, cr, 0);
+}
+
+/*
+ * Coprocessor support
+ */
+
+/* Read coprocessor */
+static int dpmv8_mrc(struct target *target, int cpnum,
+	uint32_t op1, uint32_t op2, uint32_t CRn, uint32_t CRm,
+	uint32_t *value)
+{
+	struct arm *arm = target_to_arm(target);
+	struct arm_dpm *dpm = arm->dpm;
+	int retval;
+
+	retval = dpm->prepare(dpm);
+	if (retval != ERROR_OK)
+		return retval;
+
+	LOG_DEBUG("MRC p%d, %d, r0, c%d, c%d, %d", cpnum,
+		(int) op1, (int) CRn,
+		(int) CRm, (int) op2);
+
+	/* read coprocessor register into R0; return via DCC */
+	retval = dpm->instr_read_data_r0(dpm,
+			ARMV4_5_MRC(cpnum, op1, 0, CRn, CRm, op2),
+			value);
+
+	/* (void) */ dpm->finish(dpm);
+	return retval;
+}
+
+static int dpmv8_mcr(struct target *target, int cpnum,
+	uint32_t op1, uint32_t op2, uint32_t CRn, uint32_t CRm,
+	uint32_t value)
+{
+	struct arm *arm = target_to_arm(target);
+	struct arm_dpm *dpm = arm->dpm;
+	int retval;
+
+	retval = dpm->prepare(dpm);
+	if (retval != ERROR_OK)
+		return retval;
+
+	LOG_DEBUG("MCR p%d, %d, r0, c%d, c%d, %d", cpnum,
+		(int) op1, (int) CRn,
+		(int) CRm, (int) op2);
+
+	/* read DCC into r0; then write coprocessor register from R0 */
+	retval = dpm->instr_write_data_r0(dpm,
+			ARMV4_5_MCR(cpnum, op1, 0, CRn, CRm, op2),
+			value);
+
+	/* (void) */ dpm->finish(dpm);
+	return retval;
+}
+
+/*----------------------------------------------------------------------*/
+
+/*
+ * Register access utilities
+ */
+
+int armv8_dpm_modeswitch(struct arm_dpm *dpm, enum arm_mode mode)
+{
+	struct armv8_common *armv8 = (struct armv8_common *)dpm->arm->arch_info;
+	int retval = ERROR_OK;
+	unsigned int target_el;
+	enum arm_state core_state;
+	uint32_t cpsr;
+
+	/* restore previous mode */
+	if (mode == ARM_MODE_ANY) {
+		cpsr = buf_get_u32(dpm->arm->cpsr->value, 0, 32);
+
+		LOG_DEBUG("restoring mode, cpsr = 0x%08"PRIx32, cpsr);
+
+	} else {
+		LOG_DEBUG("setting mode 0x%"PRIx32, mode);
+
+		/* else force to the specified mode */
+		if (is_arm_mode(mode))
+			cpsr = mode;
+		else
+			cpsr = mode >> 4;
+	}
+
+	switch (cpsr & 0x1f) {
+	/* aarch32 modes */
+	case ARM_MODE_USR:
+		target_el = 0;
+		break;
+	case ARM_MODE_SVC:
+	case ARM_MODE_ABT:
+	case ARM_MODE_IRQ:
+	case ARM_MODE_FIQ:
+		target_el = 1;
+		break;
+	/*
+	 * TODO: handle ARM_MODE_HYP
+	 * case ARM_MODE_HYP:
+	 *      target_el = 2;
+	 *      break;
+	 */
+	case ARM_MODE_MON:
+		target_el = 3;
+		break;
+	/* aarch64 modes */
+	default:
+		target_el = (cpsr >> 2) & 3;
+	}
+
+	if (target_el > SYSTEM_CUREL_EL3) {
+		LOG_ERROR("%s: Invalid target exception level %i", __func__, target_el);
+		return ERROR_FAIL;
+	}
+
+	LOG_DEBUG("target_el = %i, last_el = %i", target_el, dpm->last_el);
+	if (target_el > dpm->last_el) {
+		retval = dpm->instr_execute(dpm,
+				armv8_opcode(armv8, ARMV8_OPC_DCPS) | target_el);
+
+		/* DCPS clobbers registers just like an exception taken */
+		armv8_dpm_handle_exception(dpm);
+	} else {
+		core_state = armv8_dpm_get_core_state(dpm);
+		if (core_state != ARM_STATE_AARCH64) {
+			/* cannot do DRPS/ERET when already in EL0 */
+			if (dpm->last_el != 0) {
+				/* load SPSR with the desired mode and execute DRPS */
+				LOG_DEBUG("SPSR = 0x%08"PRIx32, cpsr);
+				retval = dpm->instr_write_data_r0(dpm,
+						ARMV8_MSR_GP_xPSR_T1(1, 0, 15), cpsr);
+				if (retval == ERROR_OK)
+					retval = dpm->instr_execute(dpm, armv8_opcode(armv8, ARMV8_OPC_DRPS));
+			}
+		} else {
+			/*
+			 * need to execute multiple DRPS instructions until target_el
+			 * is reached
+			 */
+			while (retval == ERROR_OK && dpm->last_el != target_el) {
+				unsigned int cur_el = dpm->last_el;
+				retval = dpm->instr_execute(dpm, armv8_opcode(armv8, ARMV8_OPC_DRPS));
+				if (cur_el == dpm->last_el) {
+					LOG_INFO("Cannot reach EL %i, SPSR corrupted?", target_el);
+					break;
+				}
+			}
+		}
+
+		/* On executing DRPS, DSPSR and DLR become UNKNOWN, mark them as dirty */
+		dpm->arm->cpsr->dirty = true;
+		dpm->arm->pc->dirty = true;
+
+		/*
+		 * re-evaluate the core state, we might be in Aarch32 state now
+		 * we rely on dpm->dscr being up-to-date
+		 */
+		core_state = armv8_dpm_get_core_state(dpm);
+		armv8_select_opcodes(armv8, core_state == ARM_STATE_AARCH64);
+		armv8_select_reg_access(armv8, core_state == ARM_STATE_AARCH64);
+	}
+
+	return retval;
+}
+
+/*
+ * Common register read, relies on armv8_select_reg_access() having been called.
+ */
+static int dpmv8_read_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
+{
+	struct armv8_common *armv8 = dpm->arm->arch_info;
+	uint64_t value_64;
+	int retval;
+
+	retval = armv8->read_reg_u64(armv8, regnum, &value_64);
+
+	if (retval == ERROR_OK) {
+		r->valid = true;
+		r->dirty = false;
+		buf_set_u64(r->value, 0, r->size, value_64);
+		if (r->size == 64)
+			LOG_DEBUG("READ: %s, %16.8llx", r->name, (unsigned long long) value_64);
+		else
+			LOG_DEBUG("READ: %s, %8.8x", r->name, (unsigned int) value_64);
+	}
+	return ERROR_OK;
+}
+
+/*
+ * Common register write, relies on armv8_select_reg_access() having been called.
+ */
+static int dpmv8_write_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
+{
+	struct armv8_common *armv8 = dpm->arm->arch_info;
+	int retval = ERROR_FAIL;
+	uint64_t value_64;
+
+	value_64 = buf_get_u64(r->value, 0, r->size);
+
+	retval = armv8->write_reg_u64(armv8, regnum, value_64);
+	if (retval == ERROR_OK) {
+		r->dirty = false;
+		if (r->size == 64)
+			LOG_DEBUG("WRITE: %s, %16.8llx", r->name, (unsigned long long)value_64);
+		else
+			LOG_DEBUG("WRITE: %s, %8.8x", r->name, (unsigned int)value_64);
+	}
+
+	return ERROR_OK;
+}
+
+/**
+ * Read basic registers of the the current context:  R0 to R15, and CPSR;
+ * sets the core mode (such as USR or IRQ) and state (such as ARM or Thumb).
+ * In normal operation this is called on entry to halting debug state,
+ * possibly after some other operations supporting restore of debug state
+ * or making sure the CPU is fully idle (drain write buffer, etc).
+ */
+int armv8_dpm_read_current_registers(struct arm_dpm *dpm)
+{
+	struct arm *arm = dpm->arm;
+	struct armv8_common *armv8 = (struct armv8_common *)arm->arch_info;
+	struct reg_cache *cache;
+	struct reg *r;
+	uint32_t cpsr;
+	int retval;
+
+	retval = dpm->prepare(dpm);
+	if (retval != ERROR_OK)
+		return retval;
+
+	cache = arm->core_cache;
+
+	/* read R0 first (it's used for scratch), then CPSR */
+	r = cache->reg_list + ARMV8_R0;
+	if (!r->valid) {
+		retval = dpmv8_read_reg(dpm, r, ARMV8_R0);
+		if (retval != ERROR_OK)
+			goto fail;
+	}
+	r->dirty = true;
+
+	/* read R1, too, it will be clobbered during memory access */
+	r = cache->reg_list + ARMV8_R1;
+	if (!r->valid) {
+		retval = dpmv8_read_reg(dpm, r, ARMV8_R1);
+		if (retval != ERROR_OK)
+			goto fail;
+	}
+
+	/* read cpsr to r0 and get it back */
+	retval = dpm->instr_read_data_r0(dpm,
+			armv8_opcode(armv8, READ_REG_DSPSR), &cpsr);
+	if (retval != ERROR_OK)
+		goto fail;
+
+	/* update core mode and state */
+	armv8_set_cpsr(arm, cpsr);
+
+	for (unsigned int i = ARMV8_PC; i < cache->num_regs ; i++) {
+		struct arm_reg *arm_reg;
+
+		r = armv8_reg_current(arm, i);
+		if (r->valid)
+			continue;
+
+		/*
+		 * Only read registers that are available from the
+		 * current EL (or core mode).
+		 */
+		arm_reg = r->arch_info;
+		if (arm_reg->mode != ARM_MODE_ANY &&
+				dpm->last_el != armv8_curel_from_core_mode(arm_reg->mode))
+			continue;
+
+		retval = dpmv8_read_reg(dpm, r, i);
+		if (retval != ERROR_OK)
+			goto fail;
+
+	}
+
+fail:
+	dpm->finish(dpm);
+	return retval;
+}
+
+/* Avoid needless I/O ... leave breakpoints and watchpoints alone
+ * unless they're removed, or need updating because of single-stepping
+ * or running debugger code.
+ */
+static int dpmv8_maybe_update_bpwp(struct arm_dpm *dpm, bool bpwp,
+	struct dpm_bpwp *xp, int *set_p)
+{
+	int retval = ERROR_OK;
+	bool disable;
+
+	if (!set_p) {
+		if (!xp->dirty)
+			goto done;
+		xp->dirty = false;
+		/* removed or startup; we must disable it */
+		disable = true;
+	} else if (bpwp) {
+		if (!xp->dirty)
+			goto done;
+		/* disabled, but we must set it */
+		xp->dirty = disable = false;
+		*set_p = true;
+	} else {
+		if (!*set_p)
+			goto done;
+		/* set, but we must temporarily disable it */
+		xp->dirty = disable = true;
+		*set_p = false;
+	}
+
+	if (disable)
+		retval = dpm->bpwp_disable(dpm, xp->number);
+	else
+		retval = dpm->bpwp_enable(dpm, xp->number,
+				xp->address, xp->control);
+
+	if (retval != ERROR_OK)
+		LOG_ERROR("%s: can't %s HW %spoint %d",
+			disable ? "disable" : "enable",
+			target_name(dpm->arm->target),
+			(xp->number < 16) ? "break" : "watch",
+			xp->number & 0xf);
+done:
+	return retval;
+}
+
+static int dpmv8_add_breakpoint(struct target *target, struct breakpoint *bp);
+
+/**
+ * Writes all modified core registers for all processor modes.  In normal
+ * operation this is called on exit from halting debug state.
+ *
+ * @param dpm: represents the processor
+ * @param bpwp: true ensures breakpoints and watchpoints are set,
+ *	false ensures they are cleared
+ */
+int armv8_dpm_write_dirty_registers(struct arm_dpm *dpm, bool bpwp)
+{
+	struct arm *arm = dpm->arm;
+	struct reg_cache *cache = arm->core_cache;
+	int retval;
+
+	retval = dpm->prepare(dpm);
+	if (retval != ERROR_OK)
+		goto done;
+
+	/* If we're managing hardware breakpoints for this core, enable
+	 * or disable them as requested.
+	 *
+	 * REVISIT We don't yet manage them for ANY cores.  Eventually
+	 * we should be able to assume we handle them; but until then,
+	 * cope with the hand-crafted breakpoint code.
+	 */
+	if (arm->target->type->add_breakpoint == dpmv8_add_breakpoint) {
+		for (unsigned i = 0; i < dpm->nbp; i++) {
+			struct dpm_bp *dbp = dpm->dbp + i;
+			struct breakpoint *bp = dbp->bp;
+
+			retval = dpmv8_maybe_update_bpwp(dpm, bpwp, &dbp->bpwp,
+					bp ? &bp->set : NULL);
+			if (retval != ERROR_OK)
+				goto done;
+		}
+	}
+
+	/* enable/disable watchpoints */
+	for (unsigned i = 0; i < dpm->nwp; i++) {
+		struct dpm_wp *dwp = dpm->dwp + i;
+		struct watchpoint *wp = dwp->wp;
+
+		retval = dpmv8_maybe_update_bpwp(dpm, bpwp, &dwp->bpwp,
+				wp ? &wp->set : NULL);
+		if (retval != ERROR_OK)
+			goto done;
+	}
+
+	/* NOTE:  writes to breakpoint and watchpoint registers might
+	 * be queued, and need (efficient/batched) flushing later.
+	 */
+
+	/* Restore original core mode and state */
+	retval = armv8_dpm_modeswitch(dpm, ARM_MODE_ANY);
+	if (retval != ERROR_OK)
+		goto done;
+
+	/* check everything except our scratch register R0 */
+	for (unsigned i = 1; i < cache->num_regs; i++) {
+		struct arm_reg *r;
+
+		/* skip PC and CPSR */
+		if (i == ARMV8_PC || i == ARMV8_xPSR)
+			continue;
+		/* skip invalid */
+		if (!cache->reg_list[i].valid)
+			continue;
+		/* skip non-dirty */
+		if (!cache->reg_list[i].dirty)
+			continue;
+
+		/* skip all registers not on the current EL */
+		r = cache->reg_list[i].arch_info;
+		if (r->mode != ARM_MODE_ANY &&
+				dpm->last_el != armv8_curel_from_core_mode(r->mode))
+			continue;
+
+		retval = dpmv8_write_reg(dpm, &cache->reg_list[i], i);
+		if (retval != ERROR_OK)
+			break;
+	}
+
+	/* flush CPSR and PC */
+	if (retval == ERROR_OK)
+		retval = dpmv8_write_reg(dpm, &cache->reg_list[ARMV8_xPSR], ARMV8_xPSR);
+	if (retval == ERROR_OK)
+		retval = dpmv8_write_reg(dpm, &cache->reg_list[ARMV8_PC], ARMV8_PC);
+	/* flush R0 -- it's *very* dirty by now */
+	if (retval == ERROR_OK)
+		retval = dpmv8_write_reg(dpm, &cache->reg_list[0], 0);
+	if (retval == ERROR_OK)
+		dpm->instr_cpsr_sync(dpm);
+done:
+	dpm->finish(dpm);
+	return retval;
+}
+
+/*
+ * Standard ARM register accessors ... there are three methods
+ * in "struct arm", to support individual read/write and bulk read
+ * of registers.
+ */
+
+static int armv8_dpm_read_core_reg(struct target *target, struct reg *r,
+	int regnum, enum arm_mode mode)
+{
+	struct arm *arm = target_to_arm(target);
+	struct arm_dpm *dpm = target_to_arm(target)->dpm;
+	int retval;
+	int max = arm->core_cache->num_regs;
+
+	if (regnum < 0 || regnum >= max)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	/*
+	 * REVISIT what happens if we try to read SPSR in a core mode
+	 * which has no such register?
+	 */
+	retval = dpm->prepare(dpm);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = dpmv8_read_reg(dpm, r, regnum);
+	if (retval != ERROR_OK)
+		goto fail;
+
+fail:
+	/* (void) */ dpm->finish(dpm);
+	return retval;
+}
+
+static int armv8_dpm_write_core_reg(struct target *target, struct reg *r,
+	int regnum, enum arm_mode mode, uint8_t *value)
+{
+	struct arm *arm = target_to_arm(target);
+	struct arm_dpm *dpm = target_to_arm(target)->dpm;
+	int retval;
+	int max = arm->core_cache->num_regs;
+
+	if (regnum < 0 || regnum > max)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	/* REVISIT what happens if we try to write SPSR in a core mode
+	 * which has no such register?
+	 */
+
+	retval = dpm->prepare(dpm);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = dpmv8_write_reg(dpm, r, regnum);
+
+	/* always clean up, regardless of error */
+	dpm->finish(dpm);
+
+	return retval;
+}
+
+static int armv8_dpm_full_context(struct target *target)
+{
+	struct arm *arm = target_to_arm(target);
+	struct arm_dpm *dpm = arm->dpm;
+	struct reg_cache *cache = arm->core_cache;
+	int retval;
+	bool did_read;
+
+	retval = dpm->prepare(dpm);
+	if (retval != ERROR_OK)
+		goto done;
+
+	do {
+		enum arm_mode mode = ARM_MODE_ANY;
+
+		did_read = false;
+
+		/* We "know" arm_dpm_read_current_registers() was called so
+		 * the unmapped registers (R0..R7, PC, AND CPSR) and some
+		 * view of R8..R14 are current.  We also "know" oddities of
+		 * register mapping: special cases for R8..R12 and SPSR.
+		 *
+		 * Pick some mode with unread registers and read them all.
+		 * Repeat until done.
+		 */
+		for (unsigned i = 0; i < cache->num_regs; i++) {
+			struct arm_reg *r;
+
+			if (cache->reg_list[i].valid)
+				continue;
+			r = cache->reg_list[i].arch_info;
+
+			/* may need to pick a mode and set CPSR */
+			if (!did_read) {
+				did_read = true;
+				mode = r->mode;
+
+				/* For regular (ARM_MODE_ANY) R8..R12
+				 * in case we've entered debug state
+				 * in FIQ mode we need to patch mode.
+				 */
+				if (mode != ARM_MODE_ANY)
+					retval = armv8_dpm_modeswitch(dpm, mode);
+				else
+					retval = armv8_dpm_modeswitch(dpm, ARM_MODE_USR);
+
+				if (retval != ERROR_OK)
+					goto done;
+			}
+			if (r->mode != mode)
+				continue;
+
+			/* CPSR was read, so "R16" must mean SPSR */
+			retval = dpmv8_read_reg(dpm,
+					&cache->reg_list[i],
+					(r->num == 16) ? 17 : r->num);
+			if (retval != ERROR_OK)
+				goto done;
+		}
+
+	} while (did_read);
+
+	retval = armv8_dpm_modeswitch(dpm, ARM_MODE_ANY);
+	/* (void) */ dpm->finish(dpm);
+done:
+	return retval;
+}
+
+
+/*----------------------------------------------------------------------*/
+
+/*
+ * Breakpoint and Watchpoint support.
+ *
+ * Hardware {break,watch}points are usually left active, to minimize
+ * debug entry/exit costs.  When they are set or cleared, it's done in
+ * batches.  Also, DPM-conformant hardware can update debug registers
+ * regardless of whether the CPU is running or halted ... though that
+ * fact isn't currently leveraged.
+ */
+
+static int dpmv8_bpwp_setup(struct arm_dpm *dpm, struct dpm_bpwp *xp,
+	uint32_t addr, uint32_t length)
+{
+	uint32_t control;
+
+	control = (1 << 0)	/* enable */
+		| (3 << 1);	/* both user and privileged access */
+
+	/* Match 1, 2, or all 4 byte addresses in this word.
+	 *
+	 * FIXME:  v7 hardware allows lengths up to 2 GB for BP and WP.
+	 * Support larger length, when addr is suitably aligned.  In
+	 * particular, allow watchpoints on 8 byte "double" values.
+	 *
+	 * REVISIT allow watchpoints on unaligned 2-bit values; and on
+	 * v7 hardware, unaligned 4-byte ones too.
+	 */
+	switch (length) {
+		case 1:
+			control |= (1 << (addr & 3)) << 5;
+			break;
+		case 2:
+			/* require 2-byte alignment */
+			if (!(addr & 1)) {
+				control |= (3 << (addr & 2)) << 5;
+				break;
+			}
+		/* FALL THROUGH */
+		case 4:
+			/* require 4-byte alignment */
+			if (!(addr & 3)) {
+				control |= 0xf << 5;
+				break;
+			}
+		/* FALL THROUGH */
+		default:
+			LOG_ERROR("unsupported {break,watch}point length/alignment");
+			return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	/* other shared control bits:
+	 * bits 15:14 == 0 ... both secure and nonsecure states (v6.1+ only)
+	 * bit 20 == 0 ... not linked to a context ID
+	 * bit 28:24 == 0 ... not ignoring N LSBs (v7 only)
+	 */
+
+	xp->address = addr & ~3;
+	xp->control = control;
+	xp->dirty = true;
+
+	LOG_DEBUG("BPWP: addr %8.8" PRIx32 ", control %" PRIx32 ", number %d",
+		xp->address, control, xp->number);
+
+	/* hardware is updated in write_dirty_registers() */
+	return ERROR_OK;
+}
+
+static int dpmv8_add_breakpoint(struct target *target, struct breakpoint *bp)
+{
+	struct arm *arm = target_to_arm(target);
+	struct arm_dpm *dpm = arm->dpm;
+	int retval = ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+
+	if (bp->length < 2)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	if (!dpm->bpwp_enable)
+		return retval;
+
+	/* FIXME we need a generic solution for software breakpoints. */
+	if (bp->type == BKPT_SOFT)
+		LOG_DEBUG("using HW bkpt, not SW...");
+
+	for (unsigned i = 0; i < dpm->nbp; i++) {
+		if (!dpm->dbp[i].bp) {
+			retval = dpmv8_bpwp_setup(dpm, &dpm->dbp[i].bpwp,
+					bp->address, bp->length);
+			if (retval == ERROR_OK)
+				dpm->dbp[i].bp = bp;
+			break;
+		}
+	}
+
+	return retval;
+}
+
+static int dpmv8_remove_breakpoint(struct target *target, struct breakpoint *bp)
+{
+	struct arm *arm = target_to_arm(target);
+	struct arm_dpm *dpm = arm->dpm;
+	int retval = ERROR_COMMAND_SYNTAX_ERROR;
+
+	for (unsigned i = 0; i < dpm->nbp; i++) {
+		if (dpm->dbp[i].bp == bp) {
+			dpm->dbp[i].bp = NULL;
+			dpm->dbp[i].bpwp.dirty = true;
+
+			/* hardware is updated in write_dirty_registers() */
+			retval = ERROR_OK;
+			break;
+		}
+	}
+
+	return retval;
+}
+
+static int dpmv8_watchpoint_setup(struct arm_dpm *dpm, unsigned index_t,
+	struct watchpoint *wp)
+{
+	int retval;
+	struct dpm_wp *dwp = dpm->dwp + index_t;
+	uint32_t control;
+
+	/* this hardware doesn't support data value matching or masking */
+	if (wp->value || wp->mask != ~(uint32_t)0) {
+		LOG_DEBUG("watchpoint values and masking not supported");
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	}
+
+	retval = dpmv8_bpwp_setup(dpm, &dwp->bpwp, wp->address, wp->length);
+	if (retval != ERROR_OK)
+		return retval;
+
+	control = dwp->bpwp.control;
+	switch (wp->rw) {
+		case WPT_READ:
+			control |= 1 << 3;
+			break;
+		case WPT_WRITE:
+			control |= 2 << 3;
+			break;
+		case WPT_ACCESS:
+			control |= 3 << 3;
+			break;
+	}
+	dwp->bpwp.control = control;
+
+	dpm->dwp[index_t].wp = wp;
+
+	return retval;
+}
+
+static int dpmv8_add_watchpoint(struct target *target, struct watchpoint *wp)
+{
+	struct arm *arm = target_to_arm(target);
+	struct arm_dpm *dpm = arm->dpm;
+	int retval = ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+
+	if (dpm->bpwp_enable) {
+		for (unsigned i = 0; i < dpm->nwp; i++) {
+			if (!dpm->dwp[i].wp) {
+				retval = dpmv8_watchpoint_setup(dpm, i, wp);
+				break;
+			}
+		}
+	}
+
+	return retval;
+}
+
+static int dpmv8_remove_watchpoint(struct target *target, struct watchpoint *wp)
+{
+	struct arm *arm = target_to_arm(target);
+	struct arm_dpm *dpm = arm->dpm;
+	int retval = ERROR_COMMAND_SYNTAX_ERROR;
+
+	for (unsigned i = 0; i < dpm->nwp; i++) {
+		if (dpm->dwp[i].wp == wp) {
+			dpm->dwp[i].wp = NULL;
+			dpm->dwp[i].bpwp.dirty = true;
+
+			/* hardware is updated in write_dirty_registers() */
+			retval = ERROR_OK;
+			break;
+		}
+	}
+
+	return retval;
+}
+
+void armv8_dpm_report_wfar(struct arm_dpm *dpm, uint64_t addr)
+{
+	switch (dpm->arm->core_state) {
+		case ARM_STATE_ARM:
+		case ARM_STATE_AARCH64:
+			addr -= 8;
+			break;
+		case ARM_STATE_THUMB:
+		case ARM_STATE_THUMB_EE:
+			addr -= 4;
+			break;
+		case ARM_STATE_JAZELLE:
+			/* ?? */
+			break;
+		default:
+			LOG_DEBUG("Unknown core_state");
+			break;
+	}
+	dpm->wp_pc = addr;
+}
+
+/*
+ * Handle exceptions taken in debug state. This happens mostly for memory
+ * accesses that violated a MMU policy. Taking an exception while in debug
+ * state clobbers certain state registers on the target exception level.
+ * Just mark those registers dirty so that they get restored on resume.
+ * This works both for Aarch32 and Aarch64 states.
+ *
+ * This function must not perform any actions that trigger another exception
+ * or a recursion will happen.
+ */
+void armv8_dpm_handle_exception(struct arm_dpm *dpm)
+{
+	struct armv8_common *armv8 = dpm->arm->arch_info;
+	struct reg_cache *cache = dpm->arm->core_cache;
+	enum arm_state core_state;
+	uint64_t dlr;
+	uint32_t dspsr;
+	unsigned int el;
+
+	static const int clobbered_regs_by_el[3][5] = {
+		{ ARMV8_PC, ARMV8_xPSR, ARMV8_ELR_EL1, ARMV8_ESR_EL1, ARMV8_SPSR_EL1 },
+		{ ARMV8_PC, ARMV8_xPSR, ARMV8_ELR_EL2, ARMV8_ESR_EL2, ARMV8_SPSR_EL2 },
+		{ ARMV8_PC, ARMV8_xPSR, ARMV8_ELR_EL3, ARMV8_ESR_EL3, ARMV8_SPSR_EL3 },
+	};
+
+	el = (dpm->dscr >> 8) & 3;
+
+	/* safety check, must not happen since EL0 cannot be a target for an exception */
+	if (el < SYSTEM_CUREL_EL1 || el > SYSTEM_CUREL_EL3) {
+		LOG_ERROR("%s: EL %i is invalid, DSCR corrupted?", __func__, el);
+		return;
+	}
+
+	/* Clear sticky error */
+	mem_ap_write_u32(armv8->debug_ap,
+		armv8->debug_base + CPUV8_DBG_DRCR, DRCR_CSE);
+
+	armv8->read_reg_u64(armv8, ARMV8_xPSR, &dlr);
+	dspsr = dlr;
+	armv8->read_reg_u64(armv8, ARMV8_PC, &dlr);
+
+	LOG_DEBUG("Exception taken to EL %i, DLR=0x%016"PRIx64" DSPSR=0x%08"PRIx32,
+			el, dlr, dspsr);
+
+	/* mark all clobbered registers as dirty */
+	for (int i = 0; i < 5; i++)
+		cache->reg_list[clobbered_regs_by_el[el-1][i]].dirty = true;
+
+	/*
+	 * re-evaluate the core state, we might be in Aarch64 state now
+	 * we rely on dpm->dscr being up-to-date
+	 */
+	core_state = armv8_dpm_get_core_state(dpm);
+	armv8_select_opcodes(armv8, core_state == ARM_STATE_AARCH64);
+	armv8_select_reg_access(armv8, core_state == ARM_STATE_AARCH64);
+
+	armv8_dpm_modeswitch(dpm, ARM_MODE_ANY);
+}
+
+/*----------------------------------------------------------------------*/
+
+/*
+ * Other debug and support utilities
+ */
+
+void armv8_dpm_report_dscr(struct arm_dpm *dpm, uint32_t dscr)
+{
+	struct target *target = dpm->arm->target;
+
+	dpm->dscr = dscr;
+	dpm->last_el = (dscr >> 8) & 3;
+
+	/* Examine debug reason */
+	switch (DSCR_ENTRY(dscr)) {
+		/* FALL THROUGH -- assume a v6 core in abort mode */
+		case DSCRV8_ENTRY_EXT_DEBUG:	/* EDBGRQ */
+			target->debug_reason = DBG_REASON_DBGRQ;
+			break;
+		case DSCRV8_ENTRY_HALT_STEP_EXECLU:	/* HALT step */
+		case DSCRV8_ENTRY_HALT_STEP_NORMAL: /* Halt step*/
+		case DSCRV8_ENTRY_HALT_STEP:
+			target->debug_reason = DBG_REASON_SINGLESTEP;
+			break;
+		case DSCRV8_ENTRY_HLT:	/* HLT instruction (software breakpoint) */
+		case DSCRV8_ENTRY_BKPT:	/* SW BKPT (?) */
+		case DSCRV8_ENTRY_RESET_CATCH:	/* Reset catch */
+		case DSCRV8_ENTRY_OS_UNLOCK:  /*OS unlock catch*/
+		case DSCRV8_ENTRY_EXCEPTION_CATCH:  /*exception catch*/
+		case DSCRV8_ENTRY_SW_ACCESS_DBG: /*SW access dbg register*/
+			target->debug_reason = DBG_REASON_BREAKPOINT;
+			break;
+		case DSCRV8_ENTRY_WATCHPOINT:	/* asynch watchpoint */
+			target->debug_reason = DBG_REASON_WATCHPOINT;
+			break;
+		default:
+			target->debug_reason = DBG_REASON_UNDEFINED;
+			break;
+	}
+
+}
+
+/*----------------------------------------------------------------------*/
+
+/*
+ * Setup and management support.
+ */
+
+/**
+ * Hooks up this DPM to its associated target; call only once.
+ * Initially this only covers the register cache.
+ *
+ * Oh, and watchpoints.  Yeah.
+ */
+int armv8_dpm_setup(struct arm_dpm *dpm)
+{
+	struct arm *arm = dpm->arm;
+	struct target *target = arm->target;
+	struct reg_cache *cache;
+	arm->dpm = dpm;
+
+	/* register access setup */
+	arm->full_context = armv8_dpm_full_context;
+	arm->read_core_reg = armv8_dpm_read_core_reg;
+	arm->write_core_reg = armv8_dpm_write_core_reg;
+
+	if (arm->core_cache == NULL) {
+		cache = armv8_build_reg_cache(target);
+		if (!cache)
+			return ERROR_FAIL;
+	}
+
+	/* coprocessor access setup */
+	arm->mrc = dpmv8_mrc;
+	arm->mcr = dpmv8_mcr;
+
+	dpm->prepare = dpmv8_dpm_prepare;
+	dpm->finish = dpmv8_dpm_finish;
+
+	dpm->instr_execute = dpmv8_instr_execute;
+	dpm->instr_write_data_dcc = dpmv8_instr_write_data_dcc;
+	dpm->instr_write_data_dcc_64 = dpmv8_instr_write_data_dcc_64;
+	dpm->instr_write_data_r0 = dpmv8_instr_write_data_r0;
+	dpm->instr_write_data_r0_64 = dpmv8_instr_write_data_r0_64;
+	dpm->instr_cpsr_sync = dpmv8_instr_cpsr_sync;
+
+	dpm->instr_read_data_dcc = dpmv8_instr_read_data_dcc;
+	dpm->instr_read_data_dcc_64 = dpmv8_instr_read_data_dcc_64;
+	dpm->instr_read_data_r0 = dpmv8_instr_read_data_r0;
+	dpm->instr_read_data_r0_64 = dpmv8_instr_read_data_r0_64;
+
+	dpm->arm_reg_current = armv8_reg_current;
+
+/*	dpm->bpwp_enable = dpmv8_bpwp_enable; */
+	dpm->bpwp_disable = dpmv8_bpwp_disable;
+
+	/* breakpoint setup -- optional until it works everywhere */
+	if (!target->type->add_breakpoint) {
+		target->type->add_breakpoint = dpmv8_add_breakpoint;
+		target->type->remove_breakpoint = dpmv8_remove_breakpoint;
+	}
+
+	/* watchpoint setup */
+	target->type->add_watchpoint = dpmv8_add_watchpoint;
+	target->type->remove_watchpoint = dpmv8_remove_watchpoint;
+
+	/* FIXME add vector catch support */
+
+	dpm->nbp = 1 + ((dpm->didr >> 12) & 0xf);
+	dpm->dbp = calloc(dpm->nbp, sizeof *dpm->dbp);
+
+	dpm->nwp = 1 + ((dpm->didr >> 20) & 0xf);
+	dpm->dwp = calloc(dpm->nwp, sizeof *dpm->dwp);
+
+	if (!dpm->dbp || !dpm->dwp) {
+		free(dpm->dbp);
+		free(dpm->dwp);
+		return ERROR_FAIL;
+	}
+
+	LOG_INFO("%s: hardware has %d breakpoints, %d watchpoints",
+		target_name(target), dpm->nbp, dpm->nwp);
+
+	/* REVISIT ... and some of those breakpoints could match
+	 * execution context IDs...
+	 */
+
+	return ERROR_OK;
+}
+
+/**
+ * Reinitializes DPM state at the beginning of a new debug session
+ * or after a reset which may have affected the debug module.
+ */
+int armv8_dpm_initialize(struct arm_dpm *dpm)
+{
+	/* Disable all breakpoints and watchpoints at startup. */
+	if (dpm->bpwp_disable) {
+		unsigned i;
+
+		for (i = 0; i < dpm->nbp; i++) {
+			dpm->dbp[i].bpwp.number = i;
+			(void) dpm->bpwp_disable(dpm, i);
+		}
+		for (i = 0; i < dpm->nwp; i++) {
+			dpm->dwp[i].bpwp.number = 16 + i;
+			(void) dpm->bpwp_disable(dpm, 16 + i);
+		}
+	} else
+		LOG_WARNING("%s: can't disable breakpoints and watchpoints",
+			target_name(dpm->arm->target));
+
+	return ERROR_OK;
+}
diff --git a/src/target/armv8_dpm.h b/src/target/armv8_dpm.h
new file mode 100644
index 0000000..c039359
--- /dev/null
+++ b/src/target/armv8_dpm.h
@@ -0,0 +1,122 @@
+/*
+ * Copyright (C) 2009 by David Brownell
+ *
+ * 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.
+ */
+
+#ifndef OPENOCD_TARGET_ARMV8_DPM_H
+#define OPENOCD_TARGET_ARMV8_DPM_H
+
+#include "arm_dpm.h"
+
+/* forward-declare struct armv8_common */
+struct armv8_common;
+
+/**
+ * This wraps an implementation of DPM primitives.  Each interface
+ * provider supplies a structure like this, which is the glue between
+ * upper level code and the lower level hardware access.
+ *
+ * It is a PRELIMINARY AND INCOMPLETE set of primitives, starting with
+ * support for CPU register access.
+ */
+int armv8_dpm_setup(struct arm_dpm *dpm);
+int armv8_dpm_initialize(struct arm_dpm *dpm);
+
+int armv8_dpm_read_current_registers(struct arm_dpm *);
+int armv8_dpm_modeswitch(struct arm_dpm *dpm, enum arm_mode mode);
+
+
+int armv8_dpm_write_dirty_registers(struct arm_dpm *, bool bpwp);
+
+void armv8_dpm_report_wfar(struct arm_dpm *, uint64_t wfar);
+
+/* DSCR bits; see ARMv7a arch spec section C10.3.1.
+ * Not all v7 bits are valid in v6.
+ */
+#define DSCR_DEBUG_STATUS_MASK		(0x1F <<  0)
+#define DSCR_ERR					(0x1 <<  6)
+#define DSCR_SYS_ERROR_PEND			(0x1 <<  7)
+#define DSCR_CUR_EL					(0x3 <<  8)
+#define DSCR_EL_STATUS_MASK			(0xF << 10)
+#define DSCR_HDE					(0x1 << 14)
+#define DSCR_SDD					(0x1 << 16)
+#define DSCR_NON_SECURE             (0x1 << 18)
+#define DSCR_MA						(0x1 << 20)
+#define DSCR_TDA					(0x1 << 21)
+#define DSCR_INTDIS_MASK			(0x3 << 22)
+#define DSCR_ITE					(0x1 << 24)
+#define DSCR_PIPE_ADVANCE           (0x1 << 25)
+#define DSCR_TXU					(0x1 << 26)
+#define DSCR_RTO					(0x1 << 27) /* bit 28 is reserved */
+#define DSCR_ITO					(0x1 << 28)
+#define DSCR_DTR_TX_FULL            (0x1 << 29)
+#define DSCR_DTR_RX_FULL            (0x1 << 30) /* bit 31 is reserved */
+
+
+
+/* Methods of entry into debug mode */
+#define DSCRV8_ENTRY_NON_DEBUG			(0x2)
+#define DSCRV8_ENTRY_RESTARTING			(0x1)
+#define DSCRV8_ENTRY_BKPT				(0x7)
+#define DSCRV8_ENTRY_EXT_DEBUG			(0x13)
+#define DSCRV8_ENTRY_HALT_STEP_NORMAL	(0x1B)
+#define DSCRV8_ENTRY_HALT_STEP_EXECLU	(0x1F)
+#define DSCRV8_ENTRY_OS_UNLOCK			(0x23)
+#define DSCRV8_ENTRY_RESET_CATCH		(0x27)
+#define DSCRV8_ENTRY_WATCHPOINT			(0x2B)
+#define DSCRV8_ENTRY_HLT				(0x2F)
+#define DSCRV8_ENTRY_SW_ACCESS_DBG		(0x33)
+#define DSCRV8_ENTRY_EXCEPTION_CATCH	(0x37)
+#define DSCRV8_ENTRY_HALT_STEP			(0x3B)
+#define DSCRV8_HALT_MASK			(0x3C)
+
+/*DRCR registers*/
+#define DRCR_CSE				(1 << 2)
+#define DRCR_CSPA				(1 << 3)
+#define DRCR_CBRRQ				(1 << 4)
+
+
+/* DTR modes */
+#define DSCR_EXT_DCC_NON_BLOCKING     (0x0 << 20)
+#define DSCR_EXT_DCC_STALL_MODE       (0x1 << 20)
+#define DSCR_EXT_DCC_FAST_MODE        (0x2 << 20)  /* bits 22, 23 are reserved */
+
+
+/* DRCR (debug run control register) bits */
+#define DRCR_HALT				(1 << 0)
+#define DRCR_RESTART			(1 << 1)
+#define DRCR_CLEAR_EXCEPTIONS	(1 << 2)
+
+/* PRSR (processor debug status register) bits */
+#define PRSR_PU					(1 << 0)
+#define PRSR_SPD				(1 << 1)
+#define PRSR_RESET				(1 << 2)
+#define PRSR_SR					(1 << 3)
+#define PRSR_HALT				(1 << 4)
+#define PRSR_OSLK				(1 << 5)
+#define PRSR_DLK				(1 << 6)
+#define PRSR_EDAD				(1 << 7)
+#define PRSR_SDAD				(1 << 8)
+#define PRSR_EPMAD				(1 << 9)
+#define PRSR_SPMAD				(1 << 10)
+#define PRSR_SDR				(1 << 11)
+
+/* PRCR (processor debug control register) bits */
+#define PRCR_CORENPDRQ			(1 << 0)
+#define PRCR_CWRR				(1 << 2)
+#define PRCR_COREPURQ			(1 << 3)
+
+void armv8_dpm_report_dscr(struct arm_dpm *dpm, uint32_t dcsr);
+void armv8_dpm_handle_exception(struct arm_dpm *dpm);
+enum arm_state armv8_dpm_get_core_state(struct arm_dpm *dpm);
+
+#endif /* OPENOCD_TARGET_ARM_DPM_H */
diff --git a/src/target/armv8_opcodes.c b/src/target/armv8_opcodes.c
new file mode 100644
index 0000000..d3c0b3f
--- /dev/null
+++ b/src/target/armv8_opcodes.c
@@ -0,0 +1,82 @@
+/*
+ * Copyright (C) 2015 by Matthias Welwarsky <matthias.welwarsky@sysgo.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include <stdint.h>
+#include <stdbool.h>
+
+#include "armv8.h"
+#include "armv8_opcodes.h"
+
+static const uint32_t a64_opcodes[ARMV8_OPC_NUM] = {
+		[READ_REG_CTR]		= ARMV8_MRS(SYSTEM_CTR, 0),
+		[READ_REG_CLIDR]	= ARMV8_MRS(SYSTEM_CLIDR, 0),
+		[READ_REG_CSSELR]	= ARMV8_MRS(SYSTEM_CSSELR, 0),
+		[READ_REG_CCSIDR]	= ARMV8_MRS(SYSTEM_CCSIDR, 0),
+		[WRITE_REG_CSSELR]	= ARMV8_MSR_GP(SYSTEM_CSSELR, 0),
+		[READ_REG_MPIDR]	= ARMV8_MRS(SYSTEM_MPIDR, 0),
+		[READ_REG_DTRRX]	= ARMV8_MRS(SYSTEM_DBG_DTRRX_EL0, 0),
+		[WRITE_REG_DTRTX]	= ARMV8_MSR_GP(SYSTEM_DBG_DTRTX_EL0, 0),
+		[WRITE_REG_DSPSR]	= ARMV8_MSR_DSPSR(0),
+		[READ_REG_DSPSR]	= ARMV8_MRS_DSPSR(0),
+		[ARMV8_OPC_DSB_SY]	= ARMV8_DSB_SY,
+		[ARMV8_OPC_DCPS]	= ARMV8_DCPS(0, 11),
+		[ARMV8_OPC_DRPS]	= ARMV8_DRPS,
+		[ARMV8_OPC_ISB_SY]	= ARMV8_ISB,
+		[ARMV8_OPC_DCCISW]	= ARMV8_SYS(SYSTEM_DCCISW, 0),
+		[ARMV8_OPC_DCCIVAC]	= ARMV8_SYS(SYSTEM_DCCIVAC, 0),
+		[ARMV8_OPC_ICIVAU]	= ARMV8_SYS(SYSTEM_ICIVAU, 0),
+		[ARMV8_OPC_HLT]		= ARMV8_HLT(11),
+};
+
+static const uint32_t t32_opcodes[ARMV8_OPC_NUM] = {
+		[READ_REG_CTR]		= ARMV4_5_MRC(15, 0, 0, 0, 0, 1),
+		[READ_REG_CLIDR]	= ARMV4_5_MRC(15, 1, 0, 0, 0, 1),
+		[READ_REG_CSSELR]	= ARMV4_5_MRC(15, 2, 0, 0, 0, 0),
+		[READ_REG_CCSIDR]	= ARMV4_5_MRC(15, 1, 0, 0, 0, 0),
+		[WRITE_REG_CSSELR]	= ARMV4_5_MCR(15, 2, 0, 0, 0, 0),
+		[READ_REG_MPIDR]	= ARMV4_5_MRC(15, 0, 0, 0, 0, 5),
+		[READ_REG_DTRRX]	= ARMV4_5_MRC(14, 0, 0, 0, 5, 0),
+		[WRITE_REG_DTRTX]	= ARMV4_5_MCR(14, 0, 0, 0, 5, 0),
+		[WRITE_REG_DSPSR]	= ARMV8_MCR_DSPSR(0),
+		[READ_REG_DSPSR]	= ARMV8_MRC_DSPSR(0),
+		[ARMV8_OPC_DSB_SY]	= ARMV8_DSB_SY_T1,
+		[ARMV8_OPC_DCPS]	= ARMV8_DCPS_T1(0),
+		[ARMV8_OPC_DRPS]	= ARMV8_ERET_T1,
+		[ARMV8_OPC_ISB_SY]	= ARMV8_ISB_SY_T1,
+		[ARMV8_OPC_DCCISW]	= ARMV4_5_MCR(15, 0, 0, 7, 14, 2),
+		[ARMV8_OPC_DCCIVAC]	= ARMV4_5_MCR(15, 0, 0, 7, 14, 1),
+		[ARMV8_OPC_ICIVAU]	= ARMV4_5_MCR(15, 0, 0, 7, 5, 1),
+		[ARMV8_OPC_HLT]		= ARMV8_HLT_A1(11),
+};
+
+void armv8_select_opcodes(struct armv8_common *armv8, bool state_is_aarch64)
+{
+	if (state_is_aarch64)
+		armv8->opcodes = &a64_opcodes[0];
+	else
+		armv8->opcodes = &t32_opcodes[0];
+}
+
+uint32_t armv8_opcode(struct armv8_common *armv8, enum armv8_opcode code)
+{
+	if ((int)code >= ARMV8_OPC_NUM)
+		return -1;
+
+	return *(armv8->opcodes + code);
+}
diff --git a/src/target/armv8_opcodes.h b/src/target/armv8_opcodes.h
new file mode 100644
index 0000000..2d8ddd8
--- /dev/null
+++ b/src/target/armv8_opcodes.h
@@ -0,0 +1,189 @@
+/*
+ * Copyright (C) 2015 by pierrr kuo
+ * vichy.kuo@gmail.com
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+#ifndef OPENOCD_TARGET_ARMV8_OPCODES_H
+#define OPENOCD_TARGET_ARMV8_OPCODES_H
+
+#include "arm_opcodes.h"
+
+#define SYSTEM_CUREL_MASK		0xC0
+#define SYSTEM_CUREL_SHIFT		6
+#define SYSTEM_CUREL_EL0		0x0
+#define SYSTEM_CUREL_EL1		0x1
+#define SYSTEM_CUREL_EL2		0x2
+#define SYSTEM_CUREL_EL3		0x3
+#define SYSTEM_CUREL_NONCH		0xF
+#define SYSTEM_AARCH64			0x1
+
+#define SYSTEM_AAR64_MODE_EL0t	0x0
+#define SYSTEM_AAR64_MODE_EL1t	0x4
+#define SYSTEM_AAR64_MODE_EL1h	0x5
+#define SYSTEM_AAR64_MODE_EL2t	0x8
+#define SYSTEM_AAR64_MODE_EL2h	0x9
+#define SYSTEM_AAR64_MODE_EL3t	0xC
+#define SYSTEM_AAR64_MODE_EL3h	0xd
+
+#define SYSTEM_DAIF			0b1101101000010001
+#define SYSTEM_DAIF_MASK		0x3C0
+#define SYSTEM_DAIF_SHIFT		6
+
+#define SYSTEM_ELR_EL1			0b1100001000000001
+#define SYSTEM_ELR_EL2			0b1110001000000001
+#define SYSTEM_ELR_EL3			0b1111001000000001
+
+#define SYSTEM_SCTLR_EL1	0b1100000010000000
+#define SYSTEM_SCTLR_EL2	0b1110000010000000
+#define SYSTEM_SCTLR_EL3	0b1111000010000000
+
+#define SYSTEM_FPCR			0b1101101000100000
+#define SYSTEM_FPSR			0b1101101000100001
+#define SYSTEM_DAIF			0b1101101000010001
+#define SYSTEM_NZCV			0b1101101000010000
+#define SYSTEM_SP_EL0			0b1100001000001000
+#define SYSTEM_SP_EL1			0b1110001000001000
+#define SYSTEM_SP_EL2			0b1111001000001000
+#define SYSTEM_SP_SEL			0b1100001000010000
+#define SYSTEM_SPSR_ABT			0b1110001000011001
+#define SYSTEM_SPSR_FIQ			0b1110001000011011
+#define SYSTEM_SPSR_IRQ			0b1110001000011000
+#define SYSTEM_SPSR_UND			0b1110001000011010
+
+#define SYSTEM_SPSR_EL1			0b1100001000000000
+#define SYSTEM_SPSR_EL2			0b1110001000000000
+#define SYSTEM_SPSR_EL3			0b1111001000000000
+
+#define SYSTEM_ISR_EL1			0b1100011000001000
+
+#define SYSTEM_DBG_DSPSR_EL0    0b1101101000101000
+#define SYSTEM_DBG_DLR_EL0		0b1101101000101001
+#define SYSTEM_DBG_DTRRX_EL0	0b1001100000101000
+#define SYSTEM_DBG_DTRTX_EL0	0b1001100000101000
+#define SYSTEM_DBG_DBGDTR_EL0	0b1001100000100000
+
+#define SYSTEM_CCSIDR			0b1100100000000000
+#define SYSTEM_CLIDR			0b1100100000000001
+#define SYSTEM_CSSELR			0b1101000000000000
+#define SYSTEM_CTYPE			0b1101100000000001
+#define SYSTEM_CTR				0b1101100000000001
+
+#define SYSTEM_DCCISW			0b0100001111110010
+#define SYSTEM_DCCSW			0b0100001111010010
+#define SYSTEM_ICIVAU			0b0101101110101001
+#define SYSTEM_DCCVAU			0b0101101111011001
+#define SYSTEM_DCCIVAC			0b0101101111110001
+
+#define SYSTEM_MPIDR			0b1100000000000101
+
+#define SYSTEM_TCR_EL1			0b1100000100000010
+#define SYSTEM_TCR_EL2			0b1110000100000010
+#define SYSTEM_TCR_EL3			0b1111000100000010
+
+#define SYSTEM_TTBR0_EL1		0b1100000100000000
+#define SYSTEM_TTBR0_EL2		0b1110000100000000
+#define SYSTEM_TTBR0_EL3		0b1111000100000000
+#define SYSTEM_TTBR1_EL1		0b1100000100000001
+
+/* ARMv8 address translation */
+#define SYSTEM_PAR_EL1			0b1100001110100000
+#define SYSTEM_ATS12E0R			0b0110001111000110
+#define SYSTEM_ATS12E1R			0b0110001111000100
+#define SYSTEM_ATS1E2R			0b0110001111000000
+#define SYSTEM_ATS1E3R			0b0111001111000000
+
+/* fault status and fault address */
+#define SYSTEM_FAR_EL1			0b1100001100000000
+#define SYSTEM_FAR_EL2			0b1110001100000000
+#define SYSTEM_FAR_EL3			0b1111001100000000
+#define SYSTEM_ESR_EL1			0b1100001010010000
+#define SYSTEM_ESR_EL2			0b1110001010010000
+#define SYSTEM_ESR_EL3			0b1111001010010000
+
+#define ARMV8_MRS_DSPSR(Rt)	(0xd53b4500 | (Rt))
+#define ARMV8_MSR_DSPSR(Rt)	(0xd51b4500 | (Rt))
+#define ARMV8_MRS_DLR(Rt)	(0xd53b4520 | (Rt))
+#define ARMV8_MSR_DLR(Rt)	(0xd51b4520 | (Rt))
+
+/* T32 instruction to access coprocessor registers */
+#define ARMV8_MCR_T1(cp, CRn, opc1, CRm, opc2, Rt) ARMV4_5_MCR(cp, opc1, Rt, CRn, CRm, opc2)
+#define ARMV8_MRC_T1(cp, CRn, opc1, CRm, opc2, Rt) ARMV4_5_MRC(cp, opc1, Rt, CRn, CRm, opc2)
+
+/* T32 instructions to access DSPSR and DLR */
+#define ARMV8_MRC_DSPSR(Rt) ARMV8_MRC_T1(15, 4, 3, 5, 0, Rt)
+#define ARMV8_MCR_DSPSR(Rt) ARMV8_MCR_T1(15, 4, 3, 5, 0, Rt)
+#define ARMV8_MRC_DLR(Rt)	ARMV8_MRC_T1(15, 4, 3, 5, 1, Rt)
+#define ARMV8_MCR_DLR(Rt)	ARMV8_MCR_T1(15, 4, 3, 5, 1, Rt)
+
+#define ARMV8_DCPS1(IM)		(0xd4a00001 | (((IM) & 0xFFFF) << 5))
+#define ARMV8_DCPS2(IM)		(0xd4a00002 | (((IM) & 0xFFFF) << 5))
+#define ARMV8_DCPS3(IM)		(0xd4a00003 | (((IM) & 0xFFFF) << 5))
+#define ARMV8_DCPS(EL, IM)	(0xd4a00000 | (((IM) & 0xFFFF) << 5) | EL)
+#define ARMV8_DCPS_T1(EL)	(0xf78f8000 | EL)
+#define ARMV8_DRPS		0xd6bf03e0
+#define ARMV8_ERET_T1		0xf3de8f00
+
+#define ARMV8_DSB_SY				0xd5033F9F
+#define ARMV8_DSB_SY_T1				0xf3bf8f4f
+#define ARMV8_ISB				0xd5033fdf
+#define ARMV8_ISB_SY_T1				0xf3bf8f6f
+
+#define ARMV8_MRS(System, Rt)	(0xd5300000 | ((System) << 5) | (Rt))
+/* ARM V8 Move to system register. */
+#define ARMV8_MSR_GP(System, Rt) \
+	(0xd5100000 | ((System) << 5) | (Rt))
+/* ARM V8 Move immediate to process state field. */
+#define ARMV8_MSR_IM(Op1, CRm, Op2) \
+	(0xd500401f | ((Op1) << 16)  | ((CRm) << 8) | ((Op2) << 5))
+
+#define ARMV8_MRS_T1(R, M1, Rd, M) (0xF3E08020 | (R << 20) | (M1 << 16) | (Rd << 8) | (M << 4))
+#define ARMV8_MRS_xPSR_T1(R, Rd) (0xF3EF8000 | (R << 20) | (Rd << 8))
+#define ARMV8_MSR_GP_T1(R, M1, Rd, M) (0xF3808020 | (R << 20) | (M1 << 8) | (Rd << 16) | (M << 4))
+#define ARMV8_MSR_GP_xPSR_T1(R, Rn, mask) (0xF3808000 | (R << 20) | (Rn << 16) | (mask << 8))
+
+#define ARMV8_BKPT(Im) (0xD4200000 | ((Im & 0xffff) << 5))
+#define ARMV8_HLT(Im) (0x0D4400000 | ((Im & 0xffff) << 5))
+#define ARMV8_HLT_A1(Im) (0xE1000070 | ((Im & 0xFFF0) << 4) | (Im & 0xF))
+
+#define ARMV8_MOVFSP_64(Rt) ((1 << 31) | 0x11000000 | (0x1f << 5) | (Rt))
+#define ARMV8_MOVTSP_64(Rt) ((1 << 31) | 0x11000000 | (Rt << 5) | (0x1F))
+#define ARMV8_MOVFSP_32(Rt) (0x11000000 | (0x1f << 5) | (Rt))
+#define ARMV8_MOVTSP_32(Rt) (0x11000000 | (Rt << 5) | (0x1F))
+
+#define ARMV8_SYS(System, Rt) (0xD5080000 | ((System) << 5) | Rt)
+
+enum armv8_opcode {
+	READ_REG_CTR,
+	READ_REG_CLIDR,
+	READ_REG_CSSELR,
+	READ_REG_CCSIDR,
+	WRITE_REG_CSSELR,
+	READ_REG_MPIDR,
+	READ_REG_DTRRX,
+	WRITE_REG_DTRTX,
+	WRITE_REG_DSPSR,
+	READ_REG_DSPSR,
+	ARMV8_OPC_DSB_SY,
+	ARMV8_OPC_DCPS,
+	ARMV8_OPC_DRPS,
+	ARMV8_OPC_ISB_SY,
+	ARMV8_OPC_DCCISW,
+	ARMV8_OPC_DCCIVAC,
+	ARMV8_OPC_ICIVAU,
+	ARMV8_OPC_HLT,
+	ARMV8_OPC_NUM,
+};
+
+extern uint32_t armv8_opcode(struct armv8_common *armv8, enum armv8_opcode);
+extern void armv8_select_opcodes(struct armv8_common *armv8, bool state_is_aarch64);
+
+#endif /* OPENOCD_TARGET_ARMV8_OPCODES_H */
diff --git a/src/target/avr32_ap7k.c b/src/target/avr32_ap7k.c
index e5634f2..f8da8d5 100644
--- a/src/target/avr32_ap7k.c
+++ b/src/target/avr32_ap7k.c
@@ -312,7 +312,7 @@ static int avr32_ap7k_deassert_reset(struct target *target)
 }
 
 static int avr32_ap7k_resume(struct target *target, int current,
-	uint32_t address, int handle_breakpoints, int debug_execution)
+	target_addr_t address, int handle_breakpoints, int debug_execution)
 {
 	struct avr32_ap7k_common *ap7k = target_to_ap7k(target);
 	struct breakpoint *breakpoint = NULL;
@@ -348,7 +348,7 @@ static int avr32_ap7k_resume(struct target *target, int current,
 		/* Single step past breakpoint at current address */
 		breakpoint = breakpoint_find(target, resume_pc);
 		if (breakpoint) {
-			LOG_DEBUG("unset breakpoint at 0x%8.8" PRIx32 "", breakpoint->address);
+			LOG_DEBUG("unset breakpoint at 0x%8.8" TARGET_PRIxADDR "", breakpoint->address);
 #if 0
 			avr32_ap7k_unset_breakpoint(target, breakpoint);
 			avr32_ap7k_single_step_core(target);
@@ -394,7 +394,7 @@ static int avr32_ap7k_resume(struct target *target, int current,
 }
 
 static int avr32_ap7k_step(struct target *target, int current,
-	uint32_t address, int handle_breakpoints)
+	target_addr_t address, int handle_breakpoints)
 {
 	LOG_ERROR("%s: implement me", __func__);
 
@@ -431,12 +431,12 @@ static int avr32_ap7k_remove_watchpoint(struct target *target,
 	return ERROR_OK;
 }
 
-static int avr32_ap7k_read_memory(struct target *target, uint32_t address,
+static int avr32_ap7k_read_memory(struct target *target, target_addr_t address,
 	uint32_t size, uint32_t count, uint8_t *buffer)
 {
 	struct avr32_ap7k_common *ap7k = target_to_ap7k(target);
 
-	LOG_DEBUG("address: 0x%8.8" PRIx32 ", size: 0x%8.8" PRIx32 ", count: 0x%8.8" PRIx32 "",
+	LOG_DEBUG("address: 0x%8.8" TARGET_PRIxADDR ", size: 0x%8.8" PRIx32 ", count: 0x%8.8" PRIx32 "",
 		address,
 		size,
 		count);
@@ -472,12 +472,12 @@ static int avr32_ap7k_read_memory(struct target *target, uint32_t address,
 	return ERROR_OK;
 }
 
-static int avr32_ap7k_write_memory(struct target *target, uint32_t address,
+static int avr32_ap7k_write_memory(struct target *target, target_addr_t address,
 	uint32_t size, uint32_t count, const uint8_t *buffer)
 {
 	struct avr32_ap7k_common *ap7k = target_to_ap7k(target);
 
-	LOG_DEBUG("address: 0x%8.8" PRIx32 ", size: 0x%8.8" PRIx32 ", count: 0x%8.8" PRIx32 "",
+	LOG_DEBUG("address: 0x%8.8" TARGET_PRIxADDR ", size: 0x%8.8" PRIx32 ", count: 0x%8.8" PRIx32 "",
 		address,
 		size,
 		count);
diff --git a/src/target/avrt.c b/src/target/avrt.c
index 40a1297..1e1898c 100644
--- a/src/target/avrt.c
+++ b/src/target/avrt.c
@@ -33,9 +33,9 @@ static int avr_init_target(struct command_context *cmd_ctx, struct target *targe
 static int avr_arch_state(struct target *target);
 static int avr_poll(struct target *target);
 static int avr_halt(struct target *target);
-static int avr_resume(struct target *target, int current, uint32_t address,
+static int avr_resume(struct target *target, int current, target_addr_t address,
 		int handle_breakpoints, int debug_execution);
-static int avr_step(struct target *target, int current, uint32_t address,
+static int avr_step(struct target *target, int current, target_addr_t address,
 		int handle_breakpoints);
 
 static int avr_assert_reset(struct target *target);
@@ -116,14 +116,14 @@ static int avr_halt(struct target *target)
 	return ERROR_OK;
 }
 
-static int avr_resume(struct target *target, int current, uint32_t address,
+static int avr_resume(struct target *target, int current, target_addr_t address,
 		int handle_breakpoints, int debug_execution)
 {
 	LOG_DEBUG("%s", __func__);
 	return ERROR_OK;
 }
 
-static int avr_step(struct target *target, int current, uint32_t address, int handle_breakpoints)
+static int avr_step(struct target *target, int current, target_addr_t address, int handle_breakpoints)
 {
 	LOG_DEBUG("%s", __func__);
 	return ERROR_OK;
diff --git a/src/target/breakpoints.c b/src/target/breakpoints.c
index 9591714..7cf4a69 100644
--- a/src/target/breakpoints.c
+++ b/src/target/breakpoints.c
@@ -42,7 +42,7 @@ static const char * const watchpoint_rw_strings[] = {
 static int bpwp_unique_id;
 
 int breakpoint_add_internal(struct target *target,
-	uint32_t address,
+	target_addr_t address,
 	uint32_t length,
 	enum breakpoint_type type)
 {
@@ -60,7 +60,7 @@ int breakpoint_add_internal(struct target *target,
 			 * breakpoint" ... check all the parameters before
 			 * succeeding.
 			 */
-			LOG_DEBUG("Duplicate Breakpoint address: 0x%08" PRIx32 " (BP %" PRIu32 ")",
+			LOG_DEBUG("Duplicate Breakpoint address: " TARGET_ADDR_FMT " (BP %" PRIu32 ")",
 				address, breakpoint->unique_id);
 			return ERROR_OK;
 		}
@@ -98,7 +98,7 @@ fail:
 			return retval;
 	}
 
-	LOG_DEBUG("added %s breakpoint at 0x%8.8" PRIx32 " of length 0x%8.8x, (BPID: %" PRIu32 ")",
+	LOG_DEBUG("added %s breakpoint at " TARGET_ADDR_FMT " of length 0x%8.8x, (BPID: %" PRIu32 ")",
 		breakpoint_type_strings[(*breakpoint_p)->type],
 		(*breakpoint_p)->address, (*breakpoint_p)->length,
 		(*breakpoint_p)->unique_id);
@@ -159,7 +159,7 @@ int context_breakpoint_add_internal(struct target *target,
 }
 
 int hybrid_breakpoint_add_internal(struct target *target,
-	uint32_t address,
+	target_addr_t address,
 	uint32_t asid,
 	uint32_t length,
 	enum breakpoint_type type)
@@ -180,7 +180,7 @@ int hybrid_breakpoint_add_internal(struct target *target,
 				asid, breakpoint->unique_id);
 			return -1;
 		} else if ((breakpoint->address == address) && (breakpoint->asid == 0)) {
-			LOG_DEBUG("Duplicate Breakpoint IVA: 0x%08" PRIx32 " (BP %" PRIu32 ")",
+			LOG_DEBUG("Duplicate Breakpoint IVA: " TARGET_ADDR_FMT " (BP %" PRIu32 ")",
 				address, breakpoint->unique_id);
 			return -1;
 
@@ -208,7 +208,7 @@ int hybrid_breakpoint_add_internal(struct target *target,
 		return retval;
 	}
 	LOG_DEBUG(
-		"added %s Hybrid breakpoint at address 0x%8.8" PRIx32 " of length 0x%8.8x, (BPID: %" PRIu32 ")",
+		"added %s Hybrid breakpoint at address " TARGET_ADDR_FMT " of length 0x%8.8x, (BPID: %" PRIu32 ")",
 		breakpoint_type_strings[(*breakpoint_p)->type],
 		(*breakpoint_p)->address,
 		(*breakpoint_p)->length,
@@ -218,7 +218,7 @@ int hybrid_breakpoint_add_internal(struct target *target,
 }
 
 int breakpoint_add(struct target *target,
-	uint32_t address,
+	target_addr_t address,
 	uint32_t length,
 	enum breakpoint_type type)
 {
@@ -263,7 +263,7 @@ int context_breakpoint_add(struct target *target,
 		return context_breakpoint_add_internal(target, asid, length, type);
 }
 int hybrid_breakpoint_add(struct target *target,
-	uint32_t address,
+	target_addr_t address,
 	uint32_t asid,
 	uint32_t length,
 	enum breakpoint_type type)
@@ -310,7 +310,7 @@ static void breakpoint_free(struct target *target, struct breakpoint *breakpoint
 	free(breakpoint);
 }
 
-int breakpoint_remove_internal(struct target *target, uint32_t address)
+int breakpoint_remove_internal(struct target *target, target_addr_t address)
 {
 	struct breakpoint *breakpoint = target->breakpoints;
 
@@ -329,11 +329,11 @@ int breakpoint_remove_internal(struct target *target, uint32_t address)
 		return 1;
 	} else {
 		if (!target->smp)
-			LOG_ERROR("no breakpoint at address 0x%8.8" PRIx32 " found", address);
+			LOG_ERROR("no breakpoint at address " TARGET_ADDR_FMT " found", address);
 		return 0;
 	}
 }
-void breakpoint_remove(struct target *target, uint32_t address)
+void breakpoint_remove(struct target *target, target_addr_t address)
 {
 	int found = 0;
 	if (target->smp) {
@@ -346,7 +346,7 @@ void breakpoint_remove(struct target *target, uint32_t address)
 			head = head->next;
 		}
 		if (found == 0)
-			LOG_ERROR("no breakpoint at address 0x%8.8" PRIx32 " found", address);
+			LOG_ERROR("no breakpoint at address " TARGET_ADDR_FMT " found", address);
 	} else
 		breakpoint_remove_internal(target, address);
 }
@@ -375,7 +375,7 @@ void breakpoint_clear_target(struct target *target)
 
 }
 
-struct breakpoint *breakpoint_find(struct target *target, uint32_t address)
+struct breakpoint *breakpoint_find(struct target *target, target_addr_t address)
 {
 	struct breakpoint *breakpoint = target->breakpoints;
 
@@ -388,7 +388,7 @@ struct breakpoint *breakpoint_find(struct target *target, uint32_t address)
 	return NULL;
 }
 
-int watchpoint_add(struct target *target, uint32_t address, uint32_t length,
+int watchpoint_add(struct target *target, target_addr_t address, uint32_t length,
 	enum watchpoint_rw rw, uint32_t value, uint32_t mask)
 {
 	struct watchpoint *watchpoint = target->watchpoints;
@@ -402,8 +402,8 @@ int watchpoint_add(struct target *target, uint32_t address, uint32_t length,
 				|| watchpoint->value != value
 				|| watchpoint->mask != mask
 				|| watchpoint->rw != rw) {
-				LOG_ERROR("address 0x%8.8" PRIx32
-					"already has watchpoint %d",
+				LOG_ERROR("address " TARGET_ADDR_FMT
+					" already has watchpoint %d",
 					address, watchpoint->unique_id);
 				return ERROR_FAIL;
 			}
@@ -436,7 +436,7 @@ int watchpoint_add(struct target *target, uint32_t address, uint32_t length,
 		default:
 			reason = "unrecognized error";
 bye:
-			LOG_ERROR("can't add %s watchpoint at 0x%8.8" PRIx32 ", %s",
+			LOG_ERROR("can't add %s watchpoint at " TARGET_ADDR_FMT ", %s",
 				watchpoint_rw_strings[(*watchpoint_p)->rw],
 				address, reason);
 			free(*watchpoint_p);
@@ -444,7 +444,7 @@ bye:
 			return retval;
 	}
 
-	LOG_DEBUG("added %s watchpoint at 0x%8.8" PRIx32
+	LOG_DEBUG("added %s watchpoint at " TARGET_ADDR_FMT
 		" of length 0x%8.8" PRIx32 " (WPID: %d)",
 		watchpoint_rw_strings[(*watchpoint_p)->rw],
 		(*watchpoint_p)->address,
@@ -475,7 +475,7 @@ static void watchpoint_free(struct target *target, struct watchpoint *watchpoint
 	free(watchpoint);
 }
 
-void watchpoint_remove(struct target *target, uint32_t address)
+void watchpoint_remove(struct target *target, target_addr_t address)
 {
 	struct watchpoint *watchpoint = target->watchpoints;
 
@@ -488,7 +488,7 @@ void watchpoint_remove(struct target *target, uint32_t address)
 	if (watchpoint)
 		watchpoint_free(target, watchpoint);
 	else
-		LOG_ERROR("no watchpoint at address 0x%8.8" PRIx32 " found", address);
+		LOG_ERROR("no watchpoint at address " TARGET_ADDR_FMT " found", address);
 }
 
 void watchpoint_clear_target(struct target *target)
@@ -499,7 +499,8 @@ void watchpoint_clear_target(struct target *target)
 		watchpoint_free(target, target->watchpoints);
 }
 
-int watchpoint_hit(struct target *target, enum watchpoint_rw *rw, uint32_t *address)
+int watchpoint_hit(struct target *target, enum watchpoint_rw *rw,
+		   target_addr_t *address)
 {
 	int retval;
 	struct watchpoint *hit_watchpoint;
@@ -511,7 +512,7 @@ int watchpoint_hit(struct target *target, enum watchpoint_rw *rw, uint32_t *addr
 	*rw = hit_watchpoint->rw;
 	*address = hit_watchpoint->address;
 
-	LOG_DEBUG("Found hit watchpoint at 0x%8.8" PRIx32 " (WPID: %d)",
+	LOG_DEBUG("Found hit watchpoint at " TARGET_ADDR_FMT " (WPID: %d)",
 		hit_watchpoint->address,
 		hit_watchpoint->unique_id);
 
diff --git a/src/target/breakpoints.h b/src/target/breakpoints.h
index 6e260ab..51bd05a 100644
--- a/src/target/breakpoints.h
+++ b/src/target/breakpoints.h
@@ -19,6 +19,8 @@
 #ifndef OPENOCD_TARGET_BREAKPOINTS_H
 #define OPENOCD_TARGET_BREAKPOINTS_H
 
+#include <stdint.h>
+
 struct target;
 
 enum breakpoint_type {
@@ -31,7 +33,7 @@ enum watchpoint_rw {
 };
 
 struct breakpoint {
-	uint32_t address;
+	target_addr_t address;
 	uint32_t asid;
 	int length;
 	enum breakpoint_type type;
@@ -43,7 +45,7 @@ struct breakpoint {
 };
 
 struct watchpoint {
-	uint32_t address;
+	target_addr_t address;
 	uint32_t length;
 	uint32_t mask;
 	uint32_t value;
@@ -55,22 +57,23 @@ struct watchpoint {
 
 void breakpoint_clear_target(struct target *target);
 int breakpoint_add(struct target *target,
-		uint32_t address, uint32_t length, enum breakpoint_type type);
+		target_addr_t address, uint32_t length, enum breakpoint_type type);
 int context_breakpoint_add(struct target *target,
 		uint32_t asid, uint32_t length, enum breakpoint_type type);
 int hybrid_breakpoint_add(struct target *target,
-		uint32_t address, uint32_t asid, uint32_t length, enum breakpoint_type type);
-void breakpoint_remove(struct target *target, uint32_t address);
+		target_addr_t address, uint32_t asid, uint32_t length, enum breakpoint_type type);
+void breakpoint_remove(struct target *target, target_addr_t address);
 
-struct breakpoint *breakpoint_find(struct target *target, uint32_t address);
+struct breakpoint *breakpoint_find(struct target *target, target_addr_t address);
 
 void watchpoint_clear_target(struct target *target);
 int watchpoint_add(struct target *target,
-		uint32_t address, uint32_t length,
+		target_addr_t address, uint32_t length,
 		enum watchpoint_rw rw, uint32_t value, uint32_t mask);
-void watchpoint_remove(struct target *target, uint32_t address);
+void watchpoint_remove(struct target *target, target_addr_t address);
 
 /* report type and address of just hit watchpoint */
-int watchpoint_hit(struct target *target, enum watchpoint_rw *rw, uint32_t *address);
+int watchpoint_hit(struct target *target, enum watchpoint_rw *rw,
+		target_addr_t *address);
 
 #endif /* OPENOCD_TARGET_BREAKPOINTS_H */
diff --git a/src/target/cortex_a.c b/src/target/cortex_a.c
index d1590f6..5d90e34 100644
--- a/src/target/cortex_a.c
+++ b/src/target/cortex_a.c
@@ -53,6 +53,8 @@
 #include "target_request.h"
 #include "target_type.h"
 #include "arm_opcodes.h"
+#include "arm_semihosting.h"
+#include "jtag/swd.h"
 #include <helper/time_support.h>
 
 static int cortex_a_poll(struct target *target);
@@ -73,7 +75,7 @@ static int cortex_a_dap_write_coreregister_u32(struct target *target,
 static int cortex_a_mmu(struct target *target, int *enabled);
 static int cortex_a_mmu_modify(struct target *target, int enable);
 static int cortex_a_virt2phys(struct target *target,
-	uint32_t virt, uint32_t *phys);
+	target_addr_t virt, target_addr_t *phys);
 static int cortex_a_read_cpu_memory(struct target *target,
 	uint32_t address, uint32_t size, uint32_t count, uint8_t *buffer);
 
@@ -197,77 +199,15 @@ static int cortex_a_mmu_modify(struct target *target, int enable)
 /*
  * Cortex-A Basic debug access, very low level assumes state is saved
  */
-static int cortex_a8_init_debug_access(struct target *target)
-{
-	struct armv7a_common *armv7a = target_to_armv7a(target);
-	int retval;
-
-	LOG_DEBUG(" ");
-
-	/* Unlocking the debug registers for modification
-	 * The debugport might be uninitialised so try twice */
-	retval = mem_ap_write_atomic_u32(armv7a->debug_ap,
-			armv7a->debug_base + CPUDBG_LOCKACCESS, 0xC5ACCE55);
-	if (retval != ERROR_OK) {
-		/* try again */
-		retval = mem_ap_write_atomic_u32(armv7a->debug_ap,
-				armv7a->debug_base + CPUDBG_LOCKACCESS, 0xC5ACCE55);
-		if (retval == ERROR_OK)
-			LOG_USER(
-				"Locking debug access failed on first, but succeeded on second try.");
-	}
-
-	return retval;
-}
-
-/*
- * Cortex-A Basic debug access, very low level assumes state is saved
- */
 static int cortex_a_init_debug_access(struct target *target)
 {
 	struct armv7a_common *armv7a = target_to_armv7a(target);
 	int retval;
-	uint32_t dbg_osreg;
-	uint32_t cortex_part_num;
-	struct cortex_a_common *cortex_a = target_to_cortex_a(target);
-
-	LOG_DEBUG(" ");
-	cortex_part_num = (cortex_a->cpuid & CORTEX_A_MIDR_PARTNUM_MASK) >>
-		CORTEX_A_MIDR_PARTNUM_SHIFT;
-
-	switch (cortex_part_num) {
-	case CORTEX_A7_PARTNUM:
-	case CORTEX_A15_PARTNUM:
-		retval = mem_ap_read_atomic_u32(armv7a->debug_ap,
-						    armv7a->debug_base + CPUDBG_OSLSR,
-						    &dbg_osreg);
-		if (retval != ERROR_OK)
-			return retval;
-
-		LOG_DEBUG("DBGOSLSR  0x%" PRIx32, dbg_osreg);
-
-		if (dbg_osreg & CPUDBG_OSLAR_LK_MASK)
-			/* Unlocking the DEBUG OS registers for modification */
-			retval = mem_ap_write_atomic_u32(armv7a->debug_ap,
-							     armv7a->debug_base + CPUDBG_OSLAR,
-							     0);
-		break;
-
-	case CORTEX_A5_PARTNUM:
-	case CORTEX_A8_PARTNUM:
-	case CORTEX_A9_PARTNUM:
-	default:
-		retval = cortex_a8_init_debug_access(target);
-	}
-
-	if (retval != ERROR_OK)
-		return retval;
-	/* Clear Sticky Power Down status Bit in PRSR to enable access to
-	   the registers in the Core Power Domain */
-	retval = mem_ap_read_atomic_u32(armv7a->debug_ap,
-			armv7a->debug_base + CPUDBG_PRSR, &dbg_osreg);
-	LOG_DEBUG("target->coreid %" PRId32 " DBGPRSR  0x%" PRIx32, target->coreid, dbg_osreg);
 
+	/* lock memory-mapped access to debug registers to prevent
+	 * software interference */
+	retval = mem_ap_write_atomic_u32(armv7a->debug_ap,
+			armv7a->debug_base + CPUDBG_LOCKACCESS, 0);
 	if (retval != ERROR_OK)
 		return retval;
 
@@ -852,7 +792,8 @@ static int cortex_a_halt_smp(struct target *target)
 	head = target->head;
 	while (head != (struct target_list *)NULL) {
 		curr = head->target;
-		if ((curr != target) && (curr->state != TARGET_HALTED))
+		if ((curr != target) && (curr->state != TARGET_HALTED)
+			&& target_was_examined(curr))
 			retval += cortex_a_halt(curr);
 		head = head->next;
 	}
@@ -915,6 +856,10 @@ static int cortex_a_poll(struct target *target)
 					if (retval != ERROR_OK)
 						return retval;
 				}
+
+				if (arm_semihosting(target, &retval) != 0)
+					return retval;
+
 				target_call_event_callbacks(target,
 					TARGET_EVENT_HALTED);
 			}
@@ -934,12 +879,8 @@ static int cortex_a_poll(struct target *target)
 					TARGET_EVENT_DEBUG_HALTED);
 			}
 		}
-	} else if (DSCR_RUN_MODE(dscr) == DSCR_CORE_RESTARTED)
+	} else
 		target->state = TARGET_RUNNING;
-	else {
-		LOG_DEBUG("Unknown target state dscr = 0x%08" PRIx32, dscr);
-		target->state = TARGET_UNKNOWN;
-	}
 
 	return retval;
 }
@@ -992,7 +933,7 @@ static int cortex_a_halt(struct target *target)
 }
 
 static int cortex_a_internal_restore(struct target *target, int current,
-	uint32_t *address, int handle_breakpoints, int debug_execution)
+	target_addr_t *address, int handle_breakpoints, int debug_execution)
 {
 	struct armv7a_common *armv7a = target_to_armv7a(target);
 	struct arm *arm = &armv7a->arm;
@@ -1047,6 +988,9 @@ static int cortex_a_internal_restore(struct target *target, int current,
 		case ARM_STATE_JAZELLE:
 			LOG_ERROR("How do I resume into Jazelle state??");
 			return ERROR_FAIL;
+		case ARM_STATE_AARCH64:
+			LOG_ERROR("Shoudn't be in AARCH64 state");
+			return ERROR_FAIL;
 	}
 	LOG_DEBUG("resume pc = 0x%08" PRIx32, resume_pc);
 	buf_set_u32(arm->pc->value, 0, 32, resume_pc);
@@ -1147,11 +1091,12 @@ static int cortex_a_restore_smp(struct target *target, int handle_breakpoints)
 	int retval = 0;
 	struct target_list *head;
 	struct target *curr;
-	uint32_t address;
+	target_addr_t address;
 	head = target->head;
 	while (head != (struct target_list *)NULL) {
 		curr = head->target;
-		if ((curr != target) && (curr->state != TARGET_RUNNING)) {
+		if ((curr != target) && (curr->state != TARGET_RUNNING)
+			&& target_was_examined(curr)) {
 			/*  resume current address , not in step mode */
 			retval += cortex_a_internal_restore(curr, 1, &address,
 					handle_breakpoints, 0);
@@ -1164,7 +1109,7 @@ static int cortex_a_restore_smp(struct target *target, int handle_breakpoints)
 }
 
 static int cortex_a_resume(struct target *target, int current,
-	uint32_t address, int handle_breakpoints, int debug_execution)
+	target_addr_t address, int handle_breakpoints, int debug_execution)
 {
 	int retval = 0;
 	/* dummy resume for smp toggle in order to reduce gdb impact  */
@@ -1188,11 +1133,11 @@ static int cortex_a_resume(struct target *target, int current,
 	if (!debug_execution) {
 		target->state = TARGET_RUNNING;
 		target_call_event_callbacks(target, TARGET_EVENT_RESUMED);
-		LOG_DEBUG("target resumed at 0x%" PRIx32, address);
+		LOG_DEBUG("target resumed at " TARGET_ADDR_FMT, address);
 	} else {
 		target->state = TARGET_DEBUG_RUNNING;
 		target_call_event_callbacks(target, TARGET_EVENT_DEBUG_RESUMED);
-		LOG_DEBUG("target debug resumed at 0x%" PRIx32, address);
+		LOG_DEBUG("target debug resumed at " TARGET_ADDR_FMT, address);
 	}
 
 	return ERROR_OK;
@@ -1201,7 +1146,7 @@ static int cortex_a_resume(struct target *target, int current,
 static int cortex_a_debug_entry(struct target *target)
 {
 	int i;
-	uint32_t regfile[16], cpsr, dscr;
+	uint32_t regfile[16], cpsr, spsr, dscr;
 	int retval = ERROR_OK;
 	struct working_area *regfile_working_area = NULL;
 	struct cortex_a_common *cortex_a = target_to_cortex_a(target);
@@ -1250,6 +1195,7 @@ static int cortex_a_debug_entry(struct target *target)
 	if (cortex_a->fast_reg_read)
 		target_alloc_working_area(target, 64, &regfile_working_area);
 
+
 	/* First load register acessible through core debug port*/
 	if (!regfile_working_area)
 		retval = arm_dpm_read_current_registers(&armv7a->dpm);
@@ -1294,6 +1240,19 @@ static int cortex_a_debug_entry(struct target *target)
 		reg->dirty = reg->valid;
 	}
 
+	if (arm->spsr) {
+		/* read Saved PSR */
+		retval = cortex_a_dap_read_coreregister_u32(target, &spsr, 17);
+		/*  store current spsr */
+		if (retval != ERROR_OK)
+			return retval;
+
+		reg = arm->spsr;
+		buf_set_u32(reg->value, 0, 32, spsr);
+		reg->valid = 1;
+		reg->dirty = 0;
+	}
+
 #if 0
 /* TODO, Move this */
 	uint32_t cp15_control_register, cp15_cacr, cp15_nacr;
@@ -1384,7 +1343,7 @@ int cortex_a_set_dscr_bits(struct target *target, unsigned long bit_mask, unsign
 	return retval;
 }
 
-static int cortex_a_step(struct target *target, int current, uint32_t address,
+static int cortex_a_step(struct target *target, int current, target_addr_t address,
 	int handle_breakpoints)
 {
 	struct cortex_a_common *cortex_a = target_to_cortex_a(target);
@@ -1910,16 +1869,23 @@ static int cortex_a_assert_reset(struct target *target)
 		/* REVISIT handle "pulls" cases, if there's
 		 * hardware that needs them to work.
 		 */
-		if (target->reset_halt)
-			if (jtag_get_reset_config() & RESET_SRST_NO_GATING)
-				jtag_add_reset(0, 1);
+
+		/*
+		 * FIXME: fix reset when transport is SWD. This is a temporary
+		 * work-around for release v0.10 that is not intended to stay!
+		 */
+		if (transport_is_swd() ||
+				(target->reset_halt && (jtag_get_reset_config() & RESET_SRST_NO_GATING)))
+			jtag_add_reset(0, 1);
+
 	} else {
 		LOG_ERROR("%s: how to reset?", target_name(target));
 		return ERROR_FAIL;
 	}
 
 	/* registers are now invalid */
-	register_cache_invalidate(armv7a->arm.core_cache);
+	if (target_was_examined(target))
+		register_cache_invalidate(armv7a->arm.core_cache);
 
 	target->state = TARGET_RESET;
 
@@ -1935,17 +1901,22 @@ static int cortex_a_deassert_reset(struct target *target)
 	/* be certain SRST is off */
 	jtag_add_reset(0, 0);
 
-	retval = cortex_a_poll(target);
-	if (retval != ERROR_OK)
-		return retval;
+	if (target_was_examined(target)) {
+		retval = cortex_a_poll(target);
+		if (retval != ERROR_OK)
+			return retval;
+	}
 
 	if (target->reset_halt) {
 		if (target->state != TARGET_HALTED) {
 			LOG_WARNING("%s: ran after reset and before halt ...",
 				target_name(target));
-			retval = target_halt(target);
-			if (retval != ERROR_OK)
-				return retval;
+			if (target_was_examined(target)) {
+				retval = target_halt(target);
+				if (retval != ERROR_OK)
+					return retval;
+			} else
+				target->state = TARGET_UNKNOWN;
 		}
 	}
 
@@ -2667,7 +2638,7 @@ out:
  */
 
 static int cortex_a_read_phys_memory(struct target *target,
-	uint32_t address, uint32_t size,
+	target_addr_t address, uint32_t size,
 	uint32_t count, uint8_t *buffer)
 {
 	struct armv7a_common *armv7a = target_to_armv7a(target);
@@ -2678,7 +2649,7 @@ static int cortex_a_read_phys_memory(struct target *target,
 	if (!count || !buffer)
 		return ERROR_COMMAND_SYNTAX_ERROR;
 
-	LOG_DEBUG("Reading memory at real address 0x%" PRIx32 "; size %" PRId32 "; count %" PRId32,
+	LOG_DEBUG("Reading memory at real address " TARGET_ADDR_FMT "; size %" PRId32 "; count %" PRId32,
 		address, size, count);
 
 	if (armv7a->memory_ap_available && (apsel == armv7a->memory_ap->ap_num))
@@ -2692,14 +2663,14 @@ static int cortex_a_read_phys_memory(struct target *target,
 	return retval;
 }
 
-static int cortex_a_read_memory(struct target *target, uint32_t address,
+static int cortex_a_read_memory(struct target *target, target_addr_t address,
 	uint32_t size, uint32_t count, uint8_t *buffer)
 {
 	int retval;
 
 	/* cortex_a handles unaligned memory access */
-	LOG_DEBUG("Reading memory at address 0x%" PRIx32 "; size %" PRId32 "; count %" PRId32, address,
-		size, count);
+	LOG_DEBUG("Reading memory at address " TARGET_ADDR_FMT "; size %" PRId32 "; count %" PRId32,
+		address, size, count);
 
 	cortex_a_prep_memaccess(target, 0);
 	retval = cortex_a_read_cpu_memory(target, address, size, count, buffer);
@@ -2708,11 +2679,11 @@ static int cortex_a_read_memory(struct target *target, uint32_t address,
 	return retval;
 }
 
-static int cortex_a_read_memory_ahb(struct target *target, uint32_t address,
+static int cortex_a_read_memory_ahb(struct target *target, target_addr_t address,
 	uint32_t size, uint32_t count, uint8_t *buffer)
 {
 	int mmu_enabled = 0;
-	uint32_t virt, phys;
+	target_addr_t virt, phys;
 	int retval;
 	struct armv7a_common *armv7a = target_to_armv7a(target);
 	struct adiv5_dap *swjdp = armv7a->arm.dap;
@@ -2722,8 +2693,8 @@ static int cortex_a_read_memory_ahb(struct target *target, uint32_t address,
 		return target_read_memory(target, address, size, count, buffer);
 
 	/* cortex_a handles unaligned memory access */
-	LOG_DEBUG("Reading memory at address 0x%" PRIx32 "; size %" PRId32 "; count %" PRId32, address,
-		size, count);
+	LOG_DEBUG("Reading memory at address " TARGET_ADDR_FMT "; size %" PRId32 "; count %" PRId32,
+		address, size, count);
 
 	/* determine if MMU was enabled on target stop */
 	if (!armv7a->is_armv7r) {
@@ -2738,7 +2709,8 @@ static int cortex_a_read_memory_ahb(struct target *target, uint32_t address,
 		if (retval != ERROR_OK)
 			return retval;
 
-		LOG_DEBUG("Reading at virtual address. Translating v:0x%" PRIx32 " to r:0x%" PRIx32,
+		LOG_DEBUG("Reading at virtual address. "
+			  "Translating v:" TARGET_ADDR_FMT " to r:" TARGET_ADDR_FMT,
 			  virt, phys);
 		address = phys;
 	}
@@ -2752,7 +2724,7 @@ static int cortex_a_read_memory_ahb(struct target *target, uint32_t address,
 }
 
 static int cortex_a_write_phys_memory(struct target *target,
-	uint32_t address, uint32_t size,
+	target_addr_t address, uint32_t size,
 	uint32_t count, const uint8_t *buffer)
 {
 	struct armv7a_common *armv7a = target_to_armv7a(target);
@@ -2763,8 +2735,8 @@ static int cortex_a_write_phys_memory(struct target *target,
 	if (!count || !buffer)
 		return ERROR_COMMAND_SYNTAX_ERROR;
 
-	LOG_DEBUG("Writing memory to real address 0x%" PRIx32 "; size %" PRId32 "; count %" PRId32, address,
-		size, count);
+	LOG_DEBUG("Writing memory to real address " TARGET_ADDR_FMT "; size %" PRId32 "; count %" PRId32,
+		address, size, count);
 
 	if (armv7a->memory_ap_available && (apsel == armv7a->memory_ap->ap_num))
 		return mem_ap_write_buf(armv7a->memory_ap, buffer, size, count, address);
@@ -2777,14 +2749,14 @@ static int cortex_a_write_phys_memory(struct target *target,
 	return retval;
 }
 
-static int cortex_a_write_memory(struct target *target, uint32_t address,
+static int cortex_a_write_memory(struct target *target, target_addr_t address,
 	uint32_t size, uint32_t count, const uint8_t *buffer)
 {
 	int retval;
 
 	/* cortex_a handles unaligned memory access */
-	LOG_DEBUG("Writing memory at address 0x%" PRIx32 "; size %" PRId32 "; count %" PRId32, address,
-		size, count);
+	LOG_DEBUG("Writing memory at address " TARGET_ADDR_FMT "; size %" PRId32 "; count %" PRId32,
+		address, size, count);
 
 	/* memory writes bypass the caches, must flush before writing */
 	armv7a_cache_auto_flush_on_write(target, address, size * count);
@@ -2795,11 +2767,11 @@ static int cortex_a_write_memory(struct target *target, uint32_t address,
 	return retval;
 }
 
-static int cortex_a_write_memory_ahb(struct target *target, uint32_t address,
+static int cortex_a_write_memory_ahb(struct target *target, target_addr_t address,
 	uint32_t size, uint32_t count, const uint8_t *buffer)
 {
 	int mmu_enabled = 0;
-	uint32_t virt, phys;
+	target_addr_t virt, phys;
 	int retval;
 	struct armv7a_common *armv7a = target_to_armv7a(target);
 	struct adiv5_dap *swjdp = armv7a->arm.dap;
@@ -2809,8 +2781,8 @@ static int cortex_a_write_memory_ahb(struct target *target, uint32_t address,
 		return target_write_memory(target, address, size, count, buffer);
 
 	/* cortex_a handles unaligned memory access */
-	LOG_DEBUG("Writing memory at address 0x%" PRIx32 "; size %" PRId32 "; count %" PRId32, address,
-		size, count);
+	LOG_DEBUG("Writing memory at address " TARGET_ADDR_FMT "; size %" PRId32 "; count %" PRId32,
+		address, size, count);
 
 	/* determine if MMU was enabled on target stop */
 	if (!armv7a->is_armv7r) {
@@ -2825,7 +2797,8 @@ static int cortex_a_write_memory_ahb(struct target *target, uint32_t address,
 		if (retval != ERROR_OK)
 			return retval;
 
-		LOG_DEBUG("Writing to virtual address. Translating v:0x%" PRIx32 " to r:0x%" PRIx32,
+		LOG_DEBUG("Writing to virtual address. "
+			  "Translating v:" TARGET_ADDR_FMT " to r:" TARGET_ADDR_FMT,
 			  virt,
 			  phys);
 		address = phys;
@@ -2839,7 +2812,7 @@ static int cortex_a_write_memory_ahb(struct target *target, uint32_t address,
 	return retval;
 }
 
-static int cortex_a_read_buffer(struct target *target, uint32_t address,
+static int cortex_a_read_buffer(struct target *target, target_addr_t address,
 				uint32_t count, uint8_t *buffer)
 {
 	uint32_t size;
@@ -2873,7 +2846,7 @@ static int cortex_a_read_buffer(struct target *target, uint32_t address,
 	return ERROR_OK;
 }
 
-static int cortex_a_write_buffer(struct target *target, uint32_t address,
+static int cortex_a_write_buffer(struct target *target, target_addr_t address,
 				 uint32_t count, const uint8_t *buffer)
 {
 	uint32_t size;
@@ -2953,9 +2926,10 @@ static int cortex_a_examine_first(struct target *target)
 	struct cortex_a_common *cortex_a = target_to_cortex_a(target);
 	struct armv7a_common *armv7a = &cortex_a->armv7a_common;
 	struct adiv5_dap *swjdp = armv7a->arm.dap;
+
 	int i;
 	int retval = ERROR_OK;
-	uint32_t didr, ctypr, ttypr, cpuid, dbg_osreg;
+	uint32_t didr, cpuid, dbg_osreg;
 
 	retval = dap_dp_init(swjdp);
 	if (retval != ERROR_OK) {
@@ -3017,79 +2991,69 @@ static int cortex_a_examine_first(struct target *target)
 		armv7a->debug_base = target->dbgbase;
 
 	retval = mem_ap_read_atomic_u32(armv7a->debug_ap,
-			armv7a->debug_base + CPUDBG_CPUID, &cpuid);
-	if (retval != ERROR_OK)
-		return retval;
-
-	retval = mem_ap_read_atomic_u32(armv7a->debug_ap,
-			armv7a->debug_base + CPUDBG_CPUID, &cpuid);
-	if (retval != ERROR_OK) {
-		LOG_DEBUG("Examine %s failed", "CPUID");
-		return retval;
-	}
-
-	retval = mem_ap_read_atomic_u32(armv7a->debug_ap,
-			armv7a->debug_base + CPUDBG_CTYPR, &ctypr);
-	if (retval != ERROR_OK) {
-		LOG_DEBUG("Examine %s failed", "CTYPR");
-		return retval;
-	}
-
-	retval = mem_ap_read_atomic_u32(armv7a->debug_ap,
-			armv7a->debug_base + CPUDBG_TTYPR, &ttypr);
+			armv7a->debug_base + CPUDBG_DIDR, &didr);
 	if (retval != ERROR_OK) {
-		LOG_DEBUG("Examine %s failed", "TTYPR");
+		LOG_DEBUG("Examine %s failed", "DIDR");
 		return retval;
 	}
 
 	retval = mem_ap_read_atomic_u32(armv7a->debug_ap,
-			armv7a->debug_base + CPUDBG_DIDR, &didr);
+			armv7a->debug_base + CPUDBG_CPUID, &cpuid);
 	if (retval != ERROR_OK) {
-		LOG_DEBUG("Examine %s failed", "DIDR");
+		LOG_DEBUG("Examine %s failed", "CPUID");
 		return retval;
 	}
 
-	LOG_DEBUG("cpuid = 0x%08" PRIx32, cpuid);
-	LOG_DEBUG("ctypr = 0x%08" PRIx32, ctypr);
-	LOG_DEBUG("ttypr = 0x%08" PRIx32, ttypr);
 	LOG_DEBUG("didr = 0x%08" PRIx32, didr);
+	LOG_DEBUG("cpuid = 0x%08" PRIx32, cpuid);
 
-	cortex_a->cpuid = cpuid;
-	cortex_a->ctypr = ctypr;
-	cortex_a->ttypr = ttypr;
 	cortex_a->didr = didr;
+	cortex_a->cpuid = cpuid;
 
-	/* Unlocking the debug registers */
-	if ((cpuid & CORTEX_A_MIDR_PARTNUM_MASK) >> CORTEX_A_MIDR_PARTNUM_SHIFT ==
-		CORTEX_A15_PARTNUM) {
-
-		retval = mem_ap_write_atomic_u32(armv7a->debug_ap,
-						     armv7a->debug_base + CPUDBG_OSLAR,
-						     0);
-
-		if (retval != ERROR_OK)
-			return retval;
+	retval = mem_ap_read_atomic_u32(armv7a->debug_ap,
+				    armv7a->debug_base + CPUDBG_PRSR, &dbg_osreg);
+	if (retval != ERROR_OK)
+		return retval;
+	LOG_DEBUG("target->coreid %" PRId32 " DBGPRSR  0x%" PRIx32, target->coreid, dbg_osreg);
 
+	if ((dbg_osreg & PRSR_POWERUP_STATUS) == 0) {
+		LOG_ERROR("target->coreid %" PRId32 " powered down!", target->coreid);
+		target->state = TARGET_UNKNOWN; /* TARGET_NO_POWER? */
+		return ERROR_TARGET_INIT_FAILED;
 	}
-	/* Unlocking the debug registers */
-	if ((cpuid & CORTEX_A_MIDR_PARTNUM_MASK) >> CORTEX_A_MIDR_PARTNUM_SHIFT ==
-		CORTEX_A7_PARTNUM) {
 
-		retval = mem_ap_write_atomic_u32(armv7a->debug_ap,
-						     armv7a->debug_base + CPUDBG_OSLAR,
-						     0);
-
-		if (retval != ERROR_OK)
-			return retval;
+	if (dbg_osreg & PRSR_STICKY_RESET_STATUS)
+		LOG_DEBUG("target->coreid %" PRId32 " was reset!", target->coreid);
 
-	}
+	/* Read DBGOSLSR and check if OSLK is implemented */
 	retval = mem_ap_read_atomic_u32(armv7a->debug_ap,
-					    armv7a->debug_base + CPUDBG_PRSR, &dbg_osreg);
-
+				armv7a->debug_base + CPUDBG_OSLSR, &dbg_osreg);
 	if (retval != ERROR_OK)
 		return retval;
+	LOG_DEBUG("target->coreid %" PRId32 " DBGOSLSR 0x%" PRIx32, target->coreid, dbg_osreg);
 
-	LOG_DEBUG("target->coreid %" PRId32 " DBGPRSR  0x%" PRIx32, target->coreid, dbg_osreg);
+	/* check if OS Lock is implemented */
+	if ((dbg_osreg & OSLSR_OSLM) == OSLSR_OSLM0 || (dbg_osreg & OSLSR_OSLM) == OSLSR_OSLM1) {
+		/* check if OS Lock is set */
+		if (dbg_osreg & OSLSR_OSLK) {
+			LOG_DEBUG("target->coreid %" PRId32 " OSLock set! Trying to unlock", target->coreid);
+
+			retval = mem_ap_write_atomic_u32(armv7a->debug_ap,
+							armv7a->debug_base + CPUDBG_OSLAR,
+							0);
+			if (retval == ERROR_OK)
+				retval = mem_ap_read_atomic_u32(armv7a->debug_ap,
+							armv7a->debug_base + CPUDBG_OSLSR, &dbg_osreg);
+
+			/* if we fail to access the register or cannot reset the OSLK bit, bail out */
+			if (retval != ERROR_OK || (dbg_osreg & OSLSR_OSLK) != 0) {
+				LOG_ERROR("target->coreid %" PRId32 " OSLock sticky, core not powered?",
+						target->coreid);
+				target->state = TARGET_UNKNOWN; /* TARGET_NO_POWER? */
+				return ERROR_TARGET_INIT_FAILED;
+			}
+		}
+	}
 
 	armv7a->arm.core_type = ARM_MODE_MON;
 
@@ -3149,6 +3113,7 @@ static int cortex_a_init_target(struct command_context *cmd_ctx,
 	struct target *target)
 {
 	/* examine_first() does a bunch of this */
+	arm_semihosting_init(target);
 	return ERROR_OK;
 }
 
@@ -3238,7 +3203,7 @@ static int cortex_a_mmu(struct target *target, int *enabled)
 }
 
 static int cortex_a_virt2phys(struct target *target,
-	uint32_t virt, uint32_t *phys)
+	target_addr_t virt, target_addr_t *phys)
 {
 	int retval = ERROR_FAIL;
 	struct armv7a_common *armv7a = target_to_armv7a(target);
@@ -3256,7 +3221,8 @@ static int cortex_a_virt2phys(struct target *target,
 		retval = cortex_a_mmu_modify(target, 1);
 		if (retval != ERROR_OK)
 			goto done;
-		retval = armv7a_mmu_translate_va_pa(target, virt,  phys, 1);
+		retval = armv7a_mmu_translate_va_pa(target, (uint32_t)virt,
+						    (uint32_t *)phys, 1);
 	}
 done:
 	return retval;
@@ -3568,8 +3534,8 @@ struct target_type cortexr4_target = {
 	/* REVISIT allow exporting VFP3 registers ... */
 	.get_gdb_reg_list = arm_get_gdb_reg_list,
 
-	.read_memory = cortex_a_read_memory,
-	.write_memory = cortex_a_write_memory,
+	.read_memory = cortex_a_read_phys_memory,
+	.write_memory = cortex_a_write_phys_memory,
 
 	.checksum_memory = arm_checksum_memory,
 	.blank_check_memory = arm_blank_check_memory,
diff --git a/src/target/cortex_a.h b/src/target/cortex_a.h
index ea08c67..ff03432 100644
--- a/src/target/cortex_a.h
+++ b/src/target/cortex_a.h
@@ -97,8 +97,6 @@ struct cortex_a_common {
 	int fast_reg_read;
 
 	uint32_t cpuid;
-	uint32_t ctypr;
-	uint32_t ttypr;
 	uint32_t didr;
 
 	enum cortex_a_isrmasking_mode isrmasking_mode;
diff --git a/src/target/cortex_m.c b/src/target/cortex_m.c
index a6a9309..e80cd23 100644
--- a/src/target/cortex_m.c
+++ b/src/target/cortex_m.c
@@ -682,7 +682,7 @@ void cortex_m_enable_breakpoints(struct target *target)
 }
 
 static int cortex_m_resume(struct target *target, int current,
-	uint32_t address, int handle_breakpoints, int debug_execution)
+	target_addr_t address, int handle_breakpoints, int debug_execution)
 {
 	struct armv7m_common *armv7m = target_to_armv7m(target);
 	struct breakpoint *breakpoint = NULL;
@@ -750,7 +750,7 @@ static int cortex_m_resume(struct target *target, int current,
 		/* Single step past breakpoint at current address */
 		breakpoint = breakpoint_find(target, resume_pc);
 		if (breakpoint) {
-			LOG_DEBUG("unset breakpoint at 0x%8.8" PRIx32 " (ID: %" PRIu32 ")",
+			LOG_DEBUG("unset breakpoint at " TARGET_ADDR_FMT " (ID: %" PRIu32 ")",
 				breakpoint->address,
 				breakpoint->unique_id);
 			cortex_m_unset_breakpoint(target, breakpoint);
@@ -782,7 +782,7 @@ static int cortex_m_resume(struct target *target, int current,
 
 /* int irqstepcount = 0; */
 static int cortex_m_step(struct target *target, int current,
-	uint32_t address, int handle_breakpoints)
+	target_addr_t address, int handle_breakpoints)
 {
 	struct cortex_m_common *cortex_m = target_to_cm(target);
 	struct armv7m_common *armv7m = &cortex_m->armv7m;
@@ -981,6 +981,18 @@ static int cortex_m_assert_reset(struct target *target)
 
 	bool srst_asserted = false;
 
+	if (!target_was_examined(target)) {
+		if (jtag_reset_config & RESET_HAS_SRST) {
+			adapter_assert_reset();
+			if (target->reset_halt)
+				LOG_ERROR("Target not examined, will not halt after reset!");
+			return ERROR_OK;
+		} else {
+			LOG_ERROR("Target not examined, reset NOT asserted!");
+			return ERROR_FAIL;
+		}
+	}
+
 	if ((jtag_reset_config & RESET_HAS_SRST) &&
 	    (jtag_reset_config & RESET_SRST_NO_GATING)) {
 		adapter_assert_reset();
@@ -1101,7 +1113,8 @@ static int cortex_m_deassert_reset(struct target *target)
 	enum reset_types jtag_reset_config = jtag_get_reset_config();
 
 	if ((jtag_reset_config & RESET_HAS_SRST) &&
-	    !(jtag_reset_config & RESET_SRST_NO_GATING)) {
+	    !(jtag_reset_config & RESET_SRST_NO_GATING) &&
+		target_was_examined(target)) {
 		int retval = dap_dp_init(armv7m->debug_ap->dap);
 		if (retval != ERROR_OK) {
 			LOG_ERROR("DP initialisation failed");
@@ -1185,7 +1198,7 @@ int cortex_m_set_breakpoint(struct target *target, struct breakpoint *breakpoint
 		breakpoint->set = true;
 	}
 
-	LOG_DEBUG("BPID: %" PRIu32 ", Type: %d, Address: 0x%08" PRIx32 " Length: %d (set=%d)",
+	LOG_DEBUG("BPID: %" PRIu32 ", Type: %d, Address: " TARGET_ADDR_FMT " Length: %d (set=%d)",
 		breakpoint->unique_id,
 		(int)(breakpoint->type),
 		breakpoint->address,
@@ -1206,7 +1219,7 @@ int cortex_m_unset_breakpoint(struct target *target, struct breakpoint *breakpoi
 		return ERROR_OK;
 	}
 
-	LOG_DEBUG("BPID: %" PRIu32 ", Type: %d, Address: 0x%08" PRIx32 " Length: %d (set=%d)",
+	LOG_DEBUG("BPID: %" PRIu32 ", Type: %d, Address: " TARGET_ADDR_FMT " Length: %d (set=%d)",
 		breakpoint->unique_id,
 		(int)(breakpoint->type),
 		breakpoint->address,
@@ -1651,7 +1664,7 @@ static int cortex_m_store_core_reg_u32(struct target *target,
 	return ERROR_OK;
 }
 
-static int cortex_m_read_memory(struct target *target, uint32_t address,
+static int cortex_m_read_memory(struct target *target, target_addr_t address,
 	uint32_t size, uint32_t count, uint8_t *buffer)
 {
 	struct armv7m_common *armv7m = target_to_armv7m(target);
@@ -1665,7 +1678,7 @@ static int cortex_m_read_memory(struct target *target, uint32_t address,
 	return mem_ap_read_buf(armv7m->debug_ap, buffer, size, count, address);
 }
 
-static int cortex_m_write_memory(struct target *target, uint32_t address,
+static int cortex_m_write_memory(struct target *target, target_addr_t address,
 	uint32_t size, uint32_t count, const uint8_t *buffer)
 {
 	struct armv7m_common *armv7m = target_to_armv7m(target);
@@ -1683,6 +1696,7 @@ static int cortex_m_init_target(struct command_context *cmd_ctx,
 	struct target *target)
 {
 	armv7m_build_reg_cache(target);
+	arm_semihosting_init(target);
 	return ERROR_OK;
 }
 
@@ -1695,6 +1709,7 @@ void cortex_m_deinit_target(struct target *target)
 	cortex_m_dwt_free(target);
 	armv7m_free_reg_cache(target);
 
+	free(target->private_config);
 	free(cortex_m);
 }
 
@@ -1898,11 +1913,15 @@ int cortex_m_examine(struct target *target)
 			return retval;
 		}
 
-		/* Search for the MEM-AP */
-		retval = dap_find_ap(swjdp, AP_TYPE_AHB_AP, &armv7m->debug_ap);
-		if (retval != ERROR_OK) {
-			LOG_ERROR("Could not find MEM-AP to control the core");
-			return retval;
+		if (cortex_m->apsel < 0) {
+			/* Search for the MEM-AP */
+			retval = dap_find_ap(swjdp, AP_TYPE_AHB_AP, &armv7m->debug_ap);
+			if (retval != ERROR_OK) {
+				LOG_ERROR("Could not find MEM-AP to control the core");
+				return retval;
+			}
+		} else {
+			armv7m->debug_ap = dap_ap(swjdp, cortex_m->apsel);
 		}
 
 		/* Leave (only) generic DAP stuff for debugport_init(); */
@@ -1976,10 +1995,14 @@ int cortex_m_examine(struct target *target)
 			armv7m->arm.core_cache->num_regs = ARMV7M_NUM_CORE_REGS_NOFP;
 		}
 
-		if ((i == 3 || i == 4 || i == 7) && !armv7m->stlink) {
-			/* Cortex-M3/M4/M7 have at least 4096 bytes autoincrement range,
-			 * s. ARM IHI 0031C: MEM-AP 7.2.2 */
-			armv7m->debug_ap->tar_autoincr_block = (1 << 12);
+		if (!armv7m->stlink) {
+			if (i == 3 || i == 4)
+				/* Cortex-M3/M4 have 4096 bytes autoincrement range,
+				 * s. ARM IHI 0031C: MEM-AP 7.2.2 */
+				armv7m->debug_ap->tar_autoincr_block = (1 << 12);
+			else if (i == 7)
+				/* Cortex-M7 has only 1024 bytes autoincrement range */
+				armv7m->debug_ap->tar_autoincr_block = (1 << 10);
 		}
 
 		/* Configure trace modules */
@@ -2162,6 +2185,13 @@ static int cortex_m_target_create(struct target *target, Jim_Interp *interp)
 	cortex_m->common_magic = CORTEX_M_COMMON_MAGIC;
 	cortex_m_init_arch_info(target, cortex_m, target->tap);
 
+	if (target->private_config != NULL) {
+		struct adiv5_private_config *pc =
+				(struct adiv5_private_config *)target->private_config;
+		cortex_m->apsel = pc->ap_num;
+	} else
+		cortex_m->apsel = -1;
+
 	return ERROR_OK;
 }
 
@@ -2423,6 +2453,7 @@ struct target_type cortexm_target = {
 
 	.commands = cortex_m_command_handlers,
 	.target_create = cortex_m_target_create,
+	.target_jim_configure = adiv5_jim_configure,
 	.init_target = cortex_m_init_target,
 	.examine = cortex_m_examine,
 	.deinit_target = cortex_m_deinit_target,
diff --git a/src/target/cortex_m.h b/src/target/cortex_m.h
index eabaac4..3d9714b 100644
--- a/src/target/cortex_m.h
+++ b/src/target/cortex_m.h
@@ -188,6 +188,8 @@ struct cortex_m_common {
 	enum cortex_m_isrmasking_mode isrmasking_mode;
 
 	struct armv7m_common armv7m;
+
+	int apsel;
 };
 
 static inline struct cortex_m_common *
diff --git a/src/target/dsp563xx.c b/src/target/dsp563xx.c
index 783a019..1d728df 100644
--- a/src/target/dsp563xx.c
+++ b/src/target/dsp563xx.c
@@ -1117,7 +1117,7 @@ static int dsp563xx_halt(struct target *target)
 
 static int dsp563xx_resume(struct target *target,
 	int current,
-	uint32_t address,
+	target_addr_t address,
 	int handle_breakpoints,
 	int debug_execution)
 {
@@ -1290,7 +1290,7 @@ static int dsp563xx_step_ex(struct target *target,
 
 static int dsp563xx_step(struct target *target,
 	int current,
-	uint32_t address,
+	target_addr_t address,
 	int handle_breakpoints)
 {
 	int err;
@@ -1374,7 +1374,7 @@ static int dsp563xx_deassert_reset(struct target *target)
 static int dsp563xx_run_algorithm(struct target *target,
 	int num_mem_params, struct mem_param *mem_params,
 	int num_reg_params, struct reg_param *reg_params,
-	uint32_t entry_point, uint32_t exit_point,
+	target_addr_t entry_point, target_addr_t exit_point,
 	int timeout_ms, void *arch_info)
 {
 	int i;
@@ -1592,7 +1592,7 @@ static int dsp563xx_read_memory_core(struct target *target,
 
 static int dsp563xx_read_memory(struct target *target,
 	int mem_type,
-	uint32_t address,
+	target_addr_t address,
 	uint32_t size,
 	uint32_t count,
 	uint8_t *buffer)
@@ -1660,7 +1660,7 @@ static int dsp563xx_read_memory(struct target *target,
 }
 
 static int dsp563xx_read_memory_default(struct target *target,
-	uint32_t address,
+	target_addr_t address,
 	uint32_t size,
 	uint32_t count,
 	uint8_t *buffer)
@@ -1671,7 +1671,7 @@ static int dsp563xx_read_memory_default(struct target *target,
 }
 
 static int dsp563xx_read_buffer_default(struct target *target,
-	uint32_t address,
+	target_addr_t address,
 	uint32_t size,
 	uint8_t *buffer)
 {
@@ -1682,7 +1682,7 @@ static int dsp563xx_read_buffer_default(struct target *target,
 
 static int dsp563xx_write_memory_core(struct target *target,
 	int mem_type,
-	uint32_t address,
+	target_addr_t address,
 	uint32_t size,
 	uint32_t count,
 	const uint8_t *buffer)
@@ -1694,7 +1694,7 @@ static int dsp563xx_write_memory_core(struct target *target,
 	const uint8_t *b;
 
 	LOG_DEBUG(
-		"memtype: %d address: 0x%8.8" PRIx32 ", size: 0x%8.8" PRIx32 ", count: 0x%8.8" PRIx32 "",
+		"memtype: %d address: 0x%8.8" TARGET_PRIxADDR ", size: 0x%8.8" PRIx32 ", count: 0x%8.8" PRIx32 "",
 		mem_type,
 		address,
 		size,
@@ -1766,7 +1766,7 @@ static int dsp563xx_write_memory_core(struct target *target,
 
 static int dsp563xx_write_memory(struct target *target,
 	int mem_type,
-	uint32_t address,
+	target_addr_t address,
 	uint32_t size,
 	uint32_t count,
 	const uint8_t *buffer)
@@ -1834,7 +1834,7 @@ static int dsp563xx_write_memory(struct target *target,
 }
 
 static int dsp563xx_write_memory_default(struct target *target,
-	uint32_t address,
+	target_addr_t address,
 	uint32_t size,
 	uint32_t count,
 	const uint8_t *buffer)
@@ -1844,7 +1844,7 @@ static int dsp563xx_write_memory_default(struct target *target,
 }
 
 static int dsp563xx_write_buffer_default(struct target *target,
-	uint32_t address,
+	target_addr_t address,
 	uint32_t size,
 	const uint8_t *buffer)
 {
diff --git a/src/target/dsp5680xx.c b/src/target/dsp5680xx.c
index 205d8fe..a50f2cd 100644
--- a/src/target/dsp5680xx.c
+++ b/src/target/dsp5680xx.c
@@ -1011,7 +1011,7 @@ static int dsp5680xx_poll(struct target *target)
 }
 
 static int dsp5680xx_resume(struct target *target, int current,
-			    uint32_t address, int hb, int d)
+			    target_addr_t address, int hb, int d)
 {
 	if (target->state == TARGET_RUNNING) {
 		LOG_USER("Target already running.");
@@ -1168,7 +1168,7 @@ static int dsp5680xx_read_32_single(struct target *t, uint32_t a,
 	return retval;
 }
 
-static int dsp5680xx_read(struct target *t, uint32_t a, uint32_t size,
+static int dsp5680xx_read(struct target *t, target_addr_t a, uint32_t size,
 			  uint32_t count, uint8_t *buf)
 {
 	struct target *target = t;
@@ -1423,7 +1423,7 @@ static int dsp5680xx_write_32(struct target *t, uint32_t a, uint32_t c,
  *
  * @return
  */
-static int dsp5680xx_write(struct target *t, uint32_t a, uint32_t s, uint32_t c,
+static int dsp5680xx_write(struct target *t, target_addr_t a, uint32_t s, uint32_t c,
 			   const uint8_t *b)
 {
 	/* TODO Cannot write 32bit to odd address, will write 0x12345678  as 0x5678 0x0012 */
@@ -1468,7 +1468,7 @@ static int dsp5680xx_write(struct target *t, uint32_t a, uint32_t s, uint32_t c,
 	return retval;
 }
 
-static int dsp5680xx_write_buffer(struct target *t, uint32_t a, uint32_t size,
+static int dsp5680xx_write_buffer(struct target *t, target_addr_t a, uint32_t size,
 				  const uint8_t *b)
 {
 	check_halt_and_debug(t);
@@ -1485,7 +1485,7 @@ static int dsp5680xx_write_buffer(struct target *t, uint32_t a, uint32_t size,
  *
  * @return
  */
-static int dsp5680xx_read_buffer(struct target *t, uint32_t a, uint32_t size,
+static int dsp5680xx_read_buffer(struct target *t, target_addr_t a, uint32_t size,
 				 uint8_t *buf)
 {
 	check_halt_and_debug(t);
@@ -1505,7 +1505,7 @@ static int dsp5680xx_read_buffer(struct target *t, uint32_t a, uint32_t size,
  *
  * @return
  */
-static int dsp5680xx_checksum_memory(struct target *t, uint32_t a, uint32_t s,
+static int dsp5680xx_checksum_memory(struct target *t, target_addr_t a, uint32_t s,
 				     uint32_t *checksum)
 {
 	return ERROR_FAIL;
@@ -2262,7 +2262,7 @@ int dsp5680xx_f_lock(struct target *target)
 	return retval;
 }
 
-static int dsp5680xx_step(struct target *target, int current, uint32_t address,
+static int dsp5680xx_step(struct target *target, int current, target_addr_t address,
 			  int handle_breakpoints)
 {
 	err_check(ERROR_FAIL, DSP5680XX_ERROR_NOT_IMPLEMENTED_STEP,
diff --git a/src/target/feroceon.c b/src/target/feroceon.c
index f12e4e4..6b14ab6 100644
--- a/src/target/feroceon.c
+++ b/src/target/feroceon.c
@@ -460,7 +460,7 @@ static int feroceon_examine_debug_reason(struct target *target)
 }
 
 static int feroceon_bulk_write_memory(struct target *target,
-		uint32_t address, uint32_t count, const uint8_t *buffer)
+		target_addr_t address, uint32_t count, const uint8_t *buffer)
 {
 	int retval;
 	struct arm *arm = target->arch_info;
@@ -565,7 +565,7 @@ static int feroceon_bulk_write_memory(struct target *target,
 			buf_get_u32(arm->core_cache->reg_list[0].value, 0, 32);
 		if (endaddress != address + count*4) {
 			LOG_ERROR("DCC write failed,"
-				" expected end address 0x%08" PRIx32
+				" expected end address 0x%08" TARGET_PRIxADDR
 				" got 0x%0" PRIx32 "",
 				address + count*4, endaddress);
 			retval = ERROR_FAIL;
diff --git a/src/target/hla_target.c b/src/target/hla_target.c
index e02abc4..78dc8c5 100644
--- a/src/target/hla_target.c
+++ b/src/target/hla_target.c
@@ -341,7 +341,7 @@ static int adapter_init_target(struct command_context *cmd_ctx,
 	LOG_DEBUG("%s", __func__);
 
 	armv7m_build_reg_cache(target);
-
+	arm_semihosting_init(target);
 	return ERROR_OK;
 }
 
@@ -584,7 +584,7 @@ static int adapter_halt(struct target *target)
 }
 
 static int adapter_resume(struct target *target, int current,
-		uint32_t address, int handle_breakpoints,
+		target_addr_t address, int handle_breakpoints,
 		int debug_execution)
 {
 	int res;
@@ -594,8 +594,8 @@ static int adapter_resume(struct target *target, int current,
 	struct breakpoint *breakpoint = NULL;
 	struct reg *pc;
 
-	LOG_DEBUG("%s %d 0x%08" PRIx32 " %d %d", __func__, current, address,
-			handle_breakpoints, debug_execution);
+	LOG_DEBUG("%s %d " TARGET_ADDR_FMT " %d %d", __func__, current,
+			address, handle_breakpoints, debug_execution);
 
 	if (target->state != TARGET_HALTED) {
 		LOG_WARNING("target not halted");
@@ -642,7 +642,7 @@ static int adapter_resume(struct target *target, int current,
 		/* Single step past breakpoint at current address */
 		breakpoint = breakpoint_find(target, resume_pc);
 		if (breakpoint) {
-			LOG_DEBUG("unset breakpoint at 0x%8.8" PRIx32 " (ID: %" PRIu32 ")",
+			LOG_DEBUG("unset breakpoint at " TARGET_ADDR_FMT " (ID: %" PRIu32 ")",
 					breakpoint->address,
 					breakpoint->unique_id);
 			cortex_m_unset_breakpoint(target, breakpoint);
@@ -675,7 +675,7 @@ static int adapter_resume(struct target *target, int current,
 }
 
 static int adapter_step(struct target *target, int current,
-		uint32_t address, int handle_breakpoints)
+		target_addr_t address, int handle_breakpoints)
 {
 	int res;
 	struct hl_interface_s *adapter = target_to_adapter(target);
@@ -738,7 +738,7 @@ static int adapter_step(struct target *target, int current,
 	return ERROR_OK;
 }
 
-static int adapter_read_memory(struct target *target, uint32_t address,
+static int adapter_read_memory(struct target *target, target_addr_t address,
 		uint32_t size, uint32_t count,
 		uint8_t *buffer)
 {
@@ -747,12 +747,13 @@ static int adapter_read_memory(struct target *target, uint32_t address,
 	if (!count || !buffer)
 		return ERROR_COMMAND_SYNTAX_ERROR;
 
-	LOG_DEBUG("%s 0x%08" PRIx32 " %" PRIu32 " %" PRIu32, __func__, address, size, count);
+	LOG_DEBUG("%s " TARGET_ADDR_FMT " %" PRIu32 " %" PRIu32,
+			  __func__, address, size, count);
 
 	return adapter->layout->api->read_mem(adapter->handle, address, size, count, buffer);
 }
 
-static int adapter_write_memory(struct target *target, uint32_t address,
+static int adapter_write_memory(struct target *target, target_addr_t address,
 		uint32_t size, uint32_t count,
 		const uint8_t *buffer)
 {
@@ -761,7 +762,8 @@ static int adapter_write_memory(struct target *target, uint32_t address,
 	if (!count || !buffer)
 		return ERROR_COMMAND_SYNTAX_ERROR;
 
-	LOG_DEBUG("%s 0x%08" PRIx32 " %" PRIu32 " %" PRIu32, __func__, address, size, count);
+	LOG_DEBUG("%s " TARGET_ADDR_FMT " %" PRIu32 " %" PRIu32,
+			  __func__, address, size, count);
 
 	return adapter->layout->api->write_mem(adapter->handle, address, size, count, buffer);
 }
diff --git a/src/target/image.c b/src/target/image.c
index 0612ea7..f97d904 100644
--- a/src/target/image.c
+++ b/src/target/image.c
@@ -729,7 +729,7 @@ int image_open(struct image *image, const char *url, const char *type_string)
 		retval = image_ihex_buffer_complete(image);
 		if (retval != ERROR_OK) {
 			LOG_ERROR(
-				"failed buffering IHEX image, check daemon output for additional information");
+				"failed buffering IHEX image, check server output for additional information");
 			fileio_close(image_ihex->fileio);
 			return retval;
 		}
@@ -780,7 +780,7 @@ int image_open(struct image *image, const char *url, const char *type_string)
 		retval = image_mot_buffer_complete(image);
 		if (retval != ERROR_OK) {
 			LOG_ERROR(
-				"failed buffering S19 image, check daemon output for additional information");
+				"failed buffering S19 image, check server output for additional information");
 			fileio_close(image_mot->fileio);
 			return retval;
 		}
diff --git a/src/target/image.h b/src/target/image.h
index 9bf7af3..9907a5f 100644
--- a/src/target/image.h
+++ b/src/target/image.h
@@ -46,7 +46,7 @@ enum image_type {
 };
 
 struct imagesection {
-	uint32_t base_address;
+	target_addr_t base_address;
 	uint32_t size;
 	int flags;
 	void *private;		/* private data */
diff --git a/src/target/lakemont.c b/src/target/lakemont.c
index 27efc69..2bd12fd 100644
--- a/src/target/lakemont.c
+++ b/src/target/lakemont.c
@@ -994,7 +994,7 @@ int lakemont_halt(struct target *t)
 	}
 }
 
-int lakemont_resume(struct target *t, int current, uint32_t address,
+int lakemont_resume(struct target *t, int current, target_addr_t address,
 			int handle_breakpoints, int debug_execution)
 {
 	struct breakpoint *bp = NULL;
@@ -1036,7 +1036,7 @@ int lakemont_resume(struct target *t, int current, uint32_t address,
 }
 
 int lakemont_step(struct target *t, int current,
-			uint32_t address, int handle_breakpoints)
+			target_addr_t address, int handle_breakpoints)
 {
 	struct x86_32_common *x86_32 = target_to_x86_32(t);
 	uint32_t eflags = buf_get_u32(x86_32->cache->reg_list[EFLAGS].value, 0, 32);
diff --git a/src/target/lakemont.h b/src/target/lakemont.h
index b07a056..98efd44 100644
--- a/src/target/lakemont.h
+++ b/src/target/lakemont.h
@@ -95,10 +95,10 @@ int lakemont_init_arch_info(struct target *t, struct x86_32_common *x86_32);
 int lakemont_poll(struct target *t);
 int lakemont_arch_state(struct target *t);
 int lakemont_halt(struct target *t);
-int lakemont_resume(struct target *t, int current, uint32_t address,
+int lakemont_resume(struct target *t, int current, target_addr_t address,
 			int handle_breakpoints, int debug_execution);
 int lakemont_step(struct target *t, int current,
-			uint32_t address, int handle_breakpoints);
+			target_addr_t address, int handle_breakpoints);
 int lakemont_reset_assert(struct target *t);
 int lakemont_reset_deassert(struct target *t);
 int lakemont_update_after_probemode_entry(struct target *t);
diff --git a/src/target/ls1_sap.c b/src/target/ls1_sap.c
index 944e725..bc46ed4 100644
--- a/src/target/ls1_sap.c
+++ b/src/target/ls1_sap.c
@@ -64,14 +64,14 @@ static int ls1_sap_halt(struct target *target)
 	return ERROR_OK;
 }
 
-static int ls1_sap_resume(struct target *target, int current, uint32_t address,
+static int ls1_sap_resume(struct target *target, int current, target_addr_t address,
 		int handle_breakpoints, int debug_execution)
 {
 	LOG_DEBUG("%s", __func__);
 	return ERROR_OK;
 }
 
-static int ls1_sap_step(struct target *target, int current, uint32_t address,
+static int ls1_sap_step(struct target *target, int current, target_addr_t address,
 				int handle_breakpoints)
 {
 	LOG_DEBUG("%s", __func__);
@@ -127,7 +127,7 @@ static void ls1_sap_set_addr_high(struct jtag_tap *tap, uint16_t addr_high)
 }
 
 static void ls1_sap_memory_cmd(struct jtag_tap *tap, uint32_t address,
-			       int32_t size, int read)
+			       int32_t size, bool rnw)
 {
 	struct scan_field field;
 	uint8_t cmd[8];
@@ -138,7 +138,7 @@ static void ls1_sap_memory_cmd(struct jtag_tap *tap, uint32_t address,
 	field.out_value = cmd;
 	buf_set_u64(cmd, 0, 9, 0);
 	buf_set_u64(cmd, 9, 3, size);
-	buf_set_u64(cmd, 12, 1, !!read);
+	buf_set_u64(cmd, 12, 1, rnw);
 	buf_set_u64(cmd, 13, 3, 0);
 	buf_set_u64(cmd, 16, 32, address);
 	buf_set_u64(cmd, 48, 16, 0);
@@ -178,10 +178,10 @@ static void ls1_sap_memory_write(struct jtag_tap *tap, uint32_t size,
 	jtag_add_dr_scan(tap, 1, &field, TAP_IDLE);
 }
 
-static int ls1_sap_read_memory(struct target *target, uint32_t address,
+static int ls1_sap_read_memory(struct target *target, target_addr_t address,
 			       uint32_t size, uint32_t count, uint8_t *buffer)
 {
-	LOG_DEBUG("Reading memory at physical address 0x%" PRIx32
+	LOG_DEBUG("Reading memory at physical address 0x%" TARGET_PRIxADDR
 		  "; size %" PRId32 "; count %" PRId32, address, size, count);
 
 	if (count == 0 || buffer == NULL)
@@ -190,7 +190,7 @@ static int ls1_sap_read_memory(struct target *target, uint32_t address,
 	ls1_sap_set_addr_high(target->tap, 0);
 
 	while (count--) {
-		ls1_sap_memory_cmd(target->tap, address, size, 1);
+		ls1_sap_memory_cmd(target->tap, address, size, true);
 		ls1_sap_memory_read(target->tap, size, buffer);
 		address += size;
 		buffer += size;
@@ -199,11 +199,11 @@ static int ls1_sap_read_memory(struct target *target, uint32_t address,
 	return jtag_execute_queue();
 }
 
-static int ls1_sap_write_memory(struct target *target, uint32_t address,
+static int ls1_sap_write_memory(struct target *target, target_addr_t address,
 				uint32_t size, uint32_t count,
 				const uint8_t *buffer)
 {
-	LOG_DEBUG("Writing memory at physical address 0x%" PRIx32
+	LOG_DEBUG("Writing memory at physical address 0x%" TARGET_PRIxADDR
 		  "; size %" PRId32 "; count %" PRId32, address, size, count);
 
 
@@ -213,7 +213,7 @@ static int ls1_sap_write_memory(struct target *target, uint32_t address,
 	ls1_sap_set_addr_high(target->tap, 0);
 
 	while (count--) {
-		ls1_sap_memory_cmd(target->tap, address, size, 0);
+		ls1_sap_memory_cmd(target->tap, address, size, false);
 		ls1_sap_memory_write(target->tap, size, buffer);
 		address += size;
 		buffer += size;
diff --git a/src/target/mips32.c b/src/target/mips32.c
index 3292446..93fb4e6 100644
--- a/src/target/mips32.c
+++ b/src/target/mips32.c
@@ -34,7 +34,7 @@
 #include "register.h"
 
 static const char *mips_isa_strings[] = {
-	"MIPS32", "MIPS16"
+	"MIPS32", "MIPS16", "", "MICRO MIPS32",
 };
 
 #define MIPS32_GDB_DUMMY_FP_REG 1
@@ -375,6 +375,7 @@ int mips32_init_arch_info(struct target *target, struct mips32_common *mips32, s
 	target->arch_info = mips32;
 	mips32->common_magic = MIPS32_COMMON_MAGIC;
 	mips32->fast_data_area = NULL;
+	mips32->isa_imp = MIPS32_ONLY;	/* default */
 
 	/* has breakpoint/watchpoint unit been scanned */
 	mips32->bp_scanned = 0;
@@ -383,16 +384,18 @@ int mips32_init_arch_info(struct target *target, struct mips32_common *mips32, s
 	mips32->ejtag_info.tap = tap;
 	mips32->read_core_reg = mips32_read_core_reg;
 	mips32->write_core_reg = mips32_write_core_reg;
-
-	mips32->ejtag_info.scan_delay = 2000000;	/* Initial default value */
+	/* if unknown endianness defaults to little endian, 1 */
+	mips32->ejtag_info.endianness = target->endianness == TARGET_BIG_ENDIAN ? 0 : 1;
+	mips32->ejtag_info.scan_delay = MIPS32_SCAN_DELAY_LEGACY_MODE;
 	mips32->ejtag_info.mode = 0;			/* Initial default value */
-
+	mips32->ejtag_info.isa = 0;	/* isa on debug mips32, updated by poll function */
+	mips32->ejtag_info.config_regs = 0;	/* no config register read */
 	return ERROR_OK;
 }
 
 /* run to exit point. return error if exit point was not reached. */
-static int mips32_run_and_wait(struct target *target, uint32_t entry_point,
-		int timeout_ms, uint32_t exit_point, struct mips32_common *mips32)
+static int mips32_run_and_wait(struct target *target, target_addr_t entry_point,
+		int timeout_ms, target_addr_t exit_point, struct mips32_common *mips32)
 {
 	uint32_t pc;
 	int retval;
@@ -425,8 +428,8 @@ static int mips32_run_and_wait(struct target *target, uint32_t entry_point,
 
 int mips32_run_algorithm(struct target *target, int num_mem_params,
 		struct mem_param *mem_params, int num_reg_params,
-		struct reg_param *reg_params, uint32_t entry_point,
-		uint32_t exit_point, int timeout_ms, void *arch_info)
+		struct reg_param *reg_params, target_addr_t entry_point,
+		target_addr_t exit_point, int timeout_ms, void *arch_info)
 {
 	struct mips32_common *mips32 = target_to_mips32(target);
 	struct mips32_algorithm *mips32_algorithm_info = arch_info;
@@ -696,57 +699,109 @@ int mips32_enable_interrupts(struct target *target, int enable)
 	return ERROR_OK;
 }
 
-int mips32_checksum_memory(struct target *target, uint32_t address,
+/* read config to config3 cp0 registers and log isa implementation */
+int mips32_read_config_regs(struct target *target)
+{
+	struct mips32_common *mips32 = target_to_mips32(target);
+	struct mips_ejtag *ejtag_info = &mips32->ejtag_info;
+
+	if (ejtag_info->config_regs == 0)
+		for (int i = 0; i != 4; i++) {
+			int retval = mips32_cp0_read(ejtag_info, &ejtag_info->config[i], 16, i);
+			if (retval != ERROR_OK) {
+				LOG_ERROR("isa info not available, failed to read cp0 config register: %" PRId32, i);
+				ejtag_info->config_regs = 0;
+				return retval;
+			}
+			ejtag_info->config_regs = i + 1;
+			if ((ejtag_info->config[i] & (1 << 31)) == 0)
+				break;	/* no more config registers implemented */
+		}
+	else
+		return ERROR_OK;	/* already succesfully read */
+
+	LOG_DEBUG("read  %"PRId32" config registers", ejtag_info->config_regs);
+
+	if (ejtag_info->impcode & EJTAG_IMP_MIPS16) {
+		mips32->isa_imp = MIPS32_MIPS16;
+		LOG_USER("MIPS32 with MIPS16 support implemented");
+
+	} else if (ejtag_info->config_regs >= 4) {	/* config3 implemented */
+		unsigned isa_imp = (ejtag_info->config[3] & MIPS32_CONFIG3_ISA_MASK) >> MIPS32_CONFIG3_ISA_SHIFT;
+		if (isa_imp == 1) {
+			mips32->isa_imp = MMIPS32_ONLY;
+			LOG_USER("MICRO MIPS32 only implemented");
+
+		} else if (isa_imp != 0) {
+			mips32->isa_imp = MIPS32_MMIPS32;
+			LOG_USER("MIPS32 and MICRO MIPS32 implemented");
+		}
+	}
+
+	if (mips32->isa_imp == MIPS32_ONLY)	/* initial default value */
+		LOG_USER("MIPS32 only implemented");
+
+	return ERROR_OK;
+}
+int mips32_checksum_memory(struct target *target, target_addr_t address,
 		uint32_t count, uint32_t *checksum)
 {
 	struct working_area *crc_algorithm;
 	struct reg_param reg_params[2];
 	struct mips32_algorithm mips32_info;
 
-	/* see contrib/loaders/checksum/mips32.s for src */
+	struct mips32_common *mips32 = target_to_mips32(target);
+	struct mips_ejtag *ejtag_info = &mips32->ejtag_info;
 
-	static const uint32_t mips_crc_code[] = {
-		0x248C0000,		/* addiu	$t4, $a0, 0 */
-		0x24AA0000,		/* addiu	$t2, $a1, 0 */
-		0x2404FFFF,		/* addiu	$a0, $zero, 0xffffffff */
-		0x10000010,		/* beq		$zero, $zero, ncomp */
-		0x240B0000,		/* addiu	$t3, $zero, 0 */
+	/* see contrib/loaders/checksum/mips32.s for src */
+	uint32_t isa = ejtag_info->isa ? 1 : 0;
+
+	uint32_t mips_crc_code[] = {
+		MIPS32_ADDIU(isa, 12, 4, 0),			/* addiu	$t4, $a0, 0 */
+		MIPS32_ADDIU(isa, 10, 5, 0),			/* addiu	$t2, $a1, 0 */
+		MIPS32_ADDIU(isa, 4, 0, 0xFFFF),		/* addiu	$a0, $zero, 0xffff */
+		MIPS32_BEQ(isa, 0, 0, 0x10 << isa),		/* beq		$zero, $zero, ncomp */
+		MIPS32_ADDIU(isa, 11, 0, 0),			/* addiu	$t3, $zero, 0 */
 						/* nbyte: */
-		0x81850000,		/* lb		$a1, ($t4) */
-		0x218C0001,		/* addi		$t4, $t4, 1 */
-		0x00052E00,		/* sll		$a1, $a1, 24 */
-		0x3C0204C1,		/* lui		$v0, 0x04c1 */
-		0x00852026,		/* xor		$a0, $a0, $a1 */
-		0x34471DB7,		/* ori		$a3, $v0, 0x1db7 */
-		0x00003021,		/* addu		$a2, $zero, $zero */
-						/* loop: */
-		0x00044040,		/* sll		$t0, $a0, 1 */
-		0x24C60001,		/* addiu	$a2, $a2, 1 */
-		0x28840000,		/* slti		$a0, $a0, 0 */
-		0x01074826,		/* xor		$t1, $t0, $a3 */
-		0x0124400B,		/* movn		$t0, $t1, $a0 */
-		0x28C30008,		/* slti		$v1, $a2, 8 */
-		0x1460FFF9,		/* bne		$v1, $zero, loop */
-		0x01002021,		/* addu		$a0, $t0, $zero */
-						/* ncomp: */
-		0x154BFFF0,		/* bne		$t2, $t3, nbyte */
-		0x256B0001,		/* addiu	$t3, $t3, 1 */
-		0x7000003F,		/* sdbbp */
+		MIPS32_LB(isa, 5, 0, 12),			/* lb		$a1, ($t4) */
+		MIPS32_ADDI(isa, 12, 12, 1),			/* addi		$t4, $t4, 1 */
+		MIPS32_SLL(isa, 5, 5, 24),			/* sll		$a1, $a1, 24 */
+		MIPS32_LUI(isa, 2, 0x04c1),			/* lui		$v0, 0x04c1 */
+		MIPS32_XOR(isa, 4, 4, 5),			/* xor		$a0, $a0, $a1 */
+		MIPS32_ORI(isa, 7, 2, 0x1db7),			/* ori		$a3, $v0, 0x1db7 */
+		MIPS32_ADDU(isa, 6, 0, 0),			/* addu		$a2, $zero, $zero */
+						/* loop */
+		MIPS32_SLL(isa, 8, 4, 1),			/* sll		$t0, $a0, 1 */
+		MIPS32_ADDIU(isa, 6, 6, 1),			/* addiu	$a2, $a2, 1 */
+		MIPS32_SLTI(isa, 4, 4, 0),			/* slti		$a0, $a0, 0 */
+		MIPS32_XOR(isa, 9, 8, 7),			/* xor		$t1, $t0, $a3 */
+		MIPS32_MOVN(isa, 8, 9, 4),			/* movn		$t0, $t1, $a0 */
+		MIPS32_SLTI(isa, 3, 6, 8),			/* slti		$v1, $a2, 8 */
+		MIPS32_BNE(isa, 3, 0, NEG16(7 << isa)),		/* bne		$v1, $zero, loop */
+		MIPS32_ADDU(isa, 4, 8, 0),			/* addu		$a0, $t0, $zero */
+						/* ncomp */
+		MIPS32_BNE(isa, 10, 11, NEG16(16 << isa)),	/* bne		$t2, $t3, nbyte */
+		MIPS32_ADDIU(isa, 11, 11, 1),			/* addiu	$t3, $t3, 1 */
+		MIPS32_SDBBP(isa),
 	};
 
 	/* make sure we have a working area */
 	if (target_alloc_working_area(target, sizeof(mips_crc_code), &crc_algorithm) != ERROR_OK)
 		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 
+	pracc_swap16_array(ejtag_info, mips_crc_code, ARRAY_SIZE(mips_crc_code));
+
 	/* convert mips crc code into a buffer in target endianness */
 	uint8_t mips_crc_code_8[sizeof(mips_crc_code)];
 	target_buffer_set_u32_array(target, mips_crc_code_8,
 					ARRAY_SIZE(mips_crc_code), mips_crc_code);
 
-	target_write_buffer(target, crc_algorithm->address, sizeof(mips_crc_code), mips_crc_code_8);
+	int retval = target_write_buffer(target, crc_algorithm->address, sizeof(mips_crc_code), mips_crc_code_8);
+	if (retval != ERROR_OK)
+		return retval;
 
 	mips32_info.common_magic = MIPS32_COMMON_MAGIC;
-	mips32_info.isa_mode = MIPS32_ISA_MIPS32;
+	mips32_info.isa_mode = isa ? MIPS32_ISA_MMIPS32 : MIPS32_ISA_MIPS32;	/* run isa as in debug mode */
 
 	init_reg_param(&reg_params[0], "r4", 32, PARAM_IN_OUT);
 	buf_set_u32(reg_params[0].value, 0, 32, address);
@@ -756,9 +811,8 @@ int mips32_checksum_memory(struct target *target, uint32_t address,
 
 	int timeout = 20000 * (1 + (count / (1024 * 1024)));
 
-	int retval = target_run_algorithm(target, 0, NULL, 2, reg_params,
-			crc_algorithm->address, crc_algorithm->address + (sizeof(mips_crc_code) - 4), timeout,
-			&mips32_info);
+	retval = target_run_algorithm(target, 0, NULL, 2, reg_params, crc_algorithm->address,
+				      crc_algorithm->address + (sizeof(mips_crc_code) - 4), timeout, &mips32_info);
 
 	if (retval == ERROR_OK)
 		*checksum = buf_get_u32(reg_params[0].value, 0, 32);
@@ -771,37 +825,51 @@ int mips32_checksum_memory(struct target *target, uint32_t address,
 	return retval;
 }
 
-/** Checks whether a memory region is zeroed. */
+/** Checks whether a memory region is erased. */
 int mips32_blank_check_memory(struct target *target,
-		uint32_t address, uint32_t count, uint32_t *blank)
+		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[3];
 	struct mips32_algorithm mips32_info;
 
-	static const uint32_t erase_check_code[] = {
+	struct mips32_common *mips32 = target_to_mips32(target);
+	struct mips_ejtag *ejtag_info = &mips32->ejtag_info;
+
+	if (erased_value != 0xff) {
+		LOG_ERROR("Erase value 0x%02" PRIx8 " not yet supported for MIPS32",
+			erased_value);
+		return ERROR_FAIL;
+	}
+	uint32_t isa = ejtag_info->isa ? 1 : 0;
+	uint32_t erase_check_code[] = {
 						/* nbyte: */
-		0x80880000,		/* lb		$t0, ($a0) */
-		0x00C83024,		/* and		$a2, $a2, $t0 */
-		0x24A5FFFF,		/* addiu	$a1, $a1, -1 */
-		0x14A0FFFC,		/* bne		$a1, $zero, nbyte */
-		0x24840001,		/* addiu	$a0, $a0, 1 */
-		0x7000003F		/* sdbbp */
+		MIPS32_LB(isa, 8, 0, 4),			/* lb		$t0, ($a0) */
+		MIPS32_AND(isa, 6, 6, 8),			/* and		$a2, $a2, $t0 */
+		MIPS32_ADDIU(isa, 5, 5, NEG16(1)),		/* addiu	$a1, $a1, -1 */
+		MIPS32_BNE(isa, 5, 0, NEG16(4 << isa)),		/* bne		$a1, $zero, nbyte */
+		MIPS32_ADDIU(isa, 4, 4, 1),			/* addiu	$a0, $a0, 1 */
+		MIPS32_SDBBP(isa)				/* sdbbp */
 	};
 
 	/* make sure we have a working area */
 	if (target_alloc_working_area(target, sizeof(erase_check_code), &erase_check_algorithm) != ERROR_OK)
 		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 
+	pracc_swap16_array(ejtag_info, erase_check_code, ARRAY_SIZE(erase_check_code));
+
 	/* convert erase check code into a buffer in target endianness */
 	uint8_t erase_check_code_8[sizeof(erase_check_code)];
 	target_buffer_set_u32_array(target, erase_check_code_8,
 					ARRAY_SIZE(erase_check_code), erase_check_code);
 
-	target_write_buffer(target, erase_check_algorithm->address, sizeof(erase_check_code), erase_check_code_8);
+	int retval = target_write_buffer(target, erase_check_algorithm->address,
+						sizeof(erase_check_code), erase_check_code_8);
+	if (retval != ERROR_OK)
+		return retval;
 
 	mips32_info.common_magic = MIPS32_COMMON_MAGIC;
-	mips32_info.isa_mode = MIPS32_ISA_MIPS32;
+	mips32_info.isa_mode = isa ? MIPS32_ISA_MMIPS32 : MIPS32_ISA_MIPS32;
 
 	init_reg_param(&reg_params[0], "r4", 32, PARAM_OUT);
 	buf_set_u32(reg_params[0].value, 0, 32, address);
@@ -810,12 +878,10 @@ int mips32_blank_check_memory(struct target *target,
 	buf_set_u32(reg_params[1].value, 0, 32, count);
 
 	init_reg_param(&reg_params[2], "r6", 32, PARAM_IN_OUT);
-	buf_set_u32(reg_params[2].value, 0, 32, 0xff);
+	buf_set_u32(reg_params[2].value, 0, 32, erased_value);
 
-	int retval = target_run_algorithm(target, 0, NULL, 3, reg_params,
-			erase_check_algorithm->address,
-			erase_check_algorithm->address + (sizeof(erase_check_code) - 4),
-			10000, &mips32_info);
+	retval = target_run_algorithm(target, 0, NULL, 3, reg_params, erase_check_algorithm->address,
+			erase_check_algorithm->address + (sizeof(erase_check_code) - 4), 10000, &mips32_info);
 
 	if (retval == ERROR_OK)
 		*blank = buf_get_u32(reg_params[2].value, 0, 32);
@@ -911,7 +977,7 @@ COMMAND_HANDLER(mips32_handle_scan_delay_command)
 			return ERROR_COMMAND_SYNTAX_ERROR;
 
 	command_print(CMD_CTX, "scan delay: %d nsec", ejtag_info->scan_delay);
-	if (ejtag_info->scan_delay >= 2000000) {
+	if (ejtag_info->scan_delay >= MIPS32_SCAN_DELAY_LEGACY_MODE) {
 		ejtag_info->mode = 0;
 		command_print(CMD_CTX, "running in legacy mode");
 	} else {
diff --git a/src/target/mips32.h b/src/target/mips32.h
index bfd2cf5..928598f 100644
--- a/src/target/mips32.h
+++ b/src/target/mips32.h
@@ -58,9 +58,14 @@
 #define MIPS32_CONFIG1_DL_SHIFT 10
 #define MIPS32_CONFIG1_DL_MASK (0x7 << MIPS32_CONFIG1_DL_SHIFT)
 
+#define MIPS32_CONFIG3_ISA_SHIFT	14
+#define MIPS32_CONFIG3_ISA_MASK		(3 << MIPS32_CONFIG3_ISA_SHIFT)
+
 #define MIPS32_ARCH_REL1 0x0
 #define MIPS32_ARCH_REL2 0x1
 
+#define MIPS32_SCAN_DELAY_LEGACY_MODE 2000000
+
 /* offsets into mips32 core register cache */
 enum {
 	MIPS32_PC = 37,
@@ -71,6 +76,14 @@ enum {
 enum mips32_isa_mode {
 	MIPS32_ISA_MIPS32 = 0,
 	MIPS32_ISA_MIPS16E = 1,
+	MIPS32_ISA_MMIPS32 = 3,
+};
+
+enum mips32_isa_imp {
+	MIPS32_ONLY = 0,
+	MMIPS32_ONLY = 1,
+	MIPS32_MIPS16 = 2,
+	MIPS32_MMIPS32 = 3,
 };
 
 struct mips32_comparator {
@@ -86,6 +99,7 @@ struct mips32_common {
 	struct mips_ejtag ejtag_info;
 	uint32_t core_regs[MIPS32NUMCOREREGS];
 	enum mips32_isa_mode isa_mode;
+	enum mips32_isa_imp isa_imp;
 
 	/* working area for fastdata access */
 	struct working_area *fast_data_area;
@@ -120,44 +134,49 @@ struct mips32_algorithm {
 	enum mips32_isa_mode isa_mode;
 };
 
-#define MIPS32_OP_ADDIU 0x21
-#define MIPS32_OP_ANDI	0x0C
-#define MIPS32_OP_BEQ	0x04
-#define MIPS32_OP_BGTZ	0x07
-#define MIPS32_OP_BNE	0x05
-#define MIPS32_OP_ADDI	0x08
-#define MIPS32_OP_AND	0x24
-#define MIPS32_OP_CACHE	0x2F
-#define MIPS32_OP_COP0	0x10
-#define MIPS32_OP_J	0x02
-#define MIPS32_OP_JR	0x08
-#define MIPS32_OP_LUI	0x0F
-#define MIPS32_OP_LW	0x23
-#define MIPS32_OP_LBU	0x24
-#define MIPS32_OP_LHU	0x25
-#define MIPS32_OP_MFHI	0x10
-#define MIPS32_OP_MTHI	0x11
-#define MIPS32_OP_MFLO	0x12
-#define MIPS32_OP_MTLO	0x13
-#define MIPS32_OP_RDHWR 0x3B
-#define MIPS32_OP_SB	0x28
-#define MIPS32_OP_SH	0x29
-#define MIPS32_OP_SW	0x2B
-#define MIPS32_OP_ORI	0x0D
-#define MIPS32_OP_XORI	0x0E
-#define MIPS32_OP_XOR	0x26
-#define MIPS32_OP_SLTU  0x2B
-#define MIPS32_OP_SRL	0x03
-#define MIPS32_OP_SYNCI	0x1F
-
-#define MIPS32_OP_REGIMM	0x01
-#define MIPS32_OP_SDBBP		0x3F
-#define MIPS32_OP_SPECIAL	0x00
-#define MIPS32_OP_SPECIAL2	0x07
-#define MIPS32_OP_SPECIAL3	0x1F
-
-#define MIPS32_COP0_MF	0x00
-#define MIPS32_COP0_MT	0x04
+#define MIPS32_OP_ADDU	0x21u
+#define MIPS32_OP_ADDIU	0x09u
+#define MIPS32_OP_ANDI	0x0Cu
+#define MIPS32_OP_BEQ	0x04u
+#define MIPS32_OP_BGTZ	0x07u
+#define MIPS32_OP_BNE	0x05u
+#define MIPS32_OP_ADDI	0x08u
+#define MIPS32_OP_AND	0x24u
+#define MIPS32_OP_CACHE	0x2Fu
+#define MIPS32_OP_COP0	0x10u
+#define MIPS32_OP_J	0x02u
+#define MIPS32_OP_JR	0x08u
+#define MIPS32_OP_LUI	0x0Fu
+#define MIPS32_OP_LW	0x23u
+#define MIPS32_OP_LB	0x20u
+#define MIPS32_OP_LBU	0x24u
+#define MIPS32_OP_LHU	0x25u
+#define MIPS32_OP_MFHI	0x10u
+#define MIPS32_OP_MTHI	0x11u
+#define MIPS32_OP_MFLO	0x12u
+#define MIPS32_OP_MTLO	0x13u
+#define MIPS32_OP_RDHWR	0x3Bu
+#define MIPS32_OP_SB	0x28u
+#define MIPS32_OP_SH	0x29u
+#define MIPS32_OP_SW	0x2Bu
+#define MIPS32_OP_ORI	0x0Du
+#define MIPS32_OP_XORI	0x0Eu
+#define MIPS32_OP_XOR	0x26u
+#define MIPS32_OP_SLTU	0x2Bu
+#define MIPS32_OP_SRL	0x03u
+#define MIPS32_OP_SYNCI	0x1Fu
+#define MIPS32_OP_SLL	0x00u
+#define MIPS32_OP_SLTI	0x0Au
+#define MIPS32_OP_MOVN	0x0Bu
+
+#define MIPS32_OP_REGIMM	0x01u
+#define MIPS32_OP_SDBBP	0x3Fu
+#define MIPS32_OP_SPECIAL	0x00u
+#define MIPS32_OP_SPECIAL2	0x07u
+#define MIPS32_OP_SPECIAL3	0x1Fu
+
+#define MIPS32_COP0_MF	0x00u
+#define MIPS32_COP0_MT	0x04u
 
 #define MIPS32_R_INST(opcode, rs, rt, rd, shamt, funct) \
 	(((opcode) << 26) | ((rs) << 21) | ((rt) << 16) | ((rd) << 11) | ((shamt) << 6) | (funct))
@@ -165,41 +184,52 @@ struct mips32_algorithm {
 	(((opcode) << 26) | ((rs) << 21) | ((rt) << 16) | (immd))
 #define MIPS32_J_INST(opcode, addr)	(((opcode) << 26) | (addr))
 
-#define MIPS32_NOP						0
-#define MIPS32_ADDI(tar, src, val)		MIPS32_I_INST(MIPS32_OP_ADDI, src, tar, val)
-#define MIPS32_ADDU(dst, src, tar)		MIPS32_R_INST(MIPS32_OP_SPECIAL, src, tar, dst, 0, MIPS32_OP_ADDIU)
-#define MIPS32_AND(reg, off, val)		MIPS32_R_INST(0, off, val, reg, 0, MIPS32_OP_AND)
-#define MIPS32_ANDI(tar, src, val)		MIPS32_I_INST(MIPS32_OP_ANDI, src, tar, val)
-#define MIPS32_B(off)					MIPS32_BEQ(0, 0, off)
-#define MIPS32_BEQ(src, tar, off)		MIPS32_I_INST(MIPS32_OP_BEQ, src, tar, off)
-#define MIPS32_BGTZ(reg, off)			MIPS32_I_INST(MIPS32_OP_BGTZ, reg, 0, off)
-#define MIPS32_BNE(src, tar, off)		MIPS32_I_INST(MIPS32_OP_BNE, src, tar, off)
-#define MIPS32_CACHE(op, off, base)		MIPS32_I_INST(MIPS32_OP_CACHE, base, op, off)
-#define MIPS32_J(tar)				MIPS32_J_INST(MIPS32_OP_J, tar)
-#define MIPS32_JR(reg)					MIPS32_R_INST(0, reg, 0, 0, 0, MIPS32_OP_JR)
-#define MIPS32_MFC0(gpr, cpr, sel)		MIPS32_R_INST(MIPS32_OP_COP0, MIPS32_COP0_MF, gpr, cpr, 0, sel)
-#define MIPS32_MTC0(gpr, cpr, sel)		MIPS32_R_INST(MIPS32_OP_COP0, MIPS32_COP0_MT, gpr, cpr, 0, sel)
-#define MIPS32_LBU(reg, off, base)		MIPS32_I_INST(MIPS32_OP_LBU, base, reg, off)
-#define MIPS32_LHU(reg, off, base)		MIPS32_I_INST(MIPS32_OP_LHU, base, reg, off)
-#define MIPS32_LUI(reg, val)			MIPS32_I_INST(MIPS32_OP_LUI, 0, reg, val)
-#define MIPS32_LW(reg, off, base)		MIPS32_I_INST(MIPS32_OP_LW, base, reg, off)
-#define MIPS32_MFLO(reg)				MIPS32_R_INST(0, 0, 0, reg, 0, MIPS32_OP_MFLO)
-#define MIPS32_MFHI(reg)				MIPS32_R_INST(0, 0, 0, reg, 0, MIPS32_OP_MFHI)
-#define MIPS32_MTLO(reg)				MIPS32_R_INST(0, reg, 0, 0, 0, MIPS32_OP_MTLO)
-#define MIPS32_MTHI(reg)				MIPS32_R_INST(0, reg, 0, 0, 0, MIPS32_OP_MTHI)
-#define MIPS32_ORI(tar, src, val)		MIPS32_I_INST(MIPS32_OP_ORI, src, tar, val)
-#define MIPS32_XORI(tar, src, val)		MIPS32_I_INST(MIPS32_OP_XORI, src, tar, val)
-#define MIPS32_RDHWR(tar, dst)			MIPS32_R_INST(MIPS32_OP_SPECIAL3, 0, tar, dst, 0, MIPS32_OP_RDHWR)
-#define MIPS32_SB(reg, off, base)		MIPS32_I_INST(MIPS32_OP_SB, base, reg, off)
-#define MIPS32_SH(reg, off, base)		MIPS32_I_INST(MIPS32_OP_SH, base, reg, off)
-#define MIPS32_SW(reg, off, base)		MIPS32_I_INST(MIPS32_OP_SW, base, reg, off)
-#define MIPS32_XOR(reg, val1, val2)		MIPS32_R_INST(0, val1, val2, reg, 0, MIPS32_OP_XOR)
-#define MIPS32_SRL(reg, src, off)		MIPS32_R_INST(0, 0, src, reg, off, MIPS32_OP_SRL)
-#define MIPS32_SLTU(dst, src, tar)		MIPS32_R_INST(MIPS32_OP_SPECIAL, src, tar, dst, 0, MIPS32_OP_SLTU)
-#define MIPS32_SYNCI(off, base)			MIPS32_I_INST(MIPS32_OP_REGIMM, base, MIPS32_OP_SYNCI, off)
-
-#define MIPS32_SYNC			0xF
-#define MIPS32_SYNCI_STEP	0x1	/* reg num od address step size to be used with synci instruction */
+#define MIPS32_ISA_NOP				0
+#define MIPS32_ISA_ADDI(tar, src, val)		MIPS32_I_INST(MIPS32_OP_ADDI, src, tar, val)
+#define MIPS32_ISA_ADDIU(tar, src, val)		MIPS32_I_INST(MIPS32_OP_ADDIU, src, tar, val)
+#define MIPS32_ISA_ADDU(dst, src, tar)		MIPS32_R_INST(MIPS32_OP_SPECIAL, src, tar, dst, 0, MIPS32_OP_ADDU)
+#define MIPS32_ISA_AND(dst, src, tar)		MIPS32_R_INST(0, src, tar, dst, 0, MIPS32_OP_AND)
+#define MIPS32_ISA_ANDI(tar, src, val)		MIPS32_I_INST(MIPS32_OP_ANDI, src, tar, val)
+
+#define MIPS32_ISA_B(off)			MIPS32_ISA_BEQ(0, 0, off)
+#define MIPS32_ISA_BEQ(src, tar, off)		MIPS32_I_INST(MIPS32_OP_BEQ, src, tar, off)
+#define MIPS32_ISA_BGTZ(reg, off)		MIPS32_I_INST(MIPS32_OP_BGTZ, reg, 0, off)
+#define MIPS32_ISA_BNE(src, tar, off)		MIPS32_I_INST(MIPS32_OP_BNE, src, tar, off)
+#define MIPS32_ISA_CACHE(op, off, base)		MIPS32_I_INST(MIPS32_OP_CACHE, base, op, off)
+#define MIPS32_ISA_J(tar)			MIPS32_J_INST(MIPS32_OP_J, (0x0FFFFFFFu & (tar)) >> 2)
+#define MIPS32_ISA_JR(reg)			MIPS32_R_INST(0, reg, 0, 0, 0, MIPS32_OP_JR)
+
+#define MIPS32_ISA_LB(reg, off, base)		MIPS32_I_INST(MIPS32_OP_LB, base, reg, off)
+#define MIPS32_ISA_LBU(reg, off, base)		MIPS32_I_INST(MIPS32_OP_LBU, base, reg, off)
+#define MIPS32_ISA_LHU(reg, off, base)		MIPS32_I_INST(MIPS32_OP_LHU, base, reg, off)
+#define MIPS32_ISA_LUI(reg, val)		MIPS32_I_INST(MIPS32_OP_LUI, 0, reg, val)
+#define MIPS32_ISA_LW(reg, off, base)		MIPS32_I_INST(MIPS32_OP_LW, base, reg, off)
+
+#define MIPS32_ISA_MFC0(gpr, cpr, sel)		MIPS32_R_INST(MIPS32_OP_COP0, MIPS32_COP0_MF, gpr, cpr, 0, sel)
+#define MIPS32_ISA_MTC0(gpr, cpr, sel)		MIPS32_R_INST(MIPS32_OP_COP0, MIPS32_COP0_MT, gpr, cpr, 0, sel)
+#define MIPS32_ISA_MFLO(reg)			MIPS32_R_INST(0, 0, 0, reg, 0, MIPS32_OP_MFLO)
+#define MIPS32_ISA_MFHI(reg)			MIPS32_R_INST(0, 0, 0, reg, 0, MIPS32_OP_MFHI)
+#define MIPS32_ISA_MTLO(reg)			MIPS32_R_INST(0, reg, 0, 0, 0, MIPS32_OP_MTLO)
+#define MIPS32_ISA_MTHI(reg)			MIPS32_R_INST(0, reg, 0, 0, 0, MIPS32_OP_MTHI)
+
+#define MIPS32_ISA_MOVN(dst, src, tar)		MIPS32_R_INST(MIPS32_OP_SPECIAL, src, tar, dst, 0, MIPS32_OP_MOVN)
+#define MIPS32_ISA_ORI(tar, src, val)		MIPS32_I_INST(MIPS32_OP_ORI, src, tar, val)
+#define MIPS32_ISA_RDHWR(tar, dst)		MIPS32_R_INST(MIPS32_OP_SPECIAL3, 0, tar, dst, 0, MIPS32_OP_RDHWR)
+#define MIPS32_ISA_SB(reg, off, base)		MIPS32_I_INST(MIPS32_OP_SB, base, reg, off)
+#define MIPS32_ISA_SH(reg, off, base)		MIPS32_I_INST(MIPS32_OP_SH, base, reg, off)
+#define MIPS32_ISA_SW(reg, off, base)		MIPS32_I_INST(MIPS32_OP_SW, base, reg, off)
+
+#define MIPS32_ISA_SLL(dst, src, sa)		MIPS32_R_INST(MIPS32_OP_SPECIAL, 0, src, dst, sa, MIPS32_OP_SLL)
+#define MIPS32_ISA_SLTI(tar, src, val)		MIPS32_I_INST(MIPS32_OP_SLTI, src, tar, val)
+#define MIPS32_ISA_SLTU(dst, src, tar)		MIPS32_R_INST(MIPS32_OP_SPECIAL, src, tar, dst, 0, MIPS32_OP_SLTU)
+#define MIPS32_ISA_SRL(reg, src, off)		MIPS32_R_INST(0, 0, src, reg, off, MIPS32_OP_SRL)
+#define MIPS32_ISA_SYNC				0xFu
+#define MIPS32_ISA_SYNCI(off, base)		MIPS32_I_INST(MIPS32_OP_REGIMM, base, MIPS32_OP_SYNCI, off)
+
+#define MIPS32_ISA_XOR(reg, val1, val2)		MIPS32_R_INST(0, val1, val2, reg, 0, MIPS32_OP_XOR)
+#define MIPS32_ISA_XORI(tar, src, val)		MIPS32_I_INST(MIPS32_OP_XORI, src, tar, val)
+
+#define MIPS32_ISA_SYNCI_STEP		0x1	/* reg num od address step size to be used with synci instruction */
 
 /**
  * Cache operations definitions
@@ -211,9 +241,158 @@ struct mips32_algorithm {
 #define MIPS32_CACHE_I_HIT_INVALIDATE ((0x0 << 0) | (0x4 << 2))
 
 /* ejtag specific instructions */
-#define MIPS32_DRET					0x4200001F
-#define MIPS32_SDBBP				0x7000003F	/* MIPS32_J_INST(MIPS32_OP_SPECIAL2, MIPS32_OP_SDBBP) */
-#define MIPS16_SDBBP				0xE801
+#define MIPS32_ISA_DRET				0x4200001Fu
+/* MIPS32_ISA_J_INST(MIPS32_ISA_OP_SPECIAL2, MIPS32_ISA_OP_SDBBP) */
+#define MIPS32_ISA_SDBBP			0x7000003Fu
+#define MIPS16_ISA_SDBBP			0xE801u
+
+/*MICRO MIPS INSTRUCTIONS, see doc MD00582 */
+#define POOL32A					0X00u
+#define POOL32AXf				0x3Cu
+#define POOL32B					0x08u
+#define POOL32I					0x10u
+#define MMIPS32_OP_ADDI			0x04u
+#define MMIPS32_OP_ADDIU		0x0Cu
+#define MMIPS32_OP_ADDU			0x150u
+#define MMIPS32_OP_AND			0x250u
+#define MMIPS32_OP_ANDI			0x34u
+#define MMIPS32_OP_BEQ			0x25u
+#define MMIPS32_OP_BGTZ			0x06u
+#define MMIPS32_OP_BNE			0x2Du
+#define MMIPS32_OP_CACHE		0x06u
+#define MMIPS32_OP_J			0x35u
+#define MMIPS32_OP_JALR			0x03Cu
+#define MMIPS32_OP_LB			0x07u
+#define MMIPS32_OP_LBU			0x05u
+#define MMIPS32_OP_LHU			0x0Du
+#define MMIPS32_OP_LUI			0x0Du
+#define MMIPS32_OP_LW			0x3Fu
+#define MMIPS32_OP_MFC0			0x03u
+#define MMIPS32_OP_MTC0			0x0Bu
+#define MMIPS32_OP_MFLO			0x075u
+#define MMIPS32_OP_MFHI			0x035u
+#define MMIPS32_OP_MTLO			0x0F5u
+#define MMIPS32_OP_MTHI			0x0B5u
+#define MMIPS32_OP_MOVN			0x018u
+#define MMIPS32_OP_ORI			0x14u
+#define MMIPS32_OP_RDHWR		0x1ACu
+#define MMIPS32_OP_SB			0x06u
+#define MMIPS32_OP_SH			0x0Eu
+#define MMIPS32_OP_SW			0x3Eu
+#define MMIPS32_OP_SLTU			0x390u
+#define MMIPS32_OP_SLL			0x000u
+#define MMIPS32_OP_SLTI			0x24u
+#define MMIPS32_OP_SRL			0x040u
+#define MMIPS32_OP_SYNCI		0x10u
+#define MMIPS32_OP_XOR			0x310u
+#define MMIPS32_OP_XORI			0x1Cu
+
+#define MMIPS32_ADDI(tar, src, val)		MIPS32_I_INST(MMIPS32_OP_ADDI, tar, src, val)
+#define MMIPS32_ADDIU(tar, src, val)		MIPS32_I_INST(MMIPS32_OP_ADDIU, tar, src, val)
+#define MMIPS32_ADDU(dst, src, tar)		MIPS32_R_INST(POOL32A, tar, src, dst, 0, MMIPS32_OP_ADDU)
+#define MMIPS32_AND(dst, src, tar)		MIPS32_R_INST(POOL32A, tar, src, dst, 0, MMIPS32_OP_AND)
+#define MMIPS32_ANDI(tar, src, val)		MIPS32_I_INST(MMIPS32_OP_ANDI, tar, src, val)
+
+#define MMIPS32_B(off)				MMIPS32_BEQ(0, 0, off)
+#define MMIPS32_BEQ(src, tar, off)		MIPS32_I_INST(MMIPS32_OP_BEQ, tar, src, off)
+#define MMIPS32_BGTZ(reg, off)			MIPS32_I_INST(POOL32I, MMIPS32_OP_BGTZ, reg, off)
+#define MMIPS32_BNE(src, tar, off)		MIPS32_I_INST(MMIPS32_OP_BNE, tar, src, off)
+#define MMIPS32_CACHE(op, off, base)		MIPS32_R_INST(POOL32B, op, base, MMIPS32_OP_CACHE << 1, 0, off)
+
+#define MMIPS32_J(tar)				MIPS32_J_INST(MMIPS32_OP_J, ((0x07FFFFFFu & ((tar) >> 1))))
+#define MMIPS32_JR(reg)				MIPS32_R_INST(POOL32A, 0, reg, 0, MMIPS32_OP_JALR, POOL32AXf)
+#define MMIPS32_LB(reg, off, base)		MIPS32_I_INST(MMIPS32_OP_LB, reg, base, off)
+#define MMIPS32_LBU(reg, off, base)		MIPS32_I_INST(MMIPS32_OP_LBU, reg, base, off)
+#define MMIPS32_LHU(reg, off, base)		MIPS32_I_INST(MMIPS32_OP_LHU, reg, base, off)
+#define MMIPS32_LUI(reg, val)			MIPS32_I_INST(POOL32I, MMIPS32_OP_LUI, reg, val)
+#define MMIPS32_LW(reg, off, base)		MIPS32_I_INST(MMIPS32_OP_LW, reg, base, off)
+
+#define MMIPS32_MFC0(gpr, cpr, sel)		MIPS32_R_INST(POOL32A, gpr, cpr, sel, MMIPS32_OP_MFC0, POOL32AXf)
+#define MMIPS32_MFLO(reg)			MIPS32_R_INST(POOL32A, 0, reg, 0, MMIPS32_OP_MFLO, POOL32AXf)
+#define MMIPS32_MFHI(reg)			MIPS32_R_INST(POOL32A, 0, reg, 0, MMIPS32_OP_MFHI, POOL32AXf)
+#define MMIPS32_MTC0(gpr, cpr, sel)		MIPS32_R_INST(POOL32A, gpr, cpr, sel, MMIPS32_OP_MTC0, POOL32AXf)
+#define MMIPS32_MTLO(reg)			MIPS32_R_INST(POOL32A, 0, reg, 0, MMIPS32_OP_MTLO, POOL32AXf)
+#define MMIPS32_MTHI(reg)			MIPS32_R_INST(POOL32A, 0, reg, 0, MMIPS32_OP_MTHI, POOL32AXf)
+
+#define MMIPS32_MOVN(dst, src, tar)		MIPS32_R_INST(POOL32A, tar, src, dst, 0, MMIPS32_OP_MOVN)
+#define MMIPS32_NOP				0
+#define MMIPS32_ORI(tar, src, val)		MIPS32_I_INST(MMIPS32_OP_ORI, tar, src, val)
+#define MMIPS32_RDHWR(tar, dst)			MIPS32_R_INST(POOL32A, dst, tar, 0, MMIPS32_OP_RDHWR, POOL32AXf)
+#define MMIPS32_SB(reg, off, base)		MIPS32_I_INST(MMIPS32_OP_SB, reg, base, off)
+#define MMIPS32_SH(reg, off, base)		MIPS32_I_INST(MMIPS32_OP_SH, reg, base, off)
+#define MMIPS32_SW(reg, off, base)		MIPS32_I_INST(MMIPS32_OP_SW, reg, base, off)
+
+#define MMIPS32_SRL(reg, src, off)		MIPS32_R_INST(POOL32A, reg, src, off, 0, MMIPS32_OP_SRL)
+#define MMIPS32_SLTU(dst, src, tar)		MIPS32_R_INST(POOL32A, tar, src, dst, 0, MMIPS32_OP_SLTU)
+#define MMIPS32_SYNCI(off, base)		MIPS32_I_INST(POOL32I, MMIPS32_OP_SYNCI, base, off)
+#define MMIPS32_SLL(dst, src, sa)		MIPS32_R_INST(POOL32A, dst, src, sa, 0, MMIPS32_OP_SLL)
+#define MMIPS32_SLTI(tar, src, val)		MIPS32_I_INST(MMIPS32_OP_SLTI, tar, src, val)
+#define MMIPS32_SYNC				0x00001A7Cu /* MIPS32_R_INST(POOL32A, 0, 0, 0, 0x1ADu, POOL32AXf) */
+
+#define MMIPS32_XOR(reg, val1, val2)		MIPS32_R_INST(POOL32A, val1, val2, reg, 0, MMIPS32_OP_XOR)
+#define MMIPS32_XORI(tar, src, val)		MIPS32_I_INST(MMIPS32_OP_XORI, tar, src, val)
+
+#define MMIPS32_SYNCI_STEP	0x1u	/* reg num od address step size to be used with synci instruction */
+
+
+/* ejtag specific instructions */
+#define MMIPS32_DRET			0x0000E37Cu	/* MIPS32_R_INST(POOL32A, 0, 0, 0, 0x38D, POOL32AXf) */
+#define MMIPS32_SDBBP			0x0000DB7Cu	/* MIPS32_R_INST(POOL32A, 0, 0, 0, 0x1BD, POOL32AXf) */
+#define MMIPS16_SDBBP			0x46C0u		/* POOL16C instr */
+
+/* instruction code with isa selection */
+#define MIPS32_NOP				0	/* same for both isa's */
+#define MIPS32_ADDI(isa, tar, src, val)		(isa ? MMIPS32_ADDI(tar, src, val) : MIPS32_ISA_ADDI(tar, src, val))
+#define MIPS32_ADDIU(isa, tar, src, val)	(isa ? MMIPS32_ADDIU(tar, src, val) : MIPS32_ISA_ADDIU(tar, src, val))
+#define MIPS32_ADDU(isa, dst, src, tar)		(isa ? MMIPS32_ADDU(dst, src, tar) : MIPS32_ISA_ADDU(dst, src, tar))
+#define MIPS32_AND(isa, dst, src, tar)		(isa ? MMIPS32_AND(dst, src, tar) : MIPS32_ISA_AND(dst, src, tar))
+#define MIPS32_ANDI(isa, tar, src, val)		(isa ? MMIPS32_ANDI(tar, src, val) : MIPS32_ISA_ANDI(tar, src, val))
+
+#define MIPS32_B(isa, off)			(isa ? MMIPS32_B(off) : MIPS32_ISA_B(off))
+#define MIPS32_BEQ(isa, src, tar, off)		(isa ? MMIPS32_BEQ(src, tar, off) : MIPS32_ISA_BEQ(src, tar, off))
+#define MIPS32_BGTZ(isa, reg, off)		(isa ? MMIPS32_BGTZ(reg, off) : MIPS32_ISA_BGTZ(reg, off))
+#define MIPS32_BNE(isa, src, tar, off)		(isa ? MMIPS32_BNE(src, tar, off) : MIPS32_ISA_BNE(src, tar, off))
+#define MIPS32_CACHE(isa, op, off, base)	(isa ? MMIPS32_CACHE(op, off, base) : MIPS32_ISA_CACHE(op, off, base))
+
+#define MIPS32_J(isa, tar)			(isa ? MMIPS32_J(tar) : MIPS32_ISA_J(tar))
+#define MIPS32_JR(isa, reg)			(isa ? MMIPS32_JR(reg) : MIPS32_ISA_JR(reg))
+#define MIPS32_LB(isa, reg, off, base)		(isa ? MMIPS32_LB(reg, off, base) : MIPS32_ISA_LB(reg, off, base))
+#define MIPS32_LBU(isa, reg, off, base)		(isa ? MMIPS32_LBU(reg, off, base) : MIPS32_ISA_LBU(reg, off, base))
+#define MIPS32_LHU(isa, reg, off, base)		(isa ? MMIPS32_LHU(reg, off, base) : MIPS32_ISA_LHU(reg, off, base))
+#define MIPS32_LW(isa, reg, off, base)		(isa ? MMIPS32_LW(reg, off, base) : MIPS32_ISA_LW(reg, off, base))
+#define MIPS32_LUI(isa, reg, val)		(isa ? MMIPS32_LUI(reg, val) : MIPS32_ISA_LUI(reg, val))
+
+#define MIPS32_MFC0(isa, gpr, cpr, sel)		(isa ? MMIPS32_MFC0(gpr, cpr, sel) : MIPS32_ISA_MFC0(gpr, cpr, sel))
+#define MIPS32_MTC0(isa, gpr, cpr, sel)		(isa ? MMIPS32_MTC0(gpr, cpr, sel) : MIPS32_ISA_MTC0(gpr, cpr, sel))
+#define MIPS32_MFLO(isa, reg)			(isa ? MMIPS32_MFLO(reg) : MIPS32_ISA_MFLO(reg))
+#define MIPS32_MFHI(isa, reg)			(isa ? MMIPS32_MFHI(reg) : MIPS32_ISA_MFHI(reg))
+#define MIPS32_MTLO(isa, reg)			(isa ? MMIPS32_MTLO(reg) : MIPS32_ISA_MTLO(reg))
+#define MIPS32_MTHI(isa, reg)			(isa ? MMIPS32_MTHI(reg) : MIPS32_ISA_MTHI(reg))
+
+#define MIPS32_MOVN(isa, dst, src, tar)		(isa ? MMIPS32_MOVN(dst, src, tar) : MIPS32_ISA_MOVN(dst, src, tar))
+#define MIPS32_ORI(isa, tar, src, val)		(isa ? MMIPS32_ORI(tar, src, val) : MIPS32_ISA_ORI(tar, src, val))
+#define MIPS32_RDHWR(isa, tar, dst)		(isa ? MMIPS32_RDHWR(tar, dst) : MIPS32_ISA_RDHWR(tar, dst))
+#define MIPS32_SB(isa, reg, off, base)		(isa ? MMIPS32_SB(reg, off, base) : MIPS32_ISA_SB(reg, off, base))
+#define MIPS32_SH(isa, reg, off, base)		(isa ? MMIPS32_SH(reg, off, base) : MIPS32_ISA_SH(reg, off, base))
+#define MIPS32_SW(isa, reg, off, base)		(isa ? MMIPS32_SW(reg, off, base) : MIPS32_ISA_SW(reg, off, base))
+
+#define MIPS32_SLL(isa, dst, src, sa)		(isa ? MMIPS32_SLL(dst, src, sa) : MIPS32_ISA_SLL(dst, src, sa))
+#define MIPS32_SLTI(isa, tar, src, val)		(isa ? MMIPS32_SLTI(tar, src, val) : MIPS32_ISA_SLTI(tar, src, val))
+#define MIPS32_SLTU(isa, dst, src, tar)		(isa ? MMIPS32_SLTU(dst, src, tar) : MIPS32_ISA_SLTU(dst, src, tar))
+#define MIPS32_SRL(isa, reg, src, off)		(isa ? MMIPS32_SRL(reg, src, off) : MIPS32_ISA_SRL(reg, src, off))
+
+#define MIPS32_SYNCI(isa, off, base)		(isa ? MMIPS32_SYNCI(off, base) : MIPS32_ISA_SYNCI(off, base))
+#define MIPS32_SYNC(isa)			(isa ? MMIPS32_SYNC : MIPS32_ISA_SYNC)
+#define MIPS32_XOR(isa, reg, val1, val2)	(isa ? MMIPS32_XOR(reg, val1, val2) : MIPS32_ISA_XOR(reg, val1, val2))
+#define MIPS32_XORI(isa, tar, src, val)		(isa ? MMIPS32_XORI(tar, src, val) : MIPS32_ISA_XORI(tar, src, val))
+
+#define MIPS32_SYNCI_STEP			0x1
+
+/* ejtag specific instructions */
+#define MIPS32_DRET(isa)			(isa ? MMIPS32_DRET : MIPS32_ISA_DRET)
+#define MIPS32_SDBBP(isa)			(isa ? MMIPS32_SDBBP : MIPS32_ISA_SDBBP)
+
+#define MIPS16_SDBBP(isa)			(isa ? MMIPS16_SDBBP : MIPS16_ISA_SDBBP)
 
 extern const struct command_registration mips32_command_handlers[];
 
@@ -230,7 +409,7 @@ struct reg_cache *mips32_build_reg_cache(struct target *target);
 int mips32_run_algorithm(struct target *target,
 		int num_mem_params, struct mem_param *mem_params,
 		int num_reg_params, struct reg_param *reg_params,
-		uint32_t entry_point, uint32_t exit_point,
+		target_addr_t entry_point, target_addr_t exit_point,
 		int timeout_ms, void *arch_info);
 
 int mips32_configure_break_unit(struct target *target);
@@ -239,14 +418,16 @@ int mips32_enable_interrupts(struct target *target, int enable);
 
 int mips32_examine(struct target *target);
 
+int mips32_read_config_regs(struct target *target);
+
 int mips32_register_commands(struct command_context *cmd_ctx);
 
 int mips32_get_gdb_reg_list(struct target *target,
 		struct reg **reg_list[], int *reg_list_size,
 		enum target_register_class reg_class);
-int mips32_checksum_memory(struct target *target, uint32_t address,
+int mips32_checksum_memory(struct target *target, target_addr_t address,
 		uint32_t count, uint32_t *checksum);
 int mips32_blank_check_memory(struct target *target,
-		uint32_t address, uint32_t count, uint32_t *blank);
+		target_addr_t address, uint32_t count, uint32_t *blank, uint8_t erased_value);
 
 #endif /* OPENOCD_TARGET_MIPS32_H */
diff --git a/src/target/mips32_pracc.c b/src/target/mips32_pracc.c
index 7cc0424..790c8dc 100644
--- a/src/target/mips32_pracc.c
+++ b/src/target/mips32_pracc.c
@@ -73,31 +73,20 @@
 #include "mips32.h"
 #include "mips32_pracc.h"
 
-struct mips32_pracc_context {
-	uint32_t *local_oparam;
-	int num_oparam;
-	const uint32_t *code;
-	int code_len;
-	uint32_t stack[32];
-	int stack_offset;
-	struct mips_ejtag *ejtag_info;
-};
-
-static int wait_for_pracc_rw(struct mips_ejtag *ejtag_info, uint32_t *ctrl)
+static int wait_for_pracc_rw(struct mips_ejtag *ejtag_info)
 {
-	uint32_t ejtag_ctrl;
 	int64_t then = timeval_ms();
 
 	/* wait for the PrAcc to become "1" */
 	mips_ejtag_set_instr(ejtag_info, EJTAG_INST_CONTROL);
 
 	while (1) {
-		ejtag_ctrl = ejtag_info->ejtag_ctrl;
-		int retval = mips_ejtag_drscan_32(ejtag_info, &ejtag_ctrl);
+		ejtag_info->pa_ctrl = ejtag_info->ejtag_ctrl;
+		int retval = mips_ejtag_drscan_32(ejtag_info, &ejtag_info->pa_ctrl);
 		if (retval != ERROR_OK)
 			return retval;
 
-		if (ejtag_ctrl & EJTAG_CTRL_PRACC)
+		if (ejtag_info->pa_ctrl & EJTAG_CTRL_PRACC)
 			break;
 
 		int64_t timeout = timeval_ms() - then;
@@ -107,43 +96,38 @@ static int wait_for_pracc_rw(struct mips_ejtag *ejtag_info, uint32_t *ctrl)
 		}
 	}
 
-	*ctrl = ejtag_ctrl;
 	return ERROR_OK;
 }
 
 /* Shift in control and address for a new processor access, save them in ejtag_info */
 static int mips32_pracc_read_ctrl_addr(struct mips_ejtag *ejtag_info)
 {
-	int retval = wait_for_pracc_rw(ejtag_info, &ejtag_info->pa_ctrl);
+	int retval = wait_for_pracc_rw(ejtag_info);
 	if (retval != ERROR_OK)
 		return retval;
 
 	mips_ejtag_set_instr(ejtag_info, EJTAG_INST_ADDRESS);
-	ejtag_info->pa_addr = 0;
-	retval = mips_ejtag_drscan_32(ejtag_info, &ejtag_info->pa_addr);
 
-	return retval;
+	ejtag_info->pa_addr = 0;
+	return  mips_ejtag_drscan_32(ejtag_info, &ejtag_info->pa_addr);
 }
 
 /* Finish processor access */
-static int mips32_pracc_finish(struct mips_ejtag *ejtag_info)
+static void mips32_pracc_finish(struct mips_ejtag *ejtag_info)
 {
 	uint32_t ctrl = ejtag_info->ejtag_ctrl & ~EJTAG_CTRL_PRACC;
 	mips_ejtag_set_instr(ejtag_info, EJTAG_INST_CONTROL);
 	mips_ejtag_drscan_32_out(ejtag_info, ctrl);
-
-	return jtag_execute_queue();
 }
 
 int mips32_pracc_clean_text_jump(struct mips_ejtag *ejtag_info)
 {
-	uint32_t jt_code = MIPS32_J((0x0FFFFFFF & MIPS32_PRACC_TEXT) >> 2);
-	int retval;
-
+	uint32_t jt_code = MIPS32_J(ejtag_info->isa, MIPS32_PRACC_TEXT);
+	pracc_swap16_array(ejtag_info, &jt_code, 1);
 	/* do 3 0/nops to clean pipeline before a jump to pracc text, NOP in delay slot */
 	for (int i = 0; i != 5; i++) {
 		/* Wait for pracc */
-		retval = wait_for_pracc_rw(ejtag_info, &ejtag_info->pa_ctrl);
+		int retval = wait_for_pracc_rw(ejtag_info);
 		if (retval != ERROR_OK)
 			return retval;
 
@@ -153,25 +137,21 @@ int mips32_pracc_clean_text_jump(struct mips_ejtag *ejtag_info)
 		mips_ejtag_drscan_32_out(ejtag_info, data);
 
 		/* finish pa */
-		retval = mips32_pracc_finish(ejtag_info);
-		if (retval != ERROR_OK)
-			return retval;
+		mips32_pracc_finish(ejtag_info);
 	}
 
 	if (ejtag_info->mode != 0)	/* async mode support only for MIPS ... */
 		return ERROR_OK;
 
 	for (int i = 0; i != 2; i++) {
-		retval = mips32_pracc_read_ctrl_addr(ejtag_info);
+		int retval = mips32_pracc_read_ctrl_addr(ejtag_info);
 		if (retval != ERROR_OK)
 			return retval;
 
-		if (ejtag_info->pa_addr != MIPS32_PRACC_TEXT) {		/* LEXRA/BMIPS ?, shift out another NOP, max 2 */
+		if (ejtag_info->pa_addr != MIPS32_PRACC_TEXT) {	/* LEXRA/BMIPS ?, shift out another NOP, max 2 */
 			mips_ejtag_set_instr(ejtag_info, EJTAG_INST_DATA);
 			mips_ejtag_drscan_32_out(ejtag_info, MIPS32_NOP);
-			retval = mips32_pracc_finish(ejtag_info);
-			if (retval != ERROR_OK)
-				return retval;
+			mips32_pracc_finish(ejtag_info);
 		} else
 			break;
 	}
@@ -179,10 +159,11 @@ int mips32_pracc_clean_text_jump(struct mips_ejtag *ejtag_info)
 	return ERROR_OK;
 }
 
-int mips32_pracc_exec(struct mips_ejtag *ejtag_info, struct pracc_queue_info *ctx, uint32_t *param_out)
+int mips32_pracc_exec(struct mips_ejtag *ejtag_info, struct pracc_queue_info *ctx,
+					uint32_t *param_out, bool check_last)
 {
 	int code_count = 0;
-	int store_pending = 0;		/* increases with every store instruction at dmseg, decreases with every store pa */
+	int store_pending = 0;		/* increases with every store instr at dmseg, decreases with every store pa */
 	uint32_t max_store_addr = 0;	/* for store pa address testing */
 	bool restart = 0;		/* restarting control */
 	int restart_count = 0;
@@ -205,12 +186,12 @@ int mips32_pracc_exec(struct mips_ejtag *ejtag_info, struct pracc_queue_info *ct
 			LOG_DEBUG("restarting code");
 		}
 
-		retval = mips32_pracc_read_ctrl_addr(ejtag_info);		/* update current pa info: control and address */
+		retval = mips32_pracc_read_ctrl_addr(ejtag_info); /* update current pa info: control and address */
 		if (retval != ERROR_OK)
 			return retval;
 
 		/* Check for read or write access */
-		if (ejtag_info->pa_ctrl & EJTAG_CTRL_PRNW) {						/* write/store access */
+		if (ejtag_info->pa_ctrl & EJTAG_CTRL_PRNW) {				/* write/store access */
 			/* Check for pending store from a previous store instruction at dmseg */
 			if (store_pending == 0) {
 				LOG_DEBUG("unexpected write at address %" PRIx32, ejtag_info->pa_addr);
@@ -221,8 +202,8 @@ int mips32_pracc_exec(struct mips_ejtag *ejtag_info, struct pracc_queue_info *ct
 					return ERROR_JTAG_DEVICE_ERROR;
 			} else {
 				/* check address */
-				if (ejtag_info->pa_addr < MIPS32_PRACC_PARAM_OUT || ejtag_info->pa_addr > max_store_addr) {
-
+				if (ejtag_info->pa_addr < MIPS32_PRACC_PARAM_OUT ||
+						ejtag_info->pa_addr > max_store_addr) {
 					LOG_DEBUG("writing at unexpected address %" PRIx32, ejtag_info->pa_addr);
 					return ERROR_JTAG_DEVICE_ERROR;
 				}
@@ -246,7 +227,8 @@ int mips32_pracc_exec(struct mips_ejtag *ejtag_info, struct pracc_queue_info *ct
 							ejtag_info->pa_addr, MIPS32_PRACC_TEXT + code_count * 4);
 
 					/* restart code execution only in some cases */
-					if (code_count == 1 && ejtag_info->pa_addr == MIPS32_PRACC_TEXT && restart_count == 0) {
+					if (code_count == 1 && ejtag_info->pa_addr == MIPS32_PRACC_TEXT &&
+										restart_count == 0) {
 						LOG_DEBUG("restarting, without clean jump");
 						restart_count++;
 						code_count = 0;
@@ -255,18 +237,17 @@ int mips32_pracc_exec(struct mips_ejtag *ejtag_info, struct pracc_queue_info *ct
 						restart = 1;
 						continue;
 					}
-
 					return ERROR_JTAG_DEVICE_ERROR;
 				}
 				/* check for store instruction at dmseg */
-				uint32_t store_addr = ctx->pracc_list[ctx->max_code + code_count];
+				uint32_t store_addr = ctx->pracc_list[code_count].addr;
 				if (store_addr != 0) {
 					if (store_addr > max_store_addr)
 						max_store_addr = store_addr;
 					store_pending++;
 				}
 
-				instr = ctx->pracc_list[code_count++];
+				instr = ctx->pracc_list[code_count++].instr;
 				if (code_count == ctx->code_count)	/* last instruction, start final check */
 					final_check = 1;
 
@@ -284,13 +265,14 @@ int mips32_pracc_exec(struct mips_ejtag *ejtag_info, struct pracc_queue_info *ct
 					}
 				} else {
 					if (ejtag_info->pa_addr != (MIPS32_PRACC_TEXT + code_count * 4)) {
-						LOG_DEBUG("unexpected read address in final check: %" PRIx32 ", expected: %x",
-							  ejtag_info->pa_addr, MIPS32_PRACC_TEXT + code_count * 4);
+						LOG_DEBUG("unexpected read address in final check: %"
+							PRIx32 ", expected: %x", ejtag_info->pa_addr,
+							MIPS32_PRACC_TEXT + code_count * 4);
 						return ERROR_JTAG_DEVICE_ERROR;
 					}
 				}
 				if (!pass) {
-					if ((code_count - ctx->code_count) > 1) {	 /* allow max 2 instruction delay slot */
+					if ((code_count - ctx->code_count) > 1) { /* allow max 2 instr delay slot */
 						LOG_DEBUG("failed to jump back to pracc text");
 						return ERROR_JTAG_DEVICE_ERROR;
 					}
@@ -308,12 +290,10 @@ int mips32_pracc_exec(struct mips_ejtag *ejtag_info, struct pracc_queue_info *ct
 			mips_ejtag_drscan_32_out(ejtag_info, instr);
 		}
 		/* finish processor access, let the processor eat! */
-		retval = mips32_pracc_finish(ejtag_info);
-		if (retval != ERROR_OK)
-			return retval;
+		mips32_pracc_finish(ejtag_info);
 
-		if (instr == MIPS32_DRET)	/* after leaving debug mode nothing to do */
-			return ERROR_OK;
+		if (final_check && !check_last)			/* last instr, don't check, execute and exit */
+			return jtag_execute_queue();
 
 		if (store_pending == 0 && pass) {	/* store access done, but after passing pracc text */
 			LOG_DEBUG("warning: store access pass pracc text");
@@ -327,34 +307,63 @@ inline void pracc_queue_init(struct pracc_queue_info *ctx)
 	ctx->retval = ERROR_OK;
 	ctx->code_count = 0;
 	ctx->store_count = 0;
-
-	ctx->pracc_list = malloc(2 * ctx->max_code * sizeof(uint32_t));
-	if (ctx->pracc_list == NULL) {
-		LOG_ERROR("Out of memory");
-		ctx->retval = ERROR_FAIL;
-	}
+	ctx->max_code = 0;
+	ctx->pracc_list = NULL;
+	ctx->isa = ctx->ejtag_info->isa ? 1 : 0;
 }
 
-inline void pracc_add(struct pracc_queue_info *ctx, uint32_t addr, uint32_t instr)
+void pracc_add(struct pracc_queue_info *ctx, uint32_t addr, uint32_t instr)
 {
-	ctx->pracc_list[ctx->max_code + ctx->code_count] = addr;
-	ctx->pracc_list[ctx->code_count++] = instr;
+	if (ctx->retval != ERROR_OK)	/* On previous out of memory, return */
+		return;
+	if (ctx->code_count == ctx->max_code) {
+		void *p = realloc(ctx->pracc_list, sizeof(pa_list) * (ctx->max_code + PRACC_BLOCK));
+		if (p) {
+			ctx->max_code += PRACC_BLOCK;
+			ctx->pracc_list = p;
+		} else {
+			ctx->retval = ERROR_FAIL;	/* Out of memory */
+			return;
+		}
+	}
+	ctx->pracc_list[ctx->code_count].instr = instr;
+	ctx->pracc_list[ctx->code_count++].addr = addr;
 	if (addr)
 		ctx->store_count++;
 }
 
+void pracc_add_li32(struct pracc_queue_info *ctx, uint32_t reg_num, uint32_t data, bool optimize)
+{
+	if (LOWER16(data) == 0 && optimize)
+		pracc_add(ctx, 0, MIPS32_LUI(ctx->isa, reg_num, UPPER16(data)));	/* load only upper value */
+	else if (UPPER16(data) == 0 && optimize)
+		pracc_add(ctx, 0, MIPS32_ORI(ctx->isa, reg_num, 0, LOWER16(data)));	/* load only lower */
+	else {
+		pracc_add(ctx, 0, MIPS32_LUI(ctx->isa, reg_num, UPPER16(data)));	/* load upper and lower */
+		pracc_add(ctx, 0, MIPS32_ORI(ctx->isa, reg_num, reg_num, LOWER16(data)));
+	}
+}
+
 inline void pracc_queue_free(struct pracc_queue_info *ctx)
 {
-	if (ctx->code_count > ctx->max_code)	/* Only for internal check, will be erased */
-		LOG_ERROR("Internal error, code count: %d > max code: %d", ctx->code_count, ctx->max_code);
 	if (ctx->pracc_list != NULL)
 		free(ctx->pracc_list);
 }
 
-int mips32_pracc_queue_exec(struct mips_ejtag *ejtag_info, struct pracc_queue_info *ctx, uint32_t *buf)
+int mips32_pracc_queue_exec(struct mips_ejtag *ejtag_info, struct pracc_queue_info *ctx,
+					uint32_t *buf, bool check_last)
 {
+	if (ctx->retval != ERROR_OK) {
+		LOG_ERROR("Out of memory");
+		return ERROR_FAIL;
+	}
+
+	if (ejtag_info->isa && ejtag_info->endianness)
+		for (int i = 0; i != ctx->code_count; i++)
+			ctx->pracc_list[i].instr = SWAP16(ctx->pracc_list[i].instr);
+
 	if (ejtag_info->mode == 0)
-		return mips32_pracc_exec(ejtag_info, ctx, buf);
+		return mips32_pracc_exec(ejtag_info, ctx, buf, check_last);
 
 	union scan_in {
 		uint8_t scan_96[12];
@@ -377,16 +386,16 @@ int mips32_pracc_queue_exec(struct mips_ejtag *ejtag_info, struct pracc_queue_in
 	mips_ejtag_set_instr(ejtag_info, EJTAG_INST_ALL);
 
 	int scan_count = 0;
-	for (int i = 0; i != 2 * ctx->code_count; i++) {
-		uint32_t data = 0;
-		if (i & 1u) {			/* Check store address from previous instruction, if not the first */
-			if (i < 2 || 0 == ctx->pracc_list[ctx->max_code + (i / 2) - 1])
-				continue;
-		} else
-			data = ctx->pracc_list[i / 2];
-
+	for (int i = 0; i != ctx->code_count; i++) {
 		jtag_add_clocks(num_clocks);
-		mips_ejtag_add_scan_96(ejtag_info, ejtag_ctrl, data, scan_in[scan_count++].scan_96);
+		mips_ejtag_add_scan_96(ejtag_info, ejtag_ctrl, ctx->pracc_list[i].instr,
+				       scan_in[scan_count++].scan_96);
+
+		/* Check store address from previous instruction, if not the first */
+		if (i > 0 && ctx->pracc_list[i - 1].addr) {
+			jtag_add_clocks(num_clocks);
+			mips_ejtag_add_scan_96(ejtag_info, ejtag_ctrl, 0, scan_in[scan_count++].scan_96);
+		}
 	}
 
 	int retval = jtag_execute_queue();		/* execute queued scans */
@@ -395,24 +404,35 @@ int mips32_pracc_queue_exec(struct mips_ejtag *ejtag_info, struct pracc_queue_in
 
 	uint32_t fetch_addr = MIPS32_PRACC_TEXT;		/* start address */
 	scan_count = 0;
-	for (int i = 0; i != 2 * ctx->code_count; i++) {				/* verify every pracc access */
-		uint32_t store_addr = 0;
-		if (i & 1u) {			/* Read store addres from previous instruction, if not the first */
-			store_addr = ctx->pracc_list[ctx->max_code + (i / 2) - 1];
-			if (i < 2 || 0 == store_addr)
-				continue;
-		}
-
+	for (int i = 0; i != ctx->code_count; i++) {				/* verify every pracc access */
+		/* check pracc bit */
 		ejtag_ctrl = buf_get_u32(scan_in[scan_count].scan_32.ctrl, 0, 32);
+		uint32_t addr = buf_get_u32(scan_in[scan_count].scan_32.addr, 0, 32);
 		if (!(ejtag_ctrl & EJTAG_CTRL_PRACC)) {
 			LOG_ERROR("Error: access not pending  count: %d", scan_count);
 			retval = ERROR_FAIL;
 			goto exit;
 		}
+		if (ejtag_ctrl & EJTAG_CTRL_PRNW) {
+			LOG_ERROR("Not a fetch/read access, count: %d", scan_count);
+			retval = ERROR_FAIL;
+			goto exit;
+		}
+		if (addr != fetch_addr) {
+			LOG_ERROR("Fetch addr mismatch, read: %" PRIx32 " expected: %" PRIx32 " count: %d",
+					  addr, fetch_addr, scan_count);
+			retval = ERROR_FAIL;
+			goto exit;
+		}
+		fetch_addr += 4;
+		scan_count++;
 
-		uint32_t addr = buf_get_u32(scan_in[scan_count].scan_32.addr, 0, 32);
+		/* check if previous intrucction is a store instruction at dmesg */
+		if (i > 0 && ctx->pracc_list[i - 1].addr) {
+			uint32_t store_addr = ctx->pracc_list[i - 1].addr;
+			ejtag_ctrl = buf_get_u32(scan_in[scan_count].scan_32.ctrl, 0, 32);
+			addr = buf_get_u32(scan_in[scan_count].scan_32.addr, 0, 32);
 
-		if (store_addr != 0) {
 			if (!(ejtag_ctrl & EJTAG_CTRL_PRNW)) {
 				LOG_ERROR("Not a store/write access, count: %d", scan_count);
 				retval = ERROR_FAIL;
@@ -420,28 +440,14 @@ int mips32_pracc_queue_exec(struct mips_ejtag *ejtag_info, struct pracc_queue_in
 			}
 			if (addr != store_addr) {
 				LOG_ERROR("Store address mismatch, read: %" PRIx32 " expected: %" PRIx32 " count: %d",
-						addr, store_addr, scan_count);
+							      addr, store_addr, scan_count);
 				retval = ERROR_FAIL;
 				goto exit;
 			}
 			int buf_index = (addr - MIPS32_PRACC_PARAM_OUT) / 4;
 			buf[buf_index] = buf_get_u32(scan_in[scan_count].scan_32.data, 0, 32);
-
-		} else {
-			if (ejtag_ctrl & EJTAG_CTRL_PRNW) {
-				LOG_ERROR("Not a fetch/read access, count: %d", scan_count);
-				retval = ERROR_FAIL;
-				goto exit;
-			}
-			if (addr != fetch_addr) {
-				LOG_ERROR("Fetch addr mismatch, read: %" PRIx32 " expected: %" PRIx32 " count: %d",
-					  addr, fetch_addr, scan_count);
-				retval = ERROR_FAIL;
-				goto exit;
-			}
-			fetch_addr += 4;
+			scan_count++;
 		}
-		scan_count++;
 	}
 exit:
 	free(scan_in);
@@ -450,23 +456,19 @@ exit:
 
 int mips32_pracc_read_u32(struct mips_ejtag *ejtag_info, uint32_t addr, uint32_t *buf)
 {
-	struct pracc_queue_info ctx = {.max_code = 8};
+	struct pracc_queue_info ctx = {.ejtag_info = ejtag_info};
 	pracc_queue_init(&ctx);
-	if (ctx.retval != ERROR_OK)
-		goto exit;
 
-	pracc_add(&ctx, 0, MIPS32_LUI(15, PRACC_UPPER_BASE_ADDR));			/* $15 = MIPS32_PRACC_BASE_ADDR */
-	pracc_add(&ctx, 0, MIPS32_LUI(8, UPPER16((addr + 0x8000))));		/* load  $8 with modified upper address */
-	pracc_add(&ctx, 0, MIPS32_LW(8, LOWER16(addr), 8));				/* lw $8, LOWER16(addr)($8) */
+	pracc_add(&ctx, 0, MIPS32_LUI(ctx.isa, 15, PRACC_UPPER_BASE_ADDR));	/* $15 = MIPS32_PRACC_BASE_ADDR */
+	pracc_add(&ctx, 0, MIPS32_LUI(ctx.isa, 8, UPPER16((addr + 0x8000)))); /* load  $8 with modified upper addr */
+	pracc_add(&ctx, 0, MIPS32_LW(ctx.isa, 8, LOWER16(addr), 8));			/* lw $8, LOWER16(addr)($8) */
 	pracc_add(&ctx, MIPS32_PRACC_PARAM_OUT,
-				MIPS32_SW(8, PRACC_OUT_OFFSET, 15));			/* sw $8,PRACC_OUT_OFFSET($15) */
-	pracc_add(&ctx, 0, MIPS32_LUI(8, UPPER16(ejtag_info->reg8)));		/* restore upper 16 of $8 */
-	pracc_add(&ctx, 0, MIPS32_ORI(8, 8, LOWER16(ejtag_info->reg8)));		/* restore lower 16 of $8 */
-	pracc_add(&ctx, 0, MIPS32_B(NEG16(ctx.code_count + 1)));					/* jump to start */
-	pracc_add(&ctx, 0, MIPS32_MFC0(15, 31, 0));					/* move COP0 DeSave to $15 */
+				MIPS32_SW(ctx.isa, 8, PRACC_OUT_OFFSET, 15));	/* sw $8,PRACC_OUT_OFFSET($15) */
+	pracc_add_li32(&ctx, 8, ejtag_info->reg8, 0);				/* restore $8 */
+	pracc_add(&ctx, 0, MIPS32_B(ctx.isa, NEG16((ctx.code_count + 1) << ctx.isa)));		/* jump to start */
+	pracc_add(&ctx, 0, MIPS32_MFC0(ctx.isa, 15, 31, 0));				/* move COP0 DeSave to $15 */
 
-	ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, buf);
-exit:
+	ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, buf, 1);
 	pracc_queue_free(&ctx);
 	return ctx.retval;
 }
@@ -476,12 +478,10 @@ int mips32_pracc_read_mem(struct mips_ejtag *ejtag_info, uint32_t addr, int size
 	if (count == 1 && size == 4)
 		return mips32_pracc_read_u32(ejtag_info, addr, (uint32_t *)buf);
 
-	uint32_t *data = NULL;
-	struct pracc_queue_info ctx = {.max_code = 256 * 3 + 8 + 1};	/* alloc memory for the worst case */
+	struct pracc_queue_info ctx = {.ejtag_info = ejtag_info};
 	pracc_queue_init(&ctx);
-	if (ctx.retval != ERROR_OK)
-		goto exit;
 
+	uint32_t *data = NULL;
 	if (size != 4) {
 		data = malloc(256 * sizeof(uint32_t));
 		if (data == NULL) {
@@ -497,45 +497,44 @@ int mips32_pracc_read_mem(struct mips_ejtag *ejtag_info, uint32_t addr, int size
 	while (count) {
 		ctx.code_count = 0;
 		ctx.store_count = 0;
+
 		int this_round_count = (count > 256) ? 256 : count;
 		uint32_t last_upper_base_addr = UPPER16((addr + 0x8000));
 
-		pracc_add(&ctx, 0, MIPS32_LUI(15, PRACC_UPPER_BASE_ADDR));			/* $15 = MIPS32_PRACC_BASE_ADDR */
-		pracc_add(&ctx, 0, MIPS32_LUI(9, last_upper_base_addr));		/* load the upper memory address in $9 */
+		pracc_add(&ctx, 0, MIPS32_LUI(ctx.isa, 15, PRACC_UPPER_BASE_ADDR)); /* $15 = MIPS32_PRACC_BASE_ADDR */
+		pracc_add(&ctx, 0, MIPS32_LUI(ctx.isa, 9, last_upper_base_addr));	/* upper memory addr to $9 */
 
 		for (int i = 0; i != this_round_count; i++) {			/* Main code loop */
 			uint32_t upper_base_addr = UPPER16((addr + 0x8000));
-			if (last_upper_base_addr != upper_base_addr) {			/* if needed, change upper address in $9 */
-				pracc_add(&ctx, 0, MIPS32_LUI(9, upper_base_addr));
+			if (last_upper_base_addr != upper_base_addr) {	/* if needed, change upper addr in $9 */
+				pracc_add(&ctx, 0, MIPS32_LUI(ctx.isa, 9, upper_base_addr));
 				last_upper_base_addr = upper_base_addr;
 			}
 
-			if (size == 4)
-				pracc_add(&ctx, 0, MIPS32_LW(8, LOWER16(addr), 9));		/* load from memory to $8 */
+			if (size == 4)				/* load from memory to $8 */
+				pracc_add(&ctx, 0, MIPS32_LW(ctx.isa, 8, LOWER16(addr), 9));
 			else if (size == 2)
-				pracc_add(&ctx, 0, MIPS32_LHU(8, LOWER16(addr), 9));
+				pracc_add(&ctx, 0, MIPS32_LHU(ctx.isa, 8, LOWER16(addr), 9));
 			else
-				pracc_add(&ctx, 0, MIPS32_LBU(8, LOWER16(addr), 9));
+				pracc_add(&ctx, 0, MIPS32_LBU(ctx.isa, 8, LOWER16(addr), 9));
 
-			pracc_add(&ctx, MIPS32_PRACC_PARAM_OUT + i * 4,
-					  MIPS32_SW(8, PRACC_OUT_OFFSET + i * 4, 15));		/* store $8 at param out */
+			pracc_add(&ctx, MIPS32_PRACC_PARAM_OUT + i * 4,			/* store $8 at param out */
+					  MIPS32_SW(ctx.isa, 8, PRACC_OUT_OFFSET + i * 4, 15));
 			addr += size;
 		}
-		pracc_add(&ctx, 0, MIPS32_LUI(8, UPPER16(ejtag_info->reg8)));		/* restore upper 16 bits of reg 8 */
-		pracc_add(&ctx, 0, MIPS32_ORI(8, 8, LOWER16(ejtag_info->reg8)));	/* restore lower 16 bits of reg 8 */
-		pracc_add(&ctx, 0, MIPS32_LUI(9, UPPER16(ejtag_info->reg9)));		/* restore upper 16 bits of reg 9 */
-		pracc_add(&ctx, 0, MIPS32_ORI(9, 9, LOWER16(ejtag_info->reg9)));	/* restore lower 16 bits of reg 9 */
+		pracc_add_li32(&ctx, 8, ejtag_info->reg8, 0);				/* restore $8 */
+		pracc_add_li32(&ctx, 9, ejtag_info->reg9, 0);				/* restore $9 */
 
-		pracc_add(&ctx, 0, MIPS32_B(NEG16(ctx.code_count + 1)));				/* jump to start */
-		pracc_add(&ctx, 0, MIPS32_MFC0(15, 31, 0));					/* restore $15 from DeSave */
+		pracc_add(&ctx, 0, MIPS32_B(ctx.isa, NEG16((ctx.code_count + 1) << ctx.isa)));	/* jump to start */
+		pracc_add(&ctx, 0, MIPS32_MFC0(ctx.isa, 15, 31, 0));			/* restore $15 from DeSave */
 
 		if (size == 4) {
-			ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, buf32);
+			ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, buf32, 1);
 			if (ctx.retval != ERROR_OK)
 				goto exit;
 			buf32 += this_round_count;
 		} else {
-			ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, data);
+			ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, data, 1);
 			if (ctx.retval != ERROR_OK)
 				goto exit;
 
@@ -558,68 +557,37 @@ exit:
 
 int mips32_cp0_read(struct mips_ejtag *ejtag_info, uint32_t *val, uint32_t cp0_reg, uint32_t cp0_sel)
 {
-	struct pracc_queue_info ctx = {.max_code = 7};
+	struct pracc_queue_info ctx = {.ejtag_info = ejtag_info};
 	pracc_queue_init(&ctx);
-	if (ctx.retval != ERROR_OK)
-		goto exit;
 
-	pracc_add(&ctx, 0, MIPS32_LUI(15, PRACC_UPPER_BASE_ADDR));			/* $15 = MIPS32_PRACC_BASE_ADDR */
-	pracc_add(&ctx, 0, MIPS32_MFC0(8, 0, 0) | (cp0_reg << 11) | cp0_sel);	/* move COP0 [cp0_reg select] to $8 */
+	pracc_add(&ctx, 0, MIPS32_LUI(ctx.isa, 15, PRACC_UPPER_BASE_ADDR));	/* $15 = MIPS32_PRACC_BASE_ADDR */
+	pracc_add(&ctx, 0, MIPS32_MFC0(ctx.isa, 8, cp0_reg, cp0_sel));		/* move cp0 reg / sel to $8 */
 	pracc_add(&ctx, MIPS32_PRACC_PARAM_OUT,
-				MIPS32_SW(8, PRACC_OUT_OFFSET, 15));			/* store $8 to pracc_out */
-	pracc_add(&ctx, 0, MIPS32_MFC0(15, 31, 0));					/* move COP0 DeSave to $15 */
-	pracc_add(&ctx, 0, MIPS32_LUI(8, UPPER16(ejtag_info->reg8)));		/* restore upper 16 bits  of $8 */
-	pracc_add(&ctx, 0, MIPS32_B(NEG16(ctx.code_count + 1)));					/* jump to start */
-	pracc_add(&ctx, 0, MIPS32_ORI(8, 8, LOWER16(ejtag_info->reg8)));		/* restore lower 16 bits of $8 */
+				MIPS32_SW(ctx.isa, 8, PRACC_OUT_OFFSET, 15));	/* store $8 to pracc_out */
+	pracc_add(&ctx, 0, MIPS32_MFC0(ctx.isa, 15, 31, 0));				/* restore $15 from DeSave */
+	pracc_add(&ctx, 0, MIPS32_LUI(ctx.isa, 8, UPPER16(ejtag_info->reg8)));	/* restore upper 16 bits  of $8 */
+	pracc_add(&ctx, 0, MIPS32_B(ctx.isa, NEG16((ctx.code_count + 1) << ctx.isa)));		/* jump to start */
+	pracc_add(&ctx, 0, MIPS32_ORI(ctx.isa, 8, 8, LOWER16(ejtag_info->reg8))); /* restore lower 16 bits of $8 */
 
-	ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, val);
-exit:
+	ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, val, 1);
 	pracc_queue_free(&ctx);
 	return ctx.retval;
-
-	/**
-	 * Note that our input parametes cp0_reg and cp0_sel
-	 * are numbers (not gprs) which make part of mfc0 instruction opcode.
-	 *
-	 * These are not fix, but can be different for each mips32_cp0_read() function call,
-	 * and that is why we must insert them directly into opcode,
-	 * i.e. we can not pass it on EJTAG microprogram stack (via param_in),
-	 * and put them into the gprs later from MIPS32_PRACC_STACK
-	 * because mfc0 do not use gpr as a parameter for the cp0_reg and select part,
-	 * but plain (immediate) number.
-	 *
-	 * MIPS32_MTC0 is implemented via MIPS32_R_INST macro.
-	 * In order to insert our parameters, we must change rd and funct fields.
-	 *
-	 * code[2] |= (cp0_reg << 11) | cp0_sel;   change rd and funct of MIPS32_R_INST macro
-	 **/
 }
 
 int mips32_cp0_write(struct mips_ejtag *ejtag_info, uint32_t val, uint32_t cp0_reg, uint32_t cp0_sel)
 {
-	struct pracc_queue_info ctx = {.max_code = 6};
+	struct pracc_queue_info ctx = {.ejtag_info = ejtag_info};
 	pracc_queue_init(&ctx);
-	if (ctx.retval != ERROR_OK)
-		goto exit;
-
-	pracc_add(&ctx, 0, MIPS32_LUI(15, UPPER16(val)));				/* Load val to $15 */
-	pracc_add(&ctx, 0, MIPS32_ORI(15, 15, LOWER16(val)));
 
-	pracc_add(&ctx, 0, MIPS32_MTC0(15, 0, 0) | (cp0_reg << 11) | cp0_sel);	/* write cp0 reg / sel */
+	pracc_add_li32(&ctx, 15, val, 0);				/* Load val to $15 */
 
-	pracc_add(&ctx, 0, MIPS32_B(NEG16(ctx.code_count + 1)));					/* jump to start */
-	pracc_add(&ctx, 0, MIPS32_MFC0(15, 31, 0));					/* move COP0 DeSave to $15 */
+	pracc_add(&ctx, 0, MIPS32_MTC0(ctx.isa, 15, cp0_reg, cp0_sel));		/* write $15 to cp0 reg / sel */
+	pracc_add(&ctx, 0, MIPS32_B(ctx.isa, NEG16((ctx.code_count + 1) << ctx.isa)));		/* jump to start */
+	pracc_add(&ctx, 0, MIPS32_MFC0(ctx.isa, 15, 31, 0));			/* restore $15 from DeSave */
 
-	ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, NULL);
-exit:
+	ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, NULL, 1);
 	pracc_queue_free(&ctx);
 	return ctx.retval;
-
-	/**
-	 * Note that MIPS32_MTC0 macro is implemented via MIPS32_R_INST macro.
-	 * In order to insert our parameters, we must change rd and funct fields.
-	 * code[3] |= (cp0_reg << 11) | cp0_sel;   change rd and funct fields of MIPS32_R_INST macro
-	 **/
 }
 
 /**
@@ -652,26 +620,25 @@ exit:
 static int mips32_pracc_synchronize_cache(struct mips_ejtag *ejtag_info,
 					 uint32_t start_addr, uint32_t end_addr, int cached, int rel)
 {
-	struct pracc_queue_info ctx = {.max_code = 256 * 2 + 5};
+	struct pracc_queue_info ctx = {.ejtag_info = ejtag_info};
 	pracc_queue_init(&ctx);
-	if (ctx.retval != ERROR_OK)
-		goto exit;
+
 	/** Find cache line size in bytes */
 	uint32_t clsiz;
 	if (rel) {	/* Release 2 (rel = 1) */
-		pracc_add(&ctx, 0, MIPS32_LUI(15, PRACC_UPPER_BASE_ADDR));			/* $15 = MIPS32_PRACC_BASE_ADDR */
+		pracc_add(&ctx, 0, MIPS32_LUI(ctx.isa, 15, PRACC_UPPER_BASE_ADDR)); /* $15 = MIPS32_PRACC_BASE_ADDR */
 
-		pracc_add(&ctx, 0, MIPS32_RDHWR(8, MIPS32_SYNCI_STEP));			/* load synci_step value to $8 */
+		pracc_add(&ctx, 0, MIPS32_RDHWR(ctx.isa, 8, MIPS32_SYNCI_STEP)); /* load synci_step value to $8 */
 
 		pracc_add(&ctx, MIPS32_PRACC_PARAM_OUT,
-				MIPS32_SW(8, PRACC_OUT_OFFSET, 15));			/* store $8 to pracc_out */
+				MIPS32_SW(ctx.isa, 8, PRACC_OUT_OFFSET, 15));		/* store $8 to pracc_out */
 
-		pracc_add(&ctx, 0, MIPS32_LUI(8, UPPER16(ejtag_info->reg8)));			/* restore upper 16 bits  of $8 */
-		pracc_add(&ctx, 0, MIPS32_ORI(8, 8, LOWER16(ejtag_info->reg8)));		/* restore lower 16 bits of $8 */
-		pracc_add(&ctx, 0, MIPS32_B(NEG16(ctx.code_count + 1)));					/* jump to start */
-		pracc_add(&ctx, 0, MIPS32_MFC0(15, 31, 0));					/* move COP0 DeSave to $15 */
+		pracc_add_li32(&ctx, 8, ejtag_info->reg8, 0);				/* restore $8 */
 
-		ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, &clsiz);
+		pracc_add(&ctx, 0, MIPS32_B(ctx.isa, NEG16((ctx.code_count + 1) << ctx.isa)));	/* jump to start */
+		pracc_add(&ctx, 0, MIPS32_MFC0(ctx.isa, 15, 31, 0));			/* restore $15 from DeSave */
+
+		ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, &clsiz, 1);
 		if (ctx.retval != ERROR_OK)
 			goto exit;
 
@@ -704,47 +671,50 @@ static int mips32_pracc_synchronize_cache(struct mips_ejtag *ejtag_info,
 	end_addr |= clsiz - 1;
 
 	ctx.code_count = 0;
+	ctx.store_count = 0;
+
 	int count = 0;
 	uint32_t last_upper_base_addr = UPPER16((start_addr + 0x8000));
 
-	pracc_add(&ctx, 0, MIPS32_LUI(15, last_upper_base_addr));		/* load upper memory base address to $15 */
+	pracc_add(&ctx, 0, MIPS32_LUI(ctx.isa, 15, last_upper_base_addr)); /* load upper memory base addr to $15 */
 
 	while (start_addr <= end_addr) {						/* main loop */
 		uint32_t upper_base_addr = UPPER16((start_addr + 0x8000));
-		if (last_upper_base_addr != upper_base_addr) {				/* if needed, change upper address in $15 */
-			pracc_add(&ctx, 0, MIPS32_LUI(15, upper_base_addr));
+		if (last_upper_base_addr != upper_base_addr) {		/* if needed, change upper addr in $15 */
+			pracc_add(&ctx, 0, MIPS32_LUI(ctx.isa, 15, upper_base_addr));
 			last_upper_base_addr = upper_base_addr;
 		}
-		if (rel)
-			pracc_add(&ctx, 0, MIPS32_SYNCI(LOWER16(start_addr), 15));		/* synci instruction, offset($15) */
+		if (rel)			/* synci instruction, offset($15) */
+			pracc_add(&ctx, 0, MIPS32_SYNCI(ctx.isa, LOWER16(start_addr), 15));
 
 		else {
-			if (cached == 3)
-				pracc_add(&ctx, 0, MIPS32_CACHE(MIPS32_CACHE_D_HIT_WRITEBACK,
-							LOWER16(start_addr), 15));		/* cache Hit_Writeback_D, offset($15) */
-
-			pracc_add(&ctx, 0, MIPS32_CACHE(MIPS32_CACHE_I_HIT_INVALIDATE,
-							LOWER16(start_addr), 15));		/* cache Hit_Invalidate_I, offset($15) */
+			if (cached == 3)	/* cache Hit_Writeback_D, offset($15) */
+				pracc_add(&ctx, 0, MIPS32_CACHE(ctx.isa, MIPS32_CACHE_D_HIT_WRITEBACK,
+							LOWER16(start_addr), 15));
+			/* cache Hit_Invalidate_I, offset($15) */
+			pracc_add(&ctx, 0, MIPS32_CACHE(ctx.isa, MIPS32_CACHE_I_HIT_INVALIDATE,
+							LOWER16(start_addr), 15));
 		}
 		start_addr += clsiz;
 		count++;
-		if (count == 256 && start_addr <= end_addr) {				/* more ?, then execute code list */
-			pracc_add(&ctx, 0, MIPS32_B(NEG16(ctx.code_count + 1)));		/* jump to start */
-			pracc_add(&ctx, 0, MIPS32_NOP);						/* nop in delay slot */
+		if (count == 256 && start_addr <= end_addr) {			/* more ?, then execute code list */
+			pracc_add(&ctx, 0, MIPS32_B(ctx.isa, NEG16((ctx.code_count + 1) << ctx.isa)));	/* to start */
+			pracc_add(&ctx, 0, MIPS32_NOP);					/* nop in delay slot */
 
-			ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, NULL);
+			ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, NULL, 1);
 			if (ctx.retval != ERROR_OK)
 				goto exit;
 
-			ctx.code_count = 0;
+			ctx.code_count = 0;	/* reset counters for another loop */
+			ctx.store_count = 0;
 			count = 0;
 		}
 	}
-	pracc_add(&ctx, 0, MIPS32_SYNC);
-	pracc_add(&ctx, 0, MIPS32_B(NEG16(ctx.code_count + 1)));					/* jump to start */
-	pracc_add(&ctx, 0, MIPS32_MFC0(15, 31, 0));					/* restore $15 from DeSave*/
+	pracc_add(&ctx, 0, MIPS32_SYNC(ctx.isa));
+	pracc_add(&ctx, 0, MIPS32_B(ctx.isa, NEG16((ctx.code_count + 1) << ctx.isa)));		/* jump to start */
+	pracc_add(&ctx, 0, MIPS32_MFC0(ctx.isa, 15, 31, 0));				/* restore $15 from DeSave*/
 
-	ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, NULL);
+	ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, NULL, 1);
 exit:
 	pracc_queue_free(&ctx);
 	return ctx.retval;
@@ -753,10 +723,8 @@ exit:
 static int mips32_pracc_write_mem_generic(struct mips_ejtag *ejtag_info,
 		uint32_t addr, int size, int count, const void *buf)
 {
-	struct pracc_queue_info ctx = {.max_code = 128 * 3 + 5 + 1};	/* alloc memory for the worst case */
+	struct pracc_queue_info ctx = {.ejtag_info = ejtag_info};
 	pracc_queue_init(&ctx);
-	if (ctx.retval != ERROR_OK)
-		goto exit;
 
 	const uint32_t *buf32 = buf;
 	const uint16_t *buf16 = buf;
@@ -765,50 +733,43 @@ static int mips32_pracc_write_mem_generic(struct mips_ejtag *ejtag_info,
 	while (count) {
 		ctx.code_count = 0;
 		ctx.store_count = 0;
+
 		int this_round_count = (count > 128) ? 128 : count;
 		uint32_t last_upper_base_addr = UPPER16((addr + 0x8000));
-
-		pracc_add(&ctx, 0, MIPS32_LUI(15, last_upper_base_addr));		/* load $15 with memory base address */
+			      /* load $15 with memory base address */
+		pracc_add(&ctx, 0, MIPS32_LUI(ctx.isa, 15, last_upper_base_addr));
 
 		for (int i = 0; i != this_round_count; i++) {
 			uint32_t upper_base_addr = UPPER16((addr + 0x8000));
-			if (last_upper_base_addr != upper_base_addr) {
-				pracc_add(&ctx, 0, MIPS32_LUI(15, upper_base_addr));	/* if needed, change upper address in $15*/
+			if (last_upper_base_addr != upper_base_addr) {	/* if needed, change upper address in $15*/
+				pracc_add(&ctx, 0, MIPS32_LUI(ctx.isa, 15, upper_base_addr));
 				last_upper_base_addr = upper_base_addr;
 			}
 
-			if (size == 4) {			/* for word writes check if one half word is 0 and load it accordingly */
-				if (LOWER16(*buf32) == 0)
-					pracc_add(&ctx, 0, MIPS32_LUI(8, UPPER16(*buf32)));		/* load only upper value */
-				else if (UPPER16(*buf32) == 0)
-						pracc_add(&ctx, 0, MIPS32_ORI(8, 0, LOWER16(*buf32)));	/* load only lower */
-				else {
-					pracc_add(&ctx, 0, MIPS32_LUI(8, UPPER16(*buf32)));		/* load upper and lower */
-					pracc_add(&ctx, 0, MIPS32_ORI(8, 8, LOWER16(*buf32)));
-				}
-				pracc_add(&ctx, 0, MIPS32_SW(8, LOWER16(addr), 15));		/* store word to memory */
+			if (size == 4) {
+				pracc_add_li32(&ctx, 8, *buf32, 1);		/* load with li32, optimize */
+				pracc_add(&ctx, 0, MIPS32_SW(ctx.isa, 8, LOWER16(addr), 15)); /* store word to mem */
 				buf32++;
 
 			} else if (size == 2) {
-				pracc_add(&ctx, 0, MIPS32_ORI(8, 0, *buf16));		/* load lower value */
-				pracc_add(&ctx, 0, MIPS32_SH(8, LOWER16(addr), 15));	/* store half word to memory */
+				pracc_add(&ctx, 0, MIPS32_ORI(ctx.isa, 8, 0, *buf16));		/* load lower value */
+				pracc_add(&ctx, 0, MIPS32_SH(ctx.isa, 8, LOWER16(addr), 15)); /* store half word */
 				buf16++;
 
 			} else {
-				pracc_add(&ctx, 0, MIPS32_ORI(8, 0, *buf8));		/* load lower value */
-				pracc_add(&ctx, 0, MIPS32_SB(8, LOWER16(addr), 15));	/* store byte to memory */
+				pracc_add(&ctx, 0, MIPS32_ORI(ctx.isa, 8, 0, *buf8));		/* load lower value */
+				pracc_add(&ctx, 0, MIPS32_SB(ctx.isa, 8, LOWER16(addr), 15));	/* store byte */
 				buf8++;
 			}
 			addr += size;
 		}
 
-		pracc_add(&ctx, 0, MIPS32_LUI(8, UPPER16(ejtag_info->reg8)));		/* restore upper 16 bits of reg 8 */
-		pracc_add(&ctx, 0, MIPS32_ORI(8, 8, LOWER16(ejtag_info->reg8)));	/* restore lower 16 bits of reg 8 */
+		pracc_add_li32(&ctx, 8, ejtag_info->reg8, 0);				/* restore $8 */
 
-		pracc_add(&ctx, 0, MIPS32_B(NEG16(ctx.code_count + 1)));				/* jump to start */
-		pracc_add(&ctx, 0, MIPS32_MFC0(15, 31, 0));				/* restore $15 from DeSave */
+		pracc_add(&ctx, 0, MIPS32_B(ctx.isa, NEG16((ctx.code_count + 1) << ctx.isa)));	/* jump to start */
+		pracc_add(&ctx, 0, MIPS32_MFC0(ctx.isa, 15, 31, 0));			/* restore $15 from DeSave */
 
-		ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, NULL);
+		ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, NULL, 1);
 		if (ctx.retval != ERROR_OK)
 			goto exit;
 		count -= this_round_count;
@@ -875,95 +836,77 @@ int mips32_pracc_write_mem(struct mips_ejtag *ejtag_info, uint32_t addr, int siz
 
 int mips32_pracc_write_regs(struct mips_ejtag *ejtag_info, uint32_t *regs)
 {
-	static const uint32_t cp0_write_code[] = {
-		MIPS32_MTC0(1, 12, 0),							/* move $1 to status */
-		MIPS32_MTLO(1),									/* move $1 to lo */
-		MIPS32_MTHI(1),									/* move $1 to hi */
-		MIPS32_MTC0(1, 8, 0),							/* move $1 to badvaddr */
-		MIPS32_MTC0(1, 13, 0),							/* move $1 to cause*/
-		MIPS32_MTC0(1, 24, 0),							/* move $1 to depc (pc) */
-	};
-
-	struct pracc_queue_info ctx = {.max_code = 37 * 2 + 7 + 1};
+	struct pracc_queue_info ctx = {.ejtag_info = ejtag_info};
 	pracc_queue_init(&ctx);
-	if (ctx.retval != ERROR_OK)
-		goto exit;
 
-	/* load registers 2 to 31 with lui and ori instructions, check if some instructions can be saved */
-	for (int i = 2; i < 32; i++) {
-		if (LOWER16((regs[i])) == 0)					/* if lower half word is 0, lui instruction only */
-			pracc_add(&ctx, 0, MIPS32_LUI(i, UPPER16((regs[i]))));
-		else if (UPPER16((regs[i])) == 0)					/* if upper half word is 0, ori with $0 only*/
-			pracc_add(&ctx, 0, MIPS32_ORI(i, 0, LOWER16((regs[i]))));
-		else {									/* default, load with lui and ori instructions */
-			pracc_add(&ctx, 0, MIPS32_LUI(i, UPPER16((regs[i]))));
-			pracc_add(&ctx, 0, MIPS32_ORI(i, i, LOWER16((regs[i]))));
-		}
-	}
+	uint32_t cp0_write_code[] = {
+		MIPS32_MTC0(ctx.isa, 1, 12, 0),					/* move $1 to status */
+		MIPS32_MTLO(ctx.isa, 1),						/* move $1 to lo */
+		MIPS32_MTHI(ctx.isa, 1),						/* move $1 to hi */
+		MIPS32_MTC0(ctx.isa, 1, 8, 0),					/* move $1 to badvaddr */
+		MIPS32_MTC0(ctx.isa, 1, 13, 0),					/* move $1 to cause*/
+		MIPS32_MTC0(ctx.isa, 1, 24, 0),					/* move $1 to depc (pc) */
+	};
+
+	/* load registers 2 to 31 with li32, optimize */
+	for (int i = 2; i < 32; i++)
+		pracc_add_li32(&ctx, i, regs[i], 1);
 
 	for (int i = 0; i != 6; i++) {
-		pracc_add(&ctx, 0, MIPS32_LUI(1, UPPER16((regs[i + 32]))));		/* load CPO value in $1, with lui and ori */
-		pracc_add(&ctx, 0, MIPS32_ORI(1, 1, LOWER16((regs[i + 32]))));
-		pracc_add(&ctx, 0, cp0_write_code[i]);					/* write value from $1 to CPO register */
+		pracc_add_li32(&ctx, 1, regs[i + 32], 0);	/* load CPO value in $1 */
+		pracc_add(&ctx, 0, cp0_write_code[i]);			/* write value from $1 to CPO register */
 	}
-	pracc_add(&ctx, 0, MIPS32_MTC0(15, 31, 0));				/* load $15 in DeSave */
-	pracc_add(&ctx, 0, MIPS32_LUI(1, UPPER16((regs[1]))));			/* load upper half word in $1 */
-	pracc_add(&ctx, 0, MIPS32_B(NEG16(ctx.code_count + 1)));					/* jump to start */
-	pracc_add(&ctx, 0, MIPS32_ORI(1, 1, LOWER16((regs[1]))));		/* load lower half word in $1 */
+	pracc_add(&ctx, 0, MIPS32_MTC0(ctx.isa, 15, 31, 0));				/* load $15 in DeSave */
+	pracc_add(&ctx, 0, MIPS32_LUI(ctx.isa, 1, UPPER16((regs[1]))));		/* load upper half word in $1 */
+	pracc_add(&ctx, 0, MIPS32_B(ctx.isa, NEG16((ctx.code_count + 1) << ctx.isa)));		/* jump to start */
+	pracc_add(&ctx, 0, MIPS32_ORI(ctx.isa, 1, 1, LOWER16((regs[1]))));	/* load lower half word in $1 */
 
-	ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, NULL);
+	ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, NULL, 1);
 
 	ejtag_info->reg8 = regs[8];
 	ejtag_info->reg9 = regs[9];
-exit:
 	pracc_queue_free(&ctx);
 	return ctx.retval;
 }
 
 int mips32_pracc_read_regs(struct mips_ejtag *ejtag_info, uint32_t *regs)
 {
-	static int cp0_read_code[] = {
-		MIPS32_MFC0(8, 12, 0),							/* move status to $8 */
-		MIPS32_MFLO(8),									/* move lo to $8 */
-		MIPS32_MFHI(8),									/* move hi to $8 */
-		MIPS32_MFC0(8, 8, 0),							/* move badvaddr to $8 */
-		MIPS32_MFC0(8, 13, 0),							/* move cause to $8 */
-		MIPS32_MFC0(8, 24, 0),							/* move depc (pc) to $8 */
-	};
-
-	struct pracc_queue_info ctx = {.max_code = 49};
+	struct pracc_queue_info ctx = {.ejtag_info = ejtag_info};
 	pracc_queue_init(&ctx);
-	if (ctx.retval != ERROR_OK)
-		goto exit;
 
-	pracc_add(&ctx, 0, MIPS32_MTC0(1, 31, 0));						/* move $1 to COP0 DeSave */
-	pracc_add(&ctx, 0, MIPS32_LUI(1, PRACC_UPPER_BASE_ADDR));				/* $1 = MIP32_PRACC_BASE_ADDR */
+	uint32_t cp0_read_code[] = {
+		MIPS32_MFC0(ctx.isa, 8, 12, 0),					/* move status to $8 */
+		MIPS32_MFLO(ctx.isa, 8),						/* move lo to $8 */
+		MIPS32_MFHI(ctx.isa, 8),						/* move hi to $8 */
+		MIPS32_MFC0(ctx.isa, 8, 8, 0),					/* move badvaddr to $8 */
+		MIPS32_MFC0(ctx.isa, 8, 13, 0),					/* move cause to $8 */
+		MIPS32_MFC0(ctx.isa, 8, 24, 0),					/* move depc (pc) to $8 */
+	};
+
+	pracc_add(&ctx, 0, MIPS32_MTC0(ctx.isa, 1, 31, 0));				/* move $1 to COP0 DeSave */
+	pracc_add(&ctx, 0, MIPS32_LUI(ctx.isa, 1, PRACC_UPPER_BASE_ADDR));	/* $1 = MIP32_PRACC_BASE_ADDR */
 
 	for (int i = 2; i != 32; i++)					/* store GPR's 2 to 31 */
 		pracc_add(&ctx, MIPS32_PRACC_PARAM_OUT + (i * 4),
-				  MIPS32_SW(i, PRACC_OUT_OFFSET + (i * 4), 1));
+				  MIPS32_SW(ctx.isa, i, PRACC_OUT_OFFSET + (i * 4), 1));
 
 	for (int i = 0; i != 6; i++) {
 		pracc_add(&ctx, 0, cp0_read_code[i]);				/* load COP0 needed registers to $8 */
 		pracc_add(&ctx, MIPS32_PRACC_PARAM_OUT + (i + 32) * 4,			/* store $8 at PARAM OUT */
-				  MIPS32_SW(8, PRACC_OUT_OFFSET + (i + 32) * 4, 1));
+				  MIPS32_SW(ctx.isa, 8, PRACC_OUT_OFFSET + (i + 32) * 4, 1));
 	}
-	pracc_add(&ctx, 0, MIPS32_MFC0(8, 31, 0));					/* move DeSave to $8, reg1 value */
-	pracc_add(&ctx, MIPS32_PRACC_PARAM_OUT + 4,					/* store reg1 value from $8 to param out */
-			  MIPS32_SW(8, PRACC_OUT_OFFSET + 4, 1));
-
-	pracc_add(&ctx, 0, MIPS32_MFC0(1, 31, 0));					/* move COP0 DeSave to $1, restore reg1 */
-	pracc_add(&ctx, 0, MIPS32_B(NEG16(ctx.code_count + 1)));					/* jump to start */
-	pracc_add(&ctx, 0, MIPS32_MTC0(15, 31, 0));					/* load $15 in DeSave */
+	pracc_add(&ctx, 0, MIPS32_MFC0(ctx.isa, 8, 31, 0));			/* move DeSave to $8, reg1 value */
+	pracc_add(&ctx, MIPS32_PRACC_PARAM_OUT + 4,			/* store reg1 value from $8 to param out */
+			  MIPS32_SW(ctx.isa, 8, PRACC_OUT_OFFSET + 4, 1));
 
-	if (ejtag_info->mode == 0)
-		ctx.store_count++;	/* Needed by legacy code, due to offset from reg0 */
+	pracc_add(&ctx, 0, MIPS32_MFC0(ctx.isa, 1, 31, 0));		/* move COP0 DeSave to $1, restore reg1 */
+	pracc_add(&ctx, 0, MIPS32_B(ctx.isa, NEG16((ctx.code_count + 1) << ctx.isa)));		/* jump to start */
+	pracc_add(&ctx, 0, MIPS32_MTC0(ctx.isa, 15, 31, 0));				/* load $15 in DeSave */
 
-	ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, regs);
+	ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, regs, 1);
 
 	ejtag_info->reg8 = regs[8];	/* reg8 is saved but not restored, next called function should restore it */
 	ejtag_info->reg9 = regs[9];
-exit:
 	pracc_queue_free(&ctx);
 	return ctx.retval;
 }
@@ -978,70 +921,61 @@ exit:
 int mips32_pracc_fastdata_xfer(struct mips_ejtag *ejtag_info, struct working_area *source,
 		int write_t, uint32_t addr, int count, uint32_t *buf)
 {
+	uint32_t isa = ejtag_info->isa ? 1 : 0;
 	uint32_t handler_code[] = {
-		/* caution when editing, table is modified below */
 		/* r15 points to the start of this code */
-		MIPS32_SW(8, MIPS32_FASTDATA_HANDLER_SIZE - 4, 15),
-		MIPS32_SW(9, MIPS32_FASTDATA_HANDLER_SIZE - 8, 15),
-		MIPS32_SW(10, MIPS32_FASTDATA_HANDLER_SIZE - 12, 15),
-		MIPS32_SW(11, MIPS32_FASTDATA_HANDLER_SIZE - 16, 15),
+		MIPS32_SW(isa, 8, MIPS32_FASTDATA_HANDLER_SIZE - 4, 15),
+		MIPS32_SW(isa, 9, MIPS32_FASTDATA_HANDLER_SIZE - 8, 15),
+		MIPS32_SW(isa, 10, MIPS32_FASTDATA_HANDLER_SIZE - 12, 15),
+		MIPS32_SW(isa, 11, MIPS32_FASTDATA_HANDLER_SIZE - 16, 15),
 		/* start of fastdata area in t0 */
-		MIPS32_LUI(8, UPPER16(MIPS32_PRACC_FASTDATA_AREA)),
-		MIPS32_ORI(8, 8, LOWER16(MIPS32_PRACC_FASTDATA_AREA)),
-		MIPS32_LW(9, 0, 8),								/* start addr in t1 */
-		MIPS32_LW(10, 0, 8),							/* end addr to t2 */
-														/* loop: */
-		/* 8 */ MIPS32_LW(11, 0, 0),					/* lw t3,[t8 | r9] */
-		/* 9 */ MIPS32_SW(11, 0, 0),					/* sw t3,[r9 | r8] */
-		MIPS32_BNE(10, 9, NEG16(3)),					/* bne $t2,t1,loop */
-		MIPS32_ADDI(9, 9, 4),							/* addi t1,t1,4 */
-
-		MIPS32_LW(8, MIPS32_FASTDATA_HANDLER_SIZE - 4, 15),
-		MIPS32_LW(9, MIPS32_FASTDATA_HANDLER_SIZE - 8, 15),
-		MIPS32_LW(10, MIPS32_FASTDATA_HANDLER_SIZE - 12, 15),
-		MIPS32_LW(11, MIPS32_FASTDATA_HANDLER_SIZE - 16, 15),
-
-		MIPS32_LUI(15, UPPER16(MIPS32_PRACC_TEXT)),
-		MIPS32_ORI(15, 15, LOWER16(MIPS32_PRACC_TEXT)),
-		MIPS32_JR(15),								/* jr start */
-		MIPS32_MFC0(15, 31, 0),						/* move COP0 DeSave to $15 */
+		MIPS32_LUI(isa, 8, UPPER16(MIPS32_PRACC_FASTDATA_AREA)),
+		MIPS32_ORI(isa, 8, 8, LOWER16(MIPS32_PRACC_FASTDATA_AREA)),
+		MIPS32_LW(isa, 9, 0, 8),						/* start addr in t1 */
+		MIPS32_LW(isa, 10, 0, 8),						/* end addr to t2 */
+					/* loop: */
+		write_t ? MIPS32_LW(isa, 11, 0, 8) : MIPS32_LW(isa, 11, 0, 9),	/* from xfer area : from memory */
+		write_t ? MIPS32_SW(isa, 11, 0, 9) : MIPS32_SW(isa, 11, 0, 8),	/* to memory      : to xfer area */
+
+		MIPS32_BNE(isa, 10, 9, NEG16(3 << isa)),			/* bne $t2,t1,loop */
+		MIPS32_ADDI(isa, 9, 9, 4),					/* addi t1,t1,4 */
+
+		MIPS32_LW(isa, 8, MIPS32_FASTDATA_HANDLER_SIZE - 4, 15),
+		MIPS32_LW(isa, 9, MIPS32_FASTDATA_HANDLER_SIZE - 8, 15),
+		MIPS32_LW(isa, 10, MIPS32_FASTDATA_HANDLER_SIZE - 12, 15),
+		MIPS32_LW(isa, 11, MIPS32_FASTDATA_HANDLER_SIZE - 16, 15),
+
+		MIPS32_LUI(isa, 15, UPPER16(MIPS32_PRACC_TEXT)),
+		MIPS32_ORI(isa, 15, 15, LOWER16(MIPS32_PRACC_TEXT) | isa),	/* isa bit for JR instr */
+		MIPS32_JR(isa, 15),								/* jr start */
+		MIPS32_MFC0(isa, 15, 31, 0),					/* move COP0 DeSave to $15 */
 	};
 
-	uint32_t jmp_code[] = {
-		/* 0 */ MIPS32_LUI(15, 0),		/* addr of working area added below */
-		/* 1 */ MIPS32_ORI(15, 15, 0),	/* addr of working area added below */
-		MIPS32_JR(15),					/* jump to ram program */
-		MIPS32_NOP,
-	};
-
-	int retval, i;
-	uint32_t val, ejtag_ctrl, address;
-
 	if (source->size < MIPS32_FASTDATA_HANDLER_SIZE)
 		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 
-	if (write_t) {
-		handler_code[8] = MIPS32_LW(11, 0, 8);	/* load data from probe at fastdata area */
-		handler_code[9] = MIPS32_SW(11, 0, 9);	/* store data to RAM @ r9 */
-	} else {
-		handler_code[8] = MIPS32_LW(11, 0, 9);	/* load data from RAM @ r9 */
-		handler_code[9] = MIPS32_SW(11, 0, 8);	/* store data to probe at fastdata area */
-	}
-
-	/* write program into RAM */
+	pracc_swap16_array(ejtag_info, handler_code, ARRAY_SIZE(handler_code));
+		/* write program into RAM */
 	if (write_t != ejtag_info->fast_access_save) {
 		mips32_pracc_write_mem(ejtag_info, source->address, 4, ARRAY_SIZE(handler_code), handler_code);
 		/* save previous operation to speed to any consecutive read/writes */
 		ejtag_info->fast_access_save = write_t;
 	}
 
-	LOG_DEBUG("%s using 0x%.8" PRIx32 " for write handler", __func__, source->address);
+	LOG_DEBUG("%s using 0x%.8" TARGET_PRIxADDR " for write handler", __func__, source->address);
+
+	uint32_t jmp_code[] = {
+		MIPS32_LUI(isa, 15, UPPER16(source->address)),			/* load addr of jump in $15 */
+		MIPS32_ORI(isa, 15, 15, LOWER16(source->address) | isa),	/* isa bit for JR instr */
+		MIPS32_JR(isa, 15),						/* jump to ram program */
+		isa ? MIPS32_XORI(isa, 15, 15, 1) : MIPS32_NOP,	/* drop isa bit, needed for LW/SW instructions */
+	};
 
-	jmp_code[0] |= UPPER16(source->address);
-	jmp_code[1] |= LOWER16(source->address);
+	pracc_swap16_array(ejtag_info, jmp_code, ARRAY_SIZE(jmp_code));
 
-	for (i = 0; i < (int) ARRAY_SIZE(jmp_code); i++) {
-		retval = wait_for_pracc_rw(ejtag_info, &ejtag_ctrl);
+	/* execute jump code, with no address check */
+	for (unsigned i = 0; i < ARRAY_SIZE(jmp_code); i++) {
+		int retval = wait_for_pracc_rw(ejtag_info);
 		if (retval != ERROR_OK)
 			return retval;
 
@@ -1049,32 +983,24 @@ int mips32_pracc_fastdata_xfer(struct mips_ejtag *ejtag_info, struct working_are
 		mips_ejtag_drscan_32_out(ejtag_info, jmp_code[i]);
 
 		/* Clear the access pending bit (let the processor eat!) */
-		ejtag_ctrl = ejtag_info->ejtag_ctrl & ~EJTAG_CTRL_PRACC;
-		mips_ejtag_set_instr(ejtag_info, EJTAG_INST_CONTROL);
-		mips_ejtag_drscan_32_out(ejtag_info, ejtag_ctrl);
+		mips32_pracc_finish(ejtag_info);
 	}
 
-	/* wait PrAcc pending bit for FASTDATA write */
-	retval = wait_for_pracc_rw(ejtag_info, &ejtag_ctrl);
+	/* wait PrAcc pending bit for FASTDATA write, read address */
+	int retval = mips32_pracc_read_ctrl_addr(ejtag_info);
 	if (retval != ERROR_OK)
 		return retval;
 
 	/* next fetch to dmseg should be in FASTDATA_AREA, check */
-	address = 0;
-	mips_ejtag_set_instr(ejtag_info, EJTAG_INST_ADDRESS);
-	retval = mips_ejtag_drscan_32(ejtag_info, &address);
-	if (retval != ERROR_OK)
-		return retval;
-
-	if (address != MIPS32_PRACC_FASTDATA_AREA)
+	if (ejtag_info->pa_addr != MIPS32_PRACC_FASTDATA_AREA)
 		return ERROR_FAIL;
 
 	/* Send the load start address */
-	val = addr;
+	uint32_t val = addr;
 	mips_ejtag_set_instr(ejtag_info, EJTAG_INST_FASTDATA);
 	mips_ejtag_fastdata_scan(ejtag_info, 1, &val);
 
-	retval = wait_for_pracc_rw(ejtag_info, &ejtag_ctrl);
+	retval = wait_for_pracc_rw(ejtag_info);
 	if (retval != ERROR_OK)
 		return retval;
 
@@ -1087,11 +1013,9 @@ int mips32_pracc_fastdata_xfer(struct mips_ejtag *ejtag_info, struct working_are
 	if (ejtag_info->mode != 0)
 		num_clocks = ((uint64_t)(ejtag_info->scan_delay) * jtag_get_speed_khz() + 500000) / 1000000;
 
-	for (i = 0; i < count; i++) {
+	for (int i = 0; i < count; i++) {
 		jtag_add_clocks(num_clocks);
-		retval = mips_ejtag_fastdata_scan(ejtag_info, write_t, buf++);
-		if (retval != ERROR_OK)
-			return retval;
+		mips_ejtag_fastdata_scan(ejtag_info, write_t, buf++);
 	}
 
 	retval = jtag_execute_queue();
@@ -1100,17 +1024,11 @@ int mips32_pracc_fastdata_xfer(struct mips_ejtag *ejtag_info, struct working_are
 		return retval;
 	}
 
-	retval = wait_for_pracc_rw(ejtag_info, &ejtag_ctrl);
-	if (retval != ERROR_OK)
-		return retval;
-
-	address = 0;
-	mips_ejtag_set_instr(ejtag_info, EJTAG_INST_ADDRESS);
-	retval = mips_ejtag_drscan_32(ejtag_info, &address);
+	retval = mips32_pracc_read_ctrl_addr(ejtag_info);
 	if (retval != ERROR_OK)
 		return retval;
 
-	if (address != MIPS32_PRACC_TEXT)
+	if (ejtag_info->pa_addr != MIPS32_PRACC_TEXT)
 		LOG_ERROR("mini program did not return to start");
 
 	return retval;
diff --git a/src/target/mips32_pracc.h b/src/target/mips32_pracc.h
index 2ede5b2..b8b93c6 100644
--- a/src/target/mips32_pracc.h
+++ b/src/target/mips32_pracc.h
@@ -34,26 +34,40 @@
 #define MIPS32_PRACC_PARAM_OUT			0xFF202000
 
 #define PRACC_UPPER_BASE_ADDR			(MIPS32_PRACC_BASE_ADDR >> 16)
+#define PRACC_MAX_CODE				(MIPS32_PRACC_PARAM_OUT - MIPS32_PRACC_TEXT)
+#define PRACC_MAX_INSTRUCTIONS			(PRACC_MAX_CODE / 4)
 #define PRACC_OUT_OFFSET			(MIPS32_PRACC_PARAM_OUT - MIPS32_PRACC_BASE_ADDR)
 
 #define MIPS32_FASTDATA_HANDLER_SIZE	0x80
 #define UPPER16(uint32_t)				(uint32_t >> 16)
 #define LOWER16(uint32_t)				(uint32_t & 0xFFFF)
 #define NEG16(v)						(((~(v)) + 1) & 0xFFFF)
+#define SWAP16(v)				((LOWER16(v) << 16) | (UPPER16(v)))
 /*#define NEG18(v) (((~(v)) + 1) & 0x3FFFF)*/
 
+#define PRACC_BLOCK	128	/* 1 Kbyte */
+
+typedef struct {
+	uint32_t instr;
+	uint32_t addr;
+} pa_list;
+
 struct pracc_queue_info {
+	struct mips_ejtag *ejtag_info;
+	unsigned isa;
 	int retval;
-	const int max_code;
 	int code_count;
 	int store_count;
-	uint32_t *pracc_list;	/* Code and store addresses */
+	int max_code;		/* max intstructions with currently allocated memory */
+	pa_list *pracc_list;	/* Code and store addresses at dmseg */
 };
+
 void pracc_queue_init(struct pracc_queue_info *ctx);
 void pracc_add(struct pracc_queue_info *ctx, uint32_t addr, uint32_t instr);
+void pracc_add_li32(struct pracc_queue_info *ctx, uint32_t reg_num, uint32_t data, bool optimize);
 void pracc_queue_free(struct pracc_queue_info *ctx);
 int mips32_pracc_queue_exec(struct mips_ejtag *ejtag_info,
-			    struct pracc_queue_info *ctx, uint32_t *buf);
+			    struct pracc_queue_info *ctx, uint32_t *buf, bool check_last);
 
 int mips32_pracc_read_mem(struct mips_ejtag *ejtag_info,
 		uint32_t addr, int size, int count, void *buf);
@@ -65,7 +79,8 @@ int mips32_pracc_fastdata_xfer(struct mips_ejtag *ejtag_info, struct working_are
 int mips32_pracc_read_regs(struct mips_ejtag *ejtag_info, uint32_t *regs);
 int mips32_pracc_write_regs(struct mips_ejtag *ejtag_info, uint32_t *regs);
 
-int mips32_pracc_exec(struct mips_ejtag *ejtag_info, struct pracc_queue_info *ctx, uint32_t *param_out);
+int mips32_pracc_exec(struct mips_ejtag *ejtag_info, struct pracc_queue_info *ctx,
+				uint32_t *param_out, bool check_last);
 
 /**
  * \b mips32_cp0_read
@@ -99,4 +114,11 @@ int mips32_cp0_read(struct mips_ejtag *ejtag_info,
 int mips32_cp0_write(struct mips_ejtag *ejtag_info,
 		uint32_t val, uint32_t cp0_reg, uint32_t cp0_sel);
 
+inline void pracc_swap16_array(struct mips_ejtag *ejtag_info, uint32_t *buf, int count)
+{
+	if (ejtag_info->isa && ejtag_info->endianness)
+		for (int i = 0; i != count; i++)
+			buf[i] = SWAP16(buf[i]);
+}
+
 #endif /* OPENOCD_TARGET_MIPS32_PRACC_H */
diff --git a/src/target/mips_ejtag.c b/src/target/mips_ejtag.c
index 594711f..03a0952 100644
--- a/src/target/mips_ejtag.c
+++ b/src/target/mips_ejtag.c
@@ -28,74 +28,40 @@
 #include "mips_ejtag.h"
 #include "mips32_dmaacc.h"
 
-void mips_ejtag_set_instr(struct mips_ejtag *ejtag_info, int new_instr)
+void mips_ejtag_set_instr(struct mips_ejtag *ejtag_info, uint32_t new_instr)
 {
-	struct jtag_tap *tap;
+	assert(ejtag_info->tap != NULL);
+	struct jtag_tap *tap = ejtag_info->tap;
 
-	tap = ejtag_info->tap;
-	assert(tap != NULL);
+	if (buf_get_u32(tap->cur_instr, 0, tap->ir_length) != new_instr) {
 
-	if (buf_get_u32(tap->cur_instr, 0, tap->ir_length) != (uint32_t)new_instr) {
 		struct scan_field field;
-		uint8_t t[4];
-
 		field.num_bits = tap->ir_length;
+
+		uint8_t t[4];
 		field.out_value = t;
 		buf_set_u32(t, 0, field.num_bits, new_instr);
+
 		field.in_value = NULL;
 
 		jtag_add_ir_scan(tap, &field, TAP_IDLE);
 	}
 }
 
-int mips_ejtag_get_idcode(struct mips_ejtag *ejtag_info, uint32_t *idcode)
+int mips_ejtag_get_idcode(struct mips_ejtag *ejtag_info)
 {
-	struct scan_field field;
-	uint8_t r[4];
-
 	mips_ejtag_set_instr(ejtag_info, EJTAG_INST_IDCODE);
 
-	field.num_bits = 32;
-	field.out_value = NULL;
-	field.in_value = r;
-
-	jtag_add_dr_scan(ejtag_info->tap, 1, &field, TAP_IDLE);
-
-	int retval;
-	retval = jtag_execute_queue();
-	if (retval != ERROR_OK) {
-		LOG_ERROR("register read failed");
-		return retval;
-	}
-
-	*idcode = buf_get_u32(field.in_value, 0, 32);
-
-	return ERROR_OK;
+	ejtag_info->idcode = 0;
+	return mips_ejtag_drscan_32(ejtag_info, &ejtag_info->idcode);
 }
 
-static int mips_ejtag_get_impcode(struct mips_ejtag *ejtag_info, uint32_t *impcode)
+int mips_ejtag_get_impcode(struct mips_ejtag *ejtag_info)
 {
-	struct scan_field field;
-	uint8_t r[4];
-
 	mips_ejtag_set_instr(ejtag_info, EJTAG_INST_IMPCODE);
 
-	field.num_bits = 32;
-	field.out_value = NULL;
-	field.in_value = r;
-
-	jtag_add_dr_scan(ejtag_info->tap, 1, &field, TAP_IDLE);
-
-	int retval;
-	retval = jtag_execute_queue();
-	if (retval != ERROR_OK) {
-		LOG_ERROR("register read failed");
-		return retval;
-	}
-
-	*impcode = buf_get_u32(field.in_value, 0, 32);
-
-	return ERROR_OK;
+	ejtag_info->impcode = 0;
+	return mips_ejtag_drscan_32(ejtag_info, &ejtag_info->impcode);
 }
 
 void mips_ejtag_add_scan_96(struct mips_ejtag *ejtag_info, uint32_t ctrl, uint32_t data, uint8_t *in_scan_buf)
@@ -121,91 +87,73 @@ void mips_ejtag_add_scan_96(struct mips_ejtag *ejtag_info, uint32_t ctrl, uint32
 	keep_alive();
 }
 
-int mips_ejtag_drscan_32(struct mips_ejtag *ejtag_info, uint32_t *data)
+void mips_ejtag_drscan_32_queued(struct mips_ejtag *ejtag_info, uint32_t data_out, uint8_t *data_in)
 {
-	struct jtag_tap *tap;
-	tap  = ejtag_info->tap;
-	assert(tap != NULL);
+	assert(ejtag_info->tap != NULL);
+	struct jtag_tap *tap = ejtag_info->tap;
 
 	struct scan_field field;
-	uint8_t t[4], r[4];
-	int retval;
-
 	field.num_bits = 32;
-	field.out_value = t;
-	buf_set_u32(t, 0, field.num_bits, *data);
-	field.in_value = r;
 
+	uint8_t scan_out[4];
+	field.out_value = scan_out;
+	buf_set_u32(scan_out, 0, field.num_bits, data_out);
+
+	field.in_value = data_in;
 	jtag_add_dr_scan(tap, 1, &field, TAP_IDLE);
 
-	retval = jtag_execute_queue();
+	keep_alive();
+}
+
+int mips_ejtag_drscan_32(struct mips_ejtag *ejtag_info, uint32_t *data)
+{
+	uint8_t scan_in[4];
+	mips_ejtag_drscan_32_queued(ejtag_info, *data, scan_in);
+
+	int retval = jtag_execute_queue();
 	if (retval != ERROR_OK) {
 		LOG_ERROR("register read failed");
 		return retval;
 	}
 
-	*data = buf_get_u32(field.in_value, 0, 32);
-
-	keep_alive();
-
+	*data = buf_get_u32(scan_in, 0, 32);
 	return ERROR_OK;
 }
 
 void mips_ejtag_drscan_32_out(struct mips_ejtag *ejtag_info, uint32_t data)
 {
-	uint8_t t[4];
-	struct jtag_tap *tap;
-	tap  = ejtag_info->tap;
-	assert(tap != NULL);
-
-	struct scan_field field;
-
-	field.num_bits = 32;
-	field.out_value = t;
-	buf_set_u32(t, 0, field.num_bits, data);
-
-	field.in_value = NULL;
-
-	jtag_add_dr_scan(tap, 1, &field, TAP_IDLE);
+	mips_ejtag_drscan_32_queued(ejtag_info, data, NULL);
 }
 
-int mips_ejtag_drscan_8(struct mips_ejtag *ejtag_info, uint32_t *data)
+int mips_ejtag_drscan_8(struct mips_ejtag *ejtag_info, uint8_t *data)
 {
-	struct jtag_tap *tap;
-	tap  = ejtag_info->tap;
-	assert(tap != NULL);
+	assert(ejtag_info->tap != NULL);
+	struct jtag_tap *tap = ejtag_info->tap;
 
 	struct scan_field field;
-	uint8_t t[4] = {0, 0, 0, 0}, r[4];
-	int retval;
-
 	field.num_bits = 8;
-	field.out_value = t;
-	buf_set_u32(t, 0, field.num_bits, *data);
-	field.in_value = r;
+
+	field.out_value = data;
+	field.in_value = data;
 
 	jtag_add_dr_scan(tap, 1, &field, TAP_IDLE);
 
-	retval = jtag_execute_queue();
+	int retval = jtag_execute_queue();
 	if (retval != ERROR_OK) {
 		LOG_ERROR("register read failed");
 		return retval;
 	}
-
-	*data = buf_get_u32(field.in_value, 0, 32);
-
 	return ERROR_OK;
 }
 
 void mips_ejtag_drscan_8_out(struct mips_ejtag *ejtag_info, uint8_t data)
 {
-	struct jtag_tap *tap;
-	tap  = ejtag_info->tap;
-	assert(tap != NULL);
+	assert(ejtag_info->tap != NULL);
+	struct jtag_tap *tap = ejtag_info->tap;
 
 	struct scan_field field;
-
 	field.num_bits = 8;
+
 	field.out_value = &data;
 	field.in_value = NULL;
 
@@ -215,23 +163,20 @@ void mips_ejtag_drscan_8_out(struct mips_ejtag *ejtag_info, uint8_t data)
 /* Set (to enable) or clear (to disable stepping) the SSt bit (bit 8) in Cp0 Debug reg (reg 23, sel 0) */
 int mips_ejtag_config_step(struct mips_ejtag *ejtag_info, int enable_step)
 {
-	struct pracc_queue_info ctx = {.max_code = 7};
+	struct pracc_queue_info ctx = {.ejtag_info = ejtag_info};
 	pracc_queue_init(&ctx);
-	if (ctx.retval != ERROR_OK)
-		goto exit;
 
-	pracc_add(&ctx, 0, MIPS32_MFC0(8, 23, 0));			/* move COP0 Debug to $8 */
-	pracc_add(&ctx, 0, MIPS32_ORI(8, 8, 0x0100));			/* set SSt bit in debug reg */
+	pracc_add(&ctx, 0, MIPS32_MFC0(ctx.isa, 8, 23, 0));			/* move COP0 Debug to $8 */
+	pracc_add(&ctx, 0, MIPS32_ORI(ctx.isa, 8, 8, 0x0100));			/* set SSt bit in debug reg */
 	if (!enable_step)
-		pracc_add(&ctx, 0, MIPS32_XORI(8, 8, 0x0100));		/* clear SSt bit in debug reg */
+		pracc_add(&ctx, 0, MIPS32_XORI(ctx.isa, 8, 8, 0x0100));		/* clear SSt bit in debug reg */
 
-	pracc_add(&ctx, 0, MIPS32_MTC0(8, 23, 0));			/* move $8 to COP0 Debug */
-	pracc_add(&ctx, 0, MIPS32_LUI(8, UPPER16(ejtag_info->reg8)));		/* restore upper 16 bits  of $8 */
-	pracc_add(&ctx, 0, MIPS32_B(NEG16((ctx.code_count + 1))));			/* jump to start */
-	pracc_add(&ctx, 0, MIPS32_ORI(8, 8, LOWER16(ejtag_info->reg8)));	/* restore lower 16 bits of $8 */
+	pracc_add(&ctx, 0, MIPS32_MTC0(ctx.isa, 8, 23, 0));			/* move $8 to COP0 Debug */
+	pracc_add(&ctx, 0, MIPS32_LUI(ctx.isa, 8, UPPER16(ejtag_info->reg8)));	/* restore upper 16 bits  of $8 */
+	pracc_add(&ctx, 0, MIPS32_B(ctx.isa, NEG16((ctx.code_count + 1) << ctx.isa)));		/* jump to start */
+	pracc_add(&ctx, 0, MIPS32_ORI(ctx.isa, 8, 8, LOWER16(ejtag_info->reg8))); /* restore lower 16 bits of $8 */
 
-	ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, NULL);
-exit:
+	ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, NULL, 1);
 	pracc_queue_free(&ctx);
 	return ctx.retval;
 }
@@ -290,11 +235,11 @@ error:
 
 int mips_ejtag_exit_debug(struct mips_ejtag *ejtag_info)
 {
-	uint32_t pracc_list[] = {MIPS32_DRET, 0};
-	struct pracc_queue_info ctx = {.max_code = 1, .pracc_list = pracc_list, .code_count = 1, .store_count = 0};
+	pa_list pracc_list = {.instr = MIPS32_DRET(ejtag_info->isa), .addr = 0};
+	struct pracc_queue_info ctx = {.max_code = 1, .pracc_list = &pracc_list, .code_count = 1, .store_count = 0};
 
 	/* execute our dret instruction */
-	ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, NULL);
+	ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, NULL, 0); /* shift out instr, omit last check */
 
 	/* pic32mx workaround, false pending at low core clock */
 	jtag_add_sleep(1000);
@@ -389,12 +334,11 @@ static void ejtag_main_print_imp(struct mips_ejtag *ejtag_info)
 
 int mips_ejtag_init(struct mips_ejtag *ejtag_info)
 {
-	int retval;
-
-	retval = mips_ejtag_get_impcode(ejtag_info, &ejtag_info->impcode);
-	if (retval != ERROR_OK)
+	int retval = mips_ejtag_get_impcode(ejtag_info);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("impcode read failed");
 		return retval;
-	LOG_DEBUG("impcode: 0x%8.8" PRIx32 "", ejtag_info->impcode);
+	}
 
 	/* get ejtag version */
 	ejtag_info->ejtag_version = ((ejtag_info->impcode >> 29) & 0x07);
@@ -444,22 +388,22 @@ int mips_ejtag_init(struct mips_ejtag *ejtag_info)
 
 int mips_ejtag_fastdata_scan(struct mips_ejtag *ejtag_info, int write_t, uint32_t *data)
 {
-	struct jtag_tap *tap;
-
-	tap = ejtag_info->tap;
-	assert(tap != NULL);
+	assert(ejtag_info->tap != NULL);
+	struct jtag_tap *tap = ejtag_info->tap;
 
 	struct scan_field fields[2];
-	uint8_t spracc = 0;
-	uint8_t t[4] = {0, 0, 0, 0};
 
 	/* fastdata 1-bit register */
 	fields[0].num_bits = 1;
+
+	uint8_t spracc = 0;
 	fields[0].out_value = &spracc;
 	fields[0].in_value = NULL;
 
 	/* processor access data register 32 bit */
 	fields[1].num_bits = 32;
+
+	uint8_t t[4] = {0, 0, 0, 0};
 	fields[1].out_value = t;
 
 	if (write_t) {
diff --git a/src/target/mips_ejtag.h b/src/target/mips_ejtag.h
index 6ef0867..71f5c1b 100644
--- a/src/target/mips_ejtag.h
+++ b/src/target/mips_ejtag.h
@@ -58,6 +58,7 @@
 #define EJTAG_CTRL_DERR			(1 << 10)
 #define EJTAG_CTRL_DSTRT		(1 << 11)
 #define EJTAG_CTRL_JTAGBRK		(1 << 12)
+#define EJTAG_CTRL_DBGISA		(1 << 13)
 #define EJTAG_CTRL_SETDEV		(1 << 14)
 #define EJTAG_CTRL_PROBEN		(1 << 15)
 #define EJTAG_CTRL_PRRST		(1 << 16)
@@ -182,6 +183,9 @@ struct mips_ejtag {
 	uint32_t idcode;
 	uint32_t ejtag_ctrl;
 	int fast_access_save;
+	uint32_t config_regs;	/* number of config registers read */
+	uint32_t config[4];	/* cp0 config to config3 */
+
 	uint32_t reg8;
 	uint32_t reg9;
 	unsigned scan_delay;
@@ -189,6 +193,8 @@ struct mips_ejtag {
 	uint32_t pa_ctrl;
 	uint32_t pa_addr;
 	unsigned int ejtag_version;
+	uint32_t isa;
+	uint32_t endianness;
 
 	/* Memory-Mapped Registers. This addresses are not same on different
 	 * EJTAG versions. */
@@ -210,17 +216,16 @@ struct mips_ejtag {
 	uint32_t ejtag_dba_step_size;	/* size of step till next *DBAn register. */
 };
 
-void mips_ejtag_set_instr(struct mips_ejtag *ejtag_info,
-		int new_instr);
+void mips_ejtag_set_instr(struct mips_ejtag *ejtag_info, uint32_t new_instr);
 int mips_ejtag_enter_debug(struct mips_ejtag *ejtag_info);
 int mips_ejtag_exit_debug(struct mips_ejtag *ejtag_info);
-int mips_ejtag_get_idcode(struct mips_ejtag *ejtag_info, uint32_t *idcode);
+int mips_ejtag_get_idcode(struct mips_ejtag *ejtag_info);
 void mips_ejtag_add_scan_96(struct mips_ejtag *ejtag_info,
 			    uint32_t ctrl, uint32_t data, uint8_t *in_scan_buf);
 void mips_ejtag_drscan_32_out(struct mips_ejtag *ejtag_info, uint32_t data);
 int mips_ejtag_drscan_32(struct mips_ejtag *ejtag_info, uint32_t *data);
 void mips_ejtag_drscan_8_out(struct mips_ejtag *ejtag_info, uint8_t data);
-int mips_ejtag_drscan_8(struct mips_ejtag *ejtag_info, uint32_t *data);
+int mips_ejtag_drscan_8(struct mips_ejtag *ejtag_info, uint8_t *data);
 int mips_ejtag_fastdata_scan(struct mips_ejtag *ejtag_info, int write_t, uint32_t *data);
 
 int mips_ejtag_init(struct mips_ejtag *ejtag_info);
diff --git a/src/target/mips_m4k.c b/src/target/mips_m4k.c
index 0daa71c..7d1c06c 100644
--- a/src/target/mips_m4k.c
+++ b/src/target/mips_m4k.c
@@ -41,10 +41,10 @@ static int mips_m4k_set_breakpoint(struct target *target,
 static int mips_m4k_unset_breakpoint(struct target *target,
 		struct breakpoint *breakpoint);
 static int mips_m4k_internal_restore(struct target *target, int current,
-		uint32_t address, int handle_breakpoints,
+		target_addr_t address, int handle_breakpoints,
 		int debug_execution);
 static int mips_m4k_halt(struct target *target);
-static int mips_m4k_bulk_write_memory(struct target *target, uint32_t address,
+static int mips_m4k_bulk_write_memory(struct target *target, target_addr_t address,
 		uint32_t count, const uint8_t *buffer);
 
 static int mips_m4k_examine_debug_reason(struct target *target)
@@ -108,11 +108,14 @@ static int mips_m4k_debug_entry(struct target *target)
 	/* attempt to find halt reason */
 	mips_m4k_examine_debug_reason(target);
 
+	mips32_read_config_regs(target);
+
 	/* default to mips32 isa, it will be changed below if required */
 	mips32->isa_mode = MIPS32_ISA_MIPS32;
 
-	if (ejtag_info->impcode & EJTAG_IMP_MIPS16)
-		mips32->isa_mode = buf_get_u32(mips32->core_cache->reg_list[MIPS32_PC].value, 0, 1);
+	/* other than mips32 only and isa bit set ? */
+	if (mips32->isa_imp && buf_get_u32(mips32->core_cache->reg_list[MIPS32_PC].value, 0, 1))
+		mips32->isa_mode = mips32->isa_imp == 2 ? MIPS32_ISA_MIPS16E : MIPS32_ISA_MMIPS32;
 
 	LOG_DEBUG("entered debug state at PC 0x%" PRIx32 ", target->state: %s",
 			buf_get_u32(mips32->core_cache->reg_list[MIPS32_PC].value, 0, 32),
@@ -195,6 +198,8 @@ static int mips_m4k_poll(struct target *target)
 	if (retval != ERROR_OK)
 		return retval;
 
+	ejtag_info->isa = (ejtag_ctrl & EJTAG_CTRL_DBGISA) ? 1 : 0;
+
 	/* clear this bit before handling polling
 	 * as after reset registers will read zero */
 	if (ejtag_ctrl & EJTAG_CTRL_ROCC) {
@@ -429,7 +434,7 @@ static int mips_m4k_restore_smp(struct target *target, uint32_t address, int han
 }
 
 static int mips_m4k_internal_restore(struct target *target, int current,
-		uint32_t address, int handle_breakpoints, int debug_execution)
+		target_addr_t address, int handle_breakpoints, int debug_execution)
 {
 	struct mips32_common *mips32 = target_to_mips32(target);
 	struct mips_ejtag *ejtag_info = &mips32->ejtag_info;
@@ -449,12 +454,13 @@ static int mips_m4k_internal_restore(struct target *target, int current,
 
 	/* current = 1: continue on current pc, otherwise continue at <address> */
 	if (!current) {
+		mips_m4k_isa_filter(mips32->isa_imp, &address);
 		buf_set_u32(mips32->core_cache->reg_list[MIPS32_PC].value, 0, 32, address);
 		mips32->core_cache->reg_list[MIPS32_PC].dirty = 1;
 		mips32->core_cache->reg_list[MIPS32_PC].valid = 1;
 	}
 
-	if (ejtag_info->impcode & EJTAG_IMP_MIPS16)
+	if ((mips32->isa_imp > 1) &&  debug_execution)	/* if more than one isa supported */
 		buf_set_u32(mips32->core_cache->reg_list[MIPS32_PC].value, 0, 1, mips32->isa_mode);
 
 	if (!current)
@@ -469,7 +475,8 @@ static int mips_m4k_internal_restore(struct target *target, int current,
 		/* Single step past breakpoint at current address */
 		breakpoint = breakpoint_find(target, resume_pc);
 		if (breakpoint) {
-			LOG_DEBUG("unset breakpoint at 0x%8.8" PRIx32 "", breakpoint->address);
+			LOG_DEBUG("unset breakpoint at " TARGET_ADDR_FMT "",
+					  breakpoint->address);
 			mips_m4k_unset_breakpoint(target, breakpoint);
 			mips_m4k_single_step_core(target);
 			mips_m4k_set_breakpoint(target, breakpoint);
@@ -500,7 +507,7 @@ static int mips_m4k_internal_restore(struct target *target, int current,
 }
 
 static int mips_m4k_resume(struct target *target, int current,
-		uint32_t address, int handle_breakpoints, int debug_execution)
+		target_addr_t address, int handle_breakpoints, int debug_execution)
 {
 	int retval = ERROR_OK;
 
@@ -527,7 +534,7 @@ static int mips_m4k_resume(struct target *target, int current,
 }
 
 static int mips_m4k_step(struct target *target, int current,
-		uint32_t address, int handle_breakpoints)
+		target_addr_t address, int handle_breakpoints)
 {
 	/* get pointers to arch-specific information */
 	struct mips32_common *mips32 = target_to_mips32(target);
@@ -541,6 +548,7 @@ static int mips_m4k_step(struct target *target, int current,
 
 	/* current = 1: continue on current pc, otherwise continue at <address> */
 	if (!current) {
+		mips_m4k_isa_filter(mips32->isa_imp, &address);
 		buf_set_u32(mips32->core_cache->reg_list[MIPS32_PC].value, 0, 32, address);
 		mips32->core_cache->reg_list[MIPS32_PC].dirty = 1;
 		mips32->core_cache->reg_list[MIPS32_PC].valid = 1;
@@ -623,6 +631,11 @@ static int mips_m4k_set_breakpoint(struct target *target,
 		comparator_list[bp_num].used = 1;
 		comparator_list[bp_num].bp_value = breakpoint->address;
 
+		if (breakpoint->length != 4)			/* make sure isa bit set */
+			comparator_list[bp_num].bp_value |= 1;
+		else						/* make sure isa bit cleared */
+			comparator_list[bp_num].bp_value &= ~1;
+
 		/* EJTAG 2.0 uses 30bit IBA. First 2 bits are reserved.
 		 * Warning: there is no IB ASID registers in 2.0.
 		 * Do not set it! :) */
@@ -640,41 +653,77 @@ static int mips_m4k_set_breakpoint(struct target *target,
 				  bp_num, comparator_list[bp_num].bp_value);
 	} else if (breakpoint->type == BKPT_SOFT) {
 		LOG_DEBUG("bpid: %" PRIu32, breakpoint->unique_id);
-		if (breakpoint->length == 4) {
+
+		uint32_t isa_req = breakpoint->length & 1;	/* micro mips request bit */
+		uint32_t bplength = breakpoint->length & ~1;	/* drop micro mips request bit for length */
+		uint32_t bpaddr = breakpoint->address & ~1;	/* drop isa bit from address, if set */
+
+		if (bplength == 4) {
 			uint32_t verify = 0xffffffff;
+			uint32_t sdbbp32_instr = MIPS32_SDBBP(isa_req);
+			if (ejtag_info->endianness && isa_req)
+				sdbbp32_instr = SWAP16(sdbbp32_instr);
 
-			retval = target_read_memory(target, breakpoint->address, breakpoint->length, 1,
-					breakpoint->orig_instr);
-			if (retval != ERROR_OK)
-				return retval;
-			retval = target_write_u32(target, breakpoint->address, MIPS32_SDBBP);
-			if (retval != ERROR_OK)
-				return retval;
+			if ((breakpoint->address & 3) == 0) {	/* word alligned */
 
-			retval = target_read_u32(target, breakpoint->address, &verify);
-			if (retval != ERROR_OK)
-				return retval;
-			if (verify != MIPS32_SDBBP) {
-				LOG_ERROR("Unable to set 32bit breakpoint at address %08" PRIx32
-						" - check that memory is read/writable", breakpoint->address);
+				retval = target_read_memory(target, bpaddr, bplength, 1, breakpoint->orig_instr);
+				if (retval != ERROR_OK)
+					return retval;
+
+				retval = target_write_u32(target, bpaddr, sdbbp32_instr);
+				if (retval != ERROR_OK)
+					return retval;
+
+				retval = target_read_u32(target, bpaddr, &verify);
+				if (retval != ERROR_OK)
+					return retval;
+
+				if (verify != sdbbp32_instr)
+					verify = 0;
+
+			} else {	/* 16 bit aligned */
+				retval = target_read_memory(target, bpaddr, 2, 2, breakpoint->orig_instr);
+				if (retval != ERROR_OK)
+					return retval;
+
+				uint8_t sdbbp_buf[4];
+				target_buffer_set_u32(target, sdbbp_buf, sdbbp32_instr);
+
+				retval = target_write_memory(target, bpaddr, 2, 2, sdbbp_buf);
+				if (retval != ERROR_OK)
+					return retval;
+
+				retval = target_read_memory(target, bpaddr, 2, 2, sdbbp_buf);
+				if (retval != ERROR_OK)
+					return retval;
+
+				if (target_buffer_get_u32(target, sdbbp_buf) != sdbbp32_instr)
+					verify = 0;
+			}
+
+			if (verify == 0) {
+				LOG_ERROR("Unable to set 32bit breakpoint at address %08" PRIx64
+					" - check that memory is read/writable", breakpoint->address);
 				return ERROR_OK;
 			}
+
 		} else {
 			uint16_t verify = 0xffff;
 
-			retval = target_read_memory(target, breakpoint->address, breakpoint->length, 1,
-					breakpoint->orig_instr);
+			retval = target_read_memory(target, bpaddr, bplength, 1, breakpoint->orig_instr);
 			if (retval != ERROR_OK)
 				return retval;
-			retval = target_write_u16(target, breakpoint->address, MIPS16_SDBBP);
+
+			retval = target_write_u16(target, bpaddr, MIPS16_SDBBP(isa_req));
 			if (retval != ERROR_OK)
 				return retval;
 
-			retval = target_read_u16(target, breakpoint->address, &verify);
+			retval = target_read_u16(target, bpaddr, &verify);
 			if (retval != ERROR_OK)
 				return retval;
-			if (verify != MIPS16_SDBBP) {
-				LOG_ERROR("Unable to set 16bit breakpoint at address %08" PRIx32
+
+			if (verify != MIPS16_SDBBP(isa_req)) {
+				LOG_ERROR("Unable to set 16bit breakpoint at address %08" PRIx64
 						" - check that memory is read/writable", breakpoint->address);
 				return ERROR_OK;
 			}
@@ -717,46 +766,58 @@ static int mips_m4k_unset_breakpoint(struct target *target,
 
 	} else {
 		/* restore original instruction (kept in target endianness) */
+		uint32_t isa_req = breakpoint->length & 1;
+		uint32_t bplength = breakpoint->length & ~1;
+		uint8_t current_instr[4];
 		LOG_DEBUG("bpid: %" PRIu32, breakpoint->unique_id);
-		if (breakpoint->length == 4) {
-			uint32_t current_instr;
-
-			/* check that user program has not modified breakpoint instruction */
-			retval = target_read_memory(target, breakpoint->address, 4, 1,
-					(uint8_t *)&current_instr);
-			if (retval != ERROR_OK)
-				return retval;
-
-			/**
-			 * target_read_memory() gets us data in _target_ endianess.
-			 * If we want to use this data on the host for comparisons with some macros
-			 * we must first transform it to _host_ endianess using target_buffer_get_u32().
-			 */
-			current_instr = target_buffer_get_u32(target, (uint8_t *)&current_instr);
-
-			if (current_instr == MIPS32_SDBBP) {
-				retval = target_write_memory(target, breakpoint->address, 4, 1,
-						breakpoint->orig_instr);
+		if (bplength == 4) {
+			uint32_t sdbbp32_instr =  MIPS32_SDBBP(isa_req);
+			if (ejtag_info->endianness && isa_req)
+				sdbbp32_instr = SWAP16(sdbbp32_instr);
+
+			if ((breakpoint->address & 3) == 0) {		/* 32bit aligned */
+				/* check that user program has not modified breakpoint instruction */
+				retval = target_read_memory(target, breakpoint->address, 4, 1, current_instr);
+				if (retval != ERROR_OK)
+					return retval;
+				/**
+				* target_read_memory() gets us data in _target_ endianess.
+				* If we want to use this data on the host for comparisons with some macros
+				* we must first transform it to _host_ endianess using target_buffer_get_u16().
+				*/
+				if (sdbbp32_instr == target_buffer_get_u32(target, current_instr)) {
+					retval = target_write_memory(target, breakpoint->address, 4, 1,
+										breakpoint->orig_instr);
+					if (retval != ERROR_OK)
+						return retval;
+				}
+			} else {	/* 16bit alligned */
+				retval = target_read_memory(target, breakpoint->address, 2, 2, current_instr);
 				if (retval != ERROR_OK)
 					return retval;
+
+				if (sdbbp32_instr == target_buffer_get_u32(target, current_instr)) {
+					retval = target_write_memory(target, breakpoint->address, 2, 2,
+										breakpoint->orig_instr);
+					if (retval != ERROR_OK)
+						return retval;
+				}
 			}
 		} else {
-			uint16_t current_instr;
-
 			/* check that user program has not modified breakpoint instruction */
-			retval = target_read_memory(target, breakpoint->address, 2, 1,
-					(uint8_t *)&current_instr);
+			retval = target_read_memory(target, breakpoint->address, 2, 1, current_instr);
 			if (retval != ERROR_OK)
 				return retval;
-			current_instr = target_buffer_get_u16(target, (uint8_t *)&current_instr);
-			if (current_instr == MIPS16_SDBBP) {
+
+			if (target_buffer_get_u16(target, current_instr) == MIPS16_SDBBP(isa_req)) {
 				retval = target_write_memory(target, breakpoint->address, 2, 1,
-						breakpoint->orig_instr);
+									breakpoint->orig_instr);
 				if (retval != ERROR_OK)
 					return retval;
 			}
 		}
 	}
+
 	breakpoint->set = 0;
 
 	return ERROR_OK;
@@ -766,6 +827,12 @@ static int mips_m4k_add_breakpoint(struct target *target, struct breakpoint *bre
 {
 	struct mips32_common *mips32 = target_to_mips32(target);
 
+	if ((breakpoint->length > 5 || breakpoint->length < 2) ||		/* out of range */
+		(breakpoint->length == 4 && (breakpoint->address & 2)) ||	/* mips32 unaligned */
+		(mips32->isa_imp == MIPS32_ONLY && breakpoint->length != 4) ||	/* misp32 specific */
+		((mips32->isa_imp & 1) != (breakpoint->length & 1)))		/* isa not implemented */
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+
 	if (breakpoint->type == BKPT_HARD) {
 		if (mips32->num_inst_bpoints_avail < 1) {
 			LOG_INFO("no hardware breakpoint available");
@@ -949,13 +1016,13 @@ static void mips_m4k_enable_watchpoints(struct target *target)
 	}
 }
 
-static int mips_m4k_read_memory(struct target *target, uint32_t address,
+static int mips_m4k_read_memory(struct target *target, target_addr_t address,
 		uint32_t size, uint32_t count, uint8_t *buffer)
 {
 	struct mips32_common *mips32 = target_to_mips32(target);
 	struct mips_ejtag *ejtag_info = &mips32->ejtag_info;
 
-	LOG_DEBUG("address: 0x%8.8" PRIx32 ", size: 0x%8.8" PRIx32 ", count: 0x%8.8" PRIx32 "",
+	LOG_DEBUG("address: " TARGET_ADDR_FMT ", size: 0x%8.8" PRIx32 ", count: 0x%8.8" PRIx32 "",
 			address, size, count);
 
 	if (target->state != TARGET_HALTED) {
@@ -1008,13 +1075,13 @@ static int mips_m4k_read_memory(struct target *target, uint32_t address,
 	return retval;
 }
 
-static int mips_m4k_write_memory(struct target *target, uint32_t address,
+static int mips_m4k_write_memory(struct target *target, target_addr_t address,
 		uint32_t size, uint32_t count, const uint8_t *buffer)
 {
 	struct mips32_common *mips32 = target_to_mips32(target);
 	struct mips_ejtag *ejtag_info = &mips32->ejtag_info;
 
-	LOG_DEBUG("address: 0x%8.8" PRIx32 ", size: 0x%8.8" PRIx32 ", count: 0x%8.8" PRIx32 "",
+	LOG_DEBUG("address: " TARGET_ADDR_FMT ", size: 0x%8.8" PRIx32 ", count: 0x%8.8" PRIx32 "",
 			address, size, count);
 
 	if (target->state != TARGET_HALTED) {
@@ -1107,39 +1174,33 @@ static int mips_m4k_target_create(struct target *target, Jim_Interp *interp)
 
 static int mips_m4k_examine(struct target *target)
 {
-	int retval;
 	struct mips_m4k_common *mips_m4k = target_to_m4k(target);
 	struct mips_ejtag *ejtag_info = &mips_m4k->mips32.ejtag_info;
-	uint32_t idcode = 0;
 
 	if (!target_was_examined(target)) {
-		retval = mips_ejtag_get_idcode(ejtag_info, &idcode);
-		if (retval != ERROR_OK)
+		int retval = mips_ejtag_get_idcode(ejtag_info);
+		if (retval != ERROR_OK) {
+			LOG_ERROR("idcode read failed");
 			return retval;
-		ejtag_info->idcode = idcode;
-
-		if (((idcode >> 1) & 0x7FF) == 0x29) {
+		}
+		if (((ejtag_info->idcode >> 1) & 0x7FF) == 0x29) {
 			/* we are using a pic32mx so select ejtag port
 			 * as it is not selected by default */
 			mips_ejtag_set_instr(ejtag_info, MTAP_SW_ETAP);
-			LOG_DEBUG("PIC32MX Detected - using EJTAG Interface");
+			LOG_DEBUG("PIC32 Detected - using EJTAG Interface");
 			mips_m4k->is_pic32mx = true;
 		}
 	}
 
 	/* init rest of ejtag interface */
-	retval = mips_ejtag_init(ejtag_info);
+	int retval = mips_ejtag_init(ejtag_info);
 	if (retval != ERROR_OK)
 		return retval;
 
-	retval = mips32_examine(target);
-	if (retval != ERROR_OK)
-		return retval;
-
-	return ERROR_OK;
+	return mips32_examine(target);
 }
 
-static int mips_m4k_bulk_write_memory(struct target *target, uint32_t address,
+static int mips_m4k_bulk_write_memory(struct target *target, target_addr_t address,
 		uint32_t count, const uint8_t *buffer)
 {
 	struct mips32_common *mips32 = target_to_mips32(target);
@@ -1148,7 +1209,8 @@ static int mips_m4k_bulk_write_memory(struct target *target, uint32_t address,
 	int retval;
 	int write_t = 1;
 
-	LOG_DEBUG("address: 0x%8.8" PRIx32 ", count: 0x%8.8" PRIx32 "", address, count);
+	LOG_DEBUG("address: " TARGET_ADDR_FMT ", count: 0x%8.8" PRIx32 "",
+			  address, count);
 
 	/* check alignment */
 	if (address & 0x3u)
@@ -1175,8 +1237,8 @@ static int mips_m4k_bulk_write_memory(struct target *target, uint32_t address,
 
 	if (address <= fast_data_area->address + fast_data_area->size &&
 			fast_data_area->address <= address + count) {
-		LOG_ERROR("fast_data (0x%8.8" PRIx32 ") is within write area "
-			  "(0x%8.8" PRIx32 "-0x%8.8" PRIx32 ").",
+		LOG_ERROR("fast_data (" TARGET_ADDR_FMT ") is within write area "
+			  "(" TARGET_ADDR_FMT "-" TARGET_ADDR_FMT ").",
 			  fast_data_area->address, address, address + count);
 		LOG_ERROR("Change work-area-phys or load_image address!");
 		return ERROR_FAIL;
@@ -1339,7 +1401,7 @@ COMMAND_HANDLER(mips_m4k_handle_scan_delay_command)
 			return ERROR_COMMAND_SYNTAX_ERROR;
 
 	command_print(CMD_CTX, "scan delay: %d nsec", ejtag_info->scan_delay);
-	if (ejtag_info->scan_delay >= 2000000) {
+	if (ejtag_info->scan_delay >= MIPS32_SCAN_DELAY_LEGACY_MODE) {
 		ejtag_info->mode = 0;
 		command_print(CMD_CTX, "running in legacy mode");
 	} else {
diff --git a/src/target/mips_m4k.h b/src/target/mips_m4k.h
index cf82661..ea09ae5 100644
--- a/src/target/mips_m4k.h
+++ b/src/target/mips_m4k.h
@@ -41,6 +41,17 @@ target_to_m4k(struct target *target)
 			struct mips_m4k_common, mips32);
 }
 
+static inline void mips_m4k_isa_filter(enum mips32_isa_imp isa_imp, target_addr_t  *addr)
+{
+	if (isa_imp <= 1) {	/* if only one isa implemented */
+		target_addr_t address = (*addr & ~1) | isa_imp;
+
+		if (address != *addr) {
+			LOG_USER("Warning: isa bit changed due to isa not implemented");
+			*addr = address;
+		}
+	}
+}
 extern const struct command_registration mips_m4k_command_handlers[];
 
 #endif /* OPENOCD_TARGET_MIPS_M4K_H */
diff --git a/src/target/nds32.c b/src/target/nds32.c
index 2926b23..e4bb17f 100644
--- a/src/target/nds32.c
+++ b/src/target/nds32.c
@@ -823,7 +823,7 @@ int nds32_read_memory(struct target *target, uint32_t address,
 	return aice_read_mem_unit(aice, address, size, count, buffer);
 }
 
-int nds32_read_phys_memory(struct target *target, uint32_t address,
+int nds32_read_phys_memory(struct target *target, target_addr_t address,
 		uint32_t size, uint32_t count, uint8_t *buffer)
 {
 	struct aice_port_s *aice = target_to_aice(target);
@@ -932,7 +932,7 @@ int nds32_write_memory(struct target *target, uint32_t address,
 	return aice_write_mem_unit(aice, address, size, count, buffer);
 }
 
-int nds32_write_phys_memory(struct target *target, uint32_t address,
+int nds32_write_phys_memory(struct target *target, target_addr_t address,
 		uint32_t size, uint32_t count, const uint8_t *buffer)
 {
 	struct aice_port_s *aice = target_to_aice(target);
@@ -1674,7 +1674,7 @@ int nds32_init_arch_info(struct target *target, struct nds32 *nds32)
 	return ERROR_OK;
 }
 
-int nds32_virtual_to_physical(struct target *target, uint32_t address, uint32_t *physical)
+int nds32_virtual_to_physical(struct target *target, target_addr_t address, target_addr_t *physical)
 {
 	struct nds32 *nds32 = target_to_nds32(target);
 
@@ -1692,7 +1692,7 @@ int nds32_virtual_to_physical(struct target *target, uint32_t address, uint32_t
 	return ERROR_FAIL;
 }
 
-int nds32_cache_sync(struct target *target, uint32_t address, uint32_t length)
+int nds32_cache_sync(struct target *target, target_addr_t address, uint32_t length)
 {
 	struct aice_port_s *aice = target_to_aice(target);
 	struct nds32 *nds32 = target_to_nds32(target);
@@ -1738,7 +1738,7 @@ int nds32_cache_sync(struct target *target, uint32_t address, uint32_t length)
 			/* Because PSW.IT is turned off under debug exception, address MUST
 			 * be physical address.  L1I_VA_INVALIDATE uses PSW.IT to decide
 			 * address translation or not. */
-			uint32_t physical_addr;
+			target_addr_t physical_addr;
 			if (ERROR_FAIL == target->type->virt2phys(target, cur_address,
 						&physical_addr))
 				return ERROR_FAIL;
@@ -1764,7 +1764,7 @@ uint32_t nds32_nextpc(struct nds32 *nds32, int current, uint32_t address)
 }
 
 int nds32_step(struct target *target, int current,
-		uint32_t address, int handle_breakpoints)
+		target_addr_t address, int handle_breakpoints)
 {
 	LOG_DEBUG("target->state: %s",
 			target_state_name(target));
@@ -1778,7 +1778,7 @@ int nds32_step(struct target *target, int current,
 
 	address = nds32_nextpc(nds32, current, address);
 
-	LOG_DEBUG("STEP PC %08" PRIx32 "%s", address, !current ? "!" : "");
+	LOG_DEBUG("STEP PC %08" TARGET_PRIxADDR "%s", address, !current ? "!" : "");
 
 	/** set DSSIM */
 	uint32_t ir14_value;
@@ -2120,9 +2120,9 @@ int nds32_poll(struct target *target)
 }
 
 int nds32_resume(struct target *target, int current,
-		uint32_t address, int handle_breakpoints, int debug_execution)
+		target_addr_t address, int handle_breakpoints, int debug_execution)
 {
-	LOG_DEBUG("current %d address %08" PRIx32
+	LOG_DEBUG("current %d address %08" TARGET_PRIxADDR
 			" handle_breakpoints %d"
 			" debug_execution %d",
 			current, address, handle_breakpoints, debug_execution);
@@ -2136,7 +2136,7 @@ int nds32_resume(struct target *target, int current,
 
 	address = nds32_nextpc(nds32, current, address);
 
-	LOG_DEBUG("RESUME PC %08" PRIx32 "%s", address, !current ? "!" : "");
+	LOG_DEBUG("RESUME PC %08" TARGET_PRIxADDR "%s", address, !current ? "!" : "");
 
 	if (!debug_execution)
 		target_free_all_working_areas(target);
diff --git a/src/target/nds32.h b/src/target/nds32.h
index 88af4f3..141dbf4 100644
--- a/src/target/nds32.h
+++ b/src/target/nds32.h
@@ -400,23 +400,23 @@ extern int nds32_get_mapped_reg(struct nds32 *nds32, unsigned regnum, uint32_t *
 extern int nds32_set_mapped_reg(struct nds32 *nds32, unsigned regnum, uint32_t value);
 
 extern int nds32_edm_config(struct nds32 *nds32);
-extern int nds32_cache_sync(struct target *target, uint32_t address, uint32_t length);
+extern int nds32_cache_sync(struct target *target, target_addr_t address, uint32_t length);
 extern int nds32_mmu(struct target *target, int *enabled);
-extern int nds32_virtual_to_physical(struct target *target, uint32_t address,
-		uint32_t *physical);
-extern int nds32_read_phys_memory(struct target *target, uint32_t address,
+extern int nds32_virtual_to_physical(struct target *target, target_addr_t address,
+		target_addr_t *physical);
+extern int nds32_read_phys_memory(struct target *target, target_addr_t address,
 		uint32_t size, uint32_t count, uint8_t *buffer);
-extern int nds32_write_phys_memory(struct target *target, uint32_t address,
+extern int nds32_write_phys_memory(struct target *target, target_addr_t address,
 		uint32_t size, uint32_t count, const uint8_t *buffer);
 extern uint32_t nds32_nextpc(struct nds32 *nds32, int current, uint32_t address);
 extern int nds32_examine_debug_reason(struct nds32 *nds32);
 extern int nds32_step(struct target *target, int current,
-		uint32_t address, int handle_breakpoints);
+		target_addr_t address, int handle_breakpoints);
 extern int nds32_target_state(struct nds32 *nds32, enum target_state *state);
 extern int nds32_halt(struct target *target);
 extern int nds32_poll(struct target *target);
 extern int nds32_resume(struct target *target, int current,
-		uint32_t address, int handle_breakpoints, int debug_execution);
+		target_addr_t address, int handle_breakpoints, int debug_execution);
 extern int nds32_assert_reset(struct target *target);
 extern int nds32_init(struct nds32 *nds32);
 extern int nds32_get_gdb_fileio_info(struct target *target, struct gdb_fileio_info *fileio_info);
diff --git a/src/target/nds32_aice.c b/src/target/nds32_aice.c
index bdfafb5..e494a3e 100644
--- a/src/target/nds32_aice.c
+++ b/src/target/nds32_aice.c
@@ -42,8 +42,8 @@ int aice_write_reg_64(struct aice_port_s *aice, uint32_t num, uint64_t val)
 	return aice->port->api->write_reg_64(aice->coreid, num, val);
 }
 
-int aice_read_tlb(struct aice_port_s *aice, uint32_t virtual_address,
-		uint32_t *physical_address)
+int aice_read_tlb(struct aice_port_s *aice, target_addr_t virtual_address,
+		target_addr_t *physical_address)
 {
 	if (aice->port->api->read_tlb == NULL) {
 		LOG_WARNING("Not implemented: %s", __func__);
diff --git a/src/target/nds32_aice.h b/src/target/nds32_aice.h
index ae801ed..5ea3b16 100644
--- a/src/target/nds32_aice.h
+++ b/src/target/nds32_aice.h
@@ -23,8 +23,8 @@
 
 int aice_read_reg_64(struct aice_port_s *aice, uint32_t num, uint64_t *val);
 int aice_write_reg_64(struct aice_port_s *aice, uint32_t num, uint64_t val);
-int aice_read_tlb(struct aice_port_s *aice, uint32_t virtual_address,
-		uint32_t *physical_address);
+int aice_read_tlb(struct aice_port_s *aice, target_addr_t virtual_address,
+		target_addr_t *physical_address);
 int aice_cache_ctl(struct aice_port_s *aice, uint32_t subtype, uint32_t address);
 int aice_set_retry_times(struct aice_port_s *aice, uint32_t a_retry_times);
 int aice_program_edm(struct aice_port_s *aice, char *command_sequence);
diff --git a/src/target/nds32_tlb.c b/src/target/nds32_tlb.c
index 6a91a0f..c4bce1a 100644
--- a/src/target/nds32_tlb.c
+++ b/src/target/nds32_tlb.c
@@ -22,8 +22,8 @@
 #include "nds32_aice.h"
 #include "nds32_tlb.h"
 
-int nds32_probe_tlb(struct nds32 *nds32, const uint32_t virtual_address,
-		uint32_t *physical_address)
+int nds32_probe_tlb(struct nds32 *nds32, const target_addr_t virtual_address,
+		target_addr_t *physical_address)
 {
 	struct target *target = nds32->target;
 	struct aice_port_s *aice = target_to_aice(target);
@@ -38,8 +38,8 @@ struct page_table_walker_info_s page_table_info[PAGE_SIZE_NUM] = {
 	{0xFF000000, 22, 0x00FFE000, 11, 0x00001FFF, 0xFFFFF000, 0xFFFFE000, 0xFFFFE000},
 };
 
-int nds32_walk_page_table(struct nds32 *nds32, const uint32_t virtual_address,
-		uint32_t *physical_address)
+int nds32_walk_page_table(struct nds32 *nds32, const target_addr_t virtual_address,
+		target_addr_t *physical_address)
 {
 	struct target *target = nds32->target;
 	uint32_t value_mr1;
diff --git a/src/target/nds32_tlb.h b/src/target/nds32_tlb.h
index ada2c19..62512c1 100644
--- a/src/target/nds32_tlb.h
+++ b/src/target/nds32_tlb.h
@@ -39,9 +39,9 @@ struct page_table_walker_info_s {
 	uint32_t ppn_mask;
 };
 
-extern int nds32_probe_tlb(struct nds32 *nds32, const uint32_t virtual_address,
-		uint32_t *physical_address);
-extern int nds32_walk_page_table(struct nds32 *nds32, const uint32_t virtual_address,
-		uint32_t *physical_address);
+extern int nds32_probe_tlb(struct nds32 *nds32, const target_addr_t virtual_address,
+		target_addr_t *physical_address);
+extern int nds32_walk_page_table(struct nds32 *nds32, const target_addr_t virtual_address,
+		target_addr_t *physical_address);
 
 #endif /* OPENOCD_TARGET_NDS32_TLB_H */
diff --git a/src/target/nds32_v2.c b/src/target/nds32_v2.c
index da8bbbc..29489a0 100644
--- a/src/target/nds32_v2.c
+++ b/src/target/nds32_v2.c
@@ -112,7 +112,7 @@ static int nds32_v2_activate_hardware_breakpoint(struct target *target)
 				/* enable breakpoint (physical address) */
 				aice_write_debug_reg(aice, NDS_EDM_SR_BPC0 + hbr_index, 0xA);
 
-			LOG_DEBUG("Add hardware BP %" PRId32 " at %08" PRIx32, hbr_index,
+			LOG_DEBUG("Add hardware BP %" PRId32 " at %08" TARGET_PRIxADDR, hbr_index,
 					bp->address);
 
 			hbr_index++;
@@ -139,7 +139,7 @@ static int nds32_v2_deactivate_hardware_breakpoint(struct target *target)
 		else
 			return ERROR_FAIL;
 
-		LOG_DEBUG("Remove hardware BP %" PRId32 " at %08" PRIx32, hbr_index,
+		LOG_DEBUG("Remove hardware BP %" PRId32 " at %08" TARGET_PRIxADDR, hbr_index,
 				bp->address);
 
 		hbr_index++;
@@ -184,7 +184,7 @@ static int nds32_v2_activate_hardware_watchpoint(struct target *target)
 		/* set value */
 		aice_write_debug_reg(aice, NDS_EDM_SR_BPV0 + wp_num, 0);
 
-		LOG_DEBUG("Add hardware wathcpoint %" PRId32 " at %08" PRIx32 " mask %08" PRIx32, wp_num,
+		LOG_DEBUG("Add hardware watchpoint %" PRId32 " at %08" TARGET_PRIxADDR " mask %08" PRIx32, wp_num,
 				wp->address, wp->mask);
 
 	}
@@ -204,7 +204,7 @@ static int nds32_v2_deactivate_hardware_watchpoint(struct target *target)
 		/* disable watchpoint */
 		aice_write_debug_reg(aice, NDS_EDM_SR_BPC0 + wp_num, 0x0);
 
-		LOG_DEBUG("Remove hardware wathcpoint %" PRId32 " at %08" PRIx32 " mask %08" PRIx32,
+		LOG_DEBUG("Remove hardware watchpoint %" PRId32 " at %08" TARGET_PRIxADDR " mask %08" PRIx32,
 				wp_num, wp->address, wp->mask);
 	}
 
@@ -405,7 +405,7 @@ static int nds32_v2_deassert_reset(struct target *target)
 }
 
 static int nds32_v2_checksum_memory(struct target *target,
-		uint32_t address, uint32_t count, uint32_t *checksum)
+		target_addr_t address, uint32_t count, uint32_t *checksum)
 {
 	LOG_WARNING("Not implemented: %s", __func__);
 
@@ -561,8 +561,8 @@ static int nds32_v2_run_algorithm(struct target *target,
 		struct mem_param *mem_params,
 		int num_reg_params,
 		struct reg_param *reg_params,
-		uint32_t entry_point,
-		uint32_t exit_point,
+		target_addr_t entry_point,
+		target_addr_t exit_point,
 		int timeout_ms,
 		void *arch_info)
 {
@@ -635,11 +635,11 @@ static int nds32_v2_examine(struct target *target)
 	return ERROR_OK;
 }
 
-static int nds32_v2_translate_address(struct target *target, uint32_t *address)
+static int nds32_v2_translate_address(struct target *target, target_addr_t *address)
 {
 	struct nds32 *nds32 = target_to_nds32(target);
 	struct nds32_memory *memory = &(nds32->memory);
-	uint32_t physical_address;
+	target_addr_t physical_address;
 
 	/* Following conditions need to do address translation
 	 * 1. BUS mode
@@ -656,7 +656,7 @@ static int nds32_v2_translate_address(struct target *target, uint32_t *address)
 	return ERROR_OK;
 }
 
-static int nds32_v2_read_buffer(struct target *target, uint32_t address,
+static int nds32_v2_read_buffer(struct target *target, target_addr_t address,
 		uint32_t size, uint8_t *buffer)
 {
 	struct nds32 *nds32 = target_to_nds32(target);
@@ -676,7 +676,7 @@ static int nds32_v2_read_buffer(struct target *target, uint32_t address,
 	return nds32_read_buffer(target, address, size, buffer);
 }
 
-static int nds32_v2_write_buffer(struct target *target, uint32_t address,
+static int nds32_v2_write_buffer(struct target *target, target_addr_t address,
 		uint32_t size, const uint8_t *buffer)
 {
 	struct nds32 *nds32 = target_to_nds32(target);
@@ -696,7 +696,7 @@ static int nds32_v2_write_buffer(struct target *target, uint32_t address,
 	return nds32_write_buffer(target, address, size, buffer);
 }
 
-static int nds32_v2_read_memory(struct target *target, uint32_t address,
+static int nds32_v2_read_memory(struct target *target, target_addr_t address,
 		uint32_t size, uint32_t count, uint8_t *buffer)
 {
 	struct nds32 *nds32 = target_to_nds32(target);
@@ -716,7 +716,7 @@ static int nds32_v2_read_memory(struct target *target, uint32_t address,
 	return nds32_read_memory(target, address, size, count, buffer);
 }
 
-static int nds32_v2_write_memory(struct target *target, uint32_t address,
+static int nds32_v2_write_memory(struct target *target, target_addr_t address,
 		uint32_t size, uint32_t count, const uint8_t *buffer)
 {
 	struct nds32 *nds32 = target_to_nds32(target);
diff --git a/src/target/nds32_v3.c b/src/target/nds32_v3.c
index 43d7054..e5d146b 100644
--- a/src/target/nds32_v3.c
+++ b/src/target/nds32_v3.c
@@ -53,7 +53,7 @@ static int nds32_v3_activate_hardware_breakpoint(struct target *target)
 				/* enable breakpoint (physical address) */
 				aice_write_debug_reg(aice, NDS_EDM_SR_BPC0 + hbr_index, 0xA);
 
-			LOG_DEBUG("Add hardware BP %" PRId32 " at %08" PRIx32, hbr_index,
+			LOG_DEBUG("Add hardware BP %" PRId32 " at %08" TARGET_PRIxADDR, hbr_index,
 					bp->address);
 		} else {
 			return ERROR_FAIL;
@@ -81,7 +81,7 @@ static int nds32_v3_deactivate_hardware_breakpoint(struct target *target)
 			return ERROR_FAIL;
 		}
 
-		LOG_DEBUG("Remove hardware BP %" PRId32 " at %08" PRIx32, hbr_index,
+		LOG_DEBUG("Remove hardware BP %" PRId32 " at %08" TARGET_PRIxADDR, hbr_index,
 				bp->address);
 	}
 
@@ -128,7 +128,7 @@ static int nds32_v3_activate_hardware_watchpoint(struct target *target)
 			/* set value */
 			aice_write_debug_reg(aice, NDS_EDM_SR_BPV0 + wp_num, 0);
 
-			LOG_DEBUG("Add hardware wathcpoint %" PRId32 " at %08" PRIx32 " mask %08" PRIx32,
+			LOG_DEBUG("Add hardware watchpoint %" PRId32 " at %08" TARGET_PRIxADDR " mask %08" PRIx32,
 					wp_num, wp->address, wp->mask);
 
 			wp_num++;
@@ -169,7 +169,7 @@ static int nds32_v3_deactivate_hardware_watchpoint(struct target *target)
 			/* disable watchpoint */
 			aice_write_debug_reg(aice, NDS_EDM_SR_BPC0 + wp_num, 0x0);
 
-			LOG_DEBUG("Remove hardware wathcpoint %" PRId32 " at %08" PRIx32
+			LOG_DEBUG("Remove hardware watchpoint %" PRId32 " at %08" TARGET_PRIxADDR
 					" mask %08" PRIx32, wp_num,
 					wp->address, wp->mask);
 			wp_num++;
diff --git a/src/target/nds32_v3_common.c b/src/target/nds32_v3_common.c
index 191f4b5..271ffdd 100644
--- a/src/target/nds32_v3_common.c
+++ b/src/target/nds32_v3_common.c
@@ -368,7 +368,7 @@ int nds32_v3_target_request_data(struct target *target,
 }
 
 int nds32_v3_checksum_memory(struct target *target,
-		uint32_t address, uint32_t count, uint32_t *checksum)
+		target_addr_t address, uint32_t count, uint32_t *checksum)
 {
 	LOG_WARNING("Not implemented: %s", __func__);
 
@@ -434,8 +434,8 @@ int nds32_v3_run_algorithm(struct target *target,
 		struct mem_param *mem_params,
 		int num_reg_params,
 		struct reg_param *reg_params,
-		uint32_t entry_point,
-		uint32_t exit_point,
+		target_addr_t entry_point,
+		target_addr_t exit_point,
 		int timeout_ms,
 		void *arch_info)
 {
@@ -444,7 +444,7 @@ int nds32_v3_run_algorithm(struct target *target,
 	return ERROR_FAIL;
 }
 
-int nds32_v3_read_buffer(struct target *target, uint32_t address,
+int nds32_v3_read_buffer(struct target *target, target_addr_t address,
 		uint32_t size, uint8_t *buffer)
 {
 	struct nds32 *nds32 = target_to_nds32(target);
@@ -456,7 +456,7 @@ int nds32_v3_read_buffer(struct target *target, uint32_t address,
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
-	uint32_t physical_address;
+	target_addr_t physical_address;
 	/* BUG: If access range crosses multiple pages, the translation will not correct
 	 * for second page or so. */
 
@@ -502,7 +502,7 @@ int nds32_v3_read_buffer(struct target *target, uint32_t address,
 	return result;
 }
 
-int nds32_v3_write_buffer(struct target *target, uint32_t address,
+int nds32_v3_write_buffer(struct target *target, target_addr_t address,
 		uint32_t size, const uint8_t *buffer)
 {
 	struct nds32 *nds32 = target_to_nds32(target);
@@ -514,7 +514,7 @@ int nds32_v3_write_buffer(struct target *target, uint32_t address,
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
-	uint32_t physical_address;
+	target_addr_t physical_address;
 	/* BUG: If access range crosses multiple pages, the translation will not correct
 	 * for second page or so. */
 
@@ -564,7 +564,7 @@ int nds32_v3_write_buffer(struct target *target, uint32_t address,
 	return nds32_write_buffer(target, address, size, buffer);
 }
 
-int nds32_v3_read_memory(struct target *target, uint32_t address,
+int nds32_v3_read_memory(struct target *target, target_addr_t address,
 		uint32_t size, uint32_t count, uint8_t *buffer)
 {
 	struct nds32 *nds32 = target_to_nds32(target);
@@ -576,7 +576,7 @@ int nds32_v3_read_memory(struct target *target, uint32_t address,
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
-	uint32_t physical_address;
+	target_addr_t physical_address;
 	/* BUG: If access range crosses multiple pages, the translation will not correct
 	 * for second page or so. */
 
@@ -622,7 +622,7 @@ int nds32_v3_read_memory(struct target *target, uint32_t address,
 	return result;
 }
 
-int nds32_v3_write_memory(struct target *target, uint32_t address,
+int nds32_v3_write_memory(struct target *target, target_addr_t address,
 		uint32_t size, uint32_t count, const uint8_t *buffer)
 {
 	struct nds32 *nds32 = target_to_nds32(target);
@@ -634,7 +634,7 @@ int nds32_v3_write_memory(struct target *target, uint32_t address,
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
-	uint32_t physical_address;
+	target_addr_t physical_address;
 	/* BUG: If access range crosses multiple pages, the translation will not correct
 	 * for second page or so. */
 
diff --git a/src/target/nds32_v3_common.h b/src/target/nds32_v3_common.h
index 1f5df19..23393e5 100644
--- a/src/target/nds32_v3_common.h
+++ b/src/target/nds32_v3_common.h
@@ -34,7 +34,7 @@ void nds32_v3_common_register_callback(struct nds32_v3_common_callback *callback
 int nds32_v3_target_request_data(struct target *target,
 		uint32_t size, uint8_t *buffer);
 int nds32_v3_checksum_memory(struct target *target,
-		uint32_t address, uint32_t count, uint32_t *checksum);
+		target_addr_t address, uint32_t count, uint32_t *checksum);
 int nds32_v3_hit_watchpoint(struct target *target,
 		struct watchpoint **hit_watchpoint);
 int nds32_v3_target_create_common(struct target *target, struct nds32 *nds32);
@@ -43,17 +43,17 @@ int nds32_v3_run_algorithm(struct target *target,
 		struct mem_param *mem_params,
 		int num_reg_params,
 		struct reg_param *reg_params,
-		uint32_t entry_point,
-		uint32_t exit_point,
+		target_addr_t entry_point,
+		target_addr_t exit_point,
 		int timeout_ms,
 		void *arch_info);
-int nds32_v3_read_buffer(struct target *target, uint32_t address,
+int nds32_v3_read_buffer(struct target *target, target_addr_t address,
 		uint32_t size, uint8_t *buffer);
-int nds32_v3_write_buffer(struct target *target, uint32_t address,
+int nds32_v3_write_buffer(struct target *target, target_addr_t address,
 		uint32_t size, const uint8_t *buffer);
-int nds32_v3_read_memory(struct target *target, uint32_t address,
+int nds32_v3_read_memory(struct target *target, target_addr_t address,
 		uint32_t size, uint32_t count, uint8_t *buffer);
-int nds32_v3_write_memory(struct target *target, uint32_t address,
+int nds32_v3_write_memory(struct target *target, target_addr_t address,
 		uint32_t size, uint32_t count, const uint8_t *buffer);
 int nds32_v3_init_target(struct command_context *cmd_ctx,
 		struct target *target);
diff --git a/src/target/nds32_v3m.c b/src/target/nds32_v3m.c
index 919c0c8..86903a5 100644
--- a/src/target/nds32_v3m.c
+++ b/src/target/nds32_v3m.c
@@ -50,7 +50,7 @@ static int nds32_v3m_activate_hardware_breakpoint(struct target *target)
 				/* enable breakpoint (physical address) */
 				aice_write_debug_reg(aice, NDS_EDM_SR_BPC0 + brp_num, 0xA);
 
-			LOG_DEBUG("Add hardware BP %u at %08" PRIx32, brp_num,
+			LOG_DEBUG("Add hardware BP %u at %08" TARGET_PRIxADDR, brp_num,
 					bp->address);
 
 			brp_num--;
@@ -78,7 +78,7 @@ static int nds32_v3m_deactivate_hardware_breakpoint(struct target *target)
 		else
 			return ERROR_FAIL;
 
-		LOG_DEBUG("Remove hardware BP %u at %08" PRIx32, brp_num,
+		LOG_DEBUG("Remove hardware BP %u at %08" TARGET_PRIxADDR, brp_num,
 				bp->address);
 
 		brp_num--;
@@ -125,7 +125,7 @@ static int nds32_v3m_activate_hardware_watchpoint(struct target *target)
 			/* enable watchpoint */
 			aice_write_debug_reg(aice, NDS_EDM_SR_BPC0 + wp_num, wp_config);
 
-			LOG_DEBUG("Add hardware wathcpoint %" PRId32 " at %08" PRIx32
+			LOG_DEBUG("Add hardware watchpoint %" PRId32 " at %08" TARGET_PRIxADDR
 					" mask %08" PRIx32, wp_num, wp->address, wp->mask);
 
 			wp_num++;
@@ -166,7 +166,7 @@ static int nds32_v3m_deactivate_hardware_watchpoint(struct target *target)
 			/* disable watchpoint */
 			aice_write_debug_reg(aice, NDS_EDM_SR_BPC0 + wp_num, 0x0);
 
-			LOG_DEBUG("Remove hardware wathcpoint %" PRId32 " at %08" PRIx32
+			LOG_DEBUG("Remove hardware watchpoint %" PRId32 " at %08" TARGET_PRIxADDR
 					" mask %08" PRIx32, wp_num, wp->address, wp->mask);
 			wp_num++;
 		} else if (nds32_v3m->nds32.global_stop) {
diff --git a/src/target/openrisc/Makefile.am b/src/target/openrisc/Makefile.am
index b00a30d..5a2549a 100644
--- a/src/target/openrisc/Makefile.am
+++ b/src/target/openrisc/Makefile.am
@@ -1,18 +1,12 @@
-include $(top_srcdir)/common.mk
-
-noinst_LTLIBRARIES = libopenrisc.la
-libopenrisc_la_SOURCES = $(OPENRISC_SRC)
-
-OPENRISC_SRC = \
-	or1k.c \
-	or1k_du_adv.c \
-	or1k_tap_mohor.c \
-	or1k_tap_vjtag.c \
-	or1k_tap_xilinx_bscan.c \
-	jsp_server.c
-
-noinst_HEADERS = \
-	or1k.h \
-	or1k_du.h \
-	or1k_tap.h \
-	jsp_server.h
+noinst_LTLIBRARIES += %D%/libopenrisc.la
+%C%_libopenrisc_la_SOURCES = \
+	%D%/or1k.c \
+	%D%/or1k_du_adv.c \
+	%D%/or1k_tap_mohor.c \
+	%D%/or1k_tap_vjtag.c \
+	%D%/or1k_tap_xilinx_bscan.c \
+	%D%/jsp_server.c \
+	%D%/or1k.h \
+	%D%/or1k_du.h \
+	%D%/or1k_tap.h \
+	%D%/jsp_server.h
diff --git a/src/target/openrisc/or1k.c b/src/target/openrisc/or1k.c
index 2cc869f..3895ddf 100644
--- a/src/target/openrisc/or1k.c
+++ b/src/target/openrisc/or1k.c
@@ -861,7 +861,7 @@ static int or1k_resume_or_step(struct target *target, int current,
 		/* Single step past breakpoint at current address */
 		breakpoint = breakpoint_find(target, resume_pc);
 		if (breakpoint) {
-			LOG_DEBUG("Unset breakpoint at 0x%08" PRIx32, breakpoint->address);
+			LOG_DEBUG("Unset breakpoint at 0x%08" TARGET_PRIxADDR, breakpoint->address);
 			retval = or1k_remove_breakpoint(target, breakpoint);
 			if (retval != ERROR_OK)
 				return retval;
@@ -897,7 +897,8 @@ static int or1k_resume_or_step(struct target *target, int current,
 }
 
 static int or1k_resume(struct target *target, int current,
-		uint32_t address, int handle_breakpoints, int debug_execution)
+		       target_addr_t address, int handle_breakpoints,
+		       int debug_execution)
 {
 	return or1k_resume_or_step(target, current, address,
 				   handle_breakpoints,
@@ -906,7 +907,7 @@ static int or1k_resume(struct target *target, int current,
 }
 
 static int or1k_step(struct target *target, int current,
-		     uint32_t address, int handle_breakpoints)
+		     target_addr_t address, int handle_breakpoints)
 {
 	return or1k_resume_or_step(target, current, address,
 				   handle_breakpoints,
@@ -922,7 +923,7 @@ static int or1k_add_breakpoint(struct target *target,
 	struct or1k_du *du_core = or1k_to_du(or1k);
 	uint8_t data;
 
-	LOG_DEBUG("Adding breakpoint: addr 0x%08" PRIx32 ", len %d, type %d, set: %d, id: %" PRId32,
+	LOG_DEBUG("Adding breakpoint: addr 0x%08" TARGET_PRIxADDR ", len %d, type %d, set: %d, id: %" PRId32,
 		  breakpoint->address, breakpoint->length, breakpoint->type,
 		  breakpoint->set, breakpoint->unique_id);
 
@@ -937,7 +938,7 @@ static int or1k_add_breakpoint(struct target *target,
 					 1,
 					 &data);
 	if (retval != ERROR_OK) {
-		LOG_ERROR("Error while reading the instruction at 0x%08" PRIx32,
+		LOG_ERROR("Error while reading the instruction at 0x%08" TARGET_PRIxADDR,
 			   breakpoint->address);
 		return retval;
 	}
@@ -958,14 +959,15 @@ static int or1k_add_breakpoint(struct target *target,
 					  or1k_trap_insn);
 
 	if (retval != ERROR_OK) {
-		LOG_ERROR("Error while writing OR1K_TRAP_INSTR at 0x%08" PRIx32,
+		LOG_ERROR("Error while writing OR1K_TRAP_INSTR at 0x%08" TARGET_PRIxADDR,
 			   breakpoint->address);
 		return retval;
 	}
 
 	/* invalidate instruction cache */
+	uint32_t addr = breakpoint->address;
 	retval = du_core->or1k_jtag_write_cpu(&or1k->jtag,
-			OR1K_ICBIR_CPU_REG_ADD, 1, &breakpoint->address);
+			OR1K_ICBIR_CPU_REG_ADD, 1, &addr);
 	if (retval != ERROR_OK) {
 		LOG_ERROR("Error while invalidating the ICACHE");
 		return retval;
@@ -980,7 +982,7 @@ static int or1k_remove_breakpoint(struct target *target,
 	struct or1k_common *or1k = target_to_or1k(target);
 	struct or1k_du *du_core = or1k_to_du(or1k);
 
-	LOG_DEBUG("Removing breakpoint: addr 0x%08" PRIx32 ", len %d, type %d, set: %d, id: %" PRId32,
+	LOG_DEBUG("Removing breakpoint: addr 0x%08" TARGET_PRIxADDR ", len %d, type %d, set: %d, id: %" PRId32,
 		  breakpoint->address, breakpoint->length, breakpoint->type,
 		  breakpoint->set, breakpoint->unique_id);
 
@@ -996,14 +998,15 @@ static int or1k_remove_breakpoint(struct target *target,
 					  breakpoint->orig_instr);
 
 	if (retval != ERROR_OK) {
-		LOG_ERROR("Error while writing back the instruction at 0x%08" PRIx32,
+		LOG_ERROR("Error while writing back the instruction at 0x%08" TARGET_PRIxADDR,
 			   breakpoint->address);
 		return retval;
 	}
 
 	/* invalidate instruction cache */
+	uint32_t addr = breakpoint->address;
 	retval = du_core->or1k_jtag_write_cpu(&or1k->jtag,
-			OR1K_ICBIR_CPU_REG_ADD, 1, &breakpoint->address);
+			OR1K_ICBIR_CPU_REG_ADD, 1, &addr);
 	if (retval != ERROR_OK) {
 		LOG_ERROR("Error while invalidating the ICACHE");
 		return retval;
@@ -1026,13 +1029,13 @@ static int or1k_remove_watchpoint(struct target *target,
 	return ERROR_OK;
 }
 
-static int or1k_read_memory(struct target *target, uint32_t address,
+static int or1k_read_memory(struct target *target, target_addr_t address,
 		uint32_t size, uint32_t count, uint8_t *buffer)
 {
 	struct or1k_common *or1k = target_to_or1k(target);
 	struct or1k_du *du_core = or1k_to_du(or1k);
 
-	LOG_DEBUG("Read memory at 0x%08" PRIx32 ", size: %" PRIu32 ", count: 0x%08" PRIx32, address, size, count);
+	LOG_DEBUG("Read memory at 0x%08" TARGET_PRIxADDR ", size: %" PRIu32 ", count: 0x%08" PRIx32, address, size, count);
 
 	if (target->state != TARGET_HALTED) {
 		LOG_ERROR("Target not halted");
@@ -1053,13 +1056,13 @@ static int or1k_read_memory(struct target *target, uint32_t address,
 	return du_core->or1k_jtag_read_memory(&or1k->jtag, address, size, count, buffer);
 }
 
-static int or1k_write_memory(struct target *target, uint32_t address,
+static int or1k_write_memory(struct target *target, target_addr_t address,
 		uint32_t size, uint32_t count, const uint8_t *buffer)
 {
 	struct or1k_common *or1k = target_to_or1k(target);
 	struct or1k_du *du_core = or1k_to_du(or1k);
 
-	LOG_DEBUG("Write memory at 0x%08" PRIx32 ", size: %" PRIu32 ", count: 0x%08" PRIx32, address, size, count);
+	LOG_DEBUG("Write memory at 0x%08" TARGET_PRIxADDR ", size: %" PRIu32 ", count: 0x%08" PRIx32, address, size, count);
 
 	if (target->state != TARGET_HALTED) {
 		LOG_WARNING("Target not halted");
@@ -1203,7 +1206,7 @@ int or1k_get_gdb_fileio_info(struct target *target, struct gdb_fileio_info *file
 	return ERROR_FAIL;
 }
 
-static int or1k_checksum_memory(struct target *target, uint32_t address,
+static int or1k_checksum_memory(struct target *target, target_addr_t address,
 		uint32_t count, uint32_t *checksum) {
 
 	return ERROR_FAIL;
diff --git a/src/target/smp.c b/src/target/smp.c
index 3dc6f6d..bdf81a0 100644
--- a/src/target/smp.c
+++ b/src/target/smp.c
@@ -64,7 +64,8 @@ int gdb_read_smp_packet(struct connection *connection,
 			char hex_buffer[len * 2 + 1];
 			uint8_t buffer[len];
 			buf_set_u32(buffer, 0, len * 8, target->gdb_service->core[0]);
-			int pkt_len = hexify(hex_buffer, (char *)buffer, sizeof(buffer), sizeof(hex_buffer));
+			size_t pkt_len = hexify(hex_buffer, buffer, sizeof(buffer),
+				sizeof(hex_buffer));
 
 			retval = gdb_put_packet(connection, hex_buffer, pkt_len);
 		}
diff --git a/src/target/startup.tcl b/src/target/startup.tcl
index cf2813b..9bbc6e3 100644
--- a/src/target/startup.tcl
+++ b/src/target/startup.tcl
@@ -65,7 +65,7 @@ proc ocd_process_reset_inner { MODE } {
 	foreach t $targets {
 		if {![using_jtag] || [jtag tapisenabled [$t cget -chain-position]]} {
 			$t invoke-event examine-start
-			set err [catch "$t arp_examine"]
+			set err [catch "$t arp_examine allow-defer"]
 			if { $err == 0 } {
 				$t invoke-event examine-end
 			}
@@ -111,6 +111,12 @@ proc ocd_process_reset_inner { MODE } {
 				continue
 			}
 
+			# don't wait for targets where examination is deferred
+			# they can not be halted anyway at this point
+			if { ![$t was_examined] && [$t examine_deferred] } {
+				continue
+			}
+
 			# Wait upto 1 second for target to halt.  Why 1sec? Cause
 			# the JTAG tap reset signal might be hooked to a slow
 			# resistor/capacitor circuit - and it might take a while
@@ -135,6 +141,12 @@ proc ocd_process_reset_inner { MODE } {
 				continue
 			}
 
+			# don't wait for targets where examination is deferred
+			# they can not be halted anyway at this point
+			if { ![$t was_examined] && [$t examine_deferred] } {
+				continue
+			}
+
 			set err [catch "$t arp_waitstate halted 5000"]
 			# Did it halt?
 			if { $err == 0 } {
diff --git a/src/target/target.c b/src/target/target.c
index 7908a85..eb45faf 100644
--- a/src/target/target.c
+++ b/src/target/target.c
@@ -58,9 +58,9 @@
 /* default halt wait timeout (ms) */
 #define DEFAULT_HALT_TIMEOUT 5000
 
-static int target_read_buffer_default(struct target *target, uint32_t address,
+static int target_read_buffer_default(struct target *target, target_addr_t address,
 		uint32_t count, uint8_t *buffer);
-static int target_write_buffer_default(struct target *target, uint32_t address,
+static int target_write_buffer_default(struct target *target, target_addr_t address,
 		uint32_t count, const uint8_t *buffer);
 static int target_array2mem(Jim_Interp *interp, struct target *target,
 		int argc, Jim_Obj * const *argv);
@@ -88,6 +88,7 @@ extern struct target_type dragonite_target;
 extern struct target_type xscale_target;
 extern struct target_type cortexm_target;
 extern struct target_type cortexa_target;
+extern struct target_type aarch64_target;
 extern struct target_type cortexr4_target;
 extern struct target_type arm11_target;
 extern struct target_type ls1_sap_target;
@@ -136,7 +137,10 @@ static struct target_type *target_types[] = {
 	&or1k_target,
 	&quark_x10xx_target,
 	&quark_d20xx_target,
-        &riscv_target,
+	&riscv_target,
+#if BUILD_TARGET64
+	&aarch64_target,
+#endif
 	NULL,
 };
 
@@ -285,6 +289,10 @@ const char *target_state_name(struct target *t)
 		LOG_ERROR("Invalid target state: %d", (int)(t->state));
 		cp = "(*BUG*unknown*BUG*)";
 	}
+
+	if (!target_was_examined(t) && t->defer_examine)
+		cp = "examine deferred";
+
 	return cp;
 }
 
@@ -595,7 +603,8 @@ int target_halt(struct target *target)
  * hand the infrastructure for running such helpers might use this
  * procedure but rely on hardware breakpoint to detect termination.)
  */
-int target_resume(struct target *target, int current, uint32_t address, int handle_breakpoints, int debug_execution)
+int target_resume(struct target *target, int current, target_addr_t address,
+		int handle_breakpoints, int debug_execution)
 {
 	int retval;
 
@@ -666,7 +675,7 @@ static int target_process_reset(struct command_context *cmd_ctx, enum target_res
 }
 
 static int identity_virt2phys(struct target *target,
-		uint32_t virtual, uint32_t *physical)
+		target_addr_t virtual, target_addr_t *physical)
 {
 	*physical = virtual;
 	return ERROR_OK;
@@ -733,6 +742,9 @@ int target_examine(void)
 			continue;
 		}
 
+		if (target->defer_examine)
+			continue;
+
 		retval = target_examine_one(target);
 		if (retval != ERROR_OK)
 			return retval;
@@ -1029,7 +1041,7 @@ int target_run_flash_async_algorithm(struct target *target,
 }
 
 int target_read_memory(struct target *target,
-		uint32_t address, uint32_t size, uint32_t count, uint8_t *buffer)
+		target_addr_t address, uint32_t size, uint32_t count, uint8_t *buffer)
 {
 	if (!target_was_examined(target)) {
 		LOG_ERROR("Target not examined yet");
@@ -1043,7 +1055,7 @@ int target_read_memory(struct target *target,
 }
 
 int target_read_phys_memory(struct target *target,
-		uint32_t address, uint32_t size, uint32_t count, uint8_t *buffer)
+		target_addr_t address, uint32_t size, uint32_t count, uint8_t *buffer)
 {
 	if (!target_was_examined(target)) {
 		LOG_ERROR("Target not examined yet");
@@ -1057,7 +1069,7 @@ int target_read_phys_memory(struct target *target,
 }
 
 int target_write_memory(struct target *target,
-		uint32_t address, uint32_t size, uint32_t count, const uint8_t *buffer)
+		target_addr_t address, uint32_t size, uint32_t count, const uint8_t *buffer)
 {
 	if (!target_was_examined(target)) {
 		LOG_ERROR("Target not examined yet");
@@ -1071,7 +1083,7 @@ int target_write_memory(struct target *target,
 }
 
 int target_write_phys_memory(struct target *target,
-		uint32_t address, uint32_t size, uint32_t count, const uint8_t *buffer)
+		target_addr_t address, uint32_t size, uint32_t count, const uint8_t *buffer)
 {
 	if (!target_was_examined(target)) {
 		LOG_ERROR("Target not examined yet");
@@ -1159,7 +1171,7 @@ int target_get_gdb_reg_list(struct target *target,
 	return target->type->get_gdb_reg_list(target, reg_list, reg_list_size, reg_class);
 }
 int target_step(struct target *target,
-		int current, uint32_t address, int handle_breakpoints)
+		int current, target_addr_t address, int handle_breakpoints)
 {
 	return target->type->step(target, current, address, handle_breakpoints);
 }
@@ -1630,7 +1642,7 @@ static void print_wa_layout(struct target *target)
 	struct working_area *c = target->working_areas;
 
 	while (c) {
-		LOG_DEBUG("%c%c 0x%08"PRIx32"-0x%08"PRIx32" (%"PRIu32" bytes)",
+		LOG_DEBUG("%c%c " TARGET_ADDR_FMT "-" TARGET_ADDR_FMT " (%" PRIu32 " bytes)",
 			c->backup ? 'b' : ' ', c->free ? ' ' : '*',
 			c->address, c->address + c->size - 1, c->size);
 		c = c->next;
@@ -1715,7 +1727,7 @@ int target_alloc_working_area_try(struct target *target, uint32_t size, struct w
 		if (!enabled) {
 			if (target->working_area_phys_spec) {
 				LOG_DEBUG("MMU disabled, using physical "
-					"address for working memory 0x%08"PRIx32,
+					"address for working memory " TARGET_ADDR_FMT,
 					target->working_area_phys);
 				target->working_area = target->working_area_phys;
 			} else {
@@ -1726,7 +1738,7 @@ int target_alloc_working_area_try(struct target *target, uint32_t size, struct w
 		} else {
 			if (target->working_area_virt_spec) {
 				LOG_DEBUG("MMU enabled, using virtual "
-					"address for working memory 0x%08"PRIx32,
+					"address for working memory " TARGET_ADDR_FMT,
 					target->working_area_virt);
 				target->working_area = target->working_area_virt;
 			} else {
@@ -1769,7 +1781,8 @@ int target_alloc_working_area_try(struct target *target, uint32_t size, struct w
 	/* Split the working area into the requested size */
 	target_split_working_area(c, size);
 
-	LOG_DEBUG("allocated new working area of %"PRIu32" bytes at address 0x%08"PRIx32, size, c->address);
+	LOG_DEBUG("allocated new working area of %" PRIu32 " bytes at address " TARGET_ADDR_FMT,
+			  size, c->address);
 
 	if (target->backup_working_area) {
 		if (c->backup == NULL) {
@@ -1813,7 +1826,7 @@ static int target_restore_working_area(struct target *target, struct working_are
 	if (target->backup_working_area && area->backup != NULL) {
 		retval = target_write_memory(target, area->address, 4, area->size / 4, area->backup);
 		if (retval != ERROR_OK)
-			LOG_ERROR("failed to restore %"PRIu32" bytes of working area at address 0x%08"PRIx32,
+			LOG_ERROR("failed to restore %" PRIu32 " bytes of working area at address " TARGET_ADDR_FMT,
 					area->size, area->address);
 	}
 
@@ -1837,7 +1850,7 @@ static int target_free_working_area_restore(struct target *target, struct workin
 
 	area->free = true;
 
-	LOG_DEBUG("freed %"PRIu32" bytes of working area at address 0x%08"PRIx32,
+	LOG_DEBUG("freed %" PRIu32 " bytes of working area at address " TARGET_ADDR_FMT,
 			area->size, area->address);
 
 	/* mark user pointer invalid */
@@ -1859,6 +1872,17 @@ int target_free_working_area(struct target *target, struct working_area *area)
 	return target_free_working_area_restore(target, area, 1);
 }
 
+static void target_destroy(struct target *target)
+{
+	if (target->type->deinit_target)
+		target->type->deinit_target(target);
+
+	free(target->type);
+	free(target->trace_info);
+	free(target->cmd_name);
+	free(target);
+}
+
 void target_quit(void)
 {
 	struct target_event_callback *pe = target_event_callbacks;
@@ -1877,11 +1901,15 @@ void target_quit(void)
 	}
 	target_timer_callbacks = NULL;
 
-	for (struct target *target = all_targets;
-	     target; target = target->next) {
-		if (target->type->deinit_target)
-			target->type->deinit_target(target);
+	for (struct target *target = all_targets; target;) {
+		struct target *tmp;
+
+		tmp = target->next;
+		target_destroy(target);
+		target = tmp;
 	}
+
+	all_targets = NULL;
 }
 
 /* free resources and restore memory, if restoring memory fails,
@@ -1939,13 +1967,10 @@ int target_arch_state(struct target *target)
 {
 	int retval;
 	if (target == NULL) {
-		LOG_USER("No target has been configured");
+		LOG_WARNING("No target has been configured");
 		return ERROR_OK;
 	}
 
-	LOG_USER("%s: target state: %s", target_name(target),
-		 target_state_name(target));
-
 	if (target->state != TARGET_HALTED)
 		return ERROR_OK;
 
@@ -2022,10 +2047,10 @@ static int target_profiling_default(struct target *target, uint32_t *samples,
  * mode respectively, otherwise data is handled as quickly as
  * possible
  */
-int target_write_buffer(struct target *target, uint32_t address, uint32_t size, const uint8_t *buffer)
+int target_write_buffer(struct target *target, target_addr_t address, uint32_t size, const uint8_t *buffer)
 {
-	LOG_DEBUG("writing buffer of %i byte at 0x%8.8x",
-			(int)size, (unsigned)address);
+	LOG_DEBUG("writing buffer of %" PRIi32 " byte at " TARGET_ADDR_FMT,
+			  size, address);
 
 	if (!target_was_examined(target)) {
 		LOG_ERROR("Target not examined yet");
@@ -2036,17 +2061,18 @@ int target_write_buffer(struct target *target, uint32_t address, uint32_t size,
 		return ERROR_OK;
 
 	if ((address + size - 1) < address) {
-		/* GDB can request this when e.g. PC is 0xfffffffc*/
-		LOG_ERROR("address + size wrapped(0x%08x, 0x%08x)",
-				  (unsigned)address,
-				  (unsigned)size);
+		/* GDB can request this when e.g. PC is 0xfffffffc */
+		LOG_ERROR("address + size wrapped (" TARGET_ADDR_FMT ", 0x%08" PRIx32 ")",
+				  address,
+				  size);
 		return ERROR_FAIL;
 	}
 
 	return target->type->write_buffer(target, address, size, buffer);
 }
 
-static int target_write_buffer_default(struct target *target, uint32_t address, uint32_t count, const uint8_t *buffer)
+static int target_write_buffer_default(struct target *target,
+	target_addr_t address, uint32_t count, const uint8_t *buffer)
 {
 	uint32_t size;
 
@@ -2083,10 +2109,10 @@ static int target_write_buffer_default(struct target *target, uint32_t address,
  * mode respectively, otherwise data is handled as quickly as
  * possible
  */
-int target_read_buffer(struct target *target, uint32_t address, uint32_t size, uint8_t *buffer)
+int target_read_buffer(struct target *target, target_addr_t address, uint32_t size, uint8_t *buffer)
 {
-	LOG_DEBUG("reading buffer of %i byte at 0x%8.8x",
-			  (int)size, (unsigned)address);
+	LOG_DEBUG("reading buffer of %" PRIi32 " byte at " TARGET_ADDR_FMT,
+			  size, address);
 
 	if (!target_was_examined(target)) {
 		LOG_ERROR("Target not examined yet");
@@ -2097,8 +2123,8 @@ int target_read_buffer(struct target *target, uint32_t address, uint32_t size, u
 		return ERROR_OK;
 
 	if ((address + size - 1) < address) {
-		/* GDB can request this when e.g. PC is 0xfffffffc*/
-		LOG_ERROR("address + size wrapped(0x%08" PRIx32 ", 0x%08" PRIx32 ")",
+		/* GDB can request this when e.g. PC is 0xfffffffc */
+		LOG_ERROR("address + size wrapped (" TARGET_ADDR_FMT ", 0x%08" PRIx32 ")",
 				  address,
 				  size);
 		return ERROR_FAIL;
@@ -2107,7 +2133,7 @@ int target_read_buffer(struct target *target, uint32_t address, uint32_t size, u
 	return target->type->read_buffer(target, address, size, buffer);
 }
 
-static int target_read_buffer_default(struct target *target, uint32_t address, uint32_t count, uint8_t *buffer)
+static int target_read_buffer_default(struct target *target, target_addr_t address, uint32_t count, uint8_t *buffer)
 {
 	uint32_t size;
 
@@ -2140,7 +2166,7 @@ static int target_read_buffer_default(struct target *target, uint32_t address, u
 	return ERROR_OK;
 }
 
-int target_checksum_memory(struct target *target, uint32_t address, uint32_t size, uint32_t* crc)
+int target_checksum_memory(struct target *target, target_addr_t address, uint32_t size, uint32_t* crc)
 {
 	uint8_t *buffer;
 	int retval;
@@ -2155,7 +2181,7 @@ int target_checksum_memory(struct target *target, uint32_t address, uint32_t siz
 	if (retval != ERROR_OK) {
 		buffer = malloc(size);
 		if (buffer == NULL) {
-			LOG_ERROR("error allocating buffer for section (%d bytes)", (int)size);
+			LOG_ERROR("error allocating buffer for section (%" PRId32 " bytes)", size);
 			return ERROR_COMMAND_SYNTAX_ERROR;
 		}
 		retval = target_read_buffer(target, address, size, buffer);
@@ -2180,7 +2206,8 @@ int target_checksum_memory(struct target *target, uint32_t address, uint32_t siz
 	return retval;
 }
 
-int target_blank_check_memory(struct target *target, uint32_t address, uint32_t size, uint32_t* blank)
+int target_blank_check_memory(struct target *target, target_addr_t address, uint32_t size, uint32_t* blank,
+	uint8_t erased_value)
 {
 	int retval;
 	if (!target_was_examined(target)) {
@@ -2191,12 +2218,12 @@ int target_blank_check_memory(struct target *target, uint32_t address, uint32_t
 	if (target->type->blank_check_memory == 0)
 		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 
-	retval = target->type->blank_check_memory(target, address, size, blank);
+	retval = target->type->blank_check_memory(target, address, size, blank, erased_value);
 
 	return retval;
 }
 
-int target_read_u64(struct target *target, uint64_t address, uint64_t *value)
+int target_read_u64(struct target *target, target_addr_t address, uint64_t *value)
 {
 	uint8_t value_buf[8];
 	if (!target_was_examined(target)) {
@@ -2208,19 +2235,19 @@ int target_read_u64(struct target *target, uint64_t address, uint64_t *value)
 
 	if (retval == ERROR_OK) {
 		*value = target_buffer_get_u64(target, value_buf);
-		LOG_DEBUG("address: 0x%" PRIx64 ", value: 0x%16.16" PRIx64 "",
+		LOG_DEBUG("address: " TARGET_ADDR_FMT ", value: 0x%16.16" PRIx64 "",
 				  address,
 				  *value);
 	} else {
 		*value = 0x0;
-		LOG_DEBUG("address: 0x%" PRIx64 " failed",
+		LOG_DEBUG("address: " TARGET_ADDR_FMT " failed",
 				  address);
 	}
 
 	return retval;
 }
 
-int target_read_u32(struct target *target, uint32_t address, uint32_t *value)
+int target_read_u32(struct target *target, target_addr_t address, uint32_t *value)
 {
 	uint8_t value_buf[4];
 	if (!target_was_examined(target)) {
@@ -2232,19 +2259,19 @@ int target_read_u32(struct target *target, uint32_t address, uint32_t *value)
 
 	if (retval == ERROR_OK) {
 		*value = target_buffer_get_u32(target, value_buf);
-		LOG_DEBUG("address: 0x%8.8" PRIx32 ", value: 0x%8.8" PRIx32 "",
+		LOG_DEBUG("address: " TARGET_ADDR_FMT ", value: 0x%8.8" PRIx32 "",
 				  address,
 				  *value);
 	} else {
 		*value = 0x0;
-		LOG_DEBUG("address: 0x%8.8" PRIx32 " failed",
+		LOG_DEBUG("address: " TARGET_ADDR_FMT " failed",
 				  address);
 	}
 
 	return retval;
 }
 
-int target_read_u16(struct target *target, uint32_t address, uint16_t *value)
+int target_read_u16(struct target *target, target_addr_t address, uint16_t *value)
 {
 	uint8_t value_buf[2];
 	if (!target_was_examined(target)) {
@@ -2256,19 +2283,19 @@ int target_read_u16(struct target *target, uint32_t address, uint16_t *value)
 
 	if (retval == ERROR_OK) {
 		*value = target_buffer_get_u16(target, value_buf);
-		LOG_DEBUG("address: 0x%8.8" PRIx32 ", value: 0x%4.4x",
+		LOG_DEBUG("address: " TARGET_ADDR_FMT ", value: 0x%4.4" PRIx16,
 				  address,
 				  *value);
 	} else {
 		*value = 0x0;
-		LOG_DEBUG("address: 0x%8.8" PRIx32 " failed",
+		LOG_DEBUG("address: " TARGET_ADDR_FMT " failed",
 				  address);
 	}
 
 	return retval;
 }
 
-int target_read_u8(struct target *target, uint32_t address, uint8_t *value)
+int target_read_u8(struct target *target, target_addr_t address, uint8_t *value)
 {
 	if (!target_was_examined(target)) {
 		LOG_ERROR("Target not examined yet");
@@ -2278,19 +2305,19 @@ int target_read_u8(struct target *target, uint32_t address, uint8_t *value)
 	int retval = target_read_memory(target, address, 1, 1, value);
 
 	if (retval == ERROR_OK) {
-		LOG_DEBUG("address: 0x%8.8" PRIx32 ", value: 0x%2.2x",
+		LOG_DEBUG("address: " TARGET_ADDR_FMT ", value: 0x%2.2" PRIx8,
 				  address,
 				  *value);
 	} else {
 		*value = 0x0;
-		LOG_DEBUG("address: 0x%8.8" PRIx32 " failed",
+		LOG_DEBUG("address: " TARGET_ADDR_FMT " failed",
 				  address);
 	}
 
 	return retval;
 }
 
-int target_write_u64(struct target *target, uint64_t address, uint64_t value)
+int target_write_u64(struct target *target, target_addr_t address, uint64_t value)
 {
 	int retval;
 	uint8_t value_buf[8];
@@ -2299,7 +2326,7 @@ int target_write_u64(struct target *target, uint64_t address, uint64_t value)
 		return ERROR_FAIL;
 	}
 
-	LOG_DEBUG("address: 0x%" PRIx64 ", value: 0x%16.16" PRIx64 "",
+	LOG_DEBUG("address: " TARGET_ADDR_FMT ", value: 0x%16.16" PRIx64 "",
 			  address,
 			  value);
 
@@ -2311,7 +2338,7 @@ int target_write_u64(struct target *target, uint64_t address, uint64_t value)
 	return retval;
 }
 
-int target_write_u32(struct target *target, uint32_t address, uint32_t value)
+int target_write_u32(struct target *target, target_addr_t address, uint32_t value)
 {
 	int retval;
 	uint8_t value_buf[4];
@@ -2320,7 +2347,7 @@ int target_write_u32(struct target *target, uint32_t address, uint32_t value)
 		return ERROR_FAIL;
 	}
 
-	LOG_DEBUG("address: 0x%8.8" PRIx32 ", value: 0x%8.8" PRIx32 "",
+	LOG_DEBUG("address: " TARGET_ADDR_FMT ", value: 0x%8.8" PRIx32 "",
 			  address,
 			  value);
 
@@ -2332,7 +2359,7 @@ int target_write_u32(struct target *target, uint32_t address, uint32_t value)
 	return retval;
 }
 
-int target_write_u16(struct target *target, uint32_t address, uint16_t value)
+int target_write_u16(struct target *target, target_addr_t address, uint16_t value)
 {
 	int retval;
 	uint8_t value_buf[2];
@@ -2341,7 +2368,7 @@ int target_write_u16(struct target *target, uint32_t address, uint16_t value)
 		return ERROR_FAIL;
 	}
 
-	LOG_DEBUG("address: 0x%8.8" PRIx32 ", value: 0x%8.8x",
+	LOG_DEBUG("address: " TARGET_ADDR_FMT ", value: 0x%8.8" PRIx16,
 			  address,
 			  value);
 
@@ -2353,7 +2380,7 @@ int target_write_u16(struct target *target, uint32_t address, uint16_t value)
 	return retval;
 }
 
-int target_write_u8(struct target *target, uint32_t address, uint8_t value)
+int target_write_u8(struct target *target, target_addr_t address, uint8_t value)
 {
 	int retval;
 	if (!target_was_examined(target)) {
@@ -2361,7 +2388,7 @@ int target_write_u8(struct target *target, uint32_t address, uint8_t value)
 		return ERROR_FAIL;
 	}
 
-	LOG_DEBUG("address: 0x%8.8" PRIx32 ", value: 0x%2.2x",
+	LOG_DEBUG("address: " TARGET_ADDR_FMT ", value: 0x%2.2" PRIx8,
 			  address, value);
 
 	retval = target_write_memory(target, address, 1, 1, &value);
@@ -2371,6 +2398,87 @@ int target_write_u8(struct target *target, uint32_t address, uint8_t value)
 	return retval;
 }
 
+int target_write_phys_u64(struct target *target, target_addr_t address, uint64_t value)
+{
+	int retval;
+	uint8_t value_buf[8];
+	if (!target_was_examined(target)) {
+		LOG_ERROR("Target not examined yet");
+		return ERROR_FAIL;
+	}
+
+	LOG_DEBUG("address: " TARGET_ADDR_FMT ", value: 0x%16.16" PRIx64 "",
+			  address,
+			  value);
+
+	target_buffer_set_u64(target, value_buf, value);
+	retval = target_write_phys_memory(target, address, 8, 1, value_buf);
+	if (retval != ERROR_OK)
+		LOG_DEBUG("failed: %i", retval);
+
+	return retval;
+}
+
+int target_write_phys_u32(struct target *target, target_addr_t address, uint32_t value)
+{
+	int retval;
+	uint8_t value_buf[4];
+	if (!target_was_examined(target)) {
+		LOG_ERROR("Target not examined yet");
+		return ERROR_FAIL;
+	}
+
+	LOG_DEBUG("address: " TARGET_ADDR_FMT ", value: 0x%8.8" PRIx32 "",
+			  address,
+			  value);
+
+	target_buffer_set_u32(target, value_buf, value);
+	retval = target_write_phys_memory(target, address, 4, 1, value_buf);
+	if (retval != ERROR_OK)
+		LOG_DEBUG("failed: %i", retval);
+
+	return retval;
+}
+
+int target_write_phys_u16(struct target *target, target_addr_t address, uint16_t value)
+{
+	int retval;
+	uint8_t value_buf[2];
+	if (!target_was_examined(target)) {
+		LOG_ERROR("Target not examined yet");
+		return ERROR_FAIL;
+	}
+
+	LOG_DEBUG("address: " TARGET_ADDR_FMT ", value: 0x%8.8" PRIx16,
+			  address,
+			  value);
+
+	target_buffer_set_u16(target, value_buf, value);
+	retval = target_write_phys_memory(target, address, 2, 1, value_buf);
+	if (retval != ERROR_OK)
+		LOG_DEBUG("failed: %i", retval);
+
+	return retval;
+}
+
+int target_write_phys_u8(struct target *target, target_addr_t address, uint8_t value)
+{
+	int retval;
+	if (!target_was_examined(target)) {
+		LOG_ERROR("Target not examined yet");
+		return ERROR_FAIL;
+	}
+
+	LOG_DEBUG("address: " TARGET_ADDR_FMT ", value: 0x%2.2" PRIx8,
+			  address, value);
+
+	retval = target_write_phys_memory(target, address, 1, 1, &value);
+	if (retval != ERROR_OK)
+		LOG_DEBUG("failed: %i", retval);
+
+	return retval;
+}
+
 static int find_target(struct command_context *cmd_ctx, const char *name)
 {
 	struct target *target = get_target(name);
@@ -2885,9 +2993,9 @@ COMMAND_HANDLER(handle_resume_command)
 	/* with no CMD_ARGV, resume from current pc, addr = 0,
 	 * with one arguments, addr = CMD_ARGV[0],
 	 * handle breakpoints, not debugging */
-	uint32_t addr = 0;
+	target_addr_t addr = 0;
 	if (CMD_ARGC == 1) {
-		COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], addr);
+		COMMAND_PARSE_ADDRESS(CMD_ARGV[0], addr);
 		current = 0;
 	}
 
@@ -2904,10 +3012,10 @@ COMMAND_HANDLER(handle_step_command)
 	/* with no CMD_ARGV, step from current pc, addr = 0,
 	 * with one argument addr = CMD_ARGV[0],
 	 * handle breakpoints, debugging */
-	uint32_t addr = 0;
+	target_addr_t addr = 0;
 	int current_pc = 1;
 	if (CMD_ARGC == 1) {
-		COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], addr);
+		COMMAND_PARSE_ADDRESS(CMD_ARGV[0], addr);
 		current_pc = 0;
 	}
 
@@ -2917,7 +3025,7 @@ COMMAND_HANDLER(handle_step_command)
 }
 
 static void handle_md_output(struct command_context *cmd_ctx,
-		struct target *target, uint32_t address, unsigned size,
+		struct target *target, target_addr_t address, unsigned size,
 		unsigned count, const uint8_t *buffer)
 {
 	const unsigned line_bytecnt = 32;
@@ -2928,6 +3036,9 @@ static void handle_md_output(struct command_context *cmd_ctx,
 
 	const char *value_fmt;
 	switch (size) {
+	case 8:
+		value_fmt = "%16.16llx ";
+		break;
 	case 4:
 		value_fmt = "%8.8x ";
 		break;
@@ -2947,13 +3058,16 @@ static void handle_md_output(struct command_context *cmd_ctx,
 		if (i % line_modulo == 0) {
 			output_len += snprintf(output + output_len,
 					sizeof(output) - output_len,
-					"0x%8.8x: ",
-					(unsigned)(address + (i*size)));
+					TARGET_ADDR_FMT ": ",
+					(address + (i * size)));
 		}
 
-		uint32_t value = 0;
+		uint64_t value = 0;
 		const uint8_t *value_ptr = buffer + i * size;
 		switch (size) {
+		case 8:
+			value = target_buffer_get_u64(target, value_ptr);
+			break;
 		case 4:
 			value = target_buffer_get_u32(target, value_ptr);
 			break;
@@ -2981,6 +3095,9 @@ COMMAND_HANDLER(handle_md_command)
 
 	unsigned size = 0;
 	switch (CMD_NAME[2]) {
+	case 'd':
+		size = 8;
+		break;
 	case 'w':
 		size = 4;
 		break;
@@ -2996,7 +3113,7 @@ COMMAND_HANDLER(handle_md_command)
 
 	bool physical = strcmp(CMD_ARGV[0], "phys") == 0;
 	int (*fn)(struct target *target,
-			uint32_t address, uint32_t size_value, uint32_t count, uint8_t *buffer);
+			target_addr_t address, uint32_t size_value, uint32_t count, uint8_t *buffer);
 	if (physical) {
 		CMD_ARGC--;
 		CMD_ARGV++;
@@ -3006,8 +3123,8 @@ COMMAND_HANDLER(handle_md_command)
 	if ((CMD_ARGC < 1) || (CMD_ARGC > 2))
 		return ERROR_COMMAND_SYNTAX_ERROR;
 
-	uint32_t address;
-	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], address);
+	target_addr_t address;
+	COMMAND_PARSE_ADDRESS(CMD_ARGV[0], address);
 
 	unsigned count = 1;
 	if (CMD_ARGC == 2)
@@ -3026,14 +3143,14 @@ COMMAND_HANDLER(handle_md_command)
 }
 
 typedef int (*target_write_fn)(struct target *target,
-		uint32_t address, uint32_t size, uint32_t count, const uint8_t *buffer);
+		target_addr_t address, uint32_t size, uint32_t count, const uint8_t *buffer);
 
 static int target_fill_mem(struct target *target,
-		uint32_t address,
+		target_addr_t address,
 		target_write_fn fn,
 		unsigned data_size,
 		/* value */
-		uint32_t b,
+		uint64_t b,
 		/* count */
 		unsigned c)
 {
@@ -3048,6 +3165,9 @@ static int target_fill_mem(struct target *target,
 
 	for (unsigned i = 0; i < chunk_size; i++) {
 		switch (data_size) {
+		case 8:
+			target_buffer_set_u64(target, target_buf + i * data_size, b);
+			break;
 		case 4:
 			target_buffer_set_u32(target, target_buf + i * data_size, b);
 			break;
@@ -3096,11 +3216,11 @@ COMMAND_HANDLER(handle_mw_command)
 	if ((CMD_ARGC < 2) || (CMD_ARGC > 3))
 		return ERROR_COMMAND_SYNTAX_ERROR;
 
-	uint32_t address;
-	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], address);
+	target_addr_t address;
+	COMMAND_PARSE_ADDRESS(CMD_ARGV[0], address);
 
-	uint32_t value;
-	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], value);
+	target_addr_t value;
+	COMMAND_PARSE_ADDRESS(CMD_ARGV[1], value);
 
 	unsigned count = 1;
 	if (CMD_ARGC == 3)
@@ -3109,6 +3229,9 @@ COMMAND_HANDLER(handle_mw_command)
 	struct target *target = get_current_target(CMD_CTX);
 	unsigned wordsize;
 	switch (CMD_NAME[2]) {
+		case 'd':
+			wordsize = 8;
+			break;
 		case 'w':
 			wordsize = 4;
 			break;
@@ -3126,7 +3249,7 @@ COMMAND_HANDLER(handle_mw_command)
 }
 
 static COMMAND_HELPER(parse_load_image_command_CMD_ARGV, struct image *image,
-		uint32_t *min_address, uint32_t *max_address)
+		target_addr_t *min_address, target_addr_t *max_address)
 {
 	if (CMD_ARGC < 1 || CMD_ARGC > 5)
 		return ERROR_COMMAND_SYNTAX_ERROR;
@@ -3134,8 +3257,8 @@ static COMMAND_HELPER(parse_load_image_command_CMD_ARGV, struct image *image,
 	/* a base address isn't always necessary,
 	 * default to 0x0 (i.e. don't relocate) */
 	if (CMD_ARGC >= 2) {
-		uint32_t addr;
-		COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], addr);
+		target_addr_t addr;
+		COMMAND_PARSE_ADDRESS(CMD_ARGV[1], addr);
 		image->base_address = addr;
 		image->base_address_set = 1;
 	} else
@@ -3144,9 +3267,9 @@ static COMMAND_HELPER(parse_load_image_command_CMD_ARGV, struct image *image,
 	image->start_address_set = 0;
 
 	if (CMD_ARGC >= 4)
-		COMMAND_PARSE_NUMBER(u32, CMD_ARGV[3], *min_address);
+		COMMAND_PARSE_ADDRESS(CMD_ARGV[3], *min_address);
 	if (CMD_ARGC == 5) {
-		COMMAND_PARSE_NUMBER(u32, CMD_ARGV[4], *max_address);
+		COMMAND_PARSE_ADDRESS(CMD_ARGV[4], *max_address);
 		/* use size (given) to find max (required) */
 		*max_address += *min_address;
 	}
@@ -3162,8 +3285,8 @@ COMMAND_HANDLER(handle_load_image_command)
 	uint8_t *buffer;
 	size_t buf_cnt;
 	uint32_t image_size;
-	uint32_t min_address = 0;
-	uint32_t max_address = 0xffffffff;
+	target_addr_t min_address = 0;
+	target_addr_t max_address = -1;
 	int i;
 	struct image image;
 
@@ -3178,7 +3301,7 @@ COMMAND_HANDLER(handle_load_image_command)
 	duration_start(&bench);
 
 	if (image_open(&image, CMD_ARGV[0], (CMD_ARGC >= 3) ? CMD_ARGV[2] : NULL) != ERROR_OK)
-		return ERROR_OK;
+		return ERROR_FAIL;
 
 	image_size = 0x0;
 	retval = ERROR_OK;
@@ -3188,6 +3311,7 @@ COMMAND_HANDLER(handle_load_image_command)
 			command_print(CMD_CTX,
 						  "error allocating buffer for section (%d bytes)",
 						  (int)(image.sections[i].size));
+			retval = ERROR_FAIL;
 			break;
 		}
 
@@ -3221,7 +3345,7 @@ COMMAND_HANDLER(handle_load_image_command)
 				break;
 			}
 			image_size += length;
-			command_print(CMD_CTX, "%u bytes written at address 0x%8.8" PRIx32 "",
+			command_print(CMD_CTX, "%u bytes written at address " TARGET_ADDR_FMT "",
 					(unsigned int)length,
 					image.sections[i].base_address + offset);
 		}
@@ -3246,15 +3370,15 @@ COMMAND_HANDLER(handle_dump_image_command)
 	struct fileio *fileio;
 	uint8_t *buffer;
 	int retval, retvaltemp;
-	uint32_t address, size;
+	target_addr_t address, size;
 	struct duration bench;
 	struct target *target = get_current_target(CMD_CTX);
 
 	if (CMD_ARGC != 3)
 		return ERROR_COMMAND_SYNTAX_ERROR;
 
-	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], address);
-	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[2], size);
+	COMMAND_PARSE_ADDRESS(CMD_ARGV[1], address);
+	COMMAND_PARSE_ADDRESS(CMD_ARGV[2], size);
 
 	uint32_t buf_size = (size > 4096) ? 4096 : size;
 	buffer = malloc(buf_size);
@@ -3303,7 +3427,13 @@ COMMAND_HANDLER(handle_dump_image_command)
 	return retval;
 }
 
-static COMMAND_HELPER(handle_verify_image_command_internal, int verify)
+enum verify_mode {
+	IMAGE_TEST = 0,
+	IMAGE_VERIFY = 1,
+	IMAGE_CHECKSUM_ONLY = 2
+};
+
+static COMMAND_HELPER(handle_verify_image_command_internal, enum verify_mode verify)
 {
 	uint8_t *buffer;
 	size_t buf_cnt;
@@ -3329,8 +3459,8 @@ static COMMAND_HELPER(handle_verify_image_command_internal, int verify)
 	duration_start(&bench);
 
 	if (CMD_ARGC >= 2) {
-		uint32_t addr;
-		COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], addr);
+		target_addr_t addr;
+		COMMAND_PARSE_ADDRESS(CMD_ARGV[1], addr);
 		image.base_address = addr;
 		image.base_address_set = 1;
 	} else {
@@ -3361,7 +3491,7 @@ static COMMAND_HELPER(handle_verify_image_command_internal, int verify)
 			break;
 		}
 
-		if (verify) {
+		if (verify >= IMAGE_VERIFY) {
 			/* calculate checksum of image */
 			retval = image_calculate_checksum(buffer, buf_cnt, &checksum);
 			if (retval != ERROR_OK) {
@@ -3374,7 +3504,12 @@ static COMMAND_HELPER(handle_verify_image_command_internal, int verify)
 				free(buffer);
 				break;
 			}
-
+			if ((checksum != mem_checksum) && (verify == IMAGE_CHECKSUM_ONLY)) {
+				LOG_ERROR("checksum mismatch");
+				free(buffer);
+				retval = ERROR_FAIL;
+				goto done;
+			}
 			if (checksum != mem_checksum) {
 				/* failed crc checksum, fall back to a binary compare */
 				uint8_t *data;
@@ -3415,7 +3550,7 @@ static COMMAND_HELPER(handle_verify_image_command_internal, int verify)
 				free(data);
 			}
 		} else {
-			command_print(CMD_CTX, "address 0x%08" PRIx32 " length 0x%08zx",
+			command_print(CMD_CTX, "address " TARGET_ADDR_FMT " length 0x%08zx",
 						  image.sections[i].base_address,
 						  buf_cnt);
 		}
@@ -3439,14 +3574,19 @@ done:
 	return retval;
 }
 
+COMMAND_HANDLER(handle_verify_image_checksum_command)
+{
+	return CALL_COMMAND_HANDLER(handle_verify_image_command_internal, IMAGE_CHECKSUM_ONLY);
+}
+
 COMMAND_HANDLER(handle_verify_image_command)
 {
-	return CALL_COMMAND_HANDLER(handle_verify_image_command_internal, 1);
+	return CALL_COMMAND_HANDLER(handle_verify_image_command_internal, IMAGE_VERIFY);
 }
 
 COMMAND_HANDLER(handle_test_image_command)
 {
-	return CALL_COMMAND_HANDLER(handle_verify_image_command_internal, 0);
+	return CALL_COMMAND_HANDLER(handle_verify_image_command_internal, IMAGE_TEST);
 }
 
 static int handle_bp_command_list(struct command_context *cmd_ctx)
@@ -3457,7 +3597,7 @@ static int handle_bp_command_list(struct command_context *cmd_ctx)
 		if (breakpoint->type == BKPT_SOFT) {
 			char *buf = buf_to_str(breakpoint->orig_instr,
 					breakpoint->length, 16);
-			command_print(cmd_ctx, "IVA breakpoint: 0x%8.8" PRIx32 ", 0x%x, %i, 0x%s",
+			command_print(cmd_ctx, "IVA breakpoint: " TARGET_ADDR_FMT ", 0x%x, %i, 0x%s",
 					breakpoint->address,
 					breakpoint->length,
 					breakpoint->set, buf);
@@ -3468,13 +3608,13 @@ static int handle_bp_command_list(struct command_context *cmd_ctx)
 							breakpoint->asid,
 							breakpoint->length, breakpoint->set);
 			else if ((breakpoint->address != 0) && (breakpoint->asid != 0)) {
-				command_print(cmd_ctx, "Hybrid breakpoint(IVA): 0x%8.8" PRIx32 ", 0x%x, %i",
+				command_print(cmd_ctx, "Hybrid breakpoint(IVA): " TARGET_ADDR_FMT ", 0x%x, %i",
 							breakpoint->address,
 							breakpoint->length, breakpoint->set);
 				command_print(cmd_ctx, "\t|--->linked with ContextID: 0x%8.8" PRIx32,
 							breakpoint->asid);
 			} else
-				command_print(cmd_ctx, "Breakpoint(IVA): 0x%8.8" PRIx32 ", 0x%x, %i",
+				command_print(cmd_ctx, "Breakpoint(IVA): " TARGET_ADDR_FMT ", 0x%x, %i",
 							breakpoint->address,
 							breakpoint->length, breakpoint->set);
 		}
@@ -3485,7 +3625,7 @@ static int handle_bp_command_list(struct command_context *cmd_ctx)
 }
 
 static int handle_bp_command_set(struct command_context *cmd_ctx,
-		uint32_t addr, uint32_t asid, uint32_t length, int hw)
+		target_addr_t addr, uint32_t asid, uint32_t length, int hw)
 {
 	struct target *target = get_current_target(cmd_ctx);
 	int retval;
@@ -3493,7 +3633,7 @@ static int handle_bp_command_set(struct command_context *cmd_ctx,
 	if (asid == 0) {
 		retval = breakpoint_add(target, addr, length, hw);
 		if (ERROR_OK == retval)
-			command_print(cmd_ctx, "breakpoint set at 0x%8.8" PRIx32 "", addr);
+			command_print(cmd_ctx, "breakpoint set at " TARGET_ADDR_FMT "", addr);
 		else {
 			LOG_ERROR("Failure setting breakpoint, the same address(IVA) is already used");
 			return retval;
@@ -3528,7 +3668,7 @@ static int handle_bp_command_set(struct command_context *cmd_ctx,
 
 COMMAND_HANDLER(handle_bp_command)
 {
-	uint32_t addr;
+	target_addr_t addr;
 	uint32_t asid;
 	uint32_t length;
 	int hw = BKPT_SOFT;
@@ -3539,17 +3679,15 @@ COMMAND_HANDLER(handle_bp_command)
 
 		case 2:
 			asid = 0;
-			COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], addr);
+			COMMAND_PARSE_ADDRESS(CMD_ARGV[0], addr);
 			COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], length);
 			return handle_bp_command_set(CMD_CTX, addr, asid, length, hw);
 
 		case 3:
 			if (strcmp(CMD_ARGV[2], "hw") == 0) {
 				hw = BKPT_HARD;
-				COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], addr);
-
+				COMMAND_PARSE_ADDRESS(CMD_ARGV[0], addr);
 				COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], length);
-
 				asid = 0;
 				return handle_bp_command_set(CMD_CTX, addr, asid, length, hw);
 			} else if (strcmp(CMD_ARGV[2], "hw_ctx") == 0) {
@@ -3562,7 +3700,7 @@ COMMAND_HANDLER(handle_bp_command)
 
 		case 4:
 			hw = BKPT_HARD;
-			COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], addr);
+			COMMAND_PARSE_ADDRESS(CMD_ARGV[0], addr);
 			COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], asid);
 			COMMAND_PARSE_NUMBER(u32, CMD_ARGV[2], length);
 			return handle_bp_command_set(CMD_CTX, addr, asid, length, hw);
@@ -3577,8 +3715,8 @@ COMMAND_HANDLER(handle_rbp_command)
 	if (CMD_ARGC != 1)
 		return ERROR_COMMAND_SYNTAX_ERROR;
 
-	uint32_t addr;
-	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], addr);
+	target_addr_t addr;
+	COMMAND_PARSE_ADDRESS(CMD_ARGV[0], addr);
 
 	struct target *target = get_current_target(CMD_CTX);
 	breakpoint_remove(target, addr);
@@ -3594,7 +3732,7 @@ COMMAND_HANDLER(handle_wp_command)
 		struct watchpoint *watchpoint = target->watchpoints;
 
 		while (watchpoint) {
-			command_print(CMD_CTX, "address: 0x%8.8" PRIx32
+			command_print(CMD_CTX, "address: " TARGET_ADDR_FMT
 					", len: 0x%8.8" PRIx32
 					", r/w/a: %i, value: 0x%8.8" PRIx32
 					", mask: 0x%8.8" PRIx32,
@@ -3679,14 +3817,14 @@ COMMAND_HANDLER(handle_virt2phys_command)
 	if (CMD_ARGC != 1)
 		return ERROR_COMMAND_SYNTAX_ERROR;
 
-	uint32_t va;
-	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], va);
-	uint32_t pa;
+	target_addr_t va;
+	COMMAND_PARSE_ADDRESS(CMD_ARGV[0], va);
+	target_addr_t pa;
 
 	struct target *target = get_current_target(CMD_CTX);
 	int retval = target->type->virt2phys(target, va, &pa);
 	if (retval == ERROR_OK)
-		command_print(CMD_CTX, "Physical address 0x%08" PRIx32 "", pa);
+		command_print(CMD_CTX, "Physical address " TARGET_ADDR_FMT "", pa);
 
 	return retval;
 }
@@ -4015,7 +4153,7 @@ static int target_mem2array(Jim_Interp *interp, struct target *target, int argc,
 		sprintf(buf, "mem2array address: 0x%08" PRIx32 " is not aligned for %" PRId32 " byte reads",
 				addr,
 				width);
-		Jim_AppendStrings(interp, Jim_GetResult(interp), buf , NULL);
+		Jim_AppendStrings(interp, Jim_GetResult(interp), buf, NULL);
 		return JIM_ERR;
 	}
 
@@ -4044,10 +4182,10 @@ static int target_mem2array(Jim_Interp *interp, struct target *target, int argc,
 			retval = target_read_memory(target, addr, width, count, buffer);
 		if (retval != ERROR_OK) {
 			/* BOO !*/
-			LOG_ERROR("mem2array: Read @ 0x%08x, w=%d, cnt=%d, failed",
-					  (unsigned int)addr,
-					  (int)width,
-					  (int)count);
+			LOG_ERROR("mem2array: Read @ 0x%08" PRIx32 ", w=%" PRId32 ", cnt=%" PRId32 ", failed",
+					  addr,
+					  width,
+					  count);
 			Jim_SetResult(interp, Jim_NewEmptyStringObj(interp));
 			Jim_AppendStrings(interp, Jim_GetResult(interp), "mem2array: cannot read memory", NULL);
 			e = JIM_ERR;
@@ -4218,10 +4356,10 @@ static int target_array2mem(Jim_Interp *interp, struct target *target,
 	} else {
 		char buf[100];
 		Jim_SetResult(interp, Jim_NewEmptyStringObj(interp));
-		sprintf(buf, "array2mem address: 0x%08x is not aligned for %d byte reads",
-				(unsigned int)addr,
-				(int)width);
-		Jim_AppendStrings(interp, Jim_GetResult(interp), buf , NULL);
+		sprintf(buf, "array2mem address: 0x%08" PRIx32 " is not aligned for %" PRId32 " byte reads",
+				addr,
+				width);
+		Jim_AppendStrings(interp, Jim_GetResult(interp), buf, NULL);
 		return JIM_ERR;
 	}
 
@@ -4267,10 +4405,10 @@ static int target_array2mem(Jim_Interp *interp, struct target *target,
 			retval = target_write_memory(target, addr, width, count, buffer);
 		if (retval != ERROR_OK) {
 			/* BOO !*/
-			LOG_ERROR("array2mem: Write @ 0x%08x, w=%d, cnt=%d, failed",
-					  (unsigned int)addr,
-					  (int)width,
-					  (int)count);
+			LOG_ERROR("array2mem: Write @ 0x%08" PRIx32 ", w=%" PRId32 ", cnt=%" PRId32 ", failed",
+					  addr,
+					  width,
+					  count);
 			Jim_SetResult(interp, Jim_NewEmptyStringObj(interp));
 			Jim_AppendStrings(interp, Jim_GetResult(interp), "array2mem: cannot read memory", NULL);
 			e = JIM_ERR;
@@ -4335,7 +4473,9 @@ enum target_cfg_param {
 	TCFG_COREID,
 	TCFG_CHAIN_POSITION,
 	TCFG_DBGBASE,
+	TCFG_CTIBASE,
 	TCFG_RTOS,
+	TCFG_DEFER_EXAMINE,
 };
 
 static Jim_Nvp nvp_config_opts[] = {
@@ -4349,7 +4489,9 @@ static Jim_Nvp nvp_config_opts[] = {
 	{ .name = "-coreid",           .value = TCFG_COREID },
 	{ .name = "-chain-position",   .value = TCFG_CHAIN_POSITION },
 	{ .name = "-dbgbase",          .value = TCFG_DBGBASE },
+	{ .name = "-ctibase",          .value = TCFG_CTIBASE },
 	{ .name = "-rtos",             .value = TCFG_RTOS },
+	{ .name = "-defer-examine",    .value = TCFG_DEFER_EXAMINE },
 	{ .name = NULL, .value = -1 }
 };
 
@@ -4614,7 +4756,20 @@ no_params:
 			Jim_SetResult(goi->interp, Jim_NewIntObj(goi->interp, target->dbgbase));
 			/* loop for more */
 			break;
-
+		case TCFG_CTIBASE:
+			if (goi->isconfigure) {
+				e = Jim_GetOpt_Wide(goi, &w);
+				if (e != JIM_OK)
+					return e;
+				target->ctibase = (uint32_t)w;
+				target->ctibase_set = true;
+			} else {
+				if (goi->argc != 0)
+					goto no_params;
+			}
+			Jim_SetResult(goi->interp, Jim_NewIntObj(goi->interp, target->ctibase));
+			/* loop for more */
+			break;
 		case TCFG_RTOS:
 			/* RTOS */
 			{
@@ -4624,6 +4779,13 @@ no_params:
 			}
 			/* loop for more */
 			break;
+
+		case TCFG_DEFER_EXAMINE:
+			/* DEFER_EXAMINE */
+			target->defer_examine = true;
+			/* loop for more */
+			break;
+
 		}
 	} /* while (goi->argc) */
 
@@ -4638,12 +4800,9 @@ static int jim_target_configure(Jim_Interp *interp, int argc, Jim_Obj * const *a
 
 	Jim_GetOpt_Setup(&goi, interp, argc - 1, argv + 1);
 	goi.isconfigure = !strcmp(Jim_GetString(argv[0], NULL), "configure");
-	int need_args = 1 + goi.isconfigure;
-	if (goi.argc < need_args) {
+	if (goi.argc < 1) {
 		Jim_WrongNumArgs(goi.interp, goi.argc, goi.argv,
-			goi.isconfigure
-				? "missing: -option VALUE ..."
-				: "missing: -option ...");
+				 "missing: -option ...");
 		return JIM_ERR;
 	}
 	struct target *target = Jim_CmdPrivData(goi.interp);
@@ -4756,7 +4915,7 @@ static int jim_target_md(Jim_Interp *interp, int argc, Jim_Obj *const *argv)
 	}
 
 	int (*fn)(struct target *target,
-			uint32_t address, uint32_t size, uint32_t count, uint8_t *buffer);
+			target_addr_t address, uint32_t size, uint32_t count, uint8_t *buffer);
 	fn = target_read_memory;
 
 	int e;
@@ -4894,20 +5053,58 @@ static int jim_target_tap_disabled(Jim_Interp *interp)
 
 static int jim_target_examine(Jim_Interp *interp, int argc, Jim_Obj *const *argv)
 {
-	if (argc != 1) {
-		Jim_WrongNumArgs(interp, 1, argv, "[no parameters]");
+	bool allow_defer = false;
+
+	Jim_GetOptInfo goi;
+	Jim_GetOpt_Setup(&goi, interp, argc - 1, argv + 1);
+	if (goi.argc > 1) {
+		const char *cmd_name = Jim_GetString(argv[0], NULL);
+		Jim_SetResultFormatted(goi.interp,
+				"usage: %s ['allow-defer']", cmd_name);
 		return JIM_ERR;
 	}
+	if (goi.argc > 0 &&
+	    strcmp(Jim_GetString(argv[1], NULL), "allow-defer") == 0) {
+		/* consume it */
+		struct Jim_Obj *obj;
+		int e = Jim_GetOpt_Obj(&goi, &obj);
+		if (e != JIM_OK)
+			return e;
+		allow_defer = true;
+	}
+
 	struct target *target = Jim_CmdPrivData(interp);
 	if (!target->tap->enabled)
 		return jim_target_tap_disabled(interp);
 
+	if (allow_defer && target->defer_examine) {
+		LOG_INFO("Deferring arp_examine of %s", target_name(target));
+		LOG_INFO("Use arp_examine command to examine it manually!");
+		return JIM_OK;
+	}
+
 	int e = target->type->examine(target);
 	if (e != ERROR_OK)
 		return JIM_ERR;
 	return JIM_OK;
 }
 
+static int jim_target_was_examined(Jim_Interp *interp, int argc, Jim_Obj * const *argv)
+{
+	struct target *target = Jim_CmdPrivData(interp);
+
+	Jim_SetResultBool(interp, target_was_examined(target));
+	return JIM_OK;
+}
+
+static int jim_target_examine_deferred(Jim_Interp *interp, int argc, Jim_Obj * const *argv)
+{
+	struct target *target = Jim_CmdPrivData(interp);
+
+	Jim_SetResultBool(interp, target->defer_examine);
+	return JIM_OK;
+}
+
 static int jim_target_halt_gdb(Jim_Interp *interp, int argc, Jim_Obj *const *argv)
 {
 	if (argc != 1) {
@@ -4975,6 +5172,10 @@ static int jim_target_reset(Jim_Interp *interp, int argc, Jim_Obj *const *argv)
 				target_name(target));
 		return JIM_ERR;
 	}
+
+	if (target->defer_examine)
+		target_reset_examined(target);
+
 	/* determine if we should halt or not. */
 	target->reset_halt = !!a;
 	/* When this happens - all workareas are invalid. */
@@ -5185,6 +5386,21 @@ static const struct command_registration target_instance_command_handlers[] = {
 		.mode = COMMAND_EXEC,
 		.jim_handler = jim_target_examine,
 		.help = "used internally for reset processing",
+		.usage = "arp_examine ['allow-defer']",
+	},
+	{
+		.name = "was_examined",
+		.mode = COMMAND_EXEC,
+		.jim_handler = jim_target_was_examined,
+		.help = "used internally for reset processing",
+		.usage = "was_examined",
+	},
+	{
+		.name = "examine_deferred",
+		.mode = COMMAND_EXEC,
+		.jim_handler = jim_target_examine_deferred,
+		.help = "used internally for reset processing",
+		.usage = "examine_deferred",
 	},
 	{
 		.name = "arp_halt_gdb",
@@ -5336,14 +5552,7 @@ static int target_create(Jim_GetOptInfo *goi)
 	target->halt_issued			= false;
 
 	/* initialize trace information */
-	target->trace_info = malloc(sizeof(struct trace));
-	target->trace_info->num_trace_points         = 0;
-	target->trace_info->trace_points_size        = 0;
-	target->trace_info->trace_points             = NULL;
-	target->trace_info->trace_history_size       = 0;
-	target->trace_info->trace_history            = NULL;
-	target->trace_info->trace_history_pos        = 0;
-	target->trace_info->trace_history_overflowed = 0;
+	target->trace_info = calloc(1, sizeof(struct trace));
 
 	target->dbgmsg          = NULL;
 	target->dbg_msg_enabled = 0;
@@ -5578,7 +5787,7 @@ static const struct command_registration target_subcommand_handlers[] = {
 };
 
 struct FastLoad {
-	uint32_t address;
+	target_addr_t address;
 	uint8_t *data;
 	int length;
 
@@ -5605,8 +5814,8 @@ COMMAND_HANDLER(handle_fast_load_image_command)
 	uint8_t *buffer;
 	size_t buf_cnt;
 	uint32_t image_size;
-	uint32_t min_address = 0;
-	uint32_t max_address = 0xffffffff;
+	target_addr_t min_address = 0;
+	target_addr_t max_address = -1;
 	int i;
 
 	struct image image;
@@ -6077,6 +6286,13 @@ static const struct command_registration target_exec_command_handlers[] = {
 		.usage = "[address]",
 	},
 	{
+		.name = "mdd",
+		.handler = handle_md_command,
+		.mode = COMMAND_EXEC,
+		.help = "display memory words",
+		.usage = "['phys'] address [count]",
+	},
+	{
 		.name = "mdw",
 		.handler = handle_md_command,
 		.mode = COMMAND_EXEC,
@@ -6098,6 +6314,13 @@ static const struct command_registration target_exec_command_handlers[] = {
 		.usage = "['phys'] address [count]",
 	},
 	{
+		.name = "mwd",
+		.handler = handle_mw_command,
+		.mode = COMMAND_EXEC,
+		.help = "write memory word",
+		.usage = "['phys'] address value [count]",
+	},
+	{
 		.name = "mww",
 		.handler = handle_mw_command,
 		.mode = COMMAND_EXEC,
@@ -6160,6 +6383,12 @@ static const struct command_registration target_exec_command_handlers[] = {
 		.usage = "filename address size",
 	},
 	{
+		.name = "verify_image_checksum",
+		.handler = handle_verify_image_checksum_command,
+		.mode = COMMAND_EXEC,
+		.usage = "filename [offset [type]]",
+	},
+	{
 		.name = "verify_image",
 		.handler = handle_verify_image_command,
 		.mode = COMMAND_EXEC,
diff --git a/src/target/target.h b/src/target/target.h
index 0cee117..53f9e26 100644
--- a/src/target/target.h
+++ b/src/target/target.h
@@ -93,7 +93,7 @@ enum target_endianness {
 };
 
 struct working_area {
-	uint32_t address;
+	target_addr_t address;
 	uint32_t size;
 	bool free;
 	uint8_t *backup;
@@ -125,11 +125,14 @@ enum target_register_class {
 /* target_type.h contains the full definition of struct target_type */
 struct target {
 	struct target_type *type;			/* target type definition (name, access functions) */
-	const char *cmd_name;				/* tcl Name of target */
+	char *cmd_name;				/* tcl Name of target */
 	int target_number;					/* DO NOT USE!  field to be removed in 2010 */
 	struct jtag_tap *tap;				/* where on the jtag chain is this */
 	int32_t coreid;						/* which device on the TAP? */
 
+	/** Should we defer examine to later */
+	bool defer_examine;
+
 	/**
 	 * Indicates whether this target has been examined.
 	 *
@@ -153,9 +156,9 @@ struct target {
 	uint32_t working_area;				/* working area (initialised RAM). Evaluated
 										 * upon first allocation from virtual/physical address. */
 	bool working_area_virt_spec;		/* virtual address specified? */
-	uint32_t working_area_virt;			/* virtual address */
-	bool working_area_phys_spec;		/* virtual address specified? */
-	uint32_t working_area_phys;			/* physical address */
+	target_addr_t working_area_virt;			/* virtual address */
+	bool working_area_phys_spec;		/* physical address specified? */
+	target_addr_t working_area_phys;			/* physical address */
 	uint32_t working_area_size;			/* size in bytes */
 	uint32_t backup_working_area;		/* whether the content of the working area has to be preserved */
 	struct working_area *working_areas;/* list of allocated working areas */
@@ -170,6 +173,7 @@ struct target {
 	struct debug_msg_receiver *dbgmsg;	/* list of debug message receivers */
 	uint32_t dbg_msg_enabled;			/* debug message status */
 	void *arch_info;					/* architecture specific information */
+	void *private_config;				/* pointer to target specific config data (for jim_configure hook) */
 	struct target *next;				/* next target in list */
 
 	int display;						/* display async info in telnet session. Do not display
@@ -181,6 +185,11 @@ struct target {
 	uint32_t dbgbase;					/* Really a Cortex-A specific option, but there is no
 										 * system in place to support target specific options
 										 * currently. */
+
+	 bool ctibase_set;					 /* By default the debug base is not set */
+	 uint32_t ctibase;					 /* Really a Cortex-A specific option, but there is no
+										  * system in place to support target specific options
+										  * currently. */
 	struct rtos *rtos;					/* Instance of Real Time Operating System support */
 	bool rtos_auto_detect;				/* A flag that indicates that the RTOS has been specified as "auto"
 										 * and must be detected when symbols are offered */
@@ -349,7 +358,7 @@ int target_unregister_trace_callback(
  * yet it is possible to detect error conditions.
  */
 int target_poll(struct target *target);
-int target_resume(struct target *target, int current, uint32_t address,
+int target_resume(struct target *target, int current, target_addr_t address,
 		int handle_breakpoints, int debug_execution);
 int target_halt(struct target *target);
 int target_call_event_callbacks(struct target *target, enum target_event event);
@@ -470,7 +479,7 @@ int target_get_gdb_reg_list(struct target *target,
  * This routine is a wrapper for target->type->step.
  */
 int target_step(struct target *target,
-		int current, uint32_t address, int handle_breakpoints);
+		int current, target_addr_t address, int handle_breakpoints);
 /**
  * Run an algorithm on the @a target given.
  *
@@ -523,9 +532,9 @@ int target_run_flash_async_algorithm(struct target *target,
  * This routine is a wrapper for target->type->read_memory.
  */
 int target_read_memory(struct target *target,
-		uint32_t address, uint32_t size, uint32_t count, uint8_t *buffer);
+		target_addr_t address, uint32_t size, uint32_t count, uint8_t *buffer);
 int target_read_phys_memory(struct target *target,
-		uint32_t address, uint32_t size, uint32_t count, uint8_t *buffer);
+		target_addr_t address, uint32_t size, uint32_t count, uint8_t *buffer);
 /**
  * Write @a count items of @a size bytes to the memory of @a target at
  * the @a address given. @a address must be aligned to @a size
@@ -544,9 +553,9 @@ int target_read_phys_memory(struct target *target,
  * This routine is wrapper for target->type->write_memory.
  */
 int target_write_memory(struct target *target,
-		uint32_t address, uint32_t size, uint32_t count, const uint8_t *buffer);
+		target_addr_t address, uint32_t size, uint32_t count, const uint8_t *buffer);
 int target_write_phys_memory(struct target *target,
-		uint32_t address, uint32_t size, uint32_t count, const uint8_t *buffer);
+		target_addr_t address, uint32_t size, uint32_t count, const uint8_t *buffer);
 
 /*
  * Write to target memory using the virtual address.
@@ -573,13 +582,13 @@ int target_write_phys_memory(struct target *target,
  * peripheral registers which do not support byte operations.
  */
 int target_write_buffer(struct target *target,
-		uint32_t address, uint32_t size, const uint8_t *buffer);
+		target_addr_t address, uint32_t size, const uint8_t *buffer);
 int target_read_buffer(struct target *target,
-		uint32_t address, uint32_t size, uint8_t *buffer);
+		target_addr_t address, uint32_t size, uint8_t *buffer);
 int target_checksum_memory(struct target *target,
-		uint32_t address, uint32_t size, uint32_t *crc);
+		target_addr_t address, uint32_t size, uint32_t *crc);
 int target_blank_check_memory(struct target *target,
-		uint32_t address, uint32_t size, uint32_t *blank);
+		target_addr_t address, uint32_t size, uint32_t *blank, uint8_t erased_value);
 int target_wait_state(struct target *target, enum target_state state, int ms);
 
 /**
@@ -655,14 +664,19 @@ void target_buffer_set_u64_array(struct target *target, uint8_t *buffer, uint32_
 void target_buffer_set_u32_array(struct target *target, uint8_t *buffer, uint32_t count, const uint32_t *srcbuf);
 void target_buffer_set_u16_array(struct target *target, uint8_t *buffer, uint32_t count, const uint16_t *srcbuf);
 
-int target_read_u64(struct target *target, uint64_t address, uint64_t *value);
-int target_read_u32(struct target *target, uint32_t address, uint32_t *value);
-int target_read_u16(struct target *target, uint32_t address, uint16_t *value);
-int target_read_u8(struct target *target, uint32_t address, uint8_t *value);
-int target_write_u64(struct target *target, uint64_t address, uint64_t value);
-int target_write_u32(struct target *target, uint32_t address, uint32_t value);
-int target_write_u16(struct target *target, uint32_t address, uint16_t value);
-int target_write_u8(struct target *target, uint32_t address, uint8_t value);
+int target_read_u64(struct target *target, target_addr_t address, uint64_t *value);
+int target_read_u32(struct target *target, target_addr_t address, uint32_t *value);
+int target_read_u16(struct target *target, target_addr_t address, uint16_t *value);
+int target_read_u8(struct target *target, target_addr_t address, uint8_t *value);
+int target_write_u64(struct target *target, target_addr_t address, uint64_t value);
+int target_write_u32(struct target *target, target_addr_t address, uint32_t value);
+int target_write_u16(struct target *target, target_addr_t address, uint16_t value);
+int target_write_u8(struct target *target, target_addr_t address, uint8_t value);
+
+int target_write_phys_u64(struct target *target, target_addr_t address, uint64_t value);
+int target_write_phys_u32(struct target *target, target_addr_t address, uint32_t value);
+int target_write_phys_u16(struct target *target, target_addr_t address, uint16_t value);
+int target_write_phys_u8(struct target *target, target_addr_t address, uint8_t value);
 
 /* Issues USER() statements with target state information */
 int target_arch_state(struct target *target);
diff --git a/src/target/target_type.h b/src/target/target_type.h
index 95c0fef..0960e6d 100644
--- a/src/target/target_type.h
+++ b/src/target/target_type.h
@@ -54,11 +54,10 @@ struct target_type {
 	/* halt will log a warning, but return ERROR_OK if the target is already halted. */
 	int (*halt)(struct target *target);
 	/* See target.c target_resume() for documentation. */
-	int (*resume)(struct target *target, int current, uint32_t address,
+	int (*resume)(struct target *target, int current, target_addr_t address,
 			int handle_breakpoints, int debug_execution);
-	int (*step)(struct target *target, int current, uint32_t address,
+	int (*step)(struct target *target, int current, target_addr_t address,
 			int handle_breakpoints);
-
 	/* target reset control. assert reset can be invoked when OpenOCD and
 	 * the target is out of sync.
 	 *
@@ -112,27 +111,27 @@ struct target_type {
 	 * Target memory read callback.  Do @b not call this function
 	 * directly, use target_read_memory() instead.
 	 */
-	int (*read_memory)(struct target *target, uint32_t address,
+	int (*read_memory)(struct target *target, target_addr_t address,
 			uint32_t size, uint32_t count, uint8_t *buffer);
 	/**
 	 * Target memory write callback.  Do @b not call this function
 	 * directly, use target_write_memory() instead.
 	 */
-	int (*write_memory)(struct target *target, uint32_t address,
+	int (*write_memory)(struct target *target, target_addr_t address,
 			uint32_t size, uint32_t count, const uint8_t *buffer);
 
 	/* Default implementation will do some fancy alignment to improve performance, target can override */
-	int (*read_buffer)(struct target *target, uint32_t address,
+	int (*read_buffer)(struct target *target, target_addr_t address,
 			uint32_t size, uint8_t *buffer);
 
 	/* Default implementation will do some fancy alignment to improve performance, target can override */
-	int (*write_buffer)(struct target *target, uint32_t address,
+	int (*write_buffer)(struct target *target, target_addr_t address,
 			uint32_t size, const uint8_t *buffer);
 
-	int (*checksum_memory)(struct target *target, uint32_t address,
+	int (*checksum_memory)(struct target *target, target_addr_t address,
 			uint32_t count, uint32_t *checksum);
-	int (*blank_check_memory)(struct target *target, uint32_t address,
-			uint32_t count, uint32_t *blank);
+	int (*blank_check_memory)(struct target *target, target_addr_t address,
+			uint32_t count, uint32_t *blank, uint8_t erased_value);
 
 	/*
 	 * target break-/watchpoint control
@@ -176,15 +175,15 @@ struct target_type {
 	 */
 	int (*run_algorithm)(struct target *target, int num_mem_params,
 			struct mem_param *mem_params, int num_reg_params,
-			struct reg_param *reg_param, uint32_t entry_point,
-			uint32_t exit_point, int timeout_ms, void *arch_info);
+			struct reg_param *reg_param, target_addr_t entry_point,
+			target_addr_t exit_point, int timeout_ms, void *arch_info);
 	int (*start_algorithm)(struct target *target, int num_mem_params,
 			struct mem_param *mem_params, int num_reg_params,
-			struct reg_param *reg_param, uint32_t entry_point,
-			uint32_t exit_point, void *arch_info);
+			struct reg_param *reg_param, target_addr_t entry_point,
+			target_addr_t exit_point, void *arch_info);
 	int (*wait_algorithm)(struct target *target, int num_mem_params,
 			struct mem_param *mem_params, int num_reg_params,
-			struct reg_param *reg_param, uint32_t exit_point,
+			struct reg_param *reg_param, target_addr_t exit_point,
 			int timeout_ms, void *arch_info);
 
 	const struct command_registration *commands;
@@ -234,7 +233,7 @@ struct target_type {
 	/* translate from virtual to physical address. Default implementation is successful
 	 * no-op(i.e. virtual==physical).
 	 */
-	int (*virt2phys)(struct target *target, uint32_t address, uint32_t *physical);
+	int (*virt2phys)(struct target *target, target_addr_t address, target_addr_t *physical);
 
 	/* read directly from physical memory. caches are bypassed and untouched.
 	 *
@@ -244,13 +243,13 @@ struct target_type {
 	 *
 	 * Default implementation is to call read_memory.
 	 */
-	int (*read_phys_memory)(struct target *target, uint32_t phys_address,
+	int (*read_phys_memory)(struct target *target, target_addr_t phys_address,
 			uint32_t size, uint32_t count, uint8_t *buffer);
 
 	/*
 	 * same as read_phys_memory, except that it writes...
 	 */
-	int (*write_phys_memory)(struct target *target, uint32_t phys_address,
+	int (*write_phys_memory)(struct target *target, target_addr_t phys_address,
 			uint32_t size, uint32_t count, const uint8_t *buffer);
 
 	int (*mmu)(struct target *target, int *enabled);
diff --git a/src/target/x86_32_common.c b/src/target/x86_32_common.c
index 3f4c7aa..34f92ea 100644
--- a/src/target/x86_32_common.c
+++ b/src/target/x86_32_common.c
@@ -48,8 +48,8 @@ static int read_mem(struct target *t, uint32_t size,
 			uint32_t addr, uint8_t *buf);
 static int write_mem(struct target *t, uint32_t size,
 			uint32_t addr, const uint8_t *buf);
-static int calcaddr_pyhsfromlin(struct target *t, uint32_t addr,
-			uint32_t *physaddr);
+static int calcaddr_physfromlin(struct target *t, target_addr_t addr,
+			target_addr_t *physaddr);
 static int read_phys_mem(struct target *t, uint32_t phys_address,
 			uint32_t size, uint32_t count, uint8_t *buffer);
 static int write_phys_mem(struct target *t, uint32_t phys_address,
@@ -113,7 +113,7 @@ int x86_32_common_mmu(struct target *t, int *enabled)
 	return ERROR_OK;
 }
 
-int x86_32_common_virt2phys(struct target *t, uint32_t address, uint32_t *physical)
+int x86_32_common_virt2phys(struct target *t, target_addr_t address, target_addr_t *physical)
 {
 	struct x86_32_common *x86_32 = target_to_x86_32(t);
 
@@ -134,8 +134,8 @@ int x86_32_common_virt2phys(struct target *t, uint32_t address, uint32_t *physic
 
 	} else {
 		/* target halted in protected mode */
-		if (calcaddr_pyhsfromlin(t, address, physical) != ERROR_OK) {
-			LOG_ERROR("%s failed to calculate physical address from 0x%08" PRIx32,
+		if (calcaddr_physfromlin(t, address, physical) != ERROR_OK) {
+			LOG_ERROR("%s failed to calculate physical address from " TARGET_ADDR_FMT,
 					__func__, address);
 			return ERROR_FAIL;
 		}
@@ -143,7 +143,7 @@ int x86_32_common_virt2phys(struct target *t, uint32_t address, uint32_t *physic
 	return ERROR_OK;
 }
 
-int x86_32_common_read_phys_mem(struct target *t, uint32_t phys_address,
+int x86_32_common_read_phys_mem(struct target *t, target_addr_t phys_address,
 			uint32_t size, uint32_t count, uint8_t *buffer)
 {
 	struct x86_32_common *x86_32 = target_to_x86_32(t);
@@ -226,7 +226,7 @@ static int read_phys_mem(struct target *t, uint32_t phys_address,
 	return retval;
 }
 
-int x86_32_common_write_phys_mem(struct target *t, uint32_t phys_address,
+int x86_32_common_write_phys_mem(struct target *t, target_addr_t phys_address,
 			uint32_t size, uint32_t count, const uint8_t *buffer)
 {
 	struct x86_32_common *x86_32 = target_to_x86_32(t);
@@ -235,7 +235,7 @@ int x86_32_common_write_phys_mem(struct target *t, uint32_t phys_address,
 
 	check_not_halted(t);
 	if (!count || !buffer || !phys_address) {
-		LOG_ERROR("%s invalid params count=0x%" PRIx32 ", buf=%p, addr=0x%08" PRIx32,
+		LOG_ERROR("%s invalid params count=0x%" PRIx32 ", buf=%p, addr=" TARGET_ADDR_FMT,
 				__func__, count, buffer, phys_address);
 		return ERROR_COMMAND_ARGUMENT_INVALID;
 	}
@@ -444,7 +444,7 @@ static int write_mem(struct target *t, uint32_t size,
 	return retval;
 }
 
-int calcaddr_pyhsfromlin(struct target *t, uint32_t addr, uint32_t *physaddr)
+int calcaddr_physfromlin(struct target *t, target_addr_t addr, target_addr_t *physaddr)
 {
 	uint8_t entry_buffer[8];
 
@@ -568,16 +568,16 @@ int calcaddr_pyhsfromlin(struct target *t, uint32_t addr, uint32_t *physaddr)
 	return ERROR_OK;
 }
 
-int x86_32_common_read_memory(struct target *t, uint32_t addr,
+int x86_32_common_read_memory(struct target *t, target_addr_t addr,
 			uint32_t size, uint32_t count, uint8_t *buf)
 {
 	int retval = ERROR_OK;
 	struct x86_32_common *x86_32 = target_to_x86_32(t);
-	LOG_DEBUG("addr=0x%08" PRIx32 ", size=%" PRIu32 ", count=0x%" PRIx32 ", buf=%p",
+	LOG_DEBUG("addr=" TARGET_ADDR_FMT ", size=%" PRIu32 ", count=0x%" PRIx32 ", buf=%p",
 			addr, size, count, buf);
 	check_not_halted(t);
 	if (!count || !buf || !addr) {
-		LOG_ERROR("%s invalid params count=0x%" PRIx32 ", buf=%p, addr=0x%08" PRIx32,
+		LOG_ERROR("%s invalid params count=0x%" PRIx32 ", buf=%p, addr=" TARGET_ADDR_FMT,
 				__func__, count, buf, addr);
 		return ERROR_COMMAND_ARGUMENT_INVALID;
 	}
@@ -591,9 +591,10 @@ int x86_32_common_read_memory(struct target *t, uint32_t addr,
 			LOG_ERROR("%s could not disable paging", __func__);
 			return retval;
 		}
-		uint32_t physaddr = 0;
-		if (calcaddr_pyhsfromlin(t, addr, &physaddr) != ERROR_OK) {
-			LOG_ERROR("%s failed to calculate physical address from 0x%08" PRIx32, __func__, addr);
+		target_addr_t physaddr = 0;
+		if (calcaddr_physfromlin(t, addr, &physaddr) != ERROR_OK) {
+			LOG_ERROR("%s failed to calculate physical address from " TARGET_ADDR_FMT,
+					  __func__, addr);
 			retval = ERROR_FAIL;
 		}
 		/* TODO: !!! Watch out for page boundaries
@@ -603,7 +604,8 @@ int x86_32_common_read_memory(struct target *t, uint32_t addr,
 
 		if (retval == ERROR_OK
 			&& x86_32_common_read_phys_mem(t, physaddr, size, count, buf) != ERROR_OK) {
-			LOG_ERROR("%s failed to read memory from physical address 0x%08" PRIx32, __func__, physaddr);
+			LOG_ERROR("%s failed to read memory from physical address " TARGET_ADDR_FMT,
+					  __func__, physaddr);
 			retval = ERROR_FAIL;
 		}
 		/* restore PG bit if it was cleared prior (regardless of retval) */
@@ -615,7 +617,8 @@ int x86_32_common_read_memory(struct target *t, uint32_t addr,
 	} else {
 		/* paging is off - linear address is physical address */
 		if (x86_32_common_read_phys_mem(t, addr, size, count, buf) != ERROR_OK) {
-			LOG_ERROR("%s failed to read memory from address 0%08" PRIx32, __func__, addr);
+			LOG_ERROR("%s failed to read memory from address " TARGET_ADDR_FMT,
+					  __func__, addr);
 			retval = ERROR_FAIL;
 		}
 	}
@@ -623,16 +626,16 @@ int x86_32_common_read_memory(struct target *t, uint32_t addr,
 	return retval;
 }
 
-int x86_32_common_write_memory(struct target *t, uint32_t addr,
+int x86_32_common_write_memory(struct target *t, target_addr_t addr,
 			uint32_t size, uint32_t count, const uint8_t *buf)
 {
 	int retval = ERROR_OK;
 	struct x86_32_common *x86_32 = target_to_x86_32(t);
-	LOG_DEBUG("addr=0x%08" PRIx32 ", size=%" PRIu32 ", count=0x%" PRIx32 ", buf=%p",
+	LOG_DEBUG("addr=" TARGET_ADDR_FMT ", size=%" PRIu32 ", count=0x%" PRIx32 ", buf=%p",
 			addr, size, count, buf);
 	check_not_halted(t);
 	if (!count || !buf || !addr) {
-		LOG_ERROR("%s invalid params count=0x%" PRIx32 ", buf=%p, addr=0x%08" PRIx32,
+		LOG_ERROR("%s invalid params count=0x%" PRIx32 ", buf=%p, addr=" TARGET_ADDR_FMT,
 					__func__, count, buf, addr);
 		return ERROR_COMMAND_ARGUMENT_INVALID;
 	}
@@ -645,9 +648,9 @@ int x86_32_common_write_memory(struct target *t, uint32_t addr,
 			LOG_ERROR("%s could not disable paging", __func__);
 			return retval;
 		}
-		uint32_t physaddr = 0;
-		if (calcaddr_pyhsfromlin(t, addr, &physaddr) != ERROR_OK) {
-			LOG_ERROR("%s failed to calculate physical address from 0x%08" PRIx32,
+		target_addr_t physaddr = 0;
+		if (calcaddr_physfromlin(t, addr, &physaddr) != ERROR_OK) {
+			LOG_ERROR("%s failed to calculate physical address from " TARGET_ADDR_FMT,
 					__func__, addr);
 			retval = ERROR_FAIL;
 		}
@@ -657,7 +660,7 @@ int x86_32_common_write_memory(struct target *t, uint32_t addr,
 		 */
 		if (retval == ERROR_OK
 			&& x86_32_common_write_phys_mem(t, physaddr, size, count, buf) != ERROR_OK) {
-			LOG_ERROR("%s failed to write memory to physical address 0x%08" PRIx32,
+			LOG_ERROR("%s failed to write memory to physical address " TARGET_ADDR_FMT,
 					__func__, physaddr);
 			retval = ERROR_FAIL;
 		}
@@ -671,7 +674,7 @@ int x86_32_common_write_memory(struct target *t, uint32_t addr,
 
 		/* paging is off - linear address is physical address */
 		if (x86_32_common_write_phys_mem(t, addr, size, count, buf) != ERROR_OK) {
-			LOG_ERROR("%s failed to write memory to address 0x%08" PRIx32,
+			LOG_ERROR("%s failed to write memory to address " TARGET_ADDR_FMT,
 					__func__, addr);
 			retval = ERROR_FAIL;
 		}
@@ -852,7 +855,7 @@ int x86_32_common_remove_watchpoint(struct target *t, struct watchpoint *wp)
 
 int x86_32_common_add_breakpoint(struct target *t, struct breakpoint *bp)
 {
-	LOG_DEBUG("type=%d, addr=0x%08" PRIx32, bp->type, bp->address);
+	LOG_DEBUG("type=%d, addr=" TARGET_ADDR_FMT, bp->type, bp->address);
 	if (check_not_halted(t))
 		return ERROR_TARGET_NOT_HALTED;
 	/* set_breakpoint() will return ERROR_TARGET_RESOURCE_NOT_AVAILABLE if all
@@ -863,7 +866,7 @@ int x86_32_common_add_breakpoint(struct target *t, struct breakpoint *bp)
 
 int x86_32_common_remove_breakpoint(struct target *t, struct breakpoint *bp)
 {
-	LOG_DEBUG("type=%d, addr=0x%08" PRIx32, bp->type, bp->address);
+	LOG_DEBUG("type=%d, addr=" TARGET_ADDR_FMT, bp->type, bp->address);
 	if (check_not_halted(t))
 		return ERROR_TARGET_NOT_HALTED;
 	if (bp->set)
@@ -1003,7 +1006,7 @@ static int unset_hwbp(struct target *t, struct breakpoint *bp)
 	debug_reg_list[hwbp_num].used = 0;
 	debug_reg_list[hwbp_num].bp_value = 0;
 
-	LOG_USER("%s hardware breakpoint %" PRIu32 " removed from 0x%08" PRIx32 " (hwreg=%d)",
+	LOG_USER("%s hardware breakpoint %" PRIu32 " removed from " TARGET_ADDR_FMT " (hwreg=%d)",
 			__func__, bp->unique_id, bp->address, hwbp_num);
 	return ERROR_OK;
 }
@@ -1012,11 +1015,11 @@ static int set_swbp(struct target *t, struct breakpoint *bp)
 {
 	struct x86_32_common *x86_32 = target_to_x86_32(t);
 	LOG_DEBUG("id %" PRIx32, bp->unique_id);
-	uint32_t physaddr;
+	target_addr_t physaddr;
 	uint8_t opcode = SW_BP_OPCODE;
 	uint8_t readback;
 
-	if (calcaddr_pyhsfromlin(t, bp->address, &physaddr) != ERROR_OK)
+	if (calcaddr_physfromlin(t, bp->address, &physaddr) != ERROR_OK)
 		return ERROR_FAIL;
 	if (read_phys_mem(t, physaddr, 1, 1, bp->orig_instr))
 		return ERROR_FAIL;
@@ -1032,7 +1035,7 @@ static int set_swbp(struct target *t, struct breakpoint *bp)
 		return ERROR_FAIL;
 
 	if (readback != SW_BP_OPCODE) {
-		LOG_ERROR("%s software breakpoint error at 0x%08" PRIx32 ", check memory",
+		LOG_ERROR("%s software breakpoint error at " TARGET_ADDR_FMT ", check memory",
 				__func__, bp->address);
 		LOG_ERROR("%s readback=0x%02" PRIx8 " orig=0x%02" PRIx8 "",
 				__func__, readback, *bp->orig_instr);
@@ -1059,7 +1062,7 @@ static int set_swbp(struct target *t, struct breakpoint *bp)
 			addto = addto->next;
 		addto->next = new_patch;
 	}
-	LOG_USER("%s software breakpoint %" PRIu32 " set at 0x%08" PRIx32,
+	LOG_USER("%s software breakpoint %" PRIu32 " set at " TARGET_ADDR_FMT,
 			__func__, bp->unique_id, bp->address);
 	return ERROR_OK;
 }
@@ -1068,11 +1071,11 @@ static int unset_swbp(struct target *t, struct breakpoint *bp)
 {
 	struct x86_32_common *x86_32 = target_to_x86_32(t);
 	LOG_DEBUG("id %" PRIx32, bp->unique_id);
-	uint32_t physaddr;
+	target_addr_t physaddr;
 	uint8_t current_instr;
 
 	/* check that user program has not modified breakpoint instruction */
-	if (calcaddr_pyhsfromlin(t, bp->address, &physaddr) != ERROR_OK)
+	if (calcaddr_physfromlin(t, bp->address, &physaddr) != ERROR_OK)
 		return ERROR_FAIL;
 	if (read_phys_mem(t, physaddr, 1, 1, &current_instr))
 		return ERROR_FAIL;
@@ -1081,7 +1084,7 @@ static int unset_swbp(struct target *t, struct breakpoint *bp)
 		if (write_phys_mem(t, physaddr, 1, 1, bp->orig_instr))
 			return ERROR_FAIL;
 	} else {
-		LOG_ERROR("%s software breakpoint remove error at 0x%08" PRIx32 ", check memory",
+		LOG_ERROR("%s software breakpoint remove error at " TARGET_ADDR_FMT ", check memory",
 				__func__, bp->address);
 		LOG_ERROR("%s current=0x%02" PRIx8 " orig=0x%02" PRIx8 "",
 				__func__, current_instr, *bp->orig_instr);
@@ -1107,7 +1110,7 @@ static int unset_swbp(struct target *t, struct breakpoint *bp)
 		}
 	}
 
-	LOG_USER("%s software breakpoint %" PRIu32 " removed from 0x%08" PRIx32,
+	LOG_USER("%s software breakpoint %" PRIu32 " removed from " TARGET_ADDR_FMT,
 			__func__, bp->unique_id, bp->address);
 	return ERROR_OK;
 }
@@ -1116,7 +1119,7 @@ static int set_breakpoint(struct target *t, struct breakpoint *bp)
 {
 	int error = ERROR_OK;
 	struct x86_32_common *x86_32 = target_to_x86_32(t);
-	LOG_DEBUG("type=%d, addr=0x%08" PRIx32, bp->type, bp->address);
+	LOG_DEBUG("type=%d, addr=" TARGET_ADDR_FMT, bp->type, bp->address);
 	if (bp->set) {
 		LOG_ERROR("breakpoint already set");
 		return error;
@@ -1124,7 +1127,7 @@ static int set_breakpoint(struct target *t, struct breakpoint *bp)
 	if (bp->type == BKPT_HARD) {
 		error = set_hwbp(t, bp);
 		if (error != ERROR_OK) {
-			LOG_ERROR("%s error setting hardware breakpoint at 0x%08" PRIx32,
+			LOG_ERROR("%s error setting hardware breakpoint at " TARGET_ADDR_FMT,
 					__func__, bp->address);
 			return error;
 		}
@@ -1132,7 +1135,7 @@ static int set_breakpoint(struct target *t, struct breakpoint *bp)
 		if (x86_32->sw_bpts_supported(t)) {
 			error = set_swbp(t, bp);
 			if (error != ERROR_OK) {
-				LOG_ERROR("%s error setting software breakpoint at 0x%08" PRIx32,
+				LOG_ERROR("%s error setting software breakpoint at " TARGET_ADDR_FMT,
 						__func__, bp->address);
 				return error;
 			}
@@ -1147,7 +1150,7 @@ static int set_breakpoint(struct target *t, struct breakpoint *bp)
 
 static int unset_breakpoint(struct target *t, struct breakpoint *bp)
 {
-	LOG_DEBUG("type=%d, addr=0x%08" PRIx32, bp->type, bp->address);
+	LOG_DEBUG("type=%d, addr=" TARGET_ADDR_FMT, bp->type, bp->address);
 	if (!bp->set) {
 		LOG_WARNING("breakpoint not set");
 		return ERROR_OK;
@@ -1155,13 +1158,13 @@ static int unset_breakpoint(struct target *t, struct breakpoint *bp)
 
 	if (bp->type == BKPT_HARD) {
 		if (unset_hwbp(t, bp) != ERROR_OK) {
-			LOG_ERROR("%s error removing hardware breakpoint at 0x%08" PRIx32,
+			LOG_ERROR("%s error removing hardware breakpoint at " TARGET_ADDR_FMT,
 					__func__, bp->address);
 			return ERROR_FAIL;
 		}
 	} else {
 		if (unset_swbp(t, bp) != ERROR_OK) {
-			LOG_ERROR("%s error removing software breakpoint at 0x%08" PRIx32,
+			LOG_ERROR("%s error removing software breakpoint at " TARGET_ADDR_FMT,
 					__func__, bp->address);
 			return ERROR_FAIL;
 		}
@@ -1175,7 +1178,7 @@ static int set_watchpoint(struct target *t, struct watchpoint *wp)
 	struct x86_32_common *x86_32 = target_to_x86_32(t);
 	struct x86_32_dbg_reg *debug_reg_list = x86_32->hw_break_list;
 	int wp_num = 0;
-	LOG_DEBUG("type=%d, addr=0x%08" PRIx32, wp->rw, wp->address);
+	LOG_DEBUG("type=%d, addr=" TARGET_ADDR_FMT, wp->rw, wp->address);
 
 	if (wp->set) {
 		LOG_ERROR("%s watchpoint already set", __func__);
@@ -1220,7 +1223,7 @@ static int set_watchpoint(struct target *t, struct watchpoint *wp)
 	wp->set = wp_num + 1;
 	debug_reg_list[wp_num].used = 1;
 	debug_reg_list[wp_num].bp_value = wp->address;
-	LOG_USER("'%s' watchpoint %d set at 0x%08" PRIx32 " with length %" PRIu32 " (hwreg=%d)",
+	LOG_USER("'%s' watchpoint %d set at " TARGET_ADDR_FMT " with length %" PRIu32 " (hwreg=%d)",
 			wp->rw == WPT_READ ? "read" : wp->rw == WPT_WRITE ?
 			"write" : wp->rw == WPT_ACCESS ? "access" : "?",
 			wp->unique_id, wp->address, wp->length, wp_num);
@@ -1231,7 +1234,7 @@ static int unset_watchpoint(struct target *t, struct watchpoint *wp)
 {
 	struct x86_32_common *x86_32 = target_to_x86_32(t);
 	struct x86_32_dbg_reg *debug_reg_list = x86_32->hw_break_list;
-	LOG_DEBUG("type=%d, addr=0x%08" PRIx32, wp->rw, wp->address);
+	LOG_DEBUG("type=%d, addr=" TARGET_ADDR_FMT, wp->rw, wp->address);
 	if (!wp->set) {
 		LOG_WARNING("watchpoint not set");
 		return ERROR_OK;
@@ -1249,7 +1252,7 @@ static int unset_watchpoint(struct target *t, struct watchpoint *wp)
 	debug_reg_list[wp_num].bp_value = 0;
 	wp->set = 0;
 
-	LOG_USER("'%s' watchpoint %d removed from 0x%08" PRIx32 " with length %" PRIu32 " (hwreg=%d)",
+	LOG_USER("'%s' watchpoint %d removed from " TARGET_ADDR_FMT " with length %" PRIu32 " (hwreg=%d)",
 			wp->rw == WPT_READ ? "read" : wp->rw == WPT_WRITE ?
 			"write" : wp->rw == WPT_ACCESS ? "access" : "?",
 			wp->unique_id, wp->address, wp->length, wp_num);
diff --git a/src/target/x86_32_common.h b/src/target/x86_32_common.h
index b5877da..0aaa963 100644
--- a/src/target/x86_32_common.h
+++ b/src/target/x86_32_common.h
@@ -309,14 +309,14 @@ int x86_32_get_gdb_reg_list(struct target *t,
 int x86_32_common_init_arch_info(struct target *target,
 			struct x86_32_common *x86_32);
 int x86_32_common_mmu(struct target *t, int *enabled);
-int x86_32_common_virt2phys(struct target *t, uint32_t address, uint32_t *physical);
-int x86_32_common_read_phys_mem(struct target *t, uint32_t phys_address,
+int x86_32_common_virt2phys(struct target *t, target_addr_t address, target_addr_t *physical);
+int x86_32_common_read_phys_mem(struct target *t, target_addr_t phys_address,
 			uint32_t size, uint32_t count, uint8_t *buffer);
-int x86_32_common_write_phys_mem(struct target *t, uint32_t phys_address,
+int x86_32_common_write_phys_mem(struct target *t, target_addr_t phys_address,
 			uint32_t size, uint32_t count, const uint8_t *buffer);
-int x86_32_common_read_memory(struct target *t, uint32_t addr,
+int x86_32_common_read_memory(struct target *t, target_addr_t addr,
 			uint32_t size, uint32_t count, uint8_t *buf);
-int x86_32_common_write_memory(struct target *t, uint32_t addr,
+int x86_32_common_write_memory(struct target *t, target_addr_t addr,
 			uint32_t size, uint32_t count, const uint8_t *buf);
 int x86_32_common_read_io(struct target *t, uint32_t addr,
 			uint32_t size, uint8_t *buf);
diff --git a/src/target/xscale.c b/src/target/xscale.c
index 140ea58..8fe8a2c 100644
--- a/src/target/xscale.c
+++ b/src/target/xscale.c
@@ -59,7 +59,7 @@
 
 /* forward declarations */
 static int xscale_resume(struct target *, int current,
-	uint32_t address, int handle_breakpoints, int debug_execution);
+	target_addr_t address, int handle_breakpoints, int debug_execution);
 static int xscale_debug_entry(struct target *);
 static int xscale_restore_banked(struct target *);
 static int xscale_get_reg(struct reg *reg);
@@ -73,7 +73,7 @@ static int xscale_read_trace(struct target *);
  * mini-ICache, which is 2K of code writable only via JTAG.
  */
 static const uint8_t xscale_debug_handler[] = {
-#include "xscale_debug.inc"
+#include "../../contrib/loaders/debug/xscale/debug_handler.inc"
 };
 
 static const char *const xscale_reg_list[] = {
@@ -1120,7 +1120,7 @@ static void xscale_free_trace_data(struct xscale_common *xscale)
 }
 
 static int xscale_resume(struct target *target, int current,
-	uint32_t address, int handle_breakpoints, int debug_execution)
+	target_addr_t address, int handle_breakpoints, int debug_execution)
 {
 	struct xscale_common *xscale = target_to_xscale(target);
 	struct arm *arm = &xscale->arm;
@@ -1165,7 +1165,8 @@ static int xscale_resume(struct target *target, int current,
 			enum trace_mode saved_trace_mode;
 
 			/* there's a breakpoint at the current PC, we have to step over it */
-			LOG_DEBUG("unset breakpoint at 0x%8.8" PRIx32 "", breakpoint->address);
+			LOG_DEBUG("unset breakpoint at " TARGET_ADDR_FMT "",
+				breakpoint->address);
 			xscale_unset_breakpoint(target, breakpoint);
 
 			/* calculate PC of next instruction */
@@ -1222,7 +1223,8 @@ static int xscale_resume(struct target *target, int current,
 			LOG_DEBUG("disable single-step");
 			xscale_disable_single_step(target);
 
-			LOG_DEBUG("set breakpoint at 0x%8.8" PRIx32 "", breakpoint->address);
+			LOG_DEBUG("set breakpoint at " TARGET_ADDR_FMT "",
+				breakpoint->address);
 			xscale_set_breakpoint(target, breakpoint);
 		}
 	}
@@ -1384,7 +1386,7 @@ static int xscale_step_inner(struct target *target, int current,
 }
 
 static int xscale_step(struct target *target, int current,
-	uint32_t address, int handle_breakpoints)
+	target_addr_t address, int handle_breakpoints)
 {
 	struct arm *arm = target_to_arm(target);
 	struct breakpoint *breakpoint = NULL;
@@ -1778,7 +1780,7 @@ dirty:
 	return ERROR_OK;
 }
 
-static int xscale_read_memory(struct target *target, uint32_t address,
+static int xscale_read_memory(struct target *target, target_addr_t address,
 	uint32_t size, uint32_t count, uint8_t *buffer)
 {
 	struct xscale_common *xscale = target_to_xscale(target);
@@ -1786,7 +1788,7 @@ static int xscale_read_memory(struct target *target, uint32_t address,
 	uint32_t i;
 	int retval;
 
-	LOG_DEBUG("address: 0x%8.8" PRIx32 ", size: 0x%8.8" PRIx32 ", count: 0x%8.8" PRIx32,
+	LOG_DEBUG("address: " TARGET_ADDR_FMT ", size: 0x%8.8" PRIx32 ", count: 0x%8.8" PRIx32,
 		address,
 		size,
 		count);
@@ -1864,7 +1866,7 @@ static int xscale_read_memory(struct target *target, uint32_t address,
 	return ERROR_OK;
 }
 
-static int xscale_read_phys_memory(struct target *target, uint32_t address,
+static int xscale_read_phys_memory(struct target *target, target_addr_t address,
 	uint32_t size, uint32_t count, uint8_t *buffer)
 {
 	struct xscale_common *xscale = target_to_xscale(target);
@@ -1879,13 +1881,13 @@ static int xscale_read_phys_memory(struct target *target, uint32_t address,
 	return ERROR_FAIL;
 }
 
-static int xscale_write_memory(struct target *target, uint32_t address,
+static int xscale_write_memory(struct target *target, target_addr_t address,
 	uint32_t size, uint32_t count, const uint8_t *buffer)
 {
 	struct xscale_common *xscale = target_to_xscale(target);
 	int retval;
 
-	LOG_DEBUG("address: 0x%8.8" PRIx32 ", size: 0x%8.8" PRIx32 ", count: 0x%8.8" PRIx32,
+	LOG_DEBUG("address: " TARGET_ADDR_FMT ", size: 0x%8.8" PRIx32 ", count: 0x%8.8" PRIx32,
 		address,
 		size,
 		count);
@@ -1963,7 +1965,7 @@ static int xscale_write_memory(struct target *target, uint32_t address,
 	return ERROR_OK;
 }
 
-static int xscale_write_phys_memory(struct target *target, uint32_t address,
+static int xscale_write_phys_memory(struct target *target, target_addr_t address,
 	uint32_t size, uint32_t count, const uint8_t *buffer)
 {
 	struct xscale_common *xscale = target_to_xscale(target);
@@ -3093,7 +3095,7 @@ COMMAND_HANDLER(xscale_handle_cache_info_command)
 }
 
 static int xscale_virt2phys(struct target *target,
-	uint32_t virtual, uint32_t *physical)
+	target_addr_t virtual, target_addr_t *physical)
 {
 	struct xscale_common *xscale = target_to_xscale(target);
 	uint32_t cb;
diff --git a/src/target/xscale/build.sh b/src/target/xscale/build.sh
deleted file mode 100755
index fc828b2..0000000
--- a/src/target/xscale/build.sh
+++ /dev/null
@@ -1,7 +0,0 @@
-arm-none-eabi-gcc -c debug_handler.S -o debug_handler.o
-arm-none-eabi-ld -EL -n -Tdebug_handler.cmd debug_handler.o -o debug_handler.out
-arm-none-eabi-objcopy -O binary debug_handler.out debug_handler.bin
-
-#arm-none-eabi-gcc -mbig-endian -c debug_handler.S -o debug_handler_be.o
-#arm-none-eabi-ld -EB -n -Tdebug_handler.cmd debug_handler_be.o -o debug_handler_be.out
-#arm-none-eabi-objcopy -O binary debug_handler_be.out debug_handler_be.bin
diff --git a/src/target/xscale/debug_handler.S b/src/target/xscale/debug_handler.S
deleted file mode 100644
index 66dfa88..0000000
--- a/src/target/xscale/debug_handler.S
+++ /dev/null
@@ -1,716 +0,0 @@
-/***************************************************************************
- *   Copyright (C) 2006 by Dominic Rath                                    *
- *   Dominic.Rath@gmx.de                                                   *
- *                                                                         *
- *   This program is free software; you can redistribute it and/or modify  *
- *   it under the terms of the GNU General Public License as published by  *
- *   the Free Software Foundation; either version 2 of the License, or     *
- *   (at your option) any later version.                                   *
- *                                                                         *
- *   This program is distributed in the hope that it will be useful,       *
- *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
- *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
- *   GNU General Public License for more details.                          *
- *                                                                         *
- *   You should have received a copy of the GNU General Public License     *
- *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
- ***************************************************************************/
-#include "protocol.h"
-
-    .text
-    .align  4
-
-@ Disable thumb mode
-    .code 32
-
-@ send word to debugger
-.macro m_send_to_debugger reg
-1:
-	mrc p14, 0, r15, c14, c0, 0
-	bvs 1b
-	mcr p14, 0, \reg, c8, c0, 0
-.endm
-
-@ receive word from debugger
-.macro m_receive_from_debugger reg
-1:
-	mrc p14, 0, r15, c14, c0, 0
-	bpl 1b
-	mrc p14, 0, \reg, c9, c0, 0
-.endm
-
-@ save register on debugger, small
-.macro m_small_save_reg reg
-	mov r0, \reg
-	bl send_to_debugger
-.endm
-
-@ save status register on debugger, small
-.macro m_small_save_psr
-	mrs r0, spsr
-	bl send_to_debugger
-.endm
-
-@ wait for all outstanding coprocessor accesses to complete
-.macro m_cpwait
-	mrc p15, 0, r0, c2, c0, 0
-	mov r0, r0
-	sub pc, pc, #4
-.endm
-
-.global reset_handler
-.global undef_handler
-.global swi_handler
-.global prefetch_abort_handler
-.global data_abort_handler
-.global irq_handler
-.global fiq_handler
-
-.section .part1 , "ax"
-
-reset_handler:
-	@ read DCSR
-	mrc p14, 0, r13, c10, c0
-	@ check if global enable bit (GE) is set
-	ands r13, r13, #0x80000000
-
-	bne debug_handler
-
-	@ set global enable bit (GE)
-	mov r13, #0xc0000000
-	mcr p14, 0, r13, c10, c0
-
-debug_handler:
-
-	@ save r0 without modifying other registers
-	m_send_to_debugger r0
-
-	@ save lr (program PC) without branching (use macro)
-	m_send_to_debugger r14
-
-	@ save non-banked registers and spsr (program CPSR)
-	m_small_save_reg r1
-	m_small_save_reg r2
-	m_small_save_reg r3
-	m_small_save_reg r4
-	m_small_save_reg r5
-	m_small_save_reg r6
-	m_small_save_reg r7
-	m_small_save_psr
-
-	mrs r0, spsr
-
-	@ prepare program PSR for debug use (clear Thumb, set I/F to disable interrupts)
-	bic r0, r0, #PSR_T
-	orr r0, r0, #(PSR_I | PSR_F)
-
-	@ examine mode bits
-	and r1, r0, #MODE_MASK
-	cmp r1, #MODE_USR
-
-	bne not_user_mode
-
-	@ replace USR mode with SYS
-	bic r0, r0, #MODE_MASK
-	orr r0, r0, #MODE_SYS
-
-not_user_mode:
-
-	b save_banked_registers
-
-@ command loop
-@ wait for command from debugger, than execute desired function
-get_command:
-	bl receive_from_debugger
-
-	@ 0x0n - register access
-	cmp r0, #0x0
-	beq get_banked_registers
-
-	cmp r0, #0x1
-	beq set_banked_registers
-
-	@ 0x1n - read memory
-	cmp r0, #0x11
-	beq read_byte
-
-	cmp r0, #0x12
-	beq read_half_word
-
-	cmp r0, #0x14
-	beq read_word
-
-	@ 0x2n - write memory
-	cmp r0, #0x21
-	beq write_byte
-
-	cmp r0, #0x22
-	beq write_half_word
-
-	cmp r0, #0x24
-	beq write_word
-
-	@ 0x3n - program execution
-	cmp r0, #0x30
-	beq resume
-
-	cmp r0, #0x31
-	beq resume_w_trace
-
-	@ 0x4n - coprocessor access
-	cmp r0, #0x40
-	beq read_cp_reg
-
-	cmp r0, #0x41
-	beq write_cp_reg
-
-	@ 0x5n - cache and mmu functions
-	cmp r0, #0x50
-	beq clean_d_cache
-
-	cmp r0, #0x51
-	beq invalidate_d_cache
-
-	cmp r0, #0x52
-	beq invalidate_i_cache
-
-	cmp r0, #0x53
-	beq cpwait
-
-	@ 0x6n - misc functions
-	cmp r0, #0x60
-	beq clear_sa
-
-	cmp r0, #0x61
-	beq read_trace_buffer
-
-	cmp r0, #0x62
-	beq clean_trace_buffer
-
-	@ return (back to get_command)
-	b get_command
-
-@ ----
-
-@ resume program execution
-resume:
-	@ restore CPSR (SPSR_dbg)
-	bl receive_from_debugger
-	msr spsr, r0
-
-	@ restore registers (r7 - r0)
-	bl receive_from_debugger @ r7
-	mov r7, r0
-	bl receive_from_debugger @ r6
-	mov r6, r0
-	bl receive_from_debugger @ r5
-	mov r5, r0
-	bl receive_from_debugger @ r4
-	mov r4, r0
-	bl receive_from_debugger @ r3
-	mov r3, r0
-	bl receive_from_debugger @ r2
-	mov r2, r0
-	bl receive_from_debugger @ r1
-	mov r1, r0
-	bl receive_from_debugger @ r0
-
-	@ resume addresss
-	m_receive_from_debugger lr
-
-	@ branch back to application code, restoring CPSR
-	subs pc, lr, #0
-
-@ get banked registers
-@ receive mode bits from host, then run into save_banked_registers to
-
-get_banked_registers:
-	bl receive_from_debugger
-
-@ save banked registers
-@ r0[4:0]: desired mode bits
-save_banked_registers:
-	@ backup CPSR
-	mrs r7, cpsr
-	msr cpsr_c, r0
-	nop
-
-	@ keep current mode bits in r1 for later use
-	and r1, r0, #MODE_MASK
-
-	@ backup banked registers
-	m_send_to_debugger r8
-	m_send_to_debugger r9
-	m_send_to_debugger r10
-	m_send_to_debugger r11
-	m_send_to_debugger r12
-	m_send_to_debugger r13
-	m_send_to_debugger r14
-
-	@ if not in SYS mode (or USR, which we replaced with SYS before)
-	cmp r1, #MODE_SYS
-
-	beq no_spsr_to_save
-
-	@ backup SPSR
-	mrs r0, spsr
-	m_send_to_debugger r0
-
-no_spsr_to_save:
-
-	@ restore CPSR for SDS
-	msr cpsr_c, r7
-	nop
-
-   	@ return
-	b get_command
-
-@ ----
-
-
-@ set banked registers
-@ receive mode bits from host, then run into save_banked_registers to
-
-set_banked_registers:
-	bl receive_from_debugger
-
-@ restore banked registers
-@ r0[4:0]: desired mode bits
-restore_banked_registers:
-	@ backup CPSR
-	mrs r7, cpsr
-	msr cpsr_c, r0
-	nop
-
-	@ keep current mode bits in r1 for later use
-	and r1, r0, #MODE_MASK
-
-	@ set banked registers
-	m_receive_from_debugger r8
-	m_receive_from_debugger r9
-	m_receive_from_debugger r10
-	m_receive_from_debugger r11
-	m_receive_from_debugger r12
-	m_receive_from_debugger r13
-	m_receive_from_debugger r14
-
-	@ if not in SYS mode (or USR, which we replaced with SYS before)
-	cmp r1, #MODE_SYS
-
-	beq no_spsr_to_restore
-
-	@ set SPSR
-	m_receive_from_debugger r0
-	msr spsr, r0
-
-no_spsr_to_restore:
-
-	@ restore CPSR for SDS
-	msr cpsr_c, r7
-	nop
-
-   	@ return
-	b get_command
-
-@ ----
-
-read_byte:
-	@ r2: address
-	bl receive_from_debugger
-	mov r2, r0
-
-	@ r1: count
-	bl receive_from_debugger
-	mov r1, r0
-
-rb_loop:
-	ldrb r0, [r2], #1
-
-	@ drain write- (and fill-) buffer to work around XScale errata
-	mcr p15, 0, r8, c7, c10, 4
-
-	bl send_to_debugger
-
-	subs r1, r1, #1
-	bne rb_loop
-
-	@ return
-	b get_command
-
-@ ----
-
-read_half_word:
-	@ r2: address
-	bl receive_from_debugger
-	mov r2, r0
-
-	@ r1: count
-	bl receive_from_debugger
-	mov r1, r0
-
-rh_loop:
-	ldrh r0, [r2], #2
-
-	@ drain write- (and fill-) buffer to work around XScale errata
-	mcr p15, 0, r8, c7, c10, 4
-
-	bl send_to_debugger
-
-	subs r1, r1, #1
-	bne rh_loop
-
-	@ return
-	b get_command
-
-@ ----
-
-read_word:
-	@ r2: address
-	bl receive_from_debugger
-	mov r2, r0
-
-	@ r1: count
-	bl receive_from_debugger
-	mov r1, r0
-
-rw_loop:
-	ldr r0, [r2], #4
-
-	@ drain write- (and fill-) buffer to work around XScale errata
-	mcr p15, 0, r8, c7, c10, 4
-
-	bl send_to_debugger
-
-	subs r1, r1, #1
-	bne rw_loop
-
-	@ return
-	b get_command
-
-@ ----
-
-write_byte:
-	@ r2: address
-	bl receive_from_debugger
-	mov r2, r0
-
-	@ r1: count
-	bl receive_from_debugger
-	mov r1, r0
-
-wb_loop:
-	bl receive_from_debugger
-	strb r0, [r2], #1
-
-	@ drain write- (and fill-) buffer to work around XScale errata
-	mcr p15, 0, r8, c7, c10, 4
-
-	subs r1, r1, #1
-	bne wb_loop
-
-	@ return
-	b get_command
-
-@ ----
-
-write_half_word:
-	@ r2: address
-	bl receive_from_debugger
-	mov r2, r0
-
-	@ r1: count
-	bl receive_from_debugger
-	mov r1, r0
-
-wh_loop:
-	bl receive_from_debugger
-	strh r0, [r2], #2
-
-	@ drain write- (and fill-) buffer to work around XScale errata
-	mcr p15, 0, r8, c7, c10, 4
-
-	subs r1, r1, #1
-	bne wh_loop
-
-	@ return
-	b get_command
-
-@ ----
-
-write_word:
-	@ r2: address
-	bl receive_from_debugger
-	mov r2, r0
-
-	@ r1: count
-	bl receive_from_debugger
-	mov r1, r0
-
-ww_loop:
-	bl receive_from_debugger
-	str r0, [r2], #4
-
-	@ drain write- (and fill-) buffer to work around XScale errata
-	mcr p15, 0, r8, c7, c10, 4
-
-	subs r1, r1, #1
-	bne ww_loop
-
-	@ return
-	b get_command
-
-@ ----
-
-clear_sa:
-	@ read DCSR
-	mrc p14, 0, r0, c10, c0
-
-	@ clear SA bit
-	bic r0, r0, #0x20
-
-	@ write DCSR
-	mcr p14, 0, r0, c10, c0
-
-	@ return
-	b get_command
-
-@ ----
-
-clean_d_cache:
-	@ r0: cache clean area
-	bl receive_from_debugger
-
-	mov r1, #1024
-clean_loop:
-	mcr p15, 0, r0, c7, c2, 5
-	add r0, r0, #32
-	subs r1, r1, #1
-	bne clean_loop
-
-	@ return
-	b get_command
-
-@ ----
-
-invalidate_d_cache:
-	mcr p15, 0, r0, c7, c6, 0
-
-	@ return
-	b get_command
-
-@ ----
-
-invalidate_i_cache:
-	mcr p15, 0, r0, c7, c5, 0
-
-	@ return
-	b get_command
-
-@ ----
-
-cpwait:
-	m_cpwait
-
-	@return
-	b get_command
-
-@ ----
-
-.section .part2 , "ax"
-
-read_cp_reg:
-	@ requested cp register
-	bl receive_from_debugger
-
-	adr r1, read_cp_table
-	add pc, r1, r0, lsl #3
-
-read_cp_table:
-	mrc p15, 0, r0, c0, c0, 0  @ XSCALE_MAINID
-	b read_cp_reg_reply
-	mrc p15, 0, r0, c0, c0, 1  @ XSCALE_CACHETYPE
-	b read_cp_reg_reply
-	mrc p15, 0, r0, c1, c0, 0  @ XSCALE_CTRL
-	b read_cp_reg_reply
-	mrc p15, 0, r0, c1, c0, 1  @ XSCALE_AUXCTRL
-	b read_cp_reg_reply
-	mrc p15, 0, r0, c2, c0, 0  @ XSCALE_TTB
-	b read_cp_reg_reply
-	mrc p15, 0, r0, c3, c0, 0  @ XSCALE_DAC
-	b read_cp_reg_reply
-	mrc p15, 0, r0, c5, c0, 0  @ XSCALE_FSR
-	b read_cp_reg_reply
-	mrc p15, 0, r0, c6, c0, 0  @ XSCALE_FAR
-	b read_cp_reg_reply
-	mrc p15, 0, r0, c13, c0, 0  @ XSCALE_PID
-	b read_cp_reg_reply
-	mrc p15, 0, r0, c15, c0, 0  @ XSCALE_CP_ACCESS
-	b read_cp_reg_reply
-	mrc p15, 0, r0, c14, c8, 0  @ XSCALE_IBCR0
-	b read_cp_reg_reply
-	mrc p15, 0, r0, c14, c9, 0  @ XSCALE_IBCR1
-	b read_cp_reg_reply
-	mrc p15, 0, r0, c14, c0, 0  @ XSCALE_DBR0
-	b read_cp_reg_reply
-	mrc p15, 0, r0, c14, c3, 0  @ XSCALE_DBR1
-	b read_cp_reg_reply
-	mrc p15, 0, r0, c14, c4, 0  @ XSCALE_DBCON
-	b read_cp_reg_reply
-	mrc p14, 0, r0, c11, c0, 0 @ XSCALE_TBREG
-	b read_cp_reg_reply
-	mrc p14, 0, r0, c12, c0, 0 @ XSCALE_CHKPT0
-	b read_cp_reg_reply
-	mrc p14, 0, r0, c13, c0, 0 @ XSCALE_CHKPT1
-	b read_cp_reg_reply
-	mrc p14, 0, r0, c10, c0, 0 @ XSCALE_DCSR
-	b read_cp_reg_reply
-
-read_cp_reg_reply:
-	bl send_to_debugger
-
-	@ return
-	b get_command
-
-@ ----
-
-write_cp_reg:
-	@ requested cp register
-	bl receive_from_debugger
-	mov r1, r0
-
-	@ value to be written
-	bl receive_from_debugger
-
-	adr r2, write_cp_table
-	add pc, r2, r1, lsl #3
-
-write_cp_table:
-	mcr p15, 0, r0, c0, c0, 0  @ XSCALE_MAINID (0x0)
-	b get_command
-	mcr p15, 0, r0, c0, c0, 1  @ XSCALE_CACHETYPE (0x1)
-	b get_command
-	mcr p15, 0, r0, c1, c0, 0  @ XSCALE_CTRL (0x2)
-	b get_command
-	mcr p15, 0, r0, c1, c0, 1  @ XSCALE_AUXCTRL (0x3)
-	b get_command
-	mcr p15, 0, r0, c2, c0, 0  @ XSCALE_TTB (0x4)
-	b get_command
-	mcr p15, 0, r0, c3, c0, 0  @ XSCALE_DAC (0x5)
-	b get_command
-	mcr p15, 0, r0, c5, c0, 0  @ XSCALE_FSR (0x6)
-	b get_command
-	mcr p15, 0, r0, c6, c0, 0  @ XSCALE_FAR (0x7)
-	b get_command
-	mcr p15, 0, r0, c13, c0, 0  @ XSCALE_PID (0x8)
-	b get_command
-	mcr p15, 0, r0, c15, c0, 0  @ XSCALE_CP_ACCESS (0x9)
-	b get_command
-	mcr p15, 0, r0, c14, c8, 0  @ XSCALE_IBCR0 (0xa)
-	b get_command
-	mcr p15, 0, r0, c14, c9, 0  @ XSCALE_IBCR1 (0xb)
-	b get_command
-	mcr p15, 0, r0, c14, c0, 0  @ XSCALE_DBR0 (0xc)
-	b get_command
-	mcr p15, 0, r0, c14, c3, 0  @ XSCALE_DBR1 (0xd)
-	b get_command
-	mcr p15, 0, r0, c14, c4, 0  @ XSCALE_DBCON (0xe)
-	b get_command
-	mcr p14, 0, r0, c11, c0, 0 @ XSCALE_TBREG (0xf)
-	b get_command
-	mcr p14, 0, r0, c12, c0, 0 @ XSCALE_CHKPT0 (0x10)
-	b get_command
-	mcr p14, 0, r0, c13, c0, 0 @ XSCALE_CHKPT1 (0x11)
-	b get_command
-	mcr p14, 0, r0, c10, c0, 0 @ XSCALE_DCSR (0x12)
-	b get_command
-
-@ ----
-
-read_trace_buffer:
-
-	@ dump 256 entries from trace buffer
-	mov	r1, #256
-read_tb_loop:
-	mrc p14, 0, r0, c11, c0, 0 @ XSCALE_TBREG
-	bl send_to_debugger
-	subs r1, r1, #1
-	bne read_tb_loop
-
-	@ dump checkpoint register 0
-	mrc p14, 0, r0, c12, c0, 0 @ XSCALE_CHKPT0 (0x10)
-	bl send_to_debugger
-
-	@ dump checkpoint register 1
-	mrc p14, 0, r0, c13, c0, 0 @ XSCALE_CHKPT1 (0x11)
-	bl send_to_debugger
-
-	@ return
-	b get_command
-
-@ ----
-
-clean_trace_buffer:
-
-	@ clean 256 entries from trace buffer
-	mov	r1, #256
-clean_tb_loop:
-	mrc p14, 0, r0, c11, c0, 0 @ XSCALE_TBREG
-	subs r1, r1, #1
-	bne clean_tb_loop
-
-	@ return
-	b get_command
-
-@ ----
-
-
-@ resume program execution with trace buffer enabled
-resume_w_trace:
-	@ restore CPSR (SPSR_dbg)
-	bl receive_from_debugger
-	msr spsr, r0
-
-	@ restore registers (r7 - r0)
-	bl receive_from_debugger @ r7
-	mov r7, r0
-	bl receive_from_debugger @ r6
-	mov r6, r0
-	bl receive_from_debugger @ r5
-	mov r5, r0
-	bl receive_from_debugger @ r4
-	mov r4, r0
-	bl receive_from_debugger @ r3
-	mov r3, r0
-	bl receive_from_debugger @ r2
-	mov r2, r0
-	bl receive_from_debugger @ r1
-	mov r1, r0
-	bl receive_from_debugger @ r0
-
-	@ resume addresss
-	m_receive_from_debugger lr
-
-	mrc p14, 0, r13, c10, c0, 0 @ XSCALE_DCSR
-	orr r13, r13, #1
-	mcr p14, 0, r13, c10, c0, 0 @ XSCALE_DCSR
-
-	@ branch back to application code, restoring CPSR
-	subs pc, lr, #0
-
-undef_handler:
-swi_handler:
-prefetch_abort_handler:
-data_abort_handler:
-irq_handler:
-fiq_handler:
-1:
-	b 1b
-
-send_to_debugger:
-	m_send_to_debugger r0
-	mov pc, lr
-
-receive_from_debugger:
-	m_receive_from_debugger r0
-	mov pc, lr
-
diff --git a/src/target/xscale/debug_handler.bin b/src/target/xscale/debug_handler.bin
deleted file mode 100755
index 2dde185..0000000
--- a/src/target/xscale/debug_handler.bin
+++ /dev/null
diff --git a/src/target/xscale/debug_handler.cmd b/src/target/xscale/debug_handler.cmd
deleted file mode 100644
index d943b13..0000000
--- a/src/target/xscale/debug_handler.cmd
+++ /dev/null
@@ -1,49 +0,0 @@
-/* identify the Entry Point  */
-ENTRY(reset_handler)
-
-/* specify the mini-ICache memory areas  */
-MEMORY
-{
-	mini_icache_0 (x)  				: ORIGIN = 0x0, LENGTH = 1024	/* first part of mini icache (sets 0-31) */
-	mini_icache_1 (x)  				: ORIGIN = 0x400, LENGTH = 1024	/* second part of mini icache (sets 0-31) */
-}
-
-/* now define the output sections  */
-SECTIONS
-{
-	.part1 :
-	{
-		LONG(0)
-		LONG(0)
-		LONG(0)
-		LONG(0)
-		LONG(0)
-		LONG(0)
-		LONG(0)
-		LONG(0)
-		*(.part1)
-	} >mini_icache_0
-
-	.part2 :
-	{
-		LONG(0)
-		LONG(0)
-		LONG(0)
-		LONG(0)
-		LONG(0)
-		LONG(0)
-		LONG(0)
-		LONG(0)
-		*(.part2)
-		FILL(0x0)
-	} >mini_icache_1
-
-	/DISCARD/ :
-	{
-		*(.text)
-		*(.glue_7)
-		*(.glue_7t)
-		*(.data)
-		*(.bss)
-	}
-}
diff --git a/src/transport/Makefile.am b/src/transport/Makefile.am
index 7c6224a..9076d9b 100644
--- a/src/transport/Makefile.am
+++ b/src/transport/Makefile.am
@@ -1,11 +1,4 @@
-include $(top_srcdir)/common.mk
-
-#METASOURCES = AUTO
-noinst_LTLIBRARIES = libtransport.la
-libtransport_la_SOURCES = \
-	transport.c
-
-noinst_HEADERS = \
-	transport.h
-
-MAINTAINERCLEANFILES = $(srcdir)/Makefile.in
+noinst_LTLIBRARIES += %D%/libtransport.la
+%C%_libtransport_la_SOURCES = \
+	%D%/transport.c \
+	%D%/transport.h
diff --git a/src/xsvf/Makefile.am b/src/xsvf/Makefile.am
index 1b9cfab..61e6fb9 100644
--- a/src/xsvf/Makefile.am
+++ b/src/xsvf/Makefile.am
@@ -1,8 +1,2 @@
-include $(top_srcdir)/common.mk
-
-METASOURCES = AUTO
-noinst_LTLIBRARIES = libxsvf.la
-noinst_HEADERS = xsvf.h
-libxsvf_la_SOURCES = xsvf.c
-
-MAINTAINERCLEANFILES = $(srcdir)/Makefile.in
+noinst_LTLIBRARIES += %D%/libxsvf.la
+%C%_libxsvf_la_SOURCES = %D%/xsvf.c %D%/xsvf.h
author	Tim Newsome <tim@sifive.com>	2017-06-13 11:52:50 -0700
committer	Tim Newsome <tim@sifive.com>	2017-06-13 11:52:50 -0700
commit	845c2f6b692736d3f6ee113908b98cb60ec60119 (patch)
tree	32b9315ff67d97c397184dfe98b7523fed95dcb1 /src
parent	7af58e6283c8e7d350b6b55c93a1d326326ed831 (diff)
parent	1025be363e2bf42f1613083223a2322cc3a9bd4c (diff)
download	riscv-openocd-845c2f6b692736d3f6ee113908b98cb60ec60119.zip riscv-openocd-845c2f6b692736d3f6ee113908b98cb60ec60119.tar.gz riscv-openocd-845c2f6b692736d3f6ee113908b98cb60ec60119.tar.bz2