[PATCH v3 2/5] target/nios2: Shadow register set

[Amir Gonnen <amir.gonnen@neuroblade.ai>]

Implement shadow register set and related instructions
rdprs, wrprs. Fix eret to update either status or sstatus
according to current register set.
eret also changes register set when needed.

Signed-off-by: Amir Gonnen <amir.gonnen@neuroblade.ai>
---
 target/nios2/cpu.c       |  1 +
 target/nios2/cpu.h       | 48 +++++++++++++++++++++++++++---
 target/nios2/helper.h    |  1 +
 target/nios2/op_helper.c | 18 +++++++++++
 target/nios2/translate.c | 64 ++++++++++++++++++++++++++++++++++++----
 5 files changed, 123 insertions(+), 9 deletions(-)

diff --git a/target/nios2/cpu.c b/target/nios2/cpu.c
index 6975ae4bdb..026ee18b01 100644
--- a/target/nios2/cpu.c
+++ b/target/nios2/cpu.c
@@ -54,6 +54,7 @@ static void nios2_cpu_reset(DeviceState *dev)
     ncc->parent_reset(dev);
 
     memset(env->regs, 0, sizeof(uint32_t) * NUM_CORE_REGS);
+    memset(env->shadow_regs, 0, sizeof(uint32_t) * NUM_REG_SETS * NUM_GP_REGS);
     env->regs[R_PC] = cpu->reset_addr;
 
 #if defined(CONFIG_USER_ONLY)
diff --git a/target/nios2/cpu.h b/target/nios2/cpu.h
index a00e4229ce..dbb4c968df 100644
--- a/target/nios2/cpu.h
+++ b/target/nios2/cpu.h
@@ -23,6 +23,7 @@
 
 #include "exec/cpu-defs.h"
 #include "hw/core/cpu.h"
+#include "hw/registerfields.h"
 #include "qom/object.h"
 
 typedef struct CPUNios2State CPUNios2State;
@@ -57,9 +58,14 @@ struct Nios2CPUClass {
 #define EXCEPTION_ADDRESS     0x00000004
 #define FAST_TLB_MISS_ADDRESS 0x00000008
 
+#define NUM_GP_REGS 32
+#define NUM_CR_REGS 32
 
 /* GP regs + CR regs + PC */
-#define NUM_CORE_REGS (32 + 32 + 1)
+#define NUM_CORE_REGS (NUM_GP_REGS + NUM_CR_REGS + 1)
+
+/* 63 shadow register sets. 0 is the primary set */
+#define NUM_REG_SETS 64
 
 /* General purpose register aliases */
 #define R_ZERO   0
@@ -80,15 +86,15 @@ struct Nios2CPUClass {
 #define R_RA     31
 
 /* Control register aliases */
-#define CR_BASE  32
+#define CR_BASE  NUM_GP_REGS
 #define CR_STATUS    (CR_BASE + 0)
 #define   CR_STATUS_PIE  (1 << 0)
 #define   CR_STATUS_U    (1 << 1)
 #define   CR_STATUS_EH   (1 << 2)
 #define   CR_STATUS_IH   (1 << 3)
 #define   CR_STATUS_IL   (63 << 4)
-#define   CR_STATUS_CRS  (63 << 10)
-#define   CR_STATUS_PRS  (63 << 16)
+FIELD(CR_STATUS, CRS, 10, 6)
+FIELD(CR_STATUS, PRS, 16, 6)
 #define   CR_STATUS_NMI  (1 << 22)
 #define   CR_STATUS_RSIE (1 << 23)
 #define CR_ESTATUS   (CR_BASE + 1)
@@ -131,6 +137,7 @@ struct Nios2CPUClass {
 
 /* Other registers */
 #define R_PC         64
+#define R_SSTATUS    30
 
 /* Exceptions */
 #define EXCP_BREAK    0x1000
@@ -157,6 +164,7 @@ struct Nios2CPUClass {
 
 struct CPUNios2State {
     uint32_t regs[NUM_CORE_REGS];
+    uint32_t shadow_regs[NUM_REG_SETS][NUM_GP_REGS];
 
 #if !defined(CONFIG_USER_ONLY)
     Nios2MMU mmu;
@@ -245,4 +253,36 @@ static inline void cpu_get_tb_cpu_state(CPUNios2State *env, target_ulong *pc,
     *flags = (env->regs[CR_STATUS] & (CR_STATUS_EH | CR_STATUS_U));
 }
 
+static inline uint32_t cpu_get_crs(const CPUNios2State *env)
+{
+    return FIELD_EX32(env->regs[CR_STATUS], CR_STATUS, CRS);
+}
+
+static inline uint32_t cpu_get_prs(const CPUNios2State *env)
+{
+    return FIELD_EX32(env->regs[CR_STATUS], CR_STATUS, PRS);
+}
+
+static inline void cpu_change_reg_set(CPUNios2State *env, uint32_t prev_set,
+                                      uint32_t new_set)
+{
+    if (new_set == prev_set) {
+        return;
+    }
+    memcpy(env->shadow_regs[prev_set], env->regs,
+           sizeof(uint32_t) * NUM_GP_REGS);
+    memcpy(env->regs, env->shadow_regs[new_set],
+           sizeof(uint32_t) * NUM_GP_REGS);
+    env->regs[CR_STATUS] =
+        FIELD_DP32(env->regs[CR_STATUS], CR_STATUS, PRS, prev_set);
+    env->regs[CR_STATUS] =
+        FIELD_DP32(env->regs[CR_STATUS], CR_STATUS, CRS, new_set);
+}
+
+static inline void cpu_set_crs(CPUNios2State *env, uint32_t value)
+{
+    uint32_t crs = cpu_get_crs(env);
+    cpu_change_reg_set(env, crs, value);
+}
+
 #endif /* NIOS2_CPU_H */
diff --git a/target/nios2/helper.h b/target/nios2/helper.h
index a44ecfdf7a..2e400b1f12 100644
--- a/target/nios2/helper.h
+++ b/target/nios2/helper.h
@@ -18,6 +18,7 @@
  * <http://www.gnu.org/licenses/lgpl-2.1.html>
  */
 
+DEF_HELPER_2(eret, void, env, i32)
 DEF_HELPER_FLAGS_2(raise_exception, TCG_CALL_NO_WG, noreturn, env, i32)
 
 #if !defined(CONFIG_USER_ONLY)
diff --git a/target/nios2/op_helper.c b/target/nios2/op_helper.c
index caa885f7b4..c8ce399332 100644
--- a/target/nios2/op_helper.c
+++ b/target/nios2/op_helper.c
@@ -30,3 +30,21 @@ void helper_raise_exception(CPUNios2State *env, uint32_t index)
     cs->exception_index = index;
     cpu_loop_exit(cs);
 }
+
+void helper_eret(CPUNios2State *env, uint32_t new_pc)
+{
+    uint32_t crs = cpu_get_crs(env);
+    if (crs == 0) {
+        env->regs[CR_STATUS] = env->regs[CR_ESTATUS];
+    } else {
+        env->regs[CR_STATUS] = env->regs[R_SSTATUS];
+    }
+
+    /*
+     * At this point CRS was updated by the above assignment to CR_STATUS.
+     * Therefore we need to retrieve the new value of CRS and potentially
+     * switch the register set
+     */
+    cpu_change_reg_set(env, crs, cpu_get_crs(env));
+    env->regs[R_PC] = new_pc;
+}
diff --git a/target/nios2/translate.c b/target/nios2/translate.c
index 007c17e6e9..f1ac1bf126 100644
--- a/target/nios2/translate.c
+++ b/target/nios2/translate.c
@@ -205,6 +205,34 @@ static void call(DisasContext *dc, uint32_t code, uint32_t flags)
 /*
  * I-Type instructions
  */
+
+/*
+ * rB <- prs.rA + sigma(IMM16)
+ */
+static void rdprs(DisasContext *dc, uint32_t code, uint32_t flags)
+{
+    I_TYPE(instr, code);
+
+    gen_check_supervisor(dc);
+
+    TCGv_i32 t = tcg_temp_new_i32();
+    TCGv_ptr p = tcg_temp_new_ptr();
+
+    tcg_gen_extract_i32(t, cpu_R[CR_STATUS],
+                        R_CR_STATUS_PRS_SHIFT,
+                        R_CR_STATUS_PRS_LENGTH);
+    tcg_gen_muli_i32(t, t, sizeof(uint32_t) * NUM_GP_REGS);
+    tcg_gen_ext_i32_ptr(p, t);
+
+    tcg_gen_add_ptr(p, p, cpu_env);
+    tcg_gen_ld_i32(t, p, offsetof(CPUNios2State, shadow_regs)
+                    + sizeof(uint32_t) * instr.a);
+    tcg_gen_addi_i32(cpu_R[instr.b], t, instr.imm16.s);
+
+    tcg_temp_free_ptr(p);
+    tcg_temp_free_i32(t);
+}
+
 /* Load instructions */
 static void gen_ldx(DisasContext *dc, uint32_t code, uint32_t flags)
 {
@@ -365,7 +393,7 @@ static const Nios2Instruction i_type_instructions[] = {
     INSTRUCTION_FLG(gen_stx, MO_SL),                  /* stwio */
     INSTRUCTION_FLG(gen_bxx, TCG_COND_LTU),           /* bltu */
     INSTRUCTION_FLG(gen_ldx, MO_UL),                  /* ldwio */
-    INSTRUCTION_UNIMPLEMENTED(),                      /* rdprs */
+    INSTRUCTION(rdprs),                               /* rdprs */
     INSTRUCTION_ILLEGAL(),
     INSTRUCTION_FLG(handle_r_type_instr, 0),          /* R-Type */
     INSTRUCTION_NOP(),                                /* flushd */
@@ -378,16 +406,42 @@ static const Nios2Instruction i_type_instructions[] = {
 /*
  * R-Type instructions
  */
+
+/*
+ * prs.rC <- rA
+ */
+static void wrprs(DisasContext *dc, uint32_t code, uint32_t flags)
+{
+    R_TYPE(instr, code);
+
+    gen_check_supervisor(dc);
+
+    TCGv_i32 t = tcg_temp_new_i32();
+    TCGv_ptr p = tcg_temp_new_ptr();
+
+    tcg_gen_extract_i32(t, cpu_R[CR_STATUS],
+                        R_CR_STATUS_PRS_SHIFT,
+                        R_CR_STATUS_PRS_LENGTH);
+    tcg_gen_muli_i32(t, t, sizeof(uint32_t) * NUM_GP_REGS);
+    tcg_gen_ext_i32_ptr(p, t);
+
+    tcg_gen_add_ptr(p, p, cpu_env);
+    tcg_gen_st_i32(cpu_R[instr.a], p, offsetof(CPUNios2State, shadow_regs)
+                   + sizeof(uint32_t) * instr.c);
+
+    tcg_temp_free_ptr(p);
+    tcg_temp_free_i32(t);
+}
+
 /*
- * status <- estatus
+ * status <- CRS == 0? estatus: sstatus
  * PC <- ea
  */
 static void eret(DisasContext *dc, uint32_t code, uint32_t flags)
 {
     gen_check_supervisor(dc);
 
-    tcg_gen_mov_tl(cpu_R[CR_STATUS], cpu_R[CR_ESTATUS]);
-    tcg_gen_mov_tl(cpu_R[R_PC], cpu_R[R_EA]);
+    gen_helper_eret(cpu_env, cpu_R[R_EA]);
 
     dc->base.is_jmp = DISAS_JUMP;
 }
@@ -665,7 +719,7 @@ static const Nios2Instruction r_type_instructions[] = {
     INSTRUCTION_ILLEGAL(),
     INSTRUCTION(slli),                                /* slli */
     INSTRUCTION(sll),                                 /* sll */
-    INSTRUCTION_UNIMPLEMENTED(),                      /* wrprs */
+    INSTRUCTION(wrprs),                               /* wrprs */
     INSTRUCTION_ILLEGAL(),
     INSTRUCTION(or),                                  /* or */
     INSTRUCTION(mulxsu),                              /* mulxsu */
-- 
2.25.1