aarch64: fix mode switching

DCPS only allows to enter higher ELs, for lower ELs you need to
use DRPS. Also, of course the encoding differs between A64 and T32.
Both DCPS and DRPS also clobber DLR and DSPSR, which then need to be
restored on resume.

Change-Id: Ifa3dcfa94212702e57170bd59fd0bb25495fb6fd
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index 02f17d81d3a550647b29ce51db1c25010ffd4f8e..88e10c104ec4fff20535df062a3016aa36e67c62 100644 (file)
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -487,7 +487,7 @@ static int aarch64_internal_restore(struct target *target, int current,
        buf_set_u64(arm->pc->value, 0, 64, resume_pc);
        arm->pc->dirty = 1;
        arm->pc->valid = 1;
-       dpmv8_modeswitch(&armv8->dpm, ARM_MODE_ANY);
+       armv8_dpm_modeswitch(&armv8->dpm, ARM_MODE_ANY);
 
        /* called it now before restoring context because it uses cpu
         * register r0 for restoring system control register */
@@ -697,7 +697,7 @@ static int aarch64_post_debug_entry(struct target *target)
 
        switch (armv8->arm.core_mode) {
                case ARMV8_64_EL0T:
-                       dpmv8_modeswitch(&armv8->dpm, ARMV8_64_EL1T);
+                       armv8_dpm_modeswitch(&armv8->dpm, ARMV8_64_EL1H);
                        /* fall through */
                case ARMV8_64_EL1T:
                case ARMV8_64_EL1H:
@@ -738,7 +738,7 @@ static int aarch64_post_debug_entry(struct target *target)
                        break;
        }
 
-       dpmv8_modeswitch(&armv8->dpm, ARM_MODE_ANY);
+       armv8_dpm_modeswitch(&armv8->dpm, ARM_MODE_ANY);
 
        LOG_DEBUG("System_register: %8.8" PRIx32, aarch64->system_control_reg);
        aarch64->system_control_reg_curr = aarch64->system_control_reg;

diff --git a/src/target/armv8.c b/src/target/armv8.c
index 32fe048e0edb099d91ad60dac58d81787024748e..b55c153f5a6f92366e1e3d78d6a5ba1593ac8e42 100644 (file)
--- a/src/target/armv8.c
+++ b/src/target/armv8.c
@@ -781,6 +781,7 @@ int armv8_mmu_translate_va_pa(struct target *target, target_addr_t va,
        struct armv8_common *armv8 = target_to_armv8(target);
        struct arm *arm = target_to_arm(target);
        struct arm_dpm *dpm = &armv8->dpm;
+       enum arm_mode target_mode = ARM_MODE_ANY;
        uint32_t retval;
        uint32_t instr = 0;
        uint64_t par;
@@ -801,12 +802,12 @@ int armv8_mmu_translate_va_pa(struct target *target, target_addr_t va,
        case SYSTEM_CUREL_EL0:
                instr = ARMV8_SYS(SYSTEM_ATS12E0R, 0);
                /* can only execute instruction at EL2 */
-               dpmv8_modeswitch(dpm, ARMV8_64_EL2T);
+               target_mode = ARMV8_64_EL2H;
                break;
        case SYSTEM_CUREL_EL1:
                instr = ARMV8_SYS(SYSTEM_ATS12E1R, 0);
                /* can only execute instruction at EL2 */
-               dpmv8_modeswitch(dpm, ARMV8_64_EL2T);
+               target_mode = ARMV8_64_EL2H;
                break;
        case SYSTEM_CUREL_EL2:
                instr = ARMV8_SYS(SYSTEM_ATS1E2R, 0);
@@ -819,16 +820,23 @@ int armv8_mmu_translate_va_pa(struct target *target, target_addr_t va,
                break;
        };
 
+       if (target_mode != ARM_MODE_ANY)
+               armv8_dpm_modeswitch(dpm, target_mode);
+
        /* write VA to R0 and execute translation instruction */
        retval = dpm->instr_write_data_r0_64(dpm, instr, (uint64_t)va);
        /* read result from PAR_EL1 */
        if (retval == ERROR_OK)
                retval = dpm->instr_read_data_r0_64(dpm, ARMV8_MRS(SYSTEM_PAR_EL1, 0), &par);
 
+       /* switch back to saved PE mode */
+       if (target_mode != ARM_MODE_ANY)
+               armv8_dpm_modeswitch(dpm, ARM_MODE_ANY);
+
        dpm->finish(dpm);
 
-       /* switch back to saved PE mode */
-       dpmv8_modeswitch(dpm, ARM_MODE_ANY);
+       if (retval != ERROR_OK)
+               return retval;
 
        if (retval != ERROR_OK)
                return retval;

diff --git a/src/target/armv8_dpm.c b/src/target/armv8_dpm.c
index a92f92c6cd047e042d189c67ae029a3c1e546fe5..d6f2b8768ac1523b365cf011d9c0a365e0ba8e30 100644 (file)
--- a/src/target/armv8_dpm.c
+++ b/src/target/armv8_dpm.c
@@ -610,53 +610,103 @@ static int dpmv8_msr(struct target *target, uint32_t op0,
  * Register access utilities
  */
 
-/* Toggles between recorded core mode (USR, SVC, etc) and a temporary one.
- * Routines *must* restore the original mode before returning!!
- */
-int dpmv8_modeswitch(struct arm_dpm *dpm, enum arm_mode mode)
+int armv8_dpm_modeswitch(struct arm_dpm *dpm, enum arm_mode mode)
 {
        struct armv8_common *armv8 = (struct armv8_common *)dpm->arm->arch_info;
-       int retval;
+       int retval = ERROR_OK;
+       unsigned int target_el;
+       enum arm_state core_state;
        uint32_t cpsr;
 
        /* restore previous mode */
-       if (mode == ARM_MODE_ANY)
+       if (mode == ARM_MODE_ANY) {
                cpsr = buf_get_u32(dpm->arm->cpsr->value, 0, 32);
 
-       /* else force to the specified mode */
-       else
-               cpsr = mode >> 4;
+               LOG_DEBUG("restoring mode, cpsr = 0x%08"PRIx32, cpsr);
 
-       switch ((cpsr & 0xC) >> 2) {
-                       case SYSTEM_CUREL_EL1:
-                               retval = dpm->instr_execute(dpm, ARMV8_DCPS1(11));
-                               if (retval != ERROR_OK)
-                                       return retval;
-                               break;
-                       case SYSTEM_CUREL_EL2:
-                               retval = dpm->instr_execute(dpm, ARMV8_DCPS2(11));
-                               if (retval != ERROR_OK)
-                                       return retval;
-                               break;
-                       break;
-                       case SYSTEM_CUREL_EL3:
-                               retval = dpm->instr_execute(dpm, ARMV8_DCPS3(11));
-                               if (retval != ERROR_OK)
-                                       return retval;
-                               break;
-                       break;
-                       default:
-                               LOG_DEBUG("unknow mode 0x%x", (unsigned) ((cpsr & 0xC) >> 2));
-                               break;
+       } else {
+               LOG_DEBUG("setting mode 0x%"PRIx32, mode);
+
+               /* else force to the specified mode */
+               if (is_arm_mode(mode))
+                       cpsr = mode;
+               else
+                       cpsr = mode >> 4;
        }
 
+       switch (cpsr & 0x1f) {
+       /* aarch32 modes */
+       case ARM_MODE_USR:
+               target_el = 0;
+               break;
+       case ARM_MODE_SVC:
+       case ARM_MODE_ABT:
+       case ARM_MODE_IRQ:
+       case ARM_MODE_FIQ:
+               target_el = 1;
+               break;
+       /*
+        * TODO: handle ARM_MODE_HYP
+        * case ARM_MODE_HYP:
+        *      target_el = 2;
+        *      break;
+        */
+       case ARM_MODE_MON:
+               target_el = 3;
+               break;
+       /* aarch64 modes */
+       default:
+               target_el = (cpsr >> 2) & 3;
+       }
 
-       retval = dpm->instr_write_data_r0(dpm, armv8_opcode(armv8, WRITE_REG_DSPSR), cpsr);
-       if (retval != ERROR_OK)
-               return retval;
+       if (target_el > SYSTEM_CUREL_EL3) {
+               LOG_ERROR("%s: Invalid target exception level %i", __func__, target_el);
+               return ERROR_FAIL;
+       }
+
+       LOG_DEBUG("target_el = %i, last_el = %i", target_el, dpm->last_el);
+       if (target_el > dpm->last_el) {
+               retval = dpm->instr_execute(dpm,
+                               armv8_opcode(armv8, ARMV8_OPC_DCPS) | target_el);
+       } else {
+               core_state = armv8_dpm_get_core_state(dpm);
+               if (core_state != ARM_STATE_AARCH64) {
+                       /* cannot do DRPS/ERET when already in EL0 */
+                       if (dpm->last_el != 0) {
+                               /* load SPSR with the desired mode and execute DRPS */
+                               LOG_DEBUG("SPSR = 0x%08"PRIx32, cpsr);
+                               retval = dpm->instr_write_data_r0(dpm,
+                                               ARMV8_MSR_GP_xPSR_T1(1, 0, 15), cpsr);
+                               if (retval == ERROR_OK)
+                                       retval = dpm->instr_execute(dpm, armv8_opcode(armv8, ARMV8_OPC_DRPS));
+                       }
+               } else {
+                       /*
+                        * need to execute multiple DRPS instructions until target_el
+                        * is reached
+                        */
+                       while (retval == ERROR_OK && dpm->last_el != target_el) {
+                               unsigned int cur_el = dpm->last_el;
+                               retval = dpm->instr_execute(dpm, armv8_opcode(armv8, ARMV8_OPC_DRPS));
+                               if (cur_el == dpm->last_el) {
+                                       LOG_INFO("Cannot reach EL %i, SPSR corrupted?", target_el);
+                                       break;
+                               }
+                       }
+               }
 
-       if (dpm->instr_cpsr_sync)
-               retval = dpm->instr_cpsr_sync(dpm);
+               /* On executing DRPS, DSPSR and DLR become UNKNOWN, mark them as dirty */
+               dpm->arm->cpsr->dirty = true;
+               dpm->arm->pc->dirty = true;
+
+               /*
+                * re-evaluate the core state, we might be in Aarch32 state now
+                * we rely on dpm->dscr being up-to-date
+                */
+               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);
+       }
 
        return retval;
 }
@@ -875,7 +925,7 @@ int armv8_dpm_write_dirty_registers(struct arm_dpm *dpm, bool bpwp)
         */
 
        /* Restore original core mode and state */
-       retval = dpmv8_modeswitch(dpm, ARM_MODE_ANY);
+       retval = armv8_dpm_modeswitch(dpm, ARM_MODE_ANY);
        if (retval != ERROR_OK)
                goto done;
 
@@ -1022,9 +1072,9 @@ static int armv8_dpm_full_context(struct target *target)
                                 * in FIQ mode we need to patch mode.
                                 */
                                if (mode != ARM_MODE_ANY)
-                                       retval = dpmv8_modeswitch(dpm, mode);
+                                       retval = armv8_dpm_modeswitch(dpm, mode);
                                else
-                                       retval = dpmv8_modeswitch(dpm, ARM_MODE_USR);
+                                       retval = armv8_dpm_modeswitch(dpm, ARM_MODE_USR);
 
                                if (retval != ERROR_OK)
                                        goto done;
@@ -1042,7 +1092,7 @@ static int armv8_dpm_full_context(struct target *target)
 
        } while (did_read);
 
-       retval = dpmv8_modeswitch(dpm, ARM_MODE_ANY);
+       retval = armv8_dpm_modeswitch(dpm, ARM_MODE_ANY);
        /* (void) */ dpm->finish(dpm);
 done:
        return retval;

diff --git a/src/target/armv8_dpm.h b/src/target/armv8_dpm.h
index 6470461e14650d8cb14de0b90e9a54dce0fd9606..48e2ca181245974d0b3040df5c398ab1292b4cf6 100644 (file)
--- a/src/target/armv8_dpm.h
+++ b/src/target/armv8_dpm.h
@@ -32,7 +32,7 @@ int armv8_dpm_setup(struct arm_dpm *dpm);
 int armv8_dpm_initialize(struct arm_dpm *dpm);
 
 int armv8_dpm_read_current_registers(struct arm_dpm *);
-int dpmv8_modeswitch(struct arm_dpm *dpm, enum arm_mode mode);
+int armv8_dpm_modeswitch(struct arm_dpm *dpm, enum arm_mode mode);
 
 
 int armv8_dpm_write_dirty_registers(struct arm_dpm *, bool bpwp);

diff --git a/src/target/armv8_opcodes.c b/src/target/armv8_opcodes.c
index 3e18a02a66ff6db70783ed315a5e12c292902e87..75ea946184d30222634d5a2c39894738fa5b2ab4 100644 (file)
--- a/src/target/armv8_opcodes.c
+++ b/src/target/armv8_opcodes.c
@@ -33,7 +33,9 @@ static const uint32_t a64_opcodes[ARMV8_OPC_NUM] = {
                [WRITE_REG_DTRTX]       = ARMV8_MSR_GP(SYSTEM_DBG_DTRTX_EL0, 0),
                [WRITE_REG_DSPSR]       = ARMV8_MSR_DSPSR(0),
                [READ_REG_DSPSR]        = ARMV8_MRS_DSPSR(0),
-               [ARMV8_OPC_DSB_SY]  = ARMV8_DSB_SY,
+               [ARMV8_OPC_DSB_SY]      = ARMV8_DSB_SY,
+               [ARMV8_OPC_DCPS]        = ARMV8_DCPS(0, 11),
+               [ARMV8_OPC_DRPS]        = ARMV8_DRPS,
 };
 
 static const uint32_t t32_opcodes[ARMV8_OPC_NUM] = {
@@ -47,6 +49,8 @@ static const uint32_t t32_opcodes[ARMV8_OPC_NUM] = {
                [WRITE_REG_DSPSR]       = ARMV8_MCR_DSPSR(0),
                [READ_REG_DSPSR]        = ARMV8_MRC_DSPSR(0),
                [ARMV8_OPC_DSB_SY]      = ARMV8_DSB_SY_T1,
+               [ARMV8_OPC_DCPS]        = ARMV8_DCPS_T1(0),
+               [ARMV8_OPC_DRPS]        = ARMV8_ERET_T1,
 };
 
 void armv8_select_opcodes(struct armv8_common *armv8, bool state_is_aarch64)

diff --git a/src/target/armv8_opcodes.h b/src/target/armv8_opcodes.h
index cb20c84a966a4bf57bd59426eda5dec4497ba568..e57e7e9db747e4bc339a0f868529298cd024a53c 100644 (file)
--- a/src/target/armv8_opcodes.h
+++ b/src/target/armv8_opcodes.h
@@ -125,9 +125,13 @@
 #define ARMV8_MRC_DLR(Rt)      ARMV8_MRC_T1(15, 4, 3, 5, 1, Rt)
 #define ARMV8_MCR_DLR(Rt)      ARMV8_MCR_T1(15, 4, 3, 5, 1, Rt)
 
-#define ARMV8_DCPS1(IM)        (0xd4a00001 | (((IM) & 0xFFFF) << 5))
-#define ARMV8_DCPS2(IM)        (0xd4a00002 | (((IM) & 0xFFFF) << 5))
-#define ARMV8_DCPS3(IM)        (0xd4a00003 | (((IM) & 0xFFFF) << 5))
+#define ARMV8_DCPS1(IM)                (0xd4a00001 | (((IM) & 0xFFFF) << 5))
+#define ARMV8_DCPS2(IM)                (0xd4a00002 | (((IM) & 0xFFFF) << 5))
+#define ARMV8_DCPS3(IM)                (0xd4a00003 | (((IM) & 0xFFFF) << 5))
+#define ARMV8_DCPS(EL, IM)     (0xd4a00000 | (((IM) & 0xFFFF) << 5) | EL)
+#define ARMV8_DCPS_T1(EL)      (0xf78f8000 | EL)
+#define ARMV8_DRPS             0xd6bf03e0
+#define ARMV8_ERET_T1          0xf3de8f00
 
 #define ARMV8_DSB_SY                           0xd5033F9F
 #define ARMV8_DSB_SY_T1                                0xf3bf8f4f
@@ -166,6 +170,8 @@ enum armv8_opcode {
        WRITE_REG_DSPSR,
        READ_REG_DSPSR,
        ARMV8_OPC_DSB_SY,
+       ARMV8_OPC_DCPS,
+       ARMV8_OPC_DRPS,
        ARMV8_OPC_NUM,
 };