From 638e449421e998d47d19408da094a9da5cecc33b Mon Sep 17 00:00:00 2001
From: Michel Heily <michelheily@gmail.com>
Date: Fri, 5 Jul 2019 16:21:24 +0300
Subject: [PATCH] Impl Thumb 18 (Branch)

Former-commit-id: 152dab739a1fa1b9d324084fc810b8540af9c3d7
---
 src/arm7tdmi/thumb/display.rs | 12 ++++++++++++
 src/arm7tdmi/thumb/exec.rs    | 17 ++++++++++++++---
 2 files changed, 26 insertions(+), 3 deletions(-)

diff --git a/src/arm7tdmi/thumb/display.rs b/src/arm7tdmi/thumb/display.rs
index 2d9dcf3..f9e1c80 100644
--- a/src/arm7tdmi/thumb/display.rs
+++ b/src/arm7tdmi/thumb/display.rs
@@ -170,6 +170,17 @@ impl ThumbInstruction {
         )
     }
 
+    fn fmt_thumb_branch(&self, f: &mut fmt::Formatter) -> fmt::Result {
+        write!(
+            f,
+            "b\t{addr:#x}",
+            addr = {
+                let offset = (self.offset11() << 21) >> 20;
+                (self.pc as i32 + 4).wrapping_add(offset) as Addr
+            }
+        )
+    }
+
     fn fmt_thumb_branch_long_with_link(&self, f: &mut fmt::Formatter) -> fmt::Result {
         write!(f, "bl\t#0x{:08x}", {
             let offset11 = self.offset11();
@@ -198,6 +209,7 @@ impl fmt::Display for ThumbInstruction {
             ThumbFormat::AddSp => self.fmt_thumb_add_sp(f),
             ThumbFormat::PushPop => self.fmt_thumb_push_pop(f),
             ThumbFormat::BranchConditional => self.fmt_thumb_branch_with_cond(f),
+            ThumbFormat::Branch => self.fmt_thumb_branch(f),
             ThumbFormat::BranchLongWithLink => self.fmt_thumb_branch_long_with_link(f),
             _ => write!(f, "({:?})", self),
         }
diff --git a/src/arm7tdmi/thumb/exec.rs b/src/arm7tdmi/thumb/exec.rs
index 1090c45..f5afd66 100644
--- a/src/arm7tdmi/thumb/exec.rs
+++ b/src/arm7tdmi/thumb/exec.rs
@@ -132,11 +132,11 @@ impl Core {
                 insn.rs()
             };
             let arm_alu_op: ArmOpCode = insn.format5_op().into();
-            let op1 = self.gpr[dst_reg] as i32;
-            let op2 = self.gpr[src_reg] as i32;
+            let op1 = self.get_reg(dst_reg) as i32;
+            let op2 = self.get_reg(src_reg) as i32;
             let result = self.alu(arm_alu_op, op1, op2, true);
             if let Some(result) = result {
-                self.gpr[dst_reg] = result as u32;
+                self.set_reg(dst_reg, result as u32);
             }
             Ok(CpuPipelineAction::IncPC)
         }
@@ -269,6 +269,16 @@ impl Core {
         }
     }
 
+    fn exec_thumb_branch(
+        &mut self,
+        _bus: &mut Bus,
+        insn: ThumbInstruction,
+    ) -> CpuExecResult {
+        let offset = ((insn.offset11() << 21) >> 20) as i32;
+        self.pc = (self.pc as i32).wrapping_add(offset) as u32;
+        Ok(CpuPipelineAction::Flush)
+    }
+
     fn exec_thumb_branch_long_with_link(
         &mut self,
         _bus: &mut Bus,
@@ -305,6 +315,7 @@ impl Core {
             ThumbFormat::AddSp => self.exec_thumb_add_sp(bus, insn),
             ThumbFormat::PushPop => self.exec_thumb_push_pop(bus, insn),
             ThumbFormat::BranchConditional => self.exec_thumb_branch_with_cond(bus, insn),
+            ThumbFormat::Branch => self.exec_thumb_branch(bus, insn),
             ThumbFormat::BranchLongWithLink => self.exec_thumb_branch_long_with_link(bus, insn),
             _ => unimplemented!("thumb not implemented {:#x?}", insn),
         }