From 6bd31e73fb910d59066adeb11b9d85411e2c743e Mon Sep 17 00:00:00 2001
From: taitep <taitep@taitep.se>
Date: Fri, 10 Oct 2025 19:01:04 +0200
Subject: [PATCH] some debugging stuff and SECOND OPCODE!

---
 src/core.rs                 |  2 +-
 src/instructions/opcodes.rs |  6 +++++-
 src/instructions/rvi.rs     | 29 ++++++++++++++++++++++++++---
 3 files changed, 32 insertions(+), 5 deletions(-)

diff --git a/src/core.rs b/src/core.rs
index 9e82521..6570e76 100644
--- a/src/core.rs
+++ b/src/core.rs
@@ -68,7 +68,7 @@ impl Core {
                     }
                 }
             } else {
-                eprintln!("Invalid Instruction");
+                eprintln!("Invalid Instruction 0x{:08x} 0b{:032b}", instr.0, instr.0);
                 break;
             }
         }
diff --git a/src/instructions/opcodes.rs b/src/instructions/opcodes.rs
index 33f824d..04059a2 100644
--- a/src/instructions/opcodes.rs
+++ b/src/instructions/opcodes.rs
@@ -3,4 +3,8 @@
 
 pub(super) const OP_IMM: u8 = 0b00100;
 
-pub(super) const FUNCT3_ADDI: u8 = 0x0;
+pub(super) const FUNCT3_ADDI: u8 = 0b000;
+
+pub(super) const STORE: u8 = 0b01000;
+
+pub(super) const FUNCT3_SD: u8 = 0b011;
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(()),
+    }
+}