Implement LH

1 files changed, 21 insertions, 1 deletions

diff --git a/src/instructions/rvi/mem.rs b/src/instructions/rvi/mem.rs
index 3f89d70..c57e079 100644
--- a/src/instructions/rvi/mem.rs
+++ b/src/instructions/rvi/mem.rs
@@ -1,5 +1,5 @@
 use crate::{
-    consts::{Addr, Byte, DWord, Word},
+    consts::{Addr, Byte, DWord, HWord, Word},
     core::Core,
     instructions::{Instruction, InstructionResult},
     mem::PageNum,
@@ -66,6 +66,26 @@ pub fn sw(core: &mut Core, instr: Instruction) -> InstructionResult {
     }
 }
 
+pub fn lh(core: &mut Core, instr: Instruction) -> InstructionResult {
+    let addr = core.reg_read(instr.rs1()).wrapping_add(instr.imm_i());
+
+    if !addr.is_multiple_of(std::mem::size_of::<HWord>() as Addr) {
+        return InstructionResult::Exception(());
+    }
+
+    let page = (addr / 4096) as PageNum;
+    let offset = (addr / 2 & ((4096 / 2 as Addr) - 1)) as u16;
+
+    match core.mem.read_hword(page, offset) {
+        Ok(x) => {
+            core.reg_write(instr.rd(), x as i16 as i64 as DWord);
+            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());