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.

91 files changed, 9456 insertions, 2717 deletions

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/xscale/protocol.h b/src/target/armv8_cache.h
index cb01655..fa46e16 100644
--- a/src/target/xscale/protocol.h
+++ b/src/target/armv8_cache.h
@@ -1,6 +1,6 @@
 /***************************************************************************
- *   Copyright (C) 2006 by Dominic Rath                                    *
- *   Dominic.Rath@gmx.de                                                   *
+ *   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  *
@@ -15,54 +15,12 @@
  *   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_
 
-#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
+#include "armv8.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
+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);
 
-#define MODE_ANY 0x40
-#define MODE_CURRENT 0x80
-
-#define MODE_MASK 0x1f
-#define PSR_I 0x80
-#define PSR_F 0x40
-#define PSR_T 0x20
-
-#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_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)
-	}
-}