[prev in list] [next in list] [prev in thread] [next in thread]
List: openocd-development
Subject: [OpenOCD-devel] [PATCH]: f7e7702 aarch64: fix mode switching
From: gerrit () openocd ! org (gerrit)
Date: 2016-10-20 18:08:28
Message-ID: 20161020180828.42BD61980BC2 () mail ! openocd ! org
[Download RAW message or body]
This is an automated email from Gerrit.
Matthias Welwarsky (matthias@welwarsky.de) just uploaded a new patch set to Gerrit, \
which you can find at http://openocd.zylin.com/3816
-- gerrit
commit f7e7702b53b2df4c5eef350355bac4efad55ccdc
Author: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Thu Oct 20 11:31:40 2016 +0200
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 02f17d8..88e10c1 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 241fec9..b3c8e68 100644
--- a/src/target/armv8.c
+++ b/src/target/armv8.c
@@ -787,6 +787,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;
@@ -807,12 +808,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);
@@ -825,16 +826,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 (par & 1) {
LOG_ERROR("Address translation failed at stage %i, FST=%x, PTW=%i",
diff --git a/src/target/armv8_dpm.c b/src/target/armv8_dpm.c
index a92f92c..d6f2b87 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 6470461..48e2ca1 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 3e18a02..75ea946 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 cb20c84..e57e7e9 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,
};
--
------------------------------------------------------------------------------
Check out the vibrant tech community on one of the world's most
engaging tech sites, SlashDot.org! http://sdm.link/slashdot
_______________________________________________
OpenOCD-devel mailing list
OpenOCD-devel@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/openocd-devel
[prev in list] [next in list] [prev in thread] [next in thread]
Configure |
About |
News |
Add a list |
Sponsored by KoreLogic