mips32, in wait_for_pracc_rw() use ejtag_info->pa_ctrl

[openocd.git] / src / target / armv8_dpm.c

diff --git a/src/target/armv8_dpm.c b/src/target/armv8_dpm.c
index 56e2eb8fef5e7f0ec995bee57338dc9750fd8032..f4e7a07996c651546c92812b04f4f085a76305cd 100644 (file)
--- a/src/target/armv8_dpm.c
+++ b/src/target/armv8_dpm.c
@@ -51,17 +51,11 @@ enum arm_state armv8_dpm_get_core_state(struct arm_dpm *dpm)
 {
        int el = (dpm->dscr >> 8) & 0x3;
        int rw = (dpm->dscr >> 10) & 0xF;
-       int pos;
 
        dpm->last_el = el;
 
-       /* find the first '0' in DSCR.RW */
-       for (pos = 3; pos >= 0; pos--) {
-               if ((rw & (1 << pos)) == 0)
-                       break;
-       }
-
-       if (el > pos)
+       /* 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;
@@ -724,14 +718,22 @@ int armv8_dpm_read_current_registers(struct arm_dpm *dpm)
        cache = arm->core_cache;
 
        /* read R0 first (it's used for scratch), then CPSR */
-       r = cache->reg_list + 0;
+       r = cache->reg_list + ARMV8_R0;
        if (!r->valid) {
-               retval = dpmv8_read_reg(dpm, r, 0);
+               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);
@@ -741,7 +743,7 @@ int armv8_dpm_read_current_registers(struct arm_dpm *dpm)
        /* update core mode and state */
        armv8_set_cpsr(arm, cpsr);
 
-       for (unsigned int i = 1; i < cache->num_regs ; i++) {
+       for (unsigned int i = ARMV8_PC; i < cache->num_regs ; i++) {
                struct arm_reg *arm_reg;
 
                r = armv8_reg_current(arm, i);
@@ -1310,6 +1312,8 @@ void armv8_dpm_handle_exception(struct arm_dpm *dpm)
        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);
 }
 
 /*----------------------------------------------------------------------*/