diff --git a/src/target/Makefile.am b/src/target/Makefile.am
index 0021f44d5..e936d3f40 100644
--- a/src/target/Makefile.am
+++ b/src/target/Makefile.am
@@ -75,6 +75,7 @@ ARMV7_SRC = \
 
 ARMV8_SRC = \
 	%D%/armv8_dpm.c \
+	%D%/armv8_opcodes.c \
 	%D%/aarch64.c \
 	%D%/armv8.c \
 	%D%/armv8_cache.c
diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index 8d74dbdbc..824a042de 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -90,7 +90,10 @@ static int aarch64_restore_system_control_reg(struct target *target)
 					return retval;
 			break;
 			default:
-				LOG_DEBUG("unknow cpu state 0x%x" PRIx32, armv8->arm.core_state);
+				retval = armv8->arm.mcr(target, 15, 0, 0, 1, 0, aarch64->system_control_reg);
+				if (retval != ERROR_OK)
+					return retval;
+				break;
 			}
 	}
 	return retval;
@@ -531,6 +534,7 @@ static int aarch64_instr_write_data_r0(struct arm_dpm *dpm,
 	uint32_t opcode, uint32_t data)
 {
 	struct aarch64_common *a8 = dpm_to_a8(dpm);
+
 	uint32_t dscr = DSCR_ITE;
 	int retval;
 
@@ -539,9 +543,7 @@ static int aarch64_instr_write_data_r0(struct arm_dpm *dpm,
 		return retval;
 
 	retval = aarch64_exec_opcode(
-			a8->armv8_common.arm.target,
-			ARMV8_MRS(SYSTEM_DBG_DTRRX_EL0, 0),
-			&dscr);
+			a8->armv8_common.arm.target, armv8_opcode(&a8->armv8_common, READ_REG_DTRRX), &dscr);
 	if (retval != ERROR_OK)
 		return retval;
 
@@ -584,12 +586,11 @@ static int aarch64_instr_write_data_r0_64(struct arm_dpm *dpm,
 static int aarch64_instr_cpsr_sync(struct arm_dpm *dpm)
 {
 	struct target *target = dpm->arm->target;
+	struct armv8_common *armv8 = target_to_armv8(target);
 	uint32_t dscr = DSCR_ITE;
 
 	/* "Prefetch flush" after modifying execution status in CPSR */
-	return aarch64_exec_opcode(target,
-			DSB_SY,
-			&dscr);
+	return aarch64_exec_opcode(target, armv8_opcode(armv8, ARMV8_OPC_DSB_SY), &dscr);
 }
 
 static int aarch64_instr_read_data_dcc(struct arm_dpm *dpm,
@@ -645,9 +646,7 @@ static int aarch64_instr_read_data_r0(struct arm_dpm *dpm,
 
 	/* write R0 to DCC */
 	retval = aarch64_exec_opcode(
-			a8->armv8_common.arm.target,
-			ARMV8_MSR_GP(SYSTEM_DBG_DTRTX_EL0, 0),  /* msr dbgdtr_el0, x0 */
-			&dscr);
+			a8->armv8_common.arm.target, armv8_opcode(&a8->armv8_common, WRITE_REG_DTRTX), &dscr);
 	if (retval != ERROR_OK)
 		return retval;
 
@@ -999,20 +998,6 @@ static int aarch64_internal_restore(struct target *target, int current,
 	/* registers are now invalid */
 	register_cache_invalidate(arm->core_cache);
 
-#if 0
-	/* the front-end may request us not to handle breakpoints */
-	if (handle_breakpoints) {
-		/* Single step past breakpoint at current address */
-		breakpoint = breakpoint_find(target, resume_pc);
-		if (breakpoint) {
-			LOG_DEBUG("unset breakpoint at 0x%8.8x", breakpoint->address);
-			cortex_m3_unset_breakpoint(target, breakpoint);
-			cortex_m3_single_step_core(target);
-			cortex_m3_set_breakpoint(target, breakpoint);
-		}
-	}
-#endif
-
 	return retval;
 }
 
@@ -1202,8 +1187,10 @@ static int aarch64_post_debug_entry(struct target *target)
 	struct armv8_common *armv8 = &aarch64->armv8_common;
 	int retval;
 
+	/* clear sticky errors */
 	mem_ap_write_atomic_u32(armv8->debug_ap,
-				    armv8->debug_base + CPUV8_DBG_DRCR, 1<<2);
+				    armv8->debug_base + CPUV8_DBG_DRCR, DRCR_CSE);
+
 	switch (armv8->arm.core_mode) {
 		case ARMV8_64_EL0T:
 		case ARMV8_64_EL1T:
@@ -1234,8 +1221,12 @@ static int aarch64_post_debug_entry(struct target *target)
 				return retval;
 		break;
 		default:
-			LOG_DEBUG("unknow cpu state 0x%x" PRIx32, armv8->arm.core_state);
+			retval = armv8->arm.mrc(target, 15, 0, 0, 1, 0, &aarch64->system_control_reg);
+			if (retval != ERROR_OK)
+				return retval;
+			break;
 	}
+
 	LOG_DEBUG("System_register: %8.8" PRIx32, aarch64->system_control_reg);
 	aarch64->system_control_reg_curr = aarch64->system_control_reg;
 
diff --git a/src/target/armv8.c b/src/target/armv8.c
index c351dfdff..306a06e7a 100644
--- a/src/target/armv8.c
+++ b/src/target/armv8.c
@@ -38,7 +38,7 @@
 #include "target_type.h"
 
 static const char * const armv8_state_strings[] = {
-	"ARM", "Thumb", "Jazelle", "ThumbEE", "ARM64",
+	"AArch32", "Thumb", "Jazelle", "ThumbEE", "AArch64",
 };
 
 static const struct {
@@ -52,6 +52,30 @@ static const struct {
 } 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,
@@ -260,9 +284,59 @@ void armv8_set_cpsr(struct arm *arm, uint32_t cpsr)
 		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,
+			T32_FMTITR(ARMV4_5_MRC(15, 0, 0, 5, 0, 0)),
+			&dfsr);
+	if (retval != ERROR_OK)
+		goto done;
+
+	retval = dpm->instr_read_data_r0(dpm,
+			T32_FMTITR(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,
+			T32_FMTITR(ARMV4_5_MRC(15, 0, 0, 6, 0, 0)),
+			&dfar);
+	if (retval != ERROR_OK)
+		goto done;
+
+	retval = dpm->instr_read_data_r0(dpm,
+			T32_FMTITR(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 void armv8_show_fault_registers(struct target *target)
 {
-	/* TODO */
+	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)
@@ -294,6 +368,45 @@ static uint8_t armv8_pa_size(uint32_t ps)
 	return ret;
 }
 
+static 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,
+			T32_FMTITR(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 int armv8_read_ttbcr(struct target *target)
 {
 	struct armv8_common *armv8 = target_to_armv8(target);
@@ -528,14 +641,13 @@ static int armv8_read_mpidr(struct target *target)
 	struct armv8_common *armv8 = target_to_armv8(target);
 	struct arm_dpm *dpm = armv8->arm.dpm;
 	uint32_t mpidr;
+
 	retval = dpm->prepare(dpm);
 	if (retval != ERROR_OK)
 		goto done;
 	/* MRC p15,0,<Rd>,c0,c0,5; read Multiprocessor ID register*/
 
-	retval = dpm->instr_read_data_r0(dpm,
-			ARMV8_MRS(SYSTEM_MPIDR, 0),
-			&mpidr);
+	retval = dpm->instr_read_data_r0(dpm, armv8_opcode(armv8, READ_REG_MPIDR), &mpidr);
 	if (retval != ERROR_OK)
 		goto done;
 	if (mpidr & 1<<31) {
@@ -566,18 +678,21 @@ int armv8_identify_cache(struct target *target)
 	uint32_t cache_selected, clidr;
 	uint32_t cache_i_reg, cache_d_reg;
 	struct armv8_cache_common *cache = &(armv8->armv8_mmu.armv8_cache);
-	armv8_read_ttbcr(target);
-	retval = dpm->prepare(dpm);
+	int is_aarch64 = armv8->arm.core_state == ARM_STATE_AARCH64;
 
+	retval = is_aarch64 ? armv8_read_ttbcr(target) : armv8_read_ttbcr32(target);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = dpm->prepare(dpm);
 	if (retval != ERROR_OK)
 		goto done;
-	/*	retrieve CLIDR
-	 *	mrc p15, 1, r0, c0, c0, 1		@ read clidr */
-	retval = dpm->instr_read_data_r0(dpm,
-			ARMV8_MRS(SYSTEM_CLIDR, 0),
-			&clidr);
+
+	/*	retrieve CLIDR */
+	retval = dpm->instr_read_data_r0(dpm, armv8_opcode(armv8, READ_REG_CLIDR), &clidr);
 	if (retval != ERROR_OK)
 		goto done;
+
 	clidr = (clidr & 0x7000000) >> 23;
 	LOG_INFO("number of cache level %" PRIx32, (uint32_t)(clidr / 2));
 	if ((clidr / 2) > 1) {
@@ -586,18 +701,13 @@ int armv8_identify_cache(struct target *target)
 		LOG_ERROR("cache l2 present :not supported");
 	}
 	/*	retrieve selected cache*/
-	retval = dpm->instr_read_data_r0(dpm,
-			ARMV8_MRS(SYSTEM_CSSELR, 0),
-			&cache_selected);
+	retval = dpm->instr_read_data_r0(dpm, armv8_opcode(armv8, READ_REG_CSSELR), &cache_selected);
 	if (retval != ERROR_OK)
 		goto done;
 
-
 	/* select instruction cache
 	 *	[0]  : 1 instruction cache selection , 0 data cache selection */
-	retval = dpm->instr_write_data_r0(dpm,
-			ARMV8_MRS(SYSTEM_CSSELR, 0),
-			1);
+	retval = dpm->instr_write_data_r0(dpm, armv8_opcode(armv8, WRITE_REG_CSSELR), 1);
 	if (retval != ERROR_OK)
 		goto done;
 
@@ -605,30 +715,21 @@ int armv8_identify_cache(struct target *target)
 	 * MRC P15,1,<RT>,C0, C0,0 ;on cortex A9 read CCSIDR
 	 * [2:0] line size	001 eight word per line
 	 * [27:13] NumSet 0x7f 16KB, 0xff 32Kbytes, 0x1ff 64Kbytes */
-	retval = dpm->instr_read_data_r0(dpm,
-			ARMV8_MRS(SYSTEM_CCSIDR, 0),
-			&cache_i_reg);
+	retval = dpm->instr_read_data_r0(dpm, armv8_opcode(armv8, READ_REG_CCSIDR), &cache_i_reg);
 	if (retval != ERROR_OK)
 		goto done;
 
 	/*	select data cache*/
-	retval = dpm->instr_write_data_r0(dpm,
-			ARMV8_MRS(SYSTEM_CSSELR, 0),
-			0);
+	retval = dpm->instr_write_data_r0(dpm, armv8_opcode(armv8, WRITE_REG_CSSELR), 0);
 	if (retval != ERROR_OK)
 		goto done;
 
-	retval = dpm->instr_read_data_r0(dpm,
-			ARMV8_MRS(SYSTEM_CCSIDR, 0),
-			&cache_d_reg);
+	retval = dpm->instr_read_data_r0(dpm, armv8_opcode(armv8, READ_REG_CCSIDR), &cache_d_reg);
 	if (retval != ERROR_OK)
 		goto done;
 
 	/*	restore selected cache	*/
-	dpm->instr_write_data_r0(dpm,
-		ARMV8_MRS(SYSTEM_CSSELR, 0),
-		cache_selected);
-
+	dpm->instr_write_data_r0(dpm, armv8_opcode(armv8, WRITE_REG_CSSELR), cache_selected);
 	if (retval != ERROR_OK)
 		goto done;
 	dpm->finish(dpm);
@@ -770,6 +871,7 @@ int armv8_arch_state(struct target *target)
 
 	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);
diff --git a/src/target/armv8.h b/src/target/armv8.h
index f6859b299..fa3674b14 100644
--- a/src/target/armv8.h
+++ b/src/target/armv8.h
@@ -116,6 +116,10 @@ struct armv8_mmu_common {
 	uint64_t ttbr0_mask;/*  masked to be used  */
 	uint32_t os_border;
 
+	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;
@@ -133,6 +137,8 @@ struct armv8_common {
 	uint32_t cti_base;
 	struct adiv5_ap *debug_ap;
 
+	const uint32_t *opcodes;
+
 	/* mdir */
 	uint8_t multi_processor_system;
 	uint8_t cluster_id;
@@ -144,7 +150,6 @@ struct armv8_common {
 	uint32_t page_size;
 	uint64_t ttbr_base;
 
-	/* cache specific to V7 Memory Management Unit compatible with v4_5*/
 	struct armv8_mmu_common armv8_mmu;
 
 	/* Direct processor core register read and writes */
diff --git a/src/target/armv8_dpm.c b/src/target/armv8_dpm.c
index 8c0d45a3f..c3d5ec4de 100644
--- a/src/target/armv8_dpm.c
+++ b/src/target/armv8_dpm.c
@@ -164,6 +164,7 @@ static int dpmv8_msr(struct target *target, uint32_t op0,
  */
 int dpmv8_modeswitch(struct arm_dpm *dpm, enum arm_mode mode)
 {
+	struct armv8_common *armv8 = (struct armv8_common *)dpm->arm->arch_info;
 	int retval;
 	uint32_t cpsr;
 
@@ -199,7 +200,7 @@ int dpmv8_modeswitch(struct arm_dpm *dpm, enum arm_mode mode)
 	}
 
 
-	retval = dpm->instr_write_data_r0(dpm, ARMV8_MSR_DSPSR(0), cpsr);
+	retval = dpm->instr_write_data_r0(dpm, armv8_opcode(armv8, WRITE_REG_DSPSR), cpsr);
 	if (retval != ERROR_OK)
 		return retval;
 
@@ -209,6 +210,86 @@ int dpmv8_modeswitch(struct arm_dpm *dpm, enum arm_mode mode)
 	return retval;
 }
 
+static int dpmv8_read_reg32(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
+{
+	uint32_t value;
+	int retval = ERROR_FAIL;
+	bool valid = true;
+
+	switch (regnum) {
+	case 0 ... 14:
+		/* return via DCC:  "MCR p14, 0, Rnum, c0, c5, 0" */
+		retval = dpm->instr_read_data_dcc(dpm,
+			T32_FMTITR(ARMV4_5_MCR(14, 0, regnum, 0, 5, 0)),
+			&value);
+		break;
+		case ARMV8_R31:
+			retval = dpm->instr_read_data_dcc(dpm,
+				T32_FMTITR(ARMV4_5_MCR(14, 0, 13, 0, 5, 0)),
+				&value);
+				break;
+		case ARMV8_PC:
+			retval = dpm->instr_read_data_r0(dpm,
+				T32_FMTITR(ARMV8_MRC_DLR(0)),
+				&value);
+			break;
+		case ARMV8_xPSR:
+			retval = dpm->instr_read_data_r0(dpm,
+				T32_FMTITR(ARMV8_MRC_DSPSR(0)),
+				&value);
+			break;
+		default:
+			LOG_DEBUG("READ: %s ignored", r->name);
+			retval = ERROR_OK;
+			value = 0xFFFFFFFF;
+			valid = false;
+			break;
+	}
+
+	if (retval == ERROR_OK) {
+		r->valid = valid;
+		r->dirty = false;
+		buf_set_u64(r->value, 0, 32, value);
+		LOG_DEBUG("READ: %s, %8.8x", r->name, (unsigned) value);
+	}
+	return retval;
+}
+
+static int dpmv8_write_reg32(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
+{
+	int retval;
+	uint64_t value = buf_get_u64(r->value, 0, 32);
+
+	switch (regnum) {
+		case 0 ... 14:
+			/* load register from DCC:  "MRC p14, 0, Rnum, c0, c5, 0" */
+			retval = dpm->instr_write_data_dcc(dpm,
+					T32_FMTITR(ARMV4_5_MRC(14, 0, regnum, 0, 5, 0)), value);
+			break;
+		case ARMV8_PC:/* PC
+			 * read r0 from DCC; then "MOV pc, r0" */
+			retval = dpm->instr_write_data_r0(dpm,
+					T32_FMTITR(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,
+					T32_FMTITR(ARMV8_MCR_DSPSR(0)), value);
+			break;
+		default:
+			retval = ERROR_OK;
+			LOG_DEBUG("WRITE: %s ignored", r->name);
+			break;
+	}
+
+	if (retval == ERROR_OK) {
+		r->dirty = false;
+		LOG_DEBUG("WRITE: %s, %8.8x", r->name, (unsigned) value);
+	}
+
+	return retval;
+}
+
 /* just read the register -- rely on the core mode being right */
 static int dpmv8_read_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
 {
@@ -222,20 +303,21 @@ static int dpmv8_read_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
 				ARMV8_MSR_GP(SYSTEM_DBG_DBGDTR_EL0, regnum),
 				&value_64);
 			break;
-		case 31:
+		case ARMV8_R31:
 			retval = dpm->instr_read_data_r0_64(dpm,
 				ARMV8_MOVFSP_64(0),
 				&value_64);
 			break;
-		case 32:
+		case ARMV8_PC:
 			retval = dpm->instr_read_data_r0_64(dpm,
 				ARMV8_MRS_DLR(0),
 				&value_64);
 			break;
-		case 33:
+		case ARMV8_xPSR:
 			retval = dpm->instr_read_data_r0(dpm,
 				ARMV8_MRS_DSPSR(0),
 				&value);
+			break;
 		default:
 			LOG_DEBUG("READ: %s fail", r->name);
 			break;
@@ -244,11 +326,13 @@ static int dpmv8_read_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
 	if (retval == ERROR_OK) {
 		r->valid = true;
 		r->dirty = false;
-		buf_set_u64(r->value, 0, 32, value_64);
-		if (r->size == 64)
+		if (r->size == 64) {
+			buf_set_u64(r->value, 0, 64, value_64);
 			LOG_DEBUG("READ: %s, %16.8llx", r->name, (unsigned long long) value_64);
-		else
+		} else {
+			buf_set_u32(r->value, 0, 32, value);
 			LOG_DEBUG("READ: %s, %8.8x", r->name, (unsigned) value);
+		}
 	}
 	return retval;
 }
@@ -267,23 +351,24 @@ static int dpmv8_write_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
 				ARMV8_MRS(SYSTEM_DBG_DBGDTR_EL0, regnum),
 				value_64);
 			break;
-		case 31:
+		case ARMV8_R31:
 			value_64 = buf_get_u64(r->value, 0, 64);
 			retval = dpm->instr_write_data_r0_64(dpm,
 				ARMV8_MOVTSP_64(0),
 				value_64);
 			break;
-		case 32:
+		case ARMV8_PC:
 			value_64 = buf_get_u64(r->value, 0, 64);
 			retval = dpm->instr_write_data_r0_64(dpm,
 				ARMV8_MSR_DLR(0),
 				value_64);
 			break;
-		case 33:
+		case ARMV8_xPSR:
 			value = buf_get_u32(r->value, 0, 32);
 			retval = dpm->instr_write_data_r0(dpm,
 				ARMV8_MSR_DSPSR(0),
 				value);
+			break;
 		default:
 			LOG_DEBUG("write: %s fail", r->name);
 			break;
@@ -301,6 +386,36 @@ static int dpmv8_write_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
 	return retval;
 }
 
+static inline enum arm_state dpm_get_core_state(uint32_t dscr)
+{
+	int el = (dscr >> 8) & 0x3;
+	int rw = (dscr >> 10) & 0xF;
+
+	LOG_DEBUG("EL:%i, RW:0x%x", el, rw);
+
+	/* DSCR.RW = 0b1111 - all EL are using AArch64 state */
+	if (rw == 0xF)
+		return ARM_STATE_AARCH64;
+
+	/* DSCR.RW = 0b1110 - all EL > 0 are using AArch64 state */
+	if (rw == 0xE && el > 0)
+		return ARM_STATE_AARCH64;
+
+	/* DSCR.RW = 0b110x - all EL > 1 are using Aarch64 state */
+	if ((rw & 0xE) == 0xC && el > 1)
+		return ARM_STATE_AARCH64;
+
+	/* DSCR.RW = 0b10xx - all EL > 2 are using Aarch64 state */
+	if ((rw & 0xC) == 0x8 && el > 2)
+		return ARM_STATE_AARCH64;
+
+	/* DSCR.RW = 0b0xxx - all EL are using AArch32 state */
+	if ((rw & 0x8) == 0)
+		return ARM_STATE_ARM;
+
+	return ARM_STATE_ARM;
+}
+
 /**
  * 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).
@@ -311,7 +426,10 @@ static int dpmv8_write_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
 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;
+	enum arm_state core_state;
 	uint32_t cpsr;
+
 	int retval;
 	struct reg *r;
 
@@ -319,16 +437,22 @@ int armv8_dpm_read_current_registers(struct arm_dpm *dpm)
 	if (retval != ERROR_OK)
 		return retval;
 
+	core_state = dpm_get_core_state(dpm->dscr);
+
+	armv8_select_opcodes(armv8, core_state);
+
 	/* read R0 first (it's used for scratch), then CPSR */
 	r = arm->core_cache->reg_list + 0;
 	if (!r->valid) {
-		retval = dpmv8_read_reg(dpm, r, 0);
+		retval = core_state == ARM_STATE_AARCH64 ?
+					dpmv8_read_reg(dpm, r, 0) : dpmv8_read_reg32(dpm, r, 0);
 		if (retval != ERROR_OK)
 			goto fail;
 	}
 	r->dirty = true;
+
 	/* read cpsr to r0 and get it back */
-	retval = dpm->instr_read_data_r0(dpm, ARMV8_MRS_DSPSR(0), &cpsr);
+	retval = dpm->instr_read_data_r0(dpm, armv8_opcode(armv8, READ_REG_DSPSR), &cpsr);
 	if (retval != ERROR_OK)
 		goto fail;
 
@@ -341,7 +465,9 @@ int armv8_dpm_read_current_registers(struct arm_dpm *dpm)
 		if (r->valid)
 			continue;
 
-		retval = dpmv8_read_reg(dpm, r, i);
+		retval = core_state == ARM_STATE_AARCH64 ?
+					dpmv8_read_reg(dpm, r, i) : dpmv8_read_reg32(dpm, r, i);
+
 		if (retval != ERROR_OK)
 			goto fail;
 	}
@@ -419,6 +545,7 @@ 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;
+	bool is_aarch64 = arm->core_state == ARM_STATE_AARCH64;
 
 	retval = dpm->prepare(dpm);
 	if (retval != ERROR_OK)
@@ -480,9 +607,8 @@ int armv8_dpm_write_dirty_registers(struct arm_dpm *dpm, bool bpwp)
 		r = cache->reg_list[i].arch_info;
 		regnum = r->num;
 
-		retval = dpmv8_write_reg(dpm,
-				&cache->reg_list[i],
-				regnum);
+		retval = is_aarch64 ? dpmv8_write_reg(dpm, &cache->reg_list[i], regnum)
+				: dpmv8_write_reg32(dpm, &cache->reg_list[i], regnum);
 		if (retval != ERROR_OK)
 			goto done;
 	}
@@ -497,13 +623,15 @@ int armv8_dpm_write_dirty_registers(struct arm_dpm *dpm, bool bpwp)
 		goto done;
 	arm->cpsr->dirty = false;
 
-	retval = dpmv8_write_reg(dpm, arm->pc, (arm->core_cache->num_regs - 2));
+	retval = is_aarch64 ? dpmv8_write_reg(dpm, arm->pc, (arm->core_cache->num_regs - 2))
+			: dpmv8_write_reg32(dpm, arm->pc, (arm->core_cache->num_regs - 2));
 	if (retval != ERROR_OK)
 		goto done;
 	arm->pc->dirty = false;
 
 	/* flush R0 -- it's *very* dirty by now */
-	retval = dpmv8_write_reg(dpm, &cache->reg_list[0], 0);
+	retval = is_aarch64 ? dpmv8_write_reg(dpm, &cache->reg_list[0], 0)
+			: dpmv8_write_reg32(dpm, &cache->reg_list[0], 0);
 	if (retval != ERROR_OK)
 		goto done;
 	cache->reg_list[0].dirty = false;
@@ -538,7 +666,8 @@ static int armv8_dpm_read_core_reg(struct target *target, struct reg *r,
 	if (retval != ERROR_OK)
 		return retval;
 
-	retval = dpmv8_read_reg(dpm, r, regnum);
+	retval = arm->core_state == ARM_STATE_AARCH64 ?
+			dpmv8_read_reg(dpm, r, regnum) : dpmv8_read_reg32(dpm, r, regnum);
 	if (retval != ERROR_OK)
 		goto fail;
 
@@ -566,7 +695,9 @@ static int armv8_dpm_write_core_reg(struct target *target, struct reg *r,
 	if (retval != ERROR_OK)
 		return retval;
 
-	retval = dpmv8_write_reg(dpm, r, regnum);
+	retval = arm->core_state == ARM_STATE_AARCH64 ?
+			dpmv8_write_reg(dpm, r, regnum) : dpmv8_write_reg32(dpm, r, regnum);
+
 	/* always clean up, regardless of error */
 
 	/* (void) */ dpm->finish(dpm);
diff --git a/src/target/armv8_opcodes.c b/src/target/armv8_opcodes.c
new file mode 100644
index 000000000..78b60e045
--- /dev/null
+++ b/src/target/armv8_opcodes.c
@@ -0,0 +1,66 @@
+/*
+ * 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_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,
+};
+
+static const uint32_t t32_opcodes[ARMV8_OPC_NUM] = {
+		[READ_REG_CLIDR]	= T32_FMTITR(ARMV4_5_MRC(15, 1, 0, 0, 0, 1)),
+		[READ_REG_CSSELR]	= T32_FMTITR(ARMV4_5_MRC(15, 2, 0, 0, 0, 0)),
+		[READ_REG_CCSIDR]	= T32_FMTITR(ARMV4_5_MRC(15, 1, 0, 0, 0, 0)),
+		[WRITE_REG_CSSELR]	= T32_FMTITR(ARMV4_5_MCR(15, 2, 0, 0, 0, 0)),
+		[READ_REG_MPIDR]	= T32_FMTITR(ARMV4_5_MRC(15, 0, 0, 0, 0, 5)),
+		[READ_REG_DTRRX]	= T32_FMTITR(ARMV4_5_MRC(14, 0, 0, 0, 5, 0)),
+		[WRITE_REG_DTRTX]	= T32_FMTITR(ARMV4_5_MCR(14, 0, 0, 0, 5, 0)),
+		[WRITE_REG_DSPSR]	= T32_FMTITR(ARMV8_MCR_DSPSR(0)),
+		[READ_REG_DSPSR]	= T32_FMTITR(ARMV8_MRC_DSPSR(0)),
+		[ARMV8_OPC_DSB_SY]	= T32_FMTITR(ARMV8_DSB_SY_T1),
+};
+
+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
index 89d7440a8..9f5ebf8dc 100644
--- a/src/target/armv8_opcodes.h
+++ b/src/target/armv8_opcodes.h
@@ -95,8 +95,6 @@
 #define SYSTEM_TTBR0_EL3		0b1111000100000000
 #define SYSTEM_TTBR1_EL1		0b1100000100000001
 
-
-
 #define ARMV8_MRS_DSPSR(Rt)	(0xd53b4500 | (Rt))
 #define ARMV8_MSR_DSPSR(Rt)	(0xd51b4500 | (Rt))
 #define ARMV8_MRS_DLR(Rt)	(0xd53b4520 | (Rt))
@@ -105,11 +103,23 @@
 /* T32 ITR format */
 #define T32_FMTITR(instr) (((instr & 0x0000FFFF) << 16) | ((instr & 0xFFFF0000) >> 16))
 
+/* 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 DSB_SY				0xd5033F9F
+#define ARMV8_DSB_SY				0xd5033F9F
+#define ARMV8_DSB_SY_T1				0xf3bf8f4f
+
 #define ARMV8_MRS(System, Rt)	(0xd5300000 | ((System) << 5) | (Rt))
 /* ARM V8 Move to system register. */
 #define ARMV8_MSR_GP(System, Rt) \
@@ -128,4 +138,21 @@
 
 #define ARMV8_SYS(System, Rt) (0xD5080000 | ((System) << 5) | Rt)
 
-#endif /* __ARM_OPCODES_H */
+enum armv8_opcode {
+	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_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 */