From: Kevin Yang <kangyang@google.com>
Date: Mon, 12 Oct 2020 20:22:47 +0000 (-0700)
Subject: target/armv8: Handle modeswitch for aarch32 secure EL3
X-Git-Url: https://www.infradead.org/git/?a=commitdiff_plain;h=d9b614a56db9ff08b349dc29f4b257c16a442013;p=users%2Fborneoa%2Fopenocd-next.git

target/armv8: Handle modeswitch for aarch32 secure EL3

For aarch32 secure EL3
- Change target_el to 3 for SVC/ABT/IRQ/FIQ/UND/SYS for aarch32 secure
EL3
- Do not update SPSR for SYS, behavior is UNPREDICTABLE (ARMv8-A F5.1.121)
- Do not execute DRPS for SYS, behavior is UNPREDICTABLE (ARMv8-A
F5.1.51)

Change-Id: Ic1484665cd53afcccb5c20b152993a3f0407f8a2
Signed-off-by: Kevin Yang <kangyang@google.com>
Reviewed-on: https://review.openocd.org/c/openocd/+/5854
Tested-by: jenkins
Reviewed-by: Matthias Welwarsky <matthias@welwarsky.de>
Reviewed-by: Plamena Marinova <pmarinova@hilscher.com>
Reviewed-by: Antonio Borneo <borneo.antonio@gmail.com>
---

diff --git a/src/target/armv8_dpm.c b/src/target/armv8_dpm.c
index d1cee8a10..3384e82ea 100644
--- a/src/target/armv8_dpm.c
+++ b/src/target/armv8_dpm.c
@@ -542,6 +542,8 @@ int armv8_dpm_modeswitch(struct arm_dpm *dpm, enum arm_mode mode)
 	unsigned int target_el;
 	enum arm_state core_state;
 	uint32_t cpsr;
+	uint32_t rw = (dpm->dscr >> 10) & 0xF;
+	uint32_t ns = (dpm->dscr >> 18) & 0x1;
 
 	/* restore previous mode */
 	if (mode == ARM_MODE_ANY) {
@@ -564,7 +566,11 @@ int armv8_dpm_modeswitch(struct arm_dpm *dpm, enum arm_mode mode)
 	case ARM_MODE_IRQ:
 	case ARM_MODE_FIQ:
 	case ARM_MODE_SYS:
-		target_el = 1;
+		/* For Secure, EL1 if EL3 is aarch64, EL3 if EL3 is aarch32 */
+		if (ns || (rw & (1 << 3)))
+			target_el = 1;
+		else
+			target_el = 3;
 		break;
 	/*
 	 * TODO: handle ARM_MODE_HYP
@@ -598,8 +604,8 @@ int armv8_dpm_modeswitch(struct arm_dpm *dpm, enum arm_mode mode)
 	} 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) {
+			/* cannot do DRPS/ERET when in EL0 or in SYS mode */
+			if (dpm->last_el != 0 && dpm->arm->core_mode != ARM_MODE_SYS) {
 				/* load SPSR with the desired mode and execute DRPS */
 				LOG_DEBUG("SPSR = 0x%08"PRIx32, cpsr);
 				retval = dpm->instr_write_data_r0(dpm,