[Qemu-riscv] [PATCH v1 12/27] target/riscv: Add background register swapping function

[Alistair Francis]

Signed-off-by: Alistair Francis <address@hidden>
---
 target/riscv/cpu.h        |  1 +
 target/riscv/cpu_bits.h   |  5 ++++
 target/riscv/cpu_helper.c | 52 +++++++++++++++++++++++++++++++++++++++
 3 files changed, 58 insertions(+)

diff --git a/target/riscv/cpu.h b/target/riscv/cpu.h
index eed561d56e..5b3b32dbbc 100644
--- a/target/riscv/cpu.h
+++ b/target/riscv/cpu.h
@@ -320,6 +320,7 @@ void riscv_cpu_list(void);
 #define cpu_mmu_index riscv_cpu_mmu_index
 
 #ifndef CONFIG_USER_ONLY
+void riscv_cpu_swap_background_regs(CPURISCVState *env);
 int riscv_cpu_claim_interrupts(RISCVCPU *cpu, uint32_t interrupts);
 uint32_t riscv_cpu_update_mip(RISCVCPU *cpu, uint32_t mask, uint32_t value);
 #define BOOL_TO_MASK(x) (-!!(x)) /* helper for riscv_cpu_update_mip value */
diff --git a/target/riscv/cpu_bits.h b/target/riscv/cpu_bits.h
index 9c27727e6f..28117bdd32 100644
--- a/target/riscv/cpu_bits.h
+++ b/target/riscv/cpu_bits.h
@@ -550,3 +550,8 @@
 #define SIP_SSIP                           MIP_SSIP
 #define SIP_STIP                           MIP_STIP
 #define SIP_SEIP                           MIP_SEIP
+
+/* MIE masks */
+#define MIE_SEIE                           (1 << IRQ_S_EXT)
+#define MIE_STIE                           (1 << IRQ_S_TIMER)
+#define MIE_SSIE                           (1 << IRQ_S_SOFT)
diff --git a/target/riscv/cpu_helper.c b/target/riscv/cpu_helper.c
index 0116d2499c..5e5029ac0b 100644
--- a/target/riscv/cpu_helper.c
+++ b/target/riscv/cpu_helper.c
@@ -86,6 +86,58 @@ bool riscv_cpu_exec_interrupt(CPUState *cs, int 
interrupt_request)
 
 #if !defined(CONFIG_USER_ONLY)
 
+void riscv_cpu_swap_background_regs(CPURISCVState *env)
+{
+    RISCVCPU *cpu = riscv_env_get_cpu(env);
+    target_ulong tmp;
+    target_ulong mstatus_mask = MSTATUS_MXR | MSTATUS_SUM | MSTATUS_FS |
+                                MSTATUS_SPP | MSTATUS_SPIE | MSTATUS_SIE;
+    target_ulong sie_mask = MIE_SEIE | MIE_STIE | MIE_SSIE;
+
+    g_assert(riscv_has_ext(env, RVH));
+
+#if defined(TARGET_RISCV64)
+    mstatus_mask |= MSTATUS64_UXL;
+#endif
+
+    tmp = env->bsstatus & mstatus_mask;
+    env->bsstatus = env->mstatus & mstatus_mask;
+    env->mstatus = (env->mstatus & ~mstatus_mask) | tmp;
+
+    tmp = env->bsie & sie_mask;
+    env->bsie = env->mie & sie_mask;
+    env->mie = (env->mie & ~sie_mask) | tmp;
+
+    tmp = env->bstvec;
+    env->bstvec = env->stvec;
+    env->stvec = tmp;
+
+    tmp = env->bsscratch;
+    env->bsscratch = env->sscratch;
+    env->sscratch = tmp;
+
+    tmp = env->bsepc;
+    env->bsepc = env->sepc;
+    env->sepc = tmp;
+
+    tmp = env->bscause;
+    env->bscause = env->scause;
+    env->scause = tmp;
+
+    tmp = env->bstval;
+    env->bstval = env->sbadaddr;
+    env->sbadaddr = tmp;
+
+    tmp = env->bsatp;
+    env->bsatp = env->satp;
+    env->satp = tmp;
+
+    tmp = (target_ulong)atomic_read(&env->bsip);
+    tmp = riscv_cpu_update_mip(cpu, (MIP_SSIP | MIP_STIP | MIP_SEIP), tmp);
+    tmp &= MIP_SSIP | MIP_STIP | MIP_SEIP;
+    atomic_set(&env->bsip, tmp);
+}
+
 bool riscv_cpu_virt_enabled(CPURISCVState *env)
 {
     bool tmp;
-- 
2.21.0