From b8a9aaee86c6114b3edd104125cba5687494ce93 Mon Sep 17 00:00:00 2001
From: Rekai Musuka <rekai@musuka.dev>
Date: Sat, 29 Jan 2022 22:07:36 -0400
Subject: [PATCH] fix(cpu): resolve issues with unexpected PC value in THUMB

---
 src/cpu/thumb/format12.zig | 2 +-
 src/cpu/thumb/format16.zig | 2 +-
 src/cpu/thumb/format19.zig | 2 +-
 src/cpu/thumb/format6.zig  | 2 +-
 4 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/cpu/thumb/format12.zig b/src/cpu/thumb/format12.zig
index fa026aa..86df0cc 100644
--- a/src/cpu/thumb/format12.zig
+++ b/src/cpu/thumb/format12.zig
@@ -8,7 +8,7 @@ pub fn format12(comptime isSP: bool, comptime rd: u3) InstrFn {
     return struct {
         fn inner(cpu: *Arm7tdmi, _: *Bus, opcode: u16) void {
             // ADD
-            const left = if (isSP) cpu.r[13] else cpu.fakePC() & 0xFFFF_FFFC;
+            const left = if (isSP) cpu.r[13] else (cpu.r[15] + 2) & 0xFFFF_FFFD;
             const right = (opcode & 0xFF) << 2;
             const result = left + right; // TODO: What about overflows?
             cpu.r[rd] = result;
diff --git a/src/cpu/thumb/format16.zig b/src/cpu/thumb/format16.zig
index 8ec032c..e116818 100644
--- a/src/cpu/thumb/format16.zig
+++ b/src/cpu/thumb/format16.zig
@@ -19,7 +19,7 @@ pub fn format16(comptime cond: u4) InstrFn {
             };
 
             if (should_execute) {
-                cpu.r[15] = (cpu.fakePC() & 0xFFFF_FFFC) +% offset;
+                cpu.r[15] = (cpu.r[15] + 2) +% offset;
             }
         }
     }.inner;
diff --git a/src/cpu/thumb/format19.zig b/src/cpu/thumb/format19.zig
index 0c5927a..86efb03 100644
--- a/src/cpu/thumb/format19.zig
+++ b/src/cpu/thumb/format19.zig
@@ -19,7 +19,7 @@ pub fn format19(comptime is_low: bool) InstrFn {
                 cpu.r[14] = old_pc | 1;
             } else {
                 // Instruction 1
-                cpu.r[14] = (cpu.fakePC() & 0xFFFF_FFFC) + (u32SignExtend(11, @as(u32, offset)) << 12);
+                cpu.r[14] = (cpu.r[15] + 2) + (u32SignExtend(11, @as(u32, offset)) << 12);
             }
         }
     }.inner;
diff --git a/src/cpu/thumb/format6.zig b/src/cpu/thumb/format6.zig
index 392578c..27e24f2 100644
--- a/src/cpu/thumb/format6.zig
+++ b/src/cpu/thumb/format6.zig
@@ -11,7 +11,7 @@ pub fn format6(comptime rd: u3) InstrFn {
             const offset = (opcode & 0xFF) << 2;
 
             // FIXME: Should this overflow?
-            cpu.r[rd] = bus.read32((cpu.fakePC() & 0xFFFF_FFFC) + offset);
+            cpu.r[rd] = bus.read32((cpu.r[15] + 2 & 0xFFFF_FFFD) + offset);
         }
     }.inner;
 }