diff --git a/README.md b/README.md
index b19c46a..3633d0c 100644
--- a/README.md
+++ b/README.md
@@ -2,13 +2,13 @@
 An in-progress Game Boy Advance Emulator written in Zig ⚡!
 
 ## Tests 
-- [ ] [jsmolka's GBA Test Collection](https://github.com/jsmolka/gba-tests)
+- [x] [jsmolka's GBA Test Collection](https://github.com/jsmolka/gba-tests)
     - [x] `arm.gba` and `thumb.gba`
     - [x] `flash64.gba`, `flash128.gba`, `none.gba`, and `sram.gba`
     - [x] `hello.gba`, `shades.gba`, and `stripes.gba`
     - [x] `memory.gba`
     - [x] `bios.gba`
-    - [ ] `nes.gba`
+    - [x] `nes.gba`
 - [ ] [DenSinH's GBA ROMs](https://github.com/DenSinH/GBARoms)
     - [x] `eeprom-test` and `flash-test`
     - [x] `midikey2freq`
diff --git a/src/core/Bus.zig b/src/core/Bus.zig
index 56b00de..38f0f50 100644
--- a/src/core/Bus.zig
+++ b/src/core/Bus.zig
@@ -46,7 +46,7 @@ iwram: Iwram,
 ewram: Ewram,
 io: Io,
 
-cpu: ?*Arm7tdmi,
+cpu: *Arm7tdmi,
 sched: *Scheduler,
 
 pub fn init(self: *Self, allocator: Allocator, sched: *Scheduler, cpu: *Arm7tdmi, paths: FilePaths) !void {
@@ -82,9 +82,9 @@ pub fn dbgRead(self: *const Self, comptime T: type, address: u32) T {
         // General Internal Memory
         0x00 => blk: {
             if (address < Bios.size)
-                break :blk self.bios.dbgRead(T, self.cpu.?.r[15], aligned_addr);
+                break :blk self.bios.dbgRead(T, self.cpu.r[15], aligned_addr);
 
-            break :blk self.readOpenBus(T, address);
+            break :blk self.openBus(T, address);
         },
         0x02 => self.ewram.read(T, aligned_addr),
         0x03 => self.iwram.read(T, aligned_addr),
@@ -109,48 +109,63 @@ pub fn dbgRead(self: *const Self, comptime T: type, address: u32) T {
 
             break :blk @as(T, value) * multiplier;
         },
-        else => self.readOpenBus(T, address),
+        else => self.openBus(T, address),
     };
 }
 
 fn readIo(self: *const Self, comptime T: type, unaligned_address: u32) T {
     const maybe_value = io.read(self, T, forceAlign(T, unaligned_address));
-    return if (maybe_value) |value| value else self.readOpenBus(T, unaligned_address);
+    return if (maybe_value) |value| value else self.openBus(T, unaligned_address);
 }
 
-fn readOpenBus(self: *const Self, comptime T: type, address: u32) T {
-    const r15 = self.cpu.?.r[15];
+fn openBus(self: *const Self, comptime T: type, address: u32) T {
+    const r15 = self.cpu.r[15];
 
     const word = blk: {
-        // If u32 Open Bus, read recently fetched opcode (PC + 8)
-        if (!self.cpu.?.cpsr.t.read()) break :blk self.dbgRead(u32, r15 + 4);
+        // If Arm, get the most recently fetched instruction (PC + 8)
+        if (!self.cpu.cpsr.t.read()) break :blk self.cpu.pipe.stage[1].?;
+
         const page = @truncate(u8, r15 >> 24);
 
+        // PC + 2 = stage[0]
+        // PC + 4 = stage[1]
+        // PC + 6 = Need a Debug Read for this?
+
         switch (page) {
             // EWRAM, PALRAM, VRAM, and Game ROM (16-bit)
             0x02, 0x05, 0x06, 0x08...0x0D => {
-                // (PC + 4)
-                const halfword = self.dbgRead(u16, r15 + 2);
-
-                break :blk @as(u32, halfword) << 16 | halfword;
+                const halfword: u32 = @truncate(u16, self.cpu.pipe.stage[1].?);
+                break :blk halfword << 16 | halfword;
             },
+
             // BIOS or OAM (32-bit)
             0x00, 0x07 => {
                 // Aligned: (PC + 6) | (PC + 4)
                 // Unaligned: (PC + 4) | (PC + 2)
-                const offset: u32 = if (address & 3 == 0b00) 2 else 0;
+                const aligned = address & 3 == 0b00;
 
-                break :blk @as(u32, self.dbgRead(u16, r15 + 2 + offset)) << 16 | self.dbgRead(u16, r15 + offset);
+                // TODO: What to do on PC + 6?
+                const high: u32 = if (aligned) self.dbgRead(u16, r15 + 4) else @truncate(u16, self.cpu.pipe.stage[1].?);
+                const low: u32 = @truncate(u16, self.cpu.pipe.stage[@boolToInt(aligned)].?);
+
+                break :blk high << 16 | low;
             },
+
             // IWRAM (16-bit but special)
             0x03 => {
                 // Aligned: (PC + 2) | (PC + 4)
                 // Unaligned: (PC + 4) | (PC + 2)
-                const offset: u32 = if (address & 3 == 0b00) 2 else 0;
+                const aligned = address & 3 == 0b00;
 
-                break :blk @as(u32, self.dbgRead(u16, r15 + 2 - offset)) << 16 | self.dbgRead(u16, r15 + offset);
+                const high: u32 = @truncate(u16, self.cpu.pipe.stage[1 - @boolToInt(aligned)].?);
+                const low: u32 = @truncate(u16, self.cpu.pipe.stage[@boolToInt(aligned)].?);
+
+                break :blk high << 16 | low;
+            },
+            else => {
+                log.err("THUMB open bus read from 0x{X:0>2} page @0x{X:0>8}", .{ page, address });
+                @panic("invariant most-likely broken");
             },
-            else => unreachable,
         }
     };
 
@@ -167,9 +182,9 @@ pub fn read(self: *Self, comptime T: type, address: u32) T {
         // General Internal Memory
         0x00 => blk: {
             if (address < Bios.size)
-                break :blk self.bios.read(T, self.cpu.?.r[15], aligned_addr);
+                break :blk self.bios.read(T, self.cpu.r[15], aligned_addr);
 
-            break :blk self.readOpenBus(T, address);
+            break :blk self.openBus(T, address);
         },
         0x02 => self.ewram.read(T, aligned_addr),
         0x03 => self.iwram.read(T, aligned_addr),
@@ -194,7 +209,7 @@ pub fn read(self: *Self, comptime T: type, address: u32) T {
 
             break :blk @as(T, value) * multiplier;
         },
-        else => self.readOpenBus(T, address),
+        else => self.openBus(T, address),
     };
 }
 
diff --git a/src/core/cpu.zig b/src/core/cpu.zig
index 80d0a1b..6f15cae 100644
--- a/src/core/cpu.zig
+++ b/src/core/cpu.zig
@@ -242,6 +242,7 @@ pub const Arm7tdmi = struct {
     const Self = @This();
 
     r: [16]u32,
+    pipe: Pipeline,
     sched: *Scheduler,
     bus: *Bus,
     cpsr: PSR,
@@ -262,6 +263,7 @@ pub const Arm7tdmi = struct {
     pub fn init(sched: *Scheduler, bus: *Bus, log_file: ?std.fs.File) Self {
         return Self{
             .r = [_]u32{0x00} ** 16,
+            .pipe = Pipeline.init(),
             .sched = sched,
             .bus = bus,
             .cpsr = .{ .raw = 0x0000_001F },
@@ -410,28 +412,42 @@ pub const Arm7tdmi = struct {
         self.cpsr.mode.write(@enumToInt(next));
     }
 
+    /// Advances state so that the BIOS is skipped
+    ///
+    /// Note: This accesses the CPU's bus ptr so it only may be called
+    /// once the Bus has been properly initialized
+    ///
+    /// TODO: Make above notice impossible to do in code
     pub fn fastBoot(self: *Self) void {
         self.r = std.mem.zeroes([16]u32);
 
-        self.r[0] = 0x08000000;
-        self.r[1] = 0x000000EA;
+        // self.r[0] = 0x08000000;
+        // self.r[1] = 0x000000EA;
         self.r[13] = 0x0300_7F00;
         self.r[15] = 0x0800_0000;
 
         self.banked_r[bankedIdx(.Irq, .R13)] = 0x0300_7FA0;
         self.banked_r[bankedIdx(.Supervisor, .R13)] = 0x0300_7FE0;
 
-        self.cpsr.raw = 0x6000001F;
+        // self.cpsr.raw = 0x6000001F;
+        self.cpsr.raw = 0x0000_001F;
+
+        self.bus.bios.addr_latch = 0x0000_00DC + 8;
     }
 
     pub fn step(self: *Self) void {
+        defer {
+            if (!self.pipe.flushed) self.r[15] += if (self.cpsr.t.read()) 2 else @as(u32, 4);
+            self.pipe.flushed = false;
+        }
+
         if (self.cpsr.t.read()) {
-            const opcode = self.fetch(u16);
+            const opcode = @truncate(u16, self.pipe.step(self, u16) orelse return);
             if (self.logger) |*trace| trace.mgbaLog(self, opcode);
 
             thumb.lut[thumb.idx(opcode)](self, self.bus, opcode);
         } else {
-            const opcode = self.fetch(u32);
+            const opcode = self.pipe.step(self, u32) orelse return;
             if (self.logger) |*trace| trace.mgbaLog(self, opcode);
 
             if (checkCond(self.cpsr, @truncate(u4, opcode >> 28))) {
@@ -472,42 +488,41 @@ pub const Arm7tdmi = struct {
     pub fn handleInterrupt(self: *Self) void {
         const should_handle = self.bus.io.ie.raw & self.bus.io.irq.raw;
 
-        if (should_handle != 0) {
-            self.bus.io.haltcnt = .Execute;
-            // log.debug("An Interrupt was Fired!", .{});
+        // Return if IME is disabled, CPSR I is set or there is nothing to handle
+        if (!self.bus.io.ime or self.cpsr.i.read() or should_handle == 0) return;
 
-            // Either IME is not true or I in CPSR is true
-            // Don't handle interrupts
-            if (!self.bus.io.ime or self.cpsr.i.read()) return;
-            // log.debug("An interrupt was Handled!", .{});
+        // If Pipeline isn't full, we have a bug
+        std.debug.assert(self.pipe.isFull());
 
-            // retAddr.gba says r15 on it's own is off by -04h in both ARM and THUMB mode
-            const r15 = self.r[15] + 4;
-            const cpsr = self.cpsr.raw;
+        // log.debug("Handling Interrupt!", .{});
+        self.bus.io.haltcnt = .Execute;
 
-            self.changeMode(.Irq);
-            self.cpsr.t.write(false);
-            self.cpsr.i.write(true);
+        // FIXME: This seems weird, but retAddr.gba suggests I need to make these changes
+        const ret_addr = self.r[15] - if (self.cpsr.t.read()) 0 else @as(u32, 4);
+        const new_spsr = self.cpsr.raw;
 
-            self.r[14] = r15;
-            self.spsr.raw = cpsr;
-            self.r[15] = 0x000_0018;
-        }
+        self.changeMode(.Irq);
+        self.cpsr.t.write(false);
+        self.cpsr.i.write(true);
+
+        self.r[14] = ret_addr;
+        self.spsr.raw = new_spsr;
+        self.r[15] = 0x0000_0018;
+        self.pipe.reload(self);
     }
 
-    inline fn fetch(self: *Self, comptime T: type) T {
+    inline fn fetch(self: *Self, comptime T: type, address: u32) T {
         comptime std.debug.assert(T == u32 or T == u16); // Opcode may be 32-bit (ARM) or 16-bit (THUMB)
-        defer self.r[15] += if (T == u32) 4 else 2;
 
-        // FIXME: You better hope this is optimized out
+        // Bus.read will advance the scheduler. There are different timings for CPU fetches,
+        // so we want to undo what Bus.read will apply. We can do this by caching the current tick
+        // This is very dumb.
+        //
+        // FIXME: Please rework this
         const tick_cache = self.sched.tick;
-        defer self.sched.tick = tick_cache + Bus.fetch_timings[@boolToInt(T == u32)][@truncate(u4, self.r[15] >> 24)];
+        defer self.sched.tick = tick_cache + Bus.fetch_timings[@boolToInt(T == u32)][@truncate(u4, address >> 24)];
 
-        return self.bus.read(T, self.r[15]);
-    }
-
-    pub fn fakePC(self: *const Self) u32 {
-        return self.r[15] + 4;
+        return self.bus.read(T, address);
     }
 
     pub fn panic(self: *const Self, comptime format: []const u8, args: anytype) noreturn {
@@ -524,6 +539,8 @@ pub const Arm7tdmi = struct {
         std.debug.print("spsr: 0x{X:0>8} ", .{self.spsr.raw});
         prettyPrintPsr(&self.spsr);
 
+        std.debug.print("pipeline: {??X:0>8}\n", .{self.pipe.stage});
+
         if (self.cpsr.t.read()) {
             const opcode = self.bus.dbgRead(u16, self.r[15] - 4);
             const id = thumb.idx(opcode);
@@ -587,7 +604,7 @@ pub const Arm7tdmi = struct {
         const r12 = self.r[12];
         const r13 = self.r[13];
         const r14 = self.r[14];
-        const r15 = self.r[15];
+        const r15 = self.r[15] -| if (self.cpsr.t.read()) 2 else @as(u32, 4);
 
         const c_psr = self.cpsr.raw;
 
@@ -595,7 +612,7 @@ pub const Arm7tdmi = struct {
         if (self.cpsr.t.read()) {
             if (opcode >> 11 == 0x1E) {
                 // Instruction 1 of a BL Opcode, print in ARM mode
-                const other_half = self.bus.dbgRead(u16, self.r[15]);
+                const other_half = self.bus.debugRead(u16, self.r[15] - 2);
                 const bl_opcode = @as(u32, opcode) << 16 | other_half;
 
                 log_str = try std.fmt.bufPrint(&buf, arm_fmt, .{ r0, r1, r2, r3, r4, r5, r6, r7, r8, r9, r10, r11, r12, r13, r14, r15, c_psr, bl_opcode });
@@ -631,6 +648,49 @@ pub fn checkCond(cpsr: PSR, cond: u4) bool {
     };
 }
 
+const Pipeline = struct {
+    const Self = @This();
+    stage: [2]?u32,
+    flushed: bool,
+
+    fn init() Self {
+        return .{
+            .stage = [_]?u32{null} ** 2,
+            .flushed = false,
+        };
+    }
+
+    pub fn isFull(self: *const Self) bool {
+        return self.stage[0] != null and self.stage[1] != null;
+    }
+
+    pub fn step(self: *Self, cpu: *Arm7tdmi, comptime T: type) ?u32 {
+        comptime std.debug.assert(T == u32 or T == u16);
+
+        // FIXME: https://github.com/ziglang/zig/issues/12642
+        var opcode = self.stage[0];
+
+        self.stage[0] = self.stage[1];
+        self.stage[1] = cpu.fetch(T, cpu.r[15]);
+
+        return opcode;
+    }
+
+    pub fn reload(self: *Self, cpu: *Arm7tdmi) void {
+        if (cpu.cpsr.t.read()) {
+            self.stage[0] = cpu.fetch(u16, cpu.r[15]);
+            self.stage[1] = cpu.fetch(u16, cpu.r[15] + 2);
+            cpu.r[15] += 4;
+        } else {
+            self.stage[0] = cpu.fetch(u32, cpu.r[15]);
+            self.stage[1] = cpu.fetch(u32, cpu.r[15] + 4);
+            cpu.r[15] += 8;
+        }
+
+        self.flushed = true;
+    }
+};
+
 pub const PSR = extern union {
     mode: Bitfield(u32, 0, 5),
     t: Bit(u32, 5),
diff --git a/src/core/cpu/arm/block_data_transfer.zig b/src/core/cpu/arm/block_data_transfer.zig
index 637cc0a..f94cbf9 100644
--- a/src/core/cpu/arm/block_data_transfer.zig
+++ b/src/core/cpu/arm/block_data_transfer.zig
@@ -55,8 +55,10 @@ pub fn blockDataTransfer(comptime P: bool, comptime U: bool, comptime S: bool, c
 
                 if (L) {
                     cpu.r[15] = bus.read(u32, und_addr);
+                    cpu.pipe.reload(cpu);
                 } else {
-                    bus.write(u32, und_addr, cpu.r[15] + 8);
+                    // FIXME: Should r15 on write be +12 ahead?
+                    bus.write(u32, und_addr, cpu.r[15] + 4);
                 }
 
                 cpu.r[rn] = if (U) cpu.r[rn] + 0x40 else cpu.r[rn] - 0x40;
@@ -86,17 +88,23 @@ pub fn blockDataTransfer(comptime P: bool, comptime U: bool, comptime S: bool, c
                     cpu.setUserModeRegister(i, bus.read(u32, address));
                 } else {
                     const value = bus.read(u32, address);
-                    cpu.r[i] = if (i == 0xF) value & 0xFFFF_FFFC else value;
-                    if (S and i == 0xF) cpu.setCpsr(cpu.spsr.raw);
+
+                    cpu.r[i] = value;
+                    if (i == 0xF) {
+                        cpu.r[i] &= ~@as(u32, 3); // Align r15
+                        cpu.pipe.reload(cpu);
+
+                        if (S) cpu.setCpsr(cpu.spsr.raw);
+                    }
                 }
             } else {
                 if (S) {
                     // Always Transfer User mode Registers
                     // This happens regardless if r15 is in the list
                     const value = cpu.getUserModeRegister(i);
-                    bus.write(u32, address, value + if (i == 0xF) 8 else @as(u32, 0)); // PC is already 4 ahead to make 12
+                    bus.write(u32, address, value + if (i == 0xF) 4 else @as(u32, 0)); // PC is already 8 ahead to make 12
                 } else {
-                    bus.write(u32, address, cpu.r[i] + if (i == 0xF) 8 else @as(u32, 0));
+                    bus.write(u32, address, cpu.r[i] + if (i == 0xF) 4 else @as(u32, 0));
                 }
             }
         }
diff --git a/src/core/cpu/arm/branch.zig b/src/core/cpu/arm/branch.zig
index 6015eb3..c4f9594 100644
--- a/src/core/cpu/arm/branch.zig
+++ b/src/core/cpu/arm/branch.zig
@@ -9,14 +9,20 @@ const sext = @import("../../../util.zig").sext;
 pub fn branch(comptime L: bool) InstrFn {
     return struct {
         fn inner(cpu: *Arm7tdmi, _: *Bus, opcode: u32) void {
-            if (L) cpu.r[14] = cpu.r[15];
-            cpu.r[15] = cpu.fakePC() +% (sext(u32, u24, opcode) << 2);
+            if (L) cpu.r[14] = cpu.r[15] - 4;
+
+            cpu.r[15] +%= sext(u32, u24, opcode) << 2;
+            cpu.pipe.reload(cpu);
         }
     }.inner;
 }
 
 pub fn branchAndExchange(cpu: *Arm7tdmi, _: *Bus, opcode: u32) void {
     const rn = opcode & 0xF;
-    cpu.cpsr.t.write(cpu.r[rn] & 1 == 1);
-    cpu.r[15] = cpu.r[rn] & 0xFFFF_FFFE;
+
+    const thumb = cpu.r[rn] & 1 == 1;
+    cpu.r[15] = cpu.r[rn] & if (thumb) ~@as(u32, 1) else ~@as(u32, 3);
+
+    cpu.cpsr.t.write(thumb);
+    cpu.pipe.reload(cpu);
 }
diff --git a/src/core/cpu/arm/data_processing.zig b/src/core/cpu/arm/data_processing.zig
index 2a3b77d..7bc3f44 100644
--- a/src/core/cpu/arm/data_processing.zig
+++ b/src/core/cpu/arm/data_processing.zig
@@ -2,10 +2,10 @@ const Bus = @import("../../Bus.zig");
 const Arm7tdmi = @import("../../cpu.zig").Arm7tdmi;
 const InstrFn = @import("../../cpu.zig").arm.InstrFn;
 
-const rotateRight = @import("../barrel_shifter.zig").rotateRight;
-const execute = @import("../barrel_shifter.zig").execute;
+const exec = @import("../barrel_shifter.zig").exec;
+const ror = @import("../barrel_shifter.zig").ror;
 
-pub fn dataProcessing(comptime I: bool, comptime S: bool, comptime instrKind: u4) InstrFn {
+pub fn dataProcessing(comptime I: bool, comptime S: bool, comptime kind: u4) InstrFn {
     return struct {
         fn inner(cpu: *Arm7tdmi, _: *Bus, opcode: u32) void {
             const rd = @truncate(u4, opcode >> 12 & 0xF);
@@ -13,269 +13,168 @@ pub fn dataProcessing(comptime I: bool, comptime S: bool, comptime instrKind: u4
             const old_carry = @boolToInt(cpu.cpsr.c.read());
 
             // If certain conditions are met, PC is 12 ahead instead of 8
+            // TODO: Why these conditions?
             if (!I and opcode >> 4 & 1 == 1) cpu.r[15] += 4;
+            const op1 = cpu.r[rn];
 
-            const op1 = if (rn == 0xF) cpu.fakePC() else cpu.r[rn];
-
-            var op2: u32 = undefined;
-            if (I) {
-                const amount = @truncate(u8, (opcode >> 8 & 0xF) << 1);
-                op2 = rotateRight(S, &cpu.cpsr, opcode & 0xFF, amount);
-            } else {
-                op2 = execute(S, cpu, opcode);
-            }
+            const amount = @truncate(u8, (opcode >> 8 & 0xF) << 1);
+            const op2 = if (I) ror(S, &cpu.cpsr, opcode & 0xFF, amount) else exec(S, cpu, opcode);
 
             // Undo special condition from above
             if (!I and opcode >> 4 & 1 == 1) cpu.r[15] -= 4;
 
-            switch (instrKind) {
-                0x0 => {
-                    // AND
-                    const result = op1 & op2;
-                    cpu.r[rd] = result;
-                    setArmLogicOpFlags(S, cpu, rd, result);
-                },
-                0x1 => {
-                    // EOR
-                    const result = op1 ^ op2;
-                    cpu.r[rd] = result;
-                    setArmLogicOpFlags(S, cpu, rd, result);
-                },
-                0x2 => {
-                    // SUB
-                    cpu.r[rd] = armSub(S, cpu, rd, op1, op2);
-                },
-                0x3 => {
-                    // RSB
-                    cpu.r[rd] = armSub(S, cpu, rd, op2, op1);
-                },
-                0x4 => {
-                    // ADD
-                    cpu.r[rd] = armAdd(S, cpu, rd, op1, op2);
-                },
-                0x5 => {
-                    // ADC
-                    cpu.r[rd] = armAdc(S, cpu, rd, op1, op2, old_carry);
-                },
-                0x6 => {
-                    // SBC
-                    cpu.r[rd] = armSbc(S, cpu, rd, op1, op2, old_carry);
-                },
-                0x7 => {
-                    // RSC
-                    cpu.r[rd] = armSbc(S, cpu, rd, op2, op1, old_carry);
-                },
+            var result: u32 = undefined;
+            var overflow: bool = undefined;
+
+            // Perform Data Processing Logic
+            switch (kind) {
+                0x0 => result = op1 & op2, // AND
+                0x1 => result = op1 ^ op2, // EOR
+                0x2 => result = op1 -% op2, // SUB
+                0x3 => result = op2 -% op1, // RSB
+                0x4 => result = add(&overflow, op1, op2), // ADD
+                0x5 => result = adc(&overflow, op1, op2, old_carry), // ADC
+                0x6 => result = sbc(op1, op2, old_carry), // SBC
+                0x7 => result = sbc(op2, op1, old_carry), // RSC
                 0x8 => {
                     // TST
-                    if (rd == 0xF) {
-                        undefinedTestBehaviour(cpu);
-                        return;
-                    }
+                    if (rd == 0xF)
+                        return undefinedTestBehaviour(cpu);
 
-                    const result = op1 & op2;
-                    setTestOpFlags(S, cpu, opcode, result);
+                    result = op1 & op2;
                 },
                 0x9 => {
                     // TEQ
-                    if (rd == 0xF) {
-                        undefinedTestBehaviour(cpu);
-                        return;
-                    }
+                    if (rd == 0xF)
+                        return undefinedTestBehaviour(cpu);
 
-                    const result = op1 ^ op2;
-                    setTestOpFlags(S, cpu, opcode, result);
+                    result = op1 ^ op2;
                 },
                 0xA => {
                     // CMP
-                    if (rd == 0xF) {
-                        undefinedTestBehaviour(cpu);
-                        return;
-                    }
+                    if (rd == 0xF)
+                        return undefinedTestBehaviour(cpu);
 
-                    cmp(cpu, op1, op2);
+                    result = op1 -% op2;
                 },
                 0xB => {
                     // CMN
-                    if (rd == 0xF) {
-                        undefinedTestBehaviour(cpu);
-                        return;
-                    }
+                    if (rd == 0xF)
+                        return undefinedTestBehaviour(cpu);
 
-                    cmn(cpu, op1, op2);
+                    overflow = @addWithOverflow(u32, op1, op2, &result);
                 },
-                0xC => {
-                    // ORR
-                    const result = op1 | op2;
+                0xC => result = op1 | op2, // ORR
+                0xD => result = op2, // MOV
+                0xE => result = op1 & ~op2, // BIC
+                0xF => result = ~op2, // MVN
+            }
+
+            // Write to Destination Register
+            switch (kind) {
+                0x8, 0x9, 0xA, 0xB => {}, // Test Operations
+                else => {
                     cpu.r[rd] = result;
-                    setArmLogicOpFlags(S, cpu, rd, result);
+                    if (rd == 0xF) {
+                        if (S) cpu.setCpsr(cpu.spsr.raw);
+                        cpu.pipe.reload(cpu);
+                    }
                 },
-                0xD => {
-                    // MOV
-                    cpu.r[rd] = op2;
-                    setArmLogicOpFlags(S, cpu, rd, op2);
+            }
+
+            // Write Flags
+            switch (kind) {
+                0x0, 0x1, 0xC, 0xD, 0xE, 0xF => if (S and rd != 0xF) {
+                    // Logic Operation Flags
+                    cpu.cpsr.n.write(result >> 31 & 1 == 1);
+                    cpu.cpsr.z.write(result == 0);
+                    // C set by Barrel Shifter, V is unaffected
+
                 },
-                0xE => {
-                    // BIC
-                    const result = op1 & ~op2;
-                    cpu.r[rd] = result;
-                    setArmLogicOpFlags(S, cpu, rd, result);
+                0x2, 0x3 => if (S and rd != 0xF) {
+                    // SUB, RSB Flags
+                    cpu.cpsr.n.write(result >> 31 & 1 == 1);
+                    cpu.cpsr.z.write(result == 0);
+
+                    if (kind == 0x2) {
+                        // SUB specific
+                        cpu.cpsr.c.write(op2 <= op1);
+                        cpu.cpsr.v.write(((op1 ^ result) & (~op2 ^ result)) >> 31 & 1 == 1);
+                    } else {
+                        // RSB Specific
+                        cpu.cpsr.c.write(op1 <= op2);
+                        cpu.cpsr.v.write(((op2 ^ result) & (~op1 ^ result)) >> 31 & 1 == 1);
+                    }
                 },
-                0xF => {
-                    // MVN
-                    const result = ~op2;
-                    cpu.r[rd] = result;
-                    setArmLogicOpFlags(S, cpu, rd, result);
+                0x4, 0x5 => if (S and rd != 0xF) {
+                    // ADD, ADC Flags
+                    cpu.cpsr.n.write(result >> 31 & 1 == 1);
+                    cpu.cpsr.z.write(result == 0);
+                    cpu.cpsr.c.write(overflow);
+                    cpu.cpsr.v.write(((op1 ^ result) & (op2 ^ result)) >> 31 & 1 == 1);
+                },
+                0x6, 0x7 => if (S and rd != 0xF) {
+                    // SBC, RSC Flags
+                    cpu.cpsr.n.write(result >> 31 & 1 == 1);
+                    cpu.cpsr.z.write(result == 0);
+
+                    if (kind == 0x6) {
+                        // SBC specific
+                        const subtrahend = @as(u64, op2) -% old_carry +% 1;
+                        cpu.cpsr.c.write(subtrahend <= op1);
+                        cpu.cpsr.v.write(((op1 ^ result) & (~op2 ^ result)) >> 31 & 1 == 1);
+                    } else {
+                        // RSC Specific
+                        const subtrahend = @as(u64, op1) -% old_carry +% 1;
+                        cpu.cpsr.c.write(subtrahend <= op2);
+                        cpu.cpsr.v.write(((op2 ^ result) & (~op1 ^ result)) >> 31 & 1 == 1);
+                    }
+                },
+                0x8, 0x9, 0xA, 0xB => {
+                    // Test Operation Flags
+                    cpu.cpsr.n.write(result >> 31 & 1 == 1);
+                    cpu.cpsr.z.write(result == 0);
+
+                    if (kind == 0xA) {
+                        // CMP specific
+                        cpu.cpsr.c.write(op2 <= op1);
+                        cpu.cpsr.v.write(((op1 ^ result) & (~op2 ^ result)) >> 31 & 1 == 1);
+                    } else if (kind == 0xB) {
+                        // CMN specific
+                        cpu.cpsr.c.write(overflow);
+                        cpu.cpsr.v.write(((op1 ^ result) & (op2 ^ result)) >> 31 & 1 == 1);
+                    } else {
+                        // TST, TEQ specific
+                        // Barrel Shifter should always calc CPSR C in TST
+                        if (!S) _ = exec(true, cpu, opcode);
+                    }
                 },
             }
         }
     }.inner;
 }
 
-fn armSbc(comptime S: bool, cpu: *Arm7tdmi, rd: u4, left: u32, right: u32, old_carry: u1) u32 {
-    var result: u32 = undefined;
-    if (S and rd == 0xF) {
-        result = sbc(false, cpu, left, right, old_carry);
-        cpu.setCpsr(cpu.spsr.raw);
-    } else {
-        result = sbc(S, cpu, left, right, old_carry);
-    }
-
-    return result;
-}
-
-pub fn sbc(comptime S: bool, cpu: *Arm7tdmi, left: u32, right: u32, old_carry: u1) u32 {
+pub fn sbc(left: u32, right: u32, old_carry: u1) u32 {
     // TODO: Make your own version (thanks peach.bot)
     const subtrahend = @as(u64, right) -% old_carry +% 1;
-    const result = @truncate(u32, left -% subtrahend);
+    const ret = @truncate(u32, left -% subtrahend);
 
-    if (S) {
-        cpu.cpsr.n.write(result >> 31 & 1 == 1);
-        cpu.cpsr.z.write(result == 0);
-        cpu.cpsr.c.write(subtrahend <= left);
-        cpu.cpsr.v.write(((left ^ result) & (~right ^ result)) >> 31 & 1 == 1);
-    }
-
-    return result;
+    return ret;
 }
 
-fn armSub(comptime S: bool, cpu: *Arm7tdmi, rd: u4, left: u32, right: u32) u32 {
-    var result: u32 = undefined;
-    if (S and rd == 0xF) {
-        result = sub(false, cpu, left, right);
-        cpu.setCpsr(cpu.spsr.raw);
-    } else {
-        result = sub(S, cpu, left, right);
-    }
-
-    return result;
+pub fn add(overflow: *bool, left: u32, right: u32) u32 {
+    var ret: u32 = undefined;
+    overflow.* = @addWithOverflow(u32, left, right, &ret);
+    return ret;
 }
 
-pub fn sub(comptime S: bool, cpu: *Arm7tdmi, left: u32, right: u32) u32 {
-    const result = left -% right;
+pub fn adc(overflow: *bool, left: u32, right: u32, old_carry: u1) u32 {
+    var ret: u32 = undefined;
+    const first = @addWithOverflow(u32, left, right, &ret);
+    const second = @addWithOverflow(u32, ret, old_carry, &ret);
 
-    if (S) {
-        cpu.cpsr.n.write(result >> 31 & 1 == 1);
-        cpu.cpsr.z.write(result == 0);
-        cpu.cpsr.c.write(right <= left);
-        cpu.cpsr.v.write(((left ^ result) & (~right ^ result)) >> 31 & 1 == 1);
-    }
-
-    return result;
-}
-
-fn armAdd(comptime S: bool, cpu: *Arm7tdmi, rd: u4, left: u32, right: u32) u32 {
-    var result: u32 = undefined;
-    if (S and rd == 0xF) {
-        result = add(false, cpu, left, right);
-        cpu.setCpsr(cpu.spsr.raw);
-    } else {
-        result = add(S, cpu, left, right);
-    }
-
-    return result;
-}
-
-pub fn add(comptime S: bool, cpu: *Arm7tdmi, left: u32, right: u32) u32 {
-    var result: u32 = undefined;
-    const didOverflow = @addWithOverflow(u32, left, right, &result);
-
-    if (S) {
-        cpu.cpsr.n.write(result >> 31 & 1 == 1);
-        cpu.cpsr.z.write(result == 0);
-        cpu.cpsr.c.write(didOverflow);
-        cpu.cpsr.v.write(((left ^ result) & (right ^ result)) >> 31 & 1 == 1);
-    }
-
-    return result;
-}
-
-fn armAdc(comptime S: bool, cpu: *Arm7tdmi, rd: u4, left: u32, right: u32, old_carry: u1) u32 {
-    var result: u32 = undefined;
-    if (S and rd == 0xF) {
-        result = adc(false, cpu, left, right, old_carry);
-        cpu.setCpsr(cpu.spsr.raw);
-    } else {
-        result = adc(S, cpu, left, right, old_carry);
-    }
-
-    return result;
-}
-
-pub fn adc(comptime S: bool, cpu: *Arm7tdmi, left: u32, right: u32, old_carry: u1) u32 {
-    var result: u32 = undefined;
-    const did = @addWithOverflow(u32, left, right, &result);
-    const overflow = @addWithOverflow(u32, result, old_carry, &result);
-
-    if (S) {
-        cpu.cpsr.n.write(result >> 31 & 1 == 1);
-        cpu.cpsr.z.write(result == 0);
-        cpu.cpsr.c.write(did or overflow);
-        cpu.cpsr.v.write(((left ^ result) & (right ^ result)) >> 31 & 1 == 1);
-    }
-
-    return result;
-}
-
-pub fn cmp(cpu: *Arm7tdmi, left: u32, right: u32) void {
-    const result = left -% right;
-
-    cpu.cpsr.n.write(result >> 31 & 1 == 1);
-    cpu.cpsr.z.write(result == 0);
-    cpu.cpsr.c.write(right <= left);
-    cpu.cpsr.v.write(((left ^ result) & (~right ^ result)) >> 31 & 1 == 1);
-}
-
-pub fn cmn(cpu: *Arm7tdmi, left: u32, right: u32) void {
-    var result: u32 = undefined;
-    const didOverflow = @addWithOverflow(u32, left, right, &result);
-
-    cpu.cpsr.n.write(result >> 31 & 1 == 1);
-    cpu.cpsr.z.write(result == 0);
-    cpu.cpsr.c.write(didOverflow);
-    cpu.cpsr.v.write(((left ^ result) & (right ^ result)) >> 31 & 1 == 1);
-}
-
-fn setArmLogicOpFlags(comptime S: bool, cpu: *Arm7tdmi, rd: u4, result: u32) void {
-    if (S and rd == 0xF) {
-        cpu.setCpsr(cpu.spsr.raw);
-    } else {
-        setLogicOpFlags(S, cpu, result);
-    }
-}
-
-pub fn setLogicOpFlags(comptime S: bool, cpu: *Arm7tdmi, result: u32) void {
-    if (S) {
-        cpu.cpsr.n.write(result >> 31 & 1 == 1);
-        cpu.cpsr.z.write(result == 0);
-        // C set by Barrel Shifter, V is unaffected
-    }
-}
-
-fn setTestOpFlags(comptime S: bool, cpu: *Arm7tdmi, opcode: u32, result: u32) void {
-    cpu.cpsr.n.write(result >> 31 & 1 == 1);
-    cpu.cpsr.z.write(result == 0);
-    // Barrel Shifter should always calc CPSR C in TST
-    if (!S) _ = execute(true, cpu, opcode);
+    overflow.* = first or second;
+    return ret;
 }
 
 fn undefinedTestBehaviour(cpu: *Arm7tdmi) void {
diff --git a/src/core/cpu/arm/half_signed_data_transfer.zig b/src/core/cpu/arm/half_signed_data_transfer.zig
index d681da0..2b12ec4 100644
--- a/src/core/cpu/arm/half_signed_data_transfer.zig
+++ b/src/core/cpu/arm/half_signed_data_transfer.zig
@@ -15,20 +15,8 @@ pub fn halfAndSignedDataTransfer(comptime P: bool, comptime U: bool, comptime I:
             const rm = opcode & 0xF;
             const imm_offset_high = opcode >> 8 & 0xF;
 
-            var base: u32 = undefined;
-            if (rn == 0xF) {
-                base = cpu.fakePC();
-                if (!L) base += 4;
-            } else {
-                base = cpu.r[rn];
-            }
-
-            var offset: u32 = undefined;
-            if (I) {
-                offset = imm_offset_high << 4 | rm;
-            } else {
-                offset = cpu.r[rm];
-            }
+            const base = cpu.r[rn] + if (!L and rn == 0xF) 4 else @as(u32, 0);
+            const offset = if (I) imm_offset_high << 4 | rm else cpu.r[rm];
 
             const modified_base = if (U) base +% offset else base -% offset;
             var address = if (P) modified_base else base;
diff --git a/src/core/cpu/arm/single_data_transfer.zig b/src/core/cpu/arm/single_data_transfer.zig
index 3a82b0c..42bd217 100644
--- a/src/core/cpu/arm/single_data_transfer.zig
+++ b/src/core/cpu/arm/single_data_transfer.zig
@@ -14,15 +14,10 @@ pub fn singleDataTransfer(comptime I: bool, comptime P: bool, comptime U: bool,
             const rn = opcode >> 16 & 0xF;
             const rd = opcode >> 12 & 0xF;
 
-            var base: u32 = undefined;
-            if (rn == 0xF) {
-                base = cpu.fakePC();
-                if (!L) base += 4; // Offset of 12
-            } else {
-                base = cpu.r[rn];
-            }
+            // rn is r15 and L is not set, the PC is 12 ahead
+            const base = cpu.r[rn] + if (!L and rn == 0xF) 4 else @as(u32, 0);
 
-            const offset = if (I) shifter.immShift(false, cpu, opcode) else opcode & 0xFFF;
+            const offset = if (I) shifter.immediate(false, cpu, opcode) else opcode & 0xFFF;
 
             const modified_base = if (U) base +% offset else base -% offset;
             var address = if (P) modified_base else base;
@@ -40,18 +35,26 @@ pub fn singleDataTransfer(comptime I: bool, comptime P: bool, comptime U: bool,
             } else {
                 if (B) {
                     // STRB
-                    const value = if (rd == 0xF) cpu.r[rd] + 8 else cpu.r[rd];
+                    const value = cpu.r[rd] + if (rd == 0xF) 4 else @as(u32, 0); // PC is 12 ahead
                     bus.write(u8, address, @truncate(u8, value));
                 } else {
                     // STR
-                    const value = if (rd == 0xF) cpu.r[rd] + 8 else cpu.r[rd];
+                    const value = cpu.r[rd] + if (rd == 0xF) 4 else @as(u32, 0);
                     bus.write(u32, address, value);
                 }
             }
 
             address = modified_base;
-            if (W and P or !P) cpu.r[rn] = address;
-            if (L) cpu.r[rd] = result; // This emulates the LDR rd == rn behaviour
+            if (W and P or !P) {
+                cpu.r[rn] = address;
+                if (rn == 0xF) cpu.pipe.reload(cpu);
+            }
+
+            if (L) {
+                // This emulates the LDR rd == rn behaviour
+                cpu.r[rd] = result;
+                if (rd == 0xF) cpu.pipe.reload(cpu);
+            }
         }
     }.inner;
 }
diff --git a/src/core/cpu/arm/software_interrupt.zig b/src/core/cpu/arm/software_interrupt.zig
index 849a6eb..b44cf99 100644
--- a/src/core/cpu/arm/software_interrupt.zig
+++ b/src/core/cpu/arm/software_interrupt.zig
@@ -6,7 +6,7 @@ pub fn armSoftwareInterrupt() InstrFn {
     return struct {
         fn inner(cpu: *Arm7tdmi, _: *Bus, _: u32) void {
             // Copy Values from Current Mode
-            const r15 = cpu.r[15];
+            const ret_addr = cpu.r[15] - 4;
             const cpsr = cpu.cpsr.raw;
 
             // Switch Mode
@@ -14,9 +14,10 @@ pub fn armSoftwareInterrupt() InstrFn {
             cpu.cpsr.t.write(false); // Force ARM Mode
             cpu.cpsr.i.write(true); // Disable normal interrupts
 
-            cpu.r[14] = r15; // Resume Execution
+            cpu.r[14] = ret_addr; // Resume Execution
             cpu.spsr.raw = cpsr; // Previous mode CPSR
             cpu.r[15] = 0x0000_0008;
+            cpu.pipe.reload(cpu);
         }
     }.inner;
 }
diff --git a/src/core/cpu/barrel_shifter.zig b/src/core/cpu/barrel_shifter.zig
index 32afff0..14578b4 100644
--- a/src/core/cpu/barrel_shifter.zig
+++ b/src/core/cpu/barrel_shifter.zig
@@ -5,37 +5,33 @@ const CPSR = @import("../cpu.zig").PSR;
 
 const rotr = @import("../../util.zig").rotr;
 
-pub fn execute(comptime S: bool, cpu: *Arm7tdmi, opcode: u32) u32 {
+pub fn exec(comptime S: bool, cpu: *Arm7tdmi, opcode: u32) u32 {
     var result: u32 = undefined;
     if (opcode >> 4 & 1 == 1) {
-        result = registerShift(S, cpu, opcode);
+        result = register(S, cpu, opcode);
     } else {
-        result = immShift(S, cpu, opcode);
+        result = immediate(S, cpu, opcode);
     }
 
     return result;
 }
 
-fn registerShift(comptime S: bool, cpu: *Arm7tdmi, opcode: u32) u32 {
+fn register(comptime S: bool, cpu: *Arm7tdmi, opcode: u32) u32 {
     const rs_idx = opcode >> 8 & 0xF;
+    const rm = cpu.r[opcode & 0xF];
     const rs = @truncate(u8, cpu.r[rs_idx]);
 
-    const rm_idx = opcode & 0xF;
-    const rm = if (rm_idx == 0xF) cpu.fakePC() else cpu.r[rm_idx];
-
     return switch (@truncate(u2, opcode >> 5)) {
-        0b00 => logicalLeft(S, &cpu.cpsr, rm, rs),
-        0b01 => logicalRight(S, &cpu.cpsr, rm, rs),
-        0b10 => arithmeticRight(S, &cpu.cpsr, rm, rs),
-        0b11 => rotateRight(S, &cpu.cpsr, rm, rs),
+        0b00 => lsl(S, &cpu.cpsr, rm, rs),
+        0b01 => lsr(S, &cpu.cpsr, rm, rs),
+        0b10 => asr(S, &cpu.cpsr, rm, rs),
+        0b11 => ror(S, &cpu.cpsr, rm, rs),
     };
 }
 
-pub fn immShift(comptime S: bool, cpu: *Arm7tdmi, opcode: u32) u32 {
+pub fn immediate(comptime S: bool, cpu: *Arm7tdmi, opcode: u32) u32 {
     const amount = @truncate(u8, opcode >> 7 & 0x1F);
-
-    const rm_idx = opcode & 0xF;
-    const rm = if (rm_idx == 0xF) cpu.fakePC() else cpu.r[rm_idx];
+    const rm = cpu.r[opcode & 0xF];
 
     var result: u32 = undefined;
     if (amount == 0) {
@@ -64,17 +60,17 @@ pub fn immShift(comptime S: bool, cpu: *Arm7tdmi, opcode: u32) u32 {
         }
     } else {
         switch (@truncate(u2, opcode >> 5)) {
-            0b00 => result = logicalLeft(S, &cpu.cpsr, rm, amount),
-            0b01 => result = logicalRight(S, &cpu.cpsr, rm, amount),
-            0b10 => result = arithmeticRight(S, &cpu.cpsr, rm, amount),
-            0b11 => result = rotateRight(S, &cpu.cpsr, rm, amount),
+            0b00 => result = lsl(S, &cpu.cpsr, rm, amount),
+            0b01 => result = lsr(S, &cpu.cpsr, rm, amount),
+            0b10 => result = asr(S, &cpu.cpsr, rm, amount),
+            0b11 => result = ror(S, &cpu.cpsr, rm, amount),
         }
     }
 
     return result;
 }
 
-pub fn logicalLeft(comptime S: bool, cpsr: *CPSR, rm: u32, total_amount: u8) u32 {
+pub fn lsl(comptime S: bool, cpsr: *CPSR, rm: u32, total_amount: u8) u32 {
     const amount = @truncate(u5, total_amount);
     const bit_count: u8 = @typeInfo(u32).Int.bits;
 
@@ -101,7 +97,7 @@ pub fn logicalLeft(comptime S: bool, cpsr: *CPSR, rm: u32, total_amount: u8) u32
     return result;
 }
 
-pub fn logicalRight(comptime S: bool, cpsr: *CPSR, rm: u32, total_amount: u32) u32 {
+pub fn lsr(comptime S: bool, cpsr: *CPSR, rm: u32, total_amount: u32) u32 {
     const amount = @truncate(u5, total_amount);
     const bit_count: u8 = @typeInfo(u32).Int.bits;
 
@@ -125,7 +121,7 @@ pub fn logicalRight(comptime S: bool, cpsr: *CPSR, rm: u32, total_amount: u32) u
     return result;
 }
 
-pub fn arithmeticRight(comptime S: bool, cpsr: *CPSR, rm: u32, total_amount: u8) u32 {
+pub fn asr(comptime S: bool, cpsr: *CPSR, rm: u32, total_amount: u8) u32 {
     const amount = @truncate(u5, total_amount);
     const bit_count: u8 = @typeInfo(u32).Int.bits;
 
@@ -142,7 +138,7 @@ pub fn arithmeticRight(comptime S: bool, cpsr: *CPSR, rm: u32, total_amount: u8)
     return result;
 }
 
-pub fn rotateRight(comptime S: bool, cpsr: *CPSR, rm: u32, total_amount: u8) u32 {
+pub fn ror(comptime S: bool, cpsr: *CPSR, rm: u32, total_amount: u8) u32 {
     const result = rotr(u32, rm, total_amount);
 
     if (S and total_amount != 0) {
diff --git a/src/core/cpu/thumb/alu.zig b/src/core/cpu/thumb/alu.zig
index 0113860..778da99 100644
--- a/src/core/cpu/thumb/alu.zig
+++ b/src/core/cpu/thumb/alu.zig
@@ -4,16 +4,11 @@ const InstrFn = @import("../../cpu.zig").thumb.InstrFn;
 
 const adc = @import("../arm/data_processing.zig").adc;
 const sbc = @import("../arm/data_processing.zig").sbc;
-const sub = @import("../arm/data_processing.zig").sub;
-const cmp = @import("../arm/data_processing.zig").cmp;
-const cmn = @import("../arm/data_processing.zig").cmn;
-const setTestOpFlags = @import("../arm/data_processing.zig").setTestOpFlags;
-const setLogicOpFlags = @import("../arm/data_processing.zig").setLogicOpFlags;
 
-const logicalLeft = @import("../barrel_shifter.zig").logicalLeft;
-const logicalRight = @import("../barrel_shifter.zig").logicalRight;
-const arithmeticRight = @import("../barrel_shifter.zig").arithmeticRight;
-const rotateRight = @import("../barrel_shifter.zig").rotateRight;
+const lsl = @import("../barrel_shifter.zig").lsl;
+const lsr = @import("../barrel_shifter.zig").lsr;
+const asr = @import("../barrel_shifter.zig").asr;
+const ror = @import("../barrel_shifter.zig").ror;
 
 pub fn fmt4(comptime op: u4) InstrFn {
     return struct {
@@ -22,96 +17,85 @@ pub fn fmt4(comptime op: u4) InstrFn {
             const rd = opcode & 0x7;
             const carry = @boolToInt(cpu.cpsr.c.read());
 
+            const op1 = cpu.r[rd];
+            const op2 = cpu.r[rs];
+
+            var result: u32 = undefined;
+            var overflow: bool = undefined;
             switch (op) {
-                0x0 => {
-                    // AND
-                    const result = cpu.r[rd] & cpu.r[rs];
-                    cpu.r[rd] = result;
-                    setLogicOpFlags(true, cpu, result);
+                0x0 => result = op1 & op2, // AND
+                0x1 => result = op1 ^ op2, // EOR
+                0x2 => result = lsl(true, &cpu.cpsr, op1, @truncate(u8, op2)), // LSL
+                0x3 => result = lsr(true, &cpu.cpsr, op1, @truncate(u8, op2)), // LSR
+                0x4 => result = asr(true, &cpu.cpsr, op1, @truncate(u8, op2)), // ASR
+                0x5 => result = adc(&overflow, op1, op2, carry), // ADC
+                0x6 => result = sbc(op1, op2, carry), // SBC
+                0x7 => result = ror(true, &cpu.cpsr, op1, @truncate(u8, op2)), // ROR
+                0x8 => result = op1 & op2, // TST
+                0x9 => result = 0 -% op2, // NEG
+                0xA => result = op1 -% op2, // CMP
+                0xB => overflow = @addWithOverflow(u32, op1, op2, &result), // CMN
+                0xC => result = op1 | op2, // ORR
+                0xD => result = @truncate(u32, @as(u64, op2) * @as(u64, op1)),
+                0xE => result = op1 & ~op2,
+                0xF => result = ~op2,
+            }
+
+            // Write to Destination Register
+            switch (op) {
+                0x8, 0xA, 0xB => {},
+                else => cpu.r[rd] = result,
+            }
+
+            // Write Flags
+            switch (op) {
+                0x0, 0x1, 0x2, 0x3, 0x4, 0x7, 0xC, 0xE, 0xF => {
+                    // Logic Operations
+                    cpu.cpsr.n.write(result >> 31 & 1 == 1);
+                    cpu.cpsr.z.write(result == 0);
+                    // C set by Barrel Shifter, V is unaffected
                 },
-                0x1 => {
-                    // EOR
-                    const result = cpu.r[rd] ^ cpu.r[rs];
-                    cpu.r[rd] = result;
-                    setLogicOpFlags(true, cpu, result);
+                0x8, 0xA => {
+                    // Test Flags
+                    // CMN (0xB) is handled with ADC
+                    cpu.cpsr.n.write(result >> 31 & 1 == 1);
+                    cpu.cpsr.z.write(result == 0);
+
+                    if (op == 0xA) {
+                        // CMP specific
+                        cpu.cpsr.c.write(op2 <= op1);
+                        cpu.cpsr.v.write(((op1 ^ result) & (~op2 ^ result)) >> 31 & 1 == 1);
+                    }
                 },
-                0x2 => {
-                    // LSL
-                    const result = logicalLeft(true, &cpu.cpsr, cpu.r[rd], @truncate(u8, cpu.r[rs]));
-                    cpu.r[rd] = result;
-                    setLogicOpFlags(true, cpu, result);
-                },
-                0x3 => {
-                    // LSR
-                    const result = logicalRight(true, &cpu.cpsr, cpu.r[rd], @truncate(u8, cpu.r[rs]));
-                    cpu.r[rd] = result;
-                    setLogicOpFlags(true, cpu, result);
-                },
-                0x4 => {
-                    // ASR
-                    const result = arithmeticRight(true, &cpu.cpsr, cpu.r[rd], @truncate(u8, cpu.r[rs]));
-                    cpu.r[rd] = result;
-                    setLogicOpFlags(true, cpu, result);
-                },
-                0x5 => {
-                    // ADC
-                    cpu.r[rd] = adc(true, cpu, cpu.r[rd], cpu.r[rs], carry);
+                0x5, 0xB => {
+                    // ADC, CMN
+                    cpu.cpsr.n.write(result >> 31 & 1 == 1);
+                    cpu.cpsr.z.write(result == 0);
+                    cpu.cpsr.c.write(overflow);
+                    cpu.cpsr.v.write(((op1 ^ result) & (op2 ^ result)) >> 31 & 1 == 1);
                 },
                 0x6 => {
                     // SBC
-                    cpu.r[rd] = sbc(true, cpu, cpu.r[rd], cpu.r[rs], carry);
-                },
-                0x7 => {
-                    // ROR
-                    const result = rotateRight(true, &cpu.cpsr, cpu.r[rd], @truncate(u8, cpu.r[rs]));
-                    cpu.r[rd] = result;
-                    setLogicOpFlags(true, cpu, result);
-                },
-                0x8 => {
-                    // TST
-                    const result = cpu.r[rd] & cpu.r[rs];
-                    setLogicOpFlags(true, cpu, result);
+                    cpu.cpsr.n.write(result >> 31 & 1 == 1);
+                    cpu.cpsr.z.write(result == 0);
+
+                    const subtrahend = @as(u64, op2) -% carry +% 1;
+                    cpu.cpsr.c.write(subtrahend <= op1);
+                    cpu.cpsr.v.write(((op1 ^ result) & (~op2 ^ result)) >> 31 & 1 == 1);
                 },
                 0x9 => {
                     // NEG
-                    cpu.r[rd] = sub(true, cpu, 0, cpu.r[rs]);
-                },
-                0xA => {
-                    // CMP
-                    cmp(cpu, cpu.r[rd], cpu.r[rs]);
-                },
-                0xB => {
-                    // CMN
-                    cmn(cpu, cpu.r[rd], cpu.r[rs]);
-                },
-                0xC => {
-                    // ORR
-                    const result = cpu.r[rd] | cpu.r[rs];
-                    cpu.r[rd] = result;
-                    setLogicOpFlags(true, cpu, result);
+                    cpu.cpsr.n.write(result >> 31 & 1 == 1);
+                    cpu.cpsr.z.write(result == 0);
+                    cpu.cpsr.c.write(op2 <= 0);
+                    cpu.cpsr.v.write(((0 ^ result) & (~op2 ^ result)) >> 31 & 1 == 1);
                 },
                 0xD => {
-                    // MUL
-                    const temp = @as(u64, cpu.r[rs]) * @as(u64, cpu.r[rd]);
-                    const result = @truncate(u32, temp);
-                    cpu.r[rd] = result;
-
+                    // Multiplication
                     cpu.cpsr.n.write(result >> 31 & 1 == 1);
                     cpu.cpsr.z.write(result == 0);
                     // V is unaffected, assuming similar behaviour to ARMv4 MUL C is undefined
                 },
-                0xE => {
-                    // BIC
-                    const result = cpu.r[rd] & ~cpu.r[rs];
-                    cpu.r[rd] = result;
-                    setLogicOpFlags(true, cpu, result);
-                },
-                0xF => {
-                    // MVN
-                    const result = ~cpu.r[rs];
-                    cpu.r[rd] = result;
-                    setLogicOpFlags(true, cpu, result);
-                },
             }
         }
     }.inner;
diff --git a/src/core/cpu/thumb/block_data_transfer.zig b/src/core/cpu/thumb/block_data_transfer.zig
index c2df7e6..014729f 100644
--- a/src/core/cpu/thumb/block_data_transfer.zig
+++ b/src/core/cpu/thumb/block_data_transfer.zig
@@ -33,7 +33,8 @@ pub fn fmt14(comptime L: bool, comptime R: bool) InstrFn {
             if (R) {
                 if (L) {
                     const value = bus.read(u32, address);
-                    cpu.r[15] = value & 0xFFFF_FFFE;
+                    cpu.r[15] = value & ~@as(u32, 1);
+                    cpu.pipe.reload(cpu);
                 } else {
                     bus.write(u32, address, cpu.r[14]);
                 }
@@ -52,7 +53,13 @@ pub fn fmt15(comptime L: bool, comptime rb: u3) InstrFn {
             const end_address = cpu.r[rb] + 4 * countRlist(opcode);
 
             if (opcode & 0xFF == 0) {
-                if (L) cpu.r[15] = bus.read(u32, address) else bus.write(u32, address, cpu.r[15] + 4);
+                if (L) {
+                    cpu.r[15] = bus.read(u32, address);
+                    cpu.pipe.reload(cpu);
+                } else {
+                    bus.write(u32, address, cpu.r[15] + 2);
+                }
+
                 cpu.r[rb] += 0x40;
                 return;
             }
diff --git a/src/core/cpu/thumb/branch.zig b/src/core/cpu/thumb/branch.zig
index 3425f3c..4d00577 100644
--- a/src/core/cpu/thumb/branch.zig
+++ b/src/core/cpu/thumb/branch.zig
@@ -9,16 +9,13 @@ pub fn fmt16(comptime cond: u4) InstrFn {
     return struct {
         fn inner(cpu: *Arm7tdmi, _: *Bus, opcode: u16) void {
             // B
-            const offset = sext(u32, u8, opcode & 0xFF) << 1;
+            if (cond == 0xE or cond == 0xF)
+                cpu.panic("[CPU/THUMB.16] Undefined conditional branch with condition {}", .{cond});
 
-            const should_execute = switch (cond) {
-                0xE, 0xF => cpu.panic("[CPU/THUMB.16] Undefined conditional branch with condition {}", .{cond}),
-                else => checkCond(cpu.cpsr, cond),
-            };
+            if (!checkCond(cpu.cpsr, cond)) return;
 
-            if (should_execute) {
-                cpu.r[15] = (cpu.r[15] + 2) +% offset;
-            }
+            cpu.r[15] +%= sext(u32, u8, opcode & 0xFF) << 1;
+            cpu.pipe.reload(cpu);
         }
     }.inner;
 }
@@ -27,8 +24,8 @@ pub fn fmt18() InstrFn {
     return struct {
         // B but conditional
         fn inner(cpu: *Arm7tdmi, _: *Bus, opcode: u16) void {
-            const offset = sext(u32, u11, opcode & 0x7FF) << 1;
-            cpu.r[15] = (cpu.r[15] + 2) +% offset;
+            cpu.r[15] +%= sext(u32, u11, opcode & 0x7FF) << 1;
+            cpu.pipe.reload(cpu);
         }
     }.inner;
 }
@@ -41,13 +38,16 @@ pub fn fmt19(comptime is_low: bool) InstrFn {
 
             if (is_low) {
                 // Instruction 2
-                const old_pc = cpu.r[15];
+                const next_opcode = cpu.r[15] - 2;
 
                 cpu.r[15] = cpu.r[14] +% (offset << 1);
-                cpu.r[14] = old_pc | 1;
+                cpu.r[14] = next_opcode | 1;
+
+                cpu.pipe.reload(cpu);
             } else {
                 // Instruction 1
-                cpu.r[14] = (cpu.r[15] + 2) +% (sext(u32, u11, offset) << 12);
+                const lr_offset = sext(u32, u11, offset) << 12;
+                cpu.r[14] = (cpu.r[15] +% lr_offset) & ~@as(u32, 1);
             }
         }
     }.inner;
diff --git a/src/core/cpu/thumb/data_processing.zig b/src/core/cpu/thumb/data_processing.zig
index d352147..6d9a324 100644
--- a/src/core/cpu/thumb/data_processing.zig
+++ b/src/core/cpu/thumb/data_processing.zig
@@ -3,14 +3,12 @@ const std = @import("std");
 const Bus = @import("../../Bus.zig");
 const Arm7tdmi = @import("../../cpu.zig").Arm7tdmi;
 const InstrFn = @import("../../cpu.zig").thumb.InstrFn;
-const shifter = @import("../barrel_shifter.zig");
 
 const add = @import("../arm/data_processing.zig").add;
-const sub = @import("../arm/data_processing.zig").sub;
-const cmp = @import("../arm/data_processing.zig").cmp;
-const setLogicOpFlags = @import("../arm/data_processing.zig").setLogicOpFlags;
 
-const log = std.log.scoped(.Thumb1);
+const lsl = @import("../barrel_shifter.zig").lsl;
+const lsr = @import("../barrel_shifter.zig").lsr;
+const asr = @import("../barrel_shifter.zig").asr;
 
 pub fn fmt1(comptime op: u2, comptime offset: u5) InstrFn {
     return struct {
@@ -24,7 +22,7 @@ pub fn fmt1(comptime op: u2, comptime offset: u5) InstrFn {
                     if (offset == 0) {
                         break :blk cpu.r[rs];
                     } else {
-                        break :blk shifter.logicalLeft(true, &cpu.cpsr, cpu.r[rs], offset);
+                        break :blk lsl(true, &cpu.cpsr, cpu.r[rs], offset);
                     }
                 },
                 0b01 => blk: {
@@ -33,7 +31,7 @@ pub fn fmt1(comptime op: u2, comptime offset: u5) InstrFn {
                         cpu.cpsr.c.write(cpu.r[rs] >> 31 & 1 == 1);
                         break :blk @as(u32, 0);
                     } else {
-                        break :blk shifter.logicalRight(true, &cpu.cpsr, cpu.r[rs], offset);
+                        break :blk lsr(true, &cpu.cpsr, cpu.r[rs], offset);
                     }
                 },
                 0b10 => blk: {
@@ -42,7 +40,7 @@ pub fn fmt1(comptime op: u2, comptime offset: u5) InstrFn {
                         cpu.cpsr.c.write(cpu.r[rs] >> 31 & 1 == 1);
                         break :blk @bitCast(u32, @bitCast(i32, cpu.r[rs]) >> 31);
                     } else {
-                        break :blk shifter.arithmeticRight(true, &cpu.cpsr, cpu.r[rs], offset);
+                        break :blk asr(true, &cpu.cpsr, cpu.r[rs], offset);
                     }
                 },
                 else => cpu.panic("[CPU/THUMB.1] 0b{b:0>2} is not a valid op", .{op}),
@@ -50,7 +48,10 @@ pub fn fmt1(comptime op: u2, comptime offset: u5) InstrFn {
 
             // Equivalent to an ARM MOVS
             cpu.r[rd] = result;
-            setLogicOpFlags(true, cpu, result);
+
+            // Write Flags
+            cpu.cpsr.n.write(result >> 31 & 1 == 1);
+            cpu.cpsr.z.write(result == 0);
         }
     }.inner;
 }
@@ -58,28 +59,51 @@ pub fn fmt1(comptime op: u2, comptime offset: u5) InstrFn {
 pub fn fmt5(comptime op: u2, comptime h1: u1, comptime h2: u1) InstrFn {
     return struct {
         fn inner(cpu: *Arm7tdmi, _: *Bus, opcode: u16) void {
-            const src_idx = @as(u4, h2) << 3 | (opcode >> 3 & 0x7);
-            const dst_idx = @as(u4, h1) << 3 | (opcode & 0x7);
+            const rs = @as(u4, h2) << 3 | (opcode >> 3 & 0x7);
+            const rd = @as(u4, h1) << 3 | (opcode & 0x7);
 
-            const src = if (src_idx == 0xF) (cpu.r[src_idx] + 2) & 0xFFFF_FFFE else cpu.r[src_idx];
-            const dst = if (dst_idx == 0xF) (cpu.r[dst_idx] + 2) & 0xFFFF_FFFE else cpu.r[dst_idx];
+            const op1 = cpu.r[rd];
+            const op2 = cpu.r[rs];
 
+            var result: u32 = undefined;
+            var overflow: bool = undefined;
             switch (op) {
-                0b00 => {
-                    // ADD
-                    const sum = add(false, cpu, dst, src);
-                    cpu.r[dst_idx] = if (dst_idx == 0xF) sum & 0xFFFF_FFFE else sum;
-                },
-                0b01 => cmp(cpu, dst, src), // CMP
-                0b10 => {
-                    // MOV
-                    cpu.r[dst_idx] = if (dst_idx == 0xF) src & 0xFFFF_FFFE else src;
-                },
+                0b00 => result = add(&overflow, op1, op2), // ADD
+                0b01 => result = op1 -% op2, // CMP
+                0b10 => result = op2, // MOV
+                0b11 => {},
+            }
+
+            // Write to Destination Register
+            switch (op) {
+                0b01 => {}, // Test Instruction
                 0b11 => {
                     // BX
-                    cpu.cpsr.t.write(src & 1 == 1);
-                    cpu.r[15] = src & 0xFFFF_FFFE;
+                    const is_thumb = op2 & 1 == 1;
+                    cpu.r[15] = op2 & ~@as(u32, 1);
+
+                    cpu.cpsr.t.write(is_thumb);
+                    cpu.pipe.reload(cpu);
                 },
+                else => {
+                    cpu.r[rd] = result;
+                    if (rd == 0xF) {
+                        cpu.r[15] &= ~@as(u32, 1);
+                        cpu.pipe.reload(cpu);
+                    }
+                },
+            }
+
+            // Write Flags
+            switch (op) {
+                0b01 => {
+                    // CMP
+                    cpu.cpsr.n.write(result >> 31 & 1 == 1);
+                    cpu.cpsr.z.write(result == 0);
+                    cpu.cpsr.c.write(op2 <= op1);
+                    cpu.cpsr.v.write(((op1 ^ result) & (~op2 ^ result)) >> 31 & 1 == 1);
+                },
+                0b00, 0b10, 0b11 => {}, // MOV and Branch Instruction
             }
         }
     }.inner;
@@ -90,21 +114,28 @@ pub fn fmt2(comptime I: bool, is_sub: bool, rn: u3) InstrFn {
         fn inner(cpu: *Arm7tdmi, _: *Bus, opcode: u16) void {
             const rs = opcode >> 3 & 0x7;
             const rd = @truncate(u3, opcode);
+            const op1 = cpu.r[rs];
+            const op2: u32 = if (I) rn else cpu.r[rn];
 
             if (is_sub) {
                 // SUB
-                cpu.r[rd] = if (I) blk: {
-                    break :blk sub(true, cpu, cpu.r[rs], rn);
-                } else blk: {
-                    break :blk sub(true, cpu, cpu.r[rs], cpu.r[rn]);
-                };
+                const result = op1 -% op2;
+                cpu.r[rd] = result;
+
+                cpu.cpsr.n.write(result >> 31 & 1 == 1);
+                cpu.cpsr.z.write(result == 0);
+                cpu.cpsr.c.write(op2 <= op1);
+                cpu.cpsr.v.write(((op1 ^ result) & (~op2 ^ result)) >> 31 & 1 == 1);
             } else {
                 // ADD
-                cpu.r[rd] = if (I) blk: {
-                    break :blk add(true, cpu, cpu.r[rs], rn);
-                } else blk: {
-                    break :blk add(true, cpu, cpu.r[rs], cpu.r[rn]);
-                };
+                var overflow: bool = undefined;
+                const result = add(&overflow, op1, op2);
+                cpu.r[rd] = result;
+
+                cpu.cpsr.n.write(result >> 31 & 1 == 1);
+                cpu.cpsr.z.write(result == 0);
+                cpu.cpsr.c.write(overflow);
+                cpu.cpsr.v.write(((op1 ^ result) & (op2 ^ result)) >> 31 & 1 == 1);
             }
         }
     }.inner;
@@ -113,17 +144,36 @@ pub fn fmt2(comptime I: bool, is_sub: bool, rn: u3) InstrFn {
 pub fn fmt3(comptime op: u2, comptime rd: u3) InstrFn {
     return struct {
         fn inner(cpu: *Arm7tdmi, _: *Bus, opcode: u16) void {
-            const offset = @truncate(u8, opcode);
+            const op1 = cpu.r[rd];
+            const op2: u32 = opcode & 0xFF; // Offset
+
+            var overflow: bool = undefined;
+            const result: u32 = switch (op) {
+                0b00 => op2, // MOV
+                0b01 => op1 -% op2, // CMP
+                0b10 => add(&overflow, op1, op2), // ADD
+                0b11 => op1 -% op2, // SUB
+            };
+
+            // Write to Register
+            if (op != 0b01) cpu.r[rd] = result;
+
+            // Write Flags
+            cpu.cpsr.n.write(result >> 31 & 1 == 1);
+            cpu.cpsr.z.write(result == 0);
 
             switch (op) {
-                0b00 => {
-                    // MOV
-                    cpu.r[rd] = offset;
-                    setLogicOpFlags(true, cpu, offset);
+                0b00 => {}, // MOV | C set by Barrel Shifter, V is unaffected
+                0b01, 0b11 => {
+                    // SUB, CMP
+                    cpu.cpsr.c.write(op2 <= op1);
+                    cpu.cpsr.v.write(((op1 ^ result) & (~op2 ^ result)) >> 31 & 1 == 1);
+                },
+                0b10 => {
+                    // ADD
+                    cpu.cpsr.c.write(overflow);
+                    cpu.cpsr.v.write(((op1 ^ result) & (op2 ^ result)) >> 31 & 1 == 1);
                 },
-                0b01 => cmp(cpu, cpu.r[rd], offset), // CMP
-                0b10 => cpu.r[rd] = add(true, cpu, cpu.r[rd], offset), // ADD
-                0b11 => cpu.r[rd] = sub(true, cpu, cpu.r[rd], offset), // SUB
             }
         }
     }.inner;
@@ -133,10 +183,9 @@ pub fn fmt12(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.r[15] + 2) & 0xFFFF_FFFD;
+            const left = if (isSP) cpu.r[13] else cpu.r[15] & ~@as(u32, 2);
             const right = (opcode & 0xFF) << 2;
-            const result = left + right;
-            cpu.r[rd] = result;
+            cpu.r[rd] = left + right;
         }
     }.inner;
 }
diff --git a/src/core/cpu/thumb/data_transfer.zig b/src/core/cpu/thumb/data_transfer.zig
index 8dda707..5e76291 100644
--- a/src/core/cpu/thumb/data_transfer.zig
+++ b/src/core/cpu/thumb/data_transfer.zig
@@ -12,7 +12,9 @@ pub fn fmt6(comptime rd: u3) InstrFn {
         fn inner(cpu: *Arm7tdmi, bus: *Bus, opcode: u16) void {
             // LDR
             const offset = (opcode & 0xFF) << 2;
-            cpu.r[rd] = bus.read(u32, (cpu.r[15] + 2 & 0xFFFF_FFFD) + offset);
+
+            // Bit 1 of the PC intentionally ignored
+            cpu.r[rd] = bus.read(u32, (cpu.r[15] & ~@as(u32, 2)) + offset);
         }
     }.inner;
 }
diff --git a/src/core/cpu/thumb/software_interrupt.zig b/src/core/cpu/thumb/software_interrupt.zig
index abb69ba..893f902 100644
--- a/src/core/cpu/thumb/software_interrupt.zig
+++ b/src/core/cpu/thumb/software_interrupt.zig
@@ -6,7 +6,7 @@ pub fn fmt17() InstrFn {
     return struct {
         fn inner(cpu: *Arm7tdmi, _: *Bus, _: u16) void {
             // Copy Values from Current Mode
-            const r15 = cpu.r[15];
+            const ret_addr = cpu.r[15] - 2;
             const cpsr = cpu.cpsr.raw;
 
             // Switch Mode
@@ -14,9 +14,10 @@ pub fn fmt17() InstrFn {
             cpu.cpsr.t.write(false); // Force ARM Mode
             cpu.cpsr.i.write(true); // Disable normal interrupts
 
-            cpu.r[14] = r15; // Resume Execution
+            cpu.r[14] = ret_addr; // Resume Execution
             cpu.spsr.raw = cpsr; // Previous mode CPSR
             cpu.r[15] = 0x0000_0008;
+            cpu.pipe.reload(cpu);
         }
     }.inner;
 }
diff --git a/src/main.zig b/src/main.zig
index a2c5ac0..e723949 100644
--- a/src/main.zig
+++ b/src/main.zig
@@ -60,11 +60,12 @@ pub fn main() anyerror!void {
 
     var bus: Bus = undefined;
     var cpu = Arm7tdmi.init(&scheduler, &bus, log_file);
-    if (paths.bios == null) cpu.fastBoot();
 
     try bus.init(allocator, &scheduler, &cpu, paths);
     defer bus.deinit();
 
+    if (paths.bios == null) cpu.fastBoot();
+
     var gui = Gui.init(&bus.pak.title, &bus.apu, width, height);
     defer gui.deinit();
 
diff --git a/src/util.zig b/src/util.zig
index 2275451..cca1297 100644
--- a/src/util.zig
+++ b/src/util.zig
@@ -176,6 +176,7 @@ pub const Logger = struct {
 
     pub fn print(self: *Self, comptime format: []const u8, args: anytype) !void {
         try self.buf.writer().print(format, args);
+        try self.buf.flush(); // FIXME: On panics, whatever is in the buffer isn't written to file
     }
 
     pub fn mgbaLog(self: *Self, cpu: *const Arm7tdmi, opcode: u32) void {
@@ -215,7 +216,7 @@ pub const Logger = struct {
             cpu.r[12],
             cpu.r[13],
             cpu.r[14],
-            cpu.r[15],
+            cpu.r[15] - if (cpu.cpsr.t.read()) 2 else @as(u32, 4),
             cpu.cpsr.raw,
             opcode,
         };