X-Git-Url: https://review.openocd.org/gitweb?p=openocd.git;a=blobdiff_plain;f=src%2Ftarget%2Farm_dpm.c;h=55f253e464ff95e730541939b8d44de1d03d2021;hp=012316bf8ad78e65be79874d95fe79ecb4239ef8;hb=d376f7f51831ca8816bb4aca00076b0668462775;hpb=a9761c90931101d280b9443126db017f96d92e3d

diff --git a/src/target/arm_dpm.c b/src/target/arm_dpm.c
index 012316bf8a..55f253e464 100644
--- a/src/target/arm_dpm.c
+++ b/src/target/arm_dpm.c
@@ -12,9 +12,7 @@
  * 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.,
- * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
 #ifdef HAVE_CONFIG_H
@@ -51,8 +49,8 @@
 
 /* Read coprocessor */
 static int dpm_mrc(struct target *target, int cpnum,
-		uint32_t op1, uint32_t op2, uint32_t CRn, uint32_t CRm,
-		uint32_t *value)
+	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;
@@ -63,8 +61,8 @@ static int dpm_mrc(struct target *target, int cpnum,
 		return retval;
 
 	LOG_DEBUG("MRC p%d, %d, r0, c%d, c%d, %d", cpnum,
-			(int) op1, (int) CRn,
-			(int) CRm, (int) op2);
+		(int) op1, (int) CRn,
+		(int) CRm, (int) op2);
 
 	/* read coprocessor register into R0; return via DCC */
 	retval = dpm->instr_read_data_r0(dpm,
@@ -76,8 +74,8 @@ static int dpm_mrc(struct target *target, int cpnum,
 }
 
 static int dpm_mcr(struct target *target, int cpnum,
-		uint32_t op1, uint32_t op2, uint32_t CRn, uint32_t CRm,
-		uint32_t value)
+	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;
@@ -88,8 +86,8 @@ static int dpm_mcr(struct target *target, int cpnum,
 		return retval;
 
 	LOG_DEBUG("MCR p%d, %d, r0, c%d, c%d, %d", cpnum,
-			(int) op1, (int) CRn,
-			(int) CRm, (int) op2);
+		(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,
@@ -109,7 +107,7 @@ static int dpm_mcr(struct target *target, int cpnum,
 /* Toggles between recorded core mode (USR, SVC, etc) and a temporary one.
  * Routines *must* restore the original mode before returning!!
  */
-static int dpm_modeswitch(struct arm_dpm *dpm, enum arm_mode mode)
+int dpm_modeswitch(struct arm_dpm *dpm, enum arm_mode mode)
 {
 	int retval;
 	uint32_t cpsr;
@@ -132,51 +130,54 @@ static int dpm_modeswitch(struct arm_dpm *dpm, enum arm_mode mode)
 	return retval;
 }
 
-/* just read the register -- rely on the core mode being right */
-static int dpm_read_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
+
+static int dpm_read_reg32(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
 {
 	uint32_t value;
-	int retval;
+	int retval = ERROR_FAIL;
 
 	switch (regnum) {
-	case 0 ... 14:
-		/* return via DCC:  "MCR p14, 0, Rnum, c0, c5, 0" */
-		retval = dpm->instr_read_data_dcc(dpm,
+		case 0 ... 14:
+			/* 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 15:	/* PC */
-		/* "MOV r0, pc"; then return via DCC */
-		retval = dpm->instr_read_data_r0(dpm, 0xe1a0000f, &value);
-
-		/* NOTE: this seems like a slightly awkward place to update
-		 * this value ... but if the PC gets written (the only way
-		 * to change what we compute), the arch spec says subsequent
-		 * reads return values which are "unpredictable".  So this
-		 * is always right except in those broken-by-intent cases.
-		 */
-		switch (dpm->arm->core_state) {
-		case ARM_STATE_ARM:
-			value -= 8;
-			break;
-		case ARM_STATE_THUMB:
-		case ARM_STATE_THUMB_EE:
-			value -= 4;
 			break;
-		case ARM_STATE_JAZELLE:
-			/* core-specific ... ? */
-			LOG_WARNING("Jazelle PC adjustment unknown");
+		case 15:/* PC
+			 * "MOV r0, pc"; then return via DCC */
+			retval = dpm->instr_read_data_r0(dpm, 0xe1a0000f, &value);
+
+			/* NOTE: this seems like a slightly awkward place to update
+			 * this value ... but if the PC gets written (the only way
+			 * to change what we compute), the arch spec says subsequent
+			 * reads return values which are "unpredictable".  So this
+			 * is always right except in those broken-by-intent cases.
+			 */
+			switch (dpm->arm->core_state) {
+				case ARM_STATE_ARM:
+					value -= 8;
+					break;
+				case ARM_STATE_THUMB:
+				case ARM_STATE_THUMB_EE:
+					value -= 4;
+					break;
+				case ARM_STATE_JAZELLE:
+					/* core-specific ... ? */
+					LOG_WARNING("Jazelle PC adjustment unknown");
+					break;
+				case ARM_STATE_AARCH64:
+					LOG_ERROR("AARCH64: 32bit read requested");
+					break;
+			}
 			break;
-		}
-		break;
-	default:
-		/* 16: "MRS r0, CPSR"; then return via DCC
-		 * 17: "MRS r0, SPSR"; then return via DCC
-		 */
-		retval = dpm->instr_read_data_r0(dpm,
+		default:
+			/* 16: "MRS r0, CPSR"; then return via DCC
+			 * 17: "MRS r0, SPSR"; then return via DCC
+			 */
+			retval = dpm->instr_read_data_r0(dpm,
 				ARMV4_5_MRS(0, regnum & 1),
 				&value);
-		break;
+			break;
 	}
 
 	if (retval == ERROR_OK) {
@@ -189,37 +190,84 @@ static int dpm_read_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
 	return retval;
 }
 
-/* just write the register -- rely on the core mode being right */
-static int dpm_write_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
+static int dpm_read_reg64(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
 {
-	int retval;
+	uint64_t value;
+	uint32_t i;
+	int retval = ERROR_FAIL;
+
+	switch (regnum) {
+		case 0 ... 30:
+			i = 0xd5130400 + regnum; /* msr dbgdtr_el0,reg */
+			retval = dpm->instr_read_data_dcc_64(dpm, i, &value);
+			break;
+		case 31: /* SP */
+			i = 0x910003e0;
+			retval = dpm->instr_read_data_r0_64(dpm, i, &value);
+			break;
+		case 32: /* PC */
+			i = 0xd53b4520;
+			retval = dpm->instr_read_data_r0_64(dpm, i, &value);
+			break;
+		case 33: /* CPSR */
+			i = 0xd53b4500;
+			retval = dpm->instr_read_data_r0_64(dpm, i, &value);
+			break;
+
+		default:
+			break;
+	}
+
+	if (retval == ERROR_OK) {
+		buf_set_u64(r->value, 0, 64, value);
+		r->valid = true;
+		r->dirty = false;
+		LOG_DEBUG("READ: %s, %16.16llx", r->name, (long long)value);
+	}
+
+	return retval;
+}
+
+
+/* just read the register -- rely on the core mode being right */
+static int dpm_read_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
+{
+	if (r->size == 64)
+		return dpm_read_reg64(dpm, r, regnum);
+	else
+		return dpm_read_reg32(dpm, r, regnum);
+}
+
+static int dpm_write_reg32(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
+{
+	int retval = ERROR_FAIL;
 	uint32_t value = buf_get_u32(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,
+		case 0 ... 14:
+			/* 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 15:	/* PC */
-		/* read r0 from DCC; then "MOV pc, r0" */
-		retval = dpm->instr_write_data_r0(dpm, 0xe1a0f000, value);
-		break;
-	default:
-		/* 16: read r0 from DCC, then "MSR r0, CPSR_cxsf"
-		 * 17: read r0 from DCC, then "MSR r0, SPSR_cxsf"
-		 */
-		retval = dpm->instr_write_data_r0(dpm,
+			break;
+		case 15:/* PC
+			 * read r0 from DCC; then "MOV pc, r0" */
+			retval = dpm->instr_write_data_r0(dpm, 0xe1a0f000, value);
+			break;
+		default:
+			/* 16: read r0 from DCC, then "MSR r0, CPSR_cxsf"
+			 * 17: read r0 from DCC, then "MSR r0, SPSR_cxsf"
+			 */
+			retval = dpm->instr_write_data_r0(dpm,
 				ARMV4_5_MSR_GP(0, 0xf, regnum & 1),
 				value);
-		if (retval != ERROR_OK)
-			return retval;
+			if (retval != ERROR_OK)
+				return retval;
 
-		if (regnum == 16 && dpm->instr_cpsr_sync)
-			retval = dpm->instr_cpsr_sync(dpm);
+			if (regnum == 16 && dpm->instr_cpsr_sync)
+				retval = dpm->instr_cpsr_sync(dpm);
 
-		break;
+			break;
 	}
 
 	if (retval == ERROR_OK) {
@@ -231,18 +279,62 @@ static int dpm_write_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
 }
 
 /**
- * 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).
+ * Write to program counter and switch the core state (arm/thumb) according to
+ * the address.
  */
-int arm_dpm_read_current_registers(struct arm_dpm *dpm)
+static int dpm_write_pc_core_state(struct arm_dpm *dpm, struct reg *r)
+{
+	uint32_t value = buf_get_u32(r->value, 0, 32);
+
+	/* read r0 from DCC; then "BX r0" */
+	return dpm->instr_write_data_r0(dpm, ARMV4_5_BX(0), value);
+}
+
+static int dpm_write_reg64(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
+{
+	int retval = ERROR_FAIL;
+	uint32_t i;
+	uint64_t value = buf_get_u64(r->value, 0, 64);
+
+	switch (regnum) {
+		case 0 ... 30:
+			i = 0xd5330400 + regnum;
+			retval = dpm->instr_write_data_dcc_64(dpm, i, value);
+			break;
+		case 32: /* PC */
+			i = 0xd51b4520;
+			retval = dpm->instr_write_data_r0_64(dpm, i, value);
+			break;
+		default:
+			LOG_DEBUG("register %s (%16.16llx) not defined", r->name,
+				  (unsigned long long)value);
+			break;
+	}
+
+	if (retval == ERROR_OK) {
+		r->dirty = false;
+		LOG_DEBUG("WRITE: %s, %16.16llx", r->name, (unsigned long long)value);
+	}
+
+	return retval;
+}
+
+/* just write the register -- rely on the core mode being right */
+static int dpm_write_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
+{
+	if (r->size == 64)
+		return dpm_write_reg64(dpm, r, regnum);
+	else
+		return dpm_write_reg32(dpm, r, regnum);
+}
+
+static int arm_dpm_read_current_registers_i(struct arm_dpm *dpm)
 {
 	struct arm *arm = dpm->arm;
-	uint32_t cpsr;
+	uint32_t cpsr, instr, core_regs;
 	int retval;
 	struct reg *r;
+	enum arm_state core_state = arm->core_state;
 
 	retval = dpm->prepare(dpm);
 	if (retval != ERROR_OK)
@@ -257,16 +349,25 @@ int arm_dpm_read_current_registers(struct arm_dpm *dpm)
 	}
 	r->dirty = true;
 
-	retval = dpm->instr_read_data_r0(dpm, ARMV4_5_MRS(0, 0), &cpsr);
+	if (core_state == ARM_STATE_AARCH64)
+		instr = 0xd53b4500;  /* mrs x0, dspsr_el0 */
+	else
+		instr = ARMV4_5_MRS(0, 0);
+	retval = dpm->instr_read_data_r0(dpm, instr, &cpsr);
 	if (retval != ERROR_OK)
 		goto fail;
 
 	/* update core mode and state, plus shadow mapping for R8..R14 */
 	arm_set_cpsr(arm, cpsr);
+	if (core_state == ARM_STATE_AARCH64)
+		/* arm_set_cpsr changes core_state, restore it for now */
+		arm->core_state = ARM_STATE_AARCH64;
+
+	core_regs = arm->core_cache->num_regs;
 
 	/* REVISIT we can probably avoid reading R1..R14, saving time... */
-	for (unsigned i = 1; i < 16; i++) {
-		r = arm_reg_current(arm, i);
+	for (unsigned i = 1; i < core_regs; i++) {
+		r = dpm->arm_reg_current(arm, i);
 		if (r->valid)
 			continue;
 
@@ -287,12 +388,29 @@ fail:
 	return retval;
 }
 
+/**
+ * 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 arm_dpm_read_current_registers(struct arm_dpm *dpm)
+{
+	return arm_dpm_read_current_registers_i(dpm);
+}
+
+int arm_dpm_read_current_registers_64(struct arm_dpm *dpm)
+{
+	return arm_dpm_read_current_registers_i(dpm);
+}
+
 /* 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 dpm_maybe_update_bpwp(struct arm_dpm *dpm, bool bpwp,
-		struct dpm_bpwp *xp, int *set_p)
+	struct dpm_bpwp *xp, int *set_p)
 {
 	int retval = ERROR_OK;
 	bool disable;
@@ -325,69 +443,24 @@ static int dpm_maybe_update_bpwp(struct arm_dpm *dpm, bool bpwp,
 
 	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);
+			disable ? "disable" : "enable",
+			target_name(dpm->arm->target),
+			(xp->number < 16) ? "break" : "watch",
+			xp->number & 0xf);
 done:
 	return retval;
 }
 
 static int dpm_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 arm_dpm_write_dirty_registers(struct arm_dpm *dpm, bool bpwp)
+
+static int arm_dpm_write_dirty_registers_32(struct arm_dpm *dpm)
 {
 	struct arm *arm = dpm->arm;
 	struct reg_cache *cache = arm->core_cache;
 	int retval;
 	bool did_write;
 
-	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 == dpm_add_breakpoint) {
-		for (unsigned i = 0; i < dpm->nbp; i++) {
-			struct dpm_bp *dbp = dpm->dbp + i;
-			struct breakpoint *bp = dbp->bp;
-
-			retval = dpm_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 = dpm_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.
-	 */
-
 	/* Scan the registers until we find one that's both dirty and
 	 * eligible for flushing.  Flush that and everything else that
 	 * shares the same core mode setting.  Typically this won't
@@ -429,8 +502,8 @@ int arm_dpm_write_dirty_registers(struct arm_dpm *dpm, bool bpwp)
 					 * since we haven't changed it yet
 					 */
 					if (arm->core_mode == ARM_MODE_FIQ
-							&& ARM_MODE_ANY
-								!= mode)
+					    && ARM_MODE_ANY
+					    != mode)
 						tmode = ARM_MODE_USR;
 					break;
 				case 16:
@@ -440,8 +513,7 @@ int arm_dpm_write_dirty_registers(struct arm_dpm *dpm, bool bpwp)
 				}
 
 				/* REVISIT error checks */
-				if (tmode != ARM_MODE_ANY)
-				{
+				if (tmode != ARM_MODE_ANY) {
 					retval = dpm_modeswitch(dpm, tmode);
 					if (retval != ERROR_OK)
 						goto done;
@@ -451,8 +523,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;
 		}
@@ -468,6 +540,19 @@ int arm_dpm_write_dirty_registers(struct arm_dpm *dpm, bool bpwp)
 		goto done;
 	arm->cpsr->dirty = false;
 
+	/* restore the PC, make sure to also switch the core state
+	 * to whatever it was set to with "arm core_state" command.
+	 * target code will have set PC to an appropriate resume address.
+	 */
+	retval = dpm_write_pc_core_state(dpm, arm->pc);
+	if (retval != ERROR_OK)
+		goto done;
+	/* on Cortex-A5 (as found on NXP VF610 SoC), BX instruction
+	 * executed in debug state doesn't appear to set the PC,
+	 * explicitly set it with a "MOV pc, r0". This doesn't influence
+	 * CPSR on Cortex-A9 so it should be OK. Maybe due to different
+	 * debug version?
+	 */
 	retval = dpm_write_reg(dpm, arm->pc, 15);
 	if (retval != ERROR_OK)
 		goto done;
@@ -479,6 +564,106 @@ int arm_dpm_write_dirty_registers(struct arm_dpm *dpm, bool bpwp)
 		goto done;
 	cache->reg_list[0].dirty = false;
 
+done:
+	return retval;
+}
+
+static int arm_dpm_write_dirty_registers_64(struct arm_dpm *dpm)
+{
+	struct arm *arm = dpm->arm;
+	struct reg_cache *cache = arm->core_cache;
+	int retval;
+
+	/* Scan the registers until we find one that's both dirty and
+	 * eligible for flushing.  Flush that and everything else that
+	 * shares the same core mode setting.  Typically this won't
+	 * actually find anything to do...
+	 */
+
+	/* check everything except our scratch register R0 */
+	for (unsigned i = 1; i <= 32; i++) {
+		struct arm_reg *r;
+		unsigned regnum;
+
+		if (!cache->reg_list[i].dirty)
+			continue;
+
+		r = cache->reg_list[i].arch_info;
+		regnum = r->num;
+		retval = dpm_write_reg(dpm,
+				       &cache->reg_list[i],
+				       regnum);
+		if (retval != ERROR_OK)
+			goto done;
+	}
+
+	/* flush R0 -- it's *very* dirty by now */
+	retval = dpm_write_reg(dpm, &cache->reg_list[0], 0);
+	if (retval != ERROR_OK)
+		goto done;
+	cache->reg_list[0].dirty = false;
+
+done:
+	return retval;
+}
+
+/**
+ * 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 arm_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 == dpm_add_breakpoint) {
+		for (unsigned i = 0; i < dpm->nbp; i++) {
+			struct dpm_bp *dbp = dpm->dbp + i;
+			struct breakpoint *bp = dbp->bp;
+
+			retval = dpm_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 = dpm_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.
+	 */
+
+	if (cache->reg_list[0].size == 64)
+		retval = arm_dpm_write_dirty_registers_64(dpm);
+	else
+		retval = arm_dpm_write_dirty_registers_32(dpm);
+
 	/* (void) */ dpm->finish(dpm);
 done:
 	return retval;
@@ -490,34 +675,34 @@ done:
  * or MODE_ANY.
  */
 static enum arm_mode dpm_mapmode(struct arm *arm,
-		unsigned num, enum arm_mode mode)
+	unsigned num, enum arm_mode mode)
 {
 	enum arm_mode amode = arm->core_mode;
 
 	/* don't switch if the mode is already correct */
 	if (amode == ARM_MODE_SYS)
-		 amode = ARM_MODE_USR;
+		amode = ARM_MODE_USR;
 	if (mode == amode)
 		return ARM_MODE_ANY;
 
 	switch (num) {
-	/* don't switch for non-shadowed registers (r0..r7, r15/pc, cpsr) */
-	case 0 ... 7:
-	case 15:
-	case 16:
-		break;
-	/* r8..r12 aren't shadowed for anything except FIQ */
-	case 8 ... 12:
-		if (mode == ARM_MODE_FIQ)
+		/* don't switch for non-shadowed registers (r0..r7, r15/pc, cpsr) */
+		case 0 ... 7:
+		case 15:
+		case 16:
+			break;
+		/* r8..r12 aren't shadowed for anything except FIQ */
+		case 8 ... 12:
+			if (mode == ARM_MODE_FIQ)
+				return mode;
+			break;
+		/* r13/sp, and r14/lr are always shadowed */
+		case 13:
+		case 14:
 			return mode;
-		break;
-	/* r13/sp, and r14/lr are always shadowed */
-	case 13:
-	case 14:
-		return mode;
-	default:
-		LOG_WARNING("invalid register #%u", num);
-		break;
+		default:
+			LOG_WARNING("invalid register #%u", num);
+			break;
 	}
 	return ARM_MODE_ANY;
 }
@@ -530,13 +715,13 @@ static enum arm_mode dpm_mapmode(struct arm *arm,
  */
 
 static int arm_dpm_read_core_reg(struct target *target, struct reg *r,
-		int regnum, enum arm_mode mode)
+	int regnum, enum arm_mode mode)
 {
 	struct arm_dpm *dpm = target_to_arm(target)->dpm;
 	int retval;
 
 	if (regnum < 0 || regnum > 16)
-		return ERROR_INVALID_ARGUMENTS;
+		return ERROR_COMMAND_SYNTAX_ERROR;
 
 	if (regnum == 16) {
 		if (mode != ARM_MODE_ANY)
@@ -572,14 +757,14 @@ fail:
 }
 
 static int arm_dpm_write_core_reg(struct target *target, struct reg *r,
-		int regnum, enum arm_mode mode, uint32_t value)
+	int regnum, enum arm_mode mode, uint8_t *value)
 {
 	struct arm_dpm *dpm = target_to_arm(target)->dpm;
 	int retval;
 
 
 	if (regnum < 0 || regnum > 16)
-		return ERROR_INVALID_ARGUMENTS;
+		return ERROR_COMMAND_SYNTAX_ERROR;
 
 	if (regnum == 16) {
 		if (mode != ARM_MODE_ANY)
@@ -649,14 +834,15 @@ static int arm_dpm_full_context(struct target *target)
 				did_read = true;
 				mode = r->mode;
 
-				/* For R8..R12 when we've entered debug
-				 * state in FIQ mode... patch 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)
-					mode = ARM_MODE_USR;
+				if (mode != ARM_MODE_ANY)
+					retval = dpm_modeswitch(dpm, mode);
+				else
+					retval = dpm_modeswitch(dpm, ARM_MODE_USR);
 
-				/* REVISIT error checks */
-				retval = dpm_modeswitch(dpm, mode);
 				if (retval != ERROR_OK)
 					goto done;
 			}
@@ -693,7 +879,7 @@ done:
  */
 
 static int dpm_bpwp_setup(struct arm_dpm *dpm, struct dpm_bpwp *xp,
-		uint32_t addr, uint32_t length)
+	uint32_t addr, uint32_t length)
 {
 	uint32_t control;
 
@@ -710,26 +896,26 @@ static int dpm_bpwp_setup(struct arm_dpm *dpm, struct dpm_bpwp *xp,
 	 * 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;
+		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;
-		}
+		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_INVALID_ARGUMENTS;
+		default:
+			LOG_ERROR("unsupported {break,watch}point length/alignment");
+			return ERROR_COMMAND_SYNTAX_ERROR;
 	}
 
 	/* other shared control bits:
@@ -743,7 +929,7 @@ static int dpm_bpwp_setup(struct arm_dpm *dpm, struct dpm_bpwp *xp,
 	xp->dirty = true;
 
 	LOG_DEBUG("BPWP: addr %8.8" PRIx32 ", control %" PRIx32 ", number %d",
-			xp->address, control, xp->number);
+		xp->address, control, xp->number);
 
 	/* hardware is updated in write_dirty_registers() */
 	return ERROR_OK;
@@ -756,7 +942,7 @@ static int dpm_add_breakpoint(struct target *target, struct breakpoint *bp)
 	int retval = ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 
 	if (bp->length < 2)
-		return ERROR_INVALID_ARGUMENTS;
+		return ERROR_COMMAND_SYNTAX_ERROR;
 	if (!dpm->bpwp_enable)
 		return retval;
 
@@ -781,7 +967,7 @@ static int dpm_remove_breakpoint(struct target *target, struct breakpoint *bp)
 {
 	struct arm *arm = target_to_arm(target);
 	struct arm_dpm *dpm = arm->dpm;
-	int retval = ERROR_INVALID_ARGUMENTS;
+	int retval = ERROR_COMMAND_SYNTAX_ERROR;
 
 	for (unsigned i = 0; i < dpm->nbp; i++) {
 		if (dpm->dbp[i].bp == bp) {
@@ -798,7 +984,7 @@ static int dpm_remove_breakpoint(struct target *target, struct breakpoint *bp)
 }
 
 static int dpm_watchpoint_setup(struct arm_dpm *dpm, unsigned index_t,
-		struct watchpoint *wp)
+	struct watchpoint *wp)
 {
 	int retval;
 	struct dpm_wp *dwp = dpm->dwp + index_t;
@@ -816,15 +1002,15 @@ static int dpm_watchpoint_setup(struct arm_dpm *dpm, unsigned index_t,
 
 	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;
+		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;
 
@@ -855,7 +1041,7 @@ static int dpm_remove_watchpoint(struct target *target, struct watchpoint *wp)
 {
 	struct arm *arm = target_to_arm(target);
 	struct arm_dpm *dpm = arm->dpm;
-	int retval = ERROR_INVALID_ARGUMENTS;
+	int retval = ERROR_COMMAND_SYNTAX_ERROR;
 
 	for (unsigned i = 0; i < dpm->nwp; i++) {
 		if (dpm->dwp[i].wp == wp) {
@@ -874,16 +1060,17 @@ static int dpm_remove_watchpoint(struct target *target, struct watchpoint *wp)
 void arm_dpm_report_wfar(struct arm_dpm *dpm, uint32_t addr)
 {
 	switch (dpm->arm->core_state) {
-	case ARM_STATE_ARM:
-		addr -= 8;
-		break;
-	case ARM_STATE_THUMB:
-	case ARM_STATE_THUMB_EE:
-		addr -= 4;
-		break;
-	case ARM_STATE_JAZELLE:
-		/* ?? */
-		break;
+		case ARM_STATE_ARM:
+			addr -= 8;
+			break;
+		case ARM_STATE_THUMB:
+		case ARM_STATE_THUMB_EE:
+			addr -= 4;
+			break;
+		case ARM_STATE_JAZELLE:
+		case ARM_STATE_AARCH64:
+			/* ?? */
+			break;
 	}
 	dpm->wp_pc = addr;
 }
@@ -902,25 +1089,25 @@ 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) */
+		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 */
-		target->debug_reason = DBG_REASON_DBGRQ;
-		break;
-	case 1:		/* HW breakpoint */
-	case 3:		/* SW BKPT */
-	case 5:		/* vector catch */
-		target->debug_reason = DBG_REASON_BREAKPOINT;
-		break;
-	case 2:		/* asynch watchpoint */
-	case 10:	/* precise watchpoint */
-		target->debug_reason = DBG_REASON_WATCHPOINT;
-		break;
-	default:
-		target->debug_reason = DBG_REASON_UNDEFINED;
-		break;
+		case 0:	/* HALT request from debugger */
+		case 4:	/* EDBGRQ */
+			target->debug_reason = DBG_REASON_DBGRQ;
+			break;
+		case 1:	/* HW breakpoint */
+		case 3:	/* SW BKPT */
+		case 5:	/* vector catch */
+			target->debug_reason = DBG_REASON_BREAKPOINT;
+			break;
+		case 2:	/* asynch watchpoint */
+		case 10:/* precise watchpoint */
+			target->debug_reason = DBG_REASON_WATCHPOINT;
+			break;
+		default:
+			target->debug_reason = DBG_REASON_UNDEFINED;
+			break;
 	}
 }
 
@@ -940,20 +1127,27 @@ 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;
 
 	/* register access setup */
 	arm->full_context = arm_dpm_full_context;
-	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;
+	arm->read_core_reg = arm->read_core_reg ? : arm_dpm_read_core_reg;
+	arm->write_core_reg = arm->write_core_reg ? : arm_dpm_write_core_reg;
+
+	if (arm->core_cache != NULL) {
+		if (arm->core_state == ARM_STATE_AARCH64) {
+			cache = armv8_build_reg_cache(target);
+			target->reg_cache = cache;
+		} else {
+			cache = arm_build_reg_cache(target, arm);
+			*register_get_last_cache_p(&target->reg_cache) = cache;
+		}
 
-	*register_get_last_cache_p(&target->reg_cache) = cache;
+		if (!cache)
+			return ERROR_FAIL;
+	}
 
 	/* coprocessor access setup */
 	arm->mrc = dpm_mrc;
@@ -969,12 +1163,21 @@ int arm_dpm_setup(struct arm_dpm *dpm)
 	target->type->add_watchpoint = dpm_add_watchpoint;
 	target->type->remove_watchpoint = dpm_remove_watchpoint;
 
+
+	if (dpm->arm_reg_current == 0)
+		dpm->arm_reg_current = arm_reg_current;
+
 	/* FIXME add vector catch support */
 
-	dpm->nbp = 1 + ((dpm->didr >> 24) & 0xf);
-	dpm->dbp = calloc(dpm->nbp, sizeof *dpm->dbp);
+	if (arm->core_state == ARM_STATE_AARCH64) {
+		dpm->nbp = 1 + ((dpm->didr >> 24) & 0xf);
+		dpm->nwp = 1 + ((dpm->didr >> 28) & 0xf);
+	} else {
+		dpm->nbp = 1 + ((dpm->didr >> 12) & 0xf);
+		dpm->nwp = 1 + ((dpm->didr >> 20) & 0xf);
+	}
 
-	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) {
@@ -984,7 +1187,7 @@ int arm_dpm_setup(struct arm_dpm *dpm)
 	}
 
 	LOG_INFO("%s: hardware has %d breakpoints, %d watchpoints",
-			target_name(target), dpm->nbp, dpm->nwp);
+		target_name(target), dpm->nbp, dpm->nwp);
 
 	/* REVISIT ... and some of those breakpoints could match
 	 * execution context IDs...