implement CSR ops

1 files changed, 24 insertions, 7 deletions

diff --git a/src/main.rs b/src/main.rs
index 3d8008d..20dde55 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -134,8 +134,6 @@ fn handle_instruction(state: &mut CpuState) {
 
             let imm = ((((imm_high << 5) | imm_low) as i32) << 20 >> 20) as u32;
 
-            let addr = (state.registers[rs1] + imm) as usize;
-
             match f3 {
                 0 => sb(state, rs1, rs2, imm),
                 1 => sh(state, rs1, rs2, imm),
@@ -200,11 +198,30 @@ fn handle_instruction(state: &mut CpuState) {
             auipc(state, rd as usize, imm);
         }
         0b1110011 => {
-            // ecall
-            eprintln!(
-                "TRAP! syscall {} with gp state {:?}",
-                state.registers[17], pc
-            );
+            let imm = word >> 20;
+            let rd = ((word >> 7) & REGISTER_NAME_WIDTH) as usize;
+            let rs1 = ((word >> 15) & REGISTER_NAME_WIDTH) as usize;
+            let f3 = (word >> 12) & 0b111;
+
+            match f3 {
+                0 => {
+                    eprintln!(
+                        "TRAP! syscall {} with gp state {:?}",
+                        state.registers[17], pc
+                    );
+
+                    // todo: trap to CSR 0x305
+                } // ecall
+                1 => csrrw(state, rd, rs1, imm as usize),
+                2 => csrrs(state, rd, rs1, imm as usize),
+                3 => csrrc(state, rd, rs1, imm as usize),
+                5 => csrrwi(state, rd, rs1 as u32, imm as usize),
+                6 => csrrsi(state, rd, rs1 as u32, imm as usize),
+                7 => csrrci(state, rd, rs1 as u32, imm as usize),
+
+                _ => panic!("Illegal instruction: {:x}", word),
+            }
+
         } // ecall (ebreak not implemented)
         0b0101111 => {
             let f5 = word >> 27;