From 0c5020973511434f14ee106c19f2cecf6fd5ab49 Mon Sep 17 00:00:00 2001
From: Michel Heily <michelheily@gmail.com>
Date: Fri, 5 Jul 2019 16:01:16 +0300
Subject: [PATCH] Fix wrong calculation of conditional branch offset in Thumb
 mode

Former-commit-id: 4cc28b13b19b2bf45e2b0e34d9a9dc0f83f82b01
---
 src/arm7tdmi/thumb/display.rs | 4 ++--
 src/arm7tdmi/thumb/exec.rs    | 4 ++--
 2 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/arm7tdmi/thumb/display.rs b/src/arm7tdmi/thumb/display.rs
index 6eb40d4..2d9dcf3 100644
--- a/src/arm7tdmi/thumb/display.rs
+++ b/src/arm7tdmi/thumb/display.rs
@@ -164,8 +164,8 @@ impl ThumbInstruction {
             "b{cond}\t{addr:#x}",
             cond = self.cond(),
             addr = {
-                let offset = self.offset8() as i8 as i32;
-                (self.pc as i32).wrapping_add(offset) as Addr
+                let offset = ((self.offset8() as i8) << 1) as i32;
+                (self.pc as i32 + 4).wrapping_add(offset) as Addr
             }
         )
     }
diff --git a/src/arm7tdmi/thumb/exec.rs b/src/arm7tdmi/thumb/exec.rs
index fbca6d8..41961b7 100644
--- a/src/arm7tdmi/thumb/exec.rs
+++ b/src/arm7tdmi/thumb/exec.rs
@@ -247,8 +247,8 @@ impl Core {
         if !self.check_arm_cond(insn.cond()) {
             Ok(CpuPipelineAction::IncPC)
         } else {
-            let offset = insn.offset8() as i8 as i32;
-            self.pc = (insn.pc as i32).wrapping_add(offset) as u32;
+            let offset = ((insn.offset8() as i8) << 1) as i32;
+            self.pc = (self.pc as i32).wrapping_add(offset) as u32;
             Ok(CpuPipelineAction::Flush)
         }
     }