Implement ANDI and BEQ

2 files changed, 24 insertions, 0 deletions

diff --git a/src/instructions.rs b/src/instructions.rs
index 935a51f..68b43a6 100644
--- a/src/instructions.rs
+++ b/src/instructions.rs
@@ -21,6 +21,7 @@ pub(crate) fn find_and_exec(instr: Instruction, core: &mut Core) -> Option<Instr
                 0b000000 => Some(rvi::slli(core, instr)),
                 _ => None,
             },
+            0b111 => Some(rvi::andi(core, instr)),
             _ => None,
         },
         0b00110 => match instr.funct3() {
@@ -40,6 +41,11 @@ pub(crate) fn find_and_exec(instr: Instruction, core: &mut Core) -> Option<Instr
             0b100 => Some(rvi::lbu(core, instr)),
             _ => None,
         },
+        0b11000 => match instr.funct3() {
+            // BRANCH
+            0b000 => Some(rvi::beq(core, instr)),
+            _ => None,
+        },
         0b01101 => Some(rvi::lui(core, instr)),
         0b11011 => Some(rvi::jal(core, instr)),
         _ => None,
diff --git a/src/instructions/rvi.rs b/src/instructions/rvi.rs
index a212b8d..3aadc10 100644
--- a/src/instructions/rvi.rs
+++ b/src/instructions/rvi.rs
@@ -32,6 +32,14 @@ pub fn addiw(core: &mut Core, instr: Instruction) -> InstructionResult {
     InstructionResult::Normal
 }
 
+pub fn andi(core: &mut Core, instr: Instruction) -> InstructionResult {
+    core.reg_write(instr.rd(), core.reg_read(instr.rs1()) & instr.imm_i());
+
+    core.advance_pc();
+
+    InstructionResult::Normal
+}
+
 // TODO: Support misaligned memory access
 pub fn sd(core: &mut Core, instr: Instruction) -> InstructionResult {
     let addr = core.reg_read(instr.rs1()).wrapping_add(instr.imm_s());
@@ -115,6 +123,16 @@ pub fn jal(core: &mut Core, instr: Instruction) -> InstructionResult {
     InstructionResult::Normal
 }
 
+pub fn beq(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());