some debugging stuff and SECOND OPCODE!

1 files changed, 26 insertions, 3 deletions

diff --git a/src/instructions/rvi.rs b/src/instructions/rvi.rs
index 8e7dade..e3c0e09 100644
--- a/src/instructions/rvi.rs
+++ b/src/instructions/rvi.rs
@@ -1,22 +1,25 @@
 use crate::{
+    consts::{Addr, DWord},
     core::{Core, InstructionResult},
     decode::Instruction,
     instructions::{
         OpcodeHandler,
         gen_tools::insert_funct3_splitter,
-        opcodes::{FUNCT3_ADDI, OP_IMM},
+        opcodes::{FUNCT3_ADDI, FUNCT3_SD, OP_IMM, STORE},
     },
+    mem::PageNum,
 };
 
 pub(super) fn add_instrs(list: &mut [OpcodeHandler; 32]) {
     let funct3_split_op_imm = insert_funct3_splitter(&mut list[OP_IMM as usize].splitter);
     funct3_split_op_imm[FUNCT3_ADDI as usize].handler =
         Some(super::InstructionHandler { runner: addi });
+
+    let funct3_split_store = insert_funct3_splitter(&mut list[STORE as usize].splitter);
+    funct3_split_store[FUNCT3_SD as usize].handler = Some(super::InstructionHandler { runner: sd })
 }
 
 fn addi(core: &mut Core, instr: Instruction) -> InstructionResult {
-    eprintln!("Running ADDI");
-
     core.reg_write(
         instr.rd(),
         core.reg_read(instr.rs1()).wrapping_add(instr.imm_i()),
@@ -26,3 +29,23 @@ fn addi(core: &mut Core, instr: Instruction) -> InstructionResult {
 
     InstructionResult::Normal
 }
+
+fn sd(core: &mut Core, instr: Instruction) -> InstructionResult {
+    let addr = core.reg_read(instr.rs1()).wrapping_add(instr.imm_s());
+
+    if !addr.is_multiple_of(std::mem::size_of::<DWord>() as Addr) {
+        return InstructionResult::Exception(());
+    }
+
+    let page = (addr / 4096) as PageNum;
+    let offset = (addr & ((4096 / std::mem::size_of::<DWord>() as Addr) - 1)) as u16;
+    let value = core.reg_read(instr.rs2());
+
+    match core.mem.write_dword(page, offset, value) {
+        Ok(_) => {
+            core.pc = core.pc.wrapping_add(4);
+            InstructionResult::Normal
+        }
+        Err(_) => InstructionResult::Exception(()),
+    }
+}