src/core/cpu.zig

@@ -243,7 +243,7 @@ pub const Arm7tdmi = struct {
     const Self = @This();
 
     r: [16]u32,
-    pipe: Pipeline,
+    pipe: Pipline,
     sched: *Scheduler,
     bus: *Bus,
     cpsr: PSR,
@@ -264,7 +264,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(),
+            .pipe = Pipline.init(),
             .sched = sched,
             .bus = bus,
             .cpsr = .{ .raw = 0x0000_001F },
@@ -324,8 +324,21 @@ pub const Arm7tdmi = struct {
         return self.bus.io.haltcnt == .Halt;
     }
 
+    pub fn setCpsrNoFlush(self: *Self, value: u32) void {
+        if (value & 0x1F != self.cpsr.raw & 0x1F) self.changeModeFromIdx(@truncate(u5, value & 0x1F));
+        self.cpsr.raw = value;
+    }
+
     pub fn setCpsr(self: *Self, value: u32) void {
         if (value & 0x1F != self.cpsr.raw & 0x1F) self.changeModeFromIdx(@truncate(u5, value & 0x1F));
+
+        const new: PSR = .{ .raw = value };
+        if (self.cpsr.t.read() != new.t.read()) {
+            // If THUMB to ARM or ARM to THUMB, flush pipeline
+            self.r[15] &= if (new.t.read()) ~@as(u32, 1) else ~@as(u32, 3);
+            if (new.t.read()) self.pipe.reload(u16, self) else self.pipe.reload(u32, self);
+        }
+
         self.cpsr.raw = value;
     }
 
@@ -429,24 +442,22 @@ pub const Arm7tdmi = struct {
     }
 
     pub fn step(self: *Self) void {
-        defer {
+        if (self.cpsr.t.read()) blk: {
-            if (!self.pipe.flushed) self.r[15] += if (self.cpsr.t.read()) 2 else @as(u32, 4);
+            const opcode = @truncate(u16, self.pipe.step(self, u16) orelse break :blk);
-            self.pipe.flushed = false;
-        }
-
-        if (self.cpsr.t.read()) {
-            const opcode = @truncate(u16, self.pipe.step(self, u16) orelse return);
             if (cpu_logging) self.logger.?.mgbaLog(self, opcode);
 
             thumb.lut[thumb.idx(opcode)](self, self.bus, opcode);
-        } else {
+        } else blk: {
-            const opcode = self.pipe.step(self, u32) orelse return;
+            const opcode = self.pipe.step(self, u32) orelse break :blk;
             if (cpu_logging) self.logger.?.mgbaLog(self, opcode);
 
             if (checkCond(self.cpsr, @truncate(u4, opcode >> 28))) {
                 arm.lut[arm.idx(opcode)](self, self.bus, opcode);
             }
         }
+
+        if (!self.pipe.flushed) self.r[15] += if (self.cpsr.t.read()) 2 else @as(u32, 4);
+        self.pipe.flushed = false;
     }
 
     pub fn stepDmaTransfer(self: *Self) bool {
@@ -484,14 +495,13 @@ pub const Arm7tdmi = struct {
         // 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;
 
-        // If Pipeline isn't full, we have a bug
+        // If pipeline isn't full, return but reschedule the handling of the event
-        std.debug.assert(self.pipe.isFull());
+        if (!self.pipe.isFull()) return;
 
         // log.debug("Handling Interrupt!", .{});
         self.bus.io.haltcnt = .Execute;
 
-        // 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()) 2 else @as(u32, 4);
-        const ret_addr = self.r[15] - if (self.cpsr.t.read()) 0 else @as(u32, 4);
         const new_spsr = self.cpsr.raw;
 
         self.changeMode(.Irq);
@@ -501,21 +511,26 @@ pub const Arm7tdmi = struct {
         self.r[14] = ret_addr;
         self.spsr.raw = new_spsr;
         self.r[15] = 0x0000_0018;
-        self.pipe.reload(self);
+        self.pipe.reload(u32, self);
     }
 
-    inline fn fetch(self: *Self, comptime T: type, address: u32) T {
+    inline fn fetch(self: *Self, comptime T: type) 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;
 
-        // Bus.read will advance the scheduler. There are different timings for CPU fetches,
+        // FIXME: You better hope this is optimized out
-        // 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, address >> 24)];
+        defer self.sched.tick = tick_cache + Bus.fetch_timings[@boolToInt(T == u32)][@truncate(u4, self.r[15] >> 24)];
 
-        return self.bus.read(T, address);
+        return self.bus.read(T, self.r[15]);
+    }
+
+    fn debug_log(self: *const Self, file: *const File, opcode: u32) void {
+        if (self.binary_log) {
+            self.skyLog(file) catch unreachable;
+        } else {
+            self.mgbaLog(file, opcode) catch unreachable;
+        }
     }
 
     pub fn panic(self: *const Self, comptime format: []const u8, args: anytype) noreturn {
@@ -641,7 +656,7 @@ pub fn checkCond(cpsr: PSR, cond: u4) bool {
     };
 }
 
-const Pipeline = struct {
+const Pipline = struct {
     const Self = @This();
     stage: [2]?u32,
     flushed: bool,
@@ -653,6 +668,15 @@ const Pipeline = struct {
         };
     }
 
+    pub fn flush(self: *Self) void {
+        for (self.stage) |*opcode| opcode.* = null;
+        self.flushed = true;
+
+        // Note: If using this, add
+        // if (!self.pipe.flushed) self.r[15] += if (self.cpsr.t.read()) 2 else @as(u32, 4);
+        // to the end of Arm7tdmi.step
+    }
+
     pub fn isFull(self: *const Self) bool {
         return self.stage[0] != null and self.stage[1] != null;
     }
@@ -661,25 +685,26 @@ const Pipeline = struct {
         comptime std.debug.assert(T == u32 or T == u16);
 
         // FIXME: https://github.com/ziglang/zig/issues/12642
-        var opcode = self.stage[0];
+        const opcode = self.stage[0..1][0];
 
         self.stage[0] = self.stage[1];
-        self.stage[1] = cpu.fetch(T, cpu.r[15]);
+        self.stage[1] = cpu.bus.read(T, cpu.r[15]);
 
         return opcode;
     }
 
-    pub fn reload(self: *Self, cpu: *Arm7tdmi) void {
+    pub fn reload(self: *Self, comptime T: type, cpu: *Arm7tdmi) void {
-        if (cpu.cpsr.t.read()) {
+        comptime std.debug.assert(T == u32 or T == u16);
-            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;
-        }
 
+        // Sometimes, the pipeline can be reloaded twice in the same instruction
+        // This can happen if:
+        // 1. R15 is written to
+        // 2. The CPSR is written to (and T changes), so R15 is written to again
+
+        self.stage[0] = cpu.bus.read(T, cpu.r[15]);
+        self.stage[1] = cpu.bus.read(T, cpu.r[15] + if (T == u32) 4 else @as(u32, 2));
+
+        cpu.r[15] += if (T == u32) 8 else @as(u32, 4);
         self.flushed = true;
     }
 };

src/core/cpu/arm/block_data_transfer.zig

@@ -55,7 +55,7 @@ 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);
+                    cpu.pipe.reload(u32, cpu);
                 } else {
                     // FIXME: Should r15 on write be +12 ahead?
                     bus.write(u32, und_addr, cpu.r[15] + 4);
@@ -92,7 +92,7 @@ pub fn blockDataTransfer(comptime P: bool, comptime U: bool, comptime S: bool, c
                     cpu.r[i] = value;
                     if (i == 0xF) {
                         cpu.r[i] &= ~@as(u32, 3); // Align r15
-                        cpu.pipe.reload(cpu);
+                        cpu.pipe.reload(u32, cpu);
 
                         if (S) cpu.setCpsr(cpu.spsr.raw);
                     }

src/core/cpu/arm/branch.zig

@@ -12,7 +12,7 @@ pub fn branch(comptime L: bool) InstrFn {
             if (L) cpu.r[14] = cpu.r[15] - 4;
 
             cpu.r[15] +%= sext(u32, u24, opcode) << 2;
-            cpu.pipe.reload(cpu);
+            cpu.pipe.reload(u32, cpu);
         }
     }.inner;
 }
@@ -24,5 +24,5 @@ pub fn branchAndExchange(cpu: *Arm7tdmi, _: *Bus, opcode: u32) void {
     cpu.r[15] = cpu.r[rn] & if (thumb) ~@as(u32, 1) else ~@as(u32, 3);
 
     cpu.cpsr.t.write(thumb);
-    cpu.pipe.reload(cpu);
+    if (thumb) cpu.pipe.reload(u16, cpu) else cpu.pipe.reload(u32, cpu);
 }

src/core/cpu/arm/data_processing.zig

@@ -24,7 +24,7 @@ pub fn dataProcessing(comptime I: bool, comptime S: bool, comptime kind: u4) Ins
             if (!I and opcode >> 4 & 1 == 1) cpu.r[15] -= 4;
 
             var result: u32 = undefined;
-            var overflow: bool = undefined;
+            var didOverflow: bool = undefined;
 
             // Perform Data Processing Logic
             switch (kind) {
@@ -32,10 +32,10 @@ pub fn dataProcessing(comptime I: bool, comptime S: bool, comptime kind: u4) Ins
                 0x1 => result = op1 ^ op2, // EOR
                 0x2 => result = op1 -% op2, // SUB
                 0x3 => result = op2 -% op1, // RSB
-                0x4 => result = add(&overflow, op1, op2), // ADD
+                0x4 => result = newAdd(&didOverflow, op1, op2), // ADD
-                0x5 => result = adc(&overflow, op1, op2, old_carry), // ADC
+                0x5 => result = newAdc(&didOverflow, op1, op2, old_carry), // ADC
-                0x6 => result = sbc(op1, op2, old_carry), // SBC
+                0x6 => result = newSbc(op1, op2, old_carry), // SBC
-                0x7 => result = sbc(op2, op1, old_carry), // RSC
+                0x7 => result = newSbc(op2, op1, old_carry), // RSC
                 0x8 => {
                     // TST
                     if (rd == 0xF)
@@ -62,7 +62,7 @@ pub fn dataProcessing(comptime I: bool, comptime S: bool, comptime kind: u4) Ins
                     if (rd == 0xF)
                         return undefinedTestBehaviour(cpu);
 
-                    overflow = @addWithOverflow(u32, op1, op2, &result);
+                    didOverflow = @addWithOverflow(u32, op1, op2, &result);
                 },
                 0xC => result = op1 | op2, // ORR
                 0xD => result = op2, // MOV
@@ -75,59 +75,73 @@ pub fn dataProcessing(comptime I: bool, comptime S: bool, comptime kind: u4) Ins
                 0x8, 0x9, 0xA, 0xB => {}, // Test Operations
                 else => {
                     cpu.r[rd] = result;
-                    if (rd == 0xF) {
+                    if (rd == 0xF) cpu.pipe.reload(u32, cpu);
-                        if (S) cpu.setCpsr(cpu.spsr.raw);
-                        cpu.pipe.reload(cpu);
-                    }
                 },
             }
 
             // Write Flags
             switch (kind) {
-                0x0, 0x1, 0xC, 0xD, 0xE, 0xF => if (S and rd != 0xF) {
+                0x0, 0x1, 0xC, 0xD, 0xE, 0xF => {
                     // Logic Operation Flags
-                    cpu.cpsr.n.write(result >> 31 & 1 == 1);
+                    if (S) {
-                    cpu.cpsr.z.write(result == 0);
+                        if (rd == 0xF) {
-                    // C set by Barrel Shifter, V is unaffected
+                            cpu.setCpsr(cpu.spsr.raw);
-
+                        } else {
-                },
+                            cpu.cpsr.n.write(result >> 31 & 1 == 1);
-                0x2, 0x3 => if (S and rd != 0xF) {
+                            cpu.cpsr.z.write(result == 0);
-                    // SUB, RSB Flags
+                            // C set by Barrel Shifter, V is unaffected
-                    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);
                     }
                 },
-                0x4, 0x5 => if (S and rd != 0xF) {
+                0x2, 0x3 => {
-                    // ADD, ADC Flags
+                    // SUB, RSB Flags
-                    cpu.cpsr.n.write(result >> 31 & 1 == 1);
+                    if (S) {
-                    cpu.cpsr.z.write(result == 0);
+                        cpu.cpsr.n.write(result >> 31 & 1 == 1);
-                    cpu.cpsr.c.write(overflow);
+                        cpu.cpsr.z.write(result == 0);
-                    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) {
+                        if (kind == 0x2) {
-                        // SBC specific
+                            // SUB specific
-                        const subtrahend = @as(u64, op2) -% old_carry +% 1;
+                            cpu.cpsr.c.write(op2 <= op1);
-                        cpu.cpsr.c.write(subtrahend <= op1);
+                            cpu.cpsr.v.write(((op1 ^ result) & (~op2 ^ result)) >> 31 & 1 == 1);
-                        cpu.cpsr.v.write(((op1 ^ result) & (~op2 ^ result)) >> 31 & 1 == 1);
+                        } else {
-                    } else {
+                            // RSB Specific
-                        // RSC Specific
+                            cpu.cpsr.c.write(op1 <= op2);
-                        const subtrahend = @as(u64, op1) -% old_carry +% 1;
+                            cpu.cpsr.v.write(((op2 ^ result) & (~op1 ^ result)) >> 31 & 1 == 1);
-                        cpu.cpsr.c.write(subtrahend <= op2);
+                        }
-                        cpu.cpsr.v.write(((op2 ^ result) & (~op1 ^ result)) >> 31 & 1 == 1);
+
+                        if (rd == 0xF) cpu.setCpsr(cpu.spsr.raw);
+                    }
+                },
+                0x4, 0x5 => {
+                    // ADD, ADC Flags
+                    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(((op1 ^ result) & (op2 ^ result)) >> 31 & 1 == 1);
+
+                        if (rd == 0xF) cpu.setCpsr(cpu.spsr.raw);
+                    }
+                },
+                0x6, 0x7 => {
+                    // SBC, RSC Flags
+                    if (S) {
+                        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);
+                        }
+
+                        if (rd == 0xF) cpu.setCpsr(cpu.spsr.raw);
                     }
                 },
                 0x8, 0x9, 0xA, 0xB => {
@@ -141,10 +155,10 @@ pub fn dataProcessing(comptime I: bool, comptime S: bool, comptime kind: u4) Ins
                         cpu.cpsr.v.write(((op1 ^ result) & (~op2 ^ result)) >> 31 & 1 == 1);
                     } else if (kind == 0xB) {
                         // CMN specific
-                        cpu.cpsr.c.write(overflow);
+                        cpu.cpsr.c.write(didOverflow);
                         cpu.cpsr.v.write(((op1 ^ result) & (op2 ^ result)) >> 31 & 1 == 1);
                     } else {
-                        // TST, TEQ specific
+                        // TEST, TEQ specific
                         // Barrel Shifter should always calc CPSR C in TST
                         if (!S) _ = execute(true, cpu, opcode);
                     }
@@ -154,7 +168,134 @@ pub fn dataProcessing(comptime I: bool, comptime S: bool, comptime kind: u4) Ins
     }.inner;
 }
 
-pub fn sbc(left: u32, right: u32, old_carry: u1) u32 {
+// pub fn dataProcessing(comptime I: bool, comptime S: bool, comptime instrKind: u4) InstrFn {
+//     return struct {
+//         fn inner(cpu: *Arm7tdmi, _: *Bus, opcode: u32) void {
+//             const rd = @truncate(u4, opcode >> 12 & 0xF);
+//             const rn = opcode >> 16 & 0xF;
+//             const old_carry = @boolToInt(cpu.cpsr.c.read());
+
+//             // If certain conditions are met, PC is 12 ahead instead of 8
+//             // TODO: What are these conditions? I can't remember
+//             if (!I and opcode >> 4 & 1 == 1) cpu.r[15] += 4;
+//             const op1 = cpu.r[rn];
+
+//             const amount = @truncate(u8, (opcode >> 8 & 0xF) << 1);
+//             const op2 = if (I) rotateRight(S, &cpu.cpsr, opcode & 0xFF, amount) else execute(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);
+//                 },
+//                 0x8 => {
+//                     // TST
+//                     if (rd == 0xF)
+//                         return undefinedTestBehaviour(cpu);
+
+//                     const result = op1 & op2;
+//                     setTestOpFlags(S, cpu, opcode, result);
+//                 },
+//                 0x9 => {
+//                     // TEQ
+//                     if (rd == 0xF)
+//                         return undefinedTestBehaviour(cpu);
+
+//                     const result = op1 ^ op2;
+//                     setTestOpFlags(S, cpu, opcode, result);
+//                 },
+//                 0xA => {
+//                     // CMP
+//                     if (rd == 0xF)
+//                         return undefinedTestBehaviour(cpu);
+
+//                     cmp(cpu, op1, op2);
+//                 },
+//                 0xB => {
+//                     // CMN
+//                     if (rd == 0xF)
+//                         return undefinedTestBehaviour(cpu);
+
+//                     cmn(cpu, op1, op2);
+//                 },
+//                 0xC => {
+//                     // ORR
+//                     const result = op1 | op2;
+//                     cpu.r[rd] = result;
+//                     setArmLogicOpFlags(S, cpu, rd, result);
+//                 },
+//                 0xD => {
+//                     // MOV
+//                     cpu.r[rd] = op2;
+//                     setArmLogicOpFlags(S, cpu, rd, op2);
+//                 },
+//                 0xE => {
+//                     // BIC
+//                     const result = op1 & ~op2;
+//                     cpu.r[rd] = result;
+//                     setArmLogicOpFlags(S, cpu, rd, result);
+//                 },
+//                 0xF => {
+//                     // MVN
+//                     const result = ~op2;
+//                     cpu.r[rd] = result;
+//                     setArmLogicOpFlags(S, cpu, rd, result);
+//                 },
+//             }
+
+//             if (rd == 0xF) cpu.pipe.reload(u32, cpu);
+//         }
+//     }.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;
+}
+
+fn newSbc(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 ret = @truncate(u32, left -% subtrahend);
@@ -162,22 +303,157 @@ pub fn sbc(left: u32, right: u32, old_carry: u1) u32 {
     return ret;
 }
 
-pub fn add(overflow: *bool, left: u32, right: u32) u32 {
+pub fn sbc(comptime S: bool, cpu: *Arm7tdmi, 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);
+
+    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;
+}
+
+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 sub(comptime S: bool, cpu: *Arm7tdmi, left: u32, right: u32) u32 {
+    const result = left -% right;
+
+    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;
+}
+
+fn newAdd(didOverflow: *bool, left: u32, right: u32) u32 {
     var ret: u32 = undefined;
-    overflow.* = @addWithOverflow(u32, left, right, &ret);
+    didOverflow.* = @addWithOverflow(u32, left, right, &ret);
     return ret;
 }
 
-pub fn adc(overflow: *bool, left: u32, right: u32, old_carry: u1) u32 {
+pub fn add(comptime S: bool, cpu: *Arm7tdmi, left: u32, right: u32) u32 {
-    var ret: u32 = undefined;
+    var result: u32 = undefined;
-    const first = @addWithOverflow(u32, left, right, &ret);
+    const didOverflow = @addWithOverflow(u32, left, right, &result);
-    const second = @addWithOverflow(u32, ret, old_carry, &ret);
 
-    overflow.* = first or second;
+    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;
+}
+
+fn newAdc(didOverflow: *bool, left: u32, right: u32, old_carry: u1) u32 {
+    var ret: u32 = undefined;
+    const did = @addWithOverflow(u32, left, right, &ret);
+    const overflow = @addWithOverflow(u32, ret, old_carry, &ret);
+
+    didOverflow.* = did or overflow;
     return ret;
 }
 
+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);
+}
+
 fn undefinedTestBehaviour(cpu: *Arm7tdmi) void {
     @setCold(true);
-    cpu.setCpsr(cpu.spsr.raw);
+    cpu.setCpsrNoFlush(cpu.spsr.raw);
 }

src/core/cpu/arm/single_data_transfer.zig

@@ -47,13 +47,13 @@ pub fn singleDataTransfer(comptime I: bool, comptime P: bool, comptime U: bool,
             address = modified_base;
             if (W and P or !P) {
                 cpu.r[rn] = address;
-                if (rn == 0xF) cpu.pipe.reload(cpu);
+                if (rn == 0xF) cpu.pipe.reload(u32, cpu);
             }
 
             if (L) {
                 // This emulates the LDR rd == rn behaviour
                 cpu.r[rd] = result;
-                if (rd == 0xF) cpu.pipe.reload(cpu);
+                if (rd == 0xF) cpu.pipe.reload(u32, cpu);
             }
         }
     }.inner;

src/core/cpu/arm/software_interrupt.zig

@@ -17,7 +17,7 @@ pub fn armSoftwareInterrupt() InstrFn {
             cpu.r[14] = ret_addr; // Resume Execution
             cpu.spsr.raw = cpsr; // Previous mode CPSR
             cpu.r[15] = 0x0000_0008;
-            cpu.pipe.reload(cpu);
+            cpu.pipe.reload(u32, cpu);
         }
     }.inner;
 }

src/core/cpu/thumb/alu.zig

@@ -4,11 +4,16 @@ 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 lsl = @import("../barrel_shifter.zig").logicalLeft;
+const logicalLeft = @import("../barrel_shifter.zig").logicalLeft;
-const lsr = @import("../barrel_shifter.zig").logicalRight;
+const logicalRight = @import("../barrel_shifter.zig").logicalRight;
-const asr = @import("../barrel_shifter.zig").arithmeticRight;
+const arithmeticRight = @import("../barrel_shifter.zig").arithmeticRight;
-const ror = @import("../barrel_shifter.zig").rotateRight;
+const rotateRight = @import("../barrel_shifter.zig").rotateRight;
 
 pub fn fmt4(comptime op: u4) InstrFn {
     return struct {
@@ -17,85 +22,96 @@ 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 => result = op1 & op2, // AND
+                0x0 => {
-                0x1 => result = op1 ^ op2, // EOR
+                    // AND
-                0x2 => result = lsl(true, &cpu.cpsr, op1, @truncate(u8, op2)), // LSL
+                    const result = cpu.r[rd] & cpu.r[rs];
-                0x3 => result = lsr(true, &cpu.cpsr, op1, @truncate(u8, op2)), // LSR
+                    cpu.r[rd] = result;
-                0x4 => result = asr(true, &cpu.cpsr, op1, @truncate(u8, op2)), // ASR
+                    setLogicOpFlags(true, cpu, result);
-                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
                 },
-                0x8, 0xA => {
+                0x1 => {
-                    // Test Flags
+                    // EOR
-                    // CMN (0xB) is handled with ADC
+                    const result = cpu.r[rd] ^ cpu.r[rs];
-                    cpu.cpsr.n.write(result >> 31 & 1 == 1);
+                    cpu.r[rd] = result;
-                    cpu.cpsr.z.write(result == 0);
+                    setLogicOpFlags(true, cpu, result);
-
-                    if (op == 0xA) {
-                        // CMP specific
-                        cpu.cpsr.c.write(op2 <= op1);
-                        cpu.cpsr.v.write(((op1 ^ result) & (~op2 ^ result)) >> 31 & 1 == 1);
-                    }
                 },
-                0x5, 0xB => {
+                0x2 => {
-                    // ADC, CMN
+                    // LSL
-                    cpu.cpsr.n.write(result >> 31 & 1 == 1);
+                    const result = logicalLeft(true, &cpu.cpsr, cpu.r[rd], @truncate(u8, cpu.r[rs]));
-                    cpu.cpsr.z.write(result == 0);
+                    cpu.r[rd] = result;
-                    cpu.cpsr.c.write(overflow);
+                    setLogicOpFlags(true, cpu, result);
-                    cpu.cpsr.v.write(((op1 ^ result) & (op2 ^ result)) >> 31 & 1 == 1);
+                },
+                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);
                 },
                 0x6 => {
                     // SBC
-                    cpu.cpsr.n.write(result >> 31 & 1 == 1);
+                    cpu.r[rd] = sbc(true, cpu, cpu.r[rd], cpu.r[rs], carry);
-                    cpu.cpsr.z.write(result == 0);
+                },
-
+                0x7 => {
-                    const subtrahend = @as(u64, op2) -% carry +% 1;
+                    // ROR
-                    cpu.cpsr.c.write(subtrahend <= op1);
+                    const result = rotateRight(true, &cpu.cpsr, cpu.r[rd], @truncate(u8, cpu.r[rs]));
-                    cpu.cpsr.v.write(((op1 ^ result) & (~op2 ^ result)) >> 31 & 1 == 1);
+                    cpu.r[rd] = result;
+                    setLogicOpFlags(true, cpu, result);
+                },
+                0x8 => {
+                    // TST
+                    const result = cpu.r[rd] & cpu.r[rs];
+                    setLogicOpFlags(true, cpu, result);
                 },
                 0x9 => {
                     // NEG
-                    cpu.cpsr.n.write(result >> 31 & 1 == 1);
+                    cpu.r[rd] = sub(true, cpu, 0, cpu.r[rs]);
-                    cpu.cpsr.z.write(result == 0);
+                },
-                    cpu.cpsr.c.write(op2 <= 0);
+                0xA => {
-                    cpu.cpsr.v.write(((0 ^ result) & (~op2 ^ result)) >> 31 & 1 == 1);
+                    // 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);
                 },
                 0xD => {
-                    // Multiplication
+                    // MUL
+                    const temp = @as(u64, cpu.r[rs]) * @as(u64, cpu.r[rd]);
+                    const result = @truncate(u32, temp);
+                    cpu.r[rd] = result;
+
                     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;

src/core/cpu/thumb/block_data_transfer.zig

@@ -34,7 +34,7 @@ pub fn fmt14(comptime L: bool, comptime R: bool) InstrFn {
                 if (L) {
                     const value = bus.read(u32, address);
                     cpu.r[15] = value & ~@as(u32, 1);
-                    cpu.pipe.reload(cpu);
+                    cpu.pipe.reload(u16, cpu);
                 } else {
                     bus.write(u32, address, cpu.r[14]);
                 }
@@ -55,7 +55,7 @@ pub fn fmt15(comptime L: bool, comptime rb: u3) InstrFn {
             if (opcode & 0xFF == 0) {
                 if (L) {
                     cpu.r[15] = bus.read(u32, address);
-                    cpu.pipe.reload(cpu);
+                    cpu.pipe.reload(u16, cpu);
                 } else {
                     bus.write(u32, address, cpu.r[15] + 2);
                 }

src/core/cpu/thumb/branch.zig

@@ -15,7 +15,7 @@ pub fn fmt16(comptime cond: u4) InstrFn {
             if (!checkCond(cpu.cpsr, cond)) return;
 
             cpu.r[15] +%= sext(u32, u8, opcode & 0xFF) << 1;
-            cpu.pipe.reload(cpu);
+            cpu.pipe.reload(u16, cpu);
         }
     }.inner;
 }
@@ -25,7 +25,7 @@ pub fn fmt18() InstrFn {
         // B but conditional
         fn inner(cpu: *Arm7tdmi, _: *Bus, opcode: u16) void {
             cpu.r[15] +%= sext(u32, u11, opcode & 0x7FF) << 1;
-            cpu.pipe.reload(cpu);
+            cpu.pipe.reload(u16, cpu);
         }
     }.inner;
 }
@@ -43,7 +43,7 @@ pub fn fmt19(comptime is_low: bool) InstrFn {
                 cpu.r[15] = cpu.r[14] +% (offset << 1);
                 cpu.r[14] = next_opcode | 1;
 
-                cpu.pipe.reload(cpu);
+                cpu.pipe.reload(u16, cpu);
             } else {
                 // Instruction 1
                 const lr_offset = sext(u32, u11, offset) << 12;

src/core/cpu/thumb/data_processing.zig

@@ -3,12 +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 lsl = @import("../barrel_shifter.zig").logicalLeft;
+const cmp = @import("../arm/data_processing.zig").cmp;
-const lsr = @import("../barrel_shifter.zig").logicalRight;
+const setLogicOpFlags = @import("../arm/data_processing.zig").setLogicOpFlags;
-const asr = @import("../barrel_shifter.zig").arithmeticRight;
 
 pub fn fmt1(comptime op: u2, comptime offset: u5) InstrFn {
     return struct {
@@ -22,7 +22,7 @@ pub fn fmt1(comptime op: u2, comptime offset: u5) InstrFn {
                     if (offset == 0) {
                         break :blk cpu.r[rs];
                     } else {
-                        break :blk lsl(true, &cpu.cpsr, cpu.r[rs], offset);
+                        break :blk shifter.logicalLeft(true, &cpu.cpsr, cpu.r[rs], offset);
                     }
                 },
                 0b01 => blk: {
@@ -31,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 lsr(true, &cpu.cpsr, cpu.r[rs], offset);
+                        break :blk shifter.logicalRight(true, &cpu.cpsr, cpu.r[rs], offset);
                     }
                 },
                 0b10 => blk: {
@@ -40,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 asr(true, &cpu.cpsr, cpu.r[rs], offset);
+                        break :blk shifter.arithmeticRight(true, &cpu.cpsr, cpu.r[rs], offset);
                     }
                 },
                 else => cpu.panic("[CPU/THUMB.1] 0b{b:0>2} is not a valid op", .{op}),
@@ -48,10 +48,7 @@ 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;
 }
@@ -62,49 +59,35 @@ pub fn fmt5(comptime op: u2, comptime h1: u1, comptime h2: u1) InstrFn {
             const rs = @as(u4, h2) << 3 | (opcode >> 3 & 0x7);
             const rd = @as(u4, h1) << 3 | (opcode & 0x7);
 
-            const op1 = cpu.r[rd];
+            const rs_value = if (rs == 0xF) cpu.r[rs] & ~@as(u32, 1) else cpu.r[rs];
-            const op2 = cpu.r[rs];
+            const rd_value = if (rd == 0xF) cpu.r[rd] & ~@as(u32, 1) else cpu.r[rd];
 
-            var result: u32 = undefined;
-            var overflow: bool = undefined;
             switch (op) {
-                0b00 => result = add(&overflow, op1, op2), // ADD
+                0b00 => {
-                0b01 => result = op1 -% op2, // CMP
+                    // ADD
-                0b10 => result = op2, // MOV
+                    const sum = add(false, cpu, rd_value, rs_value);
-                0b11 => {},
+                    cpu.r[rd] = if (rd == 0xF) sum & ~@as(u32, 1) else sum;
-            }
+                },
-
+                0b01 => cmp(cpu, rd_value, rs_value), // CMP
-            // Write to Destination Register
+                0b10 => {
-            switch (op) {
+                    // MOV
-                0b01 => {}, // Test Instruction
+                    cpu.r[rd] = if (rd == 0xF) rs_value & ~@as(u32, 1) else rs_value;
+                },
                 0b11 => {
                     // BX
-                    const is_thumb = op2 & 1 == 1;
+                    const thumb = rs_value & 1 == 1;
-                    cpu.r[15] = op2 & ~@as(u32, 1);
+                    cpu.r[15] = rs_value & ~@as(u32, 1);
 
-                    cpu.cpsr.t.write(is_thumb);
+                    cpu.cpsr.t.write(thumb);
-                    cpu.pipe.reload(cpu);
+                    if (thumb) cpu.pipe.reload(u16, cpu) else cpu.pipe.reload(u32, cpu);
-                },
+
-                else => {
+                    // TODO: We shouldn't need to worry about the if statement
-                    cpu.r[rd] = result;
+                    // below, because in BX, rd SBZ (and H1 is guaranteed to be 0)
-                    if (rd == 0xF) {
+                    return;
-                        cpu.r[15] &= ~@as(u32, 1);
-                        cpu.pipe.reload(cpu);
-                    }
                 },
             }
 
-            // Write Flags
+            if (rd == 0xF) cpu.pipe.reload(u16, cpu);
-            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;
 }
@@ -114,28 +97,21 @@ 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
-                const result = op1 -% op2;
+                cpu.r[rd] = if (I) blk: {
-                cpu.r[rd] = result;
+                    break :blk sub(true, cpu, cpu.r[rs], rn);
-
+                } else blk: {
-                cpu.cpsr.n.write(result >> 31 & 1 == 1);
+                    break :blk sub(true, cpu, cpu.r[rs], cpu.r[rn]);
-                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
-                var overflow: bool = undefined;
+                cpu.r[rd] = if (I) blk: {
-                const result = add(&overflow, op1, op2);
+                    break :blk add(true, cpu, cpu.r[rs], rn);
-                cpu.r[rd] = result;
+                } else blk: {
-
+                    break :blk add(true, cpu, cpu.r[rs], cpu.r[rn]);
-                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;
@@ -144,36 +120,17 @@ 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 op1 = cpu.r[rd];
+            const offset = @truncate(u8, opcode);
-            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 | C set by Barrel Shifter, V is unaffected
+                0b00 => {
-                0b01, 0b11 => {
+                    // MOV
-                    // SUB, CMP
+                    cpu.r[rd] = offset;
-                    cpu.cpsr.c.write(op2 <= op1);
+                    setLogicOpFlags(true, cpu, offset);
-                    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;

src/core/cpu/thumb/software_interrupt.zig

@@ -17,7 +17,7 @@ pub fn fmt17() InstrFn {
             cpu.r[14] = ret_addr; // Resume Execution
             cpu.spsr.raw = cpsr; // Previous mode CPSR
             cpu.r[15] = 0x0000_0008;
-            cpu.pipe.reload(cpu);
+            cpu.pipe.reload(u32, cpu);
         }
     }.inner;
 }