aarch64: fix debug entry from EL0

If we enter debug state from EL0, some registers are not accessible.
Temporarily move to EL1H and back to gain access. Also, fix
armv8_dpm_modeswitch() to not immediately restore the previous state
on elevating the privilege level.

Change-Id: Ic2a92109230ff4eb6834c00ef544397a5b7ad56a
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Reviewed-on: http://openocd.zylin.com/4461
Tested-by: jenkins
Reviewed-by: Matthias Welwarsky <matthias@welwarsky.de>

5 files changed, 26 insertions, 7 deletions

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index 0630ffb..b586e24 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -1861,7 +1861,7 @@ static int aarch64_write_cpu_memory(struct target *target,
 	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);
+		armv8_dpm_handle_exception(dpm, true);
 		return ERROR_FAIL;
 	}
 
@@ -2080,7 +2080,7 @@ static int aarch64_read_cpu_memory(struct target *target,
 	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);
+		armv8_dpm_handle_exception(dpm, true);
 		return ERROR_FAIL;
 	}
 
diff --git a/src/target/armv8.c b/src/target/armv8.c
index 86bb870..e8c700a 100644
--- a/src/target/armv8.c
+++ b/src/target/armv8.c
@@ -620,6 +620,7 @@ void armv8_select_reg_access(struct armv8_common *armv8, bool is_aarch64)
 int armv8_read_mpidr(struct armv8_common *armv8)
 {
 	int retval = ERROR_FAIL;
+	struct arm *arm = &armv8->arm;
 	struct arm_dpm *dpm = armv8->arm.dpm;
 	uint32_t mpidr;
 
@@ -627,6 +628,13 @@ int armv8_read_mpidr(struct armv8_common *armv8)
 	if (retval != ERROR_OK)
 		goto done;
 
+	/* check if we're in an unprivileged mode */
+	if (armv8_curel_from_core_mode(arm->core_mode) < SYSTEM_CUREL_EL1) {
+		retval = armv8_dpm_modeswitch(dpm, ARMV8_64_EL1H);
+		if (retval != ERROR_OK)
+			return retval;
+	}
+
 	retval = dpm->instr_read_data_r0(dpm, armv8_opcode(armv8, READ_REG_MPIDR), &mpidr);
 	if (retval != ERROR_OK)
 		goto done;
@@ -642,6 +650,7 @@ int armv8_read_mpidr(struct armv8_common *armv8)
 		LOG_ERROR("mpidr not in multiprocessor format");
 
 done:
+	armv8_dpm_modeswitch(dpm, ARM_MODE_ANY);
 	dpm->finish(dpm);
 	return retval;
 }
diff --git a/src/target/armv8_cache.c b/src/target/armv8_cache.c
index 7f610c9..40965eb 100644
--- a/src/target/armv8_cache.c
+++ b/src/target/armv8_cache.c
@@ -310,6 +310,7 @@ int armv8_identify_cache(struct armv8_common *armv8)
 {
 	/*  read cache descriptor */
 	int retval = ERROR_FAIL;
+	struct arm *arm = &armv8->arm;
 	struct arm_dpm *dpm = armv8->arm.dpm;
 	uint32_t csselr, clidr, ctr;
 	uint32_t cache_reg;
@@ -320,6 +321,13 @@ int armv8_identify_cache(struct armv8_common *armv8)
 	if (retval != ERROR_OK)
 		goto done;
 
+	/* check if we're in an unprivileged mode */
+	if (armv8_curel_from_core_mode(arm->core_mode) < SYSTEM_CUREL_EL1) {
+		retval = armv8_dpm_modeswitch(dpm, ARMV8_64_EL1H);
+		if (retval != ERROR_OK)
+			return retval;
+	}
+
 	/* retrieve CTR */
 	retval = dpm->instr_read_data_r0(dpm,
 			armv8_opcode(armv8, READ_REG_CTR), &ctr);
@@ -417,6 +425,7 @@ int armv8_identify_cache(struct armv8_common *armv8)
 	}
 
 done:
+	armv8_dpm_modeswitch(dpm, ARM_MODE_ANY);
 	dpm->finish(dpm);
 	return retval;
 
diff --git a/src/target/armv8_dpm.c b/src/target/armv8_dpm.c
index 91b2f51..3c941fa 100644
--- a/src/target/armv8_dpm.c
+++ b/src/target/armv8_dpm.c
@@ -258,7 +258,7 @@ static int dpmv8_exec_opcode(struct arm_dpm *dpm,
 
 	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);
+		armv8_dpm_handle_exception(dpm, true);
 		retval = ERROR_FAIL;
 	}
 
@@ -600,7 +600,7 @@ int armv8_dpm_modeswitch(struct arm_dpm *dpm, enum arm_mode mode)
 				armv8_opcode(armv8, ARMV8_OPC_DCPS) | target_el);
 
 		/* DCPS clobbers registers just like an exception taken */
-		armv8_dpm_handle_exception(dpm);
+		armv8_dpm_handle_exception(dpm, false);
 	} else {
 		core_state = armv8_dpm_get_core_state(dpm);
 		if (core_state != ARM_STATE_AARCH64) {
@@ -1298,7 +1298,7 @@ void armv8_dpm_report_wfar(struct arm_dpm *dpm, uint64_t addr)
  * 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)
+void armv8_dpm_handle_exception(struct arm_dpm *dpm, bool do_restore)
 {
 	struct armv8_common *armv8 = dpm->arm->arch_info;
 	struct reg_cache *cache = dpm->arm->core_cache;
@@ -1344,7 +1344,8 @@ void armv8_dpm_handle_exception(struct arm_dpm *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);
+	if (do_restore)
+		armv8_dpm_modeswitch(dpm, ARM_MODE_ANY);
 }
 
 /*----------------------------------------------------------------------*/
diff --git a/src/target/armv8_dpm.h b/src/target/armv8_dpm.h
index c039359..f404403 100644
--- a/src/target/armv8_dpm.h
+++ b/src/target/armv8_dpm.h
@@ -116,7 +116,7 @@ void armv8_dpm_report_wfar(struct arm_dpm *, uint64_t wfar);
 #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);
+void armv8_dpm_handle_exception(struct arm_dpm *dpm, bool do_restore);
 enum arm_state armv8_dpm_get_core_state(struct arm_dpm *dpm);
 
 #endif /* OPENOCD_TARGET_ARM_DPM_H */