Implement SW

2 files changed, 22 insertions, 1 deletions

diff --git a/src/instructions.rs b/src/instructions.rs
index 615dd7a..c5519f8 100644
--- a/src/instructions.rs
+++ b/src/instructions.rs
@@ -36,6 +36,7 @@ pub(crate) fn find_and_exec(instr: Instruction, core: &mut Core) -> Option<Instr
         0b01000 => match instr.funct3() {
             // STORE
             0b000 => Some(rvi::sb(core, instr)),
+            0b010 => Some(rvi::sw(core, instr)),
             0b011 => Some(rvi::sd(core, instr)),
             _ => None,
         },
diff --git a/src/instructions/rvi.rs b/src/instructions/rvi.rs
index df3d9d5..cda88cf 100644
--- a/src/instructions/rvi.rs
+++ b/src/instructions/rvi.rs
@@ -5,7 +5,7 @@
 // See LICENSE file in the project root for full license text.
 
 use crate::{
-    consts::{Addr, DWord},
+    consts::{Addr, DWord, Word},
     core::{Core, InstructionResult},
     decode::Instruction,
     mem::PageNum,
@@ -118,6 +118,26 @@ pub fn ld(core: &mut Core, instr: Instruction) -> InstructionResult {
     }
 }
 
+pub fn sw(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::<Word>() as Addr) {
+        return InstructionResult::Exception(());
+    }
+
+    let page = (addr / 4096) as PageNum;
+    let offset = (addr / 4 & ((4096 / 4 as Addr) - 1)) as u16;
+    let value = core.reg_read(instr.rs2()) as Word;
+
+    match core.mem.write_word(page, offset, value) {
+        Ok(_) => {
+            core.advance_pc();
+            InstructionResult::Normal
+        }
+        Err(_) => InstructionResult::Exception(()),
+    }
+}
+
 pub fn sb(core: &mut Core, instr: Instruction) -> InstructionResult {
     let addr = core.reg_read(instr.rs1()).wrapping_add(instr.imm_s());