[12/18] target-riscv: Add system instructions

diff --git a/target-riscv/helper.h b/target-riscv/helper.h
index eeb1caf..a87a0ba 100644
--- a/target-riscv/helper.h
+++ b/target-riscv/helper.h
@@ -74,3 +74,14 @@  DEF_HELPER_FLAGS_3(fcvt_d_l, TCG_CALL_NO_RWG, i64, env, i64, i64)
 DEF_HELPER_FLAGS_3(fcvt_d_lu, TCG_CALL_NO_RWG, i64, env, i64, i64)
 #endif
 DEF_HELPER_FLAGS_2(fclass_d, TCG_CALL_NO_RWG, tl, env, i64)
+
+/* Special functions */
+#ifndef CONFIG_USER_ONLY
+DEF_HELPER_4(csrrw, tl, env, tl, tl, tl)
+DEF_HELPER_5(csrrs, tl, env, tl, tl, tl, tl)
+DEF_HELPER_5(csrrc, tl, env, tl, tl, tl, tl)
+DEF_HELPER_2(sret, tl, env, tl)
+DEF_HELPER_2(mret, tl, env, tl)
+DEF_HELPER_1(tlb_flush, void, env)
+DEF_HELPER_1(fence_i, void, env)
+#endif /* !CONFIG_USER_ONLY */
diff --git a/target-riscv/op_helper.c b/target-riscv/op_helper.c
index 1a7fb18..ee51f02 100644
--- a/target-riscv/op_helper.c
+++ b/target-riscv/op_helper.c
@@ -24,6 +24,21 @@ 
 #include "qemu/host-utils.h"
 #include "exec/helper-proto.h"
 
+int validate_priv(target_ulong priv)
+{
+    return priv == PRV_U || priv == PRV_S || priv == PRV_M;
+}
+
+void set_privilege(CPURISCVState *env, target_ulong newpriv)
+{
+    if (!validate_priv(newpriv)) {
+        printf("INVALID PRIV SET\n");
+        exit(1);
+    }
+    helper_tlb_flush(env);
+    env->priv = newpriv;
+}
+
 /* Exceptions processing helpers */
 static inline void QEMU_NORETURN do_raise_exception_err(CPURISCVState *env,
                                           uint32_t exception, uintptr_t pc)
@@ -60,7 +75,136 @@  target_ulong helper_mulhsu(CPURISCVState *env, target_ulong arg1,
 }
 #endif
 
+/*
+ * Handle writes to CSRs and any resulting special behavior
+ *
+ * Adapted from Spike's processor_t::set_csr
+ */
+inline void csr_write_helper(CPURISCVState *env, target_ulong val_to_write,
+        target_ulong csrno)
+{
+}
+
+/*
+ * Handle reads to CSRs and any resulting special behavior
+ *
+ * Adapted from Spike's processor_t::get_csr
+ */
+inline target_ulong csr_read_helper(CPURISCVState *env, target_ulong csrno)
+{
+    return 0;
+}
+
+/*
+ * Check that CSR access is allowed.
+ *
+ * Adapted from Spike's decode.h:validate_csr
+ */
+void validate_csr(CPURISCVState *env, uint64_t which, uint64_t write,
+        uint64_t new_pc) {
+    unsigned csr_priv = get_field((which), 0x300);
+    unsigned csr_read_only = get_field((which), 0xC00) == 3;
+    if (((write) && csr_read_only) || (env->priv < csr_priv)) {
+        do_raise_exception_err(env, RISCV_EXCP_ILLEGAL_INST, new_pc);
+    }
+    return;
+}
+
+target_ulong helper_csrrw(CPURISCVState *env, target_ulong src,
+        target_ulong csr, target_ulong new_pc)
+{
+    validate_csr(env, csr, 1, new_pc);
+    uint64_t csr_backup = csr_read_helper(env, csr);
+    csr_write_helper(env, src, csr);
+    return csr_backup;
+}
+
+target_ulong helper_csrrs(CPURISCVState *env, target_ulong src,
+        target_ulong csr, target_ulong new_pc, target_ulong rs1_pass)
+{
+    validate_csr(env, csr, rs1_pass != 0, new_pc);
+    uint64_t csr_backup = csr_read_helper(env, csr);
+    if (rs1_pass != 0) {
+        csr_write_helper(env, src | csr_backup, csr);
+    }
+    return csr_backup;
+}
+
+target_ulong helper_csrrc(CPURISCVState *env, target_ulong src,
+        target_ulong csr, target_ulong new_pc, target_ulong rs1_pass) {
+    validate_csr(env, csr, rs1_pass != 0, new_pc);
+    uint64_t csr_backup = csr_read_helper(env, csr);
+    if (rs1_pass != 0) {
+        csr_write_helper(env, (~src) & csr_backup, csr);
+    }
+    return csr_backup;
+}
+
+target_ulong helper_sret(CPURISCVState *env, target_ulong cpu_pc_deb)
+{
+    if (!(env->priv >= PRV_S)) {
+        helper_raise_exception(env, RISCV_EXCP_ILLEGAL_INST);
+    }
+
+    target_ulong retpc = env->csr[CSR_SEPC];
+    if (retpc & 0x3) {
+        helper_raise_exception(env, RISCV_EXCP_INST_ADDR_MIS);
+    }
+
+    target_ulong mstatus = env->csr[CSR_MSTATUS];
+    target_ulong prev_priv = get_field(mstatus, MSTATUS_SPP);
+    mstatus = set_field(mstatus, MSTATUS_UIE << prev_priv,
+                        get_field(mstatus, MSTATUS_SPIE));
+    mstatus = set_field(mstatus, MSTATUS_SPIE, 0);
+    mstatus = set_field(mstatus, MSTATUS_SPP, PRV_U);
+    set_privilege(env, prev_priv);
+    csr_write_helper(env, mstatus, CSR_MSTATUS);
+
+    return retpc;
+}
+
+target_ulong helper_mret(CPURISCVState *env, target_ulong cpu_pc_deb)
+{
+    if (!(env->priv >= PRV_M)) {
+        helper_raise_exception(env, RISCV_EXCP_ILLEGAL_INST);
+    }
+
+    target_ulong retpc = env->csr[CSR_MEPC];
+    if (retpc & 0x3) {
+        helper_raise_exception(env, RISCV_EXCP_INST_ADDR_MIS);
+    }
+
+    target_ulong mstatus = env->csr[CSR_MSTATUS];
+    target_ulong prev_priv = get_field(mstatus, MSTATUS_MPP);
+    mstatus = set_field(mstatus, MSTATUS_UIE << prev_priv,
+                        get_field(mstatus, MSTATUS_MPIE));
+    mstatus = set_field(mstatus, MSTATUS_MPIE, 0);
+    mstatus = set_field(mstatus, MSTATUS_MPP, PRV_U);
+    set_privilege(env, prev_priv);
+    csr_write_helper(env, mstatus, CSR_MSTATUS);
+
+    return retpc;
+}
+
 #ifndef CONFIG_USER_ONLY
+
+void helper_fence_i(CPURISCVState *env)
+{
+    RISCVCPU *cpu = riscv_env_get_cpu(env);
+    CPUState *cs = CPU(cpu);
+    /* Flush QEMU's TLB */
+    tlb_flush(cs, 1);
+    /* ARM port seems to not know if this is okay inside a TB
+       But we need to do it */
+    tb_flush(cs);
+}
+
+void helper_tlb_flush(CPURISCVState *env)
+{
+    RISCVCPU *cpu = riscv_env_get_cpu(env);
+    tlb_flush(CPU(cpu), 1);
+}
+
 void riscv_cpu_do_unaligned_access(CPUState *cs, vaddr addr,
                                    MMUAccessType access_type, int mmu_idx,
                                    uintptr_t retaddr)
diff --git a/target-riscv/translate.c b/target-riscv/translate.c
index de39276..51d2bf9 100644
--- a/target-riscv/translate.c
+++ b/target-riscv/translate.c
@@ -1158,6 +1158,112 @@  static inline void gen_fp_arith(DisasContext *ctx, uint32_t opc, int rd,
     tcg_temp_free(write_int_rd);
 }
 
+static inline void gen_system(DisasContext *ctx, uint32_t opc,
+                      int rd, int rs1, int csr)
+{
+    TCGv source1, csr_store, dest, rs1_pass, imm_rs1;
+    source1 = tcg_temp_new();
+    csr_store = tcg_temp_new();
+    dest = tcg_temp_new();
+    rs1_pass = tcg_temp_new();
+    imm_rs1 = tcg_temp_new();
+    gen_get_gpr(source1, rs1);
+    tcg_gen_movi_tl(rs1_pass, rs1);
+    tcg_gen_movi_tl(csr_store, csr); /* copy into temp reg to feed to helper */
+
+    switch (opc) {
+    case OPC_RISC_ECALL:
+        switch (csr) {
+        case 0x0: /* ECALL */
+            /* always generates U-level ECALL, fixed in do_interrupt handler */
+            generate_exception(ctx, RISCV_EXCP_U_ECALL);
+            tcg_gen_exit_tb(0); /* no chaining */
+            ctx->bstate = BS_BRANCH;
+            break;
+        case 0x1: /* EBREAK */
+            generate_exception(ctx, RISCV_EXCP_BREAKPOINT);
+            tcg_gen_exit_tb(0); /* no chaining */
+            ctx->bstate = BS_BRANCH;
+            break;
+        case 0x002: /* URET */
+            printf("URET unimplemented\n");
+            exit(1);
+            break;
+        case 0x102: /* SRET */
+            tcg_gen_movi_tl(cpu_PC, ctx->pc);
+            gen_helper_sret(cpu_PC, cpu_env, cpu_PC);
+            tcg_gen_exit_tb(0); /* no chaining */
+            ctx->bstate = BS_BRANCH;
+            break;
+        case 0x202: /* HRET */
+            printf("HRET unimplemented\n");
+            exit(1);
+            break;
+        case 0x302: /* MRET */
+            tcg_gen_movi_tl(cpu_PC, ctx->pc);
+            gen_helper_mret(cpu_PC, cpu_env, cpu_PC);
+            tcg_gen_exit_tb(0); /* no chaining */
+            ctx->bstate = BS_BRANCH;
+            break;
+        case 0x7b2: /* DRET */
+            printf("DRET unimplemented\n");
+            exit(1);
+            break;
+        case 0x105: /* WFI */
+            /* nop for now, as in spike */
+            break;
+        case 0x104: /* SFENCE.VM */
+            gen_helper_tlb_flush(cpu_env);
+            break;
+        default:
+            kill_unknown(ctx, RISCV_EXCP_ILLEGAL_INST);
+            break;
+        }
+        break;
+    default:
+        tcg_gen_movi_tl(cpu_PC, ctx->pc);
+        tcg_gen_movi_tl(imm_rs1, rs1);
+        switch (opc) {
+        case OPC_RISC_CSRRW:
+            gen_helper_csrrw(dest, cpu_env, source1, csr_store, cpu_PC);
+            break;
+        case OPC_RISC_CSRRS:
+            gen_helper_csrrs(dest, cpu_env, source1, csr_store, cpu_PC,
+                    rs1_pass);
+            break;
+        case OPC_RISC_CSRRC:
+            gen_helper_csrrc(dest, cpu_env, source1, csr_store, cpu_PC,
+                    rs1_pass);
+            break;
+        case OPC_RISC_CSRRWI:
+            gen_helper_csrrw(dest, cpu_env, imm_rs1, csr_store, cpu_PC);
+            break;
+        case OPC_RISC_CSRRSI:
+            gen_helper_csrrs(dest, cpu_env, imm_rs1, csr_store, cpu_PC,
+                             rs1_pass);
+            break;
+        case OPC_RISC_CSRRCI:
+            gen_helper_csrrc(dest, cpu_env, imm_rs1, csr_store, cpu_PC,
+                             rs1_pass);
+            break;
+        default:
+            kill_unknown(ctx, RISCV_EXCP_ILLEGAL_INST);
+            break;
+        }
+        gen_set_gpr(rd, dest);
+        /* end tb since we may be changing priv modes, to get mmu_index right */
+        tcg_gen_movi_tl(cpu_PC, ctx->pc + 4);
+        tcg_gen_exit_tb(0); /* no chaining */
+        ctx->bstate = BS_BRANCH;
+        break;
+    }
+    tcg_temp_free(source1);
+    tcg_temp_free(csr_store);
+    tcg_temp_free(dest);
+    tcg_temp_free(rs1_pass);
+    tcg_temp_free(imm_rs1);
+}
+
 static void decode_opc(CPURISCVState *env, DisasContext *ctx)
 {
     int rs1;
@@ -1286,6 +1392,19 @@  static void decode_opc(CPURISCVState *env, DisasContext *ctx)
         gen_fp_arith(ctx, MASK_OP_FP_ARITH(ctx->opcode), rd, rs1, rs2,
                      GET_RM(ctx->opcode));
         break;
+    case OPC_RISC_FENCE:
+        /* standard fence is nop, fence_i flushes TB (like an icache): */
+        if (ctx->opcode & 0x1000) { /* FENCE_I */
+            gen_helper_fence_i(cpu_env);
+            tcg_gen_movi_tl(cpu_PC, ctx->pc + 4);
+            tcg_gen_exit_tb(0); /* no chaining */
+            ctx->bstate = BS_BRANCH;
+        }
+        break;
+    case OPC_RISC_SYSTEM:
+        gen_system(ctx, MASK_OP_SYSTEM(ctx->opcode), rd, rs1,
+                   (ctx->opcode & 0xFFF00000) >> 20);
+        break;
     default:
         kill_unknown(ctx, RISCV_EXCP_ILLEGAL_INST);
         break;