diff options
| -rw-r--r-- | src/target/aarch64.c | 9 |
1 files changed, 9 insertions, 0 deletions
diff --git a/src/target/aarch64.c b/src/target/aarch64.c index d6b12cd..46ed49f 100644 --- a/src/target/aarch64.c +++ b/src/target/aarch64.c @@ -133,6 +133,7 @@ static int aarch64_mmu_modify(struct target *target, int enable) struct aarch64_common *aarch64 = target_to_aarch64(target); struct armv8_common *armv8 = &aarch64->armv8_common; int retval = ERROR_OK; + enum arm_mode target_mode = ARM_MODE_ANY; uint32_t instr = 0; if (enable) { @@ -158,6 +159,8 @@ static int aarch64_mmu_modify(struct target *target, int enable) switch (armv8->arm.core_mode) { case ARMV8_64_EL0T: + target_mode = ARMV8_64_EL1H; + /* fall through */ case ARMV8_64_EL1T: case ARMV8_64_EL1H: instr = ARMV8_MSR_GP(SYSTEM_SCTLR_EL1, 0); @@ -184,9 +187,15 @@ static int aarch64_mmu_modify(struct target *target, int enable) LOG_DEBUG("unknown cpu state 0x%x", armv8->arm.core_mode); break; } + if (target_mode != ARM_MODE_ANY) + armv8_dpm_modeswitch(&armv8->dpm, target_mode); retval = armv8->dpm.instr_write_data_r0(&armv8->dpm, instr, aarch64->system_control_reg_curr); + + if (target_mode != ARM_MODE_ANY) + armv8_dpm_modeswitch(&armv8->dpm, ARM_MODE_ANY); + return retval; } |