Implement LD and BNE

1 files changed, 30 insertions, 0 deletions

diff --git a/src/instructions/rvi.rs b/src/instructions/rvi.rs
index 239ae98..f182e88 100644
--- a/src/instructions/rvi.rs
+++ b/src/instructions/rvi.rs
@@ -61,6 +61,26 @@ pub fn sd(core: &mut Core, instr: Instruction) -> InstructionResult {
     }
 }
 
+pub fn ld(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 / 8 & ((4096 / 8 as Addr) - 1)) as u16;
+
+    match core.mem.read_dword(page, offset) {
+        Ok(x) => {
+            core.reg_write(instr.rd(), x);
+            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());
 
@@ -145,6 +165,16 @@ pub fn beq(core: &mut Core, instr: Instruction) -> InstructionResult {
     InstructionResult::Normal
 }
 
+pub fn bne(core: &mut Core, instr: Instruction) -> InstructionResult {
+    if core.reg_read(instr.rs1()) != core.reg_read(instr.rs2()) {
+        core.pc = core.pc.wrapping_add(instr.imm_b());
+    } else {
+        core.advance_pc();
+    }
+
+    InstructionResult::Normal
+}
+
 pub fn slli(core: &mut Core, instr: Instruction) -> InstructionResult {
     core.reg_write(instr.rd(), core.reg_read(instr.rs1()) << instr.imm_shamt());