diff --git a/src/cpu/arm/data_processing.zig b/src/cpu/arm/data_processing.zig
index 0ab6b7f..4c23253 100644
--- a/src/cpu/arm/data_processing.zig
+++ b/src/cpu/arm/data_processing.zig
@@ -33,186 +33,245 @@ pub fn dataProcessing(comptime I: bool, comptime S: bool, comptime instrKind: u4
                     // AND
                     const result = op1 & op2;
                     cpu.r[rd] = result;
-                    logicFlags(S, cpu, rd, result);
+                    setArmLogicOpFlags(S, cpu, rd, result);
                 },
                 0x1 => {
                     // EOR
                     const result = op1 ^ op2;
                     cpu.r[rd] = result;
-                    logicFlags(S, cpu, 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);
                 },
-                0x2 => cpu.r[rd] = sub(S, cpu, rd, op1, op2), // SUB
-                0x3 => cpu.r[rd] = sub(S, cpu, rd, op2, op1), // RSB
-                0x4 => cpu.r[rd] = add(S, cpu, rd, op1, op2), // ADD
                 0x5 => {
                     // ADC
-                    var result: u32 = undefined;
-
-                    const did = @addWithOverflow(u32, op1, op2, &result);
-                    const overflow = @addWithOverflow(u32, result, old_carry, &result);
-                    cpu.r[rd] = result;
-
-                    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);
-                            cpu.cpsr.c.write(did or overflow);
-                            cpu.cpsr.v.write(((op1 ^ result) & (op2 ^ result)) >> 31 & 1 == 1);
-                        }
-                    }
+                    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);
                 },
-                0x6 => cpu.r[rd] = sbc(S, cpu, rd, op1, op2, old_carry), // SBC
-                0x7 => cpu.r[rd] = sbc(S, cpu, rd, op2, op1, old_carry), // RSC
                 0x8 => {
                     // TST
                     if (rd == 0xF) {
                         undefinedTestBehaviour(cpu);
-
                         return;
                     }
 
                     const result = op1 & op2;
-                    testFlags(S, cpu, opcode, result);
+                    setTestOpFlags(S, cpu, opcode, result);
                 },
                 0x9 => {
                     // TEQ
                     if (rd == 0xF) {
                         undefinedTestBehaviour(cpu);
-
                         return;
                     }
 
                     const result = op1 ^ op2;
-                    testFlags(S, cpu, opcode, result);
+                    setTestOpFlags(S, cpu, opcode, result);
                 },
                 0xA => {
                     // CMP
                     if (rd == 0xF) {
                         undefinedTestBehaviour(cpu);
-
                         return;
                     }
 
-                    const result = op1 -% op2;
-
-                    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);
+                    cmp(cpu, op1, op2);
                 },
                 0xB => {
                     // CMN
                     if (rd == 0xF) {
                         undefinedTestBehaviour(cpu);
-
                         return;
                     }
 
-                    var result: u32 = undefined;
-                    const didOverflow = @addWithOverflow(u32, op1, op2, &result);
-
-                    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);
+                    cmn(cpu, op1, op2);
                 },
                 0xC => {
                     // ORR
                     const result = op1 | op2;
                     cpu.r[rd] = result;
-                    logicFlags(S, cpu, rd, result);
+                    setArmLogicOpFlags(S, cpu, rd, result);
                 },
                 0xD => {
                     // MOV
                     cpu.r[rd] = op2;
-                    logicFlags(S, cpu, rd, op2);
+                    setArmLogicOpFlags(S, cpu, rd, op2);
                 },
                 0xE => {
                     // BIC
                     const result = op1 & ~op2;
                     cpu.r[rd] = result;
-                    logicFlags(S, cpu, rd, result);
+                    setArmLogicOpFlags(S, cpu, rd, result);
                 },
                 0xF => {
                     // MVN
                     const result = ~op2;
                     cpu.r[rd] = result;
-                    logicFlags(S, cpu, rd, result);
+                    setArmLogicOpFlags(S, cpu, rd, result);
                 },
             }
         }
     }.inner;
 }
 
-fn sbc(comptime S: bool, cpu: *Arm7tdmi, rd: u4, left: u32, right: u32, old_carry: u1) u32 {
+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 {
     // TODO: Make your own version (thanks peach.bot)
     const subtrahend = @as(u64, right) - old_carry + 1;
     const result = @truncate(u32, left -% subtrahend);
 
     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);
-            cpu.cpsr.c.write(subtrahend <= left);
-            cpu.cpsr.v.write(((left ^ result) & (~right ^ result)) >> 31 & 1 == 1);
-        }
+        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;
 }
 
-pub fn sub(comptime S: bool, cpu: *Arm7tdmi, rd: u4, left: u32, right: u32) u32 {
+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) {
-        if (rd == 0xF) {
-            cpu.setCpsr(cpu.spsr.raw);
-        } else {
-            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);
-        }
+        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;
 }
 
-pub fn add(comptime S: bool, cpu: *Arm7tdmi, rd: u4, left: u32, right: u32) u32 {
+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) {
-        if (rd == 0xF) {
-            cpu.setCpsr(cpu.spsr.raw);
-        } else {
-            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);
-        }
+        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 logicFlags(comptime S: bool, cpu: *Arm7tdmi, rd: u4, result: u32) void {
+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) {
-        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
-        }
+        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);
     }
 }
 
-fn testFlags(comptime S: bool, cpu: *Arm7tdmi, opcode: u32, result: u32) void {
+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
diff --git a/src/cpu/thumb/format1.zig b/src/cpu/thumb/format1.zig
index 8ff6b0e..8f66e62 100644
--- a/src/cpu/thumb/format1.zig
+++ b/src/cpu/thumb/format1.zig
@@ -15,7 +15,7 @@ pub fn format1(comptime op: u2, comptime offset: u5) InstrFn {
                 0b00 => shifter.logicalLeft(true, &cpu.cpsr, cpu.r[rs], offset),
                 0b01 => shifter.logicalRight(true, &cpu.cpsr, cpu.r[rs], offset),
                 0b10 => shifter.arithmeticRight(true, &cpu.cpsr, cpu.r[rs], offset),
-                else => std.debug.panic("{} is an invalid op for Format 1 THUMB Instructions", .{op}),
+                else => std.debug.panic("[CPU|THUMB|Fmt1] {} is an invalid op", .{op}),
             };
 
             cpu.r[rd] = result;
diff --git a/src/cpu/thumb/format2.zig b/src/cpu/thumb/format2.zig
index dfb8e9f..9d87f87 100644
--- a/src/cpu/thumb/format2.zig
+++ b/src/cpu/thumb/format2.zig
@@ -16,16 +16,16 @@ pub fn format2(comptime I: bool, is_sub: bool, rn: u3) InstrFn {
             if (is_sub) {
                 // SUB
                 cpu.r[rd] = if (I) blk: {
-                    break :blk sub(true, cpu, rd, cpu.r[rs], @as(u32, rn));
+                    break :blk sub(true, cpu, cpu.r[rs], @as(u32, rn));
                 } else blk: {
-                    break :blk sub(true, cpu, rd, cpu.r[rs], cpu.r[rn]);
+                    break :blk sub(true, cpu, cpu.r[rs], cpu.r[rn]);
                 };
             } else {
                 // ADD
                 cpu.r[rd] = if (I) blk: {
-                    break :blk add(true, cpu, rd, cpu.r[rs], @as(u32, rn));
+                    break :blk add(true, cpu, cpu.r[rs], @as(u32, rn));
                 } else blk: {
-                    break :blk add(true, cpu, rd, cpu.r[rs], cpu.r[rn]);
+                    break :blk add(true, cpu, cpu.r[rs], cpu.r[rn]);
                 };
             }
         }