From: Matthias Welwarsky Date: Thu, 20 Oct 2016 09:31:40 +0000 (+0200) Subject: aarch64: fix mode switching X-Git-Tag: v0.11.0-rc1~1472 X-Git-Url: https://review.openocd.org/gitweb?p=openocd.git;a=commitdiff_plain;h=refs%2Fchanges%2F16%2F3816%2F4 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 --- diff --git a/src/target/aarch64.c b/src/target/aarch64.c index 02f17d81d3..88e10c104e 100644 --- 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 32fe048e0e..b55c153f5a 100644 --- 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 a92f92c6cd..d6f2b8768a 100644 --- 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 6470461e14..48e2ca1812 100644 --- 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 3e18a02a66..75ea946184 100644 --- 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 cb20c84a96..e57e7e9db7 100644 --- 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, };