Aarch64:Switch to EL1 from EL0 before manipulate MMU

If current core is in EL0, it cannot use 'msr sctlr_el1, x0'

Change-Id: I04e60e39e4c84f9d9de7cc87a8e438f5d2737dc3
Signed-off-by: Cheng-Shiun Tsai <cheng.shiun.tsai@gmail.com>
Reviewed-on: http://openocd.zylin.com/6051
Tested-by: jenkins
Reviewed-by: Antonio Borneo <borneo.antonio@gmail.com>
Reviewed-by: Matthias Welwarsky <matthias@welwarsky.de>

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index d6b12cd59990b9d450ddadff66b4c1c6bf25629f..46ed49f685d16e96d2dd86806a062dd477c22a07 100644 (file)
--- 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;
 }