From f39095a03b3196cd46777334c678259833c72c96 Mon Sep 17 00:00:00 2001
From: Michel Heily <michelheily@gmail.com>
Date: Sat, 27 Jul 2019 21:30:27 +0300
Subject: [PATCH] Fix thumb conditional branch offset

Former-commit-id: b42f233b7f2ca5d427b0e36860a305f7e1a26a1d
---
 src/core/arm7tdmi/thumb/display.rs | 4 ++--
 src/core/arm7tdmi/thumb/exec.rs    | 4 ++--
 src/core/arm7tdmi/thumb/mod.rs     | 6 +++---
 3 files changed, 7 insertions(+), 7 deletions(-)

diff --git a/src/core/arm7tdmi/thumb/display.rs b/src/core/arm7tdmi/thumb/display.rs
index fa6f3c4..ce4efe4 100644
--- a/src/core/arm7tdmi/thumb/display.rs
+++ b/src/core/arm7tdmi/thumb/display.rs
@@ -23,7 +23,7 @@ impl ThumbInstruction {
             "{op}\t{Rd}, #{Offset8:#x}",
             op = self.format3_op(),
             Rd = reg_string(self.rd()),
-            Offset8 = self.offset8()
+            Offset8 = self.raw & 0xff
         )
     }
 
@@ -252,7 +252,7 @@ impl ThumbInstruction {
             "b{cond}\t{addr:#x}",
             cond = self.cond(),
             addr = {
-                let offset = ((self.offset8() as i8) << 1) as i32;
+                let offset = self.bcond_offset();
                 (self.pc as i32 + 4).wrapping_add(offset) as Addr
             }
         )
diff --git a/src/core/arm7tdmi/thumb/exec.rs b/src/core/arm7tdmi/thumb/exec.rs
index 691cb4f..d066e1b 100644
--- a/src/core/arm7tdmi/thumb/exec.rs
+++ b/src/core/arm7tdmi/thumb/exec.rs
@@ -69,7 +69,7 @@ impl Core {
     ) -> CpuExecResult {
         let arm_alu_op: AluOpCode = insn.format3_op().into();
         let op1 = self.get_reg(insn.rd()) as i32;
-        let op2 = insn.offset8() as u8 as i32;
+        let op2 = ((insn.raw & 0xff) as i8) as u8 as i32;
         let result = self.alu_flags(arm_alu_op, op1, op2);
         if let Some(result) = result {
             self.set_reg(insn.rd(), result as u32);
@@ -388,7 +388,7 @@ impl Core {
         if !self.check_arm_cond(insn.cond()) {
             Ok(())
         } else {
-            let offset = ((insn.offset8() as i8) << 1) as i32;
+            let offset = insn.bcond_offset();
             self.pc = (self.pc as i32).wrapping_add(offset) as u32;
             self.flush_pipeline();
             Ok(())
diff --git a/src/core/arm7tdmi/thumb/mod.rs b/src/core/arm7tdmi/thumb/mod.rs
index b95ffc7..de32368 100644
--- a/src/core/arm7tdmi/thumb/mod.rs
+++ b/src/core/arm7tdmi/thumb/mod.rs
@@ -294,8 +294,8 @@ impl ThumbInstruction {
         self.raw.bit_range(6..11) as i8
     }
 
-    pub fn offset8(&self) -> i8 {
-        self.raw.bit_range(0..8) as i8
+    pub fn bcond_offset(&self) -> i32 {
+        ((((self.raw & 0xff) as u32) << 24) as i32) >> 23
     }
 
     pub fn offset11(&self) -> i32 {
@@ -303,7 +303,7 @@ impl ThumbInstruction {
     }
 
     pub fn word8(&self) -> u16 {
-        self.raw.bit_range(0..8) << 2
+        (self.raw & 0xff) << 2
     }
 
     pub fn is_transferring_bytes(&self) -> bool {