jtag: adapter: rework adapter related commands

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

diff --git a/src/target/armv8_dpm.c b/src/target/armv8_dpm.c
index 91b2f51718e6c0ac45feeea19764a7baf7eb628b..081eed21ba21166ef5babadd78b9537f9a36dde8 100644 (file)
--- 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;
        }
 
@@ -573,6 +573,7 @@ int armv8_dpm_modeswitch(struct arm_dpm *dpm, enum arm_mode mode)
        case ARM_MODE_ABT:
        case ARM_MODE_IRQ:
        case ARM_MODE_FIQ:
+       case ARM_MODE_SYS:
                target_el = 1;
                break;
        /*
@@ -600,7 +601,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) {
@@ -790,6 +791,10 @@ int armv8_dpm_read_current_registers(struct arm_dpm *dpm)
                                dpm->last_el != armv8_curel_from_core_mode(arm_reg->mode))
                        continue;
 
+               /* Special case: ARM_MODE_SYS has no SPSR at EL1 */
+               if (r->number == ARMV8_SPSR_EL1 && arm->core_mode == ARM_MODE_SYS)
+                       continue;
+
                retval = dpmv8_read_reg(dpm, r, i);
                if (retval != ERROR_OK)
                        goto fail;
@@ -1298,7 +1303,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 +1349,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);
 }
 
 /*----------------------------------------------------------------------*/
@@ -1375,13 +1381,15 @@ void armv8_dpm_report_dscr(struct arm_dpm *dpm, uint32_t dscr)
                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;
+               case DSCRV8_ENTRY_EXCEPTION_CATCH:  /*exception catch*/
+                       target->debug_reason = DBG_REASON_EXC_CATCH;
+                       break;
                default:
                        target->debug_reason = DBG_REASON_UNDEFINED;
                        break;