src/core/cpu.zig

@@ -335,7 +335,7 @@ pub const Arm7tdmi = struct {
         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);
-            self.pipe.flush();
+            if (new.t.read()) self.pipe.reload(u16, self) else self.pipe.reload(u32, self);
         }
 
         self.cpsr.raw = value;
@@ -428,15 +428,16 @@ pub const Arm7tdmi = struct {
     pub fn fastBoot(self: *Self) void {
         self.r = std.mem.zeroes([16]u32);
 
-        self.r[0] = 0x08000000;
+        // self.r[0] = 0x08000000;
-        self.r[1] = 0x000000EA;
+        // 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;
     }
 
     pub fn step(self: *Self) void {
@@ -454,7 +455,8 @@ pub const Arm7tdmi = struct {
             }
         }
 
-        if (!self.pipe.flushed) self.r[15] += if (self.cpsr.t.read()) 2 else @as(u32, 4);
+        if (self.pipe.flushed) self.r[15] += if (self.cpsr.t.read()) 2 else @as(u32, 4);
+        self.r[15] += if (self.cpsr.t.read()) 2 else @as(u32, 4);
         self.pipe.flushed = false;
     }
 
@@ -509,7 +511,7 @@ pub const Arm7tdmi = struct {
         self.r[14] = ret_addr;
         self.spsr.raw = new_spsr;
         self.r[15] = 0x0000_0018;
-        self.pipe.flush();
+        self.pipe.reload(u32, self);
     }
 
     inline fn fetch(self: *Self, comptime T: type) T {
@@ -667,6 +669,10 @@ const Pipline = 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 {
@@ -684,13 +690,17 @@ const Pipline = struct {
         return opcode;
     }
 
-    fn reload(self: *Self, cpu: *Arm7tdmi, comptime T: type) void {
+    pub fn reload(self: *Self, comptime T: type, cpu: *Arm7tdmi) void {
         comptime std.debug.assert(T == u32 or T == u16);
-        const inc = if (T == u32) 4 else 2;
+
+        // 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] + inc);
+        self.stage[1] = cpu.bus.read(T, cpu.r[15] + if (T == u32) 4 else @as(u32, 2));
-        cpu.r[15] += inc * 2;
+        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.flush();
+                    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.flush();
+                        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.flush();
+            cpu.pipe.reload(u32, cpu);
         }
     }.inner;
 }
@@ -22,6 +22,7 @@ pub fn branchAndExchange(cpu: *Arm7tdmi, _: *Bus, opcode: u32) void {
 
     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.flush();
+    if (thumb) cpu.pipe.reload(u16, cpu) else cpu.pipe.reload(u32, cpu);
 }

src/core/cpu/arm/data_processing.zig

@@ -5,7 +5,7 @@ const InstrFn = @import("../../cpu.zig").arm.InstrFn;
 const rotateRight = @import("../barrel_shifter.zig").rotateRight;
 const execute = @import("../barrel_shifter.zig").execute;
 
-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,7 +13,7 @@ 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: What are these conditions? I can't remember
+            // TODO: Why these conditions?
             if (!I and opcode >> 4 & 1 == 1) cpu.r[15] += 4;
             const op1 = cpu.r[rn];
 
@@ -23,103 +23,266 @@ pub fn dataProcessing(comptime I: bool, comptime S: bool, comptime instrKind: u4
             // Undo special condition from above
             if (!I and opcode >> 4 & 1 == 1) cpu.r[15] -= 4;
 
-            switch (instrKind) {
+            var result: u32 = undefined;
-                0x0 => {
+            var didOverflow: bool = undefined;
-                    // AND
+
-                    const result = op1 & op2;
+            // Perform Data Processing Logic
-                    cpu.r[rd] = result;
+            switch (kind) {
-                    setArmLogicOpFlags(S, cpu, rd, result);
+                0x0 => result = op1 & op2, // AND
-                },
+                0x1 => result = op1 ^ op2, // EOR
-                0x1 => {
+                0x2 => result = op1 -% op2, // SUB
-                    // EOR
+                0x3 => result = op2 -% op1, // RSB
-                    const result = op1 ^ op2;
+                0x4 => result = newAdd(&didOverflow, op1, op2), // ADD
-                    cpu.r[rd] = result;
+                0x5 => result = newAdc(&didOverflow, op1, op2, old_carry), // ADC
-                    setArmLogicOpFlags(S, cpu, rd, result);
+                0x6 => result = newSbc(op1, op2, old_carry), // SBC
-                },
+                0x7 => result = newSbc(op2, op1, old_carry), // RSC
-                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;
+                    result = op1 & op2;
-                    setTestOpFlags(S, cpu, opcode, result);
                 },
                 0x9 => {
                     // TEQ
                     if (rd == 0xF)
                         return undefinedTestBehaviour(cpu);
 
-                    const result = op1 ^ op2;
+                    result = op1 ^ op2;
-                    setTestOpFlags(S, cpu, opcode, result);
                 },
                 0xA => {
                     // CMP
                     if (rd == 0xF)
                         return undefinedTestBehaviour(cpu);
 
-                    cmp(cpu, op1, op2);
+                    result = op1 -% op2;
                 },
                 0xB => {
                     // CMN
                     if (rd == 0xF)
                         return undefinedTestBehaviour(cpu);
 
-                    cmn(cpu, op1, op2);
+                    didOverflow = @addWithOverflow(u32, op1, op2, &result);
                 },
-                0xC => {
+                0xC => result = op1 | op2, // ORR
-                    // ORR
+                0xD => result = op2, // MOV
-                    const result = op1 | op2;
+                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) cpu.pipe.reload(u32, cpu);
-                },
-                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.flush();
+            // Write Flags
+            switch (kind) {
+                0x0, 0x1, 0xC, 0xD, 0xE, 0xF => {
+                    // Logic Operation Flags
+                    if (S) {
+                        if (rd == 0xF) {
+                            cpu.setCpsr(cpu.spsr.raw);
+                        } else {
+                            cpu.cpsr.n.write(result >> 31 & 1 == 1);
+                            cpu.cpsr.z.write(result == 0);
+                            // C set by Barrel Shifter, V is unaffected
+                        }
+                    }
+                },
+                0x2, 0x3 => {
+                    // SUB, RSB Flags
+                    if (S) {
+                        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);
+                        }
+
+                        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 => {
+                    // 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(didOverflow);
+                        cpu.cpsr.v.write(((op1 ^ result) & (op2 ^ result)) >> 31 & 1 == 1);
+                    } else {
+                        // TEST, TEQ specific
+                        // Barrel Shifter should always calc CPSR C in TST
+                        if (!S) _ = execute(true, cpu, opcode);
+                    }
+                },
+            }
         }
     }.inner;
 }
 
+// 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) {
@@ -132,6 +295,14 @@ fn armSbc(comptime S: bool, cpu: *Arm7tdmi, rd: u4, left: u32, right: u32, old_c
     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);
+
+    return ret;
+}
+
 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;
@@ -184,6 +355,12 @@ fn armAdd(comptime S: bool, cpu: *Arm7tdmi, rd: u4, left: u32, right: u32) u32 {
     return result;
 }
 
+fn newAdd(didOverflow: *bool, left: u32, right: u32) u32 {
+    var ret: u32 = undefined;
+    didOverflow.* = @addWithOverflow(u32, left, right, &ret);
+    return ret;
+}
+
 pub fn add(comptime S: bool, cpu: *Arm7tdmi, left: u32, right: u32) u32 {
     var result: u32 = undefined;
     const didOverflow = @addWithOverflow(u32, left, right, &result);
@@ -210,6 +387,15 @@ fn armAdc(comptime S: bool, cpu: *Arm7tdmi, rd: u4, left: u32, right: u32, old_c
     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);

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.flush();
+                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.flush();
+                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.flush();
+            cpu.pipe.reload(u32, cpu);
         }
     }.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.flush();
+                    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.flush();
+                    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.flush();
+            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.flush();
+            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.flush();
+                cpu.pipe.reload(u16, cpu);
             } else {
                 // Instruction 1
                 const lr_offset = sext(u32, u11, offset) << 12;

src/core/cpu/thumb/data_processing.zig

@@ -77,10 +77,18 @@ pub fn fmt5(comptime op: u2, comptime h1: u1, comptime h2: u1) InstrFn {
                 },
                 0b11 => {
                     // BX
-                    cpu.cpsr.t.write(src & 1 == 1);
+                    const thumb = src & 1 == 1;
-                    cpu.r[15] = src & 0xFFFF_FFFE;
+                    cpu.r[15] = src & ~@as(u32, 1);
+                    cpu.cpsr.t.write(thumb);
+
+                    if (thumb) cpu.pipe.reload(u16, cpu) else cpu.pipe.reload(u32, cpu);
+
+                    // Pipeline alrady flushed
+                    return; // FIXME: Is this necessary? (Refactor out?)
                 },
             }
+
+            if (dst_idx == 0xF) cpu.pipe.reload(u16, cpu);
         }
     }.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.flush();
+            cpu.pipe.reload(u32, cpu);
         }
     }.inner;
 }

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 {