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];
}