[Qemu-devel] [RFC PATCH 18/21] target-arm: switch banked CP registers

[Sergey Fedorov]

Banked coprocessor registers are switched on two cases:
1) Entering or leaving CPU monitor mode with SCR.NS bit set;
2) Setting SCR.NS bit not from CPU monitor mode

Coprocessor register banking is done similar to CPU core register
banking. Some of SCTRL bits are common for secure and non-secure state.
Translation table base masks are updated on register switch instead
of TTBCR write.

Signed-off-by: Sergey Fedorov <address@hidden>
---
 target-arm/helper.c |   77 ++++++++++++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 76 insertions(+), 1 deletion(-)

diff --git a/target-arm/helper.c b/target-arm/helper.c
index e1e9762..7bfadb0 100644
--- a/target-arm/helper.c
+++ b/target-arm/helper.c
@@ -11,6 +11,7 @@ static inline int get_phys_addr(CPUARMState *env, uint32_t 
address,
                                 int access_type, int is_user,
                                 hwaddr *phys_ptr, int *prot,
                                 target_ulong *page_size);
+static void switch_cp15_regs(CPUARMState *env, int secure);
 #endif
 
 static int vfp_gdb_get_reg(CPUARMState *env, uint8_t *buf, int reg)
@@ -1553,6 +1554,17 @@ static int sctlr_write(CPUARMState *env, const 
ARMCPRegInfo *ri, uint64_t value)
 }
 
 #ifndef CONFIG_USER_ONLY
+static int scr_write(CPUARMState *env, const ARMCPRegInfo *ri, uint64_t value)
+{
+    if ((value & 1/*NS*/) && (env->uncached_cpsr & CPSR_M) != 
ARM_CPU_MODE_MON) {
+        /* We are going to Non-secure state; switch banked system control 
registers */
+        switch_cp15_regs(env, 0);
+    }
+
+    env->cp15.c1_scr = value;
+    return 0;
+}
+
 static int vbar_write(CPUARMState *env, const ARMCPRegInfo *ri,
                       uint64_t value)
 {
@@ -1572,7 +1584,7 @@ static const ARMCPRegInfo tz_cp_reginfo[] = {
 #ifndef CONFIG_USER_ONLY
     { .name = "SCR", .cp = 15, .crn = 1, .crm = 1, .opc1 = 0, .opc2 = 0,
       .access = PL3_RW, .fieldoffset = offsetof(CPUARMState, cp15.c1_scr),
-      .resetvalue = 0 },
+      .writefn = scr_write, .resetvalue = 0 },
     { .name = "VBAR", .cp = 15, .crn = 12, .crm = 0, .opc1 = 0, .opc2 = 0,
       .access = PL1_RW, .type = ARM_CP_BANKED, .writefn = vbar_write,
       .fieldoffset = offsetof(CPUARMState, cp15.c12_vbar),
@@ -2284,6 +2296,69 @@ void switch_mode(CPUARMState *env, int mode)
     env->regs[13] = env->banked_r13[i];
     env->regs[14] = env->banked_r14[i];
     env->spsr = env->banked_spsr[i];
+
+    if ((mode == ARM_CPU_MODE_MON || old_mode == ARM_CPU_MODE_MON) &&
+            (env->cp15.c1_scr & 1/*NS*/)) {
+        /* We are going to change Security state; switch banked system control 
registers */
+        switch_cp15_regs(env, (mode == ARM_CPU_MODE_MON));
+    }
+}
+
+void switch_cp15_regs(CPUARMState *env, int secure)
+{
+    int i;
+
+    /* Save current Security state registers */
+    i = arm_is_secure(env) ? 0 : 1;
+    env->cp15.banked_c0_cssel[i] = env->cp15.c0_cssel;
+    env->cp15.banked_c1_sys[i] = env->cp15.c1_sys;
+    env->cp15.banked_c2_base0[i] = env->cp15.c2_base0;
+    env->cp15.banked_c2_base0_hi[i] = env->cp15.c2_base0_hi;
+    env->cp15.banked_c2_base1[i] = env->cp15.c2_base1;
+    env->cp15.banked_c2_base1_hi[i] = env->cp15.c2_base1_hi;
+    env->cp15.banked_c2_control[i] = env->cp15.c2_control;
+    env->cp15.banked_c3[i] = env->cp15.c3;
+    env->cp15.banked_c5_data[i] = env->cp15.c5_data;
+    env->cp15.banked_c5_insn[i] = env->cp15.c5_insn;
+    env->cp15.banked_c6_data[i] = env->cp15.c6_data;
+    env->cp15.banked_c6_insn[i] = env->cp15.c6_insn;
+    env->cp15.banked_c7_par[i] = env->cp15.c7_par;
+    env->cp15.banked_c7_par_hi[i] = env->cp15.c7_par_hi;
+    env->cp15.banked_c13_context[i] = env->cp15.c13_context;
+    env->cp15.banked_c13_fcse[i] = env->cp15.c13_fcse;
+    env->cp15.banked_c13_tls1[i] = env->cp15.c13_tls1;
+    env->cp15.banked_c13_tls2[i] = env->cp15.c13_tls2;
+    env->cp15.banked_c13_tls3[i] = env->cp15.c13_tls3;
+
+    /* Restore new Security state registers */
+    i = secure ? 0 : 1;
+    env->cp15.c0_cssel = env->cp15.banked_c0_cssel[i];
+    /* Maintain the value of common bits */
+    env->cp15.c1_sys &= 0x8204000;
+    env->cp15.c1_sys |= env->cp15.banked_c1_sys[i] & ~0x8204000;
+    env->cp15.c2_base0 = env->cp15.banked_c2_base0[i];
+    env->cp15.c2_base0_hi = env->cp15.banked_c2_base0_hi[i];
+    env->cp15.c2_base1 = env->cp15.banked_c2_base1[i];
+    env->cp15.c2_base1_hi = env->cp15.banked_c2_base1_hi[i];
+    {
+        int maskshift;
+        env->cp15.c2_control = env->cp15.banked_c2_control[i];
+        maskshift = extract32(env->cp15.c2_control, 0, 3);
+        env->cp15.c2_mask = ~(((uint32_t)0xffffffffu) >> maskshift);
+        env->cp15.c2_base_mask = ~((uint32_t)0x3fffu >> maskshift);
+    }
+    env->cp15.c3 = env->cp15.banked_c3[i];
+    env->cp15.c5_data = env->cp15.banked_c5_data[i];
+    env->cp15.c5_insn = env->cp15.banked_c5_insn[i];
+    env->cp15.c6_data = env->cp15.banked_c6_data[i];
+    env->cp15.c6_insn = env->cp15.banked_c6_insn[i];
+    env->cp15.c7_par = env->cp15.banked_c7_par[i];
+    env->cp15.c7_par_hi = env->cp15.banked_c7_par_hi[i];
+    env->cp15.c13_context = env->cp15.banked_c13_context[i];
+    env->cp15.c13_fcse = env->cp15.banked_c13_fcse[i];
+    env->cp15.c13_tls1 = env->cp15.banked_c13_tls1[i];
+    env->cp15.c13_tls2 = env->cp15.banked_c13_tls2[i];
+    env->cp15.c13_tls3 = env->cp15.banked_c13_tls3[i];
 }
 
 static void v7m_push(CPUARMState *env, uint32_t val)
-- 
1.7.9.5