[Qemu-devel] [PATCH v3 3/4] target-arm: Add 32/64-bit register sync

[Greg Bellows <greg.bellows@linaro.org>]

Add AArch32 to AArch64 register sychronization functions.
Replace manual register synchronization with new functions in
aarch64_cpu_do_interrupt() and HELPER(exception_return)().

Signed-off-by: Greg Bellows <greg.bellows@linaro.org>

---

v2 -> v3
- Conditionalize interrupt handler update of aarch64.
---
 target-arm/helper-a64.c |  9 +++--
 target-arm/internals.h  | 89 +++++++++++++++++++++++++++++++++++++++++++++++++
 target-arm/op_helper.c  |  6 ++--
 3 files changed, 95 insertions(+), 9 deletions(-)

diff --git a/target-arm/helper-a64.c b/target-arm/helper-a64.c
index 81066ca..8c6a100 100644
--- a/target-arm/helper-a64.c
+++ b/target-arm/helper-a64.c
@@ -448,7 +448,6 @@ void aarch64_cpu_do_interrupt(CPUState *cs)
     unsigned int new_el = arm_excp_target_el(cs, cs->exception_index);
     target_ulong addr = env->cp15.vbar_el[new_el];
     unsigned int new_mode = aarch64_pstate_mode(new_el, true);
-    int i;
 
     if (arm_current_el(env) < new_el) {
         if (env->aarch64) {
@@ -512,15 +511,15 @@ void aarch64_cpu_do_interrupt(CPUState *cs)
         }
         env->elr_el[new_el] = env->regs[15];
 
-        for (i = 0; i < 15; i++) {
-            env->xregs[i] = env->regs[i];
-        }
+        aarch64_sync_32_to_64(env);
 
         env->condexec_bits = 0;
     }
 
     pstate_write(env, PSTATE_DAIF | new_mode);
-    env->aarch64 = 1;
+    if (arm_feature(env, ARM_FEATURE_AARCH64)) {
+        env->aarch64 = 1;
+    }
     aarch64_restore_sp(env, new_el);
 
     env->pc = addr;
diff --git a/target-arm/internals.h b/target-arm/internals.h
index bb171a7..626ea7d 100644
--- a/target-arm/internals.h
+++ b/target-arm/internals.h
@@ -128,6 +128,95 @@ static inline void aarch64_restore_sp(CPUARMState *env, int el)
     }
 }
 
+static inline void aarch64_sync_32_to_64(CPUARMState *env)
+{
+    int i;
+
+    /* We can blanket copy R[0:7] to X[0:7] */
+    for (i = 0; i < 8; i++) {
+        env->xregs[i] = env->regs[i];
+    }
+
+    /* If we are in USR mode then we just want to complete the above blanket
+     * copy so we get the accurate register values.  If not, then we have to go
+     * to the saved and banked user regs.
+     */
+    if ((env->uncached_cpsr & 0x1f) == ARM_CPU_MODE_USR) {
+        for (i = 8; i < 15; i++) {
+            env->xregs[i] = env->regs[i];
+        }
+    } else {
+        for (i = 8; i < 13; i++) {
+            env->xregs[i] = env->usr_regs[i-8];
+        }
+        env->xregs[13] = env->banked_r13[bank_number(ARM_CPU_MODE_USR)];
+        env->xregs[14] = env->banked_r14[bank_number(ARM_CPU_MODE_USR)];
+    }
+    env->pc = env->regs[15];
+
+    env->xregs[15] = env->banked_r13[bank_number(ARM_CPU_MODE_HYP)];
+    env->xregs[16] = env->banked_r13[bank_number(ARM_CPU_MODE_IRQ)];
+    env->xregs[17] = env->banked_r14[bank_number(ARM_CPU_MODE_IRQ)];
+    env->xregs[18] = env->banked_r13[bank_number(ARM_CPU_MODE_SVC)];
+    env->xregs[19] = env->banked_r14[bank_number(ARM_CPU_MODE_SVC)];
+    env->xregs[20] = env->banked_r13[bank_number(ARM_CPU_MODE_ABT)];
+    env->xregs[21] = env->banked_r14[bank_number(ARM_CPU_MODE_ABT)];
+    env->xregs[22] = env->banked_r13[bank_number(ARM_CPU_MODE_UND)];
+    env->xregs[23] = env->banked_r14[bank_number(ARM_CPU_MODE_UND)];
+
+    for (i = 0; i < 5; i++) {
+        env->xregs[24+i] = env->fiq_regs[i];
+    }
+    env->xregs[29] = env->banked_r13[bank_number(ARM_CPU_MODE_FIQ)];
+    env->xregs[30] = env->banked_r14[bank_number(ARM_CPU_MODE_FIQ)];
+}
+
+static inline void aarch64_sync_64_to_32(CPUARMState *env)
+{
+    int i;
+
+    /* We can blanket copy R[0:7] to X[0:7] */
+    for (i = 0; i < 8; i++) {
+        env->regs[i] = env->xregs[i];
+    }
+
+    /* If we are in USR mode then we want to complete the above blanket
+     * copy as the XREGs will contain the most recent value.
+     */
+    if ((env->uncached_cpsr & 0x1f) == ARM_CPU_MODE_USR) {
+        for (i = 8; i < 15; i++) {
+            env->regs[i] = env->xregs[i];
+        }
+    }
+
+    /* Update the user copies and banked registers so they are also up to
+     * date.
+     */
+    for (i = 8; i < 13; i++) {
+        env->usr_regs[i-8] = env->xregs[i];
+    }
+    env->banked_r13[bank_number(ARM_CPU_MODE_USR)] = env->xregs[13];
+    env->banked_r14[bank_number(ARM_CPU_MODE_USR)] = env->xregs[14];
+
+    env->regs[15] = env->pc;
+
+    env->banked_r13[bank_number(ARM_CPU_MODE_HYP)] = env->xregs[15];
+    env->banked_r13[bank_number(ARM_CPU_MODE_IRQ)] = env->xregs[16];
+    env->banked_r14[bank_number(ARM_CPU_MODE_IRQ)] = env->xregs[17];
+    env->banked_r13[bank_number(ARM_CPU_MODE_SVC)] = env->xregs[18];
+    env->banked_r14[bank_number(ARM_CPU_MODE_SVC)] = env->xregs[19];
+    env->banked_r13[bank_number(ARM_CPU_MODE_ABT)] = env->xregs[20];
+    env->banked_r14[bank_number(ARM_CPU_MODE_ABT)] = env->xregs[21];
+    env->banked_r13[bank_number(ARM_CPU_MODE_UND)] = env->xregs[22];
+    env->banked_r14[bank_number(ARM_CPU_MODE_UND)] = env->xregs[23];
+
+    for (i = 0; i < 5; i++) {
+        env->fiq_regs[i] = env->xregs[24+i];
+    }
+    env->banked_r13[bank_number(ARM_CPU_MODE_FIQ)] = env->xregs[29];
+    env->banked_r14[bank_number(ARM_CPU_MODE_FIQ)] = env->xregs[30];
+}
+
 static inline void update_spsel(CPUARMState *env, uint32_t imm)
 {
     unsigned int cur_el = arm_current_el(env);
diff --git a/target-arm/op_helper.c b/target-arm/op_helper.c
index 2bed914..7713022 100644
--- a/target-arm/op_helper.c
+++ b/target-arm/op_helper.c
@@ -465,7 +465,7 @@ void HELPER(exception_return)(CPUARMState *env)
     int cur_el = arm_current_el(env);
     unsigned int spsr_idx = aarch64_banked_spsr_index(cur_el);
     uint32_t spsr = env->banked_spsr[spsr_idx];
-    int new_el, i;
+    int new_el;
 
     aarch64_save_sp(env, cur_el);
 
@@ -491,9 +491,7 @@ void HELPER(exception_return)(CPUARMState *env)
         if (!arm_singlestep_active(env)) {
             env->uncached_cpsr &= ~PSTATE_SS;
         }
-        for (i = 0; i < 15; i++) {
-            env->regs[i] = env->xregs[i];
-        }
+        aarch64_sync_64_to_32(env);
 
         env->regs[15] = env->elr_el[1] & ~0x1;
     } else {
-- 
1.8.3.2